第18届全国大学生智能汽车竞赛四轮车开源讲解【7】

开源讲解【7】"/>

第18届全国大学生智能汽车竞赛四轮车开源讲解【7】

开源汇总写在下面

第18届全国大学生智能汽车竞赛四轮车开源讲解_Joshua.X的博客-CSDN博客

一、元素识别基础

坡道，横断，断路这三个元素个人认为是非常重要的元素，而且他们在图像上还是很像的。

因为环岛写不好无非就是不入环，加时而已，不影响完赛。但是如果把坡道，横断，断路三者判断出错，那么比赛就完了。

我在比赛前特别担心车子将坡道和横断误判，因为两个都用到了测距，而且测距所占识别权重很大。一但误判，会在坡道前避开坡道，立刻出界；也有可能在横断前减速，径直撞向路障。

不过经过我的实测，我的这三份元素识别还是很稳定的。即使我在华南省赛（7月14日后）结束后两个多月（9月末），在实验室的新赛道上不改代码直接测试，元素仍然可以稳定识别，没有出现隔夜车的情况。

下面讲一下识别这几个元素的判断依据。

1.赛道宽度

我们将车模放置在直道上，记录从下倒上每一行赛道宽度。将这记录下来，写入单片机中，记录为标准赛宽。在后续的调车中，只要摄像头不变高度，角度，型号不变，那么赛道宽度就可以定死不动。（下面是我自己实测的数值，仅供参考）

const uint8 Standard_Road_Wide[MT9V03X_H]=//标准赛宽
{ 10, 12, 14, 16, 18, 20, 22, 24, 26, 28,30, 32, 34, 36, 38, 40, 42, 44, 46, 48,50, 52, 54, 56, 58, 60, 62, 64, 66, 68,70, 72, 74, 76, 78, 80, 82, 84, 86, 88,90, 92, 94, 96, 98,100,102,104,106,108,110,112,114,116,118,120,122,124,126,128,130,132,134,136,138,140,142,144,146,148};

存的时候还是注意一下坐标系。数组下标越小，代表摄像头远处的赛道宽度，那么对应的赛道宽度应该月窄；越近赛道宽度越宽。这样保证了后续使用时候数组下标的对应一致性。

2.边界起始点

我们在环岛过程中大量利用到了边界起始点，这里在说明一下。

边界起始点就是图像中第一个不丢线的点的行数。

左边的边界起始点和视野行重合，右边起始点位于图像最下一行。

上图中左边界起始点被“暗角”影响，边界起始点将永远在最下。右边界起始点在图像靠下部分，但是没有到最底下。

出现这种情况需要考虑现场光线问题，可手动将阈值在算法算出的结果上进行调整，确保“暗角”消失。或者前期将图片裁切，缩小，使用小图像可直接有效避免“暗角”出现。

3.tof测距

这几个模块的区别就在于坡道和横断有东西挡着，断路没东西挡着。

做好这一点，就可以将断路区分掉。

由于18届智能车规则规定：所有传感器不得配置MCU。传统的tof测距有的存在外置mcu处理数据，可以直接通过串口直接发送结果，速度可以非常快，甚至可以到300+Hz,但是现在全被禁止了。

我使用的是某飞家的tof模块，最快频率30Hz，也就是30ms一次。说实话，频率不够。

某宝上也有红外测距传感器，使用adc读取。距离越近，读取的adc值越高，我也没找到他的技术手册，那我就认为adc能读多快，他就可以输出多快。有兴趣的朋友可以试一试。

我下面提到的的判断方法不依靠高频率的测距，仍然具有很强的识别率，实测过程中无误判。

4.陀螺仪

陀螺仪主要是坡道的辅助判断，我通过一阶线性滤波计算俯仰角，这里不展开介绍，各位自行搜索。

纯靠陀螺仪也有问题，跑车时间过长，会有零飘积分。

我自己实测大概车模一分钟静置会有1度左右的积分误差，跑车的话由于晃动等原因，积分会多一些，所以跑一段时间需要复位一下车模，不然可能存在陀螺仪积分超过坡道阈值，车子就认为到坡道就减速，不判元素。

二、坡道

我们将这两张照片重叠如下

可以明显看到在图像下半部分几乎没什么区别，在上半部分，坡道独有的一个特征，两别不丢线，但是赛道宽度明显增加。

而且在这种情况下，测距就可以测到前面有东西。

2.1 坡道识别

格局前面提到的特征，我们坡道的判断就可以写出来了。

1. 元素互斥，当前不是十字，环岛，横断之类的元素。
2. 截止行很长，坡道后面即使接着弯道，视野也会因为坡道的凸起也会导致截止行很长。
3. 上半部分赛宽超宽。
4. 丢线少，起始点靠下（我没有用上，有需要的可以加上）
5. 测距测到前面有东西。

/*-------------------------------------------------------------------------------------------------------------------@brief     坡道检测@param     null@return    nullSample     Ramp_Detect();@note      赛宽变宽，测距前面有东西，丢线数少，截止行上面不丢线
-------------------------------------------------------------------------------------------------------------------*/
void Ramp_Detect(void)
{int i=0;int count=0;if(Cross_Flag!=0||Island_State!=0||Barricade_Flag!=0)//互斥{return;}if(Search_Stop_Line>=66)//截止行长{for(i=MT9V03X_H-1;i>MT9V03X_H-Search_Stop_Line;i--)//赛宽过长计数{if(Road_Wide[i]-Standard_Road_Wide[i]>10)//图像赛宽比标准赛宽大{count++;//赛宽过宽行}}}if(count>=10)//赛道过宽超过某一阈值{//是图像满足一定的条件再去使用测距dl1a_get_distance();//tof测距if(dl1a_distance_mm<500)//测距测到了前面有东西{if(Ramp_Flag==0)//之前是0状态，说明是刚进坡道{Ramp_Flag=1;}else if(Ramp_Flag==3)//之前是3状态，说明现在是正在下坡{Ramp_Flag=4;//进4之后积分出坡}}}if(Ramp_Flag!=0)//蜂鸣器提示坡道状态{gpio_set_level(BEEP_PIN, 1);}else{gpio_set_level(BEEP_PIN, 0);}//赛道超宽行计数，debug使用
//ips200_show_int(50,10*16,count,5);
}

当同时满足这几点时候，进入坡道1状态。

进入1状态后自动进入2状态，此时编码器开始积分（当然，定时器计时也可以）。积分距离根据实际情况来定。积分距离过长没有触发下一个标志位，直接结束坡道。

或者俯仰角过大，即使没有测距，没有图像，也进入环岛1状态。

//坡道处理if(Ramp_Flag!=0){if(Ramp_Flag==1)//进来先给2{//2300是1mRamp_Flag=2;count_50ms=0;encoder_accu=0;}else if(Ramp_Flag==2)//250ms滤波{count_50ms++;if(count_50ms>=5){Ramp_Flag=3;count_50ms=0;}}else if(Ramp_Flag==3)//3状态跑太久，强制出坡{encoder_accu+=Speed_Right_Real;if(encoder_accu>=2500){Ramp_Flag=0;encoder_accu=0;}}if(Ramp_Flag==4)//出坡后1s不再判坡{count_50ms++;if(count_50ms>=10){Ramp_Flag=0;count_50ms=0;encoder_accu=0;}}
//            ips200_show_int(50,10*16,Ramp_Flag,5);
//            ips200_show_int(50,11*16,encoder_accu,5);}

2进入3有个250ms的滤波，作为坡道的过度阶段，防止重复进1状态。

进入3状态后如果再次触发测距，那就认为是下坡激活的测距，使用编码器积分，或者计时若干时间，进行跳出。

2.2 坡道控制

坡道需要控制的主要是降速，同时将方向控制行向下移动，再或者切换电磁寻迹。

因为车模上坡后，视野改变，大概率会看到远处的东西，用远处的视野控制车是不合适的，很容易从坡道上掉下去。

当然最重要的一点，在坡道上面，关掉其他元素的判断入口。

三、横断

横断图像与直入断路几乎一模一样，只看图像完全无法分辨。

所以他们的图像识别可以写的一样。

3.1 横断识别

代码如下

/*-------------------------------------------------------------------------------------------------------------------@brief     横断检测@param     null@return    nullSample@note      截止行很低，边界起始点靠下，tof测到前面有东西
-------------------------------------------------------------------------------------------------------------------*/
void Barricade_Detect(void)
{if(Barricade_Flag!=0||Ramp_Flag!=0||Electromagnet_Flag!=0){//元素互斥return;}if(Boundry_Start_Left>=MT9V03X_H-10&&Boundry_Start_Right>=MT9V03X_H-10&&Left_Lost_Time<=10&&Right_Lost_Time<=10&&Search_Stop_Line<=60&&Left_Line[(MT9V03X_H-Search_Stop_Line)+1]>=10&&Right_Line[(MT9V03X_H-Search_Stop_Line)+1]<=(MT9V03X_W-10)&&//截止行往下不能丢线Left_Line[(MT9V03X_H-Search_Stop_Line)+2]>=10&&Right_Line[(MT9V03X_H-Search_Stop_Line)+2]<=(MT9V03X_W-10)&&Left_Line[(MT9V03X_H-Search_Stop_Line)+3]>=10&&Right_Line[(MT9V03X_H-Search_Stop_Line)+3]<=(MT9V03X_W-10)&&Left_Line[(MT9V03X_H-Search_Stop_Line)+4]>=10&&Right_Line[(MT9V03X_H-Search_Stop_Line)+4]<=(MT9V03X_W-10)){//边界起始点靠下，截止行不长，左右丢线都少，截止行往下不丢线dl1a_get_distance();//测距/if(dl1a_distance_mm<1000)//测距前面有东西，进避障模式{Barricade_Flag=1;}}
//    ips200_show_uint(5*16,100,black_count,4);
//    ips200_show_uint(5*16,120,dl1a_distance_mm,4);
}

1. 元素互斥，当前状态状态不是横断，坡道，电磁等
2. 边界起始点靠下
3. 左右丢线少
4. 搜索截止行小于60
5. 截止行下面不丢线（可以换成截止行下面几行赛宽与正常情况基本一致）
6. 测距前面有东西。

进入横断1状态。

3.2横断控制

横断总共分为4个阶段。

图像认识到横断，同时测得前面有东西，进入1.

进入1后将相关要用到的变量清零，自动进2（从时间来说，1只存在一瞬间）。

在2状态舵机打角（左或者右打死），同时定时器计时，或编码器积分，积分或者计时满了，进3。

3状态反向打角，打角时间需要比2状态长一些，同样积分或者计时。满了进4。.

4状态交由图像自动调整，同样还是积分或者计时，满了之后就结束横断，回归自由寻迹。

4状态非常重要，他可以弥补2/3状态写死带来的误差，只要不是离开赛道太远，自由寻迹都是可以寻回赛道的。

状态切换代码如下。

        //横断路障处理区if(Barricade_Flag!=0){if(Barricade_Flag==1){count_50ms=0;encoder_accu=0;//清干净，为后续积分准备Barricade_Flag=2;}else if(Barricade_Flag==2)//2状态向右打角，同时计时{count_50ms++;//计算时间if(count_50ms>=5)//打角计时和积分都可以，看自己选择{count_50ms=0;Barricade_Flag=3;}}else if(Barricade_Flag==3){count_50ms++;//encoder_accu+=Speed_Right_Real;if(count_50ms>=10){count_50ms=0;Barricade_Flag=4;}}else if(Barricade_Flag==4)//4自由寻迹归正{encoder_accu+=Speed_Right_Real;if(encoder_accu>=1500){encoder_accu=0;Barricade_Flag=0;}}
//       ips200_show_int(50,70+16*1,Barricade_Flag,5);
//       ips200_show_int(50,70+16*3,encoder_accu,5);}

在速度控制上只需要减速即可，方向上2/3状态强制打角，我同时将err的值也改了，因为err可以同时改变后轮差速。

方向控制代码如下，

if(Barricade_Flag!=0)//横断状态单独控制，这里强改舵机{if (Obstacle_Dir==0) //过横断方向可选{if(Barricade_Flag==2){Steer_Angle=RIGHT_MAX;//强制打角，强制给误差}else if(Barricade_Flag==3){Steer_Angle=LEFT_MAX;}}else{if(Barricade_Flag==2){Steer_Angle=LEFT_MAX;//强制打角，强制给误差}else if(Barricade_Flag==3){Steer_Angle=RIGHT_MAX;}}}

可以看到，只有2和3状态强制动了方向。1状态只有一瞬间，可以忽略不计。4状态没有写，说明是回归正常寻迹。

速度控制代码如下，整体来说减速就行。

else if(Barricade_Flag!=0)//路障速度，差速{if(Barricade_Flag==2){Speed_Left_Set =230-Err*0.2;Speed_Right_Set=230+Err*0.2;}else if(Barricade_Flag==3){Speed_Left_Set =Barricade_Speed-Err*0.2;Speed_Right_Set=Barricade_Speed+Err*0.2;}}

这里说一下我用的是计时，实际理论上上编码器积分更好，对于提速用编码器更好一点。

1. 沁恒的编码器有读数bug，使用编码器积分容易出问题。
2. 横断可以调整左通过或者右通过，这样需要对应左轮或者右轮积分，比较麻烦。
3. 机械不是很正，对于积分参数要求比较高。

我在横断2状态的速度是死的，3状态速度可调，为的就是更稳定的通过横断，防止车提速后参数需要修改。

说一下，因为我们场地原因，赛道有些地方距离墙面很近，所以会出现测距误判。

不过比赛场地是很标准的，赛道周围围挡与赛道之间会有1m的间隔，所以不必考虑测距误判。

四、断路

直入断路图像和横断一样，条件都差不多。

斜入断路需要特殊处理，不然很一定会出现问题。

4.1 断路识别

由于元素互斥的问题存在，所以横断图像判断条件松一点，因为他可以使用测距辅助判断。

断路条件写的就高一点。

4.1.1 直入断路

/*-------------------------------------------------------------------------------------------------------------------@brief     断路检测@param     null@return    nullSample     Break_Road_Detect()；@note      利用最长白列，边界起始点，中线起始点，符合要求后切换电磁寻迹
-------------------------------------------------------------------------------------------------------------------*/
void Break_Road_Detect(void)
{if(Barricade_Flag!=0||Ramp_Flag!=0)//横断时候要关掉，停车之后也要关掉{return;}static uint8 break_road_state=0;//直道进入断路，提前判断if(Search_Stop_Line<=45&&//边界起始点靠下，截止行不长，左右丢线都少Boundry_Start_Left>=MT9V03X_H-10&&Boundry_Start_Right>=MT9V03X_H-10&&//边界起始行低Left_Lost_Time<=10&&Right_Lost_Time<=10&&(Boundry_Start_Left +Left_Lost_Time ==MT9V03X_H-1)&&//丢线数+起始点==MT9V03X_H-1，说明丢的都是下面的点，上面没有丢点(Boundry_Start_Right+Right_Lost_Time==MT9V03X_H-1)&&//丢线数+起始点==MT9V03X_H-1，说明丢的都是下面的点Left_Line[(MT9V03X_H-Search_Stop_Line)+1]>=10&&Right_Line[(MT9V03X_H-Search_Stop_Line)+1]<=(MT9V03X_W-10)&&//截止行往下不能丢线Left_Line[(MT9V03X_H-Search_Stop_Line)+2]>=10&&Right_Line[(MT9V03X_H-Search_Stop_Line)+2]<=(MT9V03X_W-10)&&Left_Line[(MT9V03X_H-Search_Stop_Line)+3]>=10&&Right_Line[(MT9V03X_H-Search_Stop_Line)+3]<=(MT9V03X_W-10)&&Left_Line[(MT9V03X_H-Search_Stop_Line)+4]>=10&&Right_Line[(MT9V03X_H-Search_Stop_Line)+4]<=(MT9V03X_W-10)){break_road_state=1;  //图像丢失+电磁数据正常，进入断路模式Electromagnet_Flag=1;//切换电磁寻迹}//普通情况进断路if(Electromagnet_Flag==0&&break_road_state==0&&Img_Disappear_Flag==1){//目前状态是摄像头跑，断路状态是0，图像丢了break_road_state=1;  //图像丢失+电磁数据正常，进入断路模式Electromagnet_Flag=1;//切换电磁寻迹}//判断出断路if(break_road_state==1&&Search_Stop_Line>=50&&(Boundry_Start_Left>60||Boundry_Start_Right>60)&&(75<Longest_White_Column_Left[1]&&Longest_White_Column_Left[1]<105)){//在断路状态，搜索截止行高，边界起始点靠下,最长白列居中break_road_state=0;//截止行很长，边界起始点靠下，认为正常，换回摄像头Electromagnet_Flag=0;Img_Disappear_Flag=0;}//相关参数，debug使用
//    ips200_show_float(0,16*6,Electromagnet_Flag,5,3);
//    ips200_show_float(0,16*7,break_road_state,5,3);
//    ips200_show_float(0,16*8,Img_Disappear_Flag,5,3);
}

1. 截止行位于图像中间部分。

2. 边界起始点靠下。

3. 左右丢线少。

4. 丢线数+起始点==MT9V03X_H-1，结合3，说明允许丢线，但是只允许丢下面的线。

5. 搜索截止行下面几行不丢线（当前赛宽与标准赛宽差距不大也可以）。

6. 建议加上测距，测距数据很大，说明前面没有东西。这样就可以强制与横断分开

我也不知道为什么没给断路补一下测距，按道理补上测距会更好的区分断路。

4.1.2 斜入断路

由于断路可能存在于任何地方，直道断路可以用上述代码识别，但是斜入断路用上述代码一定会出问题。

因为在斜入断路时，车子会沿着弯道的尖角冲出去，然而他应该沿着中线走。

所以我们看到三角的时候，提前切到电磁，用电磁寻迹。

根据图像特征我们写出对应代码即可。

else if(Search_Stop_Line<=45&&//截止行比较短(abs(Longest_White_Column_Left[1]-Longest_White_Column_Right[1])<15)&&//左右最长白列位置近(Road_Wide[(MT9V03X_H-Search_Stop_Line)+1]<=30)&&(Road_Wide[(MT9V03X_H-Search_Stop_Line)+2]<=30)&&//截止行下面赛宽很小(Standard_Road_Wide[(MT9V03X_H-Search_Stop_Line)+2]-Road_Wide[(MT9V03X_H-Search_Stop_Line)+2])>10&&(Standard_Road_Wide[(MT9V03X_H-Search_Stop_Line)+1]-Road_Wide[(MT9V03X_H-Search_Stop_Line)+1])>10&&//最长白列下面几行赛道宽度比正常短(Boundry_Start_Left +Left_Lost_Time ==MT9V03X_H-1)&&//丢线数+起始点==MT9V03X_H-1，说明丢的都是下面的点，上面没有丢点(Boundry_Start_Right+Right_Lost_Time==MT9V03X_H-1)&&//丢线数+起始点==MT9V03X_H-1，说明丢的都是下面的点Left_Lost_Time<10&&Right_Lost_Time<=10)//丢线数少，保证不能像90度弯一样，延伸出去{Img_Disappear_Flag=1;return;}

断路只有两种情况：直线入断路和斜入断路，进到断路切换电磁寻迹即可。

4.2 断路控制

电磁控制，见下一章。

主要是电感数据差比和丢入PD，控制舵机。

4.3 跳出断路

 if(break_road_state==1&&Search_Stop_Line>=50&&(Boundry_Start_Left>60||Boundry_Start_Right>60)&&(75<Longest_White_Column_Left[1]&&Longest_White_Column_Left[1]<105)){//在断路状态，搜索截止行高，边界起始点靠下,最长白列居中break_road_state=0;//截止行很长，边界起始点靠下，认为正常，换回摄像头Electromagnet_Flag=0;Img_Disappear_Flag=0;}

跳出条件

1. 搜索截止行很高
2. 边界起始点靠下（有一个即可）
3. 最长白列居中

认为结束横断，从电磁切换摄像头。

五、出界保护

需要注意一点，停车的优先级最高，停车不与任何元素互斥（横断除外，横断一定得出界），保证只要出界，就刹车。防止车模乱撞，撞到人，撞到车。

1.阈值

正常跑车情况阈值会稳定在某一个范围，即使场地灯光不均匀。

当前元素不是断路的情况下，当阈值异常大，或者异常小的时候，认为车模出界，刹车保护。

2.电磁

正常跑车几个电感值之和也会在某一范围内，当电磁信号值异常小（需要确保当前元素不是横断），认为车模出界，刹车保护。

当然，我的电磁几乎没有前瞻，直接立在舵机上，如果有前瞻较长的车模，这有可能不适用，因为弯道时候电磁会伸出赛道，同样电感信号很小，会误触保护。

3.图像

纯图像识别出界也很暴力，因为车模都是在赛道上跑，赛道最下面几行不可能会出现一行的黑线，那我如果检测到一整行都是黑点，那么肯定出界了，保护就好了。

for(i= MT9V03X_H-1; i>MT9V03X_H-1-10; i--)//选定区域全黑，认为丢图了{if(image_two_value[i][4]==IMG_BLACK&&image_two_value[i][5]==IMG_BLACK&&image_two_value[i][10]==IMG_BLACK&&image_two_value[i][15]==IMG_BLACK&&image_two_value[i][20]==IMG_BLACK&&image_two_value[i][25]==IMG_BLACK&&image_two_value[i][64]==IMG_BLACK&&image_two_value[i][65]==IMG_BLACK&&image_two_value[i][66]==IMG_BLACK&&image_two_value[i][67]==IMG_BLACK&&image_two_value[i][68]==IMG_BLACK&&image_two_value[i][69]==IMG_BLACK&&image_two_value[i][70]==IMG_BLACK&&image_two_value[i][75]==IMG_BLACK&&image_two_value[i][66]==IMG_BLACK&&image_two_value[i][67]==IMG_BLACK&&image_two_value[i][80]==IMG_BLACK&&image_two_value[i][90]==IMG_BLACK&&image_two_value[i][100]==IMG_BLACK&&image_two_value[i][110]==IMG_BLACK&&image_two_value[i][115]==IMG_BLACK&&image_two_value[i][120]==IMG_BLACK&&image_two_value[i][125]==IMG_BLACK&&image_two_value[i][130]==IMG_BLACK&&image_two_value[i][135]==IMG_BLACK&&image_two_value[i][136]==IMG_BLACK&&image_two_value[i][137]==IMG_BLACK&&image_two_value[i][138]==IMG_BLACK)black_line_count++;}if(black_line_count>=5){Img_Disappear_Flag=1;}}

我的实际代码还有好多类似的保护，但是我也不知道当时为什么要写，我现在读起来感觉很奇怪。大家就做个参考就好，我的代码各位看个思路就行，没必要全盘照抄。

希望能够帮助到一些人。

本人菜鸡一只，各位大佬发现问题欢迎留言指出

qq：2296449414