四轴PID思路整理

参考资料：

https://blog.csdn.net/nemol1990/article/details/45131603

https://blog.csdn.net/qq_27114397/article/details/53378941

https://blog.csdn.net/sunnyxiaohu/article/details/50601577

http://forum.eepw.com.cn/thread/259834/1

常用四轴的两种PID算法讲解(单环PID、串级PID)

这里主要讲解的PID算法属于一种线性控制器，这种控制器被广泛应用于四轴上。要控制四轴，显而易见的是控制它的角度，那么最简单，同时也是最容易想到的一种控制策略就是角度单环PID控制器，系统框图如图所示

或许有些朋友看得懂框图，但是编程实现有一定困难，在这里笔者给出了伪代码：

上述角度单环PID控制算法仅仅考虑了飞行器的角度信息，如果想增加飞行器的稳定性(增加阻尼)并提高它的控制品质，我们可以进一步的控制它的角速度，于是角度/角速度-串级PID控制算法应运而生。在这里，相信大多数朋友已经初步了解了角度单环PID的原理，但是依旧无法理解串级PID究竟有什么不同。其实很简单：它就是两个PID控制算法，只不过把他们串起来了(更精确的说是套起来)。那这么做有什么用？答案是，它增强了系统的抗干扰性(也就是增强稳定性)，因为有两个控制器控制飞行器，它会比单个控制器控制更多的变量，使得飞行器的适应能力更强。为了更为清晰的讲解串级PID，这里笔者依旧画出串级PID的原理框图，如图所示：

同样，为了帮助一些朋友编程实现，给出串级PID伪代码：

二、代码学习整理

根据上述的描述，并且参考给出的代码：https://github.com/yzhajlydy/Micro-Quadrotor

以下我个人的理解，其中外环的 角度误差 = 期望误差 - 当前误差

当前误差其实就是当前的角度如pitch, roll, yaw

那么期望误差其实就是遥控器的控制角度，遥控器的角度范围是1000~2000（天地飞07）。那么比如如果我要控制±40°的信号

/**************************实现函数********************************************
*函数原型:        void Direction_Control(void)
*功    能:        前后左右方向控制
*******************************************************************************/
void Direction_Control(void)
{
    //根据遥控器传过来的前后方向值，改变ZHONGZHI_PIT的给定值
    //按40度计算，500/40 = 12.5(pwm/度) 2度是25
    ZHONGZHI_PIT = (myControl.remoteControl[]-)/12.5;

    //根据遥控器传过来的左右方向值，改变ZHONGZHI_ROL的给定值
    ZHONGZHI_ROL = (myControl.remoteControl[]-)/12.5;
}

/**************************实现函数********************************************
*函数原型:        void Outter_PID(void)
*功    能:        外环角度控制
*******************************************************************************/
void Outter_PID(void)
{
    //计算X轴和Y轴角度偏差值
    e_pit = ZHONGZHI_PIT-myControl.pitch;
    e_rol = ZHONGZHI_ROL-myControl.roll;

    //外环PID运算
    angular_speed_X = pit_p*e_pit;
    angular_speed_Y = rol_p*e_rol;
}

然后外环的输出，是内环的输入。

/**************************实现函数********************************************
*函数原型:        void Inner_PID(void)
*功    能:        内环角速度控制
*******************************************************************************/
void Inner_PID(void)
{
    //计算X轴和Y轴角速度偏差
    e_X[] = angular_speed_X - myControl.gyro_X;
    e_Y[] = angular_speed_Y - myControl.gyro_Y;

    //===========================绕X轴内环PID运算============================================
    //积分分离，以便在偏差较大的时候可以快速的缩减偏差，在偏差较小的时候，才加入积分，消除误差
    if(e_X[]>=150.0||e_X[]<=-150.0){
        flag_X = ;
    }else{
        flag_X = ;
        e_I_X += e_X[];
    }

    //积分限幅
    if(e_I_X>MOTOR_MAXVALUE)
        e_I_X=MOTOR_MAXVALUE;
    if(e_I_X<MOTOR_MIDVALUE)
        e_I_X=MOTOR_MIDVALUE;                                  

    //位置式PID运算
    PWM_X = kp1*e_X[]+flag_X*ki1*e_I_X+kd1*(e_X[]-e_X[]);

    //===========================绕Y轴内环PID运算========================================
    //积分分离，以便在偏差较大的时候可以快速的缩减偏差，在偏差较小的时候，才加入积分，消除误差
    if(e_Y[]>=150.0||e_Y[]<=-150.0){
        flag_Y = ;
    }else{
        flag_Y = ;
        e_I_Y += e_Y[];
    }

    //积分限幅
    if(e_I_Y>MOTOR_MAXVALUE)
        e_I_Y=MOTOR_MAXVALUE;
    if(e_I_Y<MOTOR_MIDVALUE)
        e_I_Y=MOTOR_MIDVALUE;

    //位置式PID运算
    PWM_Y = kp2*e_Y[]+flag_Y*ki2*e_I_Y+kd2*(e_Y[]-e_Y[]);

    //=======================================================================================
    //记录本次偏差
    e_X[] = e_X[];                                //用本次偏差值替换上次偏差值
    e_Y[] = e_Y[];                                //用本次偏差值替换上次偏差值

}

三、PID调试整理

PID控制器我大概就是这么实现的，首先将四轴固定在单轴平衡平台上，让飞行器完成单轴平衡，主要观察姿态角的

（1）稳定性，能否平衡在期望角度；
（2）响应性，当操纵命令改变时，四轴能否即时的响应期望的变化；
（3）操纵性，由操纵员感受四轴的姿态是否已与操纵，会不会产生响应过冲。

P

因为PID有内外环之分，所以应该先从内环调节，内环稳定后再调外环。

内环的顺序是先调P,其他I、D设为0，那么如何确定P的值呢？主要的现象是等幅震荡，就是四轴会绕着轴来回摆动。

调节P时遇到的情况：

1）四轴开启后加速翻转，说明P的值方向错了。如果P值过大，可能出现自转的情况，就是绕着轴最大马力旋转（太可怕了）

2）四轴在某个角度稍稍停顿，然后掉落，说明P给小了

3）当P太小时，四轴在很大倾斜的地方，在重力与P的作用下也会震荡，这种震荡不是等幅的，也不是对称的，震荡波谷的绝对值明显要比波峰绝对值大而且距离0度会很远。这种情况要加以区分。

参考资料中说，在有一点反馈力到等幅震荡的区间内确定四轴的P值。

D

然后开始调D，如果说P是回复力的话，那么D就是阻尼。虽然它能抑制震荡，但同时也会抑制P的效果。

当震动产生时，在震动的中心点四轴震动速度最大，也是D作用最强的时刻。

加了D效果就比较明显了，四轴在外力的干扰先能明显的有回复力且，能快速稳定在平衡点了，调D就是试，当然D大了也会产生震荡，但是此时不加D时光P作用时的震荡就很小，很明显就可以看出随着D的增大，震荡减小又增大的过程。

I

调 I 的时候我先把积分限幅去掉。然后从小往大加，当调平衡时，随着油门的变大，静差应该是越来小的。

我将油门推到差不多快要将四轴推离地面的位置，看I能不能消除静差。因为如果油门给小了，静差较大，调出来的I虽然能消除静差但是I比较大，在加油门时有可能也会产生超调震荡。

最后I要再能消除静差又不产生震荡，个人认为要近可能小。最后加上合适的积分限幅。

其他资料的PID整定：

而笔者在整定串级PID时的经验则是：先整定内环PID，再整定外环P。

内环P：从小到大，拉动四轴越来越困难，越来越感觉到四轴在抵抗你的拉动；到比较大的数值时，四轴自己会高频震动，肉眼可见，此时拉扯它，它会快速的振荡几下，过几秒钟后稳定；继续增大，不用加人为干扰，自己发散翻机。

特别注意：只有内环P的时候，四轴会缓慢的往一个方向下掉，这属于正常现象。这就是系统角速度静差。

内环I：前述PID原理可以看出，积分只是用来消除静差，因此积分项系数个人觉得没必要弄的很大，因为这样做会降低系统稳定性。从小到大，四轴会定在一个位置不动，不再往下掉；继续增加I的值，四轴会不稳定，拉扯一下会自己发散。

特别注意：增加I的值，四轴的定角度能力很强，拉动他比较困难，似乎像是在钉钉子一样，但是一旦有强干扰，它就会发散。这是由于积分项太大，拉动一下积分速度快，给  的补偿非常大，因此很难拉动，给人一种很稳定的错觉。

内环D：这里的微分项D为标准的PID原理下的微分项，即本次误差-上次误差。在角速度环中的微分就是角加速度，原本四轴的震动就比较强烈，引起陀螺的值变化较大，此时做微分就更容易引入噪声。因此一般在这里可以适当做一些滑动滤波或者IIR滤波。从小到大，飞机的性能没有多大改变，只是回中的时候更加平稳；继续增加D的值，可以肉眼看到四轴在平衡位置高频震动(或者听到电机发出滋滋的声音)。前述已经说明D项属于辅助性项，因此如果机架的震动较大，D项可以忽略不加。

外环P：当内环PID全部整定完成后，飞机已经可以稳定在某一位置而不动了。此时内环P，从小到大，可以明显看到飞机从倾斜位置慢慢回中，用手拉扯它然后放手，它会慢速回中，达到平衡位置；继续增大P的值，用遥控器给不同的角度给定，可以看到飞机跟踪的速度和响应越来越快；继续增加P的值，飞机变得十分敏感，机动性能越来越强，有发散的趋势。

整体方法：
1，将内外环PID都归0，适当增加内环的P，调整P至四轴从正面朝上自然转动到正面朝下时能感受到阻力，且没有抖动，有抖动就应减小P，当P减小到无抖动或者轻微抖动时即可。

2，让内环的D慢慢增加，到你用手能明显感受到转动四轴产生排斥外力的阻力即可，D能抑制P产生的振荡，但是D过大也会导致高频振荡，调整D至系统无振荡且能抑制外界的力即可。

3，给内环一点点I，注意的是I的积分要在油门开启后才开始，油门关闭就清0，且必须有积分限幅。I推荐取越小越好，我取的是0.01，I取大了会导致系统振荡。

4，将内环P减半，将外环P调至内环的50-70倍，根据系统产生的高频振荡降低内环的D，直至高频振荡消除即可。

5，给外环一点点I，同3.

6，根据实际情况对参数进行优化调整，调整过程中要注意区分各个参数的作用，时刻记住，P是回复力，大了会低频振荡，D是抑制力，大了会高频振荡，I是静差消除力，越小越好，大了会产生振荡。

四、衰减曲线法

https://blog.csdn.net/zhuifeng_tjy163163/article/details/78961017

我自己的实验记录：

我把油门摇杆打上3格，P调到1的时候反应有点小，2的时候。用手保持飞机朝上，轻轻放手，再拉住。感觉有明显的来回感，有时候掉下去快了，回馈力也比较大。

调到4的时候还是会往下掉，调到5的时候落下去回馈过头，翻过去了。4.5还是翻过去了，4.3下落的时候顿了一下。大概4.6到4.5的时候会反转，而4.3到4.5就会顿一下再下落。

-------------------------------------------------------------------------------------------------------------

2018年8月3日 20点51分 好痛苦，调了这么久都没效果。后来用示波器看了波形速率。发现9250的DMP读取有问题。

STM32F103如果把读取速率取到100，加上其他工作诸如串口啊（改成DMA方式发送还是占用资源）、定时器啊、外部中断啊都会被影响

后来我取了80效果一顿一顿的了，取到50发现匹配的刚刚好。如果有硬件I2C或许会快点，不过出来的曲线好多了。很平滑，没有之前的折线了。

为什么会发现这个问题？我在修改PID的时候，飞机要到30度倾角才有反应。这就让我很困惑，加大到了200Hz效果依然不好，最后用示波器调试。配合上位机才定位到问题。

感觉自己都在走弯路，硬件性能都没弄好还一个劲的调PID，调的自己还很泄气。

-----------------------------------------------------------------------------------------------------------------

硬件问题解决了，飞机的反应立马好了不少。再也没有之前的顿顿的强烈反应，取而代之的是平滑的往下滑，好蛋疼。虽然角速度基本没动，但是角度一直会慢慢往下滑。我在想是不是要加上外环才能稳定，加了外环也好像不能改善。

------------------------------------------------------------------------------------------------------------------

我又调试了一下，现在四轴的情况是，内环把角速度控制在比较小的变动范围，但是飞机还是会往下翻。然后我用外环控制姿态，意味着飞机一直会调整一下调整一下。

这样的情况单轴调试效果还行，但是试飞阶段时，我发现调试油门比起飞油门小了不少，导致飞机起飞的时候非常不稳。然后再改回单轴调试发现，用起飞的油门调试时。

P是太大了，我还是要重新调整。