devbox
AllinToyou
这个屌丝很懒，什么也没留下！
关注作者
热门标签
热门文章
当前位置:   article > 正文

关于十八届智能车摄像头组控制算法一些经验和感悟_csdn智能车摄像头组

作者：AllinToyou | 2024-04-21 15:04:31

赞

踩

csdn智能车摄像头组

目录

前言

常见的控制PID

C车方向环

C车速度环

F车方向环

F车速度环

F车串环控制

F车角速度串环控制

偏差获取——电磁

元素处理及速度决策

写在后面

前言

随着十八届智能车落下帷幕，本人参加十八届智能车摄像头组，在队伍中担任主控。虽然比赛仅拿下省一。因为车电驱的问题无缘国赛。但回想这半年一路走来都是宝贵的经历，趁着快开学前整理下这半年来的调车经验，以及自己对于车控制算法的一些理解。

常见的控制PID

智能车中的PID作用对象因车而异。像C车、F车的处理思路明显不一样。C车拥有两个电机一个舵机，作用对象分开来，舵机执行方向环，电机执行速度环，两环基本独立。甚至前期C车可以只靠方向环执行转向，后面电机执行开环处理给固定占空比。而F车只有两个电机，前方万向轮仅起支撑以及减震作用。所以对于F车来说，车身的速度环和转向环就共同作用在一个对象上了。不得不考虑两个环之间的干扰。下面针对C车、F车两种车模来谈谈控制思路。

C车方向环

对C车来说最重要首先要调好的就是方向环，PID环作为一个闭环负反馈（相关知识参考《自动控制原理》），其误差获取为电感进行差比和计算得到车与赛道中线的偏差。将此偏差作为PID输入对象。闭环负反馈系统反馈的就是这个偏差。因为PID的目的就是要不断的迭代上一次结果，将这个输入偏差不断减小直至达到为0的理想状况。换句话说就是让车能够在赛道中间行驶。而PID这三个参数无非是在调整整个闭环负反馈系统的一个响应曲线。使其达到快、准、稳。

C车模由于其舵机为惯性较大，没有积分部件。所以通常采用位置式PID进行控制，即PD控制。其中P项负责舵机的响应程度。即P项越大，舵机矫正摆动幅度越大，响应越快。但注意P项虽然越大响应越快，但是大到一定程度便会震荡，其实就是系统已经太灵敏了，这时候一点点极其微小的误差都会被放大很多倍，所以引起震荡。此时需要引入D项来抑制震荡。因为D项作为上次误差与上上次误差的系数，D项除了可以抑制震荡还可以起到超前预判的作用。加大D项可以使车提前拐弯，但是D项过大也会引起系统震荡。

在调试C车方向环时,直接可以先给后车轮电机固定占空比大概2000左右,使车可以跑动。然后方向环先给定P项。慢慢加大P项使车可以拐弯。当P项加到车在直道上开始有摇摆震荡的趋势时，这时候P项就已经差不多了。可以引入D项来抑制震荡。当车在过弯时能达到既不内切又不外切时，这个速度匹配下的方向环PD参数就已经调好了。为什么说这个速度匹配下？因为实际上不同速度下车身实际PID响应曲线都不一样。速度慢时，误差更新速度变化慢（属个人理解，实际误差获取间隔取决于你的方向环定时器时间。这里这样解释只是方便与高速情况下作对比），此时PD参数要求不高。而速度处于高速速下时，这是误差的更新的变化就会十分大了。假设你的车速为2m/s，方向环放在20ms定时器中断里。20ms时间内，你的车走过的路程就已经达到4cm了。这对于电磁前瞻还是摄像头来说尤其是弯道处更新的偏差就已经十分大了。此时PD参数的要求更高，尤其对于P来说需要微调，小数点级别的加减。同样是调好P项后再调D项抑制震荡。本人调试时经验方向环D项大概是P项的10倍~20倍之间。

┗|｀O′|┛ 嗷~~对了。C车的舵机一定要限幅，防止打脚过大但机械结构卡位会烧坏舵机，下面代码中的限幅只提供参考，不建议直接接使用，因为舵机防止的机械位置或高或低都会影响限幅区间。

下面是C车方向环参考代码：

  1. //转向PD控制//电感差比和算出的偏差做位置式PD计算
  2.    My_Direction_SumError=My_Direction_KP*My_Direction_NowError+ My_Direction_KD*(My_Direction_NowError-My_Direction_lastError);
  3.    My_Direction_lastError=My_Direction_NowError;
  4.    
  5. 	 //中心偏差滤波、加权滑动平均滤波
  6. 	 My_Direction_Pre1_Error[3]= My_Direction_Pre1_Error[2];
  7. 	 My_Direction_Pre1_Error[2]= My_Direction_Pre1_Error[1];
  8. 	 My_Direction_Pre1_Error[1]= My_Direction_Pre1_Error[0];
  9. 	 My_Direction_Pre1_Error[0]= My_Direction_SumError;
  10. 	 My_Direction_Direct_Parameter=My_Direction_Pre1_Error[0]*0.8+My_Direction_Pre1_Error[1]*0.1+My_Direction_Pre1_Error[2]*0.06+My_Direction_Pre1_Error[3]*0.04 ;
  11.    
  12. 	 //此处Price_PWM就是控制量，即：控制器输出的控制量		注:这里的控制量在电机函数中会分别加减到两个电机的PWM占空比上，实现内外轮速度差，进而使小车转向
  13.    Price_PWM=(int16)(My_Direction_Direct_Parameter);//将控制量强制转化为整形
  14.  
  15. 	 steering=(1080-Price_PWM);//转向值计算公式,舵机0度为1080,0度值减去控制器输出的控制量，1080为舵机理论中值(经过多次试错，中值与舵机防止位置，高度息息相关)
  16. 	  
  17. 	if(steering<=980)
  18. 	{
  19. 	  steering=980;//限幅，防止舵机右打角过度，舵机被烧坏
  20. 	}
  21. 	if(steering>=1180)
  22. 	{
  23. 	  steering=1180;//限幅，防止舵机左打角过度，舵机被烧坏
  24. 	}
  25. 	 pwm_duty(PWMB_CH1_P74, steering);//PWMB_CH1通道打开，引脚P74(控制舵机)，占空比为steering
  26. }

C车速度环

速度环的目的是使得电机速度可以快速达到期望速度。在后续速度决策中车身变速有着十分重要的作用。由于电机要尽快达到期望速度，所以需要引入积分项，速度环通常使用增量式PI控制。P项的作用是提高响应速度，I项负责消除稳态误差。一般速度环要求响应快，可放在5ms定时器中。

在调试速度环时，先给定P项，然后给定一个很小很小（小数级别）的I项。然后逐渐加大P项，加到能看到电机响应达到一个快速的趋势。这是在逐渐加大I项，使得速度达到期望速度。调试方法是先将车身速度打印在TFT屏上。给定一个中等速度，观察车轮实际速度与理想速度之间的偏差，一般这个偏差值能维持在3以内，速度环基本上就已经调好了。之后通过改变期望速度，发现在不同速度下电机都能快速响应且误差较小。那么这套PI参数就已经调好了。

关于I的具体理解可以采用以下方法，本人觉得十分有效。关掉方向环，将电机期望速度设置为0。车模放在地上。向前推车，在松手后发现车模可以回到原来地方。一般越能回到原先位置，越能表示速度环参数匹配。加大I项带来的效果是会发现车会变得很死，越来越难推动，阻力变强，抑制车身位置变化（实际上就是在减小PID输入误差）。但是一定要注意积分限幅，不然容易造成电机失控疯转（轮胎都能给你甩飞掉）。

速度环对于F车来说最为重要。调好的速度环电机转起来应该是安静的，而且跑下来电机也不会发烫，如果电机噪声特别强烈，车跑完电机发烫严重。多半是电机速度环参数不匹配导致电机容易正反转发烫。对于速度环来说，最好一个电机对应一个速度环，总共两个速度环，一个调好了同理复制即可。也可以两个电机一个速度环。适合前期跑车要求不高，后期提速不建议使用，最好还是一个环一个作用对象。

下面是C车速度环参考代码：

  1. void Speedcontrol_L(void)
  2. {   //固定PI控制
  3.     //启动时加速
  4.     CarSpeed_L.err_new = (float)(Goal+Price_PWM) - Speed_L;//期望速度减去当前整体车速获取误差
  5.     CarSpeed_L.P = CarSpeed_L.kp*( CarSpeed_L.err_new-CarSpeed_L.err_old );
  6.     CarSpeed_L.I = CarSpeed_L.ki*CarSpeed_L.err_new;
  7.  
  8.              //I 限制幅值
  9.             if(CarSpeed_L.I > 200)
  10.                 CarSpeed_L.I = 0;
  11.  
  12.             else if(CarSpeed_L.I<-200)
  13.                 CarSpeed_L.I = 0;
  14.  
  15.     CarSpeed_L.D = CarSpeed_L.kd*(CarSpeed_L.err_new - 2*CarSpeed_L.err_old + CarSpeed_L.err_pre_old );
  16.     //速度误差，当前，上次，上上次
  17.     CarSpeed_L.err_pre_old = CarSpeed_L.err_old;
  18.     CarSpeed_L.err_old     = CarSpeed_L.err_new;
  19.     //整体PID控制
  20.     CarSpeed_L.speed_out_new = CarSpeed_L.P + CarSpeed_L.I + CarSpeed_L.D;
  21.  
  22.     //系数转化 待定
  23.     Car_L_Speed_out = Last_L_Speed_out+ CarSpeed_L.speed_out_new;
  24.     Last_L_Speed_out  = Car_L_Speed_out;
  25.  
  26.  
  27.     //速度最终输出限幅。若速度环pid调节未及时达到稳态会导致误差不断积累,超出100%占空比时导致程序直接报错
  28.     //此处进行限幅，当误差积累到100%,设定误差输出为0,速度环输出为零,避免调节量过大烧坏电驱
  29.     //按下复位键即可
  30.     if(Car_L_Speed_out > 10000)
  31.         Car_L_Speed_out = 10000;
  32.     if(Car_L_Speed_out < -10000)
  33.         Car_L_Speed_out = -10000;
  34.  
  35. }

F车方向环

F车刚开始的处理思路也可以是电机给定固定占空比，方向环在此占空比上进行加减。此种方式为并环处理。结构简单，但并环带来的结果是两个环作用在一个对象上难免会产生干扰。至于后果稍后速度环会继续说。之后也会说下另一种控制思路，串环PID。

方向环的偏差输入为摄像头获取的赛道中线与理论中线之间的偏差。其同样是PD方向环增量式PID，与C车方向环处理并无二致。鉴于上文C车方向环是使用电感作为误差输入，此处F车方向环以摄像头误差作为输入参考。同理C车摄像头方向环，F车电感方向环处理也是如此。

下面是F车方向环参考代码：

  1. void DirectionConrtol(void)
  2. {
  3.     CarDirection.err_new = piancha(30, 40);
  4.  
  5.     CarDirection.control_out_new = CarDirection.kp * CarDirection.err_new + CarDirection.kd * (CarDirection.err_new-CarDirection.err_old);
  6.     CarDirection.err_old=CarDirection.err_new;
  7.  
  8.     //方向环输出限幅
  9.     if(CarDirection.control_out_new>CarDirection.control_out_max)
  10.         CarDirection.control_out_new=CarDirection.control_out_max;
  11.     else if(CarDirection.control_out_new<-CarDirection.control_out_max)
  12.         CarDirection.control_out_new=-CarDirection.control_out_max;
  13.  
  14.     //此处为系数转化,根据实际情况调节后面乘于一个系数  可加可不加 看PID调节情况，建议加上 把数据都扩大倍数变成int型方便调节
  15.     //Car_Direction_out = CarDirection.control_out_new/10;
  16.     Car_Direction_out = CarDirection.control_out_new*10;//*5
  17.  
  18.     if(Car_Direction_out>5000)
  19.     {
  20.         Car_Direction_out = 5000;
  21.     }else if(Car_Direction_out<-5000)
  22.     {
  23.         Car_Direction_out = -5000;
  24.     }
  25. }

F车速度环

如果控制思路为并环处理，两个电机的输出分别为速度环输出加减方向环输出。此种方法简单。速度环可以沿用C车速度环的思路。来简单说下并环、串环区别（详细知识参考《自动控制原理》《计算机控制技术》）：并环PID和串环PID是两种常见的控制策略，用于处理反馈控制系统中的控制问题。它们的主要区别在于控制回路的结构和运算顺序。

并环PID控制器中，将多个PID控制器直接并联起来。每个PID控制器接收同样的输入信号，但具有不同的增益参数。然后将它们的输出信号相加，作为最终的控制信号。并环PID控制器适用于需要在不同状态下使用不同控制参数的系统，例如在不同的工作范围内，可以使用不同的PID参数。

串环PID控制器中，将多个PID控制器连接成一个串行的结构。该结构中的每个PID控制器依次处理输入信号，并将输出信号传递给下一个PID控制器。通常，串环PID控制器用于需要多级控制的系统，其中每个PID控制器都可以根据系统动态的不同需求进行参数调整。

总结来说，两者的区别在于并环PID控制器是并行结构，适用于需要在不同状态下使用不同控制参数的系统；而串环PID控制器是串行结构，适用于需要多级控制的系统。具体选择哪种结构取决于系统的需求和控制目标。

下面详细说下F车并环调试方法：可以先使车速度为0，调试方向环参数。即让两电机直接获取正负方向环输出。将车放在长直道上，此时由电感或者摄像头获取偏差均可。将车左右摆动，观察车身是否能快速回到赛道中线。P参数影响车响应，使车身回到赛道的力度变大。D项若过小车身会开始震荡。之后推着F车从直道向弯道走。观察车身是内切还是外切来调整D项，待方向环调整好后。关掉方向环，调试速度环，调试方法和C车速度环一样。两环分别调好后都打开进行并环加减。给车一定速度观察车身运动轨迹再去调试方向环，此时速度环就不要再动了。因为F车方向环速度环执行机构都是两个电机。所以不同速度下对于方向环的影响更大。所以后期处理方法即为分段PID，即不同速度区间下给定不同的方向环PD参数匹配。再之后可以考虑模糊PID。个人理解：模糊PID可以看成是分段PID的PLUS版，可以说将分段分的更细，匹配到各种情况。

以上的并环思路适合前期调车使用。毕竟两个环作用在一个执行机构上总有干扰。用这种控制方法最容易出现的问题就是车身出赛道后由于方向环和速度环输出都是一个数量级（都是PWM占空比）。由于方向环此时误差已经很大，车身会疯狂原地打转。而且电机声音异常刺耳。这对于车身机械结构和电机寿命都是损伤。所以高速不建议此控制思路。

下面是F车方向环并速度环输出参考代码（速度环参考C车）：

  1. void PwmfeedBackContrl(void)
  2. {
  3.     PWM_RightValue  = (int16)Car_R_Speed_out;//车PWM指定值
  4.     PWM_LeftValue  = (int16)Car_L_Speed_out;
  5.  
  6.     //此处即为 方向环与速度环 的叠加 此处的int转化可有可无，只需要把方向环的PID输出结果 弄成int 效果一样
  7.     //L.pwm = -((int16)PWM_PiontValue + (int)(Car_Direction_out));//使用图像方向环,前向跑时
  8.     //R.pwm = -((int16)PWM_PiontValue - (int)(Car_Direction_out));
  9.  
  10.     //L.pwm = -((int16)PWM_PiontValue - (int)(Car_Direction_out));//使用图像方向环
  11.     //R.pwm = -((int16)PWM_PiontValue + (int)(Car_Direction_out));//左右轮方向调整后
  12.  
  13.     L.pwm = (int16)PWM_LeftValue;// + (int)(Price_PWM);//(int)(Car_Direction_out);
  14.     R.pwm = (int16)PWM_RightValue;// - (int)(Price_PWM);//(int)(Car_Direction_out);
  15.  
  16.  
  17.     //L.pwm = (int)(Price_PWM);//仅电感方向环,测试方向环调参使用
  18.     //R.pwm = -(int)(Price_PWM);
  19.  
  20.     //L.pwm = -((int)(Car_Direction_out));//仅图像方向环,测试方向环调参使用
  21.     //R.pwm = -((int)(Car_Direction_out));
  22.  
  23.     //L.pwm = -(int16)PWM_PiontValue;//仅速度环,测试速度环调参使用
  24.     //R.pwm = -(int16)PWM_PiontValue;
  25.  
  26.     //L.pwm = 2000;//固定输出20%PWM,测试电驱用
  27.     //R.pwm = 2000;
  28.  
  29.     if(L.pwm > 7000)//电机最终输出限幅,最大占空比设置为正负50%,可更改，软件限幅，防止烧坏电机
  30.     {
  31.         L.pwm = 7000;
  32.     }
  33.     if(L.pwm < -7000)
  34.     {
  35.         L.pwm = -7000;
  36.     }
  37.     if(R.pwm > 7000)
  38.     {
  39.         R.pwm = 7000;
  40.     }
  41.     if(R.pwm < -7000)
  42.     {
  43.         R.pwm = -7000;
  44.     }
  45.  
  46.     //注意,因为采用负方向参考系,此处实际为右轮输出
  47.     if(L.pwm<0)
  48.     {
  49.         L.pwm=-L.pwm;
  50.         gpio_set_level(P02_6,1);//正转，实际为负向坐标系右轮
  51.         pwm_set_duty(ATOM0_CH7_P02_7, L.pwm);//右轮PWM输出
  52.     }
  53.     else
  54.     {
  55.         gpio_set_level(P02_6,0);//反转
  56.         pwm_set_duty(ATOM0_CH7_P02_7, L.pwm);
  57.     }
  58.  
  59.     if(R.pwm<0)
  60.     {
  61.         R.pwm=-R.pwm;
  62.         gpio_set_level(P02_4,1);
  63.         pwm_set_duty(ATOM0_CH5_P02_5, R.pwm);
  64.     }
  65.     else
  66.     {
  67.         gpio_set_level(P02_4,0);
  68.         pwm_set_duty(ATOM0_CH5_P02_5, R.pwm);
  69.     }
  70.  
  71. }

F车串环控制

串环控制最大的特点就是一个环的输出往往作为另一个环的输入，最终再作用至执行机构。执行机构仅接收一个输出结果，大大避免的不同环之间的干扰。F车使用串环控制能明显感觉到车声控制更加丝滑一点，电机噪音更小。

本串环控制思路采用速度环包方向环处理思想，即方向环作为内环，速度环作为外环。速度环周期为5ms精调，方向环周期为20ms，最终控制效果较好。具体方法为：将设定的期望速度与方向环的输出相加减作为速度环的输入进行控制。一定要注意，此时方向环的输出应该与期望速度为一个数量级且需要限幅。最终速度环的输出为PWM占空比。

调试方法为先调好速度环，因为速度环作为外环直接影响输出结果。所以使用串环思路必须先调好速度环，不同于并环可以先调方向环。串环需要外环速度环调好后才能调内环方向环。先调试外环时，可以将方向环关掉，速度环的输入只给期望速度与当前车身采集速度的差值。之后的调试方法和C车速度环调试方法一样。再调好速度环后开始调方向环。这时将期望速度设置为0。速度的输入就是方向环的输出与编码器采集的当前车身速度的差值。因为方向环输出与期望速度是一个数量级，所以此时方向环的输出也可以看成是对两个电机各自的期望速度。即车身左偏就左轮需要正转，右轮需要反转使车身回正减小方向环输入误差值。然后调试方向环PD值。直至手动偏离赛道后车身可以快速回正且不震荡。此时0速度下的方向环就调好了。一定注意不要手推车往前调整方向环。因为外环为速度环是开着的，手推车向前会有阻力，松手后车会回到原位置。所以之后要更改期望速度调试不同速度下的方向环PD参数。建议速度可以20区间的往上加，以便调出从停车——低速——中速——高速不同速度下的方向环PD参数。这样做的好处是方便之后分段PID进行不同期望速度下的方向环参数匹配。

以上的串环控制适合配合后期加速调车使用。不同于之前F车并环处理方式，串环最明显的效果就是只要外环速度环精调好了，车的电机噪音会很小而且方向环处理好后车身拐弯也会更丝滑，不会出现并环拐弯容易漂移的现象。同时就算车身出赛道后，由于方向环输出与速度期望为一数量级不是达到了占空比的数量级，所以车不会疯转，一般是继续直行下去。不用担心车乱跑乱撞。

下面是是F车方向环速度环串环处理参考代码：

  1. void SteerPID(float offset) {//方向环PD位置式
  2.   //当前误差，定义为寄存器变量，只能用于整型和字符型变量，提高运算速度
  3.   float iError,  //当前误差
  4.       SteerErr;  //
  5.   int PWM;
  6.  
  7.       iError = offset;  //计算当前误差
  8.  
  9.   SteerErr =
  10.       SteerPIDdata.P * iError +
  11.       SteerPIDdata.D * (iError - SteerPIDdata.LastError);  //位置式PID算式
  12.   SteerPIDdata.LastError = iError;                         //更新上次误差
  13.   if((Barrier_Flag == 0) && (Dianci_Flag == 0))//横断不用图像环，开环处理
  14.   {
  15.       Speed_Goal_l= Speed_Goal + SteerErr;   //speedgoal 和 steererr 中间的符号转弯差速反了更改
  16.       Speed_Goal_r= Speed_Goal - SteerErr;
  17.   }else if(Dianci_Flag == 1)//断路标志为1，使用电磁方向环不用摄像头方向环
  18.   {
  19.       Speed_Goal_l= Speed_Goal + Price_PWM;   //方向环为电磁方向环
  20.       Speed_Goal_r= Speed_Goal - Price_PWM;
  21.   }
  22. }
  23.  
  24.  
  25. void speed_PIDL(void)//速度环PI增量式
  26. {
  27.     if(ramp_flag == 0 && (Barrier_Flag==0 || Barrier_Flag == 7))//横断采用开环处理,所以此部分不需要速度环作用
  28.     {
  29.         Speed_Encoder_l  = speed1;//编码器采集当前速度
  30.         Speed_Erro_l     =  Speed_Goal_l - Speed_Encoder_l;   //编码器反馈目标值与当前的差值
  31.         Speed_PID_OUT_l += (Speed_P_l * (Speed_Erro_l - Speed_Lasterro_l)  +
  32.                       Speed_I_l *  Speed_Erro_l  +
  33.                       Speed_D_l * (Speed_Erro_l - 2 * Speed_Lasterro_l + Speed_Preverro_l));
  34.         if(Speed_PID_OUT_l<-9500)//此处电机输出pwm软件限幅.f车模电机额定电压8v限幅pwm防止烧坏电机
  35.             Speed_PID_OUT_l = -9500;
  36.         if(Speed_PID_OUT_l>9500)
  37.             Speed_PID_OUT_l = 9500;
  38.         Speed_Preverro_l =  Speed_Lasterro_l;
  39.         Speed_Lasterro_l =  Speed_Erro_l;
  40.     }
  41.     if(Speed_PID_OUT_l>=0)
  42.     {
  43.         gpio_set_level(P02_6,0);//正转，实际为负向坐标系右轮
  44.         pwm_set_duty(ATOM0_CH7_P02_7, Speed_PID_OUT_l);//右轮PWM输出
  45.     }
  46.     else
  47.     {
  48.         gpio_set_level(P02_6,1);//反转
  49.         pwm_set_duty(ATOM0_CH7_P02_7, -Speed_PID_OUT_l);
  50.     }
  51. }

F车角速度串环控制

关于F车串环控制还有一种思路是方向环与角速度环的串环结果再并上速度环。添加角速度环的好处是在身身质量大高速过弯时，由于惯性往往会出现甩尾。而引入陀螺仪角速度环后可以抑制这种甩尾，使车拐弯更流畅。此种方法调试好的话车模运行效果也很好。但是参数比较难调，所以此方法本人调试不多，仅提供一些参考价值。而且角速度环方向环未调试好电机噪音会异常刺耳，发烫严重。

关于角速度环的调试：关掉速度环输出，给期望速度为0，将车模左右摆动，调节PD参数直到车在摆动时有阻力，逐渐变得很死，基本很难挪动就差不多了。

下面是方向环串角速度环串环处理参考代码：

  1. void AngularSpeed(void)
  2. {
  3.     static float real_Bais=0,real_last_bias=0;
  4.     float gyro_z,real_gyro_z;
  5.  
  6.     //imu660ra_get_acc();
  7.     imu660ra_get_gyro();
  8.     //gyro_z=imu660ra_gyro_z/2000;            //实际角速度提取
  9.     gyro_z=imu660ra_gyro_z/200;
  10.  
  11. /*
  12.  
  13.     CarDirection.err_new = piancha(30, 40);
  14.     CarDirection.control_out_new = CarDirection.kp * CarDirection.err_new + CarDirection.kd * (CarDirection.err_new-CarDirection.err_old);
  15.     CarDirection.err_old=CarDirection.err_new;
  16.  
  17.     //方向环输出限幅
  18.     if(CarDirection.control_out_new>CarDirection.control_out_max)
  19.         CarDirection.control_out_new=CarDirection.control_out_max;
  20.     else if(CarDirection.control_out_new<-CarDirection.control_out_max)
  21.         CarDirection.control_out_new=-CarDirection.control_out_max;
  22.  
  23.     Car_Direction_out = CarDirection.control_out_new*5;      //期望角速度计算
  24.  
  25. */
  26.  
  27.     real_gyro_z=gyro_z*Gyro_kp;   //实际角速度计算
  28.  
  29.     //real_Bais=(-Car_Direction_out)-real_gyro_z;//方向环串角速度环
  30.     real_Bais=0-real_gyro_z;
  31.     Turn=Real_kp*real_Bais+Real_kd*(real_Bais-real_last_bias);
  32.     real_last_bias=real_Bais;
  33.  
  34. }

偏差获取——电磁

之所以叫做PID闭环控制就是因为要是整个系统闭环形成反馈，而反馈就是偏差量，让系统不断矫正。这里以电磁处理为例，由于本人不负责摄像头的识别工作，所以关于摄像头的偏差处理可以参考别的文章。

由于我们组仅在断路元素利用电磁方向环，所以电磁处理比较简单。利用左中右三个电感足以完成正常的电磁循迹。电感在采集滤波后经过差比和计算公式便可以获取到偏差。在调试时可以将各个电感值打印在TFT屏幕上将车放在赛道直道上观察电感值，其中中间电感值应为最大，左右两边电感值为对称。若不符合可以通过调节运放来改变电感值大小。

下面是电磁偏差获取参考代码：

  1. void adcinit()
  2. {
  3.     adc_init(ADC0_CH0_A0, ADC_12BIT);//运放模块1
  4. //    //system_delay_ms(50);
  5. //    adc_init(ADC0_CH1_A1, ADC_12BIT);
  6.     //system_delay_ms(50);
  7. //    adc_init(ADC0_CH2_A2, ADC_12BIT);
  8.     //system_delay_ms(50);
  9. //    adc_init(ADC0_CH3_A3, ADC_12BIT);
  10. //    //system_delay_ms(50);
  11.  
  12.     adc_init(ADC0_CH4_A4, ADC_12BIT);//运放模块2
  13.     //system_delay_ms(50);
  14.     adc_init(ADC0_CH5_A5, ADC_12BIT);
  15. //    //system_delay_ms(50);
  16. //    adc_init(ADC0_CH6_A6, ADC_12BIT);
  17. //    //system_delay_ms(50);
  18. //    adc_init(ADC0_CH7_A7, ADC_12BIT);
  19. //    //system_delay_ms(50);
  20. }
  21.  
  22.  
  23. void adccollect()
  24. {
  25.     Li = adc_mean_filter_convert(ADC0_CH0_A0, 10);//ADC0_CH0_A0
  26.     Mi = adc_mean_filter_convert(ADC0_CH5_A5, 10);//ADC0_CH5_A5
  27.     Ri = adc_mean_filter_convert(ADC0_CH4_A4, 10);//ADC0_CH3_A3     //实际电磁循迹左中右3个电感足矣
  28. //    Ki = adc_mean_filter_convert(ADC0_CH4_A4, 10);//ADC0_CH1_A1
  29. //    Ji = adc_mean_filter_convert(ADC0_CH1_A1, 10);//ADC0_CH5_A5
  30.  
  31.     adcsum=(Li+Ri+Mi)/3;//磁场强度.辅助入环判断
  32. }
  33.  
  34.  
  35. void Adc_Conduct(void)
  36. {
  37.     My_Direction_NowError=50*(Ri-Li)/(Li+Mi+Ri);         //差比和计算偏差,My_Direction.NowError是计算出偏差量，50为系数
  38.     if(Ri>Li)
  39.     {
  40.         if(My_Direction_NowError<0)
  41.         {
  42.             My_Direction_NowError=-My_Direction_NowError;//防止越界，两端过大，趋于无穷，正误差过大，变成负值
  43.         }
  44.     }
  45.     if(Ri<Li)
  46.     {
  47.         if(My_Direction_NowError>0)
  48.         {
  49.             My_Direction_NowError=-My_Direction_NowError;
  50.         }
  51.     }
  52. }

元素处理及速度决策

18届摄像头组总的元素来说可以包括：直道、S弯、十字、圆环、横断、断路、坡道、车库。

对于直道、S弯来说，其实就是方向环PID参数调节的好坏。直到对于P项没有那么敏感，期望速度可以给很快。这里提供一种直道加速策略：如果摄像头或者电感获取到的偏差绝对值小于3，且其余元素标志位为0未触发。那么速度可以一直++到一个期望较高的速度，偏差大于了就降速--。这算是一种最简单的加速策略，像其他的还有方差计算，将方向环输入偏差与速度环期望速度构成函数关系，采取动态速度规划等等。

十字元素对于电磁来说基本不用处理。因为电感垂直不切割磁感线，所以几乎没影响。对于摄像头来说，如果摄像头图像赛道两边丢边且其余元素标志位为0未触发（因为各个元素都是相互独立的，所以要用好标志位判断）。那么就是到十字了。然后根据丢边断点摄像头补线正常循迹就可以了。

圆环这里我们组的思路是利用电感辅助。其中具体摄像头识别稍后再说，此处仅说处理，进入圆环后开启陀螺仪积分，当陀螺仪积分360度一圈后固定出环即可。

横断我是进行开环处理，利用状态机的思想。闭环也可以。开环情况下当TOF激光测距到一定距离，先关掉方向环，然后进入第一状。左右轮给固定占空比一正一负，左轮正转右轮反转陀螺仪积分到一定角度进入第二状态。左右轮给相同占空比，此时编码器积分到一定距离进入第三状态。左右轮给固定占空比一负一正，左轮反转右轮正转陀螺仪积分到一定角度进入第四状态。左右轮给相同占空比，此时编码器积分到一定距离进入第五状态。左右轮给固定占空比一正一负，左轮正转右轮反转陀螺仪积分到一定角度进入第六状态。这时理论上车身已经绕过障碍物回到了赛道，再打开方向环速度环就可以了。

处理思路参考下图：

 下面是横断元素开环处理参考代码：

  1.     dl1b_get_distance();
  2.     if(dl1b_finsh_flag == 1)//dl1a_finsh_flag
  3.     {
  4.         dl1b_finsh_flag = 0;
  5.         if(dl1b_distance_mm < 500)//600
  6.         {
  7.             Tof_Flag = 1;
  8.         }
  9.         else if(600 < dl1b_distance_mm < 1200)
  10.         {
  11.             Tof_Flag = 2;
  12.         }
  13.         else
  14.         {
  15.             Tof_Flag = 0;
  16.         }
  17.     }
  18. /   横断处理   ///
  19.     if(Barrier_Flag == 1)//陀螺仪角度积分
  20.     {
  21.         static i = 0;
  22.         i++;
  23.         if(i > 30)           //刹车时间（10*10ms）
  24.         {
  25.             i = 0;
  26.             Barrier_Flag = 2;
  27.         }
  28.         Speed_PID_OUT_r = -2000;//-2000
  29.         Speed_PID_OUT_l = -2000;
  30.     }else if(Barrier_Flag == 2)
  31.     {
  32.         XZ_Angle();
  33.         hengduanjiaodu_flag1 = Angle_z;
  34.         Speed_PID_OUT_r = 2000;//2000
  35.         Speed_PID_OUT_l = -2000;//-2000
  36.         if(hengduanjiaodu_flag1 > 300)//>300
  37.         {
  38.             Barrier_Flag = 3;
  39.             Speed_PID_OUT_r = 2000;
  40.             Speed_PID_OUT_l = 2000;
  41.             hengduanjiaodu_flag1 = 0;
  42.             Angle_z = 0;
  43.         }
  44.     }else if(Barrier_Flag == 4)
  45.     {
  46.         XZ_Angle();
  47.         hengduanjiaodu_flag2 = Angle_z;
  48.         Speed_PID_OUT_r = -2000;//-2000
  49.         Speed_PID_OUT_l = 2000;//2000
  50.         if(hengduanjiaodu_flag2 < -700)//<-700
  51.         {
  52.             Barrier_Flag = 5;
  53.             Speed_PID_OUT_r = 2000;
  54.             Speed_PID_OUT_l = 2000;
  55.             hengduanjiaodu_flag2 = 0;
  56.             Angle_z = 0;
  57.         }
  58.     }else if(Barrier_Flag == 6)
  59.     {
  60.         XZ_Angle();
  61.         hengduanjiaodu_flag3 = Angle_z;
  62.         Speed_PID_OUT_r = 2000;//2000
  63.         Speed_PID_OUT_l = -2000;//-2000
  64.         if(hengduanjiaodu_flag3 > 300)//>300
  65.         {
  66.             Barrier_Flag = 7;
  67.             Speed_PID_OUT_r = 0;
  68.             Speed_PID_OUT_l = 0;
  69.             hengduanjiaodu_flag3 = 0;
  70.             Angle_z = 0;
  71.             Barrier_Flag = 0;
  72.             Barrier_off = 1;
  73.         }
  74.     }
  75.  
  76. /   横断处理   ///
  77.             if(Barrier_Flag == 3)//编码器距离积分
  78.             {
  79.                 hengduanjvli_flag1 += speed1;
  80.                 if(hengduanjvli_flag1 > 5000)//5000
  81.                 {
  82.                     Barrier_Flag = 4;
  83.                     hengduanjvli_flag1 = 0;
  84.                 }
  85.             }else if(Barrier_Flag == 5)
  86.             {
  87.                 hengduanjvli_flag2 += speed1;
  88.                 if(hengduanjvli_flag2 > 7000)//5000//3000
  89.                 {
  90.                     Barrier_Flag = 6;
  91.                     hengduanjvli_flag2 = 0;
  92.                 }
  93.             }
  94. /   横断处理   ///
  95. }

一定注意，陀螺仪角度积分要求较高，要单独放在一个10ms中断中。

横断元素也可以闭环处理，即给定方向环特定的误差输出然后执行状态机的思想。这样处理更加丝滑，但代价就是误差难以给定调试较困难。开环给定固定占空比的好处是调试方便，但是代价就是容错率较低，因为开环没有反馈，赛道脏了轮胎滑了就会转多或者转少执行失败。

断路元素电磁不用处理，至于摄像头的话可以通过赛道白点数来判断。

坡道一般不用处理，处理的话就是陀螺仪检测到俯仰角发生变化上坡减速避免飞坡（当然飞坡很帅，不过车摔下来就比较惨了）。

车库的话，我们出库入库都是利用补线处理。

具体关于圆环，横断、断路、车库的摄像头识别可以参考我队友的这篇文章：智能车摄像头组的一些经验和感悟分享

写在后面

以上就是关于这半年智能车之旅经历的一些整理了，暂时也没有想到更多。总的来说智能车是一次难忘的经历。同时也预祝下一届的车友们取得优异成绩。

声明：本文内容由网友自发贡献，不代表【wpsshop博客】立场，版权归原作者所有，本站不承担相应法律责任。如您发现有侵权的内容，请联系我们。转载请注明出处：https://www.wpsshop.cn/w/AllinToyou/article/detail/463657
推荐阅读
相关标签

Copyright © 2003-2013 www.wpsshop.cn 版权所有，并保留所有权利。

  

闽ICP备14008679号