四轴项目总结之三--pid

本篇主要介绍pid基本原理、实现和参数调试。其中主要有这几个部分的pid要进行实现：姿态的pid实现自稳. 高度pid实现定高. 循迹pid实现寻黑线而行。
一、pid的基本原理
参考：
【1】http://blog.csdn.net/nemol1990/article/details/45131603
【2】http://blog.csdn.net/super_mice/article/details/38436723
(注：将时间常数一并加入Kp,Kd,Ki中了)
1. 比例（P）控制规律：具有P控制的系统，其稳态误差可通过P控制器的增益Kp来调整：Kp越大，稳态误差越小；反之，稳态误差越大。但是Kp越大，其系统的稳定性会降低。m(t) = Kp*e(t);
由上式可知，控制器的输出m(t)与输入误差信号e(t)成比例关系，偏差减小的速度取决于比例系数Kp：Kp越大，偏差减小的越快，但是很容易引起振荡（尤其是在前向通道中存在较大的时滞环节时）；Kp减小，发生振荡的可能性小，但是调节速度变慢。单纯的P控制无法消除稳态误差，所以必须要引入积分I控制。
2. 积分（I）控制规律：由于采用了积分环节，若当前误差e(t)为0，则其输出信号m(t)有可能是一个不为0的常量。需要注意的是，引入积分环节，可以提到系统型别，使得系统可以跟踪更高阶次的输入信号，以消除稳态误差。
m(t) = Ki * | e(t)*d(t) .
3. 微分（D）控制规律：可以反应输入信号的变化趋势，具有某种预见性，可为系统引进一个有效的早期修正信号，以增加系统的阻尼程度，而从提高系统的稳定性。（tao为微分时间常数）
m(t) = Kd*d[e(t)]/dt;
4.P控制对系统性能的影响：
开环增益越大，稳态误差减小（无法消除，属于有差调节）
过渡时间缩短
稳定程度变差
5.I控制对系统性能的影响：
消除系统稳态误差（能够消除静态误差，属于无差调节）
稳定程度变差
6.D控制对系统性能的影响：
减小超调量
减小调节时间（与P控制相比较而言）
增强系统稳定性
增加系统阻尼程度

二、不同部分的pid实现
参考：
http://forum.eepw.com.cn/thread/268670/1
1、姿态部分PID
采用串级PID，外环是角度环，内环是角速度环。
角度环通过角度误差pid计算，得到角速度的期望值，送到角速度环；角速度环再做一次pid得到电机输出。
这样做的目的是，通过两级pid串联，增大了四轴的阻尼（角度误差（角速度）阻尼、角速度误差（角加速度）阻尼）。参数调节好，四轴在飞行跟随手感会很好，机动性强，指哪打哪，也能很好的做到悬停。

    #define angle_max 10.0f 
    #define angle_integral_max 1000.0f 
    /******************************************************* 函数原型： void Control_Angle(struct _out_angle *angle,struct _Rc *rc) 功 能： PID控制器(外环) *******************************************************/  
    void Control_Angle(struct _out_angle *angle,struct _Rc *rc)  
    {  
        static struct _out_angle control_angle;  
        static struct _out_angle last_angle;  
    ////////////////////////////////////////////////////////////////// 
    // 以下为角度环 
    ////////////////////////////////////////////////////////////////// 
        if(rc->ROLL>1490 && rc->ROLL<1510)    
            rc->ROLL=1500;  
        if(rc->PITCH>1490 && rc->PITCH<1510)      
            rc->PITCH=1500;  
    ////////////////////////////////////////////////////////////////// 
        control_angle.roll  = angle->roll  - (rc->ROLL  -1500)/13.0f ;  
        control_angle.pitch = angle->pitch - (rc->PITCH -1500)/13.0f;  
    ////////////////////////////////////////////////////////////////// 
        if(control_angle.roll >  angle_max)  //ROLL 
            roll.integral +=  angle_max;  
        else if(control_angle.roll < -angle_max)  
            roll.integral += -angle_max;  
        else  
            roll.integral += control_angle.roll;  

        if(roll.integral >  angle_integral_max)  
           roll.integral =  angle_integral_max;  
        if(roll.integral < -angle_integral_max)  
           roll.integral = -angle_integral_max;  
    ////////////////////////////////////////////////////////////////// 
        if(control_angle.pitch >  angle_max)//PITCH 
           pitch.integral +=  angle_max;  
        else if(control_angle.pitch < -angle_max)  
           pitch.integral += -angle_max;  
        else  
            pitch.integral += control_angle.pitch;  

        if(pitch.integral >  angle_integral_max)  
           pitch.integral =  angle_integral_max;  
        if(pitch.integral < -angle_integral_max)  
           pitch.integral = -angle_integral_max;  
    ////////////////////////////////////////////////////////////////// 
        if(rc->THROTTLE<1200)//油门较小时，积分清零 
        {  
            roll.integral  = 0;  
            pitch.integral = 0;  
        }  
    ////////////////////////////////////////////////////////////////// 
        roll.output  = roll.kp *control_angle.roll  + roll.ki *roll.integral  + roll.kd *(control_angle.roll -last_angle.roll );  
        pitch.output = pitch.kp*control_angle.pitch + pitch.ki*pitch.integral + pitch.kd*(control_angle.pitch-last_angle.pitch);  
    ////////////////////////////////////////////////////////////////// 
        last_angle.roll =control_angle.roll;  
        last_angle.pitch=control_angle.pitch;  
    }  




下面是内环代码：

view plaincopy to clipboardprint?

    #define gyro_max 50.0f 
    #define gyro_integral_max 5000.0f 
    /****************************************************************************** 函数原型： void Control_Gyro(struct _SI_float *gyro,struct _Rc *rc,uint8_t Lock) 功 能： PID控制器(内环) *******************************************************************************/   
    void Control_Gyro(struct _SI_float *gyro,struct _Rc *rc,uint8_t Lock)  
    {  
        static struct _out_angle control_gyro;  
        static struct _out_angle last_gyro;  
        int16_t throttle1,throttle2,throttle3,throttle4;  
    ////////////////////////////////////////////////////////////////// 
    // 以下为角速度环 
    ////////////////////////////////////////////////////////////////// 
        if(rc->YAW>1400 && rc->YAW<1600)  
            rc->YAW=1500;  
    ////////////////////////////////////////////////////////////////// 
        control_gyro.roll  = -roll.output - gyro->y*Radian_to_Angle;  
        control_gyro.pitch = pitch.output - gyro->x*Radian_to_Angle;  
        control_gyro.yaw   = -(rc->YAW-1500)/2.0f - gyro->z*Radian_to_Angle ;  
    ////////////////////////////////////////////////////////////////// 
        if(control_gyro.roll >  gyro_max)    //GYRO_ROLL 
            gyro_roll.integral +=  gyro_max;  
        else if(control_gyro.roll < -gyro_max)  
            gyro_roll.integral += -gyro_max;  
        else  
            gyro_roll.integral += control_gyro.roll;  

        if(gyro_roll.integral >  gyro_integral_max)  
           gyro_roll.integral =  gyro_integral_max;  
        if(gyro_roll.integral < -gyro_integral_max)  
           gyro_roll.integral = -gyro_integral_max;  
    ////////////////////////////////////////////////////////////////// 
        if(control_gyro.pitch >  gyro_max)//GYRO_PITCH 
            gyro_pitch.integral +=  gyro_max;  
        else if(control_gyro.pitch < -gyro_max)  
            gyro_pitch.integral += -gyro_max;  
        else  
            gyro_pitch.integral += control_gyro.pitch;  

        if(gyro_pitch.integral >  gyro_integral_max)  
           gyro_pitch.integral =  gyro_integral_max;  
        if(gyro_pitch.integral < -gyro_integral_max)  
           gyro_pitch.integral = -gyro_integral_max;  
    ////////////////////////////////////////////////////////////////// 
    // if(control_gyro.yaw > gyro_max)//GYRO_YAW 
    // gyro_yaw.integral += gyro_max; 
    // else if(control_gyro.yaw < -gyro_max) 
    // gyro_yaw.integral += -gyro_max; 
    // else 
            gyro_yaw.integral += control_gyro.yaw;  

        if(gyro_yaw.integral >  gyro_integral_max)  
           gyro_yaw.integral =  gyro_integral_max;  
        if(gyro_yaw.integral < -gyro_integral_max)  
           gyro_yaw.integral = -gyro_integral_max;  
    ////////////////////////////////////////////////////////////////// 
        if(rc->THROTTLE<1200)//油门较小时，积分清零 
        {  
            gyro_yaw.integral  = 0;  
        }  
    ////////////////////////////////////////////////////////////////// 
        gyro_roll.output  = gyro_roll.kp *control_gyro.roll  + gyro_roll.ki *gyro_roll.integral  + gyro_roll.kd *(control_gyro.roll -last_gyro.roll );  
        gyro_pitch.output = gyro_pitch.kp*control_gyro.pitch + gyro_pitch.ki*gyro_pitch.integral + gyro_pitch.kd*(control_gyro.pitch-last_gyro.pitch);  
        gyro_yaw.output   = gyro_yaw.kp  *control_gyro.yaw   + gyro_yaw.ki  *gyro_yaw.integral   + gyro_yaw.kd  *(control_gyro.yaw  -last_gyro.yaw  );  
    ////////////////////////////////////////////////////////////////// 
        last_gyro.roll =control_gyro.roll;  
        last_gyro.pitch=control_gyro.pitch;  
        last_gyro.yaw  =control_gyro.yaw;  
    ////////////////////////////////////////////////////////////////// 
        if(rc->THROTTLE>1200 && Lock==0)  
        {  
            throttle1 = rc->THROTTLE - 1050 + gyro_pitch.output + gyro_roll.output - gyro_yaw.output;  
            throttle2 = rc->THROTTLE - 1050 + gyro_pitch.output - gyro_roll.output + gyro_yaw.output;  
            throttle3 = rc->THROTTLE - 1050 - gyro_pitch.output + gyro_roll.output + gyro_yaw.output;  
            throttle4 = rc->THROTTLE - 1050 - gyro_pitch.output - gyro_roll.output - gyro_yaw.output;  
        }  
        else  
        {  
            throttle1=0;  
            throttle2=0;  
            throttle3=0;  
            throttle4=0;  
        }  
        Motor_Out(throttle1,throttle2,throttle3,throttle4);  
    }  



注意到，pitch和roll用到了内外环，yaw只用到了内环。 

对代码不做过多的讲解，不明白的可以留言或者@我都可以，我们一起讨论。

2、高度PID
参考：http://blog.sina.com.cn/s/blog_65e255160100qvi9.html
由于高度PID是最不好调试的，总需要飞起来调，这里有几点思路，可以供大家借鉴。高度pid用单级pid就行了，e(t)=target_high - act_high。用act_high = high*(sin(theta1)+sin(theta2)+sin(theta3))进行校准，还有别忘了滤波得到准确合理的数值。还考虑一个问题，如果和目标值相差太大，误差岂不是很大，反馈量也很大？怎么办呢？可以考虑分几次慢慢叠加。

3、循迹PID
这里我主要参考了飞思卡尔智能车的实现，其它的还有光流算法，GPS巡航，我没有去研究。
首先，摄像头的安装位置很有讲究，有些说要保证什么前瞻性的，但个人觉得如果飞行速度不是太快（用它循迹也不能太快）装正下方偏前一点的位置就行了。误差项即为采回的图像的中线和循迹黑线中心线的差值，还是用单级pid就行了。

三、PID参数的调试
其实，这对于很多人来说，算是一件很困难的事。个人觉得快刀可以斩乱麻，我们不妨先把刀磨快。这里的刀，就是一个调试助手，我们得实时的了解各项参数，这样才知道往哪个方向调，出现了什么问题。一个写的不错的开源软件叫匿名上位机，很不错的，现在又新加入了很多功能了，估计，使用别人的上位机只需要符合它的通信协议就行了，这在你的程序中要去实现。当然，如果你自己有兴趣，也可以用C#写一个简单的上位机，基于串口，或网络都可以。这里给出一个参考网址：http://wenku.baidu.com/link?url=8LlIRn1qp8HvRVKWt4owK7FiUpdTRMGzgjTrKKUCwnLEW2RzORht0bxvcfWooI47Iva4Nw-EGE-LDp9rFwjUdDsN3PHjfeo2i5tver7TEB3
调试时，可以先将飞机绑起来，做烤四轴也可以，可以参考上面我介绍的两篇文章。注意四轴是很危险的，鄙人程序跑飞了两次，所幸都躲过一劫，并没有伤到，只是有一个调的时候，割掉了一点小皮。
总的原则是：
1、先调roll或pitch，单一方向的调，调好后复制到另一方向，基本是一样的参数，yaw方向稍有不同，我的经验是Kp基本可以不变。
2、调一个方向时，先调内环，调好内环再调外环，内环调好的状态是，在平衡位置放开时，会慢慢的往一边倒（大概几秒），往一边扳时，会有较大的阻力。这是便可以加一点外环量了，如果发现不对，还需要回到内环，这样反复进行。
3、调内环、外环时，都从p->i->d的顺序依次进行。增大还是减少按照我们第一部分介绍的基本原理进行。
4、内外环只加p时，四轴基本能够等幅震荡，这时我们可以加一点点d，引入高频分量，震动就没这么厉害了，最后，如果能在某个位置平衡，但平衡位置不在中心位置时，加一点点i,i能消除稳态误差。
5、内外环是相对的，一个太硬了，另一个势必会表现的比较软，从这里可以知道调试的方向。
6、经过以上的步骤，如果roll和pitch基本差不多了，就可以去试飞了，注意检查方向有没有搞错，如果出现一起飞就炸，大概率是d太大了，引来很大的震动，试飞，主要观察能不能较好的自稳，打方向的软硬，会不会出现过调啊，响应速度怎么样，再进行相应的调试。

到这就差不多了，好好玩吧，注意安全哦。