ロボットの座標変換入門

目次 — 知りたいところから読む

座標変換で最初に決めるのは、「どの座標で書かれた点を、どの座標へ移すか」である。同じ点でもセンサから見た数値と地図から見た数値は違う。行列やクォータニオンの計算が合っていても、方向を逆に解釈するとロボットは違う場所に物体を描く。

変換の添字を方向として読む

A座標へB座標の点を写す回転をR_AB、Aから見たBの原点位置をt_ABと書く。列ベクトルを使う約束である。

p_A=R_{AB}p_B+t_{AB}

回転行列の列は、Bの各軸をAで表したものになる。並進はA座標で表す。点には並進を足すが、速度方向や単位軸のような自由ベクトルを回すだけなら並進を足さない。この区別をせず、すべての3要素配列へ同じ処理をすると不具合になる。

2Dで、Bの原点がAの(1, 2) mにあり、Bのx軸がAのy軸方向を向くとする。Bで(2, 0) mの点は次の位置になる。

R_{AB}=\begin{bmatrix}0&-1\\1&0\end{bmatrix},\qquad p_A=\begin{bmatrix}0&-1\\1&0\end{bmatrix}\begin{bmatrix}2\\0\end{bmatrix}+\begin{bmatrix}1\\2\end{bmatrix}=\begin{bmatrix}1\\4\end{bmatrix}

Pythonコードのtransforms関数は、この結果と逆変換を検査する。Python 3.12.3、NumPy 1.26.4、Matplotlib 3.6.3で実行した。python3 engineering_labs.pyで図と集計JSONを再生成できる。

回転行列の逆は転置なので、逆変換は次のとおり。

p_B=R_{AB}^T(p_A-t_{AB}),\qquad R_{BA}=R_{AB}^T,\qquad t_{BA}=-R_{AB}^Tt_{AB}

例では(1, 4)から原点(1, 2)を引いた後に−90°回すと(2, 0)へ戻る。逆向きの並進は(−2, 1)であり、単なる(−1, −2)ではない。実装では任意の点を往復変換し、元へ戻るか確認すると方向の取り違えを見つけやすい。

3Dでは4×4の行列に回転と並進をまとめられる。

T_{AB}=\begin{bmatrix}R_{AB}&t_{AB}\\0&1\end{bmatrix},\qquad T_{AC}=T_{AB}T_{BC}

右側の変換から作用する。地図A、車体B、センサCなら、センサ→車体→地図の順に点を写す。行列の積の順を入れ替えても一般には同じにならない。添字の中間Bがつながるように書くと、コードの変数名も説明しやすい。

単位軸uのまわりに角度θだけ回す単位クォータニオンは、ROSで使う成分順(x, y, z, w)なら次になる。

q=(u_x\sin(\theta/2),u_y\sin(\theta/2),u_z\sin(\theta/2),\cos(\theta/2))

z軸まわり90°は(0, 0, 0.7071, 0.7071)である。ライブラリには(w, x, y, z)順もあるため、配列をコピーする前に確認する。qと−qは同じ回転を表すので、成分の単純な差を姿勢誤差にしない。正規化の確認も必要である。

REP-103はROSの右手系とSI単位、車体のx前・y左・z上、光学フレームのz前・x右・y下を整理している。カメラだけ見え方が90°ずれる場合は、まずフレームの約束を確認する。

TFのparentとchildを見て、childの姿勢をparentで表した変換として読むと、上のT_ABとつながる。APIで点を変換するときはtarget frameとsource frameを確認する。静的な取付け変換と、走行で変わる変換は寿命が異なる。

REP-105のmapは大域的な補正で不連続になり得る一方、odomは連続性を重視するがドリフトし得る。正しい行列でも、別の時刻の姿勢を使えば動いているセンサの点はずれる。センサ融合の切り分けで、方向と時刻を一緒に調べる手順へ進める。

#基礎