1.入环的函数
（1）搜上下边线
搜上下边线，处理圆环的时，可以利用上下边线的特点。
uint8_t UpdownSideGet(uint8_t imageInput[OV7725_UART_H][OV7725_UART_W], uint8_t imageOut[2][OV7725_UART_W])
{
uint8_t i = 0, j = 0;
uint8_t last = OV7725_UART_H/2;
imageOut[0][OV7725_UART_W-1] = 0;
imageOut[1][OV7725_UART_W-1] = OV7725_UART_H-1;
//从图像中间行 从中到下 从中到上 扫描
//处理中间单独那一列的上下边线
for(i = last; i >= 0; i--)
{
if(!imageInput[i][OV7725_UART_W/2])
{
imageOut[up][OV7725_UART_W/2] = i;
break;
}
}
for(i = last; i < OV7725_UART_H; i++)
{
if(!imageInput[i][OV7725_UART_W/2])
{
imageOut[down][OV7725_UART_W/2] = i;
break;
}
}
//其他列的上下边线
//从中到左
for(i = OV7725_UART_W/2-1; i > 0; i--)//遍历每一列
{
imageOut[up][i] = 0;
imageOut[down][i] = OV7725_UART_H-1;
for(j = imageOut[0][i+1] + 5; j > 0; j--)//一列中的扫描每行 从上列的行数+10开始向上扫描
{
if(!imageInput[j][i])
{
imageOut[up][i] = j;
break;
}
}
for(j = imageOut[1][i+1] - 5; j < OV7725_UART_H; j++)
{
if(!imageInput[j][i])
{
imageOut[down][i] = j;
break;
}
}
}
//从中到右
for(i = OV7725_UART_W/2+1; i < OV7725_UART_W-1; i++)
{
imageOut[up][i] = 0;
imageOut[down][i] = OV7725_UART_H-1;
for(j = imageOut[0][i-1] + 5; j > 0; j--)
{
if(!imageInput[j][i])
{
imageOut[up][i] = j;
break;
}
}
for(j = imageOut[1][i-1] - 5; j < OV7725_UART_H; j++)
{
if(!imageInput[j][i])
{
imageOut[down][i] = j;
break;
}
}
}
return 0;
}
（2）找凸起的弧
找凸起的弧，用于圆环的检测。如下图红色的线。
RoundaboutGetArc函数中传入的num代表着，要检测的这个圆弧大小，要求这个圆弧包含多少个点。
以左圆环为例：
先判断边线丢不丢线，不丢线再进行下一步。由于是遍历一幅图像左边线数组的每一行，我们就找这样一个特征：它下面连续递减点的个数+它上面连续递减点的个数+跟它横坐标一样大的点的个数 >=我们设定的值。这个就是我们认为的圆弧
/*!
* @brief 判断左右边线是否存在弧形
* 输出的 index 圆弧的顶点位置
* @param imageInput ： 二值图像信息
* @param imageOut ： 边线数组
* @param status ： 1：左边线 2：右边线
* @param num ： 圆弧的大小 用点数表示 （连续N个增 连续N个减）
* @return 1 有弧线 0 没弧线
*/
uint8_t RoundaboutGetArc(uint8_t imageSide[OV7725_UART_H][2], uint8_t status, uint8_t num,uint8_t* index)
{
int i = 0;
uint8_t inc = 0, dec = 0, n = 0;
switch(status)
{
case 1:
for(i = START_H-2; i > END_H; i--)
{
//没有丢线
if(imageSide[i][0] != 1 && imageSide[i+1][0] != 1)
{
if(imageSide[i][0] == imageSide[i+1][0])
{
n++;
continue;
}
if(imageSide[i][0] > imageSide[i+1][0])
{
inc++;
inc+=n;
n=0;
}
else
{
dec++;
dec+=n;
n=0;
}
/* 有弧线 */
if(inc > num && dec > num)
{
*index = i + num;
return 1;
}
}
else
{
inc = 0;
dec = 0;n=0;
}
}
break;
case 2:
for(i = START_H-2; i > END_H; i--)
{
if(imageSide[i][1] != OV7725_UART_W-1 && imageSide[i+1][1] != OV7725_UART_W-1)
{
if(imageSide[i][1] == imageSide[i+1][1])
{
n++;
continue;
}
if(imageSide[i][1] > imageSide[i+1][1])
{
inc++;
inc+=n;
n = 0;
}
else
{
dec++;
dec+=n;
n=0;
}
/* 有弧线 */
if(inc > num && dec > num)
{
*index = i + num;
return 1;
}
}
else
{
inc = 0;
dec = 0;n=0;
}
}
break;
}
return 0;
}
（3）两点之间补线
/*!
* @brief 补线处理
*
* @param imageSide : 边线
* @param status : 1：左边线补线 2：右边线补线
* @param startX : 起始点 列数
* @param startY : 起始点 行数
* @param endX : 结束点 列数
* @param endY : 结束点 行数
*
* @return
*
* @note endY 一定要大于 startY
*
*/
void ImageAddingLine(uint8_t imageSide[OV7725_UART_H][2], uint8_t status, uint8_t startX, uint8_t startY, uint8_t endX, uint8_t endY)
{
int i = 0;
// 直线 x = ky + b
float k = 0.0f, b = 0.0f;
switch(status)
{
case 1://左补线
{
k = (float)((float)endX - (float)startX) / (float)((float)endY - (float)startY);
b = (float)startX - (float)startY * k;
for(i = startY; i < endY; i++)
{
imageSide[i][0] = (uint8_t)(k * i + b);
}
break;
}
case 2://右补线
{
k = (float)((float)endX - (float)startX) / (float)((float)endY - (float)startY);
b = (float)startX - (float)startY * k;
for(i = startY; i < endY; i++)
{
imageSide[i][1] = (uint8_t)(k * i + b);
}
break;
}
}
}
（4）判断上线是否单调
/*!
* @brief 判断上边线是否单调
* @param X1 :起始X点
* @param X2 :终止X点 X1 < X2
* @param imageIn ： 边线数组
*
* @return 0：不单调or错误， 1：单调递增， 2：单调递减
*
* @note
*
* @see
*
* @date 2021/11/30 星期二
*/
uint8_t RoadUpSide_Mono(uint8_t X1, uint8_t X2, uint8_t imageIn[2][OV7725_UART_W])
{
uint8_t i = 0, num = 0;
for(i = X1; i < X2-1; i++)
{
if(imageIn[0][i] >= imageIn[0][i+1])
num++;
else
num = 0;
if (num >= (X2-X1)*4/5)
return 1;
}
for(i = X1; i < X2-1; i++)
{
if(imageIn[0][i] <= imageIn[0][i+1])
num++;
else
num = 0;
if (num >= (X2-X1)*4/5)
return 2;
}
return 0;
}
2.找圆环
找圆环的特征，以左圆环为例，要求“左边线有弧，右边线单调”。左边线有弧已经说过了，这里的“右边线单调”：需要右边线在一定的横坐标范围内单调的点数大于我们设定的值，才认为是这里的“右边线单调”。
uint8_t RoadIsRoundabout(uint8_t Upimage[2][OV7725_UART_W], uint8_t imageInput[OV7725_UART_H][OV7725_UART_W], uint8_t image[OV7725_UART_H][2], uint8_t *flag)
{
uint8_t i = 0;
errL=0, errR=0;
leftState = 0, rightState = 0;
count = 0;
uint8_t num = 0, py;
// 从车头往前 左边线是否单调
for(i = START_H-2; i > END_H; i--)
{
if(image[i][0] == 1)
continue;
if(image[i][0] >= image[i+1][0]) // i是Y坐标值 0 是图像左线X坐标
{
if(image[i][0]<OV7725_UART_W/2 - 5)
num++;
else
num = 0 ;
if(num == 50)
{
num = 0;
leftState = 1; // 左单调标志
break;
}
}
else
{
num = 0;
}
if(i <= END_H+1) // 清0
num = 0;
}
errL = RoundaboutGetArc(image, 1, round_size, &py);
errR = RoundaboutGetArc(image, 2, round_size, &py);
// 右边线是否单调
for(i = START_H-2; i > END_H; i--)
{
if(image[i][1] == OV7725_UART_W-1)
continue;
if(image[i][1]<= image[i+1][1])
{
if(image[i][1]>OV7725_UART_W/2 + 5)
num++;
else
num = 0 ;
if(num == 50)
{
num = 0;
rightState = 1;
break;
}
}
else
{
num = 0;
}
if(i <= END_H+1)
num = 0;
}
// 左边单调， 检测右侧是否是环岛
if(leftState == 1 && rightState == 0 && errL == 0)
{
count = 0;
if(RoundaboutGetArc(image, 2, round_size, &count))
{
*flag = 1;
return 1;
}
else
{
return 0;
}
}
/* 右边单调， 检测左侧是否是环岛 */
if(rightState == 1 && leftState == 0&& errR == 0)
{
count = 0;
if(RoundaboutGetArc(image, 1, round_size, &count))
{
*flag = 2;
return 2;
}
}
return 0;
}
3.补线入环出环
右环为例，左环也是一样，对称写（右环为奇数状态，左环为偶数状态）。目前已经找到圆环，进入状态1。
状态1：找到圆环后，补一条线，保证车子直走。一直检查圆弧还在不在，直到弧消失了，就进入状态3。
状态3：让车子一直往右拐，一直拐到上线单调的时候。进入状态5
这个还没拐成，要继续拐。
拐到这种时候，就已经进入圆环口了，可以不用拐了。
在圆环内，都是这种图像，正常走就行了，之后的状态就是出圆环了。
状态5：出圆环，找右边凸起的位置+下线全是最低点这种情况，然后补一条线，让车子向左拐。一直拐到上线单调的时候，进入状态7.
一直拐
拐到上线单调了，进入状态7。
状态7：加大力度拐，拐到上线不单调的时候，就出圆环正常行驶了。
参考代码
具体要根据车子的速度和摄像头选择图像的大小来确定里面的参数
void RoundaboutProcess(uint8_t imageInput[OV7725_UART_H][OV7725_UART_W], uint8_t imageSide[OV7725_UART_H][2], uint8_t UpdowmSide[2][OV7725_UART_W], uint8_t* state)
{
uint8_t i = 0, err5 = 0;
uint8_t pointX = 0, pointY = 0, inc = 0, dec = 0;
uint8_t flag= 0, Down_flag = 0;
static uint8_t finderr = 0;
static uint8_t err1 = 0;
switch(*state)
{
//奇数为入右圆环
case 1:
// 检查弧线
err1 = RoundaboutGetArc(imageSide, 2, 3, &pointY);
// 有弧线 进行补线 连接弧线最右点 和 图像左下角
if(err1)
{
pointX = imageSide[pointY][1];
ImageAddingLine(imageSide, 2, pointX, pointY, OV7725_UART_W-1, START_H);
finderr = 1;
}
else
{
if(finderr)
*state = 3;//准备进入环岛
}
break;
/* 发现左环岛 环岛出口处补线 */
case 3:
for(i=1;i<OV7725_UART_H-1;i++)
{
ImageSide[i][0]= ImageSide[i][0]+50;
}
if(RoadUpSide_Mono(30, OV7725_UART_W-1,UpdowmSide) == 1)//上线单调增进入下一步
*state = 5;
break;
case 5 :
err5 = RoundaboutGetArc(imageSide, 1, 10, &pointY);
//检查下线
for(i = OV7725_UART_W-1; i > 0; i--)
{
if(UpdowmSide[1][i] == 119)
inc++;
else
dec++;
if( dec <= 15)
{
Down_flag = 1;
break;
}
}
flag = RoadUpSide_Mono(20, OV7725_UART_W,UpdowmSide);
if(flag && err5 && Down_flag)
{
*state = 7;
}
break;
case 7:
ImageAddingLine(imageSide, 1, 80, 10, 0, START_H);
flag = RoadUpSide_Mono(50, OV7725_UART_W,UpdowmSide);
if(flag==0)
{
*state = 0;
finderr = 0;
err1 = 0;
}
有问题可以私聊我咯~
可以加企鹅群 658476482 交流
另外承接各种单片机设计~
1万+
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
