ロボット技術者向け リー群の速習(3) 並進速度と角速度の剛体変換

# はじめに

__問題： 3次元の空間中に運動する物体AとBがある。Aの並進速度および角速度は既知であり、AとBが同じ剛体に取り付けられているという前提で、Bの並進速度と角速度を計算せよ。__

この問題は、ロボットの世界においてよく見られる。たとえば、オドメトリから得られた車輪中心の速度が既知であり、車両に取り付けたLiDARの速度を計算する場合や、ロボットアームの関節速度から指先の速度を計算する場合などが挙げられる。

一見簡単に見える問題だが、解くためにはリー群の知識が必要であり、容易ではない。本文は[前編](https://qiita.com/scomup/items/d82b35aeb1ecabd75e60)で議論したリー群の知識を用いて、この問題の解き方と答えを説明する。

_ちなみにこの問題は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の速度がまったく異なることがわかるよね。

<img src="https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/149168/b87431b7-3730-38f8-dd3b-ec6ee5437b60.gif" width="600"/>

_[可視化の実装](https://github.com/scomup/MathematicalRobotics/blob/main/kinematics/demo_transfrom_velocity.py)もgithubにアップロードしている。速度の定義(4)(6)から得られたBの速度も表示している。_

## ポーズの変化

### Aのポーズの変化

速度を解く前に、まず、ポーズ（位置と姿勢）の変化を考えよう。[前編](https://qiita.com/scomup/items/d82b35aeb1ecabd75e60)に説明した通り、定速度で運動した場合、$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}$は微小回転の回転角を求める([回転群を参考](https://qiita.com/scomup/items/fa9aed8870585e865117))。そのため、瞬間の角速度$\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

https://github.com/scomup/MathematicalRobotics