PX4旋翼姿态控制(1)-角度控制

---

版本：V1.9.0；
源码位置：~\src\modules\ma_att_control\ma_att_control_main.cpp；
旋翼姿态控制分为两部分，第一部分是外环角度控制，通过偏差角度作为输入得到期望角速率输入到内环角速率控制，这里先来解释外环控制。

---

1. 控制流程

旋翼在得到期望姿态四元数的时候不是立刻转化为角速率，而是类似于固定翼的纵向和横航向分离一样，四旋翼采用倾斜和旋转分离。倾斜就是将坐标轴Z轴对齐，包含俯仰和滚转，对于四旋翼而言，只需改变升力差即可，响应速度较快；滚转需要提供反力矩，因此相应时间较长，故采取倾转分离策略。

2. 基础知识(四元数和旋转矩阵)

1. 四元数$q=q_0+q_{1}i+q_{2}j+q_{3}k$；也可以写成向量形式$q=[q_0{q}_1{q}_2{q}_3]$；对于单位四元数($q_0^2+q_1^2+q_2^2+q_3^2=1$)还可以写成$q=[\cos (\theta )\sin (\theta )\vec{v}]$，其中$\vec{v}$是单位向量。
   有关四元数的基本运算可以参考其他资料，这里单列一个后文用得到的公式：
   $$q^{-1}=\frac{q^{\ast }}{||q|{|}^2}$$
   其中$q^{-1}=$是四元数的逆，$q^{\ast }=[\cos (\theta )-\sin (\theta )\vec{v}]=[q_0-q_1-q_2-q_3]$是四元数的共轭，$||q||=\sqrt{q_0^2+q_1^2+q_2^2+q_3^2}$是四元数的模。因此对于单位四元数$q_u$有：
   $$q_u^{-1}=q_u^{\ast }$$

2.1 向量旋转，坐标系旋转、坐标变换与四元数

这部分我之前一直有疑惑,现在整理出来。
本段的参考文献是程国彩的《四元数法及其应用》。

1. 四元数表示向量的旋转
   对于一个向量$\vec{R}=(r_1,r_2,r_3)$，如果他绕着某个轴$\vec{V}$旋转$\theta$角之后得到向量$\overrightarrow{R^{\prime }}=(r_1^{\prime },r_2^{\prime },r_3^{\prime })$，则可以构造下面这个四元数：
   $$q=[\cos (\theta /2)\sin (\theta /2)\ast \vec{V}]$$
   然后就可以得到这么一个关系式：
   $$\overrightarrow{R^{\prime }}=q\ast \vec{R}\ast q^{-1}$$
   结合四元数乘法就可以得到四元数表示的向量旋转矩阵：
   $$\left[\begin{array}{c}{r}_1^{\prime }\\ r_2^{\prime }\\ r_3^{\prime }\end{array}\right]=\left[\begin{array}{lll}{q}_0^2＋q_1^2-q_2^2-q_3^2& 2(q_1{q}_2-2q_0{q}_3)& 2(q_1{q}_3+2q_0{q}_2)\\ 2(q_1{q}_2+2q_0{q}_3)& q_0^2+q_2^2-q_1^2-q_3^2& 2(q_2{q}_3-2q_0{q}_1)\\ 2(q_1{q}_3-2q_0{q}_2)& 2(q_2{q}_3+2q_0{q}_1)& q_0^2+q_3^3-q_1^2-q_2^2\end{array}\right]\ast \left[\begin{array}{c}{r}_1\\ r_2\\ r_3\end{array}\right]$$
   这里面所有的向量都表示在同一个坐标系下。
2. 四元数表示坐标系旋转
   对于一个坐标系$I(基为i_1,i_2,i_3)$，如果他绕着某个轴$\vec{V}$旋转$\theta$角之后得到坐标系$e(基为e_1,e_2,e_3)$，则可以构造下面这个四元数：
   $$q=[\cos (\theta /2)\sin (\theta /2)\ast \vec{V}]$$
   然后就可以得到这么一个关系式：
   $$\left[\begin{array}{c}{e}_1\\ e_2\\ e_3\end{array}\right]=\left[\begin{array}{lll}{q}_0^2＋q_1^2-q_2^2-q_3^2& 2(q_1{q}_2+2q_0{q}_3)& 2(q_1{q}_3-2q_0{q}_2)\\ 2(q_1{q}_2-2q_0{q}_3)& q_0^2+q_2^2-q_1^2-q_3^2& 2(q_2{q}_3+2q_0{q}_1)\\ 2(q_1{q}_3+2q_0{q}_2)& 2(q_2{q}_3-2q_0{q}_1)& q_0^2+q_3^3-q_1^2-q_2^2\end{array}\right]\ast \left[\begin{array}{c}{i}_1\\ i_2\\ i_3\end{array}\right]$$
   这就是由四元数表示的坐标系旋转矩阵，可以看到这个和向量旋转矩阵正好互为专制。这里面所有的向量都是在$I$坐标系下面。
3. 坐标变换
   在这里我找到了我疑惑的解答坐标变换矩阵就是坐标系旋转矩阵。即我们可以得到下面的式子
   $$\overrightarrow{R_E}=q^{-1}\ast \overrightarrow{R_I}\ast q$$
   这里的$\overrightarrow{R_E}$是联系于坐标系$E$的四元数，$\overrightarrow{R_I}$是联系于坐标系$I$的四元数。
   这里解释一下为什么，我们以二位坐标系为例
   
   首先我们把这个等式看成是向量的旋转，因此我们有：
   $$\overrightarrow{v_2}=q^{-1}\ast \overrightarrow{v_1}\ast q$$
   由于向量旋转矩阵和坐标变换矩阵是互为转置，因此这里的$\overrightarrow{v_2}$是$\overrightarrow{v_1}$朝反方向旋转$\theta /2$得到的，这时得到的$\overrightarrow{v_2}$是在$I$坐标系下，而他的坐标值又等于${\vec{v}}_1$在$E$坐标系下的值，因此可以得到这个变换就是坐标变换，把同一个向量映射到不同的坐标系中。
4. 旋转的叠加
   假设$q_1$代表旋转为$q_a^b$（坐标系a转化到坐标系b），$q_2$代表旋转为$q_b^c$（坐标系b转化到坐标系c），那么可得
   $$q_3=q_2\ast q_1=q_b^c\ast q_a^b=q_a^c$$
   因此$q_3$就代表由坐标系a转化到c的旋转矩阵。

3. 符号说明

由于后文涉及到的符号较多，故在此具体说明。

表2.1. 坐标系

符号	说明
N	机载NED坐标系
B	机体坐标系
Ｔ	目标坐标系
M	中间坐标系，就是完成了Z轴对齐，还差偏航

表2.2. 四元数及其对应的旋转矩阵

符号	说明	旋转矩阵
q	B系向Ｎ系变换	$\vec{q}={\vec{q}}_B^N$
qd	T系向Ｎ系变换	${\vec{q}}_d={\vec{q}}_T^N$
qd_red	M系向Ｎ系变换	${\vec{q}}_{d,red}={\vec{q}}_M^N$
qd_mix	T系向M系变换	${\vec{q}}_{d,mix}={\vec{q}}_T^M$
qd_tilt	M系向B系变换，转轴在N系	${\vec{q}}_{d,tilt}{=}^N{\vec{q}}_M^B$
qd_tilt’	M系向B系变换，转轴在B系或者M系	${\vec{q}}_{d,tilt}^{\prime }={\vec{q}}_M^B$

最终使用的链式旋转公式
$${\vec{q}}_d={\vec{q}}_T^N={\vec{q}}_M^N\ast {\vec{q}}_T^M={\vec{q}}_{d,red}\ast {\vec{q}}_{d,mix}\text{\hspace{1em}(1)}$$

4. 控制过程(理论公式)

对于一个飞行器，最开始我们只知道他当前的姿态四元数$q$和目标姿态四元数$q_d$.
假设$q=[q_0{q}_1{q}_2{q}_3],q_d=[q_{0d}{q}_{1d}{q}_{2d}{q}_{3d}]$

1. 获取当前姿态和目标姿态Z轴在NED坐标系下的表示(${}^N{\vec{e}}_z^B{,}^N{\vec{e}}_{zd}^B$)
   根据四元数和旋转矩阵的关系可以得到
   $${}^N{\vec{e}}_z^B=\left[\begin{array}{c}2\left(q_0{q}_2+q_1{q}_3\right)\\ 2\left(q_2{q}_3-q_0{q}_1\right)\\ 2\left(q_0^2-q_1^2-q_2^2+q_3^2\right)\end{array}\right]$$
   $${}^N{\vec{e}}_{zd}^B=\left[\begin{array}{c}2\left(q_{0d}{q}_{2d}+q_{1d}{q}_{3d}\right)\\ 2\left(q_{2d}{q}_{3d}-q_{0d}{q}_{1d}\right)\\ 2\left(q_{0d}^2-q_{1d}^2-q_{2d}^2+q_{3d}^2\right)\end{array}\right]$$
   注意这里第二项是相减，对应书上公式(1.85)，因此对应的是坐标系的旋转，所以变换完了之后相当于是在NED轴系下。
2. 获取机体轴(B)到过渡轴(M)的旋转四元数(${\vec{q}}_M^B$)
   知道上面两个坐标轴之后，B轴旋转到M轴相当于绕两个向量叉乘的轴线旋转两个向量两个向量的夹角，因此很容易得出四元数${\vec{q}}_B^M$：
   $$\alpha =\arccos \left({}^N{\vec{e}}_z^B{,}^N{\vec{e}}_{zd}^B\right)$$
   $${\vec{q}}_{d,\text{ tilt }}=\left(\begin{array}{c}\cos (\alpha /2)\\ \sin (\alpha /2)\frac{{}^N{\vec{e}}_z^B{\times }^N{\vec{e}}_{zd}^B}{\Vert {}^N{\vec{e}}_z^B{\times }^N{\vec{e}}_{zd}^B\Vert }\end{array}\right)$$
   由于这个四元数表示的旋转轴是在N系下表示的，需要转化到B系，有下面公式：
   $${\vec{q}}_{d,\text{ tilt }}^{\prime }={\vec{q}}_N^B\cdot {\vec{q}}_{d,\mathrm{t}\mathrm{i}\mathrm{l}\mathrm{t}}\cdot {\left({\vec{q}}_N^B\right)}^{\ast }={\vec{q}}^{-1}\cdot {\vec{q}}_{d,\mathrm{t}\mathrm{i}\mathrm{l}\mathrm{t}}\cdot \vec{q}$$
3. 得到表示倾斜运动的四元数(${\vec{q}}_{d,red}$)
   $${\vec{q}}_{d,\mathrm{r}\mathrm{e}\mathrm{d}}=\vec{q}\cdot {\vec{q}}^{-1}\cdot {\vec{q}}_{d,\mathrm{t}\mathrm{i}\mathrm{l}\mathrm{t}}\cdot \vec{q}={\vec{q}}_{d,\mathrm{t}\mathrm{i}\mathrm{l}\mathrm{t}}\cdot \vec{q}$$
4. 得到表示旋转运动的四元数(${\vec{q}}_{d,mix}$)
   $${\vec{q}}_{d,mix}={\left({\vec{q}}_{d,\text{ red }}\right)}^{-1}\cdot {\vec{q}}_d$$
   根据前文的四元数几何意义，旋转运动又只是绕着Z轴旋转，因此上面的四元数可以表示成：
   $${\vec{q}}_{d,mix}={\left[\cos \frac{{\alpha }_{\mathrm{mix}}}{2}00\sin \frac{{\alpha }_{\mathrm{mix}}}{2}\right]}^{\mathrm{T}}$$
   加上限制就有
   $${\vec{q}}_{d,mix}={\left[k\cos \frac{{\alpha }_{\mathrm{mix}}}{2}00k\sin \frac{{\alpha }_{\mathrm{mix}}}{2}\right]}^{\mathrm{T}},k\in [0,1]$$
5. 得到最终期望四元数(${\vec{q}}_d^{\prime }$)
   $${\vec{q}}_d^{\prime }={\vec{q}}_{d,red}\ast {\vec{q}}_{d,mix}^{\prime }$$
6. 计算误差
   四元数误差为:
   $$\begin{gathered} {\vec{q}}_{ed}^{\prime }={\vec{q}}^{-1}\ast {\vec{q}}_d^{\prime } \\ {\vec{q}}_T^B={\vec{q}}_N^B\ast {\vec{q}}_T^N \end{gathered}$$
   这个误差还可以表示成：
   $${\vec{q}}_{ed}^{\prime }=\left[\begin{array}{c}\cos \frac{\alpha }{2}\\ \sin \frac{\alpha }{2}{\vec{r}}^B\end{array}\right]$$
   根据前面介绍可以知道，这个四元数表示的是B轴系旋转到N轴系。其中向量${\vec{r}}^B=(x_0,y_0,z_0)$是B轴系下的一个向量，将其投影到坐标轴上就可以把误差四元数表示成
   $${\vec{q}}_{ed}^{\prime }=\left[\begin{array}{c}\cos (\frac{\alpha }{2})\\ \sin (\frac{\alpha }{2})\ast x_0\ast i\\ \sin (\frac{\alpha }{2})\ast y_0\ast j\\ \sin (\frac{\alpha }{2})\ast z_0\ast k\end{array}\right]$$
   这表明误差四元数的虚部就是期望的角度。
   $${\vec{\phi }}_d=[q_1{q}_2{q}_3]$$
7. 设计控制律
   得到角度差${\vec{\phi }}_d$之后加一个比例环节得到角速度${\vec{\omega }}_d$，再加上一个前馈，其作为内环控制率进行控制，公式如下：
   $$\overrightarrow{e_{\phi }}={\vec{\phi }}_d-{\vec{\phi }}_0$$
   $${\vec{\omega }}_d=K_{\phi p}{\vec{e}}_{\phi }+K_{ff}{\vec{\omega }}_{dz}$$

注：
这部分主要参照的是PX4姿态控制算法详解
但是那篇文章有一些问题，这里进行了修正，主要就是对于四元数$q_{d,tilt}$应该是M系向B系变换，经过自己的思考之后才明白是为什么，具体原因正如上文2.1 向量旋转，坐标系旋转、坐标变换与四元数。
在代码中，四元数$q_{d,tilt}$表示的是向量${\vec{e}}_z$到向量${\vec{e}}_{zd}$的旋转，其中向量${\vec{e}}_z$在$B$系，${\vec{e}}_{zd}$在$M$系，也就是这个向量表示的是向量的旋转，根据前文可以得到，由于表示向量旋转和坐标系变换的四元数互为共轭，因此这个四元数表示的是从$M$坐标系到$B$坐标系的变换。

5. 源码解读