はじめに
ロボットの仕事では、センサーで測定されたデータを同じ剛体に取り付けられた他の位置に変換することがよくあります。前編では、オドメトリなどで測定された速度の剛体変換について議論しましたが、IMU(加速度と角速度を計測する装置)データの剛体変換について考えたことはありますか？実は、これはなかなか難しい問題です。
問題：AとBは同じ剛体に所属しています。AでのIMUの測定値(加速度と角速度)が既知の場合、Bでの加速度と角速度を計算してください。
角速度の計算については前編「速度と角速度の剛体変換」で既に説明しましたので、加速度の剛体変換について解説しようと思います。
以下の図にこの問題の2つの実応用を挙げています。
- IMUデータをLiDAR座標系に変換し、LiDARデータと統合します。
- 複数のIMUデータを同じ座標系に変換し、平均化することで精度を向上させます。
$$
\newcommand{\skew}[1]{[{#1}]_{\times}} %skew matrix
$$
解法
加速度の定義は、微小な時間間隔($\Delta{t}$)での速度の変化量を$\Delta{t}$で割ったものです。そのため、$\Delta{t}$の前後におけるBの速度をそれぞれ求めることで、Bの加速度を計算することができます。
「速度と角速度の剛体変換」によると、AとB間の剛体変換、Aの速度と角速度が既知の場合、速度の剛体変換を使用してBの速度を計算することができます。したがって、この方法を用いてBの加速度を解析しようと思います。
Aの運動学分析
上記解法の説明より、まず、$\Delta{t}$の前後でのAの速度を求めましょう。
開始時点($t_0$時刻)で、世界座標系におけるAの位置を$p_{wa}$、姿勢を$R_{wa}$、速度を$v_{wa}$とします。
このとき、Aのローカル座標における速度は以下のように計算できます。
$$
v_{a} = R_{wa}^{-1} v_{wa}
\tag{1}
$$
AでのIMUの測定値をそれぞれ$a_a$(加速度)と$\omega_a$(角速度)とします。
運動学モデルに基づいて、加速度と角速度を受け、$\Delta{t}$後の状態($t_1$時刻)は以下のように変化します。
$$
\begin{aligned}
&p^\prime_{wa} = p_{wa} + v_{wa} \Delta{t} \\
&R^\prime_{wa} = R_{wa} R_{aa^\prime} = R_{wa} \exp( \skew{\omega_a \Delta{t}}) \\
&v^\prime_{wa} = v_{wa} + R_{wa} a_a \Delta{t}
\end{aligned}
\tag{2}
$$
ここで、$\prime$は$t_1$時刻での状態を示します。
「exp」はリー代数の指数展開です。詳細は「リー群を用いた回転表現」を参考にしてください。
同様に、$t_1$時刻で、ローカル座標におけるAの速度は以下のようになります。
$$
v_{a}^{\prime} = R_{wa}^{\prime{-1}} v_{wa}^{\prime}
$$
式(2)$のR^\prime_{wa}$と$v^\prime_{wa}$を代入して、さらに展開できます。
$$
\begin{aligned}
v_{a}^{\prime}
&= \exp(-\omega_a \Delta{t}) R_{wa}^{-1} (v_{wa} + R_{wa} a_a\Delta{t}) \\
&= \exp(-\omega_a \Delta{t}) (R_{wa}^{-1} v_{wa} + a_a\Delta{t}) \\
&= \exp(-\omega_a \Delta{t}) (v_a + a_a\Delta{t})
\end{aligned}
\tag{3}
$$
したがって、式(1)と式(3)で$\Delta{t}$の前後でのAの速度を求めました。
Bの速度
AとB間の剛体変換、Aの速度と角速度が既知であれば、速度の剛体変換を使ってBの速度と角速度を計算することができます。詳細については「速度と角速度の剛体変換」を参考してください。
上記の結論に基づき、$t_0$時刻で、ローカル座標におけるBの速度を以下に計算します。
$$
v_b = \skew{t_{ba}}R_{ba} \omega_a + R_{ba} v_a
\tag{4}
$$
同様に、$t_1$時刻でにローカル座標におけるBの速度を以下のように計算できます。
$$
\begin{aligned}
v^\prime_b
&= \skew{t_{ba}}R_{ba} \omega^\prime_a + R_{ba} v^\prime_{a} \\
&= \skew{t_{ba}}R_{ba} \omega^\prime_a + R_{ba} \exp(-\omega_a \Delta{t}) (v_a + a_a\Delta{t}) \\
&= \skew{t_{ba}}R_{ba} \omega^\prime_a + R_{ba} (I + \skew{\omega_a \Delta{t}}) (v_a + a_a\Delta{t}) \\
&= \skew{t_{ba}}R_{ba} \omega^\prime_a + R_{ba}v_a + R_{ba}a_a\Delta{t} + R_{ba} \skew{\omega}(v_a + a_a\Delta{t})\Delta{t} \\
\end{aligned}
\tag{5}
$$
Bの加速度
ローカル座標におけるBの加速度を$a_b$とします。加速度は速度の変化率を表すため、以下のようにBの加速度を計算できます。
$$
\begin{aligned}
a_b = \frac{R_{ba} R_{aa^\prime} R_{ab} v^\prime_b - v_b}{\Delta{t}}
\end{aligned}
\tag{6}
$$
注意すべき点は、$v^\prime_b$の座標系は$t_1$時刻の座標系であるため、$t_0$時刻の座標系に戻す必要があることです。
この式を簡略化すると、Bの加速度は以下のように計算できます。
$$
\begin{aligned}
a_b &= \frac{R_{ba} \exp(\omega_a \Delta{t}) R_{ab} v^\prime_b - v_b}{\Delta{t}} \\\\
&\text{Do some mathematical magic...}
\\\\
&= R_{ba}a_a - \skew{ R_{ba} \omega_a}^2 t_{ba} + \skew{t_{ba}} R_{ba} \dot{\omega_a}
\end{aligned}
\tag{7}
$$
この簡略化は非常に煩雑のため、詳細は付録に添付します。
ここで、$\dot{\omega}$は角加速度であり、以下のように定義されます。
$$
\dot{\omega} = \frac{\omega^\prime - \omega}{\Delta{t}}
$$
問題の答え
したがって、Aの加速度および角速度が既知である場合、Bのローカル座標系における加速度と角速度(IMUの測定値)は以下のように計算できます。
$$
\begin{aligned}
\omega_b &= R_{ba}\omega_a \\
a_b &= R_{ba}a_a - \skew{ R_{ba} \omega_a}^2 t_{ba} + \skew{t_{ba}}R_{ba} \dot{\omega_a}
\end{aligned}
\tag{8}
$$
角速度の変換式は前編の結論です。
結果の検証＆可視化
式(8)の結果を検証するため、物体の運動分析のデモを実装しました。
- 赤の軌跡：Aの加速度と角速度から計算したAの運動軌跡
- 青の軌跡：AとBは同じ剛体に所属しているため、Aの自己位置を使用して算出したBの運動軌跡
- 水色の軌跡：式(8)で計算したBの加速度と角速度から推定されたBの運動軌跡
- 緑のキューブ：剛体のイメージ
上記のデモから、赤の軌跡と青の軌跡が一致していることが確認できるため、式(8)の結論が正しいことが証明できます。
結果の考察
式(7)は純粋な数学的計算によって加速度の剛体変換式を導出しましたが、その式の物理的な意味を考えてみましょう。
式(7)は3つの成分から構成されています。
$$
\begin{aligned}
a_b
&= R_{ba}a_a - \skew{ R_{ba} \omega_a}^2 t_{ba} + \skew{t_{ba}} R_{ba} \dot{\omega_a} \\
&= a_b^{*1} + a_b^{*2} + a_b^{*3}
\end{aligned}
$$
$a_b^{*1}$ = $R_{ba}a_a $は、Bのローカル座標系におけるAの加速度を表しています。この式は理解しやすいと思います。残りの2つの式については、一見謎だと思われるかもしれませんが、実は高校の物理学の知識でも理解できる内容です。
向心加速度
高校の物理で円運動の向心加速度を学んだことがあるかと思いますが、
回転中心の角速度が$\omega$のとき、半径がrの位置における剛体の加速度は以下のように計算できます。
$$
a = \omega^2 r
$$
三次元の場合、以下のように拡張することができます。この加速度は回転軸に垂直であり、方向は回転軸に向かいます(符号はマイナス)。
$$
\begin{aligned}
a_b^{*2}
&= -\skew{ R_{ba} \omega_a}^2 t_{ba} \\
&= -\skew{ \omega_b}^2 t_{ba} \\
\end{aligned}
$$
接線加速度
2D平面の場合、角速度が変化するとき(角加速度は0ではない)、以下のような加速度が存在します。
$$
a = r \dot{\omega}
$$
三次元の場合、以下のように拡張することができます。クロス積の性質より、この加速度は$t_{ba}$と$\dot{\omega_b}$それぞれ垂直であり、接線加速度とも呼ばれます。
$$
\begin{aligned}
a_b^{*3}
&= \skew{t_{ba}}R_{ba} \dot{\omega_a} \\
&= \skew{t_{ba}}\dot{\omega_b} \\
&= t_{ba}\times \dot{\omega_b}
\end{aligned}
$$
したがって、Bのローカル座標系で受けた加速度は、Aの加速度をBのフレームに変換した加速度$a_b^{*1}$、Aの角速度により生成された向心加速度$a_b^{*2}$、およびAの角加速度により生成された接線加速度$a_b^{*3}$という3つの成分から構成されています。
なんと、純粋な数学的分析で、剛体力学の結論を導きました！これは数学の美しさを感じますね。
付録
式(7)の証明は以下に示します。
※: 一般ユーザーは結論だけを使っても問題ありませんが、この証明自体が私にとって最も時間がかかった部分なので、メモとして残しておきます。
$$
\begin{aligned}
a_b
&= \frac{R_{ba} \exp(\omega_a \Delta{t}) R_{ab} v^\prime_b - v_b}{\Delta{t}} \\
&= \frac{R_{ba} (I + \skew{\omega_a \Delta{t}}) R_{ab} v^\prime_b - v_b}{\Delta{t}} \\
&= \frac{v^\prime_b + \skew{R_{ba}\omega_a} v^\prime_b \Delta{t} - v_b}{\Delta{t}} \\
&= \frac{\skew{t_{ba}}R_{ba} (\omega^\prime_a - \omega_a) + R_{ba}a_a\Delta{t} + R_{ba} \skew{\omega_a}(v_a + a_a\Delta{t})\Delta{t} + \skew{R_{ba}\omega_a} v^\prime_b \Delta{t}}{\Delta{t}} \\
&= \skew{t_{ba}}R_{ba} \dot{\omega_a} + R_{ba}a_a + R_{ba} \skew{\omega_a}(v_a + a_a\Delta{t}) + \skew{R_{ba}\omega_a} v^\prime_b \\
\end{aligned}
$$
$\Delta{t}$ は限りなく0に近づくため、さらに以下のように簡略化できます。
$$
\begin{aligned}
a_b
&= \skew{t_{ba}}R_{ba} \dot{\omega_a} + R_{ba}a_a + R_{ba} \skew{\omega_a}v_a + R_{ba} \skew{\omega_a} R_{ab}(\skew{t_{ba}}R_{ba} \omega^\prime_a + R_{ba}v_a ) \\
&= \skew{t_{ba}}R_{ba} \dot{\omega_a} + R_{ba}a_a + R_{ba} \skew{\omega_a} v_a + R_{ba} \skew{\omega_a} R_{ab}\skew{t_{ba}}R_{ba} \omega^\prime_a + R_{ba} \skew{\omega_a} v_a \\
&= \skew{t_{ba}}R_{ba} \dot{\omega_a} + R_{ba}a_a + R_{ba} \skew{\omega_a} R_{ab}\skew{t_{ba}}R_{ba} \omega^\prime_a \\
&= \skew{t_{ba}}R_{ba} \dot{\omega_a} + R_{ba}a_a - \skew{ R_{ba} \omega_a} \skew{R_{ba} \omega^\prime_a } t_{ba} \\
&\approx \skew{t_{ba}}R_{ba} \dot{\omega_a} + R_{ba}a_a - \skew{ R_{ba} \omega_a} ^2 t_{ba}
\end{aligned}
$$
ちなみに、「ブラウン大学の資料」などの昔の教科書では、この問題を解析するために剛体力学の分析方法が使われていました。私の知る限りでは、リー群を用いた数学解析でこの問題を議論する記事は初めてです。力学の分析よりも分かりやすいと思います。
本文のIMU測量の座標変換デモは、以下で公開しています。
kinematics/demo_transfrom_imu.py