开源飞控PX4姿态控制算法详解(转载，个人觉得写得最详细的一篇)

转载自  开源飞控PX4姿态控制算法详解 - 知乎https://zhuanlan.zhihu.com/p/81659459

倾转分离

今天的内容我们来解析开源飞控软件PX4中关于多旋翼飞行器的姿态控制算法。

首先，PX4位置控制器的输出会转换成四元数的期望传递给姿态控制器。姿态控制器在得到期望四元数之后并不是马上进行角速率期望的计算，而是先进行倾转分离的操作。

那什么是倾转分离？倾转其实是倾斜和旋转的意思，英文里一般用tilt和torsion表示，倾斜对应我们飞行器也就是机体轴系Z轴方向的变化，旋转对应我们飞行器是指绕机体轴系Z轴的旋转运动，倾转分离就是将倾斜和旋转运动从总的姿态期望中剥离出来。

为什么要进行倾转分离？有以下三个原因：

首先，在飞行器实际飞行中，电机的能力是有限的，除了用来平衡飞行器本身的重力之外，留给姿态机动的空间就不是特别充足。而倾斜和旋转两种运动的原理是不同的，倾斜运动是由不同电机差速运动产生的升力差形成的力矩驱使的，而旋转运动是由不同电机差速运动产生的反扭矩驱使的，前者在数值上往往要比后者大很多。

其次，由于绕Z轴的转动惯量也比绕X、Y轴的转动惯量要大得多，所以要改变旋转运动比改变倾斜运动的难度要大很多，所需要占用电机的控制也要大得多。在这种情况下，我们往往需要对旋转运动进行一定的限制，避免在机动时出现电机饱和的情况。

最后，多旋翼飞行器一般是沿X轴和Y轴对称，所以航向朝向哪边真没那么重要，所以在姿态控制中优先保证倾斜也是合情合理的。为此PX4中还特别设定了在航线飞行时保持航向不变的飞行模式。

知识准备

一、在开始推导算法之前，我们先定义几个坐标系的缩写：

N（NED）：机载NED坐标系；

B（Body）：机体轴系；

T（Target）：期望机体轴系；

M（Middle）：中间机体坐标系，即只包含倾斜姿态期望，对齐期望机体轴系Z轴的坐标系。

二、某坐标系绕转轴旋转后转轴在新坐标系中的向量表示与在旋转前的坐标系中的向量表示相同。

三、各四元数表示的坐标系旋转关系：

q：B系向N系旋转；

qd：T系向N系旋转；

qd_red：M系向N系旋转；

qd_mix：T系向M系旋转；

qe_tilt：B系向M系旋转，转轴在N系；

qe_tilt’：B系向M系旋转，转轴在B系或M系。

其公式表示以及链式乘法运算如下：

控制律设计

已知：

当前姿态为：

期望姿态为：

我们先从上面两个四元数中计算出当前机体轴系Z轴的单位向量以及期望四元数所对应的的机体轴系Z轴的单位向量在机载NED系下的表示。它们为:

然后，去除旋转误差后仅代表倾斜误差的四元数可通过上述两个向量求得：

这里一定要注意我们所得的四元数qe_tilt并不能参与四元数的链式乘法运算，因为他虽然代表了B系到M系的旋转，但是旋转轴却是由N系下的两个向量叉乘计算所得，也就是在N系下，而不在B系或者M系下，解决此问题有两种方法，一种是先将旋转轴通过与旋转矩阵的运算转换到B系后再用来表示四元数。另外一种是直接使用相应公式进行转换，这里我们先选择后一种：

结合当前姿态，我们可以得到仅代表倾斜运动的期望四元数为：

然后，我们根据期望四元数和倾斜期望四元数可以得到代表旋转运动的期望四元数为：

因为qd_mix是绕z轴旋转，所以一定可以表示为以下形式：

为了抑制偏航运动我们对其进行限制：

最后结合倾斜运动和受限制旋转运动的期望四元数为：

而四元数误差为：

不妨将此四元数误差表示为：

在这里，大家要理解的是这个四元数误差代表什么，代表的是期望机体轴系绕机体轴系的旋转轴r旋转α角度。这也就是说只要我们飞行器的角速度向量与旋转轴重合，那我们就能通过旋转α角度消除上面的四元数误差。为此，我们将旋转轴的α倍作为角度误差进行控制律计算，当α较小时，2sin（α/2）即为α：

得到期望角速度后，其作为内环期望进行角速度控制器的设计，角速度控制器的输出由PID以及前馈项组成，其计算公式为：

总结展望

本文对PX4中关于四元数姿态控制器的设计内容进行了详细解析，对其中的所有理论公式都进行了推导说明，解释了控制律的设计逻辑。下一篇内容我们会结合代码进行解读，以求大家对这一块内容做到完全了解，十分明白。