四旋翼飞行器的控制规律及算法实现

四旋翼飞行器的控制规律及算法实现

四旋翼的基本飞行须参考3个测量量，Pitch，Roll和yaw，然后控制4个电机的转速以达到升降、悬停等动作。

四旋翼飞行器的动力学模型复杂，受外界环境影响大，对控制器的自适应和鲁棒性要求都比较高，故控制方法挺多，比如什么反步法（Backstepping），LQ，ADRC。

不过，专家们也许都比较喜欢说术语，忽悠人吧，其实这玩意控制起来比看论文中的要简单得多，主要就是个——PID，要不分段PID，模糊PID，自适应PID，PID神经元网络.... 够啦，总之没那么复杂。

首先，四个电机，如何控制侧倾？如果单单考虑控制一个电机来侧倾，这不严谨，因为当动作其中一个电机时转速的变化改变了反扭矩的大小而对角的另一个电机产生的扭矩没有改变，此时四旋翼不仅侧倾还会在yaw上面产生额外的运动，所以控制电机应该成对去控制，比如：

定义电机排布方式如下：

M1

|

M2 —— M3

|

M4

这就是四旋翼“+”飞行模式，不难理解，要调整pitch，只要改变M1与M4的转速，调整roll，则改变M2与M3的转速，所以可以得到如下控制规律：

M1 M2 M3 M4 (H为Hold 保持)

正俯仰+ H H -

反俯仰- H H +

正横滚H + - H

反横滚H - + H

顺航向+ - - +

反航向- + + -

上升+ + + +

下降- - - -

不过自己搞的四轴并不是使用“+”模式，那没意思，我喜欢“X”模式。咋一看，似乎复杂了，其实不然，只不过是在每次动作时要操作四个电机而已。

定义电机排布方式如下：

M1 M2

\ /

/ \

M3 M4

可以这么看，把M1&M2当成’+‘模式中的M1；M3&M4当成’+‘模式中的M4，另外两组类推。

所以也可得到“X”模式的控制规律：

M1 M2 M3 M4

正俯仰+ + - -

反俯仰- - + +

正横滚- + - +

反横滚+ - + -

顺航向+ - - +

反航向- + + -

上升+ + + +

下降- - - -

好了，两种模式的控制规律表都在手了，接下来是如何控制，使其能自主悬停？这里实质上也没什么奥妙，就是通过PID，调整电机PWM，减小期望与当前姿态的误差，即可实现悬停、航线等复杂动作。

AHRS姿态解算要可靠，这是重头戏！我的经验是：开启电机，强震动下AHRS只要能保证解算的姿态误差幅度在1°以内即可。接着就是一条经典的PID公式：u(t) =

Kp*[e(t)-e(t-1)] + Ki*e(t) + Kd*[e(t)-2*e(t-1)+e(t-2)] ，u(t)即为本次给电机的控制量。

这里e(t)即为当前时刻期望姿态与测量姿态的误差。

因为是控制3维刚体运动，所以需要当前各轴误差，上次的各轴误差，上上次的各轴误差，还有三套PID参数（也可只用一套参数），可以写成如下矩阵形式：

| Up(t) | | ep(t)-ep(t-1) ep(t) ep(t)-2*ep(t-1)+ep(t-2) | | Kp |

| Ur(t) | = | er(t)-er(t-1) er(t) er(t)-2*er(t-1)+er(t-2) | * | Ki |

| Uy(t) | | ey(t)-ey(t-1) ey(t) ey(t)-2*ey(t-1)+ey(t-2) | | Kd |

Up,Ur,Uy，分别为各轴的控制量；ep,er,ey分别为各轴的姿态误差；