PID及姿态解调

PID的整定是个让很多人都感到头痛的问题，在此，我想强调一句，PID整定必须建

立在良好的姿态解算的基础上才能进行，否则根本没法弄出一个很好的效果。举个例子，

我的这个四轴，用互补滤波进行姿态处理，P最多调到3。再调大由于姿态解算不过关，P

越大输出的姿态震动越大，即使D调大系统也没法收敛，也就是说这个系统发散了。而用

卡尔曼滤波我可以把P调到4.5而且回复也有力，飞起来也稳。

在姿态解算中，有很多种方法，我这采用的就是普通的姿态角，其他的解算方法还有

四元素，欧拉角，等等，其实四元数还是要转化为欧拉角才能用于PID，不过四元数没有

欧拉角中万向节死锁的问题。具体请百度：四元数，欧拉角，万向节死锁。在此不一一赘

述。

我这的姿态解算原理比较简单，就是对三轴加速度的值进行三角函数运算即可。

举个例子，首先我从加速度计中获取数据。

Accel_y= GetData(ACCEL_YOUT_H);

Accel_x= GetData(ACCEL_XOUT_H);

Accel_z= GetData(ACCEL_ZOUT_H);

再进行转化，转化公式在MPU6050数据手册里有

Angle_ax=(Accel_x)/8192;

Angle_az=(Accel_z)/8192;

Angle_ay=(Accel_y)/8192;

好了，这里得到的就是我们需要的加速度的值，取值范围0-1，也就是多少个g，g

是重力加速度这个就不用说了吧。

之后用反三角函数进行运算：

AngleAx=atan(Angle_ax/sqrt(Angle_ay*Angle_ay+Angle_az*Angle_az))*180/3.

141592657;

公式：X轴角度=arctan（Angle_ax/开根号（Angle_ay^2+Angle_az^2））

好了，这里得到的是弧度制的角度，再乘180除以π即可得到所需的角度值。

之后要对得到的值进行滤波，为什么要滤波呢，因为加速度计对震动特别敏感，一点

震动就大姨妈，得到的角度值是不怎么准确的，不过大致在实际角度的上下。

而陀螺仪对震动不敏感，随便怎么震动都不会高潮。陀螺仪输出的数据是多少度一秒，

据此进行积分，可以得到短时间内系统的姿态。不过陀螺仪有温飘，温度偏移一点他的误

差就会大一点，故要用加速度计的值来对陀螺仪的值进行校准。修正他的温飘误差以及积

分误差。这就是互补滤波的原理，短时间内以陀螺仪的值为准，长时间内以加速度计的值

来对陀螺仪校准，二者优势互补，去掉不好的一面，这样得到的值就比较理想了。

互补滤波公式举例：Angle=0.95*(Angle-Angle_gy*dt)+0.05*AngleAx;

前面的0.95和0.05是对陀螺仪角度和角速度角度的权值，意思就是你相信哪个的程

度更大一点，因为震动是双向的，故对加速度计进行低通滤波会使加速度计输出的值接近

实际值，什么是低通滤波呢，打个比方，当前角度=0.95乘上次的角度+0.05乘测量的角

度，这就是对测量值进行低通滤波，dt是积分时间，就是定时器定时的时间，我这取8ms，

即dt=0.08。

至于卡尔曼滤波，就是一个先进行估计，再根据实际测量的值修改卡尔曼参数，最后

再算误差，里面涉及到线性代数，概率论的相关知识，在此不作赘述，我相信我写出来也

最多只有万分之一的人看得懂，何必呢。

再谈PID，得到了实时姿态，就知道了误差，知道了误差就要对电机进行控制，在PID

中，P指比例，I指积分，D指微分，换种说法，P指回复力，P的输出值=P*误差；P越

大偏差所引起的回复力越大。是系统能够回到0点的主要力。D是系统运动的阻力，D的

输出值=D*（当前误差-前一次误差），D的存在能阻止系统运动。I是系统静差的消除力，

比如你四轴的重心偏了，四轴会朝一边偏，对误差进行积分再乘以I即可消除这种机械结

构引起的偏移。I的输出值=I*所有误差积分之和。

光有P存在的情况，系统一有偏差，就会立马回复，不过到0点时由于惯性，会继续

运动，故光有P的话系统会震荡，根本停不下来。

故需要D，这个系统运动的克制力来对P进行压制，当PD呈一定比例时，系统就会

稳定在一定的值，不过有可能会有点误差，这时就得靠I来修正。

I的原理决定了误差存在的时间约久，回复力越大，I可以理解是变相的回复力，专门

用来修正PD没法消除的静差。

PID整定的方法：

I和D置0，从小往大调P，调整至用手造成一个误差，感觉回复有力即可。网上所说

的调整至等幅震荡不适合小四轴，你根本就不可能把这玩意弄到等幅震荡的状态。总得来

说，P调至你觉得回复起来有点力度就行了，不要太过于纠结。

之后从小往大加大D，在D加大的过程中，会出现震荡减小，震荡最小，震荡增大的

过程。震荡最小的那一点即very good。

调好PD之后如果系统有静差，适当加上I即可，I不能加大，加大了会导致系统震荡，

适当为宜。

这个过程一定要会区分震荡的来源，到底是P引起的还是D引起的。这个口述讲不清，

只有靠自己来体会。

调整好XY轴的PID后就该解决四轴自旋的问题了，一般来说XY轴的PID参数是一

样的，调好一边，另一边直接复制过去就行。

自旋控制方法有2种，一种是对HMC5883的值进行PD控制。一种是对Z轴陀螺仪

旋转速度进行PD控制，后面那种就够了，只不过需要飞前对陀螺仪偏差进行校准。这个

PD参数不要太纠结，Z轴稍微给点控制就稳了，毕竟XY轴才是四轴飞起来的关键。

此外，PID的要求是系统的输出量要和被调量成正比，故大四轴比小四轴好调得多，

因为大四轴有电调，电调调整的不是电机供电电压，而是力矩，也就是说大四轴输出信号

和升力是成正比的，而小四轴不是，但是我们只需要他大概是正比关系就行了，也能好好

滴做朋友的。推荐碳刷电机PWM频率为500HZ，空心杯最好是20KHZ以上。

调PID中各种问题及解决方法汇总：

四轴偏离一定角度即使加I也纠正不过来，也就是说四轴明明知道他自己偏了，就是

纠正不过来，解决方法：加大P。

D无论怎么调四轴就是要震荡，根本停不下来，解决方法，减小P

系统有细微偏差：加点点I

发现无论怎么调PID根本就不能让四轴取得一个很好的效果：换滤波方案吧，什么梯

度下降法，卡尔曼滤波算法都是很好的。

震动越大角度值和实际值偏离越大：陀螺仪装错位置了，在MPU6050模块下面弄点

泡沫双面胶可以很好解决，原则上来讲MPU6050是不能太靠近轴的。

/?tid=1115916

四轴飞行器的前后、左右、顺时针和逆时针运动是通过姿态角（角度）来控制的，遥

控器是用来设定目标姿态角的，只要测得的姿态与设定的目标姿态进行串级PID控制就可

以使四轴飞行器稳定飞行了。所以做四轴最重要、最关键的就是姿态角测量和串级PID控

制。

姿态结算：测量姿态的传感器主要是三轴加速度计、三轴陀螺仪和三轴磁力计。整个

姿态描述的原理是：通过解算地理坐标系和机体坐标系的角度位置关系来得到姿态，由于

传感器的测量误差（主要是陀螺仪的积分误差和振动引起的加速度计误差），导致测得的

机体坐标系不准，得到的姿态也就会不准确。因为地理坐标系中的四轴飞行器所受的重力

和磁场是个常量，所以将地理坐标系中的重力向量和磁场向量转换到机体坐标系中，此时

转换到机体坐标系的重力向量和磁场向量与机体坐标系中测出来的重力向量和磁场向量会

有误差，只要消除此误差，就可以校正机体坐标系，进而得到准确的姿态。消除误差的方

法有卡尔曼滤波法、互补滤波法、姿态插值法等。考虑到计算能力和现有的资料，本设计

采用互补滤波法，达到的效果也比较好。

姿态解算算法步骤：

姿态解算涉及到坐标的变换，还有互补滤波算法部分。该算法的实现步骤为：

姿态解算C语言算法具体实现：

norm =Q_rsqrt(ax*ax + ay*ay + az*az); //加速度计数据归一化

ax = ax*norm;

ay = ay *norm;

az = az *norm;

vx = 2*(q1q3- q0q2); //转换成机体坐标系中的重力向量

vy = 2*(q0q1+ q2q3);

vz = q0q0 -q1q1 - q2q2 + q3q3 ;

ex = (ay*vz -az*vy) ; //向量外积就是误差

ey = (az*vx -ax*vz) ;

ez = (ax*vy -ay*vx) ;

exInt = exInt+ VariableParameter(ex) * ex * Ki;//对误差积分

eyInt = eyInt+ VariableParameter(ey) * ey * Ki;

ezInt = ezInt+ VariableParameter(ez) * ez * Ki;

gx = gx + Kp* VariableParameter(ex) * ex + exInt; //补偿陀螺仪

gy = gy + Kp* VariableParameter(ey) * ey + eyInt;

gz = gz + Kp* VariableParameter(ez) * ez + ezInt;

q0 = q0 +(-q1*gx - q2*gy - q3*gz)*halfT; //四元数的微分方程

q1 = q1 +(q0*gx + q2*gz - q3*gy)*halfT;

q2 = q2 +(q0*gy - q1*gz + q3*gx)*halfT;

q3 = q3 +(q0*gz + q1*gy - q2*gx)*halfT;

norm =Q_rsqrt(q0q0 + q1q1 + q2q2 + q3q3); //四元数归一化

q0 = q0 *norm;

q1 = q1 *norm;

q2 = q2 *norm;

q3 = q3 *norm;

=asin(-2*q1q3 + 2*q0q2); //四元数转换成欧拉角

=atan2(2*q2q3 + 2*q0q1, -2*q1q1 - 2*q2q2 + 1);

串级PID控制：姿态角PID+姿态角速度PID

姿态角速度就是陀螺仪测量的数据，为了使四轴飞行器稳定飞行，应该使飞行器姿态

稳定的时候，姿态速度也应为零，故采用角度和角速度串级PID控制器。由于姿态速度响

应的速度比姿态响应的速度快，四轴飞行器飞行的时候引起的姿态速度变化也比姿态速度

变化大，故姿态速度控制作为内环，姿态控制作为外环，也就是角度外环和角速度内环组

成的串级PID控制器。

串级PID具体C语言代码：

if(te >= 2) //内环进行2次控制、外环进行1次控制、内环控制频率为外

环2倍

{

//*****************外环PID**************************//

//俯仰计算//

pit=pit- (Rc_ - Rc__offset)/30;

ent+= pit; //俯仰方向误差积分

//积分限幅

if(ent> ent_max)

ent= ent_max;

elseif(ent < -ent_max)

ent= -ent_max;

_out= * pit + *

ent+ * (pit - pitch_old);

pitch_old= pit; //储存 俯仰偏差

//横滚计算//

rol=rol- (Rc_ - Rc__offset)/30;

ent+= rol; //横滚方向误差积分

//积分限幅

if(ent> ent_max)

ent= ent_max;

elseif(ent < -ent_max)

ent= -ent_max;

_out = * rol + *

ent + * (rol -roll_old);

roll_old= rol; //储存 横滚偏差

//航向计算////////////

_out = * (Rc_

-Rc__offset)/10 + * .z;

yaw_old=yaw;

te = 0;

}

te++;

//*************内环(角速度环)PD****************************//

_out= * (_out +

.y *RtA);

_out= * (.y -

.y);

_out= * (_out +

.x * RtA);

_out= * (.x -

.x);

_out= * (_out +

.z * RtA);

_out= * (.z -

.z);

_out= _out + _out;

_out= _out + _out;

_out= _out + _out;

_out、_out和_out为电机输出

值。

超声波定高：

超声波定高就是测量高度数据与设定数据进行PID控制，在此I需要限幅，高度控制

输出值代替油门输出即可。下面定高视频是一键起飞、自主定高、一键降落，定高1米，

变化范围不超过5厘米。