マニピュレータ制御①～順運動学、逆運動学～
制作過程（ロボットアーム） 5自由度のマニピュレータを制作、制御します。マニピュレータで物を掴んだり、手書き文字を書いたり、イラストを描いたりすることが最終的な目標です。
あくまで趣味の延長として取り組んでいますが、将来的に私の研究の実証実験などで利用できれば一石二鳥と考えています。
また、サーボを手に入れるのに時間がかかりそうなので、まずは制御から取り掛かります。
この記事では、一度にマニピュレータの制御についてまとめると文章量が膨大になるため複数回に分けて記述します。
今回は、マニピュレータの先端速度を制御する術について書いていきます。
一般に順運動学とは、関節の角速度から先端速度や先端姿勢を求めることであり、逆運動学とは、先端速度や先端姿勢から関節の角速度を求めることをいいます。
今回作成するマニピュレータでは、直接的に操作できるパラメータは各関節のサーボの角速度であるため、マニピュレータの先端速度、姿勢を実現するために必要な関節角速度を求めることができる逆運動学を解くことが目標です。
そのためにまずは順運動学から計算して、関節角速度から先端速度、姿勢に変換する変換行列である「ヤコビアン」を計算します。
マニピュレータの定義 今回想定しているロボットアームはこのような5自由度の関節をもつマニピュレータです。
手先については、タスクに応じて手先にグリッパなどを取り付けたいと考えています。
5自由度マニピュレータのモデル 文字 定義 $\bm{r}$ 先端の座標ベクトル [cm] $θ_i$ 関節$i$の角度 [rad] $\bm{l_i}$ リンク$i$の長さのベクトル [cm] $\Sigma_i$ 座標系$i$ 太い矢印部分がマニピュレータの本体になり、回転軸が設定されている箇所に関節が存在します。
また、$\Sigma_i$の原点から$\Sigma_g$の原点までのベクトルを$\bm{P_i^g}$と定義します。
続いて、3次元（SE(3)）空間での回転行列、手先姿勢は以下のように定義します。
文字 定義 $\bm{R_i^g}$ $\Sigma_i$から見た$\Sigma_g$の姿勢を表す回転行列 $\alpha$ 手先の姿勢 Roll [rad] $\beta$ 手先の姿勢 Yaw [rad] このとき、$\bm{R_0^1}$、$\bm{R_4^5}$はz軸周りの回転であり、$\bm{R_1^2}$、$\bm{R_2^3}$、$\bm{R_3^4}$はx軸周りの回転であることから、回転行列は以下の通りです。
※視認性を良くするため、cos($θ_i$)は$c_i$、cos($θ_i$)*cos($θ_g$)は$c_{ig}$と記述します（sinも同様）
$$ \bm{R_0^1} = \bm{R_4^5} = \begin{pmatrix} c_i & -s_i & 0 \\ s_i & c_i & 0 \\ 0 & 0 & 1 \\ \end{pmatrix} $$ $$ \bm{R_1^2} = \bm{R_2^3} = \bm{R_3^4} = \begin{pmatrix} 1 & 0 & 0 \\ 0 & c_i & -s_i\\ 0 & s_i & c_i \\ \end{pmatrix} $$
