匿名pid算法详解

对串级PID和单级PID的理解(基于匿名微型六轴)

来源：本站 | 作者：小古 | 发表时间：2015-04-01 | 点击量：1565

/******************** (C) COPYRIGHT 2014 ANO Tech

*************************** * 文件名 ：ANO_FlyControl.cpp

* 描述 ：飞行控制

*********************************************************************

*************/

include \ANO_FlyControl fc;

/*

先整定内环，后整定外环。 参数整定找最佳，从小到大顺序查

先是比例后积分，最后再把微分加

曲线振荡很频繁，比例度盘要放大

曲线漂浮绕大湾，比例度盘往小扳

曲线偏离回复慢，积分时间往下降

曲线波动周期长，积分时间再加长

曲线振荡频率快，先把微分降下来

动差大来波动慢。微分时间应加长

理想曲线两个波，前高后低4比1

*/ /*

ROLL和PIT轴向按照以上公式计算PID输出，但YAW轴比较特殊，因为偏航

角法线方向刚好和地球重力平行，

这个方向的角度无法由加速度计直接测得，需要增加一个电子罗盘来替代加速度

计。如果不使用罗盘的话，

我们可以单纯的通过角速度积分来测得偏航角，缺点是由于积分环节中存在积分

漂移，偏航角随着时间的推移

会偏差越来越大。我们不使用罗盘就没有比例项，只仅使用微分环节来控制。

*/

ANO_FlyControl::ANO_FlyControl()

{ yawRate = 120; //重置PID参数 PID_Reset();

}

//重置PID参数

void ANO_FlyControl:

{

//因为YAW角度会漂移，所以参数和ROLL、PITCH不一样

pid[PIDROLL].set_pid(70, 15, 120, 2000000); //ROLL角度的内环控制系数,20000:

积分上限

pid[PIDPITCH].set_pid(70, 30, 120, 2000000);//PITCH角度的内环控制系数

pid[PIDYAW].set_pid(100, 50, 0, 2000000); //YAW角度的内环控制系数

pid[PIDLEVEL].set_pid(280, 0, 0, 0); //外环控制系数

pid[PIDMAG].set_pid(15, 0, 0, 0); //电子罗盘控制系数

} /* 【扫盲知识】

串级PID：采用的角度P和角速度PID的双闭环PID算法------>角度的误差被作

为期望输入到角速度控制器中 （角度的微分就是角速度）

对于本系统则采用了将角度控制与角速度控制级联的方式组成整个串级 PID

ID_Reset(void)

控制器。

串级 PID 算法中，角速度内环占着极为重要的地位。在对四旋翼飞行的物理模

型进

行分析后，可以知道造成系统不稳定的物理表现之一就是不稳定的角速度。 因此，若能够直接对系统的角速度进行较好的闭环控制，必然会改善系统的动态

特性

及其稳定性，通常也把角速度内环称为增稳环节。而角度外环的作用则体现在对

四旋翼飞

行器的姿态角的精确控制。 外环：输入为角度,输出为角速度 内环：输入为角速度，输出为PWM增量

使用串级pid，分为：角度环控制pid环，和角速度控制环稳定环。主调为角度

环（外环），副调为角速度环（内环）。

参数整定原则为先内后外，故在整定内环时将外环的PID均设为0 所谓外环就是只是一个P在起作用，也就是比例在起作用；P也就是修正力度，

越大越容易使飞机震荡。 震荡的特点是：频率小、幅度大

*/ /*

【横滚（Roll）和俯仰（Pitch）的控制算法】

横滚（Roll）和俯仰（Pitch）的控制算法是一样的，控制参数也比较接近。

首先得到轴姿态的角度差（angle error），将这个值乘以角度系数p后限幅（限

幅必须有，否则剧烈打舵时容易引发震荡）作为角速度控制器期望值（target_rate）。target_rate与陀螺仪得到的当前角速度作差，得到角速度误差（rate_error）乘以kp得到P。在I值小于限幅值（这个值大概在5%油门）或者rate_error与i值异号时将rate_error累加到I中。前后两次rate_error的差作为D项，值得注意的是加需要入20hz（也可以采用其它合适频率）滤波，以避免震

荡。将P,I,D三者相加并限幅（50%油门）得到最终PID输出。

*/

//串环PID调节详情参见：http://blog.csdn.net/super_mic ... 36723

//飞行器姿态外环控制

void ANO_FlyControl::Attitude_Outter_Loop(void)

{

int32_t errorAngle[2]; Vector3f Gyro_ADC;

//计算角度误差值, 角度误差值=期望值-此刻姿态值

//constrain_int32作用：32位整型数限幅，使其控制输入的最大飞行倾角不大于

25度（如果控制量比25度大，飞机早就坠毁了）

//rc.Command[ROLL]：遥控数据 imu.angle.x ：此刻姿态(角度)

//1.得到轴姿态的角度差（errorAngle）

//2.这个角度差值进行限幅(constrain_int32)（正负FLYANGLE_MAX）（限幅必须有，否则剧烈打舵时容易引发震荡）作为角速度控制器期望值（target_rate）

errorAngle[ROLL] = constrain_int32((rc.Command[ROLL] * 2) , -((int)FLYANGLE_MAX), +FLYANGLE_MAX) - imu.angle.x * 10; errorAngle[PITCH] = constrain_int32((rc.Command[PITCH] * 2) , -((int)FLYANGLE_MAX), +FLYANGLE_MAX) - imu.angle.y * 10;

//获取此时陀螺仪上的角速度，取角速度的四次平均值

Gyro_ADC = mpu6050.Get_Gyro() / 4;

/*

得到外环PID输出（角速度的差值）(实质是相当于内环的P比例项)--------> 3.target_rate与陀螺仪得到的当前角速度作差，得到角速度误差（RateError）乘以kp（外环控制系数 pid[PIDLEVEL]--->(280, 0, 0, 0)）得到给内环的P。

*/

//横滚roll：外环控制。输入为角度,输出为角速度。RateError[ROLL] 作为内环

的输入。

RateError[ROLL] = pid[PIDLEVEL].get_p(errorAngle[ROLL]) - Gyro_ADC.x;

//Gyro_ADC.x:陀螺仪X轴的值

//俯仰pitch：外环控制。输入为角度,输出为角速度。RateError[PITCH] 作为内环

的输入。

RateError[PITCH] = pid[PIDLEVEL].get_p(errorAngle[PITCH]) -

Gyro_ADC.y;//Gyro_ADC.y:陀螺仪Y轴的值

/*

偏航（Yaw）的控制算法和前两者略有不同，是将打舵量（遥控数据量rc.Command[YAW]）和角度误差的和作为角速度内环的期望值，

这样可以获得更好的动态响应。角速度内环和横滚与俯仰的控制方法一致，参数

（积分限幅值会很小，默认只有万分之8）上有不同。*/

//航向yaw：外环控制。输入为角度,输出为角速度。 RateError[YAW] 作为内环

的输入。

RateError[YAW] = ((int32_t)(yawRate) * rc.Command[YAW]) / 32 - Gyro_ADC.z;

//Gyro_ADC.z:陀螺仪Z轴的值

}

//飞行器姿态内环控制: 输入为角速度，输出为PWM增量

//内环的效果就是：减小 P比例控制带来的震荡 void ANO_FlyControl::Attitude_Inner_Loop(void)

{

int32_t PIDTerm[3];

//注意这里是i的值是0到2

//PIDROLL、PIDPITCH、PIDYAW是枚举类型，也就是0、1、2，也就是下面

的pid 、PIDTerm就是3组PID

for(u8 i=0; i<3;i++)

{

//现象：当油门低于检查值时积分清零，重新积分

//猜测：这里应该是担心飞机没飞起来时就开始有积分，会导致起飞时不稳定

if ((rc.rawData[THROTTLE]) < RC_MINCHECK)

pid.reset_I();

//get_pid函数：return get_p(error) + get_i(error, dt) + get_d(error, dt);-------->这里实际就是一个完整的PID

本文来源：https://www.bwwdw.com/article/fjy5.html