四轴姿态控制系统组成以及电机动力分配的算法实现

基本的闭环控制图如下：

 

 将各个控制单元对应到无人机上，可以做出Pitch角的反馈控制框图：

四轴姿态PID控制

单级控制框图如下：

 串级控制框图如下：

        主要是在角度控制的基础上，增加了角速度控制。期望姿态角与姿态结算出来的角度进行角度PID控制，输出的角度值，是离散型数据。又因为w（角速度）=a（角度）/t（时间）。所以可以认为输入的也是角速度。于是就实现了外环PID值作为内环PID的期望值目的。再结合陀螺仪比较精准的采集角速度的原理。内环的角速度控制可以保证飞行器测量值迅速的达到期望值。

这样一来就大大缓解了单角度环控制，由于飞机的电机转速与升力不成正比而出现的失衡（比如说在目标位置前后，左右偏移），跟随滞后等等问题。

四轴姿态控制原理四路电机动力分配

 PID计算的结果最终是要输出给电机的，所以我们要将三个姿态角的控制输出正确的分配给四个电机。顺时针开始，从第四象限开始，依次是M1，M2，M3，M4四个电机。（注意上面给的参考图片，左下侧是机头方向）

实现左右横滚运动的时候：（Roll）

需要M1，M4的转速增大。M2，M3电机的转速减少。（或者反着来，运动方向相反）并且为了使得飞行的稳定性。输出的增量和减少量是一致:

MOTOR1 += + pidRateX.out

MOTOR2 += - pidRateX.out

MOTOR3 += - pidRateX.out

MOTOR4 += + pidRateX.out

实现前后俯仰运动的时候：（Pitch）

需要M3，M4的转速增大。M1，M2电机的转速减少。（或者反着来，运动方向相反）同样的为了使得飞行的稳定性。输出的增量和减少量是一致:

MOTOR1 += - pidRateY.out

MOTOR2 += - pidRateY.out

MOTOR3 += + pidRateY.out

MOTOR4 += + pidRateY.out

实现前后偏航运动的时候：（Yaw）

实现原理：

主要是运用反扭矩：四轴无人机有一个特点，玩过无人机的人也应该知道，就是两个斜对角方向的旋翼旋转方向是不同的。这样使得顺时针方向与逆时针方向产生的反扭矩相互抵消。飞机不至于原地转圈。

在一些小的飞行器上可能会直接在两个对角线上安转正浆与反浆。这样直接控制四个电机旋转方向相同就行。

什么是反扭矩？

用直升机的例子解释：

直升机飞行主要靠旋翼产生的拉力。当旋翼由发动机通过旋转轴带动旋转时，旋翼给空气以作用力矩(或称扭矩)，空气必然在同一时间以大小相等、方向相反的反作用力矩作用于旋翼(或称反扭矩)，从而再通过旋翼将这一反作用力矩传递到直升机机体上。如果不采取措施予以平衡，那么这个反作用力矩就会使直升机逆旋翼转动方向旋转。所以直升机尾巴上的小螺旋桨就是用来抵消反扭矩，进而起到控制直升机方向的作用。

        所以如果我们要飞机向逆时针方向偏航，是不是就要增大逆时针方向上的反扭矩，减少顺时针方向的反扭矩。即增大顺时针旋转旋翼的转速，并减少逆时针旋转旋翼的转速。对应到图中需要M2，M4的转速增大。M1，M3电机的转速减少。（或者反着来，运动方向相反）同样的为了使得飞行的稳定性。输出的增量和减少量是一致:

MOTOR1 += -pidRateZ.out;

MOTOR2 += + pidRateZ.out;

MOTOR3 += - pidRateZ.out;

MOTOR4 += + pidRateZ.out ;

将三个式子加和：

MOTOR1 += + pidRateX.out - pidRateY.out -pidRateZ.out;

MOTOR2 += - pidRateX.out - pidRateY.out + pidRateZ.out;

MOTOR3 += - pidRateX.out + pidRateY.out - pidRateZ.out;

MOTOR4 += + pidRateX.out + pidRateY.out + pidRateZ.out;

        DIY需要注意的一点是，可能自己DIY的飞机的角速度方向与这个代码并不是一致的，原因是自己DIY的飞控的mup6050的方向不一定相同，所以导致一个问题就是：不同飞行器x轴加速度，y轴加速度，z轴加速度的正方向不一样。

        那么在测试调参的时候，一般是俯仰角，翻滚角，偏航角一个一个的调整。避免变量过多分不清楚的情况。