ロボットアームの制御ループ — 軌道生成からPIDと重力補償まで

• はじめに — 目標角度をそのまま書き込むと腕が跳ねます
• 台形速度プロファイル — もっとも単純な軌道
• 5次多項式軌道 — 加速度までつなげる
• 関節空間補間と作業空間補間が作る異なる経路
• PID — 3つの項がそれぞれ直している問題
• フィードフォワードと重力補償
• 制御周期と遅延が安定性を削る仕組み
• シミュレータで合わせたゲインが実機で崩れる理由
• 締めくくり — 良い制御器はたいてい良い軌道です

はじめに — 目標角度をそのまま書き込むと腕が跳ねます

逆運動学の計算が終わったとします。手先をどこに置きたいか決まり、各関節が何度になるべきかも出ました。肩0度、肘90度です。

だからコードにこう書きます。

shoulder.write(0);
elbow.write(90);

腕が跳ね出します。目標角度付近で何度か揺れ、腕全体がぶるぶる震えてから止まります。何かを持っていたら落としていたでしょう。

角度は正確に合っているのに動きがめちゃくちゃです。この記事はその隙間についてです。

問題の正体はこうです。上の2行が関節に要求したのは「0.02秒以内に90度移動しろ」です。サーボの立場では平均角速度4500度/秒、そして開始と到着の瞬間の加速度が無限大です。実際に出せるトルクには上限があるので、関節は出せる最大トルクで押し続けて目標を通り過ぎ、戻ってきてまた通り過ぎます。

制御は2層に分かれます。 上層は時間に応じて目標を作る軌道生成で、下層はその目標に実際の角度を追従させるフィードバックループです。上層がなければ下層がどれだけ良くても腕は跳ねます。下層がなければ上層がどれだけなめらかでも重力が腕を引き下ろします。

台形速度プロファイル — もっとも単純な軌道

軌道生成のもっとも古くもっとも広く使われる形です。名前のとおり速度を台形に作ります。一定の加速度で上げ、最大速度で巡航し、一定の加速度で下げます。

数字を入れてみます。肩を90度、つまり1.5708 rad動かします。この関節が出せる最大速度を1.0 rad/s、最大加速度を2.0 rad/s²とします。

加速区間で最大速度に達するまでの時間は、

t_a = v_max / a_max = 1.0 / 2.0 = 0.5秒

その間に移動する角度は、

d_a = 0.5 × a_max × t_a² = 0.5 × 2.0 × 0.25 = 0.25 rad

減速区間も対称なので同じ0.25 radです。合わせて0.5 radが加減速に使われ、残る角度は、

1.5708 - 0.5 = 1.0708 rad

この区間を最大速度で通過するので、

t_c = 1.0708 / 1.0 = 1.0708秒

全体の移動時間は、

T = 0.5 + 1.0708 + 0.5 = 2.0708秒

先ほどの0.02秒の指令と比べると100倍遅いです。そしてこの100倍が正確に腕が跳ねない理由です。

ここで重要な例外がひとつあります。移動距離が短いと、最大速度に到達する前に減速を始める必要があります。加減速に必要な最小距離が0.5 radだったので、0.2 radを動かすなら巡航区間がない三角形プロファイルになります。このとき到達する最高速度は、

v_peak = √(a_max × d) = √(2.0 × 0.2) = 0.6325 rad/s
T = 2 × v_peak / a_max = 2 × 0.6325 / 2.0 = 0.6325秒

この分岐をコードで見落とすと、短い移動でプロファイルが目標を通り過ぎてしまいます。下の実装にはその判定が入っています。

import numpy as np


def trapezoid(dq, v_max, a_max):
    """台形(または三角形)速度プロファイルの区間時間を計算します。"""
    dq = abs(dq)
    d_a = v_max ** 2 / (2 * a_max)      # 加速区間で移動する距離
    if 2 * d_a >= dq:                    # 最大速度に到達しません
        v_peak = np.sqrt(a_max * dq)
        t_a = v_peak / a_max
        return t_a, 0.0, t_a, v_peak
    t_a = v_max / a_max
    t_c = (dq - 2 * d_a) / v_max
    return t_a, t_c, t_a, v_max


def sample(t, dq, v_max, a_max):
    """時刻tでの位置と速度を返します。符号は最後に付けます。"""
    sign = 1.0 if dq >= 0 else -1.0
    t_a, t_c, t_d, v_p = trapezoid(dq, v_max, a_max)
    T = t_a + t_c + t_d
    t = min(max(t, 0.0), T)
    if t < t_a:
        q, v = 0.5 * a_max * t * t, a_max * t
    elif t < t_a + t_c:
        q, v = 0.5 * a_max * t_a ** 2 + v_p * (t - t_a), v_p
    else:
        td = t - t_a - t_c
        q = 0.5 * a_max * t_a ** 2 + v_p * t_c + v_p * td - 0.5 * a_max * td * td
        v = v_p - a_max * td
    return sign * q, sign * v


DQ, VMAX, AMAX = np.radians(90), 1.0, 2.0
ta, tc, td, vp = trapezoid(DQ, VMAX, AMAX)
print(f"加速 {ta:.4f}s  巡航 {tc:.4f}s  減速 {td:.4f}s  全体 {ta+tc+td:.4f}s")
for t in (0.0, 0.25, 0.5, 1.0, 1.5708, 2.0708):
    q, v = sample(t, DQ, VMAX, AMAX)
    print(f"  t={t:.4f}s  q={np.degrees(q):8.4f}度  v={v:.5f} rad/s")

実行結果です。

加速 0.5000s  巡航 1.0708s  減速 0.5000s  全体 2.0708s
  t=0.0000s  q=  0.0000度  v=0.00000 rad/s
  t=0.2500s  q=  3.5810度  v=0.50000 rad/s
  t=0.5000s  q= 14.3239度  v=1.00000 rad/s
  t=1.0000s  q= 42.9718度  v=1.00000 rad/s
  t=1.5708s  q= 75.6763度  v=0.99999 rad/s
  t=2.0708s  q= 90.0000度  v=0.00000 rad/s

台形プロファイルには問題がひとつ残っています。t=0とt=0.5で加速度が0から2.0へ、2.0から0へ階段状に跳びます。加速度の微分である躍度がその瞬間無限大です。軽い腕ではあまり見えませんが、リンクが長く細いとこの衝撃が構造を叩き、目に見える残留振動が残ります。

5次多項式軌道 — 加速度までつなげる

躍度問題をなくす標準的な方法は、位置を時間の多項式にして、両端で位置・速度・加速度をすべて指定することです。条件が6つなので未知数も6つ、つまり5次多項式になります。

q(t) = a0 + a1·t + a2·t² + a3·t³ + a4·t⁴ + a5·t⁵

両端で速度と加速度を0にすると係数が閉じた形で出ます。移動量をΔq、全体の時間をTとすると、

a0 = q0,  a1 = 0,  a2 = 0
a3 =  10·Δq / T³
a4 = -15·Δq / T⁴
a5 =   6·Δq / T⁵

ここで実務的に重要な数字が2つ出てきます。正規化した時間τ = t/Tで見ると、速度はτ = 0.5で、加速度はτ = 0.2113とτ = 0.7887で最大になり、その値はそれぞれこうです。

v_max = 1.875 × Δq / T
a_max = (10/√3) × Δq / T² = 5.7735 × Δq / T²

先ほどの台形と同じ条件で比較してみます。Δq = 1.5708 radをT = 2.0708秒で動かすと、

v_max = 1.875 × 1.5708 / 2.0708 = 1.4223 rad/s
a_max = 5.7735 × 1.5708 / 2.0708² = 2.1149 rad/s²

同じ時間を使うのに最大速度が1.0ではなく1.4223です。42パーセント速い速度を要求します。逆に最大速度を1.0に抑えると、

T = 1.875 × 1.5708 / 1.0 = 2.9452秒

2.0708秒の移動が2.9452秒になります。やはり42パーセントの差です。

この表が言っているのはどちらが優れているかではありません。産業用ロボットが台形系を使う理由はサイクルタイムがそのまま生産性だからで、精密測定機器やカメラジンバルが多項式を使う理由は残留振動がそのまま品質だからです。2つの性質を混ぜたS字カーブプロファイルが実際にもっともよく使われていて、台形の加減速の角を多項式で丸めた形と考えればよいです。

import numpy as np


def quintic(q0, qf, T):
    """両端の速度と加速度が0である5次多項式の係数を返します。"""
    d = qf - q0
    return np.array([q0, 0.0, 0.0, 10 * d / T ** 3, -15 * d / T ** 4, 6 * d / T ** 5])


def evaluate(coef, t):
    powers = np.array([t ** i for i in range(6)])
    dpow = np.array([i * t ** (i - 1) if i >= 1 else 0.0 for i in range(6)])
    ddpow = np.array([i * (i - 1) * t ** (i - 2) if i >= 2 else 0.0 for i in range(6)])
    return coef @ powers, coef @ dpow, coef @ ddpow


T = 2.0
c = quintic(0.0, np.radians(90), T)
print("係数:", np.round(c, 6))
for t in (0.0, 0.5, 1.0, 1.5, 2.0):
    q, v, a = evaluate(c, t)
    print(f"  t={t}s  q={np.degrees(q):8.4f}度  v={v:8.5f} rad/s  a={a:8.5f} rad/s²")
print(f"  予測最大速度 {1.875*np.radians(90)/T:.6f}、最大加速度 {10/np.sqrt(3)*np.radians(90)/T**2:.6f}")

実行結果です。

係数: [ 0.        0.        0.        1.963495 -1.472622  0.294524]
  t=0.0s  q=  0.0000度  v= 0.00000 rad/s  a= 0.00000 rad/s²
  t=0.5s  q=  9.3164度  v= 0.82835 rad/s  a= 2.20893 rad/s²
  t=1.0s  q= 45.0000度  v= 1.47262 rad/s  a= 0.00000 rad/s²
  t=1.5s  q= 80.6836度  v= 0.82835 rad/s  a=-2.20893 rad/s²
  t=2.0s  q= 90.0000度  v= 0.00000 rad/s  a= 0.00000 rad/s²
  予測最大速度 1.472622、最大加速度 2.267249

t=1.0でちょうど半分の45度を通過し速度は1.4726で最大、両端で速度と加速度がどちらも0です。数式が予測した最大速度と標本値が一致しています。

関節空間補間と作業空間補間が作る異なる経路

ここで分かれ道がひとつあります。軌道を関節角度に対して作るか、手先位置に対して作るかです。

関節空間補間は開始角度と終了角度の間を上のプロファイルで埋めます。各関節が独立して自分の軌道を追従すればよいです。計算が安く、関節速度・加速度限界を直接守れて、特異点を気にする必要がありません。

作業空間補間は開始位置と終了位置の間を直線でつなぎ、その直線上の点ごとに逆運動学を解いて関節角度を得ます。手先が実際に直線を描きます。

2つの方法が作る経路がどれだけ違うか計算してみます。順運動学編で使ったのと同じ2リンク腕です。上腕L1 = 0.20 m、前腕L2 = 0.15 m。

開始姿勢を(30度, 45度)、到着姿勢を(-30度, 45度)とすると、手先は、

開始 (0.212028, 0.244889) m
到着 (0.318094, -0.061177) m

関節空間でちょうど半分の(0度, 45度)の手先位置は、

x = 0.20·cos(0) + 0.15·cos(45度) = 0.20 + 0.106066 = 0.306066
y = 0.20·sin(0) + 0.15·sin(45度) = 0 + 0.106066 = 0.106066

作業空間で直線の中点は両端点の平均なので、

x = (0.212028 + 0.318094) / 2 = 0.265061
y = (0.244889 - 0.061177) / 2 = 0.091856

2点間の距離は、

√((0.306066 - 0.265061)² + (0.106066 - 0.091856)²)
= √(0.041005² + 0.014210²)
= √(0.00168141 + 0.00020193)
= 0.043398 m = 43.4 mm

同じ2点をつなぐのに、途中で43ミリメートルもずれます。 腕全体の長さが350ミリメートルなので12パーセントを超えます。コップに水を注ぐ動作ならこの差が成功と失敗を分けます。逆に箱の上のA地点からB地点へ移すだけの動作なら、43ミリメートル膨らんだ経路がかえって障害物を避けてくれることもあります。

最後の行が実務でもっとも痛い項目です。直線上の点ごとに逆運動学を独立に解くと、ある点で急に別の解が選ばれて肘が反対に反転することがあります。手先は依然として直線上にありますが、腕全体が1周期でひっくり返ります。解決策は毎回新しく解く代わりに、直前の解にもっとも近い解を選ぶよう強制することです。

PID — 3つの項がそれぞれ直している問題

軌道が毎周期目標角度を出したら、下層がその目標に追従する必要があります。

関節ひとつをこうモデル化します。回転慣性J = 0.02 kg·m²、粘性摩擦b = 0.05 N·m·s/rad、そして順運動学と静的トルク編で求めた肩の重力トルク1.79 N·mが下向きにかかります。目標は90度です。

J·q̈ = τ - b·q̇ - τ_g

3つの項をひとつずつ入れてみます。

比例項は誤差に比例して押します。Kp = 20にしてこれだけをオンにすると、関節が目標を大きく通り過ぎてしばらく振動します。摩擦だけではエネルギーを十分抜けません。

微分項は速度に比例して逆に引きます。物理的には人工の摩擦を作っているのです。Kd = 1.0を足すとオーバーシュートが消えます。ところが目標に到達せず5.13度手前で止まります。

この5.13度は偶然の値ではありません。静止状態では速度が0なので微分項が0で、比例項だけが重力と釣り合っています。

Kp × e = τ_g
e = 1.79 / 20 = 0.0895 rad = 5.129度

手先が肩から0.35メートル離れているので、

0.0895 rad × 0.35 m = 0.0313 m = 31.3 mm

31ミリメートル下に垂れ下がります。 腕は静かに止まっていて誤差も安定していますが、ただ間違った位置にあります。

積分項はこの残留誤差を時間をかけて積み上げて消します。Ki = 40を足すと定常偏差が0.07度まで下がります。代わりにオーバーシュートが4.75度生まれます。積分項は過去を記憶しているので反応が一拍遅れます。

import numpy as np

J_INERTIA = 0.02     # kg·m²
B_VISCOUS = 0.05     # N·m·s/rad
TAU_GRAVITY = 1.79   # N·m、肩が水平のとき重力が作るトルク
DT, T_END = 0.001, 2.0
REF = np.radians(90)


def simulate(kp, ki, kd, feedforward=0.0):
    q, dq, integral = 0.0, 0.0, 0.0
    log = []
    for _ in range(int(T_END / DT)):
        e = REF - q
        integral += e * DT
        tau = kp * e + ki * integral + kd * (0.0 - dq) + feedforward
        ddq = (tau - B_VISCOUS * dq - TAU_GRAVITY) / J_INERTIA
        dq += ddq * DT
        q += dq * DT
        log.append(q)
    return np.array(log)


for name, (kp, ki, kd, ff) in {
    "P only":      (20, 0, 0.0, 0.0),
    "PD":          (20, 0, 1.0, 0.0),
    "PID":         (20, 40, 1.0, 0.0),
    "PD+gravity":  (20, 0, 1.0, TAU_GRAVITY),
}.items():
    log = simulate(kp, ki, kd, ff)
    err = np.degrees(REF - log[-1])
    over = max(0.0, np.degrees(log.max() - REF))
    print(f"{name:<11} 最終 {np.degrees(log[-1]):8.4f}度  定常偏差 {err:8.4f}度  オーバーシュート {over:7.4f}度")

実行結果です。

P only      最終  78.4275度  定常偏差  11.5725度  オーバーシュート 69.8285度
PD          最終  84.8720度  定常偏差   5.1280度  オーバーシュート  0.0000度
PID         最終  90.0718度  定常偏差  -0.0718度  オーバーシュート  4.7486度
PD+gravity  最終  90.0000度  定常偏差  -0.0000度  オーバーシュート  0.6777度

PDの定常偏差5.1280度が手で求めた5.129度と一致します。理論とシミュレーションが同じ値を出せば、モデルが正しいという意味です。

最後の行に0.6777度のオーバーシュートが残っていることにも意味があります。重力を完全に取り除くと残るのは純粋な2次系で、このゲインでの減衰比を計算すると、

ζ = (Kd + b) / (2·√(Kp·J)) = 1.05 / (2·√(20 × 0.02)) = 1.05 / 1.2649 = 0.830

0.83なので理論的なオーバーシュートは約0.9パーセント、90度に対して0.84度です。シミュレーションの0.68度と同じ桁です。PDだけをオンにしたときオーバーシュートが0だったのは制御器がうまくやったからではなく、重力が腕をずっと下に引いてブレーキ役を果たしていたからです。重力補償はそのただのブレーキもなくすので、補償をオンにした後はKdをもう一度見直す必要があります。

積分項を使うとき必ず一緒に入れるべき装置がひとつあります。腕が物理的に塞がれているかモーターがトルク限界にぶつかると誤差が減らないのに、積分項はその間も積み上がり続けます。後で障害物が取り除かれた瞬間、ものすごく大きくなった積分項が腕を吹き飛ばします。これが積分ワインドアップで、対策は出力が飽和したら積分を止めるか、積分値そのものに上限を設けることです。

// Arduinoでの関節ひとつのPIDです。固定周期で回ることが前提です。
const float KP = 20.0f, KI = 40.0f, KD = 1.0f;
const float DT = 0.005f;             // 200Hz。この値は実際の周期と同じでなければなりません
const float I_LIMIT = 3.0f;          // 積分ワインドアップ防止の上限(N·m換算)
const float TAU_LIMIT = 8.0f;        // モーターが出せるトルクの上限

float integral = 0.0f;
float prevMeasured = 0.0f;

float pidStep(float target, float measured, float gravityFeedforward) {
  float error = target - measured;

  // 微分は誤差ではなく測定値で計算します。目標が階段状に変わると
  // 誤差の微分が瞬間的に爆発し出力が跳ねるからです。
  float derivative = -(measured - prevMeasured) / DT;
  prevMeasured = measured;

  float unsaturated = KP * error + KI * integral + KD * derivative + gravityFeedforward;

  // 出力が飽和している方向へはこれ以上積分を積み上げません。
  bool pushingIntoLimit =
      (unsaturated > TAU_LIMIT && error > 0) || (unsaturated < -TAU_LIMIT && error < 0);
  if (!pushingIntoLimit) {
    integral += error * DT;
    if (integral > I_LIMIT) integral = I_LIMIT;
    if (integral < -I_LIMIT) integral = -I_LIMIT;
  }

  float tau = KP * error + KI * integral + KD * derivative + gravityFeedforward;
  if (tau > TAU_LIMIT) tau = TAU_LIMIT;
  if (tau < -TAU_LIMIT) tau = -TAU_LIMIT;
  return tau;
}

微分を誤差ではなく測定値で計算している部分に注目してください。目標が階段状に変わる瞬間、誤差の微分は理論上無限大で、実際には1周期で出力が上限まで跳びます。測定値の微分を使うと、目標がどう変わってもこの項は実際の関節速度だけを見ます。

フィードフォワードと重力補償

上のシミュレーションの最後の行に戻ります。PDに重力トルクをそのまま足したら定常偏差が0.0000度になりました。積分項をひとつも使わずにです。

これがフィードフォワードです。フィードバックは誤差が生まれてから初めて反応します。ところが重力は予測可能です。関節角度さえ分かれば重力がどれだけかかっているか今すぐ計算できます。わざわざ垂れ下がるのを待ってから直す理由がありません。

肩の重力トルクを角度の関数として書いてみます。各質量の水平距離に重さを掛けて足せばよいです。

τ_g(θ1, θ2) = g × [ m1·(L1/2)·cos(θ1)
                  + m2·(L1·cos(θ1) + (L2/2)·cos(θ1+θ2))
                  + (m_grip + m_pay)·(L1·cos(θ1) + L2·cos(θ1+θ2)) ]

前回の記事の値(m1 = 0.15、m2 = 0.10、グリッパー0.15、ペイロード0.25 kg)を入れると、

垂直に立てるとちょうど0で、肘を折ると重心が内側に来るので減っていきます。この表がそのままフィードフォワード項です。毎周期現在の角度でこの式を計算して制御出力に足せば、フィードバックはモデルが取りこぼした残りだけを担当すればよくなります。

ここで正直に言っておくべきことがあります。この式はモデルです。実際の腕の質量分布を正確に知らなければ計算値がずれ、ずれた分は依然としてフィードバックが処理する必要があります。それでも1.79 N·m全部をフィードバックが負担するのと0.2 N·mだけを負担するのとではまったく違う問題です。必要なKpがその分小さくなり、Kpが小さくなればノイズ増幅と振動も一緒に減ります。

フィードフォワードに入れられる項は重力だけではありません。軌道からすでに目標速度と目標加速度が分かっているので、

τ_ff = J·q̈_desired + b·q̇_desired + τ_g(q_desired)

こう3つの項を全部入れると、モデルが完璧なときフィードバック出力が0になります。これを計算トルク制御と呼び、産業用ロボットの制御器がしていることはおおむねこれです。フィードバックをモデルが間違っている分だけ働かせることが制御設計の大きな方向性です。

制御周期と遅延が安定性を削る仕組み

ここまでの話はすべて連続時間でした。実際の制御器は離散的に回り、その事実が安定性に直接影響します。

デジタル制御ループには少なくとも2つの遅延があります。ひとつはゼロ次ホールドから来る平均半周期、もうひとつはセンサーを読んで計算して出力するまでの時間です。両方合わせておよそ1.5周期と見積もるのが実務的な目安です。

純粋な遅延が位相に与える影響は周波数に比例します。

位相遅れ(ラジアン) = ω × T_delay

ループ利得が1になる周波数、つまり交差周波数を20 rad/s(約3.2Hz)として計算してみます。

50Hz:   20 × 0.030 = 0.600 rad = 34.38度
1000Hz: 20 × 0.0015 = 0.030 rad = 1.72度
差:     32.66度

制御周期を1キロヘルツから50ヘルツに下げると、位相余裕32.7度がそのまま消えます。 一般的な設計目標は45度から60度の間なので、32.7度を失うと残るものがほとんどありません。ゲインは何も触っていないのにシステムが発振します。

症状の形が特徴的です。腕が目標付近で一定の周波数で震え、ゲインを下げると止まり、また上げると同じ周波数で震えます。その周波数が制御周期と関係あるなら、原因はゲインではなく遅延です。

周期が揺れることも同じ問題です。Arduinoでloop()の中にPIDを入れてdelay(5)で周期を合わせるコードをよく見かけますが、実際の周期は5ミリ秒に計算時間が足された値で、シリアル出力でもあれば大きく伸びます。微分項と積分項はどちらもDTで割って掛けるので、DTが実際と違うとゲインがその比率だけずれます。ハードウェアタイマー割り込みで固定周期を作るか、最低限実際の経過時間を測って使うほうが安全です。

遅延を作るのはソフトウェアだけではありません。通信も遅延です。ひとつのバスにデイジーチェーンでつながったスマートサーボなら、関節6個の状態を一巡して読むのにかかる時間がそのまま最小制御周期になります。I2Cでエンコーダを読むなら、I2Cバスの特性のせいでクロックストレッチングとスレーブの応答時間が周期にそのまま入り込みます。

シミュレータで合わせたゲインが実機で崩れる理由

この記事のシミュレーションでKp = 20、Kd = 1.0は非常にうまく動作しました。同じ値を実際の腕に入れるとたいてい崩れます。理由がいくつかあり、すべてモデルになかったものです。

第一に、バックラッシュです。 シミュレーションではモーター軸と関節軸は同じ角度です。実物では減速機の遊びの分だけずれます。この区間ではモーターが回っても関節が動かないので、制御器の立場では利得が0の区間です。遊びを越えて歯がかみ合う瞬間、利得が急に正常に戻ります。区間ごとに利得が違うシステムでひとつのゲインセットがすべての区間でうまく動くのは難しいです。症状は目標付近での低周波リミットサイクル、つまり小さな振幅でずっと行ったり来たりすることです。

第二に、関節の柔軟性です。 リンクを剛体としましたが、実物はたわみます。ハーモニックドライブは原理上弾性体を使い、3Dプリントのリンクは目に見えてたわみ、ベルトは伸びます。この弾性がモーターとリンクの間に共振を作り、その共振周波数より上では位相が180度反転します。シミュレーションの剛体モデルにはこの極がそもそも存在しません。

第三に、摩擦が粘性ではありません。 モデルのb·q̇は速度に比例する粘性摩擦ですが、実際の関節で支配的なのは速度と無関係なクーロン摩擦と、静止状態でより大きくなる静止摩擦です。低速でスティックスリップ、つまり付いては滑るを繰り返す現象が出ます。とてもゆっくり動けという命令がもっとも難しい命令である理由です。

第四に、エンコーダの量子化と微分ノイズです。 12ビットエンコーダは1回転を4096に分けるので分解能は0.0879度です。200Hzでこの1目盛りの差を微分すると、

0.0879度 / 0.005秒 = 17.6度/秒

関節が完全に静止していても、測定値が1目盛り揺れれば微分項は17.6度/秒に相当する速度を見ます。Kdが大きいとこのノイズがそのまま出力に出てモーターが唸ります。だから微分項にはほぼ必ず低域通過フィルタが付きます。

第五に、電源です。 シミュレーションのモーターは要求したトルクを即座に出します。実物ではモーターを駆動する電源が電流を賄えなければトルクが出ません。複数の関節が同時に加速する瞬間電圧が崩れ、制御器は自分が指令したトルクが出たと信じたまま次の計算をします。腕が重い姿勢へ行くときだけおかしくなるなら、ゲインより先に電流を測ってみてください。

実務的な順序はこうです。シミュレータから得られるのは最終ゲインではなくゲインの桁と構造です。どの項が必要か、だいたいどのくらいの大きさか、軌道が関節限界内に収まるかを確認する道具です。実機では重力補償を先にオンにし、KiとKdを0にしたままKpを振動が始まるまで上げてから半分に下げ、次にKdを上げて振動を抑え、定常偏差が残れば最後にごく小さなKiを入れます。順序を変えると何が何を直したのか分からなくなります。

締めくくり — 良い制御器はたいてい良い軌道です

この記事で扱った対策は層が違います。

軌道生成はそもそも不可能なことを要求しないことです。90度を0.02秒で行けという命令はどんな制御器でもうまくこなせません。2.07秒で行けと言えば、平凡な制御器でもうまく追従します。

フィードフォワードは分かっていることをあらかじめ入れておくことです。重力は毎瞬間計算できるのに、わざわざ誤差が生まれるのを待つ理由がありません。

フィードバックは残りを担当します。モデルが取りこぼした摩擦、予想外の外力、部品の個体差。この残りが小さいほどゲインが小さくて済み、ゲインが小さいほどシステムが寛容になります。

跳ねる腕に出会ったとき、最初に見るべきなのはゲインではなく指令です。 ゲインをどれだけいじっても直らなかった問題が軌道一行で消えることが実際によくあります。その次が遅延で、ゲインはたいてい最後です。

この順序を自分で確かめられる実験がひとつあります。制御周期を半分に減らしてみてください。症状が良くなれば遅延の問題で、変わらなければモデルの問題です。ゲインはその2つを区別した後にいじるものです。残りの背景にある数学はロボット工学に必要な数学編に順序立てて整理してあります。