智能车学习日记【三】————车库判别
文章目录
- 前言
- 一、斑马线的判别
- 二、出入库
-
- 1.入库
- 2.出库
- 总结
前言
这里记录一下我的第一版出入库，我觉得出入库最重要的还是判别斑马线和正确拉线。这里感谢我的队友DZH和学长们给我提供不同的出入库方法，斑马线的判断方法和拉线方法有来自拾牙慧者的文章进行改动的。现在可以在速度不快的情况下正常出入库且误判情况较少，由于这两天刚刚完成，还有较多不足，欢迎大家提出意见和修改！ 一些数组和参数定义在前几篇文章有说明，如果觉得这篇文章有帮助就点个赞支持一下吧！
一、斑马线的判别
这里以向右入库为例，左入库思路类似。
思路：
1.设置一个栈，用来存储黑色元素。
2.从规定行的第0列开始往左扫(左入库则相反)，如果遇到黑色元素，入栈。
3.如果遇到白色元素，出栈，出栈的同时统计栈中元素个数，如果栈中元素为4–8个（也就是黑胶的宽度），black_blocks++，否则不加。然后将栈中元素清掉。
4.如果此行的black_blocks在8个左右，times++。
5.遍历规定行，如果，times在4个左右，则确定为斑马线。
flag_find_cheku : 远端发现斑马线，补线直行，直到近处发现才补线入库
flag_starting_line : 24-29行判断拉线入库
byte flag_ruku : 入库标志位
void check_starting_line(byte start_point, byte end_point)
{
byte times = 0;
for (byte y = start_point; y <= end_point; y++)
{
byte black_blocks = 0;
byte cursor = 0; //指向栈顶的游标
for (byte x = 0; x <= 185; x++)
{
if (Pixels[y, x] == 0)
{
if (cursor >= 20)
{
break; //当黑色元素超过栈长度的操作 break;
}
else
{
cursor++;
}
}
else
{
if (cursor >= 4 && cursor <= 8)
{
black_blocks++;
cursor = 0;
}
else
{
cursor = 0;
}
}
}
if (black_blocks >= 6 && black_blocks <= 9) times++;
}
if (times >= 1)
{
flag_starting_line = 1;
flag_ruku = 1;
flag_find_cheku = 0;
SetText("入库状态");
}
else
{
flag_starting_line = 0;
}
}
二、出入库
1.入库
入库我分成了2个阶段：
1、远处发现斑马线，这个时候由于斑马线的影响，上边的线是乱的，而我们这个时候就需要补线继续直行。
2、近处发现斑马线，这时要分2个方面，要拉线入库，还是要补线直行。补线直行的思路与第一点相同。
一、远处发现斑马线
思路：下拐点的找法和正常找下拐点的方法类似。上拐点不同，因为斑马线会导致上边的线混乱，可能上边的线会偏移的很离谱，不能通过正常扫线判断来获取上拐点，所以我们可以从下拐点那一行的第0处开始往上找，找到黑白交界处，这时往左扫线，再找到黑白交界处，就是一个上拐点的位置。为了更准确一点，我们用同样的再往上多找几行，就能找到多个上拐点，取最大的上拐点作为真正右上拐点即可。
代码如下：
else if (type == 3)
{
for (i = 1; i < 65; i++)
{
if (abs(R_black[i] - R_black[i - 1]) < 3 && R_black[i + 1] - R_black[i] <= -3)
{
righty[0] = i;
rightx[0] = R_black[i];
break;
}
}
if (righty[0] > 0)
{
for (y = (byte)(i + 2); y < 69; y++)
{
if (Pixels[y, 0] == 0 && Pixels[y + 1, 0] == 0)
{
y1 = y;
break;
}
}
}
else
{
for (y = 35; y < 69; y++)
{
if (Pixels[y, 0] == 0 && Pixels[y + 1, 0] == 0)
{
y1 = y;
break;
}
}
}
for (x = 0; x < 185; x++)
{
if (Pixels[y1, x] != 0)
{
x1 = x;
break;
}
}
byte y_zhengque = 0;
for (y = (byte)(y1 + 1); y <= y1 + 3; y++)
{
byte X = 0;
for (x = 0; x < 185; x++)
{
if (Pixels[y, x] != 0)
{
X = x;
break;
}
}
if (X >= x1)
{
x1 = X;
y_zhengque = y;
}
}
y1 = y_zhengque;
righty[1] = y1;
rightx[1] = x1;
if (rightx[0] > rightx[1]) //普通扫线扫到斑马线有时会扫出几个很远的点
{
rightx[0] = 0;
righty[0] = 0;
last_guaix = 0;
last_guaiy = 0;
}
SetText("车库右下拐点为y=" + righty[0] + " x=" + rightx[0] + "右上拐点为y=" + righty[1] + " x=" + rightx[1]);
SetText("上一帧右下拐点 y=" + last_guaiy + " x=" + last_guaix);
}
二、近处斑马线
思路：近处发现了斑马线的时候，下拐点就不太重要了，主要就是通过找到右上拐点，与左下边线拉线入库。右上拐点的找寻方法依旧和上边是一样的，然后找左边线第一个不丢线的点作为左下拐点。
代码如下：
if (type == 2) //右
{
if (Pixels[0, 185] != 0)
{
for (y = 1; y < 70; y++)
{
if (Pixels[y, 185] == 0)
{
y1 = y;
break;
}
}
}
else
{
y1 = 0;
}
for (x = 185; x > 1; x--)
{
if (Pixels[y1, x] != 0)
{
x1 = x;
break;
}
}
lefty[0] = y1;
leftx[0] = x1;
//开始找右上拐点
if (Pixels[0, 0] == 0 && Pixels[1, 0] == 0 && Pixels[2, 0] == 0 && Pixels[3, 0] == 0 && Pixels[4, 0] == 0)
{
for (y = 5; y < 70; y++)
{
if (Pixels[y, 0] != 0 && y >= 8)//为白
{
y1 = y;
break;
}
}
for (y = y1; y < 68; y++)
{
if (Pixels[y, 0] == 0 && y >= 15 && Pixels[y + 2, 0] == 0)
{
y1 = y;
break;
}
}
}
else
{
for (y = 10; y < 70; y++)
{
if (Pixels[y, 0] == 0 && y >= 15 && Pixels[y + 2, 0] == 0)
{
y1 = y;
break;
}
}
}
for (x = 0; x < 185; x++)
{
if (Pixels[y1, x] != 0)
{
x1 = x;
break;
}
}
byte y_zhengque = 0;
for (y = (byte)(y1 + 1); y <= y1 + 2; y++)
{
byte X = 0;
for (x = 0; x < 185; x++)
{
if (Pixels[y, x] != 0)
{
X = x;
break;
}
}
if (X >= x1)
{
x1 = X;
y_zhengque = y;
}
}
y1 = y_zhengque;
righty[1] = y1;
rightx[1] = x1;
SetText("车库左下拐点为y=" + lefty[0] + " x=" + leftx[0] + "右上拐点为y=" + righty[1] + " x=" + rightx[1]);
}
2.出库
出库的方法有很多，可以计时打角，也可以拉线。这里分享一下我的拉线方法。
思路：拉线转弯的思路还是比较简单，因为两边都丢线，右边就往上找一个拉线点，与左下边边界拉线即可
代码如下：
if (flag_chukechange == 1)
{
SetText("状态： 转弯");
SetText("b=" + b);
advanced_regression(1, 0, 5, 10, 15);
SetText("左斜率=" + parameterB);
float k1;
k1 = parameterB;
byte y1 = 0;
byte x1 = 0;
if (Pixels[10, 0] != 0)
{
for (y = 11; y < 70; y++)
{
if (Pixels[y, 0] == 0)
{
y1 = y;
break;
}
}
}
else
{
cnt = 0;
for (y = 1; y < 70; y++)
{
if (Pixels[y, 0] != 0 && Pixels[y - 1, 0] == 0)
{
cnt = 1;
}
if (Pixels[y, 0] == 0 && Pixels[y - 1, 0] != 0 && cnt == 1)
{
cnt = 2;
y1 = y;
break;
}
}
for (y = y1; y < 70; y++)
{
if (Pixels[y1, 0] == 0)
break;
}
}
for (x = 0; x < 186; x++)
{
if (Pixels[y1, x] != 0)
{
righty[1] = y1;
rightx[1] = x;
SetText("右上拐点： y=" + righty[1] + " x=" + rightx[1]);
break;
}
if (x > 93)
{
righty[1] = y1;
rightx[1] = 0;
SetText("右上拐点： y=" + righty[1] + " x=" + rightx[1]);
break;
}
}
for (y = 5; y < 70; y++)
{
if (Pixels[y, 185] != 0)
{
lefty[0] = y;
leftx[0] = 185;
SetText("左下拐点： y=" + lefty[0] + " x=" + leftx[0]);
break;
}
if (y > 10)
{
lefty[0] = 5;
leftx[0] = 185;
SetText("左下拐点： y=" + lefty[0] + " x=" + leftx[0]);
break;
}
}
parameterB = (float)((rightx[1] - leftx[0]) * 1.0 / (righty[1] - lefty[0]));
parameterA = rightx[1] - parameterB * righty[1];
run(1, 0, righty[1], parameterB, parameterA);
for (y = 0; y < 70; y++)
{
R_black[y] = 0;
}
for (y = (byte)(righty[1]); y < 70; y++)
{
L_black[y] = L_black[righty[1] - 1];
}
run(0, 0, 69, parameterB, parameterA);
cnt = 0;
juge_lineContinuity(0, b, 3, -3, 2);
if (b > 50 && k1 >= -1.6 && k1 <= -0.5 && fps_cheku_times > fps_cheku_project)
{
outcarku = 0;
SetText("右边不为0 cnt=" + cnt + " 右边连续数=" + long_turn_flag_right);
SetText("退出出库状态机");
}
else
{
fps_cheku_times++;
}
}
总结
这些代码在正常速度的情况下能较准确的判别和补好线，但速度加大后由于车子的控制不好等原因，导致判断会比较慢，拉线比较晚，后续也要加强判断的条件，改良车子的控制。如果你觉得文章有所帮助还希望点赞鼓励一下！欢迎大家在评论区提出改进意见！
最后的最后如果想看看我这一年智能车的经历和建议可以到第一篇文章最下面浏览一下 智能车学习日记【一】