[MEMO]マニピュレータの運動学制御

ちょこちょこロボット制御についてまとめるシリーズ.
今回はマニピュレータの手先位置の制御を考える．（姿勢は考えない）
とにかく未来の自分のために今の理解を平易に書く．

# 正しいかどうかわからないので，もし参考になさる方がいれば自己責任でお願いします．．．

シミュレーション動画 (2015/9/25 追記)

青いつぶつぶのかたまりみたいなのが，LeapMotionで取得した自分の手で，
その手のところにアームの手先が来るように制御しています．

システム

アーム手先の位置姿勢を $\boldsymbol{x_{ee}}$, ロボットの状態を $\boldsymbol{\theta}$ とする． $n \geq 3$ とする．

\begin{align}
\boldsymbol{x_{ee}} &= 
\begin{bmatrix}
x_{ee} & y_{ee} & z_{ee}
\end{bmatrix}^\mathrm{T}
\in \mathrm{R}^{3}
\\
\boldsymbol{\theta} &= 
\begin{bmatrix}
\theta_1 & ... & \theta_n
\end{bmatrix}^\mathrm{T}
\in \mathrm{R}^{n}
\\
\end{align}

頑張って関節間の同次変換行列を考えて，

\begin{align}
\boldsymbol{x_{ee}} &= \boldsymbol{f}(\boldsymbol{\theta}) 
\end{align}

こんなかんじに書きくだす．もうちょっと細かく書くと

\begin{align}
\begin{bmatrix}
x_{ee} \\ y_{ee} \\ z_{ee}
\end{bmatrix}
 &= 
\begin{bmatrix}
f_{x}(\boldsymbol{\theta}) \\
f_{y}(\boldsymbol{\theta}) \\
f_{z}(\boldsymbol{\theta}) 
\end{bmatrix}
\end{align}

となる．

今回のシミュレーション動画のロボットは，上の画像の左方向x軸・上方向z軸として
根本からピッチ・ヨー・ピッチ・ヨーの自由度配置をしている．
このときの具体的な式は

\begin{align}
\begin{bmatrix}
x_{ee} \\ y_{ee} \\ z_{ee}
\end{bmatrix}
 &= 
\begin{bmatrix}
l_1 sin(\theta_1) + l_2 sin(\theta_1) cos(\theta_2) + l_3 (sin(\theta_1) cos(\theta_2) cos(\theta_3) + cos(\theta_1)  sin(\theta_3)) + l_4 (sin(\theta_1) cos(\theta_2) cos(\theta_3) cos(\theta_4) + cos(\theta_1) sin(\theta_3) cos(\theta_4) - sin(\theta_1) sin(\theta_2) sin(\theta_4))
\\
-l_2 sin(\theta_2) - l_3 sin(\theta_2) cos(\theta_3) - l_4 (sin(\theta_2) cos(\theta_3) cos(\theta_4) + cos(\theta_2) sin(\theta_4))
\\
l_0 + l_1 cos(\theta_1) + l_2 cos(\theta_1) cos(\theta_2) + l_3 (cos(\theta_1) cos(\theta_2) cos(\theta_3) - sin(\theta_1) sin(\theta_3)) + l_4 (cos(\theta_1) cos(\theta_2) cos(\theta_3) cos(\theta_4) - sin(\theta_1) sin(\theta_3) cos(\theta_4) - cos(\theta_1) sin(\theta_2) sin(\theta_4))
\end{bmatrix}
\end{align}

(ただし，$l_i$はリンク長さ．）

ただひたすら大変そうということがわかる．
この式は数学的には非線形方程式というやつで，しかも今回は$n > 3$なので（自由度(＝モータ数)が制御したい変数の数(x, y, zの3つ)より多いので）冗長系というやつもついてくる．

制御

速度を考えた制御

ロボット工学の教科書的には，微分して速度の関係を線形にすることを考える（はず）．

$sin\theta$とか$cos\theta$は，微分すると$\dot{\theta}cos\theta$とか$-\dot{\theta}sin\theta$になるので，
$\dot{\theta}$的に見れば線形になる．
つまり，

\begin{align}
\dot{\boldsymbol{x_{ee}} }
 = 
\frac{\partial \boldsymbol{f}(\boldsymbol{\theta})}{\partial \boldsymbol{\theta}}
\dot{\boldsymbol{\theta}}
\end{align}

となる．$\frac{\partial \boldsymbol{f}(\boldsymbol{\theta})}{\partial \boldsymbol{\theta}}$はいわゆるヤコビ行列（以下，$J(\boldsymbol{\theta})$とする）．

線形化してしまうと，

\begin{align}
\dot{\boldsymbol{\theta}}
 = 
J(\boldsymbol{\theta})^{-1}
\dot{\boldsymbol{x_{ee}} }
\end{align}

とヤコビ行列の逆を取ってしまえば，達成したい手先の速度から，各ジョイントの角速度を求められる．
（これをつかってPD制御とか，今回は冗長系だからヤコビ行列の逆行列取れないよねという話はさておく．前置きなので）

位置だけを考えた制御

今回は，目標位置$\boldsymbol{x_{ee}^{d}}$にアーム手先$\boldsymbol{x_{ee}}$を移動させることだけを考える．
となると，制御というより単に

\begin{align}
\boldsymbol{x_{ee}^{d}} &= \boldsymbol{f}(\boldsymbol{\theta}) 
\quad \Leftrightarrow \quad
\boldsymbol{f}(\boldsymbol{\theta}) - \boldsymbol{x_{ee}^{d}} = \boldsymbol{0}
\end{align}

という非線形方程式の解を求める，という問題になる．

マニピュレータのシステムだとsin, cosが入り混じった式になっているので，なかなか普通には解けない．
ので，数値計算で解く．

非線形方程式の数値計算にはたぶんいろいろあるので，ときどきにあった方法を使って解く．
今回はニュートン法を使う．

細かいことをめちゃくちゃ理解したわけではないけれど，
めちゃ適当にいうと，
一般的な方程式$f(x)=0$の解があるのであれば（f(x)のグラフがx軸と交点を持つならば）
とある$x=x_{n}$の点における接線のx軸との交点$x_{n+1}$は，$x_{n}$よりも解に近づいている
ということで，これを繰り返してけば解とほぼ一緒になるでしょう
ということで理解．

今回の漸化式は

\begin{align}
\boldsymbol{\theta_{n+1}}
=
\boldsymbol{\theta_{n}}
-
J(\boldsymbol{\theta})^{-1}
\left( \boldsymbol{f}(\boldsymbol{\theta}) - \boldsymbol{x_{ee}^{d}} \right)
\end{align}

今回は冗長系で，$J(\boldsymbol{\theta}) \in \mathrm{R}^{3 \times 4}$となっていて，逆行列とか存在しないので，擬似逆行列を使って，

\begin{align}
\boldsymbol{\theta_{n+1}}
=
\boldsymbol{\theta_{n}}
-
J(\boldsymbol{\theta})^{\dagger}
\left( \boldsymbol{f}(\boldsymbol{\theta}) - \boldsymbol{x_{ee}^{d}} \right)
\quad
\left( J(\boldsymbol{\theta})^{\dagger} = J^{T}(JJ^{T})^{-1} \right)
\end{align}

とする．

このへんは，ニュートン法のWikiのページの"高次元の場合"の項目の"コンピュータで計算を行う場合～"のくだりと
擬似逆行列のWikiのページの"性質"の項目の"$A$を$m \times n$行列とする．連立一次方程式$Ax=b$に対して～"のくだりを組み合わせた話になっている．

これを，$| \boldsymbol{x_{ee}^{d}} - \boldsymbol{x_{ee}} | < \epsilon$になるまで繰り返して，そのときの$\theta_{n}$をロボットに入力すればおｋ．

最後に

思ったよりwikiまとまってた．
あとこれ制御じゃないわ．．．タイトル/タグ詐欺的な感じ．．．