十三届智能汽车竞赛 信标对抗组 思路参考 程序参考（五）分段控速

系列链接：

1. 十三届智能汽车竞赛 信标对抗组 思路参考 程序参考（一）模块初始化
2. 十三届智能汽车竞赛 信标对抗组 思路参考 程序参考（二）图像处理
3. 十三届智能汽车竞赛 信标对抗组 思路参考 程序参考（三）寻灯策略Part.①
4. 十三届智能汽车竞赛 信标对抗组 思路参考 程序参考（四）寻灯策略Part.②
5. 十三届智能汽车竞赛 信标对抗组 思路参考 程序参考（五）分段控速
6. 十三届智能汽车竞赛 信标对抗组 思路参考 程序参考（六）避障

正文开始：
这段时间科研任务和工作任务比较多，导致我很久没更新了哈。。。

今天继续更新这个系列。之前已经分享了模块初始化、图像处理和寻灯策略，这几部分都融会贯通的话，小车已经有能力完成整场比赛任务了，在此基础上将车速一步一步往上提是必然的事。然而实际跑下来可能还会发现一些问题：当车速提高之后，即使使用了前面分享的速度偏移曲线的方法，有时候小车的情况还是会变得不稳定，比如每次过灯的时候与灯的距离会不一样，有时离得远有时离得近，甚至有时候灭不了灯而有时候却撞到灯上，十分影响调车体验。

究其原因，有两点：一、由于信标灯发出的红外光是闪烁的，在红外光熄灭的间隙，我们当时通过一定长短的计时来确保小车不会因为红外光闪烁而进入无灯状态，换个角度看就算信标灯是真正的被熄灭，小车也需要经过一定长短的计时才能切换到无灯状态，进而执行无灯情况下的绕圈程序，问题就出在于此，虽然信标等闪烁的频率不低，且中间熄灭的间隙时间和我们设置的计时都很短，然而对于高速运动的小车来说，这极短的间隙内已经能够行驶不短的距离，而每一次过灯的时机不尽相同，因此导致有时候离得近有时候离得远；二是小车程序本身的循环速度不够快，导致每次循环中间的时间间隔过长，这不仅与单片机本身的主频性能有关，也与程序代码是否简洁有关。

• 对于第一点，我们当时采用的方式很直接，即靠近灯时减速，以提高过灯稳定性。对于是否靠近灯，则由灯在画面中的纵坐标决定，当纵坐标超过某个阈值时则进入近灯减速阶段，这个阈值不是一个固定值，我们将它与当前车速用一次函数挂钩，当前车速越快则这个阈值越提前，直观来讲就是当车速比较慢的时候可以晚一些进入近灯减速阶段晚一些减速，而当前车速较快的时候就需要早一些进入近灯减速阶段早一些减速了。且当车距离灯较远时，为了保持车身稳定，避免打滑漂移现象，我们将车速与舵机打角用高次函数挂钩，即舵机打正时能达到最大车速，舵机打角越大，减速越多。

if((x_MID>0)&&(y_MID<58))   ///看见有灯///
  {
    dianjiSHANSHUO=0;
    if(diansha==0) jiansuxian=42-(Speed_test/40);       //计算减速线的Y值，与当前速度有关

    if((diansha==1)&&(y_MID<jiansuxian)) diansha=0;     //下一个灯在同一个画面内时可以重新计算减速线

    if(y_MID>=jiansuxian)                      //当灯在减速线Y值以下
      {
        AmSpeed=130;                           //将速度降到130
        diansha=1;
      }
    else AmSpeed=(int)(130+ZUIDAsu*pow((abs(nicePWM)-100),6)/pow(100,5));   //灯在减速线Y值以上，车速与舵机打角挂钩。
                                                                            //ZUIDAsu表示设定的最大速度，定值
                                                                            //nicePWM是舵机打角，动态值
  }

• 对于靠近灯进行减速来保证稳定性的方法，其实是我们当时的一种妥协，毕竟减速当然能够提高稳定性，也许还有更好的解决方式，可能还有更好的点子，我的分享算是抛砖引玉，如果大家有更好的方式愿意分享的话，欢迎一起评论讨论。

• 灭灯之后或者没看到灯的时候就固定速度打转。

else           /无灯
  {
    dianjiSHANSHUO++;
    if(dianjiSHANSHUO>4)
      {
        diansha=0;
        if(dianjiSHANSHUO<140) AmSpeed=120;
        else AmSpeed=0;
      }
  }

• 做了这些处理，可以很大程度上提高优质完成任务的成功率了。不过我们还是留了后手，为了避免小车在某些时候正怼上信标灯卡住不动，在检测到电机堵转持续一定时间后执行倒车，把小车从卡住状态解救出来。

if((Speed_test<=50)&&(dianjiSHANSHUO<140)&&(huisu==1))              ///堵转倒车/
  {
    daoche++;
    if(daoche>=5)
      {
        AmSpeed=-120;
        daotou++;
        dianjiSHANSHUO=0;
      }
    if(daotou>=30)
      {
        daoche=0;
        daotou=0;
        huisu=0;
      }
  }
else if(Speed_test>60)
  {
    daoche=0;
    huisu=1;
  }

• 上面得到的都是目标车速，接下来就是老生常谈的电机PID了，经过我们实验，在小车电机上I控制效果可以忽略，使用PD控制就已经足够。别的博客写了很多关于PID控制的理解，这里就不赘述了，下面直接给出我们的电机PD控制实现。有一点不同的是，我们并没有把P参数设为定值，而是通过一次函数与舵机打角挂钩，与上面提到的车速与舵机打角挂钩的思想类似，也是为了避免小车不稳定，避免大转角时打滑漂移，直观的看就是舵机打角大的时候减小电机速度的控制强度，实现更稳定的行车效果。

void SpeedPID()                ///电机PID//
{
    SpeedDifference0 =AmSpeed-Speed_test;                       //目标速度与实际速度做差
    if(SpeedDifference0>=0) SpeedKP=11-(abs(nicePWM)>>5);       //P参数与舵机打角挂钩
    else SpeedKP=9;
    SpeedPWMKP =(int)(SpeedKP*SpeedDifference0);
    SpeedPWMKD =(int)(SpeedKD*(SpeedDifference0-SpeedDifference1));

    SpeedDifference1=SpeedDifference0;
    g_SpeedControlOutNew=SpeedPWMKP-SpeedPWMKD+Speed_test;
    
    //输出限幅//
    if(g_SpeedControlOutNew  >999)        g_SpeedControlOutNew= 999;
    else if(g_SpeedControlOutNew < -600)  g_SpeedControlOutNew=-600;
}

• 最后放上完整的电机控制函数

void dianji_PWM(void)  /电机控制///
{
  if((x_MID>0)&&(y_MID<58))   ///看见有灯///
    {
      dianjiSHANSHUO=0;
      if(diansha==0) jiansuxian=42-(Speed_test/40);   //计算减速线的Y值

      if((diansha==1)&&(y_MID<jiansuxian)) diansha=0;     //下一个灯在同一个画面内时可以重新计算减速线//

      if(y_MID>=jiansuxian)                      灯在减速线Y值以下
        {
          AmSpeed=130;
          diansha=1;
        }
      else AmSpeed=(int)(130+ZUIDAsu*pow((abs(nicePWM)-100),6)/pow(100,5));//灯在减速线Y值以上
    }
  else           ///无灯
    {
      dianjiSHANSHUO++;
      if(dianjiSHANSHUO>4)
        {
          diansha=0;
          if(dianjiSHANSHUO<140) AmSpeed=120;
          else AmSpeed=0;
        }
    }
  if((Speed_test<=50)&&(dianjiSHANSHUO<140)&&(huisu==1))     堵转倒车/
    {
      daoche++;
      if(daoche>=5)
        {
          AmSpeed=-120;
          daotou++;
          dianjiSHANSHUO=0;
        }
      if(daotou>=30)
        {
          daoche=0;
          daotou=0;
          huisu=0;
        }
    }
  else if(Speed_test>60)
    {
      daoche=0;
      huisu=1;
    }
  SpeedPID(); 
}


void SpeedPID()电机PID/
{
    SpeedDifference0 =AmSpeed-Speed_test;//目标速度与实际速度做差
    if(SpeedDifference0>=0) SpeedKP=11-(abs(nicePWM)>>5);
    else SpeedKP=9;
    SpeedPWMKP =(int)(SpeedKP*SpeedDifference0);
    SpeedPWMKD =(int)(SpeedKD*(SpeedDifference0-SpeedDifference1));

    SpeedDifference1=SpeedDifference0;
    g_SpeedControlOutNew=SpeedPWMKP-SpeedPWMKD+Speed_test;
    
    //输出限幅//
      if(g_SpeedControlOutNew  >999)        g_SpeedControlOutNew= 999;
      else if(g_SpeedControlOutNew < -600)  g_SpeedControlOutNew=-600;
}

从这一系列第一篇一直看下来的同学们肯定发现了，我们将很多变量互相挂钩，且挂钩关系使用了一次函数、二次函数和更高次的函数等等，这些函数的产生方式在第三篇寻灯策略Part.①里面也有部分分享，也就是实验、取值、记录、拟合、测试、换拟合函数、再测试，拟合是使用MATLAB的函数拟合工具箱完成的。整个调车过程中我们尝试了许多类型的函数，如高斯函数、傅里叶函数等，最终综合考虑运算速度和实现效果平衡，还是选用幂次函数就足够了。

我看到现在新一届的智能汽车竞赛组委会推荐使用的芯片越来越强大，速度越来越快，还有英飞凌的多核芯片，覆盖的芯片品牌和厂商范围越来越广，可以说智能汽车竞赛越来越与时俱进，一如既往的引领自动化及相关专业大学生的创新潮流，这无疑是一件好事。下一篇我打算分享红外避障部分，不出意外的话下一篇应该就是这一系列的结尾了，希望多多少少能给做智能车竞赛的新老同学们一些帮助，也欢迎同学们评论讨论。