罗德里格斯公式(旋转矩阵)推导

文章目录

• • • 1. 推导
    • 2. 性质
    • 3. 参考

1. 推导

r作为旋转轴，\theta作为旋转角度。
我们先对旋转轴进行了单位化处理。
数学表达式为：
u = \frac{r}{\| r \|}
旋转变换可分解为垂直于轴向和平行于轴向两个部分。
即相当于计算 p向量在 u 方向上的投影值。

引用投影矩阵
P=A(A^{\top}A)^{-1}A^{\top}\\ P_u=u(u^{\top}u)^{-1}u^{\top}\\ u^{\top}u=I\\ P_u=uu^{\top}
其实也可以直接以向量的方式思考
u^{\top}p =u *p=||u|| \ ||p|| \cos \alpha
还需要除||u||
||u||=u^{\top}u \\ \frac{u^{\top}p}{u^{\top}u}
最后将值转换为向量
u\frac{u^{\top}p}{u^{\top}u}=uu^{\top}p
令a为p在旋转轴r上的分量
a=p_{\parallel}=uu^{\top}p
令b为p垂直旋转轴r的分量
b=p_{\perp}=p-a=p-uu^{\top}p
令c=u \times p,则||c|| = ||p||

b旋转\theta后得到b'

b'=b\cos \theta+c \sin \theta\\ b'=(p-uu^{\top}p) \cos \theta + u\times p \sin \theta

经过旋转变换后的变量p'等于a+b即$$
p' = a + b = uu^\top p + (p - uu^\top p)\cos\theta + u\times p\sin\theta

进一步推导可得

p' = [I\cos\theta + (1-\cos\theta)uu^\top + u_\times\sin\theta] p

其中叉乘矩阵的形式为

u_\times =
\begin{bmatrix}
0 & z & -y \
-z & 0 & x \
y & -x & 0
\end{bmatrix}

因此可以得到旋转矩阵

R = I\cos\theta + (1-\cos\theta)uu^\top + u_\times\sin\theta

#### 2\. 性质 证明性质一：任意两个旋转矩阵都是相似的，并且属于正交群。 相似矩阵具有以下性质： $A=C^{-1}BC$ 因此所有旋转矩阵均具有相同的迹值。 考虑一个具体的三维旋转矩阵 $\begin{bmatrix} \cos θ & -\sin θ & 0\\ \sin θ & \cos θ & 0\\ 0 & 0 & 1 \end{bmatrix}$ 由此可得 $tr(R)=2\cos θ+1$ #### 3\. 参考 [michigan_state_university](https://www.egr.msu.edu/~vaibhav/teaching/robotics/lec13.pdf) -duke university (https://courses.cs.duke.edu/fall13/compsci527/notes/rodrigues.pdf) -mathematics stackexchange (https://math.stackexchange.com/questions/3510272/why-should-the-trace-of-a-3d-rotation-matrix-have-these-properties)