Rodrigues 公式推导

• Rodrigues 公式推导
  • 分解$\mathbf{v}$向量
  • 分解旋转后的向量${\mathbf{v}}_{\mathbf{r}\mathbf{o}\mathbf{t}}$
  • 综合化简
  • 从旋转矩阵得到旋转轴和旋转角
    • 求旋转角
    • 求旋转轴

Rodrigues 公式推导

rodrigues公式计算旋转轴 $\mathbf{k}$ 和旋转角度 $\theta$ 得到旋转矩阵

分解$\mathbf{v}$向量

$$\begin{array}{rl}& \mathbf{k}:旋转轴的单位方向矢量\\ & \mathbf{v}:绕\mathbf{k}旋转的任意向量\\ & 将\mathbf{v}分解为垂直和平行于旋转轴\mathbf{k}的两个向量\\ & \mathbf{v}={\mathbf{v}}_{\parallel }+{\mathbf{v}}_{\perp }--(1)\\ & {\mathbf{v}}_{\parallel }=(\mathbf{v}\cdot \mathbf{k})\cdot \mathbf{k}--(2)(括号部分是\mathbf{v}在\mathbf{k}上的投影，外部份单位向量\mathbf{k}赋予方向)\\ & {\mathbf{v}}_{\perp }=\mathbf{v}-{\mathbf{v}}_{\parallel }=\mathbf{v}-(\mathbf{v}\cdot \mathbf{k})\cdot \mathbf{k}\\ & 根据公式：\mathbf{a}\times (\mathbf{b}\times \mathbf{c})=(\mathbf{a}\cdot \mathbf{c})\cdot \mathbf{b}-(\mathbf{a}\cdot \mathbf{b})\cdot \mathbf{c},且\mathbf{k}\cdot \mathbf{k}=1,\mathbf{v}\cdot \mathbf{k}=\mathbf{k}\cdot \mathbf{v}\\ & \mathbf{v}-(\mathbf{v}\cdot \mathbf{k})\cdot \mathbf{k}\\ & =\mathbf{v}-(\mathbf{k}\cdot \mathbf{v})\cdot \mathbf{k}\\ & =-(-\mathbf{k}\cdot \mathbf{k})\cdot \mathbf{v}+(-\mathbf{k}\cdot \mathbf{v})\cdot \mathbf{k}\\ & =(-\mathbf{k}\cdot \mathbf{v})\cdot \mathbf{k}-(-\mathbf{k}\cdot \mathbf{k})\cdot \mathbf{v}\\ & =-\mathbf{k}\times (\mathbf{k}\times \mathbf{v})\\ & 即有：\\ & {\mathbf{v}}_{\perp }=-\mathbf{k}\times (\mathbf{k}\times \mathbf{v})--(3)\end{array}$$

分解旋转后的向量${\mathbf{v}}_{\mathbf{r}\mathbf{o}\mathbf{t}}$

分解了$\mathbf{v}$向量之后，再来考虑旋转之后的向量,${\mathbf{v}}_{\mathbf{r}\mathbf{o}\mathbf{t}}$，同样的可以分为垂直和平行于旋转轴$\mathbf{k}$的两个向量
$$\begin{array}{rlr}& {\mathbf{v}}_{rot}={\mathbf{v}}_{\parallel rot}+{\mathbf{v}}_{\perp rot}& \\ & {\mathbf{v}}_{\parallel rot}={\mathbf{v}}_{\parallel }--(4)& \\ |& {\mathbf{v}}_{\perp rot}|=|{\mathbf{v}}_{\perp }|垂直的分量旋转后大小不变& \\ & {\mathbf{v}}_{\perp rot}=\cos \theta \cdot {\mathbf{v}}_{\perp }+\sin \theta \cdot \mathbf{k}\times {\mathbf{v}}_{\perp }--(5)可以看做平面向量的绕原点旋转& \\ & 上式中的\mathbf{k}\times {\mathbf{v}}_{\perp }可以做如下化简计算& \\ & \mathbf{k}\times {\mathbf{v}}_{\perp }& \\ & =\mathbf{k}\times (\mathbf{v}-{\mathbf{v}}_{\parallel })& \\ & =\mathbf{k}\times \mathbf{v}+\mathbf{k}\times {\mathbf{v}}_{\parallel }显然后一项中的两个向量是平行的结果为0向量& \\ & =\mathbf{k}\times \mathbf{v}--(6)& 因此公式（5）改写为：\\ & {\mathbf{v}}_{\perp rot}=\cos \theta \cdot {\mathbf{v}}_{\perp }+\sin \theta \cdot \mathbf{k}\times \mathbf{v}--(7)& \\ & 最后综合{\mathbf{v}}_{rot}& \\ & {\mathbf{v}}_{rot}& \\ & ={\mathbf{v}}_{\parallel rot}+{\mathbf{v}}_{\perp rot}& \\ & ={\mathbf{v}}_{\parallel }+\cos \theta \cdot {\mathbf{v}}_{\perp }+\sin \theta \cdot \mathbf{k}\times \mathbf{v}--(8)& \end{array}$$

综合化简

首先给出反对称阵skew，用于代替叉积计算，再将公式（8）化简形式为${\mathbf{v}}_{rot}=\mathbf{R}\cdot \mathbf{v}$
$$\begin{array}{rl}& 反对称阵skew\\ & 令\mathbf{K}=\left[\begin{array}{ccc}0& -k_z& k_y\\ k_z& 0& -k_x\\ -k_y& k_x& 0\end{array}\right]\\ & 其中\mathbf{k}=[k_x,k_y,k_z{]}^T\\ & 将叉积转换为矩阵乘法\\ & \mathbf{K}\cdot \mathbf{v}=\mathbf{k}\times \mathbf{v}--(9)\\ & \mathbf{K}\cdot (\mathbf{K}\cdot \mathbf{v})=\mathbf{k}\times (\mathbf{k}\times \mathbf{v})={\mathbf{K}}^2\cdot \mathbf{v}--(10)\end{array}$$

将公式9和10带入式8中
$$\begin{array}{rl}& {\mathbf{v}}_{rot}\\ & ={\mathbf{v}}_{\parallel }+\cos \theta \cdot {\mathbf{v}}_{\perp }+\sin \theta \cdot \mathbf{k}\times \mathbf{v}\\ & =\mathbf{v}-{\mathbf{v}}_{\perp }+\cos \theta \cdot {\mathbf{v}}_{\perp }+\sin \theta \cdot \mathbf{K}\cdot \mathbf{v}\\ & =\mathbf{v}-(1-\cos \theta )\cdot (-\mathbf{k}\times (\mathbf{k}\times \mathbf{v}))+\sin \theta \cdot \mathbf{K}\cdot \mathbf{v}\\ & =\mathbf{v}+\sin \theta \cdot \mathbf{K}\cdot \mathbf{v}+(1-\cos \theta )\cdot {\mathbf{K}}^2\cdot \mathbf{v}\\ & =(\mathbf{I}+\sin \theta \cdot \mathbf{K}+(1-\cos \theta )\cdot {\mathbf{K}}^2)\cdot \mathbf{v}--(11)\\ & \\ & \mathbf{R}=\mathbf{I}+\sin \theta \cdot \mathbf{K}+(1-\cos \theta )\cdot {\mathbf{K}}^2--(12)\\ & {\mathbf{v}}_{rot}=\mathbf{R}\cdot \mathbf{v}--(13)\end{array}$$

式11即为rodrigues公式，实现了旋转矩阵与旋转轴、旋转角度之间的转换

从旋转矩阵得到旋转轴和旋转角

求旋转角

通过公式12可以很容易根据旋转轴和旋转角得到相应的旋转向量
而反求则需要重新推导式12
$$\begin{array}{rl}& {\mathbf{v}}_{rot}\\ & ={\mathbf{v}}_{\parallel }+\cos \theta \cdot {\mathbf{v}}_{\perp }+\sin \theta \cdot \mathbf{k}\times \mathbf{v}此时分解{\mathbf{v}}_{\perp }\\ & ={\mathbf{v}}_{\parallel }+\cos \theta \cdot (\mathbf{v}-{\mathbf{v}}_{\parallel })+\sin \theta \cdot \mathbf{k}\times \mathbf{v}\\ & =\cos \theta \cdot \mathbf{v}+(1-\cos \theta )\cdot {\mathbf{v}}_{\parallel }+\sin \theta \cdot \mathbf{K}\cdot \mathbf{v}根据公式2替换{\mathbf{v}}_{\parallel }\\ & =\cos \theta \cdot \mathbf{v}+(1-\cos \theta )\cdot (\mathbf{v}\cdot \mathbf{k})\cdot \mathbf{k}+\sin \theta \cdot \mathbf{K}\cdot \mathbf{v}\\ & 已知等式成立：(\mathbf{v}\cdot \mathbf{k})\cdot \mathbf{k}=\mathbf{k}{\mathbf{k}}^T\cdot \mathbf{v}\\ & 原式\\ & =\cos \theta \cdot \mathbf{v}+(1-\cos \theta )\cdot \mathbf{k}{\mathbf{k}}^T\cdot \mathbf{v}+\sin \theta \cdot \mathbf{K}\cdot \mathbf{v}\\ & =(\cos \theta +(1-\cos \theta )\cdot \mathbf{k}{\mathbf{k}}^T+\sin \theta \cdot \mathbf{K})\cdot \mathbf{v}\\ & \mathbf{R}=\cos \theta \cdot \mathbf{I}+(1-\cos \theta )\cdot \mathbf{k}{\mathbf{k}}^T+\sin \theta \cdot \mathbf{K}\end{array}$$

重新推导后的R与原来的形式结果是一致的
然后使用矩阵的迹求解旋转角
$$\begin{array}{rl}& tr(\mathbf{R})=tr(\cos \theta \cdot \mathbf{I}+(1-\cos \theta )\cdot \mathbf{k}{\mathbf{k}}^T+\sin \theta \cdot \mathbf{K})\\ & tr(\mathbf{R})=\cos \theta \cdot tr(\mathbf{I})+(1-\cos \theta )\cdot tr(\mathbf{k}{\mathbf{k}}^T)+\sin \theta \cdot tr(\mathbf{K})\\ & tr(\mathbf{I})=3,tr(\mathbf{k}{\mathbf{k}}^T)={\mathbf{k}}^T\mathbf{k}=1,tr(\mathbf{K})=0\\ & tr(\mathbf{R})=3\cos \theta +(1-\cos \theta )=1+2\cos \theta \\ & \theta =\arccos (\frac{tr(\mathbf{R})-1}{2})\end{array}$$

求旋转轴

通过matlab的符号运算可以得到R的各个元素表示

clear;
clc;
s=sym('s'); %sin theta
c=sym('c'); %cos theta
kx=sym('kx');
ky=sym('ky');
kz=sym('kz');
K=[0,-kz,ky;kz,0,-kx;-ky,kx,0];
I=eye(3);
R=I+s*K+(1-c)*K*K;

得到的R =

[ (c - 1)ky^2 + (c - 1)kz^2 + 1, - kzs - kxky*(c - 1), kys - kxkz*(c - 1)]
[ kzs - kxky*(c - 1), (c - 1)kx^2 + (c - 1)kz^2 + 1, - kxs - kykz*(c - 1)]
[ - kys - kxkz*(c - 1), kxs - kykz*(c - 1), (c - 1)*kx^2 + (c - 1)*ky^2 + 1]

可以发现其对角线左右的元素相减可以得到kx ky kz
$$\begin{array}{rl}& \mathbf{R}(3,2)-\mathbf{R}(2,3)=2\sin \theta \cdot k_x\\ & \mathbf{R}(1,3)-\mathbf{R}(3,1)=2\sin \theta \cdot k_y\\ & \mathbf{R}(2,1)-\mathbf{R}(1,2)=2\sin \theta \cdot k_z\end{array}$$
即可求得旋转轴的单位向量