ロボット関節のPID制御

レッスン04では、目標に対する関節角度を求めました。次に必要なのは、各関節をその角度まで動かし、重力・摩擦・ペイロードに押し戻されてもその位置に保つ仕組みです。ほぼすべてのロボットで、その役割はドライブ内のフィードバックコントローラが担います。多くの場合、それはPIDコントローラです。このレッスンでは、シミュレーションした1つの関節に対してPIDコントローラを作ります。まず負荷を駆動するDCモータのモデルを作り、P、PI、PIDの順にループを閉じます。そのうえで、実際の実装で必ず直面する2つの問題を扱います。アンプが飽和したときの積分ワインドアップと、目標値のステップ変化で生じる微分キックです。すべて素のnumpyで動くので、コントローラの各行を確認できます。

関節のモデル

関節は、減速機を介してリンクを駆動するDCモータです。すべての量を関節出力側に換算するので、減速比は定数に含まれます。次の3つの式で表されます。

current      i = (u - KE * omega) / R            (armature inductance neglected)
torque       tau = KT * i
acceleration J * domega/dt = tau - B * omega - tau_load

uはアンプからの電圧、omegaは関節速度、KEは逆起電力定数、KTはトルク定数、Rは巻線抵抗、Jは慣性、Bは粘性摩擦、tau_loadはリンクにかかる重力などの負荷トルクです。逆起電力の項により、モータ自身の速度が電流を減らします。これは減衰を追加したのと同じ働きをします。電気的時定数が機械的時定数よりずっと短い場合、インダクタンスを無視するのは妥当です。以下の数値は小型の減速機付き関節を想定した例示用の値で、データシートから取ったものではありません。

モデルは1 msの固定ステップで、半陰的オイラー法（semi-implicit Euler）により積分します。加速度から速度を更新し、次に新しい速度から角度を更新します。通常のオイラー法より1行分丁寧なだけですが、振動する系でははるかに良く振る舞います。制御を加える前に、開ループでモデルを確認します。6 Vを印加し、シミュレーションした速度と時定数を公式と比較します。

import numpy as np

# Joint model: DC motor plus gearbox plus load, all referred to the joint output.
J = 0.01        # inertia, kg m^2
B = 0.01        # viscous friction, N m s/rad
KT = 0.5        # torque constant at the output, N m/A
KE = 0.5        # back-EMF constant at the output, V s/rad
R_ARM = 2.0     # armature resistance, ohm (inductance neglected)
DT = 0.001      # integration step, s

def joint_accel(omega: float, volts: float, tau_load: float) -> float:
    current = (volts - KE * omega) / R_ARM
    torque = KT * current - B * omega - tau_load
    return torque / J

# Open loop: apply 6 V with no load and integrate for 0.5 s
omega, theta = 0.0, 0.0
t63 = None
w_final = (KT / R_ARM) * 6.0 / (B + KT * KE / R_ARM)   # steady-state speed from the model
for k in range(500):
    omega += joint_accel(omega, 6.0, 0.0) * DT
    theta += omega * DT
    if t63 is None and omega >= 0.632 * w_final:
        t63 = (k + 1) * DT
tau = J / (B + KT * KE / R_ARM)
print(f"speed after 0.5 s: {omega:.2f} rad/s (model steady state {w_final:.2f} rad/s)")
print(f"time constant: simulated {t63:.3f} s, analytic {tau:.3f} s")
print(f"angle travelled: {theta:.2f} rad")

speed after 0.5 s: 11.10 rad/s (model steady state 11.11 rad/s)
time constant: simulated 0.074 s, analytic 0.074 s
angle travelled: 4.74 rad

速度は時定数74 msの1次遅れとして11.1 rad/sまで上昇し、公式の予測と一致します。速度は1次系です。角度は速度の積分なので、電圧から角度までで見ると関節は2次系で、原点に極を1つ持ちます。この極があるため、一定電圧を加えた関節は特定の角度に落ち着かず、動き続けます。位置制御にフィードバックが必要なのもこのためです。

ループ

コントローラは、目標値から測定角度を引いた誤差eを受け取り、電圧を出力します。

u = kp * e  +  ki * integral(e dt)  +  kd * de/dt

各項には役割があります。比例項は、関節が目標から離れているほど強く押します。積分項は過去の誤差を蓄積するので、小さな誤差でも続けばやがて大きな補正を生みます。微分項は誤差の変化の速さに反応し、オーバーシュートする前に動きを減衰させます。1 kHzの離散時間では、積分はe * DTの累積和になり、微分は連続する2サンプルの差をDTで割ったものになります。

以下のsimulate関数は、このループを、アンプの電圧制限、0.3 N mの一定負荷トルク、後の節で使う2つのスイッチ（アンチワインドアップの有無と、微分をどこで取るか）とともに実装します。続いて、3組のゲインで1 radのステップを実行します。

import numpy as np

J, B, KT, KE, R_ARM = 0.01, 0.01, 0.5, 0.5, 2.0   # joint model from the previous block
DT = 0.001          # controller and integrator step, s (1 kHz)
V_MAX = 12.0        # supply limit, V
TAU_LOAD = 0.3      # constant load torque, N m (for example gravity on a held link)

def simulate(kp: float, ki: float, kd: float, setpoint: float = 1.0, t_end: float = 1.2,
             anti_windup: bool = False, d_on_measurement: bool = True):
    """Closed-loop step response. Returns time, angle and commanded voltage arrays."""
    n = int(round(t_end / DT))
    theta = omega = integral = 0.0
    prev_theta, prev_err = 0.0, 0.0          # before t = 0 the setpoint and the angle were both 0
    t_log, th_log, u_log = [], [], []
    for k in range(n + 1):
        err = setpoint - theta
        if d_on_measurement:
            deriv = -(theta - prev_theta) / DT   # derivative of the measured angle, sign flipped
        else:
            deriv = (err - prev_err) / DT        # derivative of the error
        u_cmd = kp * err + ki * integral + kd * deriv
        u = min(max(u_cmd, -V_MAX), V_MAX)       # the amplifier saturates
        if not anti_windup or u == u_cmd:        # clamping: freeze the integral while saturated
            integral += err * DT
        t_log.append(k * DT)
        th_log.append(theta)
        u_log.append(u_cmd)
        prev_theta, prev_err = theta, err
        current = (u - KE * omega) / R_ARM
        alpha = (KT * current - B * omega - TAU_LOAD) / J
        omega += alpha * DT                      # semi-implicit Euler: velocity first,
        theta += omega * DT                      # then position with the new velocity
    return np.array(t_log), np.array(th_log), np.array(u_log)

def step_metrics(t, th, setpoint=1.0, band=0.02):
    overshoot = max(0.0, (th.max() - setpoint) / setpoint * 100)
    outside = np.where(np.abs(th - setpoint) > band * abs(setpoint))[0]
    settled = outside[-1] + 1 < len(t)
    t_settle = t[outside[-1] + 1] if settled else None
    return overshoot, th[-1], t_settle

gains = {"P": (10.0, 0.0, 0.0), "PI": (10.0, 20.0, 0.0), "PID": (10.0, 10.0, 0.4)}
results = {}
for name, (kp, ki, kd) in gains.items():
    t, th, u = simulate(kp, ki, kd)
    results[name] = (t, th)
    over, final, ts = step_metrics(t, th)
    ts_text = f"{ts:.3f} s" if ts is not None else "never (outside 2 % band)"
    print(f"{name:3s} kp={kp:4.1f} ki={ki:4.1f} kd={kd:3.1f}  overshoot {over:5.1f} %  "
          f"final {final:.3f} rad  2 % settling {ts_text}  peak |u| {np.abs(u).max():.1f} V")

# Coarse samples for the chart: every 40 ms
for name, (t, th) in results.items():
    pts = [[round(float(t[i]), 2), round(float(th[i]), 3)] for i in range(0, len(t), 40)]
    print(name, len(pts), pts)

P   kp=10.0 ki= 0.0 kd=0.0  overshoot   7.8 %  final 0.880 rad  2 % settling never (outside 2 % band)  peak |u| 10.0 V
PI  kp=10.0 ki=20.0 kd=0.0  overshoot  25.6 %  final 0.999 rad  2 % settling 0.569 s  peak |u| 10.1 V
PID kp=10.0 ki=10.0 kd=0.4  overshoot   0.9 %  final 0.993 rad  2 % settling 0.243 s  peak |u| 10.0 V
P 31 [[0.0, 0.0], [0.04, 0.147], [0.08, 0.453], ...]
PI 31 [[0.0, 0.0], [0.04, 0.152], [0.08, 0.482], ...]
PID 31 [[0.0, 0.0], [0.04, 0.133], [0.08, 0.384], ...]

最後の3行はここでは省略しています。下のグラフでは、それぞれ31サンプルすべてを使っています。

P: 速いが目標に届かない

kpを10 V/radにすると、関節はすばやく立ち上がり、1.078 radまでオーバーシュートしてから0.880 radで整定します。目標値には到達しません。原因は負荷です。0.3 N mに抗してリンクを静止させるには、モータに0.3 / 0.5 = 0.6 Aの電流が必要です。2 Ωの巻線にこの電流を流すには1.2 Vが必要です。比例コントローラは誤差があるときにしか電圧を出さないので、kp * eが1.2 Vに等しくなる点、つまり誤差0.12 radで整定します。kpを2倍にすればこの誤差は半分になります。しかし応答はより振動的になり、実機ではセンサノイズを増幅し、減速機の共振を励起します。比例ゲインだけでは一定の外乱を取り除けません。

振動にも簡単な説明があります。飽和を無視すると、閉ループは2次系です。固有角周波数はsqrt(25 * kp)で、約15.8 rad/sです。ここで25はKT / (R * J)です。減衰比は13.5をその周波数の2倍で割った値で、0.43です。ここで13.5は(B + KT * KE / R) / Jです。この減衰比から予測される最終値に対するオーバーシュートは22.7%です。シミュレーションでは(1.078 - 0.880) / 0.880、つまり22.5%です。

PI: 定常偏差はゼロ、ただしオーバーシュートは増える

kiを20 V/(rad s)として積分項を加えると、オフセットはなくなります。誤差が残っている間は積分が増え続け、誤差がゼロになって初めて増加が止まります。その時点で、負荷が必要とする1.2 Vを積分項だけで供給します。最終角度は0.999 radです。

代償はオーバーシュートで、25.6%です。立ち上がりの間、誤差は長時間にわたって大きな正の値をとるので、積分に電圧が蓄えられます。関節が目標値に達した時点でもこの蓄えが押し続け、負の誤差で吐き出されるまで関節はオーバーシュートします。積分動作は位相遅れを加え、位相遅れは安定余裕を削ります。

PID: 減衰を加える

微分項は動きに逆らいます。kdを0.4 V s/radにすると、関節速度1 rad/sごとに指令から0.4 Vが差し引かれます。これは、コントローラの中にだけ存在する粘性摩擦を追加したのと同じ働きをします。この減衰があるのでkiを10に下げることもでき、PIでオーバーシュートの原因になった蓄積電圧を減らせます。結果として、0.9%のオーバーシュートで、0.243 sで2%の範囲に入ります。

0.5 s以降のPIDの波形をよく見てください。0.986 radまで下がってから少しずつ戻り、1.2 sで0.993 radに達します。kiが低いため、積分が最後の数百mVを蓄えるのに時間がかかり、最後の1%がゆっくりになります。チューニングはすべてこの種のトレードオフです。ここでは、小さいオーバーシュートと遅いすそ野を交換しています。PIDチューニング早見表に、3つのゲインを決める体系的な手法をまとめています。

積分ワインドアップ

次に、同じPIDゲインで4 radという大きな移動を指令します。開始時、比例項だけで40 Vを要求しますが、アンプが出せるのは12 Vです。移動の大部分で出力は上限に張り付き、コントローラには操作の余地がありません。それでも積分は、あたかも操作できているかのように誤差を積み上げ続けます。関節がようやく目標に近づいたとき、積分にはもはや不要な大きな電圧が蓄えられており、それが関節を目標値の先まで押し出します。これが積分ワインドアップです。

最も簡単な対策はクランプで、条件付き積分とも呼ばれます。出力が飽和している間は積分を止めます。simulateでは、if not anti_windup or u == u_cmdの行がこれにあたります。以下のブロックは前のブロックのモデルとコントローラを再利用し、4 radの移動を両方の方式で実行します。続いて、微分を測定値ではなく誤差に対して取った場合の1 radステップを実行します。

import numpy as np

J, B, KT, KE, R_ARM = 0.01, 0.01, 0.5, 0.5, 2.0   # joint model from the first block
DT = 0.001          # controller and integrator step, s (1 kHz)
V_MAX = 12.0        # supply limit, V
TAU_LOAD = 0.3      # constant load torque, N m (for example gravity on a held link)

def simulate(kp: float, ki: float, kd: float, setpoint: float = 1.0, t_end: float = 1.2,
             anti_windup: bool = False, d_on_measurement: bool = True):
    """Closed-loop step response. Returns time, angle and commanded voltage arrays."""
    n = int(round(t_end / DT))
    theta = omega = integral = 0.0
    prev_theta, prev_err = 0.0, 0.0          # before t = 0 the setpoint and the angle were both 0
    t_log, th_log, u_log = [], [], []
    for k in range(n + 1):
        err = setpoint - theta
        if d_on_measurement:
            deriv = -(theta - prev_theta) / DT   # derivative of the measured angle, sign flipped
        else:
            deriv = (err - prev_err) / DT        # derivative of the error
        u_cmd = kp * err + ki * integral + kd * deriv
        u = min(max(u_cmd, -V_MAX), V_MAX)       # the amplifier saturates
        if not anti_windup or u == u_cmd:        # clamping: freeze the integral while saturated
            integral += err * DT
        t_log.append(k * DT)
        th_log.append(theta)
        u_log.append(u_cmd)
        prev_theta, prev_err = theta, err
        current = (u - KE * omega) / R_ARM
        alpha = (KT * current - B * omega - TAU_LOAD) / J
        omega += alpha * DT                      # semi-implicit Euler: velocity first,
        theta += omega * DT                      # then position with the new velocity
    return np.array(t_log), np.array(th_log), np.array(u_log)

def step_metrics(t, th, setpoint=1.0, band=0.02):
    overshoot = max(0.0, (th.max() - setpoint) / setpoint * 100)
    outside = np.where(np.abs(th - setpoint) > band * abs(setpoint))[0]
    settled = outside[-1] + 1 < len(t)
    t_settle = t[outside[-1] + 1] if settled else None
    return overshoot, th[-1], t_settle

# Integral windup: a 4 rad move saturates the amplifier for a long time
for aw in (False, True):
    t, th, u = simulate(10.0, 10.0, 0.4, setpoint=4.0, t_end=3.0, anti_windup=aw)
    over, final, ts = step_metrics(t, th, setpoint=4.0)
    ts_text = f"{ts:.2f} s" if ts is not None else "not settled"
    print(f"anti-windup {str(aw):5s}  overshoot {over:5.1f} %  2 % settling {ts_text}  final {final:.3f} rad")

# Derivative kick: the same PID with the derivative on the error instead of the measurement
for dom in (True, False):
    t, th, u = simulate(10.0, 10.0, 0.4, d_on_measurement=dom)
    over, final, ts = step_metrics(t, th)
    label = "measurement" if dom else "error      "
    print(f"D on {label}  first commanded voltage {u[0]:7.1f} V  overshoot {over:4.1f} %")

anti-windup False  overshoot  14.7 %  2 % settling 2.09 s  final 4.029 rad
anti-windup True   overshoot   1.7 %  2 % settling 0.33 s  final 4.000 rad
D on measurement  first commanded voltage    10.0 V  overshoot  0.9 %
D on error        first commanded voltage   410.0 V  overshoot  0.9 %

アンチワインドアップなしでは、4 radの移動は14.7%（0.6 rad近く）オーバーシュートし、整定に2.09 sかかります。余分な積分が抜けきるまでの間、3 sの時点でもまだ0.029 radずれています。クランプを使うと、同じゲインでオーバーシュートは1.7%、整定時間は0.33 sになります。ゲインは変わっていません。変わったのは、飽和中の積分器の振る舞いだけです。

クランプは数ある方式の1つです。バックカリキュレーション方式は、指令出力と実際に出た出力の差を積分器に戻し、制御された速さで積分を巻き戻します。どの方式を使うにしても、出力が飽和しうるコントローラには必ずアンチワインドアップが必要です。関節コントローラは大きな移動のたびに飽和します。制限は電源電圧とは限りません。電流制限、安全のために設定したトルク制限、ドライブ内の速度制限も、すべて同じ影響を生みます。

測定値に対する微分

誤差の微分は、目標値の微分から測定値の微分を引いたものです。目標値が跳ぶと、1 msの1サンプルにわたるその微分は非常に大きくなります。ここでは目標値が1ステップで0から1 radに変わるので、kd * de/dtは0.4 × 1000 = 400 Vとなり、比例項の10 Vに上乗せされます。これが微分キックです。シミュレーションではアンプが1サンプルの間それを12 Vに制限するので、応答はほとんど変わりません。実機では目標値が変わるたびに電流スパイクが生じ、減速機とドライブに負担をかけます。

代わりに測定角度の微分を符号を反転して使うと、ステップとステップの間は目標値が一定なので、動作中は同じ減衰が得られ、しかもキックは生じません。上のコードがd_on_measurement=Trueを既定値にしているのはこのためです。最初の指令電圧は410 Vから10 Vに下がります。実際には、量子化されたエンコーダ信号を微分するとノイズが増幅されるため、微分にはローパスフィルタもかけます。レッスン06でノイズを詳しく扱います。

サーボドライブでは、PIDをカスケードの1層として使うのが一般的です。位置ループの内側に速度ループ、その内側に電流（トルク）ループがあり、内側のループほど高速です。重力補償のようなモデルベースの項はフィードフォワードとして加え、積分項の負担を減らします。制御ハブとアクチュエータ・ドライブハブで、こうした構造を扱います。レッスン09では、MuJoCoでシミュレーションしたアームに対し、重力補償ありとなしでPDループを閉じます。

演習

1. TAU_LOADをゼロにしてPコントローラを再実行してください。定常偏差はいくつになりますか。それはなぜですか。次に負荷を2倍の0.6 N mにし、実行する前にPの偏差を予測してください。
2. PIのゲインのままkpを30に上げてください。オーバーシュートはどう変わり、指令電圧のピークはいくつになりますか。飽和は線形解析にどんな影響を与えたでしょうか。
3. バックカリキュレーション方式のアンチワインドアップを実装してください。uを計算した後、k_awを約5として、(u - u_cmd) * k_aw * DTを積分に加えます。4 radの応答をクランプ方式と比較してください。
4. 測定角度に標準偏差0.001 radのガウスノイズを加えてください（シードはnp.random.default_rng(0)）。kdが0.4のときの定常状態における指令電圧の標準偏差を表示し、次に微分に1次ローパスフィルタを加えて同じことを繰り返してください。

まとめ

• DCモータ関節は、電圧から速度までが1次系、電圧から角度までが2次系です。1 msの半陰的オイラー法のステップで、解析的な時定数を再現できます。
• 一定負荷のもとでP制御は、保持電圧をkpで割った定常偏差（ここでは0.12 rad）を残します。積分項はオーバーシュートと引き換えにこれを取り除きます。
• 微分項は減衰を加えます。PIDでは、シミュレーション関節は0.9%のオーバーシュートで1 radに達し、0.24 sで2%以内に整定しました。
• 出力が飽和すると積分はワインドアップします。クランプにより、4 radの移動のオーバーシュートは14.7%から1.7%に減りました。
• 目標値のステップごとのキックを避けるには微分を測定値に対して取り、センサノイズ対策としてフィルタをかけます。

さらに読む

• Åström and Murray, 「Feedback Systems: An Introduction for Scientists and Engineers」, 2nd edition, Princeton University Press, 2021：第11章「PID Control」で、チューニング、積分器のワインドアップ、実装を扱っています。著者らは全文を書籍のサイトで無料公開しています。
• Lynch and Park, 「Modern Robotics: Mechanics, Planning, and Control」, Cambridge University Press, 2017：第11章「Robot Control」で、速度入力・トルク入力によるロボット関節の誤差ダイナミクスと運動制御を扱っています。
• Siciliano, Sciavicco, Villani and Oriolo, 「Robotics: Modelling, Planning and Control」, Springer, 2009：第8章「Motion Control」。1つの関節からマニピュレータ全体へ進む段階で参照してください。