クワマイでもできる

クワマイでもわかる

数値計算で逆運動学を解く

アルゴリズム

f:id:kuwamai:20191005173233p:plain Denavit-Hartenberg parametersでマニピュレータを記述し、数値計算で逆運動学を解いた。パラメータさえ書けば色々なマニピュレータに対応できるので便利そう。運動学はにわかなので違うところあったら教えてください。

Notebook

実装の例と式はJupyter Notebookに書いた。だからこのブログは参考にしたサイトの紹介だけ。

Denavit-Hartenberg parametersと同次変換行列

Denavit-Hartenberg parametersはマニピュレータの関節角やリンクの長さを表現する際によく用いられている記法。このパラメータから同次変換行列を求め、順運動学を解く。

こちらのサイトが図や式が豊富で大変わかりやすかったです。

今回はこんな感じの3DoFなマニピュレータを考える。

f:id:kuwamai:20191005182338j:plain

Denavit-Hartenberg parametersは多分こんな感じ。

^{i-1}T_i a_i \alpha_i d_i \theta_i
^{B}T_1 0 0 0 \theta_1
^{1}T_2 0 \pi/2 0 \theta_2
^{2}T_3 1 0 0 \theta_3
^{3}T_E 1 0 0 0

同次変換行列はこれに代入していく。この式をJupyter Notebookに書いてもGitHubで見るとレイアウトが崩れるから発狂しそうだった。ここに書いておく。SやCはそれぞれsin、cosの略。


\begin{align}
^{i-1}T_i&=
\begin{bmatrix} & a_{i-1} \\ I & 0 \\ & 0 \\ O^{T} & 1 \end{bmatrix}
\begin{bmatrix} 1 & 0 & 0 & 0 \\ 0 & C_{\alpha_{i-1}} & -S_{\alpha_{i-1}} & 0 \\ 0 & S_{\alpha_{i-1}} & C_{\alpha_{i-1}} & 0 \\ 0 & 0 & 0 & 1 \end{bmatrix}
\begin{bmatrix} & 0 \\ I & 0 \\ & d_i \\ O^{T} & 1 \end{bmatrix}
\begin{bmatrix} C_{\theta_i} & -S_{\theta_i} & 0 & 0 \\ S_{\theta_i} & C_{\theta_i} & 0 & 0 \\ 0 & 0 & 1 & 0 \\ 0 & 0 & 0 & 1 \end{bmatrix} \\
&=\begin{bmatrix} C_{\theta_i} & -S_{\theta_i} & 0 & a_{i-1} \\ C_{\alpha_{i-1}}S_{\theta_i} & C_{\alpha_{i-1}}C_{\theta_i} & -S_{\alpha_{i-1}} & -S_{\alpha_{i-1}}d_i \\ S_{\alpha_{i-1}}S_{\theta_i} & S_{\alpha_{i-1}}C_{\theta_i} & C_{\alpha_{i-1}} & C_{\alpha_{i-1}}d_i \\ 0 & 0 & 0 & 1 \end{bmatrix}
\end{align}

同次変換行列を順にかけていくと、ベースから見たエンドエフェクタの位置姿勢がわかる。つまりこれで順運動学が解ける。

^{B}T_E=^{B}T_1 ^{1}T_2 ^{2}T_3 ^{3}T_E

数値計算

まず関節角ベクトルq, 手先位置ベクトルrを下記のように定義する。


\begin{align*}
q&=\begin{bmatrix} \theta _{1} \\ \theta _{2} \\ \theta _{3} \end{bmatrix}\\
r&=\begin{bmatrix}
x \\
y \\
z
\end{bmatrix}
\end{align*}

するとヤコビ行列はこんな感じ。


J\left( q\right) =
\begin{bmatrix}
\dfrac {\partial x}{\partial \theta _{1}} & \dfrac {\partial x}{\partial \theta _{2}} & \dfrac {\partial x}{\partial \theta _{3}} \\
\dfrac {\partial y}{\partial \theta _{1}} & \dfrac {\partial y}{\partial \theta _{2}} & \dfrac {\partial y}{\partial \theta _{3}} \\
\dfrac {\partial z}{\partial \theta _{1}} & \dfrac {\partial z}{\partial \theta _{2}} & \dfrac {\partial z}{\partial \theta _{3}}
\end{bmatrix}

初期値q_0を適当に設定して、下記式を繰り返し計算すると解ける。ちなみに f(q_i)が順運動学。


\begin{align*}
r_i&=f(q_i)\\
q_{i+1}&=q_i-J(q_i)^{-1}(r_i-r_{ref})
\end{align*}

f:id:kuwamai:20191005192553p:plain

計算してみるとこんな感じ。水色の棒がマニピュレータで、赤い丸が目標手先位置。計算を繰り返すごとに手先位置と赤い丸の位置が近づいていくのがわかる。

使用例

書いたコードをそのまんまROSパッケージに入れてアームを動かしてみた。PythonなのでC++と比べて遅いけど、とりあえず動くしわかりやすいのでいいかなと思ってる。

参考

マニピュレータの描画方法を参考にさせていただきました。