匿名pid算法详解

对串级PID和单级PID的理解(基于匿名微型六轴)来源：本站|作者：小古|发表时间：2015-04-01|点击量：1565/********************(C)COPYRIGHT2014ANOTech****************************文件名：ANO_FlyControl.cpp*描述：飞行控制**********************************************************************************/includeANO_FlyControl.hANO_FlyControlfc;/*先整定内环，后整定外环。参数整定找最佳，从小到大顺序查先是比例后积分，最后再把微分加曲线振荡很频繁，比例度盘要放大曲线漂浮绕大湾，比例度盘往小扳曲线偏离回复慢，积分时间往下降曲线波动周期长，积分时间再加长曲线振荡频率快，先把微分降下来动差大来波动慢。微分时间应加长理想曲线两个波，前高后低4比1*//*ROLL和PIT轴向按照以上公式计算PID输出，但YAW轴比较特殊，因为偏航角法线方向刚好和地球重力平行，这个方向的角度无法由加速度计直接测得，需要增加一个电子罗盘来替代加速度计。如果不使用罗盘的话，我们可以单纯的通过角速度积分来测得偏航角，缺点是由于积分环节中存在积分漂移，偏航角随着时间的推移会偏差越来越大。我们不使用罗盘就没有比例项，只仅使用微分环节来控制。*/ANO_FlyControl::ANO_FlyControl(){yawRate=120;//重置PID参数PID_Reset();}//重置PID参数voidANO_FlyControl:ID_Reset(void){//因为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控制器。串级PID算法中，角速度内环占着极为重要的地位。在对四旋翼飞行的物理模型进行分析后，可以知道造成系统不稳定的物理表现之一就是不稳定的角速度。因此，若能够直接对系统的角速度进行较好的闭环控制，必然会改善系统的动态特性及其稳定性，通常也把角速度内环称为增稳环节。而角度外环的作用则体现在对四旋翼飞行器的姿态角的精确控制。外环：输入为角度,输出为角速度内环：输入为角速度，输出为PWM增量使用串级pid，分为：角度环控制pid环，和角速度控制环稳定环。主调为角度环（外环），副调为角速度环（内环）。参数整定原则为先内后外，故在整定内环时将外环的PID均设为0所谓外环就是只是一个P在起作用，也就是比例在起作用；P也就是修正力度，越大越容易使飞机震荡。震荡的特点是：频率小、幅度大*//*【横滚（Roll）和俯仰（Pitch）的控制算法】横滚（Roll）和俯仰（Pitch）的控制算法是一样的，控制参数也比较接近。首先得到轴姿态的角度差（angleerror），将这个值乘以角度系数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调节详情参见：飞行器姿态外环控制voidANO_FlyControl::Attitude_Outter_Loop(void){int32_terrorAngle[2];Vector3fGyro_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比例控制带来的震荡voidANO_FlyControl::Attitude_Inner_Loop(void){int32_tPIDTerm[3];//注意这里是i的值是0到2//PIDROLL、PIDPITCH、PIDYAW是枚举类型，也就是0、1、2，也就是下面的pid、PIDTerm就是3组PIDfor(u8i=0;i3;i++){//现象：当油门低于检查值时积分清零，重新积分//猜测：这里应该是担心飞机没飞起来时就开始有积分，会导致起飞时不稳定if((rc.rawData[THROTTLE])RC_MINCHECK)pid.reset_I();//get_pid函数：returnget_p(error)+get_i(error,dt)+get_d(error,dt);--------这里实际就是一个完整的PID//PID_INNER_LOOP_TIME：2000us---0.2ms积分微分时间，每隔0.2ms操作积分和微分,RateError是外环计算的结果（从外环算出）//得到内环PID输出，直接输出转为电机控制量PIDTerm=pid.get_pid(RateError,PID_INNER_LOOP_TIME);}//对YAW角继续处理，加入遥控控制//在I值小于限幅值（这个值大概在5%油门）或者rate_error与i值异号时将rate_error累加到I中。PIDTerm[YAW]=-constrain_int32(PIDTerm[YAW],-300-abs(rc.Command[YAW]),+300+abs(rc.Command[YAW]));//PID输出转为电机控制量motor.writeMotor(rc.Command[THROTTLE],PIDTerm[ROLL],PIDTerm[PITCH],PIDTerm[YAW]);}/*【调节串环PID大概过程（注意修正反向）】1、估计大概的起飞油门。2、调整角速度内环参数。3、将角度外环加上，调整外环参数。4、横滚俯仰参数一般可取一致，将飞机解绑，抓在手中测试两个轴混合控制的效果（注意安全），有问题回到“烤四轴”继续调整，直至飞机在手中不会抽搐。5、大概设置偏航参数（不追求动态响应，起飞后头不偏即可），起飞后再观察横滚和俯仰轴向打舵的反应，如有问题回到“烤四轴”。6、横滚和俯仰ok以后，再调整偏航轴参数以达到好的动态效果。*//*【过程详解】1、要在飞机的起飞油门基础上进行PID参数的调整，否则“烤四轴”的时候调试稳定了，飞起来很可能又会晃荡。2、内环的参数最为关键！理想的内环参数能够很好地跟随打舵（角速度控制模式下的打舵）控制量。在平衡位置附近（正负30度左右），舵量突加，飞机快速响应；舵量回中，飞机立刻停止运动（几乎没有回弹和震荡）。2.1首先改变程序，将角度外环去掉，将打舵量作为内环的期望（角速度模式，在APM中叫ACRO模式，在大疆中叫手动模式）。2.2加上P，P太小，不能修正角速度误差表现为很“软”倾斜后难以修正，打舵响应也差。P太大，在平衡位置容易震荡，打舵回中或给干扰（用手突加干扰）时会震荡。合适的P能较好的对打舵进行响应，又不太会震荡，但是舵量回中后会回弹好几下才能停止（没有D）。2.3加上D，D的效果十分明显，加快打舵响应，最大的作用是能很好地抑制舵量回中后的震荡，可谓立竿见影。太大的D会在横滚俯仰混控时表现出来（尽管在“烤四轴”时的表现可能很好），具体表现是四轴抓在手里推油门会抽搐。如果这样，只能回到“烤四轴”降低D，同时P也只能跟着降低。D调整完后可以再次加大P值，以能够跟随打舵为判断标准。2.4加上I，会发现手感变得柔和了些。由于笔者“烤四轴”的装置中四轴的重心高于旋转轴，这决定了在四轴偏离水平位置后会有重力分量使得四轴会继续偏离平衡位置。I的作用就可以使得在一定角度范围内（30度左右）可以修正重力带来的影响。表现打舵使得飞机偏离平衡位置，舵量回中后飞机立刻停止转动，若没有I或太小，飞机会由于重力继续转动。3、角度外环只有一个参数P。将外环加上（在APM中叫Stabilize模式，在大疆中叫姿态模式）。打舵会对应到期望的角度。P的参数比较简单。太小，打舵不灵敏，太大，打舵回中易震荡。以合适的打舵反应速度为准。4、至此，烤四轴”效