PX4二次开发——多旋翼姿态控制

一、准备知识

1.串级PID

        PX4中多旋翼姿态控制是采用的串级PID加前馈的复合控制策略。分内外两环，外环只有一个比例环节，内环采用PID加前馈的控制方式。外环主要是由control_attitude()函数负责，由期望姿态（使用四元数表示）和当前姿态（也是使用四元数表示）得到角度差，作用于外环的比例环节（对偏航的处理比较特殊），得到期望角速率，传递给内环；内环主要是由control_attitude_rates()函数负责，由期望角速率和当前角速率得到角速率差，作用于内环的PID环节，同时期望角速率通过前馈通道累加到PID输出的控制量上，得到最终的要传递给混控器的控制量（可以理解为转矩）。此时多旋翼姿态控制模块的使命就基本完成了，接下来(src/drivers/px4fmu/fmu.cpp)由混控器根据这些控制量和混控器矩阵（仅对旋翼而言）进行力矩的分配，然后将分配到各个电机上的力矩换算成PWM值，由px4fmu最终落实到硬件上。

        PX4中的串级PID就讲这么多，深入了解请参考其他相关资料。

2.罗德里格旋转公式

3.Roll/Pitch和Yaw解耦控制

首先，简单通俗地介绍一下多旋翼姿态控制的基本原理（以四旋翼为例）。

四旋翼横滚角和俯仰角控制原理：

        电机转动产生轴向的升力，对称位置的电机转速不一样，产生升力差，进而产生改变横滚角或俯仰角的转矩。

四旋翼偏航角控制原理：

        对称位置的电机转速一样，相邻位置的电机转速不一样，而多旋翼对称位置电机转向一致，相邻位置电机转向相反，这样会产生垂直于轴向的扭矩，进而改变偏航角。

现在，我们进入正题。

为什么要解耦控制？

        因为横滚角/俯仰角和偏航角控制方式不一样，横滚角/俯仰角是考靠轴向升力之差产生转矩，而偏航角是靠扭矩，所以它们的响应速度也不一样，偏航角控制的响应速度只有前两者的一半左右，所以分开控制能获得更快的响应速度。

怎么实现解耦控制？

        先【主要】Roll和Pitch联动，使当前B系中的Z轴向期望姿态的Z轴靠拢，后【主要】进行Yaw的旋转，即对齐X和Y轴。

        实际代码中的实现：给Yaw的旋转加一个范围是[0,1]的权重，它是取的(cos<z,z_sp>)^2（两Z轴之间夹角的余弦值的平方），使得Z轴夹角越大，Yaw旋转的权重越小，反之越大。

4.“轴角法”表示误差

        当前姿态可以通过绕一条轴旋转一定的角度来达到期望姿态。用一条旋转轴u(单位向量)和一个旋转角θ来表示旋转的方法就是所谓的“轴角法”，可以表示为方向为u，模为θ的向量  θu = (θ a, θ b, θ c)T。这样乘外环比例系数后直接就可以得到期望机体角速率。（代码中对Roll和Pitch误差采用了这种方法来表示，对Yaw误差则是单独处理，然后附加在误差向量上，差别不大）

二、多旋翼姿态控制模块(mc_att_control)源码剖析（基于1.7.2版本代码）

        前面提到控制分了内外两环，外环由control_attitude()函数负责，内环由control_attitude_rates()函数负责，下面就开始剖析这两个函数。

首先外环：

然后内环：

三、最后

        PX4多旋翼位置控制就先讲到这里，有没有看得头晕眼花啊？（去年暑假我第一次接触无人机的时候反正是晕了）

        这篇文章花了本考研党接近一天时间，所有的文字都是自己动手敲的，所有的注释也都是自己亲手加上去的，如果有问题还请多多包涵，另外尊重原创哦

        （上传之后图变得不是很清楚了，单击图片可以看大图，需要原图的话发邮件给我，我把图给发过去。e-mail:hello.icq@qq.com）