Android 矢量旋转动画 旋转矢量应用

 

1、前言

从事导航、制导或者控制时，经常需要将各个物理矢量从A坐标系转换至B坐标系，在这里涉及到的坐标系旋转常用欧拉角、旋转矩阵、旋转矢量或者四元数进行表示。

2、旋转矩阵

任意坐标系${{O}_{xyz}}$和${{O}_{x$之间转换关系，都可以通过一个3X3的旋转矩阵（旋转矢量/四元数）来描述。例如矢量$V$在坐标系${{O}_{xyz}}$中的分量为$\mathbf{V}={{\left[ x,y,z \right]}^{T}}$，则在${{O}_{x$中的分量${{\mathbf{V}}_{n}}$为：
${{\mathbf{V}}_{n}}={{\mathbf{R}}_{3\times 3}}\mathbf{V}=\left[ \begin{matrix} {{R}_{11}} & {{R}_{12}} & {{R}_{13}} \\ {{R}_{21}} & {{R}_{22}} & {{R}_{23}} \\ {{R}_{31}} & {{R}_{32}} & {{R}_{33}} \\ \end{matrix} \right]\left[ \begin{matrix} x \\ y \\ z \\ \end{matrix} \right]$

****注意：****坐标变换和刚体旋转不是同一回事，区别和联系如下

（1）坐标变换，是刚体不动，坐标系进行旋转

（2）刚体旋转，是坐标系不动，对在坐标系中的刚体进行旋转

（3）坐标变换和刚体旋转是互逆过程，比如，刚体绕X轴旋转了10°，可以等效为坐标系绕着X轴旋转了-10°

（4）在不特殊说明的情况下坐标旋转均指坐标系旋转

上述旋转矩阵可以分解为旋转三次，比如常见的旋转顺序先偏航、再俯仰最后滚转，其拆解如下：
坐标系绕着X、Y和Z轴旋转的基元旋转矩阵分别是：

（1）绕X轴旋转矩阵（滚转角度$\phi$)

${{\mathbf{R}}_{x}}\left( \phi \right)=\left[ \begin{matrix} 1 & 0 & 0 \\ 0 & \cos \phi & \sin \phi \\ 0 & -\sin \phi & \cos \phi \\ \end{matrix} \right]$

（2）绕Y轴旋转矩阵(俯仰角$\theta$)
${{\mathbf{R}}_{y}}\left( \theta \right)=\left[ \begin{matrix} \cos \theta & 0 & -\sin \theta \\ 0 & 1 & 0 \\ \sin \theta & 0 & \cos \theta \\ \end{matrix} \right]$

（3）绕Z轴旋转矩阵(偏航角$\psi$)
${{\mathbf{R}}_{z}}\left( \psi \right)=\left[ \begin{matrix} \cos \psi & \sin \psi & 0 \\ -\sin \psi & \cos \psi & 0 \\ 0 & 0 & 1 \\ \end{matrix} \right]$

任何两个坐标系之间的关系都可以通过若干次上述基元旋转来实现。
比如从导航系沿着zyx三轴顺序旋转至机体坐标系，那么从导航系至机体坐标系的旋转矩阵如下：
${{\mathbf{R}}_{bn}}={{\mathbf{R}}_{x}}\left( \phi \right){{\mathbf{R}}_{y}}\left( \theta \right){{\mathbf{R}}_{z}}\left( \psi \right)$

其中，${{\mathbf{R}}_{bn}}$表示导航坐标系$n$到机体坐标系$b$的旋转矩阵，注意下标是从右往左，表明矩阵是左乘，旋转矩阵的乘法为左乘。
也就最终的旋转矩阵，通过 左乘 获得。

---

优点：无奇点，理解起来较为直观；

缺点：有九个参数(3×3的方阵)，过于冗杂，使得计算复杂度增加

3、欧拉角

上文中旋转拆解的三个角度俯仰、偏航、滚转即为欧拉角度。

欧拉角适用于小机动无人机或车类运动，因为其俯仰角度或者滚转角度就不会超过90度。

---

优点：参数少，几何上较为直观；

缺点：存在奇点（万向锁问题，例如先仰45°再俯90°，这与先俯90°再仰45°是等价的。事实上，一旦选择±90°作为俯角，就会导致第一次旋转和第三次旋转等价，整个旋转表示系统被限制在只能绕竖直轴旋转，丢失了一个表示维度。这种角度为±90°的第二次旋转使得第一次和第三次旋转的旋转轴相同的现象，称作万向锁）

4、旋转矢量

旋转矢量也叫轴角，顾名思义就是绕某条单位轴旋转一定角度，从这个意义上看，它与四元数互相转换相当方便。
$\mathbf{v}=\sigma \mathbf{n}={{\left[ \sigma l,\sigma m,\sigma n \right]}^{T}}$
其中$\sigma$为旋转角度，$n$为旋转矢量，其四元数即可以表达为：
$\begin{align} & \mathbf{Q}={{\left[ {{q}_{0}},{{q}_{1}},{{q}_{2}},{{q}_{3}} \right]}^{T}} \\ & \text{=}\left[ \begin{matrix} \cos \frac{\sigma }{2} & \mathbf{n}\sin \frac{\sigma }{2} \\ \end{matrix} \right] \\ & =\left[ \begin{matrix} \cos \frac{\sigma }{2} & l\sin \frac{\sigma }{2} & m\sin \frac{\sigma }{2} & n\sin \frac{\sigma }{2} \\ \end{matrix} \right] \end{align}$

假设同一个坐标系中的两个矢量p和q，计算让p和q重合的旋转矩阵。在这里我们利用旋转矢量则非常简单，只要绕着同时垂直p和q的矢量n旋转σ角度即可，因此现在将问题转换为计算垂直矢量和旋转角度**（PX4里面多旋翼姿态控制即利用这点）**。

（1）垂直矢量n，可以通过p和q的叉乘实现，并做归一化
$\mathbf{n}=\frac{\mathbf{p}\times \mathbf{q}}{\left\| \mathbf{p}\times \mathbf{q} \right\|}$
（2）旋转角度σ，可以通过点乘或叉乘的定义来计算
$\begin{align} & \left\| \mathbf{p}\times \mathbf{q} \right\|\text{=}\left\| \mathbf{p} \right\|\left\| \mathbf{q} \right\|\sin \sigma \\ & \mathbf{p}\cdot \mathbf{q}=\left\| \mathbf{p} \right\|\left\| \mathbf{q} \right\|\cos \sigma \end{align}$
因此旋转角度为:
$\sigma ={{\sin }^{-1}}\frac{\left\| \mathbf{p}\times \mathbf{q} \right\|}{\left\| \mathbf{p} \right\|\left\| \mathbf{q} \right\|}={{\cos }^{-1}}\frac{\mathbf{p}\cdot \mathbf{q}}{\left\| \mathbf{p} \right\|\left\| \mathbf{q} \right\|}=\tan {{2}^{-1}}\left( \left\| \mathbf{p}\times \mathbf{q} \right\|,\mathbf{p}\cdot \mathbf{q} \right)$
然后通过利用下述公式将旋转矢量转为旋转矩阵，其公式如下：
$\mathbf{R}\left( \mathbf{n},\sigma \right)=\text{expm}\left( \sigma {{\mathbf{n}}_{\times }} \right)=I+{{\mathbf{n}}_{\times }}\sin \sigma +{{\mathbf{n}}_{\times }}{{\mathbf{n}}_{\times }}\left( 1-\cos \sigma \right)$
其中
${{\mathbf{n}}_{\times }}=\left[ \begin{matrix} 0 & -n & m \\ n & 0 & -l \\ -m & l & 0 \\ \end{matrix} \right]\ \ \ \ \ \$
${\mathbf{n}}=\left[ \begin{matrix} &l \\ &m \\ &n \\ \end{matrix}\right]$
也可以通过先将旋转矢量转换为四元数，再将四元数转为旋转矩阵。

---

优点：几何上较为直观

缺点：在数学推导和数值计算上并不实用（常常很难找到旋转轴的准确位置）

5、四元数

四元数数学运算

为了方便后面的推导说明，首先定义以下两个四元数：
$\begin{align} & \mathbf{P}={{p}_{0}}+\mathbf{p}=\left[ {{p}_{0}},{{p}_{1}},{{p}_{2}},{{p}_{3}} \right] \\ & \mathbf{Q}={{q}_{0}}+\mathbf{q}\text{=}\left[ {{q}_{0}},{{q}_{1}},{{q}_{2}},{{q}_{3}} \right] \end{align}$
（1）与标量（矢量）运算

当标量（或矢量）和四元数进行数学运算时，先将标量（或矢量）升级为四元数，升级方法为
$\begin{align} & k\ \ =>\ \ \left[ k,0,0,0 \right]=k+\left( 0\mathbf{i}+0\mathbf{j}+0\mathbf{k} \right) \\ & \left[ l,m,n \right]\ \ =>\ \ \left[ 0,l,m,n \right]=0+\left( l\mathbf{i}+m\mathbf{j}+n\mathbf{k} \right) \end{align}$
（2）共轭运算

共轭运算，只需要将虚部添加一个负号即可
${{\mathbf{Q}}^{*}}=\text{conj}\left( {{q}_{0}}+\mathbf{q} \right)={{q}_{0}}-\mathbf{q}\text{=}\left[ {{q}_{0}},-{{q}_{1}},-{{q}_{2}},-{{q}_{3}} \right]$
（3）加减法运算
$\begin{align} & \mathbf{P}\pm \mathbf{Q}=\mathbf{Q}\pm \mathbf{P} \\ & =\left( {{p}_{0}}\pm {{q}_{0}} \right)+\left( \mathbf{p}\pm \mathbf{q} \right) \\ & =\left[ {{p}_{0}}\pm {{q}_{0}},{{p}_{1}}\pm {{q}_{1}},{{p}_{2}}\pm {{q}_{2}},{{p}_{3}}\pm {{q}_{3}} \right] \end{align}$
（4）乘法运算

在推导四元数乘法运算之前，先定义矢量的乘积为叉乘与点乘之差，注意不是和:
$\begin{align} & \mathbf{p}\circ \mathbf{q}=-\mathbf{p}\cdot \mathbf{q}+\mathbf{p}\times \mathbf{q} \\ & =-\left( {{p}_{1}}{{q}_{1}}+{{p}_{2}}{{q}_{2}}+{{p}_{3}}{{q}_{3}} \right)+\left( {} \right)\mathbf{i}+\left( {} \right)\mathbf{j}+\left( {} \right)\mathbf{k} \end{align}$
可以看出矢量的乘积是一个四元数。
下面推导四元数的乘法运算:
$\begin{align} & \mathbf{P}\circ \mathbf{Q}\text{=}\left( {{p}_{0}}+\mathbf{p} \right)\circ \left( {{q}_{0}}+\mathbf{q} \right) \\ & \text{=}{{p}_{0}}{{q}_{0}}+{{p}_{0}}\mathbf{q}+{{q}_{0}}\mathbf{p}+\mathbf{p}\circ \mathbf{q} \\ & \text{=}\left( {{p}_{0}}{{q}_{0}}-\mathbf{p}\cdot \mathbf{q} \right)+\left( {{p}_{0}}\mathbf{q}+{{q}_{0}}\mathbf{p}+\mathbf{p}\times \mathbf{q} \right) \end{align}$
需要注意是，四元数乘法的虚部包含叉乘运算，因此四元数乘法不支持交换律，但是满足结合律和分配律(因为叉乘不满足交换)
将式展开并写成矩阵的形式
$\mathbf{P}\circ \mathbf{Q}\text{=mat}\left( \mathbf{P} \right)*\text{col}\left( \mathbf{Q} \right)\text{=}\left[ \begin{matrix} {{p}_{0}} & -{{p}_{1}} & -{{p}_{2}} & -{{p}_{3}} \\ {{p}_{1}} & {{p}_{0}} & -{{p}_{3}} & {{p}_{2}} \\ {{p}_{2}} & {{p}_{3}} & {{p}_{0}} & -{{p}_{1}} \\ {{p}_{3}} & -{{p}_{2}} & {{p}_{1}} & {{p}_{0}} \\ \end{matrix} \right]\left[ \begin{matrix} {{q}_{0}} \\ {{q}_{1}} \\ {{q}_{2}} \\ {{q}_{3}} \\ \end{matrix} \right]$
(5) 标量与四元数的乘法
$k\circ \mathbf{Q}=\mathbf{Q}\circ k=k{{q}_{0}}+k\mathbf{q}\text{=}\left[ k{{q}_{0}},k{{q}_{1}},k{{q}_{2}},k{{q}_{3}} \right]$
(6) 矢量与四元数的乘法，直接将矢量升级为实部为0的四元数，然后采用式4计算即可

(7) 求模运算，四元数和共轭四元数的乘积
$\mathbf{Q}\circ {{\mathbf{Q}}^{*}}\text{=}{{\mathbf{Q}}^{*}}\circ \mathbf{Q}\text{=}q_{1}^{2}+q_{1}^{2}+q_{2}^{2}+q_{3}^{2}$
(8) 逆运算
${{\mathbf{Q}}^{-1}}\text{=}\frac{{{\mathbf{Q}}^{*}}}{\sqrt{\mathbf{Q}\circ {{\mathbf{Q}}^{*}}}}\text{=}\frac{\left[ {{q}_{0}},-{{q}_{1}},-{{q}_{2}},-{{q}_{3}} \right]}{\sqrt{q_{1}^{2}+q_{1}^{2}+q_{2}^{2}+q_{3}^{2}}}$
两个互逆四元数的乘积是单位四元数
${{\mathbf{Q}}^{-1}}\circ \mathbf{Q}\text{=}\mathbf{Q}\circ {{\mathbf{Q}}^{-1}}\text{=}\left[ 1,0,0,0 \right]$
注意：利用四元数进行坐标系转换时，各个坐标系之间左乘与右乘同其所选取的基底有关，不同于旋转矩阵的左乘。当其所选统一基底进行表达的四元数时，其进行左乘；当其选用不同基底时，即进行右乘（较常用）。详见邓正隆的惯性技术。

6、坐标系转换示例

上述主要是介绍相邻坐标系如何通过欧拉角、旋转矩阵、四元数和旋转矢量进行表示。工作中我们经常遇到的是不同时刻不同坐标系间的变换。这里主要介绍如何处理这一情景（不考虑平移情景，平移可以看做先旋转在矢量相加）。

在此以求取slam世界坐标系与导航坐标系的旋转矩阵举例

**背景：**无人机利用相机做slam时，其世界坐标系在初始化成功时刻t1时候才确立，记$W$系,该时刻的相机坐标系为$C_1$,该时刻无人机机体坐标系为$B_1$。无人机导航坐标系是在无人机起飞前t0时刻就确立的，在此取较常用的北东地坐标系为导航坐标系$n$，该时刻对应的机体坐标系为$B_0$。我们需要将slam获取的相机相对世界系的位姿转换至导航系，从而修正无人机在导航系的位姿；也需要将世界系的障碍物转换至导航系，故我们需要知道世界系与导航系的旋转矩阵。

**翻译：**将上述背景翻译成数学语言，即已知$n$系、$B_0$系、$C_1$、$B_1$、$C_{{C_1}{W}}$,求取$C_{{n}{W}}$。
由于相机与无人机绑定，其之间的安装角度可以通过事先校准获取，即t1时刻机体坐标系与该时刻的相机坐标系转换关系$C_{{B_1}{C_1}}$已知；而t1时刻的机体坐标系与导航坐标系可以通过无人机自身导航系统获取，即可以得到$C_{{n}{B_1}}$。
根据旋转矩阵章节的左乘链式法则可以得到如下等式：
$C_{{n}{W}} = C_{{n}{B_1}}*C_{{B_1}{C_1}}*C_{{C_1}{W}}$

7、工程应用

工程上我们进行坐标系转换时，更关注的是如何利用现有的库进行实现，而非根据上述公式重复造轮子。常见的坐标系转换矩阵库主要有以下：

1. Eigen
   eigen是一种c++库，里面集成了各种矩阵旋转和平移函数
   Eigen的官方文档

AngleAxisd rotation_vector(M_PI / 4, Vector3d(0, 0, 1));     //旋转矢量法，沿Z轴旋转 45 度
 rotation_matrix = rotation_vector.toRotationMatrix();  //旋转矢量转变为旋转矩阵
 euler_angles = rotation_matrix.eulerAngles(2, 1, 0); // ZYX顺序，欧拉角转旋转矩阵
 Quaterniond q = Quaterniond(rotation_vector);  //旋转矢量转四元数
 
 rotation_vector2.fromRotationMatrix(rotation_matrix2);   //旋转矩阵转旋转矢量
 euler_angle2 = rotation_matrix2.eulerAngles(2, 1, 0); //旋转矩阵转欧拉角
 Eigen::Quaterniond quaternion2(rotation_matrix2);    //旋转矩阵转四元数
 
 Eigen::AngleAxisd rotation_vector3 = Eigen::AngleAxisd(euler_angle3[0], Eigen::Vector3d::UnitZ()) *
                     Eigen::AngleAxisd(euler_angle3[1], Eigen::Vector3d::UnitY()) *
                     Eigen::AngleAxisd(euler_angle3[2], Eigen::Vector3d::UnitX()); //欧拉角转旋转矢量

2. matlab
   matlab里面有大量的相互转换及相关运算，在这里介绍最常见的：

dcm = angle2dcm(yaw,theta,roll,'zyx') %%欧拉角转旋转矩阵，zyx为其旋转顺序
Euler = dcm2angle(dcm);  %%旋转矩阵转换为欧拉角
quat = angle2quat(yaw,theta,roll,'zyx') %%欧拉角转四元数，zyx为其旋转顺序
Euler = quat2angle(quat);  %%四元数转换为欧拉角
dcm = quat2dcm(quat); %% 四元数转旋转矩阵
quat = dcm2quat(dcm); %% 旋转矩阵转四元数
R = roty(ang); %%绕y轴旋转ang
quatprod = quatmultiply(q,r); %%四元数相乘
quatNormalized = normalize(quat); %%四元数归一化，利用四元数进行导航解算时需要

3、 TF
TF(TransForm)是ros世界里的一个基本的也是很重要的概念，用来进行坐标转换的。

tf::Quaternion q;
q.setRPY(yaw,pitch,roll);  //欧拉角转四元数
v=q.getAxis(); //四元数转轴角
Matrix.setRotation(q); //四元数转旋转矩阵
Matrix.getEulerYPR(m_yaw,m_pitch,m_roll); //旋转矩阵转欧拉角