前言
这篇文章主要讲述智能车竞赛中如何判断坡道。
一、函数主体
void podaojudge()
{
if (countpodao != 0) countpodao = countpodao + 1;
if (countpodao == 30) countpodao = 0;
if (countpodao == 0|| countpodao>20)
{
int i = 0; //50大于70 30大于100 20大于110
float kerrorL = 0;
float kerrorR = 0;
int end = 0;
int hang;
/*
下面这一部分用于获得赛道的起始和终止行数，在坡道判定中会有用处。
*/
for (hang = 1; hang < 69; hang++) //去掉杂点
{
if (LCenter[hang] == 0 && LCenter[hang + 1] == 0)
{
end = hang - 1;
break;
}
if (My_Abs(LCenter[hang], LCenter[hang + 1]) > 20 && hang > 5)
{
end = hang - 1;
break;
}
}
//int begin = 0;
//for (i = 0; i < 50; i++)
//{
//if(R_black[i]==0&& R_black[i+1] != 0)
// {
// begin = i + 1;
// break;
// }
//}
//SetText("begin " + begin);
int beginleft =1 ;
if (L_black[0] == 186)
{
for (i = 0; i < 50; i++)
{
if (L_black[i] == 186 && L_black[i + 1] == 186 && L_black[i + 2] != 186)
{
beginleft = i + 2;
break;
}
}
}
int beginright = 1;
if (R_black[0] == 0)
{
for (i = 0; i < 50; i++)
{
if (R_black[i] == 0 && R_black[i + 1] == 0 && R_black[i + 2] != 0)
{
beginright = i + 2;
break;
}
}
}
if (beginleft >= 17) beginleft = 17;
if (beginright > 17) beginright = 17;
/*
敲黑板!
下面这一段基本上可以说是整个坡道判定的核心
比较0-20 0-end 行的斜率差值，来做出坡道的判定
这里的理论依据是，坡道情况下，整个赛道会被抬高，你会明显看到坡道那一段
的斜率和前面普通赛道上斜率出现不同。就像两个线段接在一起一样
*/
regression(1, beginleft, break_hangshu - 5);
LK0end = parameterB;
advanced_regression(1, beginleft, beginleft+(20- beginleft)/3, beginleft + (20 - beginleft) / 3*2, 20);
LK020 = parameterB;
regression(2, beginright, break_hangshu - 5);
RK0end = parameterB;
advanced_regression(2, beginright, beginright+(20- beginright)/3, beginright + (20 - beginright) / 3*2, 20);
RKB020 = parameterB;
kerrorL = FMy_Abs(LK0end, LK020);
kerrorR = FMy_Abs(RK0end, RKB020);
/*因为下面要利用赛道宽度值 这里寻找左右边线都存在的最后一行 留下标记
(只存在一边边线的行数 宽度不太有意义)*/
int kuandubreakhang=0;
for (i = 69; i > 0; i--)
{
if (L_black[i] != 0 && R_black[i] != 0)
{
kuandubreakhang = i;
break;
}
}
if (kuandubreakhang >= 32)
{
kuandubreak = L_black[kuandubreakhang ] - R_black[kuandubreakhang ]; }
if (kuandubreakhang < 32&& kuandubreakhang >= 22)
{
kuandubreak = L_black[15] - R_black[15]; }
if (kuandubreakhang < 22)
{
kuandubreak = 0;
}
int midddlecontinue = 1;
/*
显然，坡道的情况下，中线肯定是连续的，midddlecontinue == 1 很重要
*/
for (i = 1; i < break_hangshu - 10; i++)
{
if (LCenter[i] - LCenter[i - 1] > 4) {
midddlecontinue = 0; }
}
/*
下面语句判断的思路大同小异
(1)不在环岛状态机中
(2)找到0-20 0-end 左右线斜率的关系
(3)宽度值必须要满足一定的条件 (因为坡道也是赛道 宽度要足够宽)
(4)持续好几帧图像
*/
/*第一种 两米的坡道*/
if (huandao_memory == 0 && huandao_memoryforleft == 0&&
turepodaoflag == 0 && truepodaotype1 == 0 && truepodaotype2 == 0 && RKB020 >= (RK0end + 0.2) && RKB020 <= (RK0end + 0.8) && RKB020 > 1 && RKB020 < 3 &&
LK020 <= LK0end - 0.1 && LK020 <= -0.6&& LK020 >= -1.2 && LK020 >= LK0end - 0.7 && continueleft == 1 &&
continueright == 1 && pianfangleft <= 50 && pianfangright <= 50 && calkuan[break_hangshu - 3] > 40 && calkuan[break_hangshu - 4] > 40 && calkuan[break_hangshu - 5] > 40)
{
podaoflagonemeter = podaoflagonemeter + 1;
if (podaoflagonemeter == 3)
{
turepodaoflag = 1;
truelongflag = 0;
podaoflagonemeter = 0;
truepodaotype1 = 1;
truepodaotype2 = 0;
podaotype = 1;
}
}
/*第一种 两米的坡道*/
if (huandao_memory == 0 && huandao_memoryforleft == 0&&
turepodaoflag == 0 && truepodaotype1 == 0 && truepodaotype2 == 0 && RKB020 >= (RK0end + 0.05) && RKB020 <= (RK0end + 0.9) && RKB020 >1&& RKB020 < 3&&
LK020 <= LK0end - 0.2 && LK020 <= -0.2 && LK020 >= -2.4 && LK020>= LK0end - 0.9 &&continueleft == 1 &&
continueright == 1 && pianfangleft<=50 && pianfangright <= 50&&calkuan[break_hangshu-3]>40 && calkuan[break_hangshu - 4] > 40 && calkuan[break_hangshu - 5] > 40)
{
podaoflag = podaoflag + 1;
if (podaoflag == 3)
{
turepodaoflag = 1;
truelongflag = 0;
podaoflag = 0;
truepodaotype1 = 1;
truepodaotype2 = 0;
podaotype = 2;
}
}
/*
弯道接坡道 不可能有时间足够的帧数去判别
思路就是 因为40行是图像的尽头了 一般宽度20 30
如果40行左右宽度仍然大于50 60 就认为弯接坡道
进入坡道状态机
*/
//第三种 弯接两米的坡道
if ( turepodaoflag == 0 && truepodaotype1 == 0 && truepodaotype2 == 0
&&(L_black[39] - R_black[39] >= 63) && (L_black[40] - R_black[40] >= 63 ) &&
(L_black[41] - R_black[41] >= 63 )
&& (L_black[43] - R_black[43] >= 50||L_black[44] - R_black[44] >= 50 ))
{
int first10=1;
int second20=1;
int third30=1;
int fourth40=1;
for (i = 0; i < 10; i++)
{
if (L_black[i]-R_black[i] < 140)
{
first10 = 0;
break;
}
}
if (first10 == 1)
{
for (i = 10; i < 20; i++)
{
if (L_black[i] - R_black[i] < 130)
{
second20 = 0;
break;
}
}
}
if (second20 == 1)
{
for (i = 20; i < 30; i++)
{
if (L_black[i] - R_black[i] < 100)
{
third30 = 0;
break;
}
}
}
if (third30 == 1)
{
for (i = 30; i < 40; i++)
{
if (L_black[i] - R_black[i] < 70)
{
fourth40 = 0;
break;
}
}
}
if (first10 == 1 && second20 == 1 && third30 == 1 && fourth40 == 1&&huandao_memory == 0 && huandao_memoryforleft == 0)
{
podaoflagquick = podaoflagquick + 1;
if (podaoflagquick == 2)
{
turepodaoflag = 1;
truelongflag = 0;
podaoflagquick = 0;
truepodaotype1 = 1;
truepodaotype2 = 0;
podaotype = 2;
countpodao1flag = 0;
}
}
}
/*判断到坡道之后
进入坡道状态机的第二部分
下面的部分包括
上坡一定程度锁定中值
坡顶开启扫线
下坡状态
*/
/*
首先，判断到坡道之后十帧图像之内 认为是基本的上坡过程 状态持续保持为1
*/
if (turepodaoflag == 1 && countpodao1flag<=10&& truepodaotype1==1&&truepodaotype2 == 0)
{
if (countpodao1flag < 10) countpodao1flag = countpodao1flag + 1;
}
int poendleft = 0;
int poendright = 0;
int maxpoend = 0;
if (turepodaoflag == 1 && countpodao1flag <10 && truepodaotype1 == 1 && truepodaotype2 == 0)
{
if (beginleft != 0)
{
for (i = beginleft; i <=50; i++)
{
if (My_Abs(L_black[i], L_black[i + 1]) > 5)
{
poendleft = i;
break;
}
}
}
if (beginright != 0)
{
for (i = beginright; i <= 50; i++)
{
if (My_Abs(R_black[i], R_black[i + 1]) > 5)
{
poendright = i;
break;
}
}
}
/*
在这十帧图像的过程中，可能有效赛道的行数退到了只有20-30行，需要将底部赛道向上方延续，
一直补到69行。
*/
if (poendleft != 0 && poendright == 0)
{
maxpoend = poendleft;
}
if (poendleft == 0 && poendright != 0)
{
maxpoend = poendright;
}
if (poendleft != 0 && poendright != 0)
{
if(poendleft>= poendright) maxpoend = poendleft;
if (poendleft < poendright) maxpoend = poendright;
}
if (maxpoend >=3&&maxpoend <= 50 && maxpoend != 0)
{
for (i = maxpoend - 2; i <= 69; i++)
{
L_black[i] = L_black[i - 1];
R_black[i] = R_black[i - 1];
LCenter[i] = (unsigned char)((unsigned char)L_black[i] / 2 + (unsigned char)R_black[i] / 2);
}
}
}
int ponobomp = 0;
/*
ponobomp=1 的意思是每一行的左右线保持着基本的连续状态(跳变比较小，没有杂乱无章的去跳变)
*/
if (turepodaoflag == 1 && truepodaotype1 == 1 && truepodaotype2 == 0)
{
if (L_black[10] != 186 && R_black[10] != 0 && L_black[15] != 186 && R_black[15] != 0 && L_black[20] != 186 && R_black[20] != 0
&& L_black[13] != 186 && R_black[13] != 0 && L_black[18] != 186 && R_black[18] != 0
&& My_Abs(LCenter[0], LCenter[1]) <= 5 & My_Abs(LCenter[1], LCenter[2]) <= 5 & My_Abs(LCenter[2], LCenter[3]) <= 5 & My_Abs(LCenter[3], LCenter[4]) <= 5
& My_Abs(LCenter[4], LCenter[5]) <= 5 & My_Abs(LCenter[5], LCenter[6]) <= 5 & My_Abs(LCenter[6], LCenter[7]) <= 5 & My_Abs(LCenter[7], LCenter[8]) <= 5
& My_Abs(LCenter[8], LCenter[9]) <= 5 && My_Abs(LCenter[0], LCenter[9]) <= 10 && (calkuan[30] - calkuan[10] <= -10 || break_hangshu < 30)
) ponobomp = 1;
}
/*
计数满十帧之后 开始进入坡道的操作状态
十帧初始 还未到坡道顶端 左边放在186 右线放在0 中线93 相当于锁定了舵机
*/
if (turepodaoflag == 1 && truepodaotype1 == 1 && truepodaotype2 == 0 && countpodao1flag == 10&& countpodao2flag<=20)
{
for (i = 0; i <= 60; i++)
{
L_black[i] = 186;
LCenter[i] = 93;
R_black[i] = 0;
zhongold = 93;
}
countpodao2flag = countpodao2flag + 1;
saoxianflag = 1;
}
if (saoxianflag == 1&& ((podaotype==2&&countpodao2flag> 15)|| (podaotype == 1 && countpodao2flag > 15)) && truepodaotype1 == 1 && truepodaotype2 == 0&&Pixels[0][93]==0
&& Pixels[0][ 92] == 0 && Pixels[0][ 91] == 0
)
{
int leftstart = 0;
int rightstart = 0;
int leftend = 0;
int rightend = 0;
int lie;
for (lie = 93; lie >= 1; lie--)
{
if (Pixels[0][ lie - 1] == 255 && Pixels[0][ lie] == 0 && Pixels[0][ lie + 1] == 0) //黑黑白 右边开始
{
rightstart = lie - 1;
break;
}
}
if (rightstart != 0)
{
for (lie = rightstart; lie > 1; lie--)
{
if (Pixels[0][ lie - 1] == 0 && Pixels[0][ lie] == 255 && Pixels[0][ lie + 1] == 255) //白白黑 右边结束
{
rightend = lie;
break;
}
}
}
for (lie = 93; lie <= 184; lie++)
{
if (Pixels[0][ lie - 1] == 0 && Pixels[0][lie] == 0 && Pixels[0][lie + 1] == 255) //黑黑白 左边开始
{
leftstart = lie + 1;
break;
}
}
if (leftstart != 0)
{
for (lie = leftstart; lie <=184; lie++)
{
if (Pixels[0][ lie - 1] == 255 && Pixels[0][lie] == 255 && Pixels[0][lie + 1] == 0) //白白黑 左边结束
{
leftend = lie;
break;
}
}
}
int widthright = rightstart - rightend;
int widthleft = leftend - leftstart;
zhongold = 93;
if (widthright >= widthleft && widthright > 10)
{
zhongold = (rightstart + rightend) / 2;
}
if (widthright < widthleft && widthleft > 10)
{
zhongold = (leftstart + leftend) / 2;
}
}
// SetText("zhongold " + zhongold);
// SetText("zhongold " + zhongold);
// SetText("zhongold " + zhongold);
//if (turepodaoflag == 1 && truepodaotype1 ==1&& truepodaotype2 == 0&& ((kuandubreak != 0 && kuandubreak <= 65 && kuandubreakhang >= 32 && end > 30) || (kuandubreak != 0 && kuandubreak <= 85 && kuandubreakhang < 38 && kuandubreakhang >= 22 && end > 22))
// && (kerrorL <= 0.6||kerrorR <= 0.6)&&calkuan[0]<170&& countpodao1flag==10 && calkuan[0] >30
// &&(continueleft==1||continueright==1)&&break_hangshu>=25
// )//((kuandubreak != 0 && kuandubreak <= 65 &&kuandubreakhang>= 32 && end > 30) || (kuandubreak != 0 && kuandubreak <= 85 && kuandubreakhang<38&& kuandubreakhang>=22 && end > 22))
if (turepodaoflag == 1 && truepodaotype1 == 1 && truepodaotype2 == 0 && (continueleft == 1 || continueright == 1) && break_hangshu >= 25 && countpodao1flag == 10
&& (calkuan[0] > 40 && calkuan[0] < 150) && (calkuan[3] > 40 && calkuan[3] < 150) && (calkuan[6] > 40 && calkuan[6] < 150) && (calkuan[9] > 40 && calkuan[9] < 150)
&& (calkuan[12] > 40 && calkuan[12] < 150) && (calkuan[14] > 40 && calkuan[14] < 150) && (calkuan[16] > 40 && calkuan[16] < 150) && (calkuan[18] > 40 && calkuan[18] < 150)
&& ponobomp==1
)
{
saoxianflag = 0;
countpodao1flag = 0;
turepodaoflag = 1;
truepodaotype1 = 0;
truepodaotype2 =1 ;
podaotype2 = 0;
countpodao2flag = 0;
podaotype = 0;
}
if (turepodaoflag == 1 && truepodaotype1 == 0 && truepodaotype2 == 1 &&
(break_hangshu < 43||(break_hangshu > 43&&(L_black[43] - R_black[43] < 50 || L_black[44] - R_black[44] < 50 || L_black[45] - R_black[45] < 50 || L_black[46]-R_black[46]<50|| L_black[47] - R_black[47] < 50 || L_black[48] - R_black[48] < 50 || L_black[49] - R_black[49] < 50 || L_black[50] - R_black[50] < 50)))
&&( calkuan[0] > 165||(L_black[0]>=185&&R_black[0]>=30)
|| (L_black[0] <=155 && R_black[0] <=1)
)&& jishupodaotype != 4)
{
turepodaoflag = 0;
truepodaotype1 = 0;
truepodaotype2 = 0;
saoxianflag = 0;
countpodao1flag = 0;
countpodao2flag = 0;
}
if (turepodaoflag == 1 && truepodaotype1 == 0 && truepodaotype2 == 1 &&
(break_hangshu < 44)
&& (calkuan[0] > 165 || (L_black[0] >= 185 && R_black[0] >= 30)
|| (L_black[0] <= 155 && R_black[0] <= 1)
)&&jishupodaotype==4)
{
turepodaoflag = 0;
truepodaotype1 = 0;
truepodaotype2 = 0;
saoxianflag = 0;
countpodao1flag = 0;
countpodao2flag = 0;
}
//if (turepodaoflag == 1 && podaotype2 == 1 && break_hangshu < 30 && break_hangshu > 20 && (L_black[10] - R_black[10]) < 70 && midddlecontinue == 1)
//{
// turepodaoflag = 0;
// podaotype2 = 0;
//}
}
if (turepodaoflag == 0 && lasttruepodaoflag == 1)
{
countpodao = 1; }
}
//坡道结束
二、辅助函数
void advanced_regression(int type, int startline1, int endline1, int startline2, int endline2)
{
int i = 0;
int sumlines1 = endline1 - startline1;
int sumlines2 = endline2 - startline2;
int sumX = 0;
int sumY = 0;
float averageX = 0;
float averageY = 0;
float sumUp = 0;
float sumDown = 0;
if (type == 0) //拟合中线
{
/**计算sumX sumY**/
for (i = startline1; i < endline1; i++)
{
sumX += i;
sumY += LCenter[i];
}
for (i = startline2; i < endline2; i++)
{
sumX += i;
sumY += LCenter[i];
}
averageX =(float)( sumX / (sumlines1 + sumlines2)); //x的平均值
averageY = (float)(sumY / (sumlines1 + sumlines2)); //y的平均值
for (i = startline1; i < endline1; i++)
{
sumUp += (LCenter[i] - averageY) * (i - averageX);
sumDown += (i - averageX) * (i - averageX);
}
for (i = startline2; i < endline2; i++)
{
sumUp += (LCenter[i] - averageY) * (i - averageX);
sumDown += (i - averageX) * (i - averageX);
}
if (sumDown == 0) parameterB = 0;
else parameterB = sumUp / sumDown;
parameterA = averageY - parameterB * averageX;
}
else if (type == 1) //拟合左线
{
/**计算sumX sumY**/
for (i = startline1; i < endline1; i++)
{
sumX += i;
sumY += L_black[i];
}
for (i = startline2; i < endline2; i++)
{
sumX += i;
sumY += L_black[i];
}
averageX =(float)( sumX / (sumlines1 + sumlines2)); //x的平均值
averageY = (float)(sumY / (sumlines1 + sumlines2)); //y的平均值
for (i = startline1; i < endline1; i++)
{
sumUp += (L_black[i] - averageY) * (i - averageX);
sumDown += (i - averageX) * (i - averageX);
}
for (i = startline2; i < endline2; i++)
{
sumUp += (L_black[i] - averageY) * (i - averageX);
sumDown += (i - averageX) * (i - averageX);
}
if (sumDown == 0) parameterB = 0;
else parameterB = sumUp / sumDown;
parameterA = averageY - parameterB * averageX;
}
else if (type == 2) //拟合右线
{
/**计算sumX sumY**/
for (i = startline1; i < endline1; i++)
{
sumX += i;
sumY += R_black[i];
}
for (i = startline2; i < endline2; i++)
{
sumX += i;
sumY += R_black[i];
}
averageX =(float)( sumX / (sumlines1 + sumlines2)); //x的平均值
averageY = (float)(sumY / (sumlines1 + sumlines2)); //y的平均值
for (i = startline1; i < endline1; i++)
{
sumUp += (R_black[i] - averageY) * (i - averageX);
sumDown += (i - averageX) * (i - averageX);
}
for (i = startline2; i < endline2; i++)
{
sumUp += (R_black[i] - averageY) * (i - averageX);
sumDown += (i - averageX) * (i - averageX);
}
if (sumDown == 0) parameterB = 0;
else parameterB = sumUp / sumDown;
parameterA = averageY - parameterB * averageX;
}
}
void regression(int type, int startline, int endline)//最小二乘法拟合曲线，分别拟合中线，左线，右线,type表示拟合哪几条线 xy 颠倒
{
int i = 0;
int sumlines = endline - startline;
int sumX = 0;
int sumY = 0;
float averageX = 0;
float averageY = 0;
float sumUp = 0;
float sumDown = 0;
if (type == 0) //拟合中线
{
for (i = startline; i < endline; i++)
{
sumX += i;
sumY += LCenter[i];
}
if (sumlines != 0)
{
averageX = (float)(sumX / sumlines); //x的平均值
averageY =(float)( sumY / sumlines); //y的平均值
}
else
{
averageX = 0; //x的平均值
averageY = 0; //y的平均值
}
for (i = startline; i < endline; i++)
{
sumUp += (LCenter[i] - averageY) * (i - averageX);
sumDown += (i - averageX) * (i - averageX);
}
if (sumDown == 0) parameterB = 0;
else parameterB = sumUp / sumDown;
parameterA = averageY - parameterB * averageX;
}
else if (type == 1)//拟合左线
{
for (i = startline; i < endline; i++)
{
sumX += i;
sumY += L_black[i];
}
if (sumlines == 0) sumlines = 1;
averageX = (float)(sumX / sumlines); //x的平均值
averageY =(float)( sumY / sumlines); //y的平均值
for (i = startline; i < endline; i++)
{
sumUp += (L_black[i] - averageY) * (i - averageX);
sumDown += (i - averageX) * (i - averageX);
}
if (sumDown == 0) parameterB = 0;
else parameterB = sumUp / sumDown;
parameterA = averageY - parameterB * averageX;
}
else if (type == 2)//拟合右线
{
for (i = startline; i < endline; i++)
{
sumX += i;
sumY += R_black[i];
}
if (sumlines == 0) sumlines = 1;
averageX = (float)(sumX / sumlines); //x的平均值
averageY =(float)( sumY / sumlines); //y的平均值
for (i = startline; i < endline; i++)
{
sumUp += (R_black[i] - averageY) * (i - averageX);
sumDown += (i - averageX) * (i - averageX);
}
if (sumDown == 0) parameterB = 0;
else parameterB = sumUp / sumDown;
parameterA = averageY - parameterB * averageX;
}
}
//求两数之差绝对值开始
int My_Abs(int a, int b)
{
if ((a - b) > 0)
return ((int)(a - b));
else return ((int)(b - a));
}
//求两数之差绝对值结束
float FMy_Abs(float a, float b)//求两数之差绝对值的浮点数
{
if ((a - b) > 0)
return ((float)(a - b));
else return ((float)(b - a));
}
总结
1.坡道的状态机共有三个状态，判定上坡-坡顶-下坡，代码运行成功，就可以成功判断出坡道。
2.配合上辅助函数就是坡道判断完整代码，如果有不懂得请留言哦。