ろむめも

気になったこととか、調べたことをゆるくまとめます。主にプログラミング関連の話題が多いです。

Robotics③ Kinematics

以下に学習時のメモを記載します。

運動学

導入

重要な4つの概念。reference frame, generalized coordinates, degrees of freedom, homogeneous transform
Joint Space(theta) -> Forward Kinematics -> Cartesian Space(x,y,z)
Cartesian Space(x,y,z) -> Inverse Kinematics -> Joint Space(theta)
一般的に逆運動学を解くのは運動学を解くのよりも難しい。

Dofは独立でない一般化座標の数で表されます。独立でない一般化座標の数はその系を表すのに最低限必要な変数の数です。
robot工学において、configuration spaceやjoint spaceという言葉は重要です。これはアームがとりうる全てのconfigurationsを表します。path planningやobstacle avoidanceの中で使われます。

例えば3次元の世界座標系があってそこに一つの棒を置いたとします。この系を表すのに必要なのは6つの変数です。これは例えば棒の両端のx,y,zの座標やr,theta,phiで表されても良いです。二つの棒を置いた場合は12個の変数が必要になります。二つの棒を関節でつないだ場合は7Dofです。

回転およびプリズムジョイントだけを持っているアームの自由度はエンドエフェクタも含めた可動な関節の数と同じになります。例外として閉ループのアームの自由度は1です。もしアームが与えられたタスクに必要な自由度より多くの自由度を有している場合、運動学的冗長と言います。この冗長性はタスクを解くのに有利で、アームの動かし方に自由度があるという事になります。一方でコストと制御の難しさを考慮する必要があります。

一般的に産業界で使われるのは4か6自由度のアームです。特に有名なのはspherical wristです。先端にroll,yawを持っていて、次の関節にroll,yawがあり、最後にpitchを持ちます。最初の3つで先端の位置の制御をして、残りの3つで姿勢の制御をします。

関節構成のタイプを評価するのにworkspaceという概念を使うことがあります。workspaceは先端が到達可能な点の集合です。一般的にworkspaceには2つの意味に分解できて、到達可能なworkspaceと器用なworkspaceになります。器用なworkspaceとは任意の姿勢で先端部が到達できるかという追加の自由度の事です。

一般的な関節構成のタイプとしては、デカルト型(PPP)、円筒型(RPP)、擬人型(RRR)、スカラ型(RRP)、球状型(RRP)があります。また、平行リンク型も重要なタイプです。

運動学、逆運動学

空間上のパーティクルの位置は参照座標の単位ベクトルにパーティクルの座標のスカラをそれぞれ掛けた形で求められます。ここで一つの座標系を別の座標系に変換する方法を考えます。例えば原点を中心として回転させた場合、回転行列を使う事でその系の返還を表すことが出来ます。回転行列の特性としては以下のものがあります。
逆行列は転置で表すことができる
行列式は1
③行および列は直行する単位ベクトルを意味し、任意の行または列の大きさは1にひとしく、2つの行または列の内積は0になる
④列は元の座標系に対する回転した座標系の基底ベクトルを表す。
これらは3次元にしても同じです。

この計算を行うのに、今回はsympyを利用します。これはシンボルを定義して、式を抽象的に定義することができます。これは簡単なシステムにおいて方程式を簡潔に表すことができるし、浮動小数点演算においても誤差が少なくなる傾向にあります。
サンプルコード

from sympy import symbols, cos, sin, pi, simplify
from sympy.matrices import Matrix
import numpy as np
q1, q2, q3, q4 = symbols('q1:5')
A, R, O, C = symbols('A R O C')
rtd = 180./np.pi
dtr = np.pi/180.
R_x = Matrix([,,[]])
R_x.evalf(subs={q1: 45*dtr})

基本的には回転は順番によらないが、演算上の回転の順番は主に以下のものがある
Intransic Rotation = RzRyRx
Extransic Rotation = RxRyRz

回転行列を使う場合に注意しなければならないポイントは
①パフォーマンスとして3つ座標系なのに9つのパラメータが必要になる
②回転行列の反復乗算によって丸め誤差が発生し、時間と共に有効な回転行列でなくなってしまう。
③回転後に同じ平面に回転軸が来てしまった場合自由度が一つ減り、表現の自由度がうそなわれてしまう。これをgimbal lockと呼ぶ。これを防ぐにはそのような領域に近づかないようにすることです。

オイラー角を求める場合にはatan2を使う。

各関節の角度から先端の位置を求める場合に同次変換を利用する。これは位置(平行移動)のベクトルと姿勢(回転)の行列を一つの行列にまとめたものである。
同次変換を複数回繰り返した場合、結果は同じになるはずです。それは例えばA->Dに変換したいとしてA->B->Dと変換するのとA->C->Dと変換するのでは結果が変わらないことを意味します。

DHパラメータ

ここまでは各リンクの位置を求めるのに回転行列と平行移動を使ってきました。これを簡単にやる方法がDHパラメータの導出です。これまでは位置と姿勢の6つの変数を決める必要がありましたが、DHパラメータでは4つの変数を決めることで求めることができます。但し、この変数の取り方は系の取り方によって5パターン程度存在するので、あくまでも1例です。
・α=Y軸を中心としたZ軸の回転角度
・a=X軸方向の移動距離
・d=Z軸に沿って測定されたXの符号付き距離
・Θ=Z軸を中心としたX軸の回転角度
X軸方向の単位ベクトルはZi-1とZiの外積で求めることができます。DHパラメータを使って座標系を変換するときには、変換はリンクi-1の参照フレームを、リンクiの参照フレームと正確に一致するように移動するかという事を考えると良いです。
RRPR型のアームの場合を考えます。
①全ての関節に1から番号をつけます
②すべてのリンクに0から番号をつけます。0は接地部分です
③駆動軸に線を引きます
④DHパラメータが少なくなるように取り扱うリンク間の距離変数を決定します
⑤各軸の方向を決めます。例えば回転軸であれば反時計回りを正にします。そして右ねじの法則でZ軸の方向を決めます。
⑥X軸の方向を決めます。Xi-1からXiの方向に正とするのが一般的です。ただ、P型ジョイントではDHパラメータを減らすためにPの駆動方向とは垂直な方向にとることがあります。
⑦joint1の変数を減らすためframe0とframe1を一致させます。そうすることでα=a=0になり、もしjoint1がR型の関節であればd=0、P型の関節であればd=0とすることができます。
⑧先端部のX軸の方向を決めます。一般的には一つ前のframeと同じように決めます。
次にDHパラメータのテーブルを決める方法です。
α0はz0とz1の回転角度です。a0はz0からz1のx0軸上の距離です。z軸が平行であればα、x軸の原点が同じであればaは0になります。d1はx0からx1のz1軸上のsin距離です。これもx0とx1が平行であれば0になります。Θ1はx0とx1の間のz1軸周りの回転角度です。x軸が平行ではない場合、Θは0になる場合もありますが、一定のオフセットが乗る場合もあります。
0->1:x,z軸が平行なのでα,d=0、xの原点が同じなのでa=0となり、α,a,d,Θ=0,0,0,Θ1
1->2:x,z軸が平行なのでα,d=0、α,a,d,Θ=0,a1,0,Θ2
2->3:x,z軸が平行なのでα=0、x軸は平行だが関節がP型なのでdは変数となり向きに注意して、α,a,d,Θ=0,a2,-d3,Θ2
3->4:x,z軸が平行なのでα,d=0、xの原点が同じなのでa=0となり、α,a,d,Θ=0,0,0,Θ2

逆運動学

先端の位置が決まったときに、それぞれの関節をどの角度に曲げればよいでしょうか。逆運動学を解くというのは本質的にはそいう問題になります。ところが、先端の位置が決まったとしてもすべての関節の向きが一意に決まるわけではなく、複数のとりえるパターンが出来てしまったり、解が無い場合もありえます。これが逆運動学の難しさです。一つの方法としてはNewton-Raphson法を使った数値的解法です。初期値が解に十分近ければ早く収束します。ほかのポーズを求めたい場合は別の初期値を与える必要があります。ただし、同じアルゴリズムですべてのアームに利用できます。別の方法としては、閉形式解を求める方法です。ただし適用できるものは①3つの隣接する関節軸が一点で交差する②3つの隣接する関節軸が平行のどちらかを満たしている必要があります。一般的に、現在実用されている6DoFアームはどちらかを満たしています。特に最後の3つの関節は回転関節であることが多く、これは手首を再現したようなものです。そして最初の3つの関節は先端の位置を決める為に利用されます。そうすることで位置決めと先端部の操作を切り分ける事ができます。一般的な逆運動学の解法を以下に示します。
①DHパラメータテーブルを埋めます。(手首中心とframe4,5,6を一致させてとると良い)
②ベースフレームに対する手首中心の位置関係を明らかにする。
③式を満たすように各関節の位置(特に位置決め関節であるJ1,J2,J3)を決める。
④各関節の位置が求まったら、逆に先端の位置を導出する。
⑤回転行列を使って各関節におけるオイラー角のセット算出する
⑥出てきた解から最適なものを選ぶ

簡単な例としてRRP型のアームの場合を考える。
先端の位置=zcはわかっているとして、Θ1, Θ2, d3の導出をする。
Θ1 = atan2(yc,xc)
Θ2 = atan2(s, r); r^2 = xc^2 + yc^2, s = zc - d1
d3 = sqrt(r^2 + s^2) = sqrt(xc^2 + yc^2 + (zc - d1)^2)

Project

github.com

Gazeboは物理演算を含むシミュレーション環境です。内部ではgzserverとgzclientが動いていて、起動するにはまずserverを起動し、clientを起動する必要があります。いっぺんに起動する場合はgazeboを使います。

URDFはxmlファイルでrobotモデルを表すのに使います。例えば、関節の構成やリンクの長さを定義します。同じようにxacroファイルを使ってurdfファイルを複数のxacroファイルに分けることもできます。

RVizはROSを可視化するツールです。ロボットの予測、意思決定、応答のすべてを可視化することが出来ます。RVizを使うとセンサー、カメラ、Lidarデータの可視化をリアルタイムに行うことができます。RVizはシミュレータではないので物理エンジンは入っていません。そのため、衝突や力学は考慮されません。rvizを起動するには、roscoreを行ってからrosrun rviz rvizを行います。

MoveIt!は操作、運動学、制御のためのモーションプランニングフレームワークです。MoveIt!はそれ自体でモーションプランニング用のアルゴリズムを持っていませんが、インターフェースを提供しています。このインターフェースはrequest/responce方式で実現されています。特に、ロボットを干渉がない経路を生成し、予め設定した速度や加速度の制限に従って軌道を演算してくれます。

ROSのRobotStateクラスは運動学機能にアクセスできますが、逆運動学は独自で実装する必要があります。

move_groupノードはMoveIt!のプライマリノードで、パッケージの提供とセントラルハブとしての機能を持ち、ほぼすべてのコンポーネントとの通信を提供します。

Rvizを使ったtransform matrixの導出にはrosrun tf tf_echo [reference frame] [target frame]を使うと良い


続く。。。