APM姿态旋转理论基础
阅读源码的过程中发现对于一些基础理论的掌握还是不够深,因此本篇先把一些理论知识部分汇总一下。本博文会持续更新修改,所有引用均会表明出处。如有问题,欢迎指正。
以下内容均为个人认为需要掌握之处,可能不会涵盖一些太基础的理论,但是会提供一些参考的链接供大家学习(超长篇警告)。
一、坐标系
无人机领域中最重要的就是惯性坐标系和机体坐标系。
1.1 NED坐标系
因为在现实中通常需要在平面控制无人机,因此NED坐标系通常作为无人机领域的导航坐标系使用。
NED坐标系如下图绿色坐标系,即北东地坐标系,三个方向始终保持不变。
1.2 机体坐标系
机体坐标系始终与无人机本体固连,同时也表明了无人机当前的姿态。其原点位于无人机的重心,X轴朝向机头,Y轴垂直于X轴指向机身右方,Z轴按照右手法则正交于X和Y轴(这边先关注轴即可,旋转和角度表示后面需要再统一一下标准)。
通常无人机的姿态一般由惯性测量单元IMU来获取,其中陀螺仪用于获取角速度,加速度计顾名思义获取3轴的加速度,如果带有磁力计,则可以获取到磁场方向。
后面过程NED坐标系会简称为n系,机体坐标系会简称为b系。
举例来说,在n系下我们获取到的是无人机的“东南西北”的绝对位置信息,而在b系下,我们会要求无人机进行“前后左右”的增量式运动,如何将我们给机体的期望运动与导航给我们反馈的n系下的位置、姿态等绝对信息联系起来,就是我们后面要做的事情。
参考资料:
无人机运动学控制中的坐标系,及惯性坐标系与机体坐标系之间的矩阵转换 欧拉角
[飞控]聊点姿态(三)-什么是地理系和机体系?
二、欧拉角
借一张已经被用烂了的图简单来说一下欧拉角的一些基本概念:
- 欧拉角是利用3次独立的绕轴旋转运动来确定三维空间中的某一刚体姿态;
- 欧拉角可以绕着固定轴(大地坐标系) 和 运动轴(机体坐标系) 两种类型的轴进行旋转,运动过程对于前者称为外旋,后者称为内旋;
- 对于绕轴的顺序,科学界并没有明确的规定,实际上根据不同轴旋转顺序总共有24种(外旋12种,内旋12种,在各自的12种中使用2轴还是3轴旋转又可以向内各分为6种)
- 使用欧拉角时必须指明旋转顺序以及参考轴,同时不同行业内部会有具体的欧拉角定义,比如无人机类型的使用的zyx
- 理论上用欧拉角确实可以表示任意姿态,实际上在实际使用过程中会存在万向节死锁问题
- 绕固定轴进行3次旋转之后的最终姿态和以绕运动轴按相反顺序进行3次旋转之后可以获得相同的最终姿态。由此会有绕定轴旋转X-Y-Z的RPY角和绕动轴Z-Y-X旋转之分,但是两者的最终姿态是一致的(当然需要按照特定角度)
- 通常欧拉角并不用于计算过程,一般使用的是四元数进行计算,而最终的可视化是转化为欧拉角进行表示的。
在无人机领域中,我们通常使用的欧拉角旋转方式表示为(根据国内标准统一符号,记住此处的角度符号,会在旋转矩阵中用到):
- 绕 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=RnbT=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+q1i+q2j+q3k
在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
\begin{pmatrix} 0 \\ v_1' \\ \end{pmatrix}=q×\begin{pmatrix} 0 \\ v_1 \\ \end{pmatrix}×q^{-1}
(0v1′)=q×(0v1)×q−1
注意以上过程为0标量运算,具体的运算过程以及非0标量四元数运算详见下面这篇博客(内容会在APM姿态误差计算中用到)
[飞控]姿态误差(三)-四元数和轴角求误差
坐标系旋转
坐标系旋转则刚好相反,想象在一个坐标系下将向量逆时针旋转45°,这个过程是不是等价于将坐标系顺时针旋转45°呢?
因此公式表示为:
(
0
v
1
′
)
=
q
−
1
×
(
0
v
1
)
×
q
\begin{pmatrix} 0 \\ v_1' \\ \end{pmatrix}=q^{-1}×\begin{pmatrix} 0 \\ v_1 \\ \end{pmatrix}×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
- 旋转矩阵用9个元素表示3自由度旋转,表达具有冗余性。而欧拉角和旋转向量是紧凑的,但具有奇异性。
- 旋转向量用一个旋转轴ω \omegaω和旋转角t tt来描述一个旋转,所以也称轴角(Axis-Angle)。不过很明显,因为旋转角度有一定的周期性(2 π 2\pi2π一圈),所以这种表达方式具有奇异性。
- 欧拉角有一个致命缺点:万向锁。也就是在俯仰角为±90°时,第一次和第三次旋转使用的是同一个坐标轴,会丢失一个自由度,引起奇异性。
- 表达三维旋转的不带奇异性的三维向量描述方式是不存在的,它是一个三维流型,想要无奇异性的表示,仅用3个量是不够的,所以引出了四元数。
- 四元数是Hamilton找到的对于复数的扩展,它由一个实部和三个虚部组成,是一种非常紧凑、没有奇异的表达方式。然而四元数不够直观,且运算较为复杂。