SO-101のモーター制御を深掘りして調整する 〜SO-101は指示通りの位置へ移動しているのか？〜

TL;DR

• SO-101はAI（VLAなどの基盤モデルやACTなどの模倣学習）の指示からモーター内部の制御によって指定した位置に移動している。
  • 指示通りの位置へ移動できることは外乱、構造、制御の特性の影響があるので当たり前ではない。
• SO-101はAIが指示した位置にアームが完全に移動しきれていないことがある。
  • 現在のモーター制御は実質的なPD制御（I=0）であり、アームを伸ばした時など重力下で定常偏差が発生する。
  • 解決策として、特に重力の影響が大きいモーターにIゲイン（積分要素）を導入し、モーターごとの負荷に合わせたPID制御の最適化を検討した。
• LeRobotのコードはSTS3215の使い方として疑問な部分がある。
  • record.pyなどを実行するたびに、EEPROMへの書き込みが発生している。
  • read onlyなはずのアドレスへの書き込みがある。
  • サーボモーターの加速度・速度プロファイルについて、設定すれば台形速度プロファイルが実現できるが、現在の設定では目標速度に達する前に減速が始まってしまい、速度プロファイルが三角形状になる。これにより、動作の開始時と停止時に急な躍度（ジャーク）発生の可能性がある。
• モーター制御をいじるかどうかは目的しだい。

はじめに

こんにちは。ABEJAの道辻です。ABEJAではプロジェクトマネージメントを中心に色々やっておりますが、前職までは15年くらい新商品開発、新規開発をおこなう部署で機械設計、研究をしていたという経歴です。

2年前に記事を書いてから久しぶりに書きますが、AIとロボットがより近づいてきました。 tech-blog.abeja.asia

ロボットアームそのものを作ったことはありませんが、モーターを使ったシステムや制御設計などもしていたので、その視点でSO-101について検証しようと思います。 LeRobotのハッカソンに参加させてもらって、実際にACTで模倣学習を行いましたが、カクつくような動きが見られたため、PIDのゲインを調整しています。 様々なAIモデルを試すとともに、モーター側の制御についても理解を深めておいた方がいいのではないかと考えています。

この記事では、SO-101の制御システムを整理し、PIDの調整結果について紹介しようと思います。

SO-101のモーター制御概要

SO-101の制御は、Pythonスクリプトから、モーター（STS3215）内部のコントローラーへと、複数の階層を経て実行されます。

AI（VLAなどの基盤モデルやACTなどの模倣学習）の出力を受け取り、{'shoulder_pan.pos': 30}のようなPythonの辞書（action）に変換します。 actionを受け取り、正規化された値（-100～100など）を、モーター固有の動作範囲（例：500～3500）にマッピングします。 例えば、action=30という指示は2450というモーター固有の値に変換されます。 全6モーターへの指示値をSYNC_WRITEプロトコルで一つのパケットにまとめ、シリアル通信で一括送信します。 そのパケットを受け取り、モーター内部のPIDコントローラーが目標位置と現在位置の差を計算し、PWMでモーターを駆動します。

なので、厳密には今使われているAIはEnd to Endではないと考えています。計算時間のかかるAIの計算とその目標ポジションへ移動させる制御が別々になっているのは、制御周期的に理にかなっているのかなと思いますがこの辺りも今後工夫されるポイントだと予想しています。

SO-101のコードからEEPROMとSRAMに何を書き込んでいるか

STS3215サーボモーターには、特性の異なる2つのメモリが搭載されています。

• EEPROM: 電源を切っても内容が保持される不揮発性メモリです。PIDゲインやIDなど、頻繁には変更しない定数的なパラメータの書き込みに使用されます。ただし、書き込み回数に10万回などの上限があるため、頻繁な書き換えは避けるべきです。意図は分かりませんが現在のrecord.pyなどでは実行のたびに書き込みが行われています。10万回レベルまで行くことはないと思いますが、読み取って書き込もうとしてる値が同じだったら書き込み処理を行わないなどは最低限やってもいいのかと思います。
• SRAM: 電源が切れると内容が消える揮発性メモリです。目標位置（ゴールポジション）など、動作中にリアルタイムで更新する動作指示の書き込みに使用されます。

基本的にはSRAMのTarget PositionにAIからの指示が書き込まれて、Current Positionとの差分を使って、EEPROMに書き込まれているゲインでPID制御がされています。

STS3215のメモリーテーブル

DEC	HEX	デバッグツールの表示名	Area	R/W	説明
0	0x0	Firmware Main Version	EPROM	r	ファームウェアのメジャーバージョン番号。
1	0x1	Firmware Secondary Ver.	EPROM	r	ファームウェアのマイナーバージョン番号。
2	0x2	Servo Main Version	EPROM	r	サーボハードウェアのメジャーバージョン番号。
3	0x3	Servo Sub Version	EPROM	d	サーボハードウェアのマイナーバージョン番号。
5	0x5	ID	EPROM	rw	バスのユニークな識別ID。同一バス上での重複は不可。ID 254 (0xFE) はブロードキャストIDで、応答はありません。
6	0x6	Baud Rate	EPROM	rw	0～7でボーレートを指定: 1000000, 500000, 250000, 128000, 115200, 76800, 57600, 38400
7	0x7	Reserved	EPROM	-	応答遅延時間。最小単位は2µs。最大 254 * 2 = 508µs。
8	0x8	Status Return Level	EPROM	rw	0: Read/Ping命令以外は応答しない。 1: 全ての命令に応答する。
9	0x9	Min Position Limit	EPROM	rw	動作角度の最小値。マルチターンモードでは 0 を設定。
11	0xB	Max Position Limit	EPROM	rw	動作角度の最大値。マルチターンモードでは 0 を設定。
13	0xD	Max Temperature Limit	EPROM	rw	内部温度の上限。設定精度は1℃。
14	0xE	Max Input Voltage	EPROM	rw	入力電圧の上限。80に設定すると8.0V。設定精度は0.1V。
15	0xF	Min Input Voltage	EPROM	rw	入力電圧の下限。40に設定すると4.0V。設定精度は0.1V。
16	0x10	Max Torque Limit	EPROM	rw	サーボの最大出力トルク。1000 = 100%ストールトルク。起動時にこの値がTorque Limit (0x30)にコピーされます。
18	0x12	Setting Byte	EPROM	rw	特殊機能用。
19	0x13	Protection Switch	EPROM	rw	各種保護機能でトルクをオフにする条件を設定。対応するビットを1で有効化。 Bit5:過負荷, Bit4:角度, Bit3:電流, Bit2:温度, Bit1:センサー, Bit0:電圧
20	0x14	LED Alarm Condition	EPROM	rw	エラー発生時にLEDを点滅させる条件を設定。対応するビットを1で有効化。 Bit5:過負荷, Bit4:角度, Bit3:電流, Bit2:温度, Bit1:センサー, Bit0:電圧
21	0x15	Position P Gain	EPROM	rw	位置制御ループのP(比例)ゲイン。
22	0x16	Position D Gain	EPROM	rw	位置制御ループのD(微分)ゲイン。
23	0x17	Position I Gain	EPROM	rw	位置制御ループのI(積分)ゲイン。
24	0x18	Punch	EPROM	rw	最小起動トルク。1000 = 100%ストールトルク。
25	0x19	MAX I	EPROM	rw	最大電流。
26	0x1A	CW Dead Band	EPROM	rw	時計回り方向の不感帯（デッドゾーン）。
27	0x1B	CCW Dead Band	EPROM	rw	反時計回り方向の不感帯（デッドゾーン）。
28	0x1C	Overload Current	EPROM	rw	過電流保護の閾値。最大 500 * 6.5mA = 3250mA。
30	0x1E	Angular Resolution	EPROM	rw	角度の分解能。この値を変更すると制御回転数を拡張できます。マルチターン制御時は Phase(0x12) の BIT4 を 1 に設定する必要があります。
31	0x1F	Position Offset Value	EPROM	rw	現在位置のオフセット補正値。BIT11が正負の方向を示します。
33	0x21	Work Mode	EPROM	rw	0: 位置サーボモード 1: 定速モーターモード 2: PWM開ループモード 3: ステッピングサーボモード
34	0x22	Protect Torque	EPROM	rw	過負荷保護中に維持するトルク。20に設定すると最大トルクの20%。
35	0x23	Overload Protection Time	EPROM	rw	過負荷状態が継続した場合に保護機能が作動するまでの時間。200に設定すると2秒。
36	0x24	Overload Torque	EPROM	rw	過負荷保護タイマーを開始するトルクの閾値。80に設定すると最大トルクの80%。
37	0x25	Velocity P Gain	EPROM	rw	速度制御ループのP(比例)ゲイン（モード1で使用）。
38	0x26	Overcurrent Protection	EPROM	rw	過電流保護が作動するまでの時間。最大 2540ms。
39	0x27	Velocity I Gain	EPROM	rw	速度制御ループのI(積分)ゲイン（モード1で使用）。
40	0x28	Torque Enable	SRAM	rw	0: トルクOFF, 1: トルクON, 128: 現在位置を2048として補正。
41	0x29	Goal Acceleration	SRAM	rw	加減速度。10に設定すると1000 step/s²。
42	0x2A	Goal Position	SRAM	rw	目標位置。
44	0x2C	Goal PWM	SRAM	rw	PWMモードでの目標時間/デューティ比。範囲は50～1000。BIT10は方向ビット。
46	0x2E	Goal Velocity	SRAM	rw	目標速度。50 step/s = 0.732 RPM。
48	0x30	Torque Limit	SRAM	rw	リアルタイムのトルク制限値。起動時にMax Torque (0x10)の値が設定されます。
55	0x37	Lock	SRAM	rw	EPROMへの書き込みロック。0: ロック解除（設定値は電源OFF後も保存） 1: ロック（設定値は電源OFF後に破棄）
56	0x38	Present Position	SRAM	r	現在の位置。
58	0x3A	Present Velocity	SRAM	r	現在の速度。
60	0x3C	Present PWM	SRAM	r	現在の負荷。モーター駆動のデューティ比に相当。
62	0x3E	Present Input Voltage	SRAM	r	現在のサーボ入力電圧。
63	0x3F	Present Temperature	SRAM	r	現在のサーボ内部温度。
64	0x40	Sync Write Flag	SRAM	r	非同期書き込み命令使用時のフラグ。
65	0x41	Hardware Error Status	SRAM	r	エラー状態のフィードバック。対応するビットが1でエラー発生。 Bit5:過負荷, Bit4:角度, Bit3:電流, Bit2:温度, Bit1:センサー, Bit0:電圧
66	0x42	Moving Status	SRAM	r	1: 動作中, 0: 停止中。
69	0x45	Present Current	SRAM	r	現在の消費電流。
71	0x47	Goal Position 2	SRAM	r
80	0x50	Moving Threshold	DEFAULT	d
81	0x51	DTs(ms)	DEFAULT	d
82	0x52	Vk(ms)	DEFAULT	d
83	0x53	Vmin	DEFAULT	d
84	0x54	Vmax	DEFAULT	d
85	0x55	Amax	DEFAULT	d
86	0x56	KAcc	DEFAULT	d

tables.pyとデバッグツールの表示名の対応

/src/lerobot/motors/feetech/tables.pyにあるSTSのマップ名称とデバッグツールの表示名の対応を整理しています。

DEC	HEX	tables.py	デバッグツールの表示名
0	0x0	Firmware_Major_Version	Firmware Main Version
1	0x1	Firmware_Minor_Version	Firmware Secondary Ver.
2	0x2	Model_Number	Servo Main Version
3	0x3		Servo Sub Version
5	0x5	ID	ID
6	0x6	Baud_Rate	Baud Rate
7	0x7	Return_Delay_Time	Reserved
8	0x8	Response_Status_Level	Status Return Level
9	0x9	Min_Position_Limit	Min Position Limit
11	0xB	Max_Position_Limit	Max Position Limit
13	0xD	Max_Temperature_Limit	Max Temperature Limit
14	0xE	Max_Voltage_Limit	Max Input Voltage
15	0xF	Min_Voltage_Limit	Min Input Voltage
16	0x10	Max_Torque_Limit	Max Torque Limit
18	0x12	Phase	Setting Byte
19	0x13	Unloading_Condition	Protection Switch
20	0x14	LED_Alarm_Condition	LED Alarm Condition
21	0x15	P_Coefficient	Position P Gain
22	0x16	D_Coefficient	Position D Gain
23	0x17	I_Coefficient	Position I Gain
24	0x18	Minimum_Startup_Force	Punch
25	0x19		MAX I
26	0x1A	CW_Dead_Zone	CW Dead Band
27	0x1B	CCW_Dead_Zone	CCW Dead Band
28	0x1C	Protection_Current	Overload Current
30	0x1E	Angular_Resolution	Angular Resolution
31	0x1F	Homing_Offset	Position Offset Value
33	0x21	Operating_Mode	Work Mode
34	0x22	Protective_Torque	Protect Torque
35	0x23	Protection_Time	Overload Protection Time
36	0x24	Overload_Torque	Overload Torque
37	0x25	Velocity_closed_loop_ P_proportional_coefficient	Velocity P Gain
38	0x26	Over_Current_ Protection_Time	Overcurrent Protection
39	0x27	Velocity_closed_loop_ I_integral_coefficient	Velocity I Gain
40	0x28	Torque_Enable	Torque Enable
41	0x29	Acceleration	Goal Acceleration
42	0x2A	Goal_Position	Goal Position
44	0x2C	Goal_Time	Goal PWM
46	0x2E	Goal_Velocity	Goal Velocity
48	0x30	Torque_Limit	Torque Limit
55	0x37	Lock	Lock
56	0x38	Present_Position	Present Position
58	0x3A	Present_Velocity	Present Velocity
60	0x3C	Present_Load	Present PWM
62	0x3E	Present_Voltage	Present Input Voltage
63	0x3F	Present_Temperature	Present Temperature
64	0x40		Sync Write Flag
65	0x41	Status	Hardware Error Status
66	0x42	Moving	Moving Status
69	0x45	Present_Current	Present Current
71	0x47	Goal_Position_2	Goal Position 2
80	0x50	Moving_Velocity / Moving_Velocity_Threshold	Moving Threshold
81	0x51	DTs	DTs(ms)
82	0x52	Velocity_Unit_factor	Vk(ms)
83	0x53	Hts	Vmin
84	0x54	Maximum_Velocity_Limit	Vmax
85	0x55	Maximum_Acceleration	Amax
86	0x56	Acceleration_Multiplier	KAcc

PID制御の最適化

SO-101はアームを水平に伸ばした状態でモーター2へ上方向に移動させるよう指示しても大きめに動かそうとしないと動きません。これが大きめの指示を誘発してガタガタと揺れるモードになる可能性があると考えました。本当は重力の影響を考慮した制御を考える方がいいかもしれないですが、CWとCCWをモーター側で変える方法が見つからなかったので、とりあえずPI制御で目的の位置に移動するようにしました。

PID制御とは？

この問題を理解し、解決するために、まずはサーボモーターで使われているPID制御について簡単に説明します。

PID制御は、目標値に素早く正確に到達させるための、最も古典的で広く使われているフィードバック制御技術です。「目標位置」と「現在位置」の偏差（ズレ）を継続的に監視し、その偏差をゼロにするようにモーターの動きを賢く調整します。

P・I・Dはそれぞれ以下の3つの異なる制御動作の頭文字を取っており、これらを組み合わせることで理想的な動きを目指します。

• P (Proportional) - 比例制御

  • 役割: 「現在の偏差の大きさ」に比例して動作します。目標から遠いほど、大きな力で動かそうとします。ロボットアームを動かすための最も基本的な力です。

  • 課題: これだけだと、重力などの持続的な負荷に負けて、目標位置にわずかに届かない「定常偏差」が残ることがあります。

• I (Integral) - 積分制御

  • 役割: 「過去からの偏差の蓄積」に比例して動作します。時間が経っても残っている定常偏差を解消するために、じわじわと力を追加していきます。重力で垂れ下がったアームを、目標位置まで持ち上げるような働きをします。

  • 課題: 力を加えすぎる傾向があるため、目標を通り越してしまう「オーバーシュート」が発生しやすくなります。

• D (Differential) - 微分制御

  • 役割: 「偏差の未来の変化」を予測して動作します。目標位置に勢いよく近づきすぎている場合、事前にブレーキをかけるように働き、動きを安定させます。

  • 課題: オーバーシュートや到達後の振動を抑える効果がありますが、ゲインが強すぎると動きが鈍くなることもあります。

これらP・I・Dの３つの要素の強さ（ゲイン）をうまく調整することで、ロボットアームを「速く、かつ滑らかに、そして正確に」目標位置へ動かすことが可能になります。

現状と解決方針

現在のrecord.pyでは、PID係数がP=16, I=0, D=32に設定されています。Iゲインが0であるため、これは実質的なPD制御として動作しています。

configure

この設定が、特に重力下で以下のような問題を引き起こしています。

• 定常偏差: アームが水平に伸びた状態で、肩を支えるモーター2に「少しだけ上げる」指示を出しても、アームの重さに負けて目標位置に到達できない定常偏差が発生します。
• オーバーシュート: 逆に、アームを下げる際には重力で加速し、目標を通り過ぎるオーバーシュートが発生しやすくなります。
• Punch値の課題: モーターの初期トルクを決めるPunch値が、CW（時計回り）とCCW（反時計回り）で共通のため、重力に逆らう時と従う時で最適な設定ができず、妥協が必要でした。

自動的にPIDゲインを変えながら動きを記録するコードを作成して、モーターごとの負荷に応じて最良と思われる結果を示しました。

• PID調整の方針
  • 調整法などもありますがCWとCCWで状況が違うこともあるためパラメータを振って確認しました
  • PI制御

    - P制御だけだと、定常偏差がのこる（目標値にならない）
    - Iを入れて目標到達させる
        - Iが入るとオーバーシュートしやすくなるのでP制御の時よりPを小さく
        - ただし、今回のモーターでは、Iは積分されて最小動くのが遅い仕様になっている
        - Iに1など小さい値を入れると、4,5秒経ってもまだ微妙に動くので、Iを入れるなら大きめに入れるもしくは0とした方がいい
        - オーバーシュートしないなら大きめに入れてしまう
    

  • PD制御

    - P制御だけだと、目標到達後に振動する
    - Dを入れてブレーキをかける、だからP制御の時よりPを大きく
    

  • PID制御

    - 目標値到達と振動抑制が両方いる
        - Iを入れるとオーバーシュートしたり、振動しやすいのでDを入れる
        - P→I→Dと調整していくことにする
    

試験結果

現在位置からCW(Step0)→CCW(Step1)→CW(Step2)と動かして元の位置に戻る時の動きを200Hzでデータ取得しました。 ポジションはすべて中間位置に設定しています。

• shoulder_pan_motor1

  • IもDもなくて良さそうで、12と16の間で14にする

• shoulder_lift_motor2

  • Step0とStep2の下降時にオーバーシュートが大きく目標値に到達できていない、Step1の上昇時は振動している
  • Iを大きくして、目標値到達させる方が良さそう。Dも大きくして振動を抑える。

• elbow_flex_motor3

  • 2と同様だがオーバーシュート量は少なく、振動も少ないため、PIで考える。

• wrist_flex_motor4

  • P制御でも振動していない

• wrist_roll_motor5

  • P制御でも振動していない

• gripper_motor6

  • P制御でも振動していない

今回設定したPIDゲイン

Motor	P	I	D	制御方式	主な理由
1 (shoulder_pan)	14	0	0	P制御	重力影響がほぼない
2 (shoulder_lift)	32	18	32	PID制御	最も重力影響が大きく、全要素が必要
3 (elbow_flex)	24	6	0	PI制御	重力影響はあるが、振動は少ないため
4 (wrist_flex)	20	0	0	P制御	重力影響がほぼない
5 (wrist_roll)	20	0	0	P制御	重力影響がほぼない
6 (gripper)	20	0	0	P制御	重力影響がほぼない

この設定をキープするためには、record.pyなどで上書きされる部分をマスクする必要があります。 動きが急になるので、SRAMのAccelerationとVelocity値の調整を合わせたほうがいいです。

今後やりたいこと

Sim2Realも触ってみたいと思っていますが、プラントモデルにこういったモーター制御や外乱を組み込むことは重要なので、もう少し検証はしていきたいと思っています。 個人で遊んでるだけなのでいつできるかわかりませんが、今後は以下の点について改善を進めたいと考えています。

• EEPROMへの書き込み最適化: 毎回書き込むのではなく、現在の値を読み出し、変更がある場合のみ書き込むロジックを実装します。
• 加減速プロファイルの活用: SRAMのAccelerationとVelocity値を調整し、急加速・急停止を防ぐ台形制御を導入して、より滑らかな動作を目指す。現在の設定では目標速度に達する前に減速が始まってしまい、速度プロファイルが三角形状になっています。これにより、動作の開始時と停止時に急な躍度（ジャーク）が発生し、振動や『カクつく』動きの原因になる可能性があります。
• 直接PWM制御の検討: サーボ内部のコントローラーをバイパスし、PC側でフィードバックループを構成したり、重力補償や把持物体に応じた制御を検討する。ネックになるのはUSBの通信部分で100Hz程度の制御でどうにかなるかを検証する必要があります。せっかく安い構成なのでそれをできるだけ活かしていきたいです。

最終的にはこのような調整をしなくても、ロボット基盤モデルが特性を考慮してよしなに動かしてくれるようになってくるのか、パラメーターを変えてもロボット基盤モデルが勝手に対応していくれるのか、といったところは注視していこうと思います。現状のモーター制御のままでもうまく動かせるモデルが出てくるか？という意味では、改善は行わずに検証していくのも一つの方向性だと思っていますし、今すでに大量にあるデータセットとずれてくると思うので目的に応じて何をするかは考えていくべきだと思います。あくまでも今回紹介したのはすごく簡略化した構成ですし、目的やユースケースに合わせて検討してみるといいかもしれないという感じで見ていただけるとありがたいです。

We Are Hiring!

ABEJAは、テクノロジーの社会実装に取り組んでいます。 技術はもちろん、技術をどのようにして社会やビジネスに組み込んでいくかを考えるのが好きな方は、下記採用ページからエントリーください！ （新卒の方やインターンシップのエントリーもお待ちしております！）

careers.abejainc.com