2025.10.10
Unitree G1のモータ制御に積分項を追加する
こんにちは，S.T.です。
ヒューマノイドを腕を使って実際のタスクを遂行する場合，それぞれの関節角度を正確に制御できることが大前提です。Unitree G1のモータは角度目標値を指定して制御することができますが，残念ながら角度を指定するだけでは正確に動作しません。今回は，少しでも正確性を高めるための工夫を施してみます。
1.Unitree G1のモータ
Unitree G1のモータは，プログラムから以下の式の右辺のパラメータを設定することで動作します（引用：Unitree Motor SDK）。
\[ \tau = tau + k_p (q_\mathrm{des} – q) + k_d (dq_\mathrm{des} – dq) \]
設定できるパラメータは，定数項（tau），目標角度（q des），目標角度と現在角度の差にかかる係数（Kp），目標角速度（dq des），目標角速度と現在角速度の差にかかる係数（Kd）です。これは，目標角度と，目標角速度（角度の微分）であり，比例制御と微分制御を組み合わせたPD制御となっています。
2.Unitree G1の問題点
G1のモータのPD制御には，精度に課題があります。例えば，右腕を前方に90度回転させる（R_SHOULDER_PITCHを-90度にする）例を見てみましょう。今回は，目標値を設定すると，その値まで1秒で到達するような中間的な目標値を細かく計算・送信するプログラムを作成して実験しています。角度指定は500Hzで送信しています。
この写真の状態で安定します。このときのR_SHOULDER_PITCHの角度をログに出力して確認してみましょう。まずは実機の様子とグラフを見てみます。
肘を伸ばした姿勢のケースも見てみます。今回は，肘を伸ばすために，R_ELBOWを90度に設定しています。
このように，実際に指定した角度とは若干ズレた位置で安定してしまいました。これは，先述の式で計算されたモータのトルクと，重力により発生するトルクが釣り合ってしまっているためです。純正のPD制御のみでは，この問題への対処は難しいため，定数項として任意のトルクを与えることができる機能を利用して，これを補正したいと思います。
3.積分項の追加
本来であれば，アームの各リンクの重量を測定して，アームの姿勢・重量から重力の影響を打ち消すためのトルクを計算して，それを加えてあげる，というのがセオリーです。しかし，Unitree G1の各リンクの重量は公開されていないため，今回はPD制御に積分項を追加して，PID制御にすることで，誤差を小さくできるかを検証します。なお，今回，PID制御の詳細については省きます。
積分項は，目標角度と現在角度の誤差を積算し，その積算値に係数をかけたものです。今回，G1のlowstateトピックをサブスクライブし，データを受け取る度に積分値を更新していきます。また，腕を大きく動かす場合や，外部から力が加わった場合，積分値が飽和してしまうため，簡易的なアンチワインドアップとして目標角度との差が30度になるまでは積分値の加算を停止しています。積分項の係数は適当な数値を様々試した上で，0.08としました。
積分制御を追加した結果がこちらです。比較のためにbefore-afterを並べてみましょう。
一方で，数十秒程度観察していると，角度が小さく変化する動きが見られました。これは，誤差が小さくなると積分項の値が小さくなり，トルクが不足して再度誤差が開く，その後再度積分項が大きくなって再度目標値に近づく，という動作を繰り返しているためです。
この問題を解決するためには，重力補償を厳密に行い，その上で生まれた誤差を積分項で補正する，という処理が必要になります。
4.まとめ
今回は，Unitree G1のモータの制御に積分項を追加して，目標角度との誤差を小さくする実験を行いました。積分項を追加することで，重力の影響を受けて目標角度に到達できない問題に対処できることを確認できました。一方で，小さく振動する問題は残っており，精密なタスクの遂行には正確な重力補償など別のアプローチが必要なことも確認できました。
次世代システム研究室では，グループ全体のインテグレーションを支援してくれるアーキテクトを募集しています。アプリケーション開発者の方，次世代システム研究室にご興味を持って頂ける方がいらっしゃいましたら，ぜひ募集職種一覧からご応募をお願いします。
グループ研究開発本部の最新情報をTwitterで配信中です。ぜひフォローください。
Follow @GMO_RD
