環境
この記事は以下の環境で動いています。
| 項目 | 値 |
|---|---|
| CPU | Core i5-8250U |
| Ubuntu | 16.04 |
| ROS | Kinetic |
インストールについてはROS講座02 インストールを参照してください。
またこの記事のプログラムはgithubにアップロードされています。ROS講座11 gitリポジトリを参照してください。
概要
ロボットアームの制御ではアームの先端をある地点に移動させるという制御が良く行われます。しかしロボットアームへの入力として、各軸の角度を渡します。この2つを変換するのがアームの制御の基本になります。
- 「各軸の角度」->「先端の位置」に変換するのを「運動学」
- 「先端の位置」->「各軸の角度」に変換するのを「逆運動学」
と呼びます。これらをプログラムしていきます。
対象となるアーム
特に逆運動学はアームが複雑になると、そもそも解析解がなく反復法で解く必要がありますし、そこまで複雑にならなくてもベアで書くのには限界があります。ここではよくつかわれる4軸のアームを例にとります。
下の図のように根元から、yaw回転をする軸が1つ、pitch回転をする軸が3つあります。この構成だと先端の角度を水平などに指定したまま、先端のxyzの位置を指定できるために簡易的なロボットアームでよく使われる形式です。
計算の原理
運動学
たいそうな名前がついていますが、ただ三角関数の計算をするだけです。
x,y,zの座標系に、第1関節の向いているほうの向きにL軸を取ります。
L軸: L3sin(a2) + L4sin(a2+a3) + L5sin(a2+a3+a4)\\
Z軸: L1 + L2 + L3cos(a2) + L4cos(a2+a3) + L5cos(a1+a2+a3)\\
\left(
\begin{array}{ccc}
x \\
y \\
z
\end{array}
\right)
=
\left(
\begin{array}{ccc}
L軸 cos(a1) \\
L軸 sin(a1) \\
Z軸
\end{array}
\right)
逆運動学
こちらは少々複雑です。最終的に2軸で余弦定理を使うところまで追い込みます。
出力は(a1,a2,a3,a4)の4変数になるので一意な入力を得るには先端の(x,y,z)位置と選択の角度(φ)を入力とします。φ=0は先端が真上を向いている。φ=π/2は真横を向いているを表します。
問題設定から第1リンクの先と第3リンクの先の(L,Z)面での座標が求まります。
ここで第3、第4リンクだけを切り抜くと以下のようになります。ここでできる三角形の3辺の長さが分かるので、余弦定理から角度を求められます。
ソースコード
逆運動学を解くプログラムはarm_lecture/src/arm_ik.hにあります。
arm_ik_nodeでこれを使って動かします。
ビルド
cd ~/catkin_ws
catkin_make
実行
roslaunch arm_lecture arm4d_ik.launch
逆運動学の入力となる入力のx,y,z位置を赤の球で表示します。その場所に移動するアームの角度を算出して移動します。
