# 1. 坐标系的线速度和角速度

$w_s=\hat{w}\dot{\theta} \tag{1}$

\begin{aligned} \dot{\hat{x}}&=w_s \times \hat{x} \\ \dot{\hat{y}}&=w_s \times \hat{y} \\ \dot{\hat{z}}&=w_s \times \hat{z} \end{aligned} \tag{2}

$\dot{R}= \begin{bmatrix} w_s \times \hat{x} & w_s \times \hat{y} & w_s \times \hat{z} \end{bmatrix}=w_s \times R \tag{3}$

$[x]=\left[\begin{array}{ccc} 0 & -x_{3} & x_{2} \\ x_{3} & 0 & -x_{1} \\ -x_{2} & x_{1} & 0 \end{array}\right]\tag{4}$
$[x]=-[x]^T \tag{5}$

\begin{aligned} R[w]R^T &= R\begin{bmatrix} w_s\times r_1 & w_s\times r_2 &w_s\times r_3 \end{bmatrix}\\ &= \begin{bmatrix} r_1^{T}(w_s\times r_1) & r_1^T(w_s\times r_2) & r_1^T (w_s\times r_3) \\ r_2^{T}(w_s\times r_1) & r_2^T(w_s\times r_2) & r_2^T (w_s\times r_3) \\ r_3^{T}(w_s\times r_1) & r_3^T(w_s\times r_2) & r_3^T (w_s\times r_3) \end{bmatrix} \\ &= \begin{bmatrix} 0 & -r_3^Tw_s & r_2^Tw_s\\ r_3^Tw_s & 0 &-r_1^Tw_s\\ -r_2^Tw_s&r_1^Tw_s&0 \end{bmatrix}\\ &=[Rw_s] \end{aligned} \tag{6}

$\dot{R}=[w_s]R \tag{7}$

$[w_s]=\dot{R}R^{-1} \tag{8}$

$w_s=R_{sb}w_b\tag{9}$

$w_b=R_{sb}^{-1}w_s=R^{-1}w_s=R^{T}w_s \tag{10}$

\begin{aligned} [w_b]&=[R^{T}w_s] \\ &= R^T[w_s]R \\ &= R^T(\dot{R}R^{T})R \\ &=R^T\dot{R} \\ &=R^{-1}\dot{R} \end{aligned}\tag{11}

# 2. 微分方程的解

$\dot x(t)=ax(t) \tag{12}$

$x(t)=e^{at}x(0) \tag{13}$
$e^{at}$$0$附近进行泰勒展开，可得，
$e^{a t}=1+a t+\frac{(a t)^{2}}{2 !}+\frac{(a t)^{3}}{3 !}+\cdots \tag{14}$

$\dot x(t)=Ax(t) \tag{15}$

$x(t)=e^{At}x(0) \tag{16}$

$e^{A t}=1+A t+\frac{(A t)^{2}}{2 !}+\frac{(A t)^{3}}{3 !}+\cdots \tag{17}$

# 3. 指数形式的旋转

$\dot{p}= \hat{\omega} \times p \tag{18}$

$\dot{p}= [\hat{\omega}] p \tag{19}$

$p(t)=e^{[\hat{\omega}] t} p(0) \tag{20}$

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

$[\hat{\omega}][\hat{\omega}]=\hat{\omega}\hat{\omega}^{T}-I \tag{22}$

$[\hat{\omega}][\hat{\omega}][\hat{\omega}]= [\hat{\omega}]^{3}=-[\hat{\omega}] \tag{23}$

\begin{aligned} e^{[\hat{\omega}] \theta} &=I+[\hat{\omega}] \theta+[\hat{\omega}]^{2} \frac{\theta^{2}}{2 !}+[\hat{\omega}]^{3} \frac{\theta^{3}}{3 !}+\cdots \\ &=I+\left(\theta-\frac{\theta^{3}}{3 !}+\frac{\theta^{5}}{5 !}-\cdots\right)[\hat{\omega}]+\left(\frac{\theta^{2}}{2 !}-\frac{\theta^{4}}{4 !}+\frac{\theta^{6}}{6 !}-\cdots\right)[\hat{\omega}]^{2} \\ &= I+sin(\theta)[\hat{\omega}]+(1-cos(\theta))[\hat{\omega}]^{2} \end{aligned} \tag{24}

$\operatorname{Rot}(\hat{\omega}, \theta)=e^{[\hat{\omega}] \theta}=I+\sin \theta[\hat{\omega}]+(1-\cos \theta)[\hat{\omega}]^{2} \in S O(3) \tag{25}$

# 4. 旋转矩阵的对数

$\begin{array}{cll} \exp : & [\hat{\omega}] \theta \in s o(3) & \rightarrow & R \in S O(3) \\ \log : & R \in S O(3) & \rightarrow & [\hat{\omega}] \theta \in \operatorname{so}(3) \end{array}$

$\left[\begin{array}{ccc} c_{\theta}+\hat{\omega}_{1}^{2}\left(1-c_{\theta}\right) & \hat{\omega}_{1} \hat{\omega}_{2}\left(1-c_{\theta}\right)-\hat{\omega}_{3} \mathrm{s}_{\theta} & \hat{\omega}_{1} \hat{\omega}_{3}\left(1-c_{\theta}\right)+\hat{\omega}_{2} \mathrm{s}_{\theta} \\ \hat{\omega}_{1} \hat{\omega}_{2}\left(1-\mathrm{c}_{\theta}\right)+\hat{\omega}_{3} \mathrm{S}_{\theta} & c_{\theta}+\hat{\omega}_{2}^{2}\left(1-c_{\theta}\right) & \hat{\omega}_{2} \hat{\omega}_{3}\left(1-\mathrm{c}_{\theta}\right)-\hat{\omega}_{1} \mathrm{s}_{\theta} \\ \hat{\omega}_{1} \hat{\omega}_{3}\left(1-\mathrm{c}_{\theta}\right)-\hat{\omega}_{2} \mathrm{S}_{\theta} & \hat{\omega}_{2} \hat{\omega}_{3}\left(1-\mathrm{c}_{\theta}\right)+\hat{\omega}_{1} \mathrm{s}_{\theta} & \mathrm{c}_{\theta}+\hat{\omega}_{3}^{2}\left(1-\mathrm{c}_{\theta}\right) \end{array}\right] \tag{26}$

\begin{aligned} r_{32}-r_{23}&=2 \hat{\omega}_{1}sin(\theta) \\ r_{13}-r_{31}&=2 \hat{\omega}_{2}sin(\theta) \\ r_{21}-r_{12}&=2 \hat{\omega}_{3}sin(\theta) \end{aligned} \tag{27}

\begin{aligned} \hat{\omega}_{1}=\frac{1}{2sin(\theta)}(r_{32}-r_{23}) \\ \hat{\omega}_{2}=\frac{1}{2sin(\theta)}(r_{13}-r_{31}) \\ \hat{\omega}_{3}=\frac{1}{2sin(\theta)}(r_{21}-r_{12}) \end{aligned} \tag{28}

$[\hat{\omega}]=\left[\begin{array}{ccc} 0 & -\hat{\omega}_{3} & \hat{\omega}_{2} \\ \hat{\omega}_{3} & 0 & -\hat{\omega}_{1} \\ -\hat{\omega}_{2} & \hat{\omega}_{1} & 0 \end{array}\right]=\frac{1}{2 \sin \theta}\left(R-R^{\mathrm{T}}\right) \tag{29}$

$\operatorname{tr} R=r_{11}+r_{22}+r_{33}=1+2 \cos \theta \tag{30}$

1. $\theta=k\pi$，且$k$是偶数的情况下，此时相当于没有旋转，回到了原位置，$R=I$
2. $\theta=k\pi$，且$k$是奇数的情况下，此时有，
$R=e^{[\hat{\omega}] \pi}=I+2[\hat{\omega}]^{2} \tag{31}$
因为式(31)三个矩阵都是对角矩阵，所以可以得到下面的结果(利用$R$对角元素)
$\hat{\omega}_{i}=\pm \sqrt{\frac{r_{i i}+1}{2}}, \quad i=1,2,3 \tag{32}$
利用$R$非对角元素，可得，
\begin{aligned} 2 \hat{\omega}_{1} \hat{\omega}_{2} &=r_{12} \\ 2 \hat{\omega}_{2} \hat{\omega}_{3} &=r_{23} \\ 2 \hat{\omega}_{1} \hat{\omega}_{3} &=r_{13} \end{aligned} \tag{33}
利用式(32)和式(33)我们就能计算出$\hat{\omega}$，同时此时旋转的角为$\theta=\pm \pi, \pm 3 \pi, \ldots$
从上面的计算过程很容易看出来，旋转角度是以$2\pi$为周期，其实也是符合物理意义的，旋转$\pi$和旋转$3\pi$的效果是一样的，因此我们可以将旋转的角度限定在$[-\pi,\pi]$。此时计算的$\hat{\omega}\theta$的长度是$\le\pi$的。因此我们可以把$S O (3)$想象为一个半径为$\pi$实心球，如下图所示，

©️2019 CSDN 皮肤主题: 创作都市 设计师: CSDN官方博客