四旋翼无人机动力学模型及控制

Overview

我想通过这个系列的笔记，给大家分享关于四旋翼无人机的控制基础知识 (其中包括一些前沿的论文导读)。主要的思路如下：

1. 先从robotics的基础—欧拉角与旋转矩阵开始，引入四旋翼无人机的控制动力学模型。然后通过matlab实现简单的控制算法。
2. 介绍旋转矩阵的gimbal lock(死锁/万象锁)问题，同时介绍目前robotics旋转建模中最受欢迎的模型：quaternion(四元数)。以及对应的matlab代码(如何通过quaternion修改对四旋翼无人机的建模，并做简单控制)。
3. 四旋翼无人机differential flatness(微分平坦)的详细推导，以及如何利用微分平坦特性进行控制。

以上的知识点在很多其它文章/百科里面都有提到，我只是想把它们系统的整理结合起来，通过我认为容易理解的方式呈现出来，同时分享如何用简单的代码实现。我会尽可能的引用我能找到的原文出处。内容估计有不严谨的地方，希望跟大家一起学习交流。

我们这一篇，就从欧拉角与旋转矩阵开始。

欧拉角与旋转矩阵

就拿无人机来说，想要表示它在${\mathbb{R}}^3$中的旋转，我们可以首先定义三个轴(Z-Y-X)如下图Fig. 1所示。

Fig. 1

Fradi, H., Bracco, L., Canino, F. and Dugelay, J.L., 2018, September. Autonomous person detection and tracking framework using unmanned aerial vehicles (UAVs). In 2018 26th European Signal Processing Conference (EUSIPCO) (pp. 1047-1051). IEEE.

我们可以把X轴想象成鸟的躯干，Y轴是鸟的翅膀，Z轴从下往上垂直穿过鸟的身体平面。有旋转，就一定要有一个参考坐标系(第三人称视角坐标)${}_N{\mathbf{e}}_x$,${}_N{\mathbf{e}}_y$,${}_N{\mathbf{e}}_z$。我们定义当前位置大地地面正北方为${}_N{\mathbf{e}}_x=[1,0,0{]}^T$，正东方为${}_N{\mathbf{e}}_y=[0,1,0{]}^T$，垂直地面正上方为${}_N{\mathbf{e}}_z=[0,0,1{]}^T$。

定义好全局参考系(global frame)之后，我们需要定义本地参考系(body frame)，也就是第一人称视角坐标。鸟躯干为${}_B{\mathbf{e}}_x=[1,0,0{]}^T$,头为正方向。鸟翅膀为${}_B{\mathbf{e}}_y=[0,1,0{]}^T$, 右手为正方向。${}_B{\mathbf{e}}_z=[0,0,1{]}^T$从下往上垂直穿过鸟身体平面。

到这里有人会问，你的${}_N{\mathbf{e}}_x$与${}_B{\mathbf{e}}_x$都用$[1,0,0{]}^T$表示(另外两轴也同理)，那怎么去区分呢？其实它们表示的是在各自视角下的方向，我们先不考虑位移(假设两个坐标系原点重合)，那么只有当这只鸟头冲北，右翅膀朝东，身体平行地面的时候，两个坐标系才一致。我们假设这只鸟身体还是平行地面(${}_N{\mathbf{e}}_z={}_B{\mathbf{e}}_z$)，但是头逆时针旋转了$\psi =30^{\circ }$，此时从第三人称视角看过去，鸟的头方向${}_B{\mathbf{e}}_x=[1,0,0{]}^T$变成多少呢？我们画一个从上往下的俯视图Fig. 2

Fig. 2

很明显，此时如果一个向量$\mathbf{m}$在body frame下面写为:

1. ${}_B\mathbf{m}=[{}_B{m}_x,0,0{]}^T$, 那么在global frame下面就可以看做${}_N\mathbf{m}=[{}_B{m}_x\cdot cos\psi ,{}_B{m}_x\cdot sin\psi ,0{]}^T$ 。
2. ${}_B\mathbf{m}=[0,{}_B{m}_y,0{]}^T$,那么${}_N\mathbf{m}=[{}_B{m}_x\cdot -sin\psi ,{}_B{m}_x\cdot cos\psi ,0{]}^T$
3. ${}_B\mathbf{m}=[0,0,{}_B{m}_z{]}^T$，${}_N\mathbf{m}=[0,0,{}_B{m}_z{]}^T={}_B\mathbf{m}$

也就是说，如果我有一个向量${}_B\mathbf{m}=[{}_B{m}_x,{}_B{m}_y,{}_B{m}_z{]}^T$，那么：

1. 在${}_N{\mathbf{e}}_x$轴上：${}_B{m}_x$会分解${}_B{m}_x\cdot cos\psi$到${}_N{\mathbf{e}}_x$轴上，${}_B{m}_y$会分解${}_B{m}_y\cdot -sin\psi$到${}_N{\mathbf{e}}_x$轴上。${}_B{m}_z$无分解。
2. 在${}_N{\mathbf{e}}_y$轴上：${}_B{m}_x$会分解${}_B{m}_x\cdot sin\psi$到${}_N{\mathbf{e}}_y$轴上，${}_B{m}_y$会分解${}_B{m}_y\cdot cos\psi$到${}_N{\mathbf{e}}_y$轴上。${}_B{m}_z$无分解。
3. 在${}_N{\mathbf{e}}_z$轴上：${}_B{m}_x$无分解，${}_B{m}_y$无分解，${}_B{m}_z$会分解${}_B{m}_z\cdot 1$到${}_N{\mathbf{e}}_z$轴上。

我们可以用矩阵来描述${}_B\mathbf{m}$与${}_N\mathbf{m}$之间的关系：

Eq. 1
N ψ m = R z ( ψ ) ⋅ B m = [ c ( ψ ) − s ( ψ ) 0 s ( ψ ) c ( ψ ) 0 0 0 1 ] ⋅ [ B m x B m y B m z ] {}_{N\psi}\mathbf{m}=R_z(\psi)\cdot{}_B\mathbf{m}=

$$\begin{bmatrix}c(\psi) & -s(\psi) & 0\\s(\psi) & c(\psi) &0\\0 & 0 &1\end{bmatrix}$$ \cdot $$\begin{bmatrix}{}_Bm_x \\{}_Bm_y \\{}_Bm_z\end{bmatrix}$$ Nψ​m=Rz​(ψ)⋅B​m= ​c(ψ)s(ψ)0​−s(ψ)c(ψ)0​001​ ​⋅ ​B​mx​B​my​B​mz​​ ​
ps: $s(\cdot )=sin(\cdot ),c(\cdot )=cos(\cdot ),t(\cdot )=tan(\cdot )$

由于此时的$\mathbf{m}$向量只经过了一次旋转(Z), 还剩下两次旋转(Y)与(X)，所以我把第一次旋转后的${}_N\mathbf{m}$暂时写为${}_{N\psi }\mathbf{m}$。在新的坐标系${}_{N\psi }\mathbf{e}$下，我们继续旋转，不过此时围绕${}_B{\mathbf{e}}_y$轴，旋转角度为$\theta$。用同样的方法，我们可以得到${}_B\mathbf{m}$与${}_{N\theta \psi }\mathbf{m}$之间的关系：

Eq. 2
N θ ψ m = R y ( θ ) ⋅ N ψ m = R y ( θ ) ⋅ R z ( ψ ) ⋅ B m = [ c ( θ ) 0 s ( θ ) 0 1 0 − s ( θ ) 0 c ( θ ) ] ⋅ [ c ( ψ ) − s ( ψ ) 0 s ( ψ ) c ( ψ ) 0 0 0 1 ] ⋅ [ B m x B m y B m z ] {}_{N\theta\psi}\mathbf{m}=R_y(\theta)\cdot{}_{N\psi}\mathbf{m}=R_y(\theta)\cdot R_z(\psi) \cdot {}_B\mathbf{m}=

$$\begin{bmatrix}c(\theta) & 0 & s(\theta)\\0 & 1 &0\\-s(\theta) & 0 &c(\theta)\end{bmatrix}$$ \cdot $$\begin{bmatrix}c(\psi) & -s(\psi) & 0\\s(\psi) & c(\psi) &0\\0 & 0 &1\end{bmatrix}$$ \cdot $$\begin{bmatrix}{}_Bm_x \\{}_Bm_y \\{}_Bm_z\end{bmatrix}$$ Nθψ​m=Ry​(θ)⋅Nψ​m=Ry​(θ)⋅Rz​(ψ)⋅B​m= ​c(θ)0−s(θ)​010​s(θ)0c(θ)​ ​⋅ ​c(ψ)s(ψ)0​−s(ψ)c(ψ)0​001​ ​⋅ ​B​mx​B​my​B​mz​​ ​

最后，我们沿着${}_B{\mathbf{e}}_x$旋转$\varphi$。同理，${}_B\mathbf{m}$与${}_{N\varphi \theta \psi }\mathbf{m}$之间的关系如下：

Eq. 3
N ϕ θ ψ m = R x ( ϕ ) ⋅ R y ( θ ) ⋅ R z ( ψ ) ⋅ B m = [ 1 0 0 0 c ( ϕ ) − s ( ϕ ) 0 s ( ϕ ) c ( ϕ ) ] ⋅ [ c ( θ ) 0 s ( θ ) 0 1 0 − s ( θ ) 0 c ( θ ) ] ⋅ [ c ( ψ ) − s ( ψ ) 0 s ( ψ ) c ( ψ ) 0 0 0 1 ] ⋅ [ B m x B m y B m z ] {}_{N\phi\theta\psi}\mathbf{m}=R_x(\phi)\cdot R_y(\theta)\cdot R_z(\psi) \cdot {}_B\mathbf{m}=

$$\begin{bmatrix}1 & 0 & 0\\0 & c(\phi) &-s(\phi)\\0 & s(\phi) &c(\phi)\end{bmatrix}$$ \cdot $$\begin{bmatrix}c(\theta) & 0 & s(\theta)\\0 & 1 &0\\-s(\theta) & 0 &c(\theta)\end{bmatrix}$$ \cdot $$\begin{bmatrix}c(\psi) & -s(\psi) & 0\\s(\psi) & c(\psi) &0\\0 & 0 &1\end{bmatrix}$$ \cdot $$\begin{bmatrix}{}_Bm_x \\{}_Bm_y \\{}_Bm_z\end{bmatrix}$$ Nϕθψ​m=Rx​(ϕ)⋅Ry​(θ)⋅Rz​(ψ)⋅B​m= ​100​0c(ϕ)s(ϕ)​0−s(ϕ)c(ϕ)​ ​⋅ ​c(θ)0−s(θ)​010​s(θ)0c(θ)​ ​⋅ ​c(ψ)s(ψ)0​−s(ψ)c(ψ)0​001​ ​⋅ ​B​mx​B​my​B​mz​​ ​

最终，我们把这三个旋转矩阵乘起来，可以得到如下等式：

Eq. 4
N ϕ θ ψ m = R z y x ( ψ , θ , ϕ ) ⋅ B m = [ c ( θ ) c ( ψ ) s ( ϕ ) s ( θ ) c ( ψ ) − c ( ϕ ) s ( ψ ) c ( ϕ ) s ( θ ) c ( ψ ) + s ( ϕ ) s ( ψ ) c ( θ ) s ( ψ ) s ( ϕ ) s ( θ ) s ( ψ ) + c ( ϕ ) c ( ψ ) c ( ϕ ) s ( θ ) s ( ψ ) − s ( ϕ ) c ( ψ ) − s ( θ ) s ( ϕ ) c ( θ ) c ( ϕ ) c ( θ ) ] ⋅ [ B m x B m y B m z ] {}_{N\phi\theta\psi}\mathbf{m}=R_{zyx}(\psi,\theta,\phi)\cdot {}_B\mathbf{m}=

$$\begin{bmatrix}c(\theta)c(\psi) & s(\phi)s(\theta)c(\psi)-c(\phi)s(\psi) & c(\phi)s(\theta)c(\psi)+s(\phi)s(\psi)\\c(\theta)s(\psi) & s(\phi)s(\theta)s(\psi)+c(\phi)c(\psi) &c(\phi)s(\theta)s(\psi)-s(\phi)c(\psi)\\-s(\theta) & s(\phi)c(\theta) &c(\phi)c(\theta)\end{bmatrix}$$ \cdot $$\begin{bmatrix}{}_Bm_x \\{}_Bm_y \\{}_Bm_z\end{bmatrix}$$ Nϕθψ​m=Rzyx​(ψ,θ,ϕ)⋅B​m= ​c(θ)c(ψ)c(θ)s(ψ)−s(θ)​s(ϕ)s(θ)c(ψ)−c(ϕ)s(ψ)s(ϕ)s(θ)s(ψ)+c(ϕ)c(ψ)s(ϕ)c(θ)​c(ϕ)s(θ)c(ψ)+s(ϕ)s(ψ)c(ϕ)s(θ)s(ψ)−s(ϕ)c(ψ)c(ϕ)c(θ)​ ​⋅ ​B​mx​B​my​B​mz​​ ​

说了这么半天，到底什么是欧拉角，什么是旋转矩阵呢？
其实欧拉角就是我们分别沿着(Z-Y-X)旋转时对应的三个角度($\psi ,\theta ,\varphi$)。每次旋转时，所对应的$R_z(\psi ),R_y(\theta ),R_x(\varphi )$，包括它们三个的乘积$R_{zyx}(\varphi ,\theta ,\psi )$，都可以称之为旋转矩阵。其实旋转矩阵也不止这一个形式，如果我们调整旋转的顺序(Z-X-Y, X-Z-Y…)都会得到不一样的旋转矩阵，甚至也有Z-X-Z, Z-Y-Z, X-Z-X等旋转顺序。但无论哪种旋转顺序，我们都可以从原始姿态旋转成任意一个姿态。

这里必须提一下旋转矩阵的重要性质，如果你对每一个旋转矩阵求行列式，你会发现无论旋转角度是多少，它们结果都等于1。也就说明：

1. 一个向量旋转前后，模长保持不变。
2. 由于旋转前后可以用矩阵来描述，说明旋转是一个线性操作。那么两个向量旋转前夹角为$\alpha$，经过同样的旋转后，夹角仍为$\alpha$。
3. 同理可知，两个向量先叉乘再旋转，等于先各自旋转再叉乘。说明旋转前后相对姿态保持不变。

以上三个性质，大家有兴趣可以去验证一下。

至此，我们在已知欧拉角的情况下，能够把body frame下的向量转换为global frame下。这对于无人机的动力学建模至关重要，因为无人机里面的IMU传感器，只能给我们提供欧拉角，body frame下的加速度$[{}_B{a}_x,{}_B{a}_y,{}_B{a}_z{]}^T$和角速度$[{}_B{\omega }_x,{}_B{\omega }_y,{}_B{\omega }_z{]}^T$。但是我们控制的时候，需要global frame下面的位置以及速度等状态信息，方便我们控制无人机的飞行轨迹。比如我们想控制无人机从$[0,0,0{]}^T$点飞往$[10,10,5{]}^T$点，这些坐标都是global frame下的。我们都知道，无人机四个螺旋桨转起来，会提供一个${}_B{\mathbf{e}}_z$方向向上的力$f_t\cdot {}_B{\mathbf{e}}_z$. 此时，我们就可以通过旋转矩阵，将其转换到global frame下，，与重力$m\cdot \mathbf{g}=m\cdot [0,0,-9.81{]}^T$求向量和，得到合力方向, 进而通过ODE解出global frame下的速度与位置。

这一篇的最后，我想再补充一个内容：欧拉角的导数$[\dot{\varphi },\dot{\theta },\dot{\psi }{]}^T$与body frame angular velocity $[{}_B{\omega }_x,{}_B{\omega }_y,{}_B{\omega }_z{]}^T$之间的关系。这样，我们下一篇就能直接开始对四旋翼无人机进行建模。

Body Frame Angular Velocity and $[\dot{\varphi },\dot{\theta },\dot{\psi }{]}^T$

回顾一下初中的知识：角速度不光有大小，也有方向，我们可以根据下图，用右手定则，来判断角速度的方向。

Fig. 3

这个时候，有很多人就会说：那这个关系多简单，不是跟上面一样吗？${}_B{\omega }_x$就是绕着body frame x 轴转动的，右手一握，方向就是${}_B{\mathbf{e}}_x$的正方向。同理，${}_B{\omega }_y$方向是${}_B{\mathbf{e}}_y$, ${}_B{\omega }_z$方向是${}_B{\mathbf{e}}_z$。我们想把它们转换成global frame下面的$[\dot{\varphi },\dot{\theta },\dot{\psi }{]}^T$，就直接用我们前面推出来的$R_{zyx}(\varphi ,\theta ,\varphi )$矩阵乘以$[{}_B{\omega }_x,{}_B{\omega }_y,{}_B{\omega }_z{]}^T$不就好了？

答案肯定是不对的。因为这里的欧拉角$[\varphi ,\theta ,\psi {]}^T$可不是像速度、位置那样的向量。它们本身就有一个旋转顺序。比如我们上面一直用的Z-Y-X顺序, 在沿着最后一个轴(body frame x轴)旋转$\varphi$角度之后，完成整个旋转。那么你想，假设我们现在完成了前面两次(Z-Y)旋转，目前正沿着${}_B{\mathbf{e}}_x$以$2.5rad/s$的速度完成最后一次角度为$\varphi$的旋转，那是不是意味着，$\dot{\varphi }=2.5rad/s$. 同时，这个$2.5rad/s$是沿着${}_B{\mathbf{e}}_x$方向的，那么它就是${}_B{\omega }_x$! 换句话说，如果我的测得现在的${}_B{\omega }_x$，那么它会完完全全投射到$\dot{\varphi }$上面，在$\dot{\theta },\dot{\psi }$上的投影为0.

那么其它两个角度呢？其实很容易发现，这个转换其实有点类似于Z-Y-X的逆过程。${}_B{\omega }_z$直接对应最后一个旋转角$\varphi$。${}_B{\omega }_y$需要经过最后一次旋转$R_{\varphi }$，而${}_B{\omega }_z$需要经过两次旋转$R_{\varphi }\cdot R_{\theta }$。这里的公式我就不再一一推导，根据旋转顺序，把对应的旋转矩阵逆过来，就可以解出如下关系：

Eq. 5
[ ϕ ˙ θ ˙ ψ ˙ ] = T ⋅ [ B ω x B ω y B ω z ] = [ 1 s ( ϕ ) t ( θ ) c ( ϕ ) t ( θ ) 0 c ( ϕ ) − s ( ϕ ) 0 s ( ϕ ) c ( θ ) c ( ϕ ) s ( θ ) ] ⋅ [ B ω x B ω y B ω z ]

$$\begin{bmatrix} \dot{\phi} \\ \dot{\theta} \\ \dot{\psi} \end{bmatrix}$$ = T\cdot $$\begin{bmatrix} {}_B\omega_x \\ {}_B\omega_y \\ {}_B\omega_z \end{bmatrix}$$ = $$\begin{bmatrix} 1 & s(\phi)t(\theta) & c(\phi)t(\theta)\\ 0 & c(\phi) & -s(\phi) \\ 0 & \frac{s(\phi)}{c(\theta)} & \frac{c(\phi)}{s(\theta)} \end{bmatrix}$$ \cdot $$\begin{bmatrix} {}_B\omega_x \\ {}_B\omega_y \\ {}_B\omega_z \end{bmatrix}$$ ​ϕ˙​θ˙ψ˙​​ ​=T⋅ ​B​ωx​B​ωy​B​ωz​​ ​= ​100​s(ϕ)t(θ)c(ϕ)c(θ)s(ϕ)​​c(ϕ)t(θ)−s(ϕ)s(θ)c(ϕ)​​ ​⋅ ​B​ωx​B​ωy​B​ωz​​ ​
对这个公式不是很理解的，可以去看一下这个链接：https://physics.stackexchange.com/questions/429081/rotational-kinematics-and-angular-velocity-vector-transformation

小结

至此，我们大致介绍了欧拉角，以及如何用欧拉角推出旋转矩阵，从而使旋转矩阵乘上body frame下面的任意一个向量，可以转换为global frame对应的向量。也简单介绍了body frame angular velocity与欧拉角导数之间的关系。

下一节，我们将以global frame position (${}_N{p}_x,{}_N{p}_y,{}_N{p}_z$), global frame velocity (${}_N{v}_x,{}_N{v}_y,{}_N{v}_z$), Euler angles ($\varphi ,\theta ,\psi$), 以及body frame angular velocity (${}_B{\omega }_x,{}_B{\omega }_y,{}_B{\omega }_z$)这12个参数作为我们四旋翼无人机系统状态，进行动力学建模，并介绍小角度近似线性模型，以及通过简单的MATLAB代码对小角度近似线性模型进行LQR控制。

之后，我们会介绍旋转矩阵的死锁问题，并讲解新的旋转建模方式：四元数(quaternion)。

最后，我们会详细了解并推导四旋翼无人机系统的微分平坦(differential flatness)特性，以及如何利用微分平坦特性直接对非线性系统进行控制，并对比它与小角度近似LQR, 以及非线性模型预测控制(NLMPC)的控制效果。