ロボットの座標変換入門

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

入力を変えて確かめる

実験パネルを開き、実行を押すとPythonを読み込みます。停止と初期値へのリセットができます。結果はこの端末内で計算します。Pythonのインストールは不要です。

下にあるローカル実行の手順は、配布ソースを再現するための任意の手順です。ブラウザ実験には不要です。

実験パネルを別タブで開く

再現用ソースをダウンロード

前提と読み方

前提知識 ベクトルと行列の積。

入門 · 本文の条件と確認項目に沿って、計算例や比較表を追う。

ブラウザ実験の条件

ページ上部の実験で回転角、B原点の世界座標、入力点のローカル座標を変更できます。世界座標をA、ローカル座標をBとし、列ベクトルで p_A = R_AB p_B + t_AB を計算します。正の角度は反時計回り、距離はmです。初期値は本文の90°回転・平行移動(1, 2) m・入力点(2, 0) mで、変換後は(1, 4) mになります。

図の全ての位置は世界座標Aで表し、xとyは同じ縮尺です。矢印は各座標系の正方向（長さ1 m）、青い線はB原点から変換後の点への位置ベクトルです。入力点のB座標を、そのまま世界座標の点として描くことはしません。逆変換 R_AB^T (p_A - t_AB) で入力点へ戻ることを確認でき、「往復変換の誤差」は戻った点と入力点のユークリッド距離です。

Aに結果を保存して別の条件を実行すると重ねて比較できます。比較ケースA/Bは座標系A/Bとは別の表記です。この実験はノイズや時刻差のない理想的な2次元剛体変換で、3次元クォータニオンやROS TFを実行するものではありません。

ブラウザと同じ実行コードと回転カーネルも保存できます。以下のローカル実行手順は任意です。

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

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

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

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

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

90度回したセンサの例

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を再生成できる。

再生成する場合は、上のPythonコードを新しいフォルダに engineering_labs.py として保存して実行する。出力先は実行時の作業フォルダで、この図は transforms.svg、検算値は engineering-results.json の transform に入る。同じスクリプトはKalman・ICP・周波数応答・エイリアシングの図とCSVも一緒に生成する。既存の結果ファイルは上書きされるため、空の作業フォルダを使う。

逆変換は並進の符号を変えるだけではない

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

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は時刻も含む関係

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

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