智能车学习日记【二】————十字和斜入十字
智能车学习日记【二】————十字和斜入十字
文章目录
-
- 智能车学习日记【二】————十字和斜入十字
- 前言
- 一、普通十字补线
- 二、斜入十字
-
- 1.判断是否为斜入十字和斜入十字类型
- 2.找斜入十字拐点
- 总结
前言
经过了一个多月的学习，小白的我终于也是能较稳定调出了2.8m以上的四轮车，过程中也遇到了很多困难，其中就有十字补线和斜入十字补线。尤其是斜入十字也是困扰了我快一周，现在和大家分享一下我处理斜入十字的方法(我的方法其实还有很多漏洞需要改进，仅供大家参考理解一下，欢迎各位在评论区指点出可改进之处！)
一、普通十字补线
首先，我将十字补线分成了找拐点，判断是否需要补线和补线3个步骤。斜入十字也是这样判断，只是多了很多限制条件。防止在直道或弯道也补线导致图像错误。代码里的数组可以在我的第一篇文章里找到定义和含义
智能车学习日记【一】
- 找拐点
正入十字找拐点比较容易，直接贴代码
// 找拐点
if (l_down_flag == 0)
{
if (L_black[0] != 185 && L_black[1] != 185 && L_black[3] != 185)
{
for (i = 1; i < 65; i++)
{
if (abs(L_black[i] - L_black[i - 1]) < 5 && abs(L_black[i + 1] - L_black[i]) < 5 && L_black[i + 2] - L_black[i + 1] > 3 && i + 1 >= 2 && L_black[i + 1] < 180 && i + 1 < 45)
{
lefty[0] = (byte)(i + 1);
SetText(" 左下拐点 y x + " + (i + 1) + " " + L_black[i + 1]);
l_down_flag = 1;
break;
}
}
}
}
if (r_down_flag == 0)
{
if (R_black[0] != 0 && R_black[1] != 0 && R_black[3] != 0)
{
for (i = 1; i < 65; i++)
{
if (abs(R_black[i] - R_black[i - 1]) < 5 && abs(R_black[i + 1] - R_black[i]) < 5 && R_black[i + 2] - R_black[i + 1] < -3 && i + 1 >= 2 && R_black[i + 1] > 2 && i + 1 < 45)
{
righty[0] = (byte)(i + 1);
SetText(" 右下拐点 y x+ " + (i + 1) + " " + R_black[i + 1]);
r_down_flag = 1;
break;
}
}
}
}
if (l_up_flag == 0)
{
for (i = 1; i < 65; i++)
{
if (L_black[i] - L_black[i - 1] < -3 && abs(L_black[i + 1] - L_black[i]) < 4 && abs(L_black[i + 2] - L_black[i + 1]) < 4 && L_black[i] < 181 && (i > lefty[0]))
{
lefty[1] = (byte)(i);
SetText(" 左上拐点 y x+ " + (i) + " " + L_black[i]);
l_up_flag = 1;
break;
}
}
}
if (r_up_flag == 0)
{
for (i = 1; i < 65; i++)
{
if (R_black[i] - R_black[i - 1] > 3 && abs(R_black[i + 1] - R_black[i]) < 4 && abs(R_black[i + 2] - R_black[i + 1]) < 4 && R_black[i] > 2 && (i > righty[0]))
{
righty[1] = (byte)i;
SetText(" 右上拐点 y x+ " + (i) + " " + R_black[i]);
r_up_flag = 1;
break;
}
}
}
思路其实就是用了差值的方法，以左下拐点为例，第i行和第i-1行的边界点x坐标的差值在5之内，第i+1行和第i行的差值在5之内，第i+2行和第i+1行差值大于3，则我判断它为左下拐点。其他拐点找到方法类似。
- 判断是否补线和补线
拐点并不是只有在十字时才有，在弯道时也能找到，但是过弯时我们不应该补线，所以要有判断条件。
如这张正入十字的图，我们是可以通过左右扫线和上面找拐点方式找到4个拐点的，找到拐点后我们就需要判断类型和是否需要补线
这张图为入十字前补线。
我对入十字前补线的判断条件是左右两边同时丢线行大于4，并且4个拐点都找到，才执行入十字前补线。补线思路就是左下拐点和左上拐点之间用最小二乘法求斜率和截距拟合出左边线，右边同样方法拟合出右边线，然后再根据新左右边线重新拟合出中线
下面贴代码
// 入十字前补线* advanced_regression是最小二乘法求斜率截距 *run是拟合直线函数 1为左 2为右 0为中
//abu是二次扫线的计数位，可先忽略
if ((a >= 4&& (lefty[0] > 0 && lefty[1] > 0) || (righty[0] > 0 && righty[1] > 0)) || abu >= 3)
{
if (lefty[0] >= 2 && lefty[1] >= 2 && guai == 0) //找左线 入十字前
{
advanced_regression(1, lefty[0] - 2, lefty[0], lefty[1], lefty[1] + 2);
SetText(" 左斜率 + " + parameterB + " 左截距 " + parameterA);
run(1, lefty[0], lefty[1], parameterB, parameterA);
flagl = 1; //
}
if (righty[0] >= 2 && righty[1] >= 2 && guai == 0) //找右线
{
advanced_regression(2, righty[0] - 2, righty[0], righty[1], righty[1] + 2);
SetText(" 右斜率 + " + parameterB + " 右截距 " + parameterA);
run(2, righty[0], righty[1], parameterB, parameterA);
flagr = 1;
}
if ((r_down_flag == 1 || l_down_flag == 1) && guai == 0)
{
if (flagl == 1 && flagr == 0)
{
run(0, lefty[0], lefty[1], parameterB, parameterA); //拟合中线
SetText("入十字前拉线 " + a);
if (abu >= 3)
{
run(0, 0, lefty[0], parameterB, parameterA);
run(0, lefty[1], 50, parameterB, parameterA);
}
}
else
{
run(0, righty[0], righty[1], parameterB, parameterA); //拟合中线
SetText("入十字前拉线 " + a);
if (abu >= 3)
{
run(0, 0, righty[0], parameterB, parameterA);
run(0, righty[1],50, parameterB, parameterA);
}
}
}
}
这里是我参考的最小二乘法求斜率截距函数。
https://blog.csdn.net/qq_42604176/article/details/107719565
最小二乘法拟合
//拟合直线函数
void run(int type, int startline, int endline, float parameterB, float parameterA)
{
if (type == 1) //左
{
for (int i = startline; i < endline; i++)
{
L_black[i] = (byte)(parameterB * i + parameterA);
if (L_black[i] < 0)
{
L_black[i] = 185;
}
if (L_black[i] > 185)
{
L_black[i] = 185;
}
}
}
else if (type == 2) //右
{
for (int i = startline; i < endline; i++)
{
R_black[i] = (byte)(parameterB * i + parameterA);
if (R_black[i] < 0)
{
R_black[i] = 0;
}
if (R_black[i] > 185)
{
R_black[i] = 0;
}
}
}
else if (type == 0) //中
{
for (int i = startline; i < endline; i++)
{
LCenter[i] = (byte)((L_black[i] / 2 + R_black[i] / 2));
if (LCenter[i] < 0)
{
LCenter[i] = 0;
}
if (LCenter[i] > 185)
{
LCenter[i] = 185;
}
}
}
}
拟合后，就可以的得到入十字前的补线图
当然正入十字分为入十字前和入十字中，入十字中也类似，就是找到左上和右上拐点，然后向下拉线就好了
二、斜入十字
不知道有多少人和我一样遇到这种情况，在斜入十字时边线扫描不对，本该是左边线的变成了右边线，导致了中线拟合错误，车子也就跑错道路了，这种情况找到的拐点一般也不对。
1.判断是否为斜入十字和斜入十字类型
我将斜入十字分为左斜入十字和右斜入十字，上图为左斜入十字。判断是否为斜入十字。首先在一般情况下，左右边界应该是大部分正常分布在中线两端，很少出现上面很多行的右边线大于下边的中线，或上边左边线小于下边中线的情况（我还没写环岛和三岔，所以目前没遇到过），所以当遇到上图这样，上边有大量右线比下边中线大时，则判定为（左）斜入十字
代码如下：
for (i = 1; i < 40; i++)
{
if (i < 20)
{
if ((R_black[i] == 0 && L_black[i] < 184 && R_black[0] == 0 && R_black[1] == 0 && R_black[i+1] > LCenter[0] && R_black[i + 2] > LCenter[1]) || (R_black[0] == 0 && R_black[1] == 0 && R_black[i] > LCenter[5] && R_black[i] > LCenter[7] && R_black[i-1] > LCenter[7]))
{
if ((R_black[0] == 0 && R_black[1] == 0 && R_black[i] > LCenter[5] && R_black[i] > LCenter[7] && R_black[i - 1] > LCenter[7] && L_black[0] != 185 && L_black[1] != 185))
{
cntl = 9;
}
cntl++;
}
else if ((L_black[i] == 185 && R_black[i] > 1 && L_black[0] == 185 && L_black[1] == 185 && L_black[i+1] < LCenter[0] && L_black[i + 2] < LCenter[1]) || (L_black [0] == 0 && L_black[1] == 0 && L_black[i] > LCenter[5] && L_black[i] > LCenter[7] && L_black[i-1] > LCenter[7]))
{
if (L_black[0] == 0 && L_black[1] == 0 && L_black[i] > LCenter[5] && L_black[i] > LCenter[7] && L_black[i - 1] > LCenter[7] && R_black[0] != 0 && R_black[1] != 0)
{
cntr = 9;
}
cntr++;
}
}
else
{
if ((R_black[0] == 0 && R_black[1] == 0 && R_black[i] > LCenter[5] && R_black[i] > LCenter[7] && R_black[i - 1] > LCenter[7] && L_black[0] != 185 && L_black[1] != 185))
{
cntl = 9;
break;
}
else if (L_black[0] == 185 && L_black[1] == 185 && L_black[i] < LCenter[5] && L_black[i] < LCenter[7] && L_black[i - 1] < LCenter[7] && R_black[0]!=0 && R_black[1] != 0)
{
cntr = 9;
break;
}
}
}
if (cntl > 8 && cntr < 8)
{
cntl = 255; //左斜入十字
SetText("可能为左斜入十字 " + cntl);
}
else if (cntr > 8 && cntl < 8)
{
cntr = 255; //右斜入十字
SetText("可能为右斜入十字 " + cntr);
}
2.找斜入十字拐点
判断完后就是要找拐点。还是以这图为例。其实我们只要找到左上拐点，因为左下拐点可以通过上面的找拐点方式找出来，而且一般不会找错，都是上拐点找错(因为边线错误)
上拐点找的方式也特别简单，我们已经知道了现在是左斜入十字，而且上边的右边线本应该是左边线，所以我们就找上边右边线突变点。图中就是蓝点标记出来的位置，可以看见蓝点之前右边线都是0，蓝点以后的线本来应该是左边线，却被当作了右边线，所以，这个蓝点就是新的左上拐点。这样我们就找到了左下和左上拐点，进行拟合拉线就可以了
代码如下（示例）：
guai = 0;
t = 0;
if (cntl == 255)
{
for (i = 5; i < 65; i++)
{
if ((R_black[i] - R_black[i - 1] > 15) && (R_black[i] - R_black[i - 2] > 15))
{
L_black[i] = R_black[i];
R_black[i] = R_black[i - 1];
if (t == 0)
{
guai = i;
}
t++;
LCenter[i] = (byte)(R_black[i] / 2 + L_black[i] / 2);
}
}
if (guai != 0)
{
SetText("新左上拐点y x " + guai + " " + L_black[guai]);
SetText("左斜入十字 " + cntl);
}
else
{
SetText("没找到新左上拐点y x " + guai + " " + L_black[guai]);
}
}
else if (cntr == 255)
{
for (i = 5; i < 65; i++)
{
if ((L_black[i] - L_black[i - 1]) < -15 && (L_black[i] - L_black[i - 2]) < -15)
{
R_black[i] = L_black[i];
L_black[i] = L_black[i - 1];
if (t == 0)
{
guai = i;
}
t++;
LCenter[i] = (byte)(R_black[i] / 2 + L_black[i] / 2);
}
}
if (guai != 0)
{
SetText("新右上拐点y x " + guai + " " + R_black[guai]);
SetText("右斜入十字 " + cntl);
}
else
{
SetText("没找到新右上拐点y x " + guai + " " + R_black[guai]);
}
}
补好以后车子遇到斜入十字也基本不会跑错了，当然情况很多，遇到了还是要具体问题具体分析。下图左斜入的口子已经给补好了，上面的边线其实还可以改的更好看一点的，但是时间比较紧，车子也不会跑错，就没管了XD。
右斜入也是类似方法。
左斜入十字补线
总结
斜入十字确实是新手入门的一大难点之一，我也困扰也很久，现在其实也只是停留在车子跑对道路，限制了车子稳定突破2.9m以上，后面还需要经过各种修改来让图更好看，车跑的更稳。一点点见解希望能帮到大家。如果觉得有点点帮助还希望点赞支持一下新人博主，有问题可以在评论区留言哦~。欢迎大佬指点一下可以改进的地方，一起学习。
方法更新：
后续为了使图像更好看，特意加了一种特殊扫线，即在十字状态机中找一条动态中线，以动态中线为基准向左右扫线，这样可以避免中线扫歪的情况出现。
// 动态中线扫线
void hd2_findmiddle()
{
byte max_X = 0;
byte max_Y = 0;
for (x = R_black[0]; x < L_black[0]; x++) //找一条中线进行左右扫线
{
for (y = 1; y < 70; y++)
{
if (y == 69 || (Pixels[y - 1][x] != 0 && Pixels[y][x] == 0))
{
if (y > max_Y)
{
max_Y = y;
max_X = x;
break;
}
}
if (Pixels[y - 1][x] == 0 && Pixels[y][x] == 0)
{
break;
}
}
}
if (max_X == 0)
max_X = 1;
if (max_X == 185)
max_X = 184;
// SetText("max_Y=" + max_Y + " max_X=" + max_X + " 根据中线x=" + max_X + "拉线");
for (y = 1; y < 70; y++)
{
if (Pixels[y][max_X] == 0 || y == 69)
{
b = y;
break;
// SetText("新丢线行b=" + b);
}
for (x = max_X; x < 186; x++)
{
if (x == 185 || (Pixels[y][x] != 0 && Pixels[y][x - 1] != 0 && Pixels[y][x + 1] == 0)) //左边界
{
L_black[y] = x;
break;
}
}
for (x = max_X; x >= 0; x--)
{
if (x == 0 || (Pixels[y][x] != 0 && Pixels[y][x - 1] == 0 && Pixels[y][x + 1] != 0)) //右边界
{
R_black[y] = x;
break;
}
}
LCenter[y] = (byte)(R_black[y] / 2 + L_black[y] / 2);
}
}
下图的蓝色线即为找到的固定中线
最后的最后如果想看看我这一年智能车的经历和建议可以到第一篇文章最下面浏览一下 智能车学习日记【一】