学习四旋翼（一）：动力学模型和空间姿态表示

暑假期间，对于四旋翼有一点兴趣，没有亲手做，但是看了一些资料。这个系列文章只是对自己看的东西的记录，对于想要学习了解相关知识的同学没有任何参考价值！

本篇是系列第一篇，分为两部分。第一部分定性描述了四旋翼飞行的动力学原理，第二部分从旋转矩阵，欧拉角，四元数三个方面介绍了三维空间刚体运动的表示方法。

一.动力学模型

可以把四旋翼的运动模型分为动力学模型和运动学模型两个部分。由于四旋翼的动力学模型比较简单，这里只对四旋翼的运动控制定性说明。

1.升降运动
四旋翼的四个浆叶分X形或十字形，相邻的两个旋翼转动方向相反，它们产生的反向扭矩刚好相互抵消，使得四轴在偏航方向能够保持平衡。当转速增加，飞行器克服升力，高度升高。反之转速减小，高度也就随之下降。

2.俯仰运动
如图所示，当电机1转速增大，电机3转速减小，飞行器产生一个向上的力矩，随之电机1上升，电机3下降。同时飞行器会向电机3的方向前进。

3.横滚运动
如图所示，当电机1,2中的一个提高转速另一个减小，就会产生横滚运动。同时飞行器也会左右移动。

4.偏航运动
偏航运动是机体围绕Z轴旋转的过程。单个旋翼，旋转时会产生反扭距，使得机身朝反方向旋转，这也是直升机需要尾桨的原因。图中逆时针旋转的两个桨加速，顺时针旋转的两个桨减速，这样机体就能顺时针旋转。

2.三维空间刚体运动姿态表示

1.欧拉角
首先介绍欧拉发明的欧拉角说起。欧拉角应该是最容易理解的旋转表示方法。

如图所示，一般使用ZYX的顺序旋转物体，下图是一架飞机按照ZYX组合进行旋转产生欧拉角的过程，其中，ψ为偏航角，θ为俯仰角，φ为滚转角。这样使用三个参数就能唯一表示一种旋转方式。

欧拉角最大的问题是存在万向锁，数学上叫做奇异性。可以使用手中的手机感受一下。先将手机屏幕面于地面平行，绕Z轴旋转任意角度，然后将手机向上仰90度，使得屏幕面和地面垂直，这样不论如何，第三次旋转和第一次旋转将使用同一个Z轴，使得物体相当于损失了一个自由度。

2.旋转矩阵
在线性代数中学过，

[a1a2a3]=R∗[a1,a2,a3,]\begin{bmatrix} a_1\\a_2\\a_3 \end{bmatrix} \quad=R*\begin{bmatrix} a_1^,\\a_2^,\\a_3^, \end{bmatrix} \quad ⎣⎡a1a2a3⎦⎤=R∗⎣⎡a1,a2,a3,⎦⎤
将单位正交基a’转换为另一个正交基，这个转换矩阵R就是旋转矩阵。

在二维坐标系之下，旋转X轴一个角度θ，得到旋转矩阵
[cosθsinθ−sinθcosθ]\begin{bmatrix} cosθ&sinθ\\-sinθ&cosθ\\ \end{bmatrix} [cosθ−sinθsinθcosθ]
同理，按照ψ为偏航角，θ为俯仰角，φ为滚转角的顺序旋转之后，可以得到以下三个旋转矩阵。

旋转矩阵R=RψRθRφ旋转矩阵R=R_ψR_θR_φ 旋转矩阵R=RψRθRφ

当俯仰角θ达到90度时，
R=[00−1sin(φ−ψ)cos(φ−ψ)0cos(φ−ψ)−sin(φ−ψ)0]R=\begin{bmatrix} 0&0&-1\\sin(φ-ψ)&cos(φ-ψ)&0\\cos(φ-ψ)&-sin(φ-ψ)&0 \end{bmatrix} R=⎣⎡0sin(φ−ψ)cos(φ−ψ)0cos(φ−ψ)−sin(φ−ψ)−100⎦⎤
可见这个这个旋转矩阵的第一行和第三列固定不变，相当于损失了一个自由度，这也是万向锁的数学解释。

注意：不同的资料可能旋转矩阵表示不一样。本文中都是被动旋转求出的旋转矩阵。

3.四元数
先说明四元数的结构
q=q0+q1i+q2j+q3kq=q_0+q_1i+q_2j+q_3k q=q0+q1i+q2j+q3k
它拥有一个实部和三个虚部，它的设计不是随意的，而是专门为了表示旋转而发明。设一个点的坐标是【x,y,z】记为p，将它绕n轴旋转角度θ，得到点p’。
p=[0,x,y,z]=[0,v]q=[cosθ2，nsinθ2]p,=qpq−1(q−1是四元数q的逆)p=[0,x,y,z]=[0,v]\\q=[cos\frac{θ}{2}，nsin\frac{θ}{2}]\\p^,=qpq^{-1} (q^{-1}是四元数q的逆) p=[0,x,y,z]=[0,v]q=[cos2θ，nsin2θ]p,=qpq−1(q−1是四元数q的逆)
那么，四元数不存在万向锁的问题，但是不直观，所以在实际应用需要把它归一化后使用如下的代码转换成欧拉角。

angle->pit=asin(-2*q1*q3+2*q0*q2)*57.3
angle->rol=-atan2(2*q2*q3+2*q0*q1,1-2*q1*q1-2*q2*q2)*57.3
angle->yaw=atan2(2*(q0*q3+q1*q2),1-q2*q2-q3*q3)*57.3