阅读量:1
开源汇总写在下面
第18届全国大学生智能汽车竞赛四轮车开源讲解_Joshua.X的博客-CSDN博客
开源链接写在下面
https://gitee.com/joshua_xu/the-18th-smartcar
https://gitee.com/joshua_xu/the-18th-smartcar
写在前面
环岛可以说是折磨广大车友的老元素了，本人也是深受其害。调车前期在找环岛特征点；中期在优化识别，减少误判；后期在调整参数，调整控制，优化路径。
不过很遗憾，最后赛场上我还是在环岛上出现了问题，导致未能完赛。下面我将我所有环岛经验，心得分享给大家，希望给大家带来一些灵感和启发。
注：以下方案有可能会用到一个或者多个下述变量，以下变量均在开源【3】边线提取一章有讲。
const uint8 Standard_Road_Wide[MT9V03X_H];//标准赛宽数组 volatile int Left_Line[MT9V03X_H]; //左边线数组 volatile int Right_Line[MT9V03X_H];//右边线数组 volatile int Mid_Line[MT9V03X_H]; //中线数组 volatile int Road_Wide[MT9V03X_H]; //实际赛宽数组 volatile int White_Column[MT9V03X_W];//每列白列长度 volatile int Search_Stop_Line; //搜索截止行,只记录长度，想要坐标需要用视野高度减去该值 volatile int Boundry_Start_Left; //左右边界起始点 volatile int Boundry_Start_Right; //第一个非丢线点,常规边界起始点 volatile int Left_Lost_Time; //边界丢线数 volatile int Right_Lost_Time; volatile int Both_Lost_Time;//两边同时丢线数 int Longest_White_Column_Left[2]; //最长白列,[0]是最长白列的长度，也就是Search_Stop_Line搜索截止行，[1】是第某列 int Longest_White_Column_Right[2];//最长白列,[0]是最长白列的长度，也就是Search_Stop_Line搜索截止行，[1】是第某列 int Left_Lost_Flag[MT9V03X_H] ; //左丢线数组，丢线置1，没丢线置0 int Right_Lost_Flag[MT9V03X_H]; //右丢线数组，丢线置1，没丢线置0文章中所有参数，角点范围之类的东西仅作为参考。实际参数，需要实际调整！！！！！！！！！
实际上，智能车所有参数都需要根据你的实际情况进行调整，万万不可照搬不误！！！！！
一、环岛特征识别
环岛识别需要的点有以下几种。
1.角点
角点也叫拐点，和前文十字我们使用到的角点是一个东西。
判别方法也是一模一样，利用边线误差过大突然过大，我称之为“边线撕裂”。
典型图像如下：
参考代码如下：
/*------------------------------------------------------------------------------------------------------------------- @brief 右下角点检测 @param 起始点，终止点 @return 返回角点所在的行数，找不到返回0 Sample Find_Right_Down_Point(int start,int end); @note 角点检测阈值可根据实际值更改 -------------------------------------------------------------------------------------------------------------------*/ int Find_Right_Down_Point(int start,int end)//找四个角点，返回值是角点所在的行数 { int i,t; int right_down_line=0; if(Right_Lost_Time>=0.9*MT9V03X_H)//大部分都丢线，没有拐点判断的意义 return right_down_line; if(start<end) { t=start; start=end; end=t; } if(start>=MT9V03X_H-1-5)//下面5行数据不稳定，不能作为边界点来判断，舍弃 start=MT9V03X_H-1-5; if(end<=MT9V03X_H-Search_Stop_Line) end=MT9V03X_H-Search_Stop_Line; if(end<=5) end=5; for(i=start;i>=end;i--) { if(right_down_line==0&&//只找第一个符合条件的点 abs(Right_Line[i]-Right_Line[i+1])<=5&&//角点的阈值可以更改 abs(Right_Line[i+1]-Right_Line[i+2])<=5&& abs(Right_Line[i+2]-Right_Line[i+3])<=5&& (Right_Line[i]-Right_Line[i-2])<=-5&& (Right_Line[i]-Right_Line[i-3])<=-10&& (Right_Line[i]-Right_Line[i-4])<=-10) { right_down_line=i;//获取行数即可 break; } } return right_down_line; }2.边界连续性
参考下图，左边界是近乎一条直线，相邻任意两行边界的差值都很小，我们称之为“连续”的。
右边线因为角点等多种原因，边线是“撕裂的”，我称之为“边界不连续”，也就是边线断掉了。
判断方法很简单，甚至比角点还简单，只要判断上下两行之间边界撕裂情况，当大于某个阈值时，就认为边界不连续。
（角点一定是不连续，不连续不一定是角点，角点的判断的条件深度更深，建立在不连续的基础上）
典型图形如下：
参考代码如下：
/*------------------------------------------------------------------------------------------------------------------- @brief 右赛道连续性检测 @param 起始点，终止点 @return 连续返回0，不连续返回断线出行数 Sample continuity_change_flag=Continuity_Change_Right(int start,int end) @note 连续性的阈值设置为5，可更改 -------------------------------------------------------------------------------------------------------------------*/ int Continuity_Change_Right(int start,int end) { int i; int t; int continuity_change_flag=0; if(Right_Lost_Time>=0.9*MT9V03X_H)//大部分都丢线，没必要判断了 return 1; if(start>=MT9V03X_H-5)//数组越界保护 start=MT9V03X_H-5; if(end<=5) end=5; if(start<end)//都是从下往上计算的，反了就互换一下 { t=start; start=end; end=t; } for(i=start;i>=end;i--) { if(abs(Right_Line[i]-Right_Line[i-1])>=5)//连续性阈值是5，可更改 { continuity_change_flag=i; break; } } return continuity_change_flag; }3.单调性改变
常规赛道都是单调递增/单调递减的，
然而在环岛的某些状态，会看到弧，会看到其他的角，这会有单调性转转变的特点。
先看一下环岛需要判断的单调转折出现在哪些地方。
首先是刚入环岛的这个圆弧，常规来判断其实这是不太好处判断的。
我们将圆弧放大来看，可以看到右边界在向左，向上伸展，经过了圆弧处，反过来向右向上伸展。这就是单调性的转折，边界的数值在先变小，再变大。
这是即将要出环的状态，我们将那个尖角放大来看。
这里明显出现了一个尖角，但他还没有变成角点，因为撕裂不够大，相邻的两行边界差距不够大。
但是边界延伸方向却变了，从向右伸展，变成突然向左伸展，这也是单调性的改变。
我就根据单调性突变，抓住边界连续几个点先向左伸展，再向右伸展（先向右伸展，再向左伸展），这个特点写了一个单调转折点判断函数。原理很暴力，但是用来抓这几个点，效果很好。
/*------------------------------------------------------------------------------------------------------------------- @brief 单调性突变检测 @param 起始点，终止行 @return 点所在的行数，找不到返回0 Sample Find_Right_Up_Point(int start,int end); @note 前5后5它最大（最小），那他就是角点 -------------------------------------------------------------------------------------------------------------------*/ int Monotonicity_Change_Right(int start,int end)//单调性改变，返回值是单调性改变点所在的行数 { int i; int monotonicity_change_line=0; if(Right_Lost_Time>=0.9*MT9V03X_H)//大部分都丢线，没有单调性判断的意义 return monotonicity_change_line; if(start>=MT9V03X_H-1-5)//数组越界保护 start=MT9V03X_H-1-5; if(end<=5) end=5; if(start<=end) return monotonicity_change_line; for(i=start;i>=end;i--)//会读取前5后5数据，所以前面对输入范围有要求 { if(Right_Line[i]==Right_Line[i+5]&&Right_Line[i]==Right_Line[i-5]&& Right_Line[i]==Right_Line[i+4]&&Right_Line[i]==Right_Line[i-4]&& Right_Line[i]==Right_Line[i+3]&&Right_Line[i]==Right_Line[i-3]&& Right_Line[i]==Right_Line[i+2]&&Right_Line[i]==Right_Line[i-2]&& Right_Line[i]==Right_Line[i+1]&&Right_Line[i]==Right_Line[i-1]) {//一堆数据一样，显然不能作为单调转折点 continue; } else if(Right_Line[i] <Right_Line[i+5]&&Right_Line[i] <Right_Line[i-5]&& Right_Line[i] <Right_Line[i+4]&&Right_Line[i] <Right_Line[i-4]&& Right_Line[i]<=Right_Line[i+3]&&Right_Line[i]<=Right_Line[i-3]&& Right_Line[i]<=Right_Line[i+2]&&Right_Line[i]<=Right_Line[i-2]&& Right_Line[i]<=Right_Line[i+1]&&Right_Line[i]<=Right_Line[i-1]) {//就很暴力，这个数据是在前5，后5中最大的，那就是单调突变点 monotonicity_change_line=i; break; } } return monotonicity_change_line; }当然也可以将条件写的更加具体一点，比如对于转折点上下都需要有单调性要求，这个就看各位的识别要求了。
4.斜率补边界线
这是环岛中的一种补线方法，有3个参数。补线斜率，开始补线行，终止补线行。
/*------------------------------------------------------------------------------------------------------------------- @brief 通过斜率，定点补线-- @param k 输入斜率 startY 输入起始点纵坐标 endY 结束点纵坐标 @return null Sample K_Add_Boundry_Left(float k,int startY,int endY); @note 补得线直接贴在边线上 -------------------------------------------------------------------------------------------------------------------*/ void K_Add_Boundry_Left(float k,int startX,int startY,int endY) { int i,t; if(startY>=MT9V03X_H-1) startY=MT9V03X_H-1; else if(startY<=0) startY=0; if(endY>=MT9V03X_H-1) endY=MT9V03X_H-1; else if(endY<=0) endY=0; if(startY<endY)//--操作，start需要大 { t=startY; startY=endY; endY=t; } //这里有bug，下方循环--循环，需要start要大，只进行y的互换，但是没有进行x的互换 //建议进行判断，如果start更小，那就进行++访问 //这里修改各位自行操作 for(i=startY;i>=endY;i--) { Left_Line[i]=(int)((i-startY)/k+startX);//(y-y1)=k(x-x1)变形，x=(y-y1)/k+x1 if(Left_Line[i]>=MT9V03X_W-1) { Left_Line[i]=MT9V03X_W-1; } else if(Left_Line[i]<=0) { Left_Line[i]=0; } } }5.斜率画线
这和上面的斜率补线一个意思，上面补的是边线，这里补的是实线，直接将像素点涂黑。
参数需要斜率，起始点横纵坐标，结束行。
/*------------------------------------------------------------------------------------------------------------------- @brief 根据斜率划线 @param 输入斜率，定点，画一条黑线 @return null Sample K_Draw_Line(k, 20,MT9V03X_H-1 ,0) @note 补的就是一条线，需要重新扫线 -------------------------------------------------------------------------------------------------------------------*/ void K_Draw_Line(float k, int startX, int startY,int endY) { int endX=0; if(startX>=MT9V03X_W-1)//限幅处理 startX=MT9V03X_W-1; else if(startX<=0) startX=0; if(startY>=MT9V03X_H-1) startY=MT9V03X_H-1; else if(startY<=0) startY=0; if(endY>=MT9V03X_H-1) endY=MT9V03X_H-1; else if(endY<=0) endY=0; endX=(int)((endY-startY)/k+startX);//(y-y1)=k(x-x1)变形，x=(y-y1)/k+x1 Draw_Line(startX,startY,endX,endY); } /*------------------------------------------------------------------------------------------------------------------- @brief 画线 @param 输入起始点，终点坐标，补一条宽度为2的黑线 @return null Sample Draw_Line(0, 0,MT9V03X_W-1,MT9V03X_H-1); Draw_Line(MT9V03X_W-1, 0,0,MT9V03X_H-1); 画一个大× @note 补的就是一条线，需要重新扫线 -------------------------------------------------------------------------------------------------------------------*/ void Draw_Line(int startX, int startY, int endX, int endY) { int i,x,y; int start=0,end=0; if(startX>=MT9V03X_W-1)//限幅处理 startX=MT9V03X_W-1; else if(startX<=0) startX=0; if(startY>=MT9V03X_H-1) startY=MT9V03X_H-1; else if(startY<=0) startY=0; if(endX>=MT9V03X_W-1) endX=MT9V03X_W-1; else if(endX<=0) endX=0; if(endY>=MT9V03X_H-1) endY=MT9V03X_H-1; else if(endY<=0) endY=0; if(startX==endX)//一条竖线 { if (startY > endY)//互换 { start=endY; end=startY; } for (i = start; i <= end; i++) { if(i<=1) i=1; image_two_value[i][startX]=IMG_BLACK; image_two_value[i-1][startX]=IMG_BLACK; } } else if(startY == endY)//补一条横线 { if (startX > endX)//互换 { start=endX; end=startX; } for (i = start; i <= end; i++) { if(startY<=1) startY=1; image_two_value[startY][i]=IMG_BLACK; image_two_value[startY-1][i]=IMG_BLACK; } } else //上面两个是水平，竖直特殊情况，下面是常见情况 { if(startY>endY)//起始点矫正 { start=endY; end=startY; } else { start=startY; end=endY; } for (i = start; i <= end; i++)//纵向补线，保证每一行都有黑点 { x =(int)(startX+(endX-startX)*(i-startY)/(endY-startY));//两点式变形 if(x>=MT9V03X_W-1) x=MT9V03X_W-1; else if (x<=1) x=1; image_two_value[i][x] = IMG_BLACK; image_two_value[i][x-1] = IMG_BLACK; } if(startX>endX) { start=endX; end=startX; } else { start=startX; end=endX; } for (i = start; i <= end; i++)//横向补线，保证每一列都有黑点 { y =(int)(startY+(endY-startY)*(i-startX)/(endX-startX));//两点式变形 if(y>=MT9V03X_H-1) y=MT9V03X_H-1; else if (y<=0) y=0; image_two_value[y][i] = IMG_BLACK; } } }6.辅助传感器
理论上来说，依靠纯摄像头完全可以进行环岛元素的判断，我也实现过。但是现场环境千变万化，没人知道你的车子会看到什么，会判断什么。为了提高车模的稳定性，我建议在比赛规则允许的情况下使用上一些辅助的传感器，如陀螺仪，电磁来进行辅助。可以显著提高车子的稳定性。
6.1陀螺仪
因为环岛是个圆，从车环岛入环开据积分角度始积分，这样就即使只看陀螺仪数据也大概确定车模处于环岛什么状态。因为陀螺仪和图像是独立的。图像会受一些影响，进入到错误的环岛状态。这时候就可以利用陀螺仪，当角度到达某个范围时，再开启下一个阶段的判断，这样可以有效防止误判。
陀螺仪处理有很多办法，四元数，卡尔曼滤波，一节线性滤波都可以得到想要的数据。这里就不展开介绍。
6.2电磁
电磁线在环岛处有一个独一无二的特点，就是会在圆和直线的切线处会电磁线会重合。这样会造成这个点的电感值是赛道其他地方的两倍，可以抓住这一特点，作为入环的判断，或者入环的确定。
二、环岛阶段划分
我是将环岛划分到了9个阶段，其实现在想一想划分的有点太多了，有些划分的不是很合理。
我先介绍一下我当前代码中的写法，然后再介绍一下个人感觉比较好的做法。
以右环为例，左环是一样的道理。
1状态
1.判断条件
- 左边连续，右边不连续
- 左边单调，右边不单调
- 左边丢线少，右边丢线多，双边丢线少
- 搜索截止行很远
- 边界起始点很靠下
- 右下角存在角点
- 角点向上存在单调突变点
2.连线情况
- 将单调突变点向下连线
此时我们已经得到了上单调突变点坐标，计算他距离右边界的距离为L1。
我们令L2=L1*k，系数k我这里取得0.15。然后从在图像最下面一行取右边界向左移动L2距离，两点连线。（k是随便取的，只要能让补线位置合适即可，没有特别要求）
例如：
我的上图单调点坐标（100，10），摄像头图像尺寸180*70
那么L1=180-100=80，L2=80*0.15=12
L2点坐标就是（180-12，69）
那么我们将（100，10），（168，69）两点进行连线。得到了状态1的补线。
3.跳出条件
当右下角的那块角点消失，也就是当右边界起始点变高，就进入2状态。
2状态
1.判断条件
- 在1状态下边界起始点变高，也就是右下角点消失。
2.连线情况
- 将单调突变点向下连线
连线原则和状态1一样，这样就做到了边线位置与角点无关，只与单调点位置有关，即使角点判断失误，仍可以流畅前进。
状态1之所以判断角点，一方面为了1状态环岛的确认，防止误判。
另一方面找到角点坐标，向上开始寻找单调突变点，防止将角点误判为单调突变点。
2.2.1半边补线法
在2状态时，以右环为例，直接让中线=左边线+（标准长直道赛道宽度/2），右边线不用考虑，反正中线都有了，右边线无所谓。
赛道宽度是在标准长直道测得每一行的道宽度（赛宽=右边线-左边线），自行测量，不同车不一样。
赛道宽度是在标准长直道测得每一行的道宽度（赛宽=右边线-左边线），自行测量，不同车不一样。
赛道宽度是在标准长直道测得每一行的道宽度（赛宽=右边线-左边线），自行测量，不同车不一样。
3.跳出条件
我用过好几个版本的跳出条件，各有利弊，这里只供参考。
- 在2状态下，电磁信号激增，进入3状态。
- 在2状态下，单调突变点纵坐标靠下，或者横坐标靠左，进入3状态。
- 在2状态下，边界起始点靠下，说明大圆弧来到了视野下方，进入3状态。
else if(Island_State==2)//2状态下方丢线，上方即将出现大弧线 { monotonicity_change_line[0]=Monotonicity_Change_Right(70,5);//单调性改变 monotonicity_change_line[1]=Right_Line[monotonicity_change_line[0]]; Right_Add_Line((int)(MT9V03X_W-1-(monotonicity_change_line[1]*0.15)),MT9V03X_H-1,monotonicity_change_line[1],monotonicity_change_line[0]); // if(Island_State==2&&(Boundry_Start_Right>=MT9V03X_H-10))//右下角再不丢线进3 if(Island_State==2&&(Boundry_Start_Right>=MT9V03X_H-5||monotonicity_change_line[0]>50))//右下角再不丢线进3 { Island_State=3;//下方丢线，说明大弧线已经下来了 } }3状态
1.判断条件
- 在2状态下，电磁信号激增，进入3状态。
- 在2状态下，单调突变点纵坐标靠下，或者横坐标靠左，进入3状态。
- 在2状态下，边界起始点靠下，说明大圆弧来到了视野下方，进入3状态。
上个状态的跳出条件就是下个状态的进入条件。
2.连线情况
环岛3状态其实是个人认为比较麻烦的状态，他的稳定与否直接决定了你入环的情况，我就是因为这个状态没有处理好，赛场上环岛出现了问题。
3状态主要是找右上角点，可以一直找，找到了且在给定区域中，再连线。因为有时候环比较大，圆弧阶段比较长，角点会稍微等一下才会看到。
关于角点判断在环岛3状态还有个问题，是最长白列扫线自身的bug。
这是环岛3的初期，此时最长白列在左侧，是可以扫到右侧的这个角点，可以连线角点和左下点。
当角点靠下时，（因为角点判断在图最上最下5行不进行判断）认为可以停止补线。
但是，出现了这样的问题。
当角点靠下后，停止补线。由于最长白列仍然位于左边，右边线与我们期望位置不符，他会沿着当前的边线得到的误差，向着直道方向跑去。
这种情况有一个解法，进入环岛3状态，强制将最长白列锁定在右侧，
这样的话，右上角点就会变成左上角点，找到角点后，进行连线操作即可。
待角点靠下或者靠左，停止补线，仍锁定最长白列，让他继续寻找边界，就可以了。
这种情况有一点风险，就是图像有可能会“跳帧”。就比如可能上一秒图像的角点还在上方，下一秒由于卡顿，或者其他情况，角点突然靠下，结果车子没有识别到这个角点，补线就会乱补线，产生问题。
下面我介绍一下我最后使用的办法，效果其实也不是很好。
补线，这条线既是“动”的，也是“定”的。
动：以每次角点出现的位置为准，每次的位置可能不一样。
定：在3状态时，线是不动的。
首先在环岛3状态仍然限制最长白列范围。主要是限制最长白列的搜索起始列。
我左下定点坐标（20,69），那么最长白列搜索起始行就从25或者30开始。不然的话边线还是会混乱。
然后还是正常的找到右上角点，当角点出现在划定的区域内，定点和角点间的斜率lk。
开始画线。
这次画的是一条实线，就是将图像上的像素点涂黑，不是将某点记录为边界。
线画完了之后，重新扫线，也就是说在环岛3状态需要扫两次边界。
画线的目的是围城一块区域，保证边线一定在这个范围内。
画的这条线就会位置固定起来，由于这是一条死线，（不是实时抓角点的原因也是怕跳帧）在车的运行中，不一定会完美贴合角点，但是一般情况下都会在角点附近，不太影响车模运行。
这样的话，车子就可以沿着这条线和角点围城的域得区到的边界，进入环岛。
当然需要注意的是这条线的密度要够高，不能出现这种情况
线画漏了，最长白列从中穿过去了，这样右边界就会找错。
画死线的目的是圈定范围，但这样就和没画线一样。
我最后的故障点就是这条线画了，边线数据更新了，但是车子不往环岛里面进，至今也想不清为什么。
else if(Island_State==3)//下面已经出现大弧线，且上方出现角点 { if(k!=0)//已经找到点了，画一条死线 { K_Draw_Line(k,30,MT9V03X_H-1,0); Longest_White_Column();//刷新最长白列 } else//还在找点 { Right_Up_Guai[0]=Find_Right_Up_Point(40,10);//找右上拐点 Right_Up_Guai[1]=Right_Line[Right_Up_Guai[0]]; if(Right_Up_Guai[0]<10)//角点位置不对，退出环岛 { Island_State=0; Right_Island_Flag=0; } if(k==0&&(15<=Right_Up_Guai[0]&&Right_Up_Guai[0]<50)&&(70<Right_Up_Guai[1]&&Right_Up_Guai[1]<150)) {//当角点出现在给定范围内 island_state_3_up[0]= Right_Up_Guai[0]; island_state_3_up[1]= Right_Up_Guai[1]; k=(float)((float)(MT9V03X_H-island_state_3_up[0])/(float)(20-island_state_3_up[1])); K_Draw_Line(k,30,MT9V03X_H-1,0); Longest_White_Column();//刷新赛道数据 } } if((Island_State==3)&&(abs(FJ_Angle)>=60))//只依靠陀螺仪积分 { k=0;//斜率清零 Island_State=4; Longest_White_Column();//最长白列 } }3.2.1半边补线法
进到3状态直利用圆弧半边补线。
赛道宽度是在标准长直道测得每一行的道宽度（赛宽=右边线-左边线），自行测量，不同车不一样。
赛道宽度是在标准长直道测得每一行的道宽度（赛宽=右边线-左边线），自行测量，不同车不一样。
赛道宽度是在标准长直道测得每一行的道宽度（赛宽=右边线-左边线），自行测量，不同车不一样。
3.跳出条件
3状态也有很多跳出方法，各位看情况使用。
- 进入1状态时陀螺仪候开始积分，积满60度，自动跳到4。
- 进入1状态时编码器开始积分，积分一定距离（注意大小环区别），自动跳到4。
- 当右上角点比较靠下，或者比较靠左，进入状态4。
注：有很多车友发现在环岛3状态无法找到角点，这里提供两个修改点，在camera.c中，34状态时，最长白列寻找范围我进行了修改。各位可以看一下自己的图像，适当的调整最长搜索白列范围。
在camera.c最长白列巡线的最后，我为了强制入环，把环岛34状态时左右边界线强制写死了，这样边界线数组就是固定值，不存在撕裂情况，这样也会找不到边线。目前（2024年5月4日）我已将gitee库进行了更新，删掉了边线固定。
在debug时候，大家可以将边界线打在屏幕上看一下，是什么原因导致的，最常见的原因就是角点范围，或者撕裂阈值不符，这些都是需要根据实际情况来进行调整的。
4状态
1.判断条件
3状态满足条件后，自动进入4状态。
在环岛中运行都是4状态。
2.连线情况
最后跳出4状态时才开始补线。
这个状态的连线情况比较特殊。他既是“死的”，又是“活的”，和环岛3状态一致，这补的是边界线。
当转折点第一次出现在规定区域内，记录转折点坐标，还有屏幕最下方边界坐标，计算出当前斜率k。然后使用下面的斜率补线函数，补一条线。
void K_Add_Boundry_Left(float k,int startX,int startY,int endY)这个斜率k需要使用静态变量，保证只记录一次，在后续6状态，7状态都要使用这个斜率，不重新计算。
另外这一条线在4状态结束或者5状态开始补线都可以。
4.2.1半边补线
当然，图像可以的话用中线=右边界-半赛宽也是可以的，自己实测，那个效果好用哪个。
或者不处理，正常扫线找中线就行。
3.跳出条件
- 单调突变点出现，位置在规定的范围内。
- 当点出现后，顺便进行补线。
由于每次车模轨迹不同，环岛大小不同，角点出现位置不定，所以划定了一块区域，在这个区域中出现转折点都认为合理。
4跳5状态需要陀螺仪辅助，陀螺仪积分满220度以上才开启，不用陀螺仪也可以。只是怕担心如下情况出现。
由于光线的干扰，这里出现了一个单调转折点。依靠算法无法和标准转折点区分，只能在陀螺仪积分未到指定角度时暂时不开启判断。等到陀螺仪积分满足一定条件再打开判断，尽可能排除干扰。
else if(Island_State==4)//4状态完全进去环岛了 { if(FJ_Angle>200)//环岛积分200度后再打开单调转折判断 { monotonicity_change_line[0]=Monotonicity_Change_Left(90,10);//单调性改变 monotonicity_change_line[1]=Left_Line[monotonicity_change_line[0]]; if((Island_State==4)&&(35<=monotonicity_change_line[0]&&monotonicity_change_line[0]<=55&&monotonicity_change_line[1]<MT9V03X_W-10)) {//转折点在固定区域内补线，顺便进5 island_state_5_down[0]=MT9V03X_H-1; island_state_5_down[1]=Left_Line[MT9V03X_H-1]-5;//抓住第一次出现的斜率，定死 k=(float)((float)(MT9V03X_H-monotonicity_change_line[0])/(float)(island_state_5_down[1]-monotonicity_change_line[1])); K_Add_Boundry_Left(k,island_state_5_down[1],island_state_5_down[0],0); Island_State=5; } } }5状态
1.判断条件
- 单调突变点出现，位置在规定的范围内。
2.连线情况
还是沿着4状态时的补线，不动。
else if(Island_State==5)//准备出环岛 { K_Add_Boundry_Left(k,island_state_5_down[1],island_state_5_down[0],0); if(Island_State==5&&Boundry_Start_Left<MT9V03X_H-20)//左边先丢线 { Island_State=6; } }5.2.1半边补线
赛道宽度是在标准长直道测得每一行的道宽度（赛宽=右边线-左边线），自行测量，不同车不一样。
赛道宽度是在标准长直道测得每一行的道宽度（赛宽=右边线-左边线），自行测量，不同车不一样。
赛道宽度是在标准长直道测得每一行的道宽度（赛宽=右边线-左边线），自行测量，不同车不一样。
3.跳出条件
- 左下角黑块丢失，也就是边界起始点变高，进入6状态.
6状态
1.判断条件
- 左下角黑角丢失，也就是边界起始点变高，进入6状态.
2.连线情况
else if(Island_State==6)//继续出 { K_Add_Boundry_Left(k,island_state_5_down[1],island_state_5_down[0],0); if((Island_State==6)&&(Boundry_Start_Left>MT9V03X_H-10||abs(FJ_Angle)>=320))//左边先丢线 {// k=0; Island_State=7; } }和5状态一致，继续保持着4状态的补线情况。
6.2.1半边补线
赛道宽度是在标准长直道测得每一行的道宽度（赛宽=右边线-左边线），自行测量，不同车不一样。
赛道宽度是在标准长直道测得每一行的道宽度（赛宽=右边线-左边线），自行测量，不同车不一样。
赛道宽度是在标准长直道测得每一行的道宽度（赛宽=右边线-左边线），自行测量，不同车不一样。
3.跳出条件
- 左边界起始点再低变低，进入7状态。
- 或者环岛积分过多，强制进7状态。
7状态
1.判断条件
- 左边界起始点再低变低，进入7状态。
2.连线情况
7状态停止连线，由常规巡线寻找边界，自动调整。
7.2.1半边补线
赛道宽度是在标准长直道测得每一行的道宽度（赛宽=右边线-左边线），自行测量，不同车不一样。
赛道宽度是在标准长直道测得每一行的道宽度（赛宽=右边线-左边线），自行测量，不同车不一样。
赛道宽度是在标准长直道测得每一行的道宽度（赛宽=右边线-左边线），自行测量，不同车不一样。
3.跳出条件
- 寻找右上角点，角点位置合理进8。
找到角点后可以在这里补线，也可以在8状态开始进行补线。
else if(Island_State==7)//基本出环岛，找角点 { Right_Up_Guai[0]=Find_Right_Up_Point(MT9V03X_H-10,0);//获取右上点坐标，找到了去8 Right_Up_Guai[1]=Right_Line[Right_Up_Guai[0]]; if((Island_State==7)&&((Right_Up_Guai[1]>=MT9V03X_W-88&&(5<=Right_Up_Guai[0]&&Right_Up_Guai[0]<=MT9V03X_H-20))))//注意这里，对横纵坐标都有要求，因为赛道不一样，会意外出现拐点 {//当角点位置合理时，进8 Island_State=8; } }8状态
1.判断条件
- 7状态找角点，角点位置在规定的范围内自动进8。
2.连线情况
右上角点延长补线，和十字用的补线方式一样。
抓角点，角点上面几个点计算斜率，向下延长即可。
8.2.1半边补线
赛道宽度是在标准长直道测得每一行的道宽度（赛宽=右边线-左边线），自行测量，不同车不一样。
赛道宽度是在标准长直道测得每一行的道宽度（赛宽=右边线-左边线），自行测量，不同车不一样。
赛道宽度是在标准长直道测得每一行的道宽度（赛宽=右边线-左边线），自行测量，不同车不一样。
3.跳出条件
- 角点位置靠右
- 角点位置靠下
- 右边线起始点变很低，下方不在丢线
以上三点满足任意一点，都可自动进入9状态，基本认为环岛结束。顺便将相关变量清零，等待下一次使用。
else if(Island_State==8)//环岛8 { Right_Up_Guai[0]=Find_Right_Up_Point(MT9V03X_H-1,10);//获取右上点坐标 Right_Up_Guai[1]=Right_Line[Right_Up_Guai[0]]; Lengthen_Right_Boundry(Right_Up_Guai[0]-1,MT9V03X_H-1); if((Island_State==8)&&(Right_Up_Guai[0]>=MT9V03X_H-20||(Right_Up_Guai[0]<10&&Boundry_Start_Left>=MT9V03X_H-10)))//当拐点靠下时候，认为出环了，环岛结束 {//角点靠下，或者下端不丢线，认为出环了 FJ_Angle=0; Island_State=9; Right_Island_Flag=0; } }9状态
9状态时严格意义讲车模就已经离开环岛了，只是为了环岛判断之间有一定的间隔，防止连续判环。
1.判断条件
- 8状态角点靠下或者消失自动进入9状态。
2.连线情况
无
3.跳出条件
计时器计时1s后环岛状态自动归零，环岛彻底结束。
三、注意事项
环岛每一个阶段都是动态的。想要调好他，最好是让车自由跑起来， 想办法获取到他的图像，可以利用图传，或者将图像存在sd卡里，后期分析。用手推和车自己跑的话还是有不少区别的。
我在前文摄像头说的图像四个角出现“暗角”，这会非常影响我对元素的识别。
因为我好多处理用到了边界起始点，如果出现“暗角”那我边界起始点就永远是最下方，导致元素判断无法进行。
1.优化
大家可以看到，环岛的1，环岛2，环岛7，环岛8，左边线都是正常的，那么我们完全可以用左边界+（标准赛道宽度/2），直接得到中线。不依靠右边补线，然后（左+右）/2得到中线。
直接半边补线这样更加稳定。因为只要找角点，他都会有一定发的风险找不到，或者找错，不如直接用现有可靠的边界线平移获得。
我曾经试了一下，效果很好。但是因为距离比赛时间太近了，不敢改了
四、实用技巧
大家可以看到我图像左上角都有数字，这个数字基本代表着环岛的状态，这样我就可以只看图像就知道环岛的情况。
同理，还可以将路障，坡道，断路，等多种元素的状态写进去，只看图像就知道，当前判断了什么元素，在什么状态。非常实用。具体操作如下。
/*------------------------------------------------------------------------------------------------------------------- @brief 元素标志位显示 @param 二值化图片数组 @return null Sample Image_Flag_Show(MT9V03X_W,image_two_value,Island_State); @note 要在图片显示前使用，传入标志位可更改 -------------------------------------------------------------------------------------------------------------------*/ void Image_Flag_Show(uint8 MT9V03XW,uint8(*InImg)[MT9V03XW],uint8 image_flag) { for(uint8 H=1;H<8;H++) { for(uint8 W=1;W<7;W++) { switch (image_flag) { // case 0: *(*(InImg+H)+W)=Image_Flags[0][H][W]; break;//环岛0~8 case 1: *(*(InImg+H)+W)=Image_Flags[1][H][W]; break; case 2: *(*(InImg+H)+W)=Image_Flags[2][H][W]; break; case 3: *(*(InImg+H)+W)=Image_Flags[3][H][W]; break; case 4: *(*(InImg+H)+W)=Image_Flags[4][H][W]; break; case 5: *(*(InImg+H)+W)=Image_Flags[5][H][W]; break; case 6: *(*(InImg+H)+W)=Image_Flags[6][H][W]; break; case 7: *(*(InImg+H)+W)=Image_Flags[7][H][W]; break; case 8: *(*(InImg+H)+W)=Image_Flags[8][H][W]; break; case 9: *(*(InImg+H)+W)=Image_Flags[9][H][W]; break; default: break; } } } }通过修改坐标偏移量，可以在不同位置显示。
/*------------------------------------------------------------------------------------------------------------------- @brief 元素标志位显示 @param 二值化图片数组 @return null Sample Flag_Show_202(MT9V03X_W,image_two_value,Island_State); @note 第二行倒数第二个数字 -------------------------------------------------------------------------------------------------------------------*/ void Flag_Show_202(uint8 MT9V03XW,uint8(*InImg)[MT9V03XW],uint8 image_flag) { for(uint8 H=10+1;H<10+8;H++) { for(uint8 W=MT9V03XW-10+1;W<MT9V03XW-10+8;W++) { switch (image_flag) { //case 0: *(*(InImg+H)+W)=Image_Flags[0][H-10][W-(MT9V03XW-10)]; break; case 1: *(*(InImg+H)+W)=Image_Flags[1][H-10][W-(MT9V03XW-10)]; break; case 2: *(*(InImg+H)+W)=Image_Flags[2][H-10][W-(MT9V03XW-10)]; break; case 3: *(*(InImg+H)+W)=Image_Flags[3][H-10][W-(MT9V03XW-10)]; break; case 4: *(*(InImg+H)+W)=Image_Flags[4][H-10][W-(MT9V03XW-10)]; break; case 5: *(*(InImg+H)+W)=Image_Flags[5][H-10][W-(MT9V03XW-10)]; break; case 6: *(*(InImg+H)+W)=Image_Flags[6][H-10][W-(MT9V03XW-10)]; break; case 7: *(*(InImg+H)+W)=Image_Flags[7][H-10][W-(MT9V03XW-10)]; break; case 8: *(*(InImg+H)+W)=Image_Flags[8][H-10][W-(MT9V03XW-10)]; break; case 9: *(*(InImg+H)+W)=Image_Flags[9][H-10][W-(MT9V03XW-10)]; break; default: break; } } } }//数字图像数组 const uint8 Image_Flags[][9][8]={ { //0 {0X00,0X00,0X00,0X00,0X00,0X00,0X00,0X00}, {0X00,0X00,0XFF,0XFF,0XFF,0XFF,0X00,0X00}, {0X00,0XFF,0X00,0X00,0X00,0X00,0XFF,0X00}, {0X00,0XFF,0X00,0X00,0X00,0X00,0XFF,0X00}, {0X00,0XFF,0X00,0X00,0X00,0X00,0XFF,0X00}, {0X00,0XFF,0X00,0X00,0X00,0X00,0XFF,0X00}, {0X00,0XFF,0X00,0X00,0X00,0X00,0XFF,0X00}, {0X00,0X00,0XFF,0XFF,0XFF,0XFF,0X00,0X00}, {0X00,0X00,0X00,0X00,0X00,0X00,0X00,0X00}, }, { //1 {0X00,0X00,0X00,0X00,0X00,0X00,0X00,0X00}, {0X00,0X00,0XFF,0XFF,0X00,0X00,0X00,0X00}, {0X00,0X00,0X00,0XFF,0X00,0X00,0X00,0X00}, {0X00,0X00,0X00,0XFF,0X00,0X00,0X00,0X00}, {0X00,0X00,0X00,0XFF,0X00,0X00,0X00,0X00}, {0X00,0X00,0X00,0XFF,0X00,0X00,0X00,0X00}, {0X00,0X00,0X00,0XFF,0X00,0X00,0X00,0X00}, {0X00,0X00,0XFF,0XFF,0XFF,0X00,0X00,0X00}, {0X00,0X00,0X00,0X00,0X00,0X00,0X00,0X00}, }, { //2 {0X00,0X00,0X00,0X00,0X00,0X00,0X00,0X00}, {0X00,0X00,0XFF,0XFF,0XFF,0XFF,0X00,0X00}, {0X00,0XFF,0X00,0X00,0X00,0X00,0XFF,0X00}, {0X00,0X00,0X00,0X00,0X00,0X00,0XFF,0X00}, {0X00,0X00,0X00,0X00,0XFF,0XFF,0X00,0X00}, {0X00,0X00,0X00,0XFF,0X00,0X00,0X00,0X00}, {0X00,0X00,0XFF,0X00,0X00,0X00,0X00,0X00}, {0X00,0XFF,0XFF,0XFF,0XFF,0XFF,0XFF,0X00}, {0X00,0X00,0X00,0X00,0X00,0X00,0X00,0X00}, }, { //3 {0X00,0X00,0X00,0X00,0X00,0X00,0X00,0X00}, {0X00,0X00,0XFF,0XFF,0XFF,0XFF,0X00,0X00}, {0X00,0XFF,0X00,0X00,0X00,0XFF,0XFF,0X00}, {0X00,0X00,0X00,0X00,0X00,0XFF,0X00,0X00}, {0X00,0X00,0X00,0X00,0XFF,0XFF,0X00,0X00}, {0X00,0X00,0X00,0X00,0X00,0X00,0XFF,0X00}, {0X00,0XFF,0X00,0X00,0X00,0X00,0XFF,0X00}, {0X00,0X00,0XFF,0XFF,0XFF,0XFF,0X00,0X00}, {0X00,0X00,0X00,0X00,0X00,0X00,0X00,0X00}, }, { //4 {0X00,0X00,0X00,0X00,0X00,0X00,0X00,0X00}, {0X00,0X00,0X00,0X00,0XFF,0X00,0X00,0X00}, {0X00,0X00,0X00,0XFF,0XFF,0X00,0X00,0X00}, {0X00,0X00,0XFF,0X00,0XFF,0X00,0X00,0X00}, {0X00,0XFF,0X00,0X00,0XFF,0X00,0X00,0X00}, {0X00,0XFF,0X00,0X00,0XFF,0X00,0X00,0X00}, {0X00,0XFF,0XFF,0XFF,0XFF,0X00,0X00,0X00}, {0X00,0X00,0X00,0X00,0XFF,0X00,0X00,0X00}, {0X00,0X00,0X00,0XFF,0XFF,0X00,0X00,0X00}, }, { //5 {0X00,0X00,0X00,0X00,0X00,0X00,0X00,0X00}, {0X00,0XFF,0XFF,0XFF,0XFF,0XFF,0X00,0X00}, {0X00,0XFF,0X00,0X00,0X00,0X00,0X00,0X00}, {0X00,0XFF,0X00,0X00,0X00,0X00,0X00,0X00}, {0X00,0XFF,0XFF,0XFF,0XFF,0XFF,0X00,0X00}, {0X00,0X00,0X00,0X00,0X00,0XFF,0X00,0X00}, {0X00,0XFF,0X00,0X00,0X00,0XFF,0X00,0X00}, {0X00,0X00,0XFF,0XFF,0XFF,0X00,0X00,0X00}, {0X00,0X00,0X00,0X00,0X00,0X00,0X00,0X00}, }, { //6 {0X00,0X00,0X00,0X00,0X00,0X00,0X00,0X00}, {0X00,0XFF,0XFF,0XFF,0XFF,0XFF,0X00,0X00}, {0X00,0XFF,0X00,0X00,0X00,0X00,0X00,0X00}, {0X00,0XFF,0X00,0X00,0X00,0X00,0X00,0X00}, {0X00,0XFF,0XFF,0XFF,0XFF,0XFF,0X00,0X00}, {0X00,0XFF,0X00,0X00,0X00,0XFF,0X00,0X00}, {0X00,0XFF,0X00,0X00,0X00,0XFF,0X00,0X00}, {0X00,0XFF,0XFF,0XFF,0XFF,0XFF,0X00,0X00}, {0X00,0X00,0X00,0X00,0X00,0X00,0X00,0X00}, }, { //7 {0X00,0X00,0X00,0X00,0X00,0X00,0X00,0X00}, {0X00,0XFF,0XFF,0XFF,0XFF,0XFF,0X00,0X00}, {0X00,0XFF,0X00,0X00,0X00,0XFF,0X00,0X00}, {0X00,0X00,0X00,0X00,0X00,0XFF,0X00,0X00}, {0X00,0X00,0X00,0X00,0XFF,0X00,0X00,0X00}, {0X00,0X00,0X00,0XFF,0X00,0X00,0X00,0X00}, {0X00,0X00,0X00,0XFF,0X00,0X00,0X00,0X00}, {0X00,0X00,0X00,0XFF,0X00,0X00,0X00,0X00}, {0X00,0X00,0X00,0X00,0X00,0X00,0X00,0X00}, }, { //8 {0X00,0X00,0X00,0X00,0X00,0X00,0X00,0X00}, {0X00,0X00,0XFF,0XFF,0XFF,0XFF,0X00,0X00}, {0X00,0XFF,0X00,0X00,0X00,0X00,0XFF,0X00}, {0X00,0XFF,0X00,0X00,0X00,0X00,0XFF,0X00}, {0X00,0X00,0XFF,0XFF,0XFF,0XFF,0X00,0X00}, {0X00,0XFF,0X00,0X00,0X00,0X00,0XFF,0X00}, {0X00,0XFF,0X00,0X00,0X00,0X00,0XFF,0X00}, {0X00,0X00,0XFF,0XFF,0XFF,0XFF,0X00,0X00}, {0X00,0X00,0X00,0X00,0X00,0X00,0X00,0X00}, }, { //9 {0X00,0X00,0X00,0X00,0X00,0X00,0X00,0X00}, {0X00,0X00,0XFF,0XFF,0XFF,0XFF,0X00,0X00}, {0X00,0XFF,0X00,0X00,0X00,0X00,0XFF,0X00}, {0X00,0XFF,0X00,0X00,0X00,0X00,0XFF,0X00}, {0X00,0X00,0XFF,0XFF,0XFF,0XFF,0XFF,0X00}, {0X00,0X00,0X00,0X00,0X00,0X00,0XFF,0X00}, {0X00,0X00,0X00,0X00,0X00,0XFF,0X00,0X00}, {0X00,0XFF,0XFF,0XFF,0XFF,0X00,0X00,0X00}, {0X00,0X00,0X00,0X00,0X00,0X00,0X00,0X00}, }, };使用起来非常便捷，写入图像宽度，图像数组，显示的变量
Image_Flag_Show(MT9V03X_W,image_two_value,Island_State);//屏幕上打出环岛状 Flag_Show_101(MT9V03X_W,image_two_value,Ramp_Flag);//屏幕上打出环岛状态Ramp_Fla Flag_Show_102(MT9V03X_W,image_two_value,Electromagnet_Flag);//屏幕上打出环岛状 Flag_Show_202(MT9V03X_W,image_two_value,Barricade_Flag);//屏幕上打出环岛状当然，这里也是直接修改原图数组，大家注意不要影响原始图像的判断。
例如，放在左右下角会影响便捷起始点的判断。大家尽量放在图像上面，高处一般很少用到图像。
希望能够帮助到一些人。
本人菜鸡一只，各位大佬发现问题欢迎留言指出
