void AngleControl(void) 

        {

        BST_fCarAngle = Roll - CAR_ZERO_ANGLE;//DMP ROLL滚动方向角度与预设小车倾斜角度值的差得出角度   

        BST_fAngleControlOut =  BST_fCarAngle * BST_fCarAngle_P + gyro[0] * BST_fCarAngle_D ;  //角度PD控制   

        }
复制代码

  void SpeedControl(void)

        {

        BST_fCarSpeed = (BST_s32LeftMotorPulseSigma  + BST_s32RightMotorPulseSigma );  

        //左右电机脉冲数平均值作为小车当前车速

        BST_s32LeftMotorPulseSigma =BST_s32RightMotorPulseSigma = 0;  

        //全局变量 注意及时清零

        BST_fCarSpeedOld *= 0.7;

        BST_fCarSpeedOld +=BST_fCarSpeed*0.3;

        BST_fCarPosition += BST_fCarSpeedOld;  //路程  即速度积分

        BST_fCarPosition += BST_fBluetoothSpeed;   //融合蓝牙给定速度

        BST_fCarPosition +=fchaoshengbo;   //融合超声波给定速度

        if(stopflag==1)

        {

                    BST_fCarPosition=0;

        }//悬停以后的位置

        


        //积分上限设限//

        f((s32)BST_fCarPosition > CAR_POSITION_MAX)    BST_fCarPosition = CAR_POSITION_MAX;

        if((s32)BST_fCarPosition < CAR_POSITION_MIN)    BST_fCarPosition = CAR_POSITION_MIN;          

        BST_fSpeedControlOutNew = (BST_fCarSpeedOld -CAR_SPEED_SET ) * BST_fCarSpeed_P + (BST_fCarPosition - CAR_POSITION_SET ) * BST_fCarSpeed_I; //速度PI算法 速度*P +位移*I=速度PWM输出

        }
复制代码

void SysTick_Handler(void) //5ms定时器

    {  

        BST_u8MainEventCount++;   //总循环计数值

        BST_u8trig++;

        BST_u8SpeedControlCount++;  //小车速度控制调用计数值GetMotorPulse();//脉冲计算函数BST_u8SpeedControlPeriod++;

        BST_u8DirectionControlPeriod++;   //转向平滑输出计算比例值

        BST_u8DirectionControlCount++;

        AngleControl();  //角度PD控制PWNM输出

        MotorOutput();  //小车总PWM输出  

        if(BST_u8trig>=2)

        {

            UltrasonicWave_StartMeasure();   //调用超声波发送程序 给Trig脚 <10us 高电平chaoshengbo();       //计算超声波测距距离

            BST_u8trig=0;

        }

        if(BST_u8SpeedControlCount>=8)       //当计数值8时，即总系统运行40ms时候(每10个角度PWM输出中融入1个速度PWM输出，这样能保持速度PID输出不干扰角度PID输出，从而影响小车平衡)

        {

             SpeedControl();                     //车模速度控制函数   每40ms调用一次

             BST_u8SpeedControlCount=0;  //小车速度控制调用计数值清零

             BST_u8SpeedControlPeriod=0;  //平滑输出比例值清零

         }    

    }
复制代码

PID 控制是自动化控制领域应用非常广的控制方式，P 代表比例，I 代表积分，D 代表微分，从这些名词中可以看出，PID 控制是基于数学中一项重要的分支：微积分学为基础的数字化自动控制方式，它以传感器采集的数据作为输入源，按预定的 PID 参数根据特定的公式计算以后输出控制。

案例一：

一列即将到站的火车在快要到达站点的时候会切断输出动力，让其凭借惯性滑行到月台位置。

假如设置火车以 100km/h 的速度在站前 1km 的地方切断动力开始滑行，那么这个 100 比 1 就是比例 P 的含义，P 越大，它在站前开始滑行的速度越快。

滑行初始速度快的好处就是进站快，但过快的初始滑行速度会导致火车在惯性的作用下冲过月台，这样一来火车不得不进行倒车，但是因为 P 设置过大，倒车以后的滑行也会同样使火车倒过头了，这样一来，就形成了一种反复前行后退的震荡局面。而 P 设置小了，进站速度会变得非常缓慢，进站时间延长。

所以设置一个合适的 P 值是 PID 调节的首要任务。  

由于 P 是一个固定的数值，如果将火车的速度与月台的距离用一个坐标图理想化的表现出来的话，不考虑惯性及外力的作用，这两者的关系呈现出来 P 调节的结果会是一条直斜线，斜线越陡，代表进站时间越短。  不管怎样，如果只有 P 调节，火车要么设置一个比较低的 P 值以非常缓慢的速度到达目标月台，要么就是过冲了，很难设置在速度与准确度之间求得平衡。

所以接下来该是讲解 D 微分的作用的时候了。

根据上面举的例子，假如 P 等于 100 的时候，火车刚好能滑行到月台，所耗费的时间是 10 分钟。但是对应一个自稳定性能要求很高的自动化系统来说，这 10 分钟的时间太长了，可不可以加快呢？

可以，我们把 P 加大到 120，让火车司机驾驶火车在站前 1km 的地方以 120km/h 的速度开始减速滑行，然后。

站前 500 米时踩一下刹车让速度降为 80km/h。

站前 300 米再踩一下刹车让速度降为 50km/h。

站前 100 米又踩一下刹车，让速度降为 20km/h。

站前 10 米让火车在较短的时间内滑行到月台准确的位置，这样一来，进站速度会加快，原来需要 10 分钟的时间可能只需要 5 分钟就行了。

这就是 D 的作用，我们权且把 D 理解为刹车吧，如果仍旧以坐标图形象表达 D 对 P 调节的影响，那就是 D 使 P 调节出来的一条直线变成了一条曲线，在 PID 公式中，D 的左右就是改变 P 的曲线，D 的数值越大，对 P 的影响也越大。加入 D 后的曲线前期较陡，进站比较快，后期平缓，使得火车可以平稳准确的进站。

根据 PD 关系，我们可以得出一个调节步骤：

先把 D 置零，加大 P值

然后增加 D 的数值，拉低 P 调节后期的作用

使过冲现象放缓，最终调到不过冲为止

这里的 D 指的就是 D 的数值，在一般的 PID 表述中，D 越接近 0，P 作用越大，这点需要注意一下。

最后讲解 I 的作用，I 是积分

是为了消除误差而加入的参数

假如上面的例子中，火车靠站以后，离最终的目标停止线还是差了 1 米，我们虽然也可以认为这是一次合格的停车，但这毕竟是误差，如果我们认可了这 1 米的误差，那在此基础上火车第二次靠站就会有 2米的误差了。

如此以往，误差会越来越大，所以我们要把这个误差记录下来，当第二次进站的时候就可以发挥作用了，如果差了 1 米，火车驾驶员就可以在原来的 PD 调节基础上进行I 积分，延迟 1 米输出（或者提前），即 999 米开始减速，最终可以刚刚好到达停止线。

实例二：

小明接到这样一个任务：有一个水缸有点漏水(而且漏水的速度还不一定固定不变)，要求水面高度维持在某个位置，一旦发现水面高度低于要求位置，就要往水缸里加水。

小明接到任务后就一直守在水缸旁边，时间长就觉得无聊，就跑到房里看小说了，每30分钟来检查一次水面高度。水漏得太快，每次小明来检查时，水都快漏完了，离要求的高度相差很远，小明改为每3分钟来检查一次，结果每次来水都没怎么漏，不需要加水，来得太频繁做的是无用功。

几次试验后，确定每10分钟来检查一次。这个检查时间就称为采样周期。 开始小明用瓢加水，水龙头离水缸有十几米的距离，经常要跑好几趟才加够水，于是小明又改为用桶加，一加就是一桶，跑的次数少了，加水的速度也快了。但好几次将缸给加溢出了，不小心弄湿了几次鞋。小明又动脑筋，我不用瓢也不用桶，老子用盆，几次下来，发现刚刚好，不用跑太多次，也不会让水溢出。

    这个加水工具的大小就称为比例系数P，瓢、桶、盆都相当于一个不同数值的P。但是P的量控制得不好，就会出现控制的误差，例如P值过大了，水会溢出，弄湿了鞋子，如果P值过小了，加水的时间就非常长，这样的调节是一个直线，所以准确达到要求。

小明又发现水虽然不会加过量溢出了，有时会高过要求位置比较多，还是有打湿鞋的危险。他又想了个办法，在水缸上装一个漏斗，每次加水不直接倒进水缸，而是倒进漏斗让它慢慢加。这样溢出的问题解决了，但加水的速度又慢了，有时还赶不上漏水的速度。于是他试着变换不同大小口径的漏斗来控制加水的速度，最后终于找到了满意的漏斗。漏斗的加水时间就称为积分时间。

这里的漏斗漏进水缸的水量为微分系数D，这样就控制了我们进水的量。

       小明终于喘了一口，但任务的要求突然严了，水位控制的及时性要求大大提高，一旦水位过低，必须立即将水加到要求位置，而且不能高出太多，否则不给工钱。小明又为难了！于是他又开努脑筋，终于让它想到一个办法，常放一盆备用水在旁边，一发现水位低了，不经过漏斗就是一盆水下去，这样及时性是保证了，但水位有时会高多了。他又在要求水面位置上面一点将水缸要求的水平面处凿一孔，再接一根管子到下面的备用桶里这样多出的水会从上面的孔里漏出来。这个水漏出的快慢就称为微分时间。

        这里的漏水量为微分系数I

九、P、PD、PI控制器

STM32智能平衡小车.txt (171 Bytes, 下载次数: 6)

2019-8-1 10:37 上传

点击文件名下载附件