いよいよ実際に実機を動かしていきます。
エンコーダーて計測した関節角情報とPCから送られてきた目標角度を基にモーターに入力する電圧を決める部分です。
今回は以下のような制御速を採用しました。
PD制御 + 逆起電力補償（速度フィードフォワード） + 摩擦補償（摩擦一定）
$$
u =
-K_p (q-q_d)
-K_d (\dot{q}-\dot{q}_d)
+K_{ff} v_d
+K_{fr} sgn(v_d)
$$
u は入力電圧
K_p,K_d,K_{ff},K_{fr} はゲイン行列 (対角)
q は関節角、 q_d は目標関節角
特徴的なのは逆起電力補償（速度フィードフォワード）の部分です。
式の K_{ff} v_d に該当します。
v_d はモーターの目標角速度で、
今回の機体ではモーターの速度と関節の速度は同じにはなりません。
機構編で紹介があったようにワイヤー駆動による干渉があるからです。
—
実際に駆動したい関節より前の関節全てにワイヤーを中継しているため、動作の干渉という問題が生じます。
例えば下のGIF画像では一番根本のピッチ軸を手で動かしているのですが、その次の軸もそれにつられるように回転してしまっています。今回は全てのプーリー直径を等しくしたため、平行リンクと同様な動きです。
—
引用元：機構編
どういうことが起きているかというと、動かしたい関節の次の関節も同じだけ逆方向に回してしまうという状況です。
これが一番根本のyaw軸以外で起きています。
これを式で表すと、関節角速度 \dot{q} とモーター角速度 v の関係は以下のようになります。
$$
\dot{q} = B v
$$
$$
B =
\begin{bmatrix}
1 & 0 & 0 & 0 & 0 \\
0 & 1 & 0 & 0 & 0 \\
0 & -1 & 1 & 0 & 0 \\
0 & 0 & -1 & 1 & 0 \\
0 & 0 & 0 & -1 & 1
\end{bmatrix}
$$
q , v は関節を根本の方から順に上から並べたベクトルです。
B 行列が対角であれば干渉はないということですが、見てわかるように-1の要素があり、これが干渉を表しています。
今回制御では関節の目標角速度が与えられ、モーターの出すべき速度を求めたいので上の式の逆の関係を用います。
$$
v = B^{-1} \dot{q}
$$
$$
B^{-1} =
\begin{bmatrix}
1 & 0 & 0 & 0 & 0 \\
0 & 1 & 0 & 0 & 0 \\
0 & 1 & 1 & 0 & 0 \\
0 & 1 & 1 & 1 & 0 \\
0 & 1 & 1 & 1 & 1
\end{bmatrix}
$$
これを見ると先端に行くほど前の関節の影響を受けてしまうというのがわかります。
以上がモーター制御の解説でした。動きは最後の動画をご覧ください。