APM姿态旋转理论基础

• 一、坐标系
• • 1.1 NED坐标系
  • 1.2 机体坐标系
• 二、欧拉角
• • 姿态变化率与机体角速度的关系
• 三、旋转矩阵
• • 3.1 基本公式
  • 3.2 矩阵作差
  • 3.3 旋转矩阵与变换矩阵的区别
• 四、DCM
• 五、轴角法
• • 5.1 基本概念
  • 5.2 与旋转矩阵的相互转换
• 六、四元数
• • 6.1 四元数表示旋转
  • • 6.1.1 与轴角的相互转换
    • 6.1.2 四元数表示旋转
  • 6.2 四元数与旋转矩阵
  • 6.3 四元数与欧拉角
• 总结

---

阅读源码的过程中发现对于一些基础理论的掌握还是不够深，因此本篇先把一些理论知识部分汇总一下。本博文会持续更新修改，所有引用均会表明出处。如有问题，欢迎指正。

---

以下内容均为个人认为需要掌握之处，可能不会涵盖一些太基础的理论，但是会提供一些参考的链接供大家学习（超长篇警告）。

一、坐标系

无人机领域中最重要的就是惯性坐标系和机体坐标系。

1.1 NED坐标系

因为在现实中通常需要在平面控制无人机，因此NED坐标系通常作为无人机领域的导航坐标系使用。

NED坐标系如下图绿色坐标系，即北东地坐标系，三个方向始终保持不变。

1.2 机体坐标系

机体坐标系始终与无人机本体固连，同时也表明了无人机当前的姿态。其原点位于无人机的重心，X轴朝向机头，Y轴垂直于X轴指向机身右方，Z轴按照右手法则正交于X和Y轴（这边先关注轴即可，旋转和角度表示后面需要再统一一下标准）。

通常无人机的姿态一般由惯性测量单元IMU来获取，其中陀螺仪用于获取角速度，加速度计顾名思义获取3轴的加速度，如果带有磁力计，则可以获取到磁场方向。

后面过程NED坐标系会简称为n系，机体坐标系会简称为b系。

举例来说，在n系下我们获取到的是无人机的“东南西北”的绝对位置信息，而在b系下，我们会要求无人机进行“前后左右”的增量式运动，如何将我们给机体的期望运动与导航给我们反馈的n系下的位置、姿态等绝对信息联系起来，就是我们后面要做的事情。

 

参考资料：
无人机运动学控制中的坐标系，及惯性坐标系与机体坐标系之间的矩阵转换 欧拉角
[飞控]聊点姿态(三)-什么是地理系和机体系？

 

二、欧拉角

借一张已经被用烂了的图简单来说一下欧拉角的一些基本概念：

1. 欧拉角是利用3次独立的绕轴旋转运动来确定三维空间中的某一刚体姿态；
2. 欧拉角可以绕着固定轴（大地坐标系） 和 运动轴（机体坐标系） 两种类型的轴进行旋转，运动过程对于前者称为外旋，后者称为内旋；
3. 对于绕轴的顺序，科学界并没有明确的规定，实际上根据不同轴旋转顺序总共有24种（外旋12种，内旋12种，在各自的12种中使用2轴还是3轴旋转又可以向内各分为6种）
4. 使用欧拉角时必须指明旋转顺序以及参考轴，同时不同行业内部会有具体的欧拉角定义，比如无人机类型的使用的zyx
5. 理论上用欧拉角确实可以表示任意姿态，实际上在实际使用过程中会存在万向节死锁问题
6. 绕固定轴进行3次旋转之后的最终姿态和以绕运动轴按相反顺序进行3次旋转之后可以获得相同的最终姿态。由此会有绕定轴旋转X-Y-Z的RPY角和绕动轴Z-Y-X旋转之分，但是两者的最终姿态是一致的（当然需要按照特定角度）
7. 通常欧拉角并不用于计算过程，一般使用的是四元数进行计算，而最终的可视化是转化为欧拉角进行表示的。

在无人机领域中，我们通常使用的欧拉角旋转方式表示为（根据国内标准统一符号，记住此处的角度符号，会在旋转矩阵中用到）：

• 绕 Z轴 进行的 偏航 运动，称为 Yaw ，角度符号通常用 ψ 来表示。
• 绕 Y轴 进行的 俯仰 运动，称为 Pitch ，角度符号通常用 θ 来表示。
• 绕 X轴 进行的 滚转 运动，称为 Roll ，角度符号通常用 Φ 来表示。

姿态变化率与机体角速度的关系

以下内容来自全权老师的《多旋翼飞行器设计与控制》。

其中，向量 ω ω ω 表示的是机体的旋转角速度， W W W表示旋转过程， Θ Θ Θ表示RPY角，则对应的微分表示的即是姿态变化率（原谅我在编辑器里面打不出 Θ Θ Θ上的一点）。

 
参考资料：
做控制要知道的刚体旋转知识（三）欧拉角
《多旋翼飞行器设计与控制》5.2.1欧拉角小节（P90-91）

 

三、旋转矩阵

旋转矩阵表示的是坐标系的旋转

3.1 基本公式

推导就不推导了，直接给出结论，常用的绕X、Y、Z轴方向的旋转矩阵如下（注意推导时根据旋转方向会有正负号的差异）。我们通常用Φ，θ，ψ表示绕X、Y、Z轴的旋转角度。

绕轴顺序不同的话最后得到的旋转矩阵也会不同，在无人机领域通常是按照Z-Y-X进行旋转的，结论直接给出。

从n系向b系旋转的旋转矩阵 R n b R{^b_n} Rnb​（顺序Z-Y-X）

明确旋转矩阵自身为一个正交矩阵（转置与逆相等）

那么b系向n系旋转的旋转矩阵正好为 R b n = R n b T = R n b − 1 R{^n_b}= R{^b_n}^T= R{^b_n}^{-1} Rbn​=Rnb​T=Rnb​−1

3.2 矩阵作差

来源：[飞控]姿态误差(二)-旋转矩阵做差

通常欧拉角下姿态的误差能够通过期望角和当前角相减得到，但是对于由旋转矩阵描述的姿态来说，无法通过矩阵相减得到姿态误差。

通常，假设我们有 R 1 2 R{^2_1} R12​描述从坐标系1到2的旋转，而 R 2 3 R{^3_2} R23​描述从坐标系2到3的旋转，则有如下性质
R 1 3 = R 2 3 ∗ R 1 2 R{^3_1} = R{^3_2}*R{^2_1} R13​=R23​∗R12​
即可见相乘的两个矩阵的上下标相互约去得到坐标系1到坐标系3的旋转矩阵。

又由于旋转矩阵为正交矩阵，因此有
( R 2 3 ) − 1 ∗ R 1 3 = ( R 2 3 ) T ∗ R 1 3 = R 3 2 ∗ R 1 3 = R 1 2 {(R{^3_2})}^{-1}*R{^3_1} = {(R{^3_2})}^{T}*R{^3_1} = R{^2_3}*R{^3_1} = R{^2_1} (R23​)−1∗R13​=(R23​)T∗R13​=R32​∗R13​=R12​

由此可见旋转矩阵中通过左乘一个旋转矩阵的逆表示姿态作差。

3.3 旋转矩阵与变换矩阵的区别

旋转矩阵通常是以R表示3×3矩阵，而变换矩阵则更多的是用T表示的在齐次坐标系下的4×4矩阵。

一些论文里面会以群的形式描述三维空间内的旋转矩阵和变换矩阵，通常前者以SO(3)表示，后者用SE(3)表示，有兴趣的同学可以自行翻阅相关群论，此处知道如上表示即可。

 
参考资料：
三维变换中，旋转矩阵左乘与右乘有什么区别？
做控制要知道的刚体旋转知识（四）旋转矩阵/方向余弦矩阵
[飞控]姿态误差(二)-旋转矩阵做差
《多旋翼飞行器设计与控制》P91-95

 

四、DCM

上面的旋转矩阵是从欧拉角进行推导得出的，除此之外，我们还可以直接利用坐标系之间的旋转关系进行推导，此时获得的矩阵成为方向余弦矩阵DCM，实际上从某种意义上来说两者是等价的，因为最后获得的姿态是一样的。

推荐阅读：DCM Tutorial – An Introduction to Orientation Kinematics
中文翻译：方向余弦矩阵(DCM)简介

以下内容来自上文，简单总结一下：

定义全局坐标系为OXYZ，本体坐标系为Oxyz，两者原点相同。全局坐标系中定义各轴（X、Y、Z）的单位向量分别为 I 、 J 、 K I、J、K I、J、K，同理本体坐标系中各轴对应的单位向量为 i 、 j 、 k i、j、k i、j、k。

全局坐标系下的单位向量 I 、 J 、 K I、J、K I、J、K表示为

本体坐标系下的单位向量 i 、 j 、 k i、j、k i、j、k表示为

其中G表示全局坐标系，B表示本体坐标系。具体的推导过程省略，这边可以简单表示为

i x G i{_x^G} ixG​ 表示的是i向量在全局坐标系中X轴上的投影，整个过程用点乘实现计算，注意仔细理解上式，我们以后会以 c o s ( I , i ) cos(I, i) cos(I,i) 或者 I . i I.i I.i 的形式表示点乘。那么 i i i 向量在全局坐标系中的投影为

由此我们可以获得向量 i 、 j 、 k i、j、k i、j、k在全局坐标系G中的投影坐标，表示为方向余弦矩阵为

而同样的，在本体坐标系中表示全局坐标系的单位向量 I 、 J 、 K I、J、K I、J、K在本质上其实是对称的。举例来说 I B I^B IB表示的就是 I I I向量在本体坐标系的投影

由此获得向量 I 、 J 、 K I、J、K I、J、K在本体坐标系中的投影（注意 I . i I.i I.i和 i . I i.I i.I等价）

可以发现 D C M G DCM^G DCMG和 D C M B DCM^B DCMB是各为彼此的转置，并且DCM本身为一个正交矩阵。

 

五、轴角法

5.1 基本概念

由于APM中主要采用的就是轴角和四元数来计算姿态误差

先来看看什么是旋转向量，旋转向量定义为：任意旋转均可用一个旋转轴和一个旋转角来表示，由此使用一个向量，其方向与旋转轴保持一致，向量长度等于旋转角（摘自《视觉SLAM十四讲》）。

而轴角法（Axis-Angle）实际上就是旋转向量，它使用一个转轴（单位向量）和一个旋转角来描述旋转过程。

如下图所示，以z为转轴，α为转角构建轴角，将坐标系xyz旋转到X’Y’Z’。

5.2 与旋转矩阵的相互转换

只给出结论。

从轴角转换到旋转矩阵，根据罗德里格斯公式有：

符号^表示将向量转换为反对称矩阵。

可以倒推获得旋转矩阵向轴角的转换：

t r ( R ) tr(R) tr(R)表示矩阵R的迹

转轴n，在旋转轴上的向量旋转前后不发生变化
R n = n Rn=n Rn=n

 

参考资料：
做控制要知道的刚体旋转知识（一）轴角法
[飞控]姿态误差(三)-四元数和轴角求误差

 

六、四元数

首先明确四元数描述的是三维空间的旋转过程，而不是一个点。

基础理论和数学计算方式这边将不再介绍，后面均表示为国内统一标准。

下图来自《多旋翼飞行器设计与控制》

即可表示为如下形式，其中 q 0 q_0 q0​为实数， q 1 ， q 2 ， q 3 q_1，q_2，q_3 q1​，q2​，q3​为虚数。

q ⃗ = q 0 + q 1 i ⃗ + q 2 j ⃗ + q 3 k ⃗ \vec{q} = q_0+q_1\vec{i}+q_2\vec{j}+q_3\vec{k} q ​=q0​+q1​i +q2​j ​+q3​k

在APM中则是以 q 1 ， q 2 ， q 3 ， q 4 q_1，q_2，q_3，q_4 q1​，q2​，q3​，q4​进行表示。

一些基本运算可以看这篇博文：旋转表达之四元数

注意：单位四元数的逆等于它的共轭

在统一使用国内标准描述四元数时，四元数与旋转矩阵也可以像旋转矩阵一样上下标约去

6.1 四元数表示旋转

6.1.1 与轴角的相互转换

前提：轴角已知

四元数本身也是存储了一个旋转轴和一个旋转角度。如果已知一个由轴角n和θ指定的旋转。其中

n ⃗ = x i ⃗ + y j ⃗ + z k ⃗ \vec{n}=x\vec{i}+y\vec{j}+z\vec{k} n =xi +yj ​+zk

并且向量n为单位向量

∣ ∣ n ⃗ ∣ ∣ = 1 ||\vec{n}||=1 ∣∣n ∣∣=1

则用四元数描述这个旋转过程为

q = c o s θ 2 + ( x + y + z ) s i n θ 2 q=cos\frac{θ}{2}+(x+y+z)sin\frac{θ}{2} q=cos2θ​+(x+y+z)sin2θ​

表示为

q = [ q 0 , q 1 , q 2 , q 3 ] = [ c o s θ 2 , x s i n θ 2 , y s i n θ 2 , z s i n θ 2 ] = [ c o s θ 2 , n s i n θ 2 ] q=[q_0 ,q_1,q_2,q_3]=[cos\frac{θ}{2},xsin\frac{θ}{2},ysin\frac{θ}{2},zsin\frac{θ}{2}]=[cos\frac{θ}{2},nsin\frac{θ}{2}] q=[q0​,q1​,q2​,q3​]=[cos2θ​,xsin2θ​,ysin2θ​,zsin2θ​]=[cos2θ​,nsin2θ​]

那么反过来也可以求得轴角
θ = 2 ∗ a r c c o s ( q 0 ) θ=2*arccos(q_0) θ=2∗arccos(q0​) [ x , y , z ] T = [ q 1 , q 2 , q 3 ] T s i n θ 2 [x,y,z]^T=\frac{[q_1,q_2,q_3]^T}{sin\frac{θ}{2}} [x,y,z]T=sin2θ​[q1​,q2​,q3​]T​

四元数恢复轴角形式

虽然上面给出了公式，然而一种更直观的计算方法如下，前提是已知四元数表达

下图来自旋转表达之四元数

6.1.2 四元数表示旋转

向量旋转

假设 q q q是一个用四元数表示的旋转过程， v 1 ∈ R 3 v_1\in R^3 v1​∈R3表示一个向量，那么在 q q q的作用下，向量 v 1 v_1 v1​旋转为向量 v 1 ′ v_1' v1′​可表示为
( 0 v 1 ′ ) = q × ( 0 v 1 ) × q − 1

(0v′1)$$\left(\begin{array}{c}0\\ v_1^{\prime }\end{array}\right)$$

=q×

(0v1)$$\left(\begin{array}{c}0\\ v_1\end{array}\right)$$

×q^{-1} (0v1′​​)=q×(0v1​​)×q−1

注意以上过程为0标量运算，具体的运算过程以及非0标量四元数运算详见下面这篇博客（内容会在APM姿态误差计算中用到）
[飞控]姿态误差(三)-四元数和轴角求误差

坐标系旋转

坐标系旋转则刚好相反，想象在一个坐标系下将向量逆时针旋转45°，这个过程是不是等价于将坐标系顺时针旋转45°呢？

因此公式表示为：
( 0 v 1 ′ ) = q − 1 × ( 0 v 1 ) × q

(0v′1)$$\left(\begin{array}{c}0\\ v_1^{\prime }\end{array}\right)$$

=q^{-1}×

(0v1)$$\left(\begin{array}{c}0\\ v_1\end{array}\right)$$

×q (0v1′​​)=q−1×(0v1​​)×q

6.2 四元数与旋转矩阵

旋转矩阵 R b e R^e_b Rbe​是从机体坐标系b到地球固连坐标系e的转换

摘自《多旋翼飞行器设计与控制》

摘自《视觉SLAM十四讲》

其中 t r ( R ) tr(R) tr(R)表示矩阵的迹， t r ( R ) = m 11 + m 22 + m 33 tr(R) = m_{11}+m_{22}+m_{33} tr(R)=m11​+m22​+m33​

6.3 四元数与欧拉角

欧拉角转换为四元数。

回忆一下我们通常用φ，θ，ψ表示绕X、Y、Z轴的旋转角度

四元数反推欧拉角

 

参考资料：
如何形象地理解四元数？
旋转表达之四元数
[飞控]姿态误差(三)-四元数和轴角求误差
[飞控]倾转分离(补充)-等效旋转矢量(轴角)与旋转矩阵

 

总结

摘自：https://blog.csdn.net/YuYunTan/article/details/83828258

1. 旋转矩阵用9个元素表示3自由度旋转，表达具有冗余性。而欧拉角和旋转向量是紧凑的，但具有奇异性。
2. 旋转向量用一个旋转轴ω \omegaω和旋转角t tt来描述一个旋转，所以也称轴角(Axis-Angle)。不过很明显，因为旋转角度有一定的周期性（2 π 2\pi2π一圈），所以这种表达方式具有奇异性。
3. 欧拉角有一个致命缺点：万向锁。也就是在俯仰角为±90°时，第一次和第三次旋转使用的是同一个坐标轴，会丢失一个自由度，引起奇异性。
4. 表达三维旋转的不带奇异性的三维向量描述方式是不存在的，它是一个三维流型，想要无奇异性的表示，仅用3个量是不够的，所以引出了四元数。
5. 四元数是Hamilton找到的对于复数的扩展，它由一个实部和三个虚部组成，是一种非常紧凑、没有奇异的表达方式。然而四元数不够直观，且运算较为复杂。