PX4 四元数旋翼姿态控制修正

---

版本：V1.10.2；
源码位置：~\src\modules\ma_att_control\ma_att_control_main.cpp；

1. 旋翼姿态控制分为两部分，第一部分是外环角度控制，通过偏差角度作为输入得到期望角速率输入到内环角速率控制，这里先来解释外环控制。
2. 之前有一篇博客（https://blog.csdn.net/zhao23333/article/details/103310916）也讲了旋翼姿态控制，但是写的有点繁琐和复杂，这里用简单的方式来理解这个过程。
3. 通过simulink仿真验证可知公式推导是正确的。模型可以在此处下载。

---

1. 四元数介绍

具体四元数介绍可以参考我之前博客，里面关于四元数基础知识的介绍是对的，这里我只需要介绍一下本文的符号规则。

任意一个四元数都可以表示物体姿态，这里我们用 $q$ 表示姿态四元数，同时这个四元数也可以表示坐标变换，一般而言，姿态四元数表示的是体轴系和惯性系之间的坐标关系，因此这里我们用$q_I^B$表示当前飞机的姿态四元数，同时这个四元数也是表示从惯性系到体轴系的坐标变换。

以此类推，四元数$q_{B1}^{B2}$表示的是坐标轴系$B2$相对与$B1$的位置关系，如果B2是体轴系的话，那么就是表示飞机姿态在坐标系B1下的表示，这个四元数转化为欧拉角的角度也是在坐标系B1下的。

2. 旋翼四元数角度控制逻辑

旋翼角度控制分为两个部分，倾转运动和旋转运动，倾转运动是指飞行器对其Z轴的运动，旋转运动是飞行器Z轴对齐之后绕Z轴旋转之后最终到达指定姿态。

因此这里就会出现四个坐标系，惯性系$I$，初始体轴系$B$，中间状态坐标系$M$，最终体轴系$D$。

这里我们将最初姿态四元数定义为$q_I^B$，倾转运动之后的姿态四元数定义为$q_I^M$，这里的M表示的是中间状态坐标系，最终完成运动之后的姿态四元数为$q_I^D$。

3. 最终控制实现逻辑

初始姿态四元数为$q=q_I^B$，目标姿态四元数为$q_d=q_I^D$。

1. 算出Z轴在惯性系下的向量，计算得到对其Z轴之后得姿态四元数（$q_B^M$）。
   这一步通过最基础的四元数表示旋转的物理意义就可以得到，由旋转轴和旋转角度就可以表示出来。
2. 将姿态四元数表示成相对于惯性系下的姿态四元数($q_I^M$)
   通过四元数计算公式可以得到：
   $$q_I^M=q_I^B\ast q_B^M$$
3. 得到旋转运动的姿态四元数($q_M^D$)
   旋转运动也可以由公式计算得到
   $$q_M^D=(q_I^M{)}^{\prime }\ast q_I^D$$
   中间经过权重处理，根据前文的四元数几何意义，旋转运动又只是绕着Z轴旋转，因此上面的四元数可以表示成：
   $${\vec{q}}_M^D={\left[\cos \frac{{\alpha }_{\mathrm{mix}}}{2}00\sin \frac{{\alpha }_{\mathrm{mix}}}{2}\right]}^{\mathrm{T}}$$
   加上限制就有
   $${\vec{q}}_M^D={\left[\cos \frac{k{\alpha }_{\mathrm{mix}}}{2}00\sin \frac{k{\alpha }_{\mathrm{mix}}}{2}\right]}^{\mathrm{T}},k\in [0,1]$$
   这里的$K$就表示偏航旋转的比例。
4. 得到最终状态姿态四元数($q_I^D$)
   $$q_I^D=q_I^M\ast q_M^D$$

4. 仿真验证

建立仿真模型。给定初始角度为10都俯仰角，目标角度为30度俯仰角个10度偏航角，偏航比例k=0.5。

1. 查看对其z轴之后的四元数，发现是相对原先姿态有一个20度的俯仰。
   

2. 查看相对惯性系下的四元数，发现有一个30度的俯仰
   

3. 查看旋转运动四元数，发现差不多由10度偏航
   

4. 查看最终姿态，是一个30度俯仰，5度偏航
   

5. 最终显示的角度偏差