環境

この記事は以下の環境で動いています。

インストールについては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位置を赤の球で表示します。その場所に移動するアームの角度を算出して移動します。

目次ページへのリンク

ROS講座の目次へのリンク