ぱたへね

はてなダイアリーはrustの色分けができないのでこっちに来た

ロドリゲスの回転公式

Robotics

教科書から分かったところだけをまとめています。

教科書はこの本です。

www.amazon.co.jp

前回分からなかった所はここ。

 T_{04} = e^{[S_3]\theta_3}M

eの肩に行列がくる表現は、行列指数関数といってこの動画が分かりやすかったです。

www.youtube.com

肩に3x3の正方行列が乗っている時の

 e^{[S]\theta_3}

は、3x3の行列になります。肩の行列が4x4の場合は、4x4の行列になります。θはスカラーに注意してください。試験に出ます。

回転を行列指数関数で表す。

教科書p71の図3.11より。

f:id:natsutan:20211214234528p:plain
Figure 3.11

点p(0)が、点p(θ)まで、ωを軸にθ回転する時を考えます。

p(\theta) =  e^{[\hat{\omega}]\theta} p(0)

最初見た時はなんだこれはと思いましたが、行列指数関数だと思うとふむふむって思います。

行列指数関数が上手いことsinとcosになります。

Rot(\hat{\omega}, \theta) =  e^{[\hat{\omega}]\theta}  = I + \sin \theta [\hat{\omega}] + (1-\cos \theta)[\hat{\omega} ]^2

上手いことの計算は教科書をみてください。

これがロドリゲスの回転公式（Rodrigues' formula for rotations）です。

実際の計算例

教科書の図3.12から

f:id:natsutan:20211215224932p:plain
Figure 3.12

フレームbが、ω=(0, 0.866, 0.5)を軸にθ=30°回転した時を考えます。ωは単位ベクトルになっています。もし、単位ベクトルでなかったら単位ベクトルにしてから計算します。

R =  e^{[\hat{\omega_1}]\theta_1}  = I + \sin \theta_1 [\hat{\omega_1}] + (1-\cos \theta_1)[\hat{\omega_1} ]^2

sin θ1はsin(30deg)なので0.5, (1-cos θ1)は0.134になります。 [ω1]を行列にするにはこのルールでOK。

 [\hat \omega ]  =  \begin{bmatrix}  0 & -\hat \omega_3 &  \hat \omega_2 \\  \hat \omega_3 & 0 & - \hat \omega_1  \\  -\hat \omega_2  & \hat \omega_1 & 0 \end{bmatrix}  

R =  I + 0.5   \begin{bmatrix}  0 & 0.5 &  0.866 \\  0.5 & 0 & 0  \\  -0.866  & 0 & 0 \end{bmatrix} + 0.134   \begin{bmatrix}  0 & 0.5 &  0.866 \\  0.5 & 0 & 0  \\  -0.866  & 0 & 0 \end{bmatrix}^2 =  \begin{bmatrix}  0.866 & -0.250 &  0.433 \\  0.250 & 0.967 & 0.058  \\  -0.433  & 0.058 & 0.899 \end{bmatrix}

手計算は間違えそうなので、この辺からPythonで計算してます。

import numpy as np

I = np.identity(3)
omega = np.array([
  [0, -0.5, 0.866],
  [0.5, 0, 0],
  [-0.866, 0, 0]
])

R = I + np.sin(0.524) * omega + (1-np.cos(0.524)) * np.dot(omega, omega)
print(R)

上で書いた式を展開すると、このような行列になります。

f:id:natsutan:20211215231230p:plain

ちょっとTexで書く根性が無かったので教科書73ページの(3.52)から画像で持ってきました。