Rotation

Published: 2020-02-11 | Lastmod: 2020-03-03

旋转有很多种表达形式，Active/Passive，坐标/坐标系等等。 不同的表达形式得到的矩阵表达形式有较大差异，为了避免混淆，最重要的是，给自己构建一套统一的旋转表示系统。

简单旋转 #

假设有一世界坐标系\(w\)，将\(w\)坐标系逆时针旋转45°，得到一个局部坐标系\(b\)。那么，在\(w\)坐标系下，坐标为\(\begin{pmatrix}1 \\ 0\end{pmatrix}\)的点，在\(b\)坐标系下的坐标是多少呢？

采用如下公式求得：

\[ r_b = R_{bw}r_w \]

其中，\(r_w\)表示点在\(w\)坐标系下的坐标，即\(\begin{pmatrix}1 \\ 0\end{pmatrix}\)。

而\(R_{bw}\)表示\(w\)相对于\(b\)的旋转。由于\(b\)是将\(w\)逆时针旋转45°得到的，也就是说，从\(b\)出发，转到\(w\)的角度为-45°，从而得到：

\[ R_{bw}=\begin{pmatrix}\cos\theta & -\sin\theta \\ \sin\theta & \cos\theta\end{pmatrix} =\begin{pmatrix}\cos(-45\degree) & -\sin(-45\degree) \\ \sin(-45\degree) & \cos(-45\degree)\end{pmatrix} =\begin{pmatrix}\frac{\sqrt{2}}{2} & \frac{\sqrt{2}}{2} \\ -\frac{\sqrt{2}}{2} & \frac{\sqrt{2}}{2}\end{pmatrix} \]

由此可知：

\[ r_b = R_{bw}r_w=\begin{pmatrix}\frac{\sqrt{2}}{2} \\ -\frac{\sqrt{2}}{2}\end{pmatrix} \]

这正是我们所期待的结果。

三维旋转 #

旋转都是按照roll，pitch，yaw的顺序进行的。这里假设分别对应的是X，Y，Z坐标轴。 若从\(w\)坐标系出发，通过旋转roll，pitch，yaw的角度后，得到\(b\)坐标系，则\(b\)坐标系下的点\(P_b\)，在\(w\)坐标系下的坐标为：

\[ \begin{aligned} P_w &= R_{wb} * P_b \\[2mm] &= R_z(yaw) * R_y(pitch) * R_x(roll) * P_b \\[2mm] &= \begin{pmatrix} \cos &-\sin &0 \\ \sin &\cos &0 \\ 0 &0 &1 \end{pmatrix} \begin{pmatrix} \cos &0 &\sin \\ 0 &1 &0 \\ -\sin &0 &\cos \end{pmatrix} \begin{pmatrix} 1 &0 &0 \\ 0 &\cos &-\sin \\ 0 &\sin &\cos \end{pmatrix} * P_b \end{aligned} \]

请注意Y轴对应的旋转矩阵的符号，跟X，Z轴是不同的。

右手系四元数 #

\[ q=\cos\frac{\theta}{2}+u^w\cdot\sin\frac{\theta}{2} \]

其中：\(u^w\)表示在参考系\(w\)下的某个单位矢量。\(q\)描述的是将坐标系\(w\)旋转成新的坐标系\(b\)。

注意！！！
在原始Hamilton Notation的情况下，会采用\(_w^bq\)表示将坐标系\(w\)旋转成新的坐标系\(b\)，或者采用\(q_{bw}\)。这与上一节，我们所习惯的旋转矩阵出现了不统一。按照旋转矩阵的写法，应该是\(R_{wb}\)

这在实际运算过程中会带来很多麻烦，因此，我们直接采用如下规定：
采用\(_b^wq\)表示将坐标系\(w\)旋转成新的坐标系\(b\)，或者采用\(q_{wb}\)

\[ \begin{aligned} r_w&={_b^w}q\otimes r_b \otimes {_b^w}q^{-1}\\ &=q_{wb}\otimes r_b \otimes q_{wb}^{-1}\\ &=R_{wb}r_b \end{aligned} \]

我们可以注意到，VINS，组合导航，都是采用了这套符号规定！
在《捷联惯导算法与组合导航原理讲义》一书中，\(C_b^i\)表示从坐标系\(i\)转到坐标系\(b\)，对应于我们规定的旋转矩阵应该是\(R_{ib}\)。 参考 GPS/INS组合导航。

四元数与旋转矩阵的关系 #

下面这套转换关系通过Matlab代码验证过，请参考 代码文件。

\[ \mathbf{q} = q_0 + q_1 i + q_2 j + q_3 k = [s, \mathbf{v}] \]

四元数转旋转矩阵：

\[ R = \begin{bmatrix} 1 - 2 q_2^2 - 2 q_3^2 & 2q_1 q_2 - 2q_0 q_3 & 2 q_1 q_3 + 2 q_0 q_2 \\ 2q_1 q_2 + 2q_0 q_3 & 1 - 2 q_1^2 - 2 q_3^2 & 2 q_2 q_3 - 2 q_0 q_1 \\ 2 q_1 q_3 - 2 q_0 q_2 & 2 q_2 q_3 + 2 q_0 q_1 & 1 - 2 q_1^2 - 2 q_2^2 \end{bmatrix} \]

旋转矩阵转四元数：

\[ \begin{aligned} q_0 &= \frac{\sqrt{1+r_{11}+r_{22}+r_{33}}}{2} \\[2mm] q_1 &= \frac{r_{32}-r_{23}}{4q_0} \\[2mm] q_2 &= \frac{r_{13}-r_{31}}{4q_0} \\[2mm] q_3 &= \frac{r_{21}-r_{12}}{4q_0} \end{aligned} \]

四元数乘法：

\[ \begin{aligned} \mathbf{q}_1 &= [s_1, \mathbf{v}_1]=\begin{pmatrix} a_1 & b_1 & c_1 & d_1 \end{pmatrix}^T \\[2mm] \mathbf{q}_2 &= [s_2, \mathbf{v}_2]=\begin{pmatrix} a_2 & b_2 & c_2 & d_2 \end{pmatrix}^T \\[2mm] \mathbf{q}_1 \mathbf{q}_2 &= \begin{pmatrix} a_1 a_2 - b_1 b_2 - c_1 c_2 - d_1 d_2 \\ a_1 b_2 + b_1 a_2 + c_1 d_2 - d_1 c_2 \\ a_1 c_2 - b_1 d_2 + c_1 a_2 + d_1 b_2 \\ a_1 d_2 + b_1 c_2 - c_1 b_2 + d_1 a_2 \end{pmatrix} \end{aligned} \]

四元数的逆：

\[ \mathbf{q}^* = s - \mathbf{v} = [s, -\mathbf{v}] \\[2mm] \mathbf{q}^{-1} = \frac{\mathbf{q}^*}{||\mathbf{q}||^2} \]

反对称阵：

\[ [\omega]_{\times} = \begin{pmatrix} 0 &-\omega_z &\omega_y \\ \omega_z &0 &-\omega_x \\ -\omega_y &\omega_x &0 \end{pmatrix} \]

其它常用公式：

\[ a_{\times}b=-b_{\times}a \\[2mm] (Rb)_\times=Rb_{\times}R^T \]

按照上面的这一套符号系统，可以看到旋转矩阵与四元数在进行旋转计算时，完全统一：

\[ \begin{aligned} P_c &= R_{cb}R_{ba}P_a \\[2mm] &= (\mathbf{q}_{cb} \mathbf{q}_{ba})\begin{pmatrix} 0 \\ P_a\end{pmatrix} (\mathbf{q}_{cb} \mathbf{q}_{ba})^{-1} \end{aligned} \]

四元数的导数：

\[ \begin{aligned} \dot{\mathbf{q}}&=\frac{1}{2}\mathbf{\Omega}(\mathbf{\omega}_{\mathcal{L}})\mathbf{q}=\frac{1}{2}\mathbf{q}\otimes\mathbf{\omega}_{\mathcal{L}} \\[2mm] &=\frac{1}{2} \begin{pmatrix} 0 &-\omega_x &-\omega_y & -\omega_z \\ \omega_x &0 &\omega_z &-\omega_y \\ \omega_y &-\omega_z &0 &\omega_x \\ \omega_z & \omega_y &-\omega_x &0 \end{pmatrix}\mathbf{q} \end{aligned} \]

对应的旋转矩阵导数：

\[ \dot{R}=R[\omega_{\mathcal{L}}]_{\times} \]

左手系四元数 #

通过上面的定义，右手系四元数与旋转矩阵得到了统一，而左手系四元数将会反而显得比较奇怪了。

\({_G^I}q\) is the unit quaternion describing the rotation from frame {\(G\)} to frame {\(I\)}

\[ r_I={_G^I}q \cdot r_G=R_{IG} \cdot r_G \]

MSCKF采用的是这套符号规定，看论文时需要特别注意！

Next: Control
Previous: Bayesian Filter