はじめに
問題： 3次元の空間中に運動する物体AとBがある。Aの並進速度および角速度は既知であり、AとBが同じ剛体に取り付けられているという前提で、Bの並進速度と角速度を計算せよ。
この問題は、ロボットの世界においてよく見られる。たとえば、オドメトリから得られた車輪中心の速度が既知であり、車両に取り付けたLiDARの速度を計算する場合や、ロボットアームの関節速度から指先の速度を計算する場合などが挙げられる。
一見簡単に見える問題だが、解くためにはリー群の知識が必要であり、容易ではない。本文は前編で議論したリー群の知識を用いて、この問題の解き方と答えを説明する。
ちなみにこの問題はChatGPTに聞いたら、でたらめな回答をするので、要注意です。この文章を投稿したら、ChatGPTも本文を学習して、進化するかな。。。
問題の定式表現
$$
\newcommand{\skew}[1]{[{#1}]_{\times}} %skew matrix
\newcommand{\so}[1]{ \mathfrak{so}{(#1)} } %lie algebra so3
\newcommand{\se}[1]{ \mathfrak{se}{(#1)} } %lie algebra se3
\newcommand{\norm}[1]{|{#1}|} %norm
$$
AとBは1つの剛体に取り付けられており、AからBの剛体変換を$T_{ba}$で表す。Aの並進速度と角速度$\xi_{a}$が既知の場合、Bの並進速度と角速度$\xi_{b}$を計算します。
- $T_{ba}$: AをBに変換する剛体変換行列
- $\xi_{a}$: Aのローカル座標系におけるAの並進速度および角速度（6次元のベクトル）
- $v_a$： x, y, z方向の並進速度（3次元のベクトル）
- $\omega_a$： x, y, z軸の回りの角速度（3次元のベクトル）
$$
\xi_{a}=
\left[\begin{matrix}
v_a \\
\omega_a
\end{matrix}\right]
$$
- $\xi_{b}$: Bのローカル座標系におけるBの並進速度および角速度（6次元のベクトル）
可視化
以下の図はこの問題の一例を可視化したものである。A(赤い球体)とB(緑の球体)は1つのロボットアームに取り付けられており、Aが動くとBも連動する。視覚的にもAとBの速度がまったく異なることがわかるよね。
可視化の実装もgithubにアップロードしている。速度の定義(4)(6)から得られたBの速度も表示している。
ポーズの変化
Aのポーズの変化
速度を解く前に、まず、ポーズ（位置と姿勢）の変化を考えよう。前編に説明した通り、定速度で運動した場合、$T_{aa'}$（Aのポーズの変化）は$\xi_a\Delta{t}$の指数写像より計算できる。さらに$\Delta{t}$が十分小さい場合、指数写像の2次以上の項は省略でき、式(1)のように表現できる。
$$
T_{aa'} = \exp{(\widehat{\xi \Delta{t}})}
=\left[\begin{matrix}
I + \skew{\omega} \Delta{t} & v\Delta{t}\\
\mathbf{0}^T & 1
\end{matrix}\right]
\tag{1}
$$
- $\Delta{t}$: 微小の時間を示す。
- $'$: $\Delta{t}$秒後の状態であることを示す。
Bのポーズの変化
AとBは同じ剛体に取り付けられているため、$\Delta{t}$秒後の剛体変換$T_{b'a'}$と$T_{ba}$は一致する。$T_{bb'}$（Bのポーズの変化）は$T_{aa'}$からの剛体変換として計算できる。
$$
\begin{aligned}
T_{bb'} &= T_{ba} T_{aa'} T_{a'b'}= T_{ba} T_{aa'} T_{ba}^{-1} \\
&=
\left[\begin{matrix}
R_{ba} & t_{ba} \\
\mathbf{0}^T & 1
\end{matrix}\right]
\left[\begin{matrix}
R_{aa'} & t_{aa'} \\
\mathbf{0}^T & 1
\end{matrix}\right]
\left[\begin{matrix}
R_{ba}^T & -R_{ba}^T t_{ba} \\
\mathbf{0}^T & 1
\end{matrix}\right]\\
&=
\left[\begin{matrix}
R_{ba}R_{aa'}R_{ba}^T
& R_{ba}R_{aa'}(-R_{ba}^Tt_{ba})+R_{ba}t_{aa'}+t_{ba} \\
\mathbf{0}^T & 1
\end{matrix}\right]
\end{aligned}
\tag{2}
$$
式(1)を式(2)に代入すると、$T_{bb'}$の回転部と並進部は式(3)に表現できる。
$$
\begin{aligned}
T_{bb'}
&=
\left[\begin{matrix}
R_{bb'} & t_{bb'} \\
\mathbf{0}^T & 1
\end{matrix}\right]
\\
&=\left[\begin{matrix}
R_{ba}(I + \skew{\omega}\Delta{t})R_{ba}^T
& R_{ba}(I + \skew{\omega}\Delta{t})(-R_{ba}^Tt_{ba})+R_{ba}v\Delta{t}+t_{ba} \\
\mathbf{0}^T & 1
\end{matrix}\right]
\end{aligned}
\tag{3}
$$
並進速度
次は並進速度を考えよう。
物体の座標の変化量を$\Delta{x}$とすると、物体の並進速度 $v$ は以下のように定義する。
$$
v = \lim_{\Delta{t} \to 0} \frac{\Delta{x}}{\Delta{t}}
\tag{4}
$$
式(3)の並進部分$t_{bb'}$を式(4)に代入すると、Bの並進速度が式(5)のように導出できる。
$$
\begin{aligned}
v_{bb'}
&= \lim_{\Delta{t} \to 0} \frac{t_{bb'}}{\Delta{t}} \\
&= \lim_{\Delta{t} \to 0} \frac{R_{ba}(I + \skew{\omega}\Delta{t})(-R_{ba}^Tt_{ba})+R_{ba}v\Delta{t}+t_{ba}}{\Delta{t}} \\
&= -R_{ba}\skew{\omega} R_{ba}^T t_{ba} + R_{ba} v \\&= R_{ba}\skew{ R_{ba}^T t_{ba}} \omega + R_{ba} v \\
&= \skew{t_{ba}}R_{ba} \omega + R_{ba} v \\
\end{aligned}
\tag{5}
$$
$R \in SO(3) $の場合、交代行列は以下の性質がある。この性質を利用することで、上記の4行目から5行目に変換できる。
$$
\skew{Rt} = R \skew{t} R^T
$$
角速度
姿勢の変化は$\Delta{R}$が十分小さいとき、$\log{(\Delta{R})}^{\vee}$は微小回転の回転角を求める(回転群を参考)。そのため、瞬間の角速度$\omega$は以下のように定義できる。
$$
\omega = \lim_{\Delta{t} \to 0} \frac{\log{(\Delta{R})}^{\vee}}{\Delta{t}}
\tag{6}
$$
式(3)の回転部分$R_{bb'}$を式(6)に代入すると、Bの回転速度が式(7)のように導出できる。
$$
\begin{aligned}
\omega_b
&= \lim_{\Delta{t} \to 0} \frac{\log{(R_{bb'})}^{\vee}}{\Delta{t}} \\
&= \lim_{\Delta{t} \to 0} \frac{\log{(R_{ba}(I + \skew{\omega} \Delta{t} )R_{ba}^T)}^{\vee}}{\Delta{t}} \\
&= \lim_{\Delta{t} \to 0} \frac{\log{(I+R_{ba}\skew{\omega} R_{ba}^T \Delta{t})}^{\vee}}{\Delta{t}} \\
&= \lim_{\Delta{t} \to 0} \frac{\log{(I+\skew{R_{ba}\omega \Delta{t}} )}^{\vee}}{\Delta{t}} \\
&= \lim_{\Delta{t} \to 0} \frac{R_{ba}\omega \Delta{t}}{\Delta{t}} \\
&= R_{ba}\omega\\
\end{aligned}
\tag{7}
$$
並進速度と角速度
上記の説明で、並進速度(5)と角速度(7)の計算式をすべて導出した。さらに、式(8)のように1つの式にまとめることで、行列の掛け算1回で、Bの並進速度および角速度を計算することができる。
$$
\begin{aligned}
\xi_b =
\left[\begin{matrix}
v_b \\
\omega_b
\end{matrix}\right]
&=
\left[\begin{matrix}
R_{ba} & \skew{t_{ba}}R_{ba} \\
\mathbf{0} & R_{ba} \
\end{matrix}\right]
\left[\begin{matrix}
v_a \\
\omega_a
\end{matrix}\right]
\end{aligned}
\tag{8}
$$
まとめ
これで、リー群の知識を用いて、本問題の答えとその証明をすべて示した。最後の式はシンプルで美しいよね。実際のアプリケーションではとても有用な結論でした。下記のpythonデモでは、式(8)から得られた結果と速度の定義(4)(6)から推定した結果を比較することできる。$\Delta{t}$が小さくなると、これら２つの方法から得られた結果はほぼ一致し、正解であることが保証される。
速度の剛体変換のデモ：
kinematics/demo_transfrom_velocity.py