第20届缩微光电开源—循迹部分
·
在刚刚过去的20届智能车竞赛中，本人负责软件的所有工作，在这个过程中学习到了无数宝贵的知识和经验，折线镜头组不知道以后还会不会出现，但是还是开源出来，希望能帮助到有需要的人。
在这届缩微光电的比赛中，我见到的车大多数都是用的镜头，自己也是镜头循迹的。循迹主要是靠青山佬开源的元素行元素列方法，具体的算法详见下面这个链接，这个算法真是非常顶级，青山佬讲的也是非常好，在这里我只把我用元素行元素列处理元素的方法开源出来供大家参考。
【20届智能车竞赛|走进缩微组别-青山和你一起开启智能车之旅!】 https://www.bilibili.com/video/BV1RkoQYsEQu/?share_source=copy_web&vd_source=b3d6e49592556ffe946be56f617991c3
1、基本循迹
图像处理的本质其实就是处理一个二维的图像数组，以常见的120*188图像为例，定义一个image【120】【188】数组。数组的每个值就是一个0—255的灰度值，我们对这一帧图像提取边线信息进行处理，就可以进行循迹了。我用的是大津法二值化循迹，在CSDN有相当多的教程，这里就不举例了。在二值化之后，我们就得到了数值只有0和255的图像。我的摄像头高度离地15cm，前瞻30cm，前瞻这么短是因为不容易窜道，当然长一些的话也是可以的，只是需要用软件方式屏蔽窜道的干扰
1.1扫线
虽然我用的是扫线，但是对于缩微赛道还是不推荐，因为缩微赛道元素密集，对于一帧图像而言信息量太大了，对于我而言，扫线就是提取一下赛道的基本的丢线信息，几乎不会产生循迹权重，由于用到了，还是说一下，这里更推荐八领域或者迷宫法，更跟线，并且可以更好提取横向信息。
//左右扫边线，这里有时只是提取丢线信息
void findline_left_right(void) {
int16 i, j;
lose_flag = 0;
for(i = IMG_H-1; i > 5; i--) {
Findline.midline[i] = IMG_W/2;
Findline.leftline[i] = 0;
Findline.rightline[i] = IMG_W-1; // 139
Findline.leftlineflag[i] = Findline.rightlineflag[i] = 0;
Findline.lose_flag[i]=0;
if(i == IMG_H-1) { // 首行处理
// 左边界搜索列：5-50（原10-180）
for(j = 1; j <IMG_W; j++) {
if(bin_image[i][j] == 255 && bin_image[i][j-1] == 0) {
Findline.leftline[i] = j-1;
Findline.leftlineflag[i] = 1;
break;
}
}
for(j = 1; j < IMG_W; j++) {
if(bin_image[i][j] == 0 && bin_image[i][j-1] == 255) {
Findline.rightline[i] = j;
Findline.rightlineflag[i] = 1;
break;
}
}
} else { // 其他行
int16 mid_prev = Findline.midline[i+1];
// 左线搜索范围：上一行中线±30列
for(j =mid_prev-30; j < mid_prev+30 ; j++) {
if(bin_image[i][j] == 255 && bin_image[i][j-1] == 0) {
Findline.leftline[i] = j-1;
Findline.leftlineflag[i] = 1;
break;
}
}
// 右线搜索范围
for(j = mid_prev+30; j > mid_prev-30; j--) {
if(bin_image[i][j] == 0 && bin_image[i][j-1] == 255) {
Findline.rightline[i] = j;
Findline.rightlineflag[i] = 1;
break;
}
}
}
// 丢线记录
if(!Findline.leftlineflag[i] && !Findline.rightlineflag[i]) {
Findline.lose_flag[i]++;//这一行的丢线数据
lose_flag++;
Findline.midline[i] = Findline.midline[i+1];
} else if(Findline.leftlineflag[i] && Findline.rightlineflag[i]) {
Findline.width[i] = Findline.rightline[i] - Findline.leftline[i];
}
Findline.midline[i] = (Findline.leftline[i] + Findline.rightline[i]) / 2;
}
}这个就是比较基本的扫左右边线，最底行开始扫第一行扫的范围大一点，再往上就由上一行的中线位置开始扫，扫不到就记为丢线。左右边线的差可以当作宽度，可以根据这个写停车标志停车。
这里我的扫线函数基本上只是用来提取丢线信息而已，所以完全可以简化，那就是隔5行或者10行扫一次这样，不过扫的范围要大一点，这样可以简化很多资源，节省搜线时间。这是我省赛比完之后突然想到的，具体的函数就没写。
1.2丢线情况
//丢线处理函数，这里旨在处理虚线和窜道
void loseline_handler(void)
{
// 定义区域阈值
#define UP_ZONE_START 0
#define UP_ZONE_END 30// 上部区域
#define MID_ZONE_START 31
#define MID_ZONE_END 55 // 中部区域
#define DOWN_ZONE_START 56
#define DOWN_ZONE_END 99 // 下部区域
// 丢线计数阈值
#define LOSE_THRESHOLD 19
// 重置丢线标志
up_lose_flag = 0;
mid_lose_flag = 0;
down_lose_flag = 0;
// 统计上部区域丢线情况
for (int i = UP_ZONE_START; i <= UP_ZONE_END; i++) {
if (Findline.lose_flag[i] > 0) {
up_lose_flag++;
}
}
// 统计中部区域丢线情况
for (int i = MID_ZONE_START; i <= MID_ZONE_END; i++) {
if (Findline.lose_flag[i] > 0) {
mid_lose_flag++;
}
}
// 统计下部区域丢线情况
for (int i = DOWN_ZONE_START; i <= DOWN_ZONE_END; i++) {
if (Findline.lose_flag[i] > 0) {
down_lose_flag++;
}
}
// 判断丢线状态
if (up_lose_flag >= LOSE_THRESHOLD &&
mid_lose_flag >= LOSE_THRESHOLD) {
// 上中部丢线 - 急弯
LOSE = UP_MID_LOSE;
}
else if (up_lose_flag >= LOSE_THRESHOLD) {
// 上部丢线 - 直角或弯道
LOSE = UP_LOSE;
}
else {
// 无显著丢线，直道或者折线，前瞻前置
LOSE = NONE_LOSE;
}
}
后面通过处理赛道的丢线的情况，可以将其分为这几种，根据丢线情况更好的处理权重和元素。在这里还是延续上面的话，在简化之后其实不需要处理这么多，间隔处理就可以。
二、元素
2.1元素行元素列
由于缩微赛道的特殊性，使用特殊的扫线方法往往能更好地提取赛道的信息。这里不对元素行元素列做讲解，想了解的还是看上面的视频的视频链接就好（再次膜拜青山佬）。
void conduct_line_search(void) {
// 重置变量
left_conduct_line = right_conduct_line = up_conduct_line = down_conduct_line = 0;
left_conduct_flag = right_conduct_flag = 0;
up_conduct_flag_1 = down_conduct_flag_1 = up_conduct_flag_2 = down_conduct_flag_2 = 0;
left_y = right_y = up_x1 = up_x2 = down_x1 = down_x2 = 0;
// 寻找左右引导线***********************************************
for(int m=IMG_H-10;m>10;m--)//从下向上寻找
{
for(int n=1;n<25;n++)//图像左侧寻找20列
{
if(bin_image[m][n]==0&&bin_image[m+1][n]==255)// 寻找上下黑白跳变点
{
left_conduct_flag++;
left_y = m;
continue;
}
}
for(int n=IMG_W;n>IMG_W-25;n--)
{
if(bin_image[m][n]==0&&bin_image[m+1][n]==255)// 寻找上下黑白跳变点
{
right_conduct_flag++;
right_y = m;
continue;
}
}
}
// 左右引导线赋值**************
if(left_conduct_flag>=3)//引导线判断位采取累加，防止误判
{
left_conduct_line=1;
}
else
{
left_conduct_line=0;
}
if (right_conduct_flag>=3)
{
right_conduct_line=1;
}
else
{
right_conduct_line=0;
}
// 下引导线搜索 - 使用点集存储并计算平均值
Point down_x1_points[IMG_H] = {0};
Point down_x2_points[IMG_H] = {0};
uint8 down_x1_count = 0, down_x2_count = 0;
for(int m = IMG_H-1; m > IMG_H-25; m--) {
uint8 search_cnt = 0;
for(int n = 0; n < IMG_W; n++) {
if(bin_image[m][n] == 255 && bin_image[m][n-1] == 0) {
if(search_cnt == 0) {
// 记录第一个跳变点
down_x1_points[down_x1_count].row = m;
down_x1_points[down_x1_count].col = n;
down_x1_count++;
down_conduct_flag_1++;
} else if(search_cnt == 1) {
// 记录第二个跳变点
down_x2_points[down_x2_count].row = m;
down_x2_points[down_x2_count].col = n;
down_x2_count++;
down_conduct_flag_2++;
break;
}
search_cnt++;
}
}
}
// 处理 down_x1 引导线 - 计算平均值
if(down_x1_count > 0) {
uint16 sum_col = 0;
for(int i = 0; i < down_x1_count; i++) {
sum_col += down_x1_points[i].col;
}
down_x1 = sum_col / down_x1_count;
} else {
down_conduct_flag_1 = 0;
}
// 处理 down_x2 引导线 - 计算平均值
if(down_x2_count > 0) {
uint16 sum_col = 0;
for(int i = 0; i < down_x2_count; i++) {
sum_col += down_x2_points[i].col;
}
down_x2 = sum_col / down_x2_count;
} else {
down_conduct_flag_2 = 0;
}
// 下引导线判断
if(down_conduct_flag_2 >= 7) down_conduct_line = 2;
else if(down_conduct_flag_1 >= 7) down_conduct_line = 1;
else down_conduct_line = 0;
Point up_x1_points[IMG_H] = {0};
Point up_x2_points[IMG_H] = {0};
uint8 up_x1_count = 0, up_x2_count = 0;
for(int m = 10; m < 22; m++) {
uint8 search_cnt = 0;
for(int n = 12; n < IMG_W-12 ; n++) {
if(bin_image[m][n] == 255 && bin_image[m][n-1] == 0) {
if(search_cnt == 0) {
up_x1_points[up_x1_count].row = m;
up_x1_points[up_x1_count].col = n;
up_x1_count++;
up_conduct_flag_1++;
} else if(search_cnt == 1) {
up_x2_points[up_x2_count].row = m;
up_x2_points[up_x2_count].col = n;
up_x2_count++;
up_conduct_flag_2++;
break;
}
search_cnt++;
}
}
}
// 处理 up_x1 引导线
if(up_x1_count > 0) {
uint16 sum_col = 0;
for(int i = 0; i < up_x1_count; i++) {
sum_col += up_x1_points[i].col;
}
up_x1 = sum_col / up_x1_count;
up_conduct_line = 1;
} else {
up_conduct_flag_1 = 0;
}
// 处理 up_x2 引导线
if(up_x2_count > 0) {
uint16 sum_col = 0;
for(int i = 0; i < up_x2_count; i++) {
sum_col += up_x2_points[i].col;
}
up_x2 = sum_col / up_x2_count;
up_conduct_line = 2;
} else {
up_conduct_flag_2 = 0;
}
}
这样扫完元素行元素列，我们就得到了上下左右元素行元素列的情况 。（upx1 x2是上引导线的两个坐标，down同理，right_y和left_y是左右元素列的坐标）。处理得到元素行元素列之后，我们就可以根据这些信息处理元素了。
2.2拉线
所谓拉线，或者是补线，其实就是通过连接特征点，给中线数组直接赋值。全程拉线有个好处就是，几乎所有的虚线都不需要处理，所以我虚线处理几乎没写。
void connect_line(int x1, int y1, int x2, int y2) {
// 计算步数（取x和y方向的最大差值）
int dx = x2 - x1;
int dy = y2 - y1;
int steps = MAX(abs(dx), abs(dy));
// 如果两个点重合，直接设置并返回
if (steps == 0) {
if (y1 >= 0 && y1 < IMG_H) {
Findline.midline[y1] = x1;
Findline.lose_flag[y1] = 0;
}
return;
}
// 计算每一步的增量
float xIncrement = (float)dx / steps;
float yIncrement = (float)dy / steps;
// 从起点到终点绘制线段
float x = x1;
float y = y1;
for (int i = 0; i <= steps; i++) {
int ix = (int)(x + 0.5); // 四舍五入
int iy = (int)(y + 0.5); // 四舍五入
// 检查是否在有效范围内
if (iy >= 0 && iy < IMG_H) {
Findline.midline[iy] = ix;
Findline.lose_flag[iy] = 0;
}
// 移动到下一个点
x += xIncrement;
y += yIncrement;
}
}2.3直角
这里给出两种直角的拉线方法，都很合理，要看如何理解和使用
这是我省赛时使用的拉线方法，就是从底部拉线到元素列扫到的坐标，如果是右直角那么就是右边的这个点，左直角同理。下面这个函数给直角分级，结合我上面的丢线状况，UP_MID_LOSE也就是上部和中部都丢线，这种情况是比较急的弯了，给标志位赋值为2，普通上元素行扫不到那就是普通直角。
//2是急弯，1是缓和弯，赋值前瞻用
void rt_judge(void) {
if(LOSE==UP_MID_LOSE)//窜道急弯
{ //这种可能一定有一边是急弯道，因此不写else
if(left_conduct_line && right_conduct_line)
{
if(left_y>right_y)
{
l_rt_flag = 2;
r_rt_flag = 0;
}
else{
r_rt_flag = 2;
l_rt_flag = 0;
}
}
else if(left_conduct_line && !right_conduct_line) {l_rt_flag = 2; r_rt_flag = 0; }
else if(!left_conduct_line && right_conduct_line) {r_rt_flag = 2; l_rt_flag = 0; }
// 直角中线赋值
if(l_rt_flag) {
connect_line ( IMG_W/2,IMG_H,0,left_y);
}
if(r_rt_flag) {
connect_line( IMG_W/2,IMG_H,IMG_W,right_y);
}
}
else if(up_conduct_line==0) {
if(left_conduct_line && !right_conduct_line){l_rt_flag = 1; r_rt_flag = 0; }
else if(!left_conduct_line && right_conduct_line) {r_rt_flag = 1; l_rt_flag = 0; }
else l_rt_flag = r_rt_flag = 0;
if(l_rt_flag) {
connect_line ( IMG_W/2,IMG_H,0,left_y);
}
if(r_rt_flag) {
connect_line( IMG_W/2,IMG_H,IMG_W,right_y);
}
}
else if(LOSE==NONE_LOSE)
{
l_rt_flag = 0;
r_rt_flag = 0;
}
} 
第二种拉线方法是这样的 ，直接在扫到的right_y上向上拉一条直线上去，如果是用中线权重数组赋值算权重可以选这种，离得越近，拉出来的线算出的权重就越大，也是挺好用的。后来省赛没用这个，代码就先懒得写了，就是直接给中线数组赋值。
2.4环岛
2.4.0 大体思路
typedef enum {
NONE = 0,
IN = 1,
RUN = 2,
OUT = 3,
RETURN = 4
} circle_state;
circle_state STATE = NONE;这里我用了一个枚举状态机，如果想debug，把这个枚举当整数打在屏幕上就可以了。这里我给状态机分了五个状态，无环岛，入环，环内，出环，回归循迹。
2.4.1环岛类型判断
用元素行元素列也是可以秒环岛和虚线环岛的。扫到两个上引导线和下引导线（建议加个多帧检测到才判环岛，防误判）判断环岛类型的话有两个方法，第一种是判断图像左边和右边的黑白跳变点数量，选多的那个判断类型。如下
void circle_type_judge(void)
{
uint8 l_cir_flag = 0, r_cir_flag = 0;
if(!type_rec_flag)
{
for(int m = 0; m < IMG_H ;m++) // 从下向上寻找
{
// 左侧区域 (40-60列)
for(int n = 50; n < 70; n++)
{
if(bin_image[m][n] == 0 && bin_image[m+1][n] == 255) // 寻找上下黑白跳变点
{
l_cir_flag++;
}
}
// 右侧区域 (100-120列)
for(int n = 110; n > 90; n--)
{
if(bin_image[m][n] == 0 && bin_image[m+1][n] == 255) // 寻找上下黑白跳变点
{
r_cir_flag++;
}
}
}
#define MIN_CIRCLE_FLAG 5 // 最小有效标志数
if(l_cir_flag > r_cir_flag) {
if(l_cir_flag >= MIN_CIRCLE_FLAG) {
circle_type = 1; // 左环岛
} else {
circle_type = 0; // 数量不足
}
} else if(r_cir_flag > l_cir_flag) {
if(r_cir_flag >= MIN_CIRCLE_FLAG) {
circle_type = 2; // 右环岛
} else {
circle_type = 0; // 数量不足
}
} else {
// 左右标志数相等
circle_type = 0;
}
}
}还有一种方法是连接上坐标1和下坐标1，上坐标2和下坐标2，计算这两个连线上的坐标，根据坐标向左右寻找白点，很明显，白点多的一条线就是K字的直边。从而判断左右环岛，这种判断方法理想情况是可以判断斜入环岛的情况的。方法二如下：
void circle_type_judge(void)
{
if(!type_rec_flag && STATE == IN)
{
// 计算两条连接线
uint8 line1[IMG_H] = {0};
uint8 line2[IMG_H] = {0};
calculate_line_points(up_x1, 0, down_x1, IMG_H-1, line1);
calculate_line_points(up_x2, 0, down_x2, IMG_H-1, line2);
uint8 valid_rows1 = 0;
uint8 valid_rows2 = 0;
// 验证第一条连接线
for (uint8 y = 0; y < IMG_H; y++) {
uint8 x_center = line1[y];
if (x_center == 0) continue;
uint8 white_count = 0;
uint8 left = (x_center > CIR_SEARCH) ? (x_center - CIR_SEARCH) : 0;
uint8 right = (x_center + CIR_SEARCH < IMG_W) ? (x_center + CIR_SEARCH) : IMG_W-1;
for (uint8 x = left; x <= right; x++) {
if (bin_image[y][x] == 255) {
white_count++;
}
}
if (white_count > WHITE_THRESHOLD) {
valid_rows1++;
}
}
// 验证第二条连接线
for (uint8 y = 0; y < IMG_H; y++) {
uint8 x_center = line2[y];
if (x_center == 0) continue;
uint8 white_count = 0;
uint8 left = (x_center > CIR_SEARCH) ? (x_center - CIR_SEARCH) : 0;
uint8 right = (x_center + CIR_SEARCH < IMG_W) ? (x_center + CIR_SEARCH) : IMG_W-1;
for (uint8 x = left; x <= right; x++) {
if (bin_image[y][x] == 255) {
white_count++;
}
}
if (white_count > WHITE_THRESHOLD) {
valid_rows2++;
}
}
// 判断环岛类型
#define DIFF_THRESHOLD 10 // 有效行数差异阈值
if (valid_rows1 > valid_rows2 + DIFF_THRESHOLD) {
circle_type = 2; // 右环岛
type_rec_flag = 1;
}
else if (valid_rows2 > valid_rows1 + DIFF_THRESHOLD) {
circle_type = 1; // 左环岛
type_rec_flag = 1;
}
else {
// 有效行数相近，清除环岛状态
STATE = NONE;
circle_type = 0;
type_rec_flag = 0;
}
}
}void calculate_line_points(int x1, int y1, int x2, int y2, uint8 *line_array) {
// 清除之前的连接线数据
memset(line_array, 0, sizeof(uint8) * IMG_H);
// 计算线段参数
float dx = x2 - x1;
float dy = y2 - y1;
// 处理垂直线段
if (dx == 0) {
int min_y = (y1 < y2) ? y1 : y2;
int max_y = (y1 > y2) ? y1 : y2;
for (int y = min_y; y <= max_y; y++) {
if (y >= 0 && y < IMG_H) {
line_array[y] = x1;
}
}
return;
}
// 计算斜率
float slope = dy / dx;
// 根据斜率方向选择计算方式
if (fabs(slope) < 1.0f) {
// 水平方向为主
int min_x = (x1 < x2) ? x1 : x2;
int max_x = (x1 > x2) ? x1 : x2;
for (int x = min_x; x <= max_x; x++) {
int y = y1 + (int)((x - x1) * slope);
if (y >= 0 && y < IMG_H) {
line_array[y] = x;
}
}
} else {
// 垂直方向为主
int min_y = (y1 < y2) ? y1 : y2;
int max_y = (y1 > y2) ? y1 : y2;
for (int y = min_y; y <= max_y; y++) {
int x = x1 + (int)((y - y1) / slope);
if (x >= 0 && x < IMG_W && y >= 0 && y < IMG_H) {
line_array[y] = x;
}
}
}
}2.4.2入环补线
入环补线我其实尝试过两种办法，一种是补固定线（也就是补线到一个固定位置，靠陀螺仪出状态）和固定打角其实是一个思路，第二个办法是直接根据上元素行扫到的坐标，比如你元素行是从左往右扫的，扫到的第一个是up1，第二个是up2，如果是右环岛就要连up2，左环岛连up1。这两个方法在无虚线环岛上的效果相差不大，有虚线的环岛，第一种方法鲁棒性更好，虚线贴的再恶心，固定打角不会受影响。
2.4.3环内
在环岛内正常循迹就可以，我对环岛做了一些拉线和权值上的处理，可以内切一点偷路径，不过用处不大。
2.4.4出环
根据陀螺仪积分，在300度左右就可以出环了，可以在右上角1/4的位置寻找白点位置，然后取个平均，进行一个拉线。
2.4.5回到正常循迹
我回到正常循迹的条件是下元素行扫到的数量不为2（也就是图像下边看不见环岛上面的那个“V”），在不满足这个条件的时候之前，是一直从图像底面1/2处拉线到up1的。（一般来说，是默认出了环岛是个直线的，出环秒接个直角这种没试过，反正折线和直线没问题）
整体代码如下：
这里我是有个环岛锁的，也就是出环一段时间内不能入环。环岛里的陀螺仪判断条件是积分，可以用自己的，这边我把我的贴出来做个参考。
void circle_handler(void) {
if(circle_lock && circle_lock_counter > 0) {
circle_lock_counter--;
if(circle_lock_counter == 0) {
circle_lock = 0; // 解锁
}
}
char str[10];
static uint8 exit_circle_counter;
if(down_conduct_line==2&&up_conduct_line==2&&circle_type==0)
{
connect_line(IMG_W/2, IMG_H-1, (up_x1+up_x2)/2, 0);
}
if(down_conduct_line==2&&up_conduct_line==1)//只有圆环出环会出现的图像情况
{
connect_line(IMG_W/2, IMG_H-1, up_x1, 0);
}
if(STATE==IN||STATE==RUN||STATE==OUT||STATE==RETURN) {
yaw_rec_flag=1;
}
// 条件检测****************
if(up_conduct_line==2&&down_conduct_line==2&&abs(up_x1-up_x2)>15&&abs(up_x1-up_x2)<50&&abs(down_x1-down_x2)>15&&!circle_lock) {
if(STATE==NONE) {
circle_in_counter++; // 条件满足，增加计数
if (circle_in_counter >= CIRCLE_IN_THRESHOLD) {
STATE = IN; // 连续多帧满足条件，进入环岛状态
circle_in_counter = 0; // 重置计数器
}
} else {
circle_in_counter = 0; // 条件不满足，重置计数器
}
}
// OUT状态识别
if(STATE == OUT) {
// 右环岛：寻找右上方白点
if(circle_type == 2) {
uint16 sum_x = 0;
uint16 sum_y = 0;
uint16 count = 0;
// 缩小搜索范围：右上1/4区域的前半部分 (x: 105-139, y: 0-19)
for(int y = 0; y < 20; y++) {
for(int x = 105; x < IMG_W; x++) {
if(bin_image[y][x] == (uint8)RGB565_WHITE) {
sum_x += x;
sum_y += y;
count++;
}
}
}
if(count > 0) {
// 计算白点坐标平均值
uint8 avg_x = sum_x / count;
uint8 avg_y = sum_y / count;
// 从屏幕底部中心点连接平均白点位置
connect_line(IMG_W/2, IMG_H-1, avg_x, avg_y);
}
}
// 左环岛：寻找左上方白点
else if(circle_type == 1) {
uint16 sum_x = 0;
uint16 sum_y = 0;
uint16 count = 0;
// 缩小搜索范围：左上1/4区域的前半部分 (x: 0-34, y: 0-19)
for(int y = 0; y < 20; y++) {
for(int x = 0; x < 35; x++) {
if(bin_image[y][x] == (uint8)RGB565_WHITE) {
sum_x += x;
sum_y += y;
count++;
}
}
}
if(count > 0) {
// 计算白点坐标平均值
uint8 avg_x = sum_x / count;
uint8 avg_y = sum_y / count;
// 从屏幕底部中心点连接平均白点位置
connect_line(IMG_W/2, IMG_H-1, avg_x, avg_y);
}
}
// 检查是否进入RETURN状态
if(up_conduct_line ==2 ||fabs(sum_yaw)>340){
STATE = RETURN;
}
}
if(STATE == RETURN) {
if(circle_type == 2) { // 右环岛出环
uint16 jump_sum_x = 0;
uint16 jump_count = 0;
for(int y = 0; y < 20; y++) {
for(int x = 0; x<IMG_W-1; x++) {
if(bin_image[y][x] == 0 && bin_image[y][x-1] == 255) {
jump_sum_x += x; // 跳变点位置
jump_count++;
break; // 每行只取一个跳变点
}
}
}
if(jump_count > 0) {
uint8 avg_jump_x = jump_sum_x / jump_count;
connect_line(IMG_W/2, IMG_H-1, avg_jump_x, 10); // 使用中间行高度
}
}
else if(circle_type == 1) { // 左环岛出环
uint16 jump_sum_x = 0;
uint16 jump_count = 0;
for(int y = 0; y < 20; y++) {
for(int x = IMG_W-1; x > 0; x--) {
if(bin_image[y][x] == 255 && bin_image[y][x-1] == 0) {
jump_sum_x += (x-1); // 跳变点位置
jump_count++;
break; // 每行只取一个跳变点
}
}
}
if(jump_count > 0) {
uint8 avg_jump_x = jump_sum_x / jump_count;
connect_line(IMG_W/2, IMG_H-1, avg_jump_x, 10); // 使用中间行高度
}
}
if(down_conduct_line != 2&&up_conduct_line!=2&&fabs(sum_yaw)>350) {
// 增加退出计数器
exit_circle_counter++;
// 连续10帧检测到下引导线不为2才退出
if(exit_circle_counter >= 3) {
STATE = NONE;
sum_yaw = 0;
yaw_rec_flag = 0;
type_rec_flag = 0;
circle_type = 0;
first_yaw_rec = 1;
exit_circle_counter = 0; // 重置计数器
// +++ 新增：激活环岛锁定 +++
circle_lock = 1;
circle_lock_counter = CIRCLE_LOCK_FRAMES;
}
} else {
// 如果下引导线重新出现，重置计数器
exit_circle_counter = 0;
}
}
// 状态转换条件
if(fabs(sum_yaw) > 15&& STATE == IN) {
STATE = RUN;
}
if(fabs(sum_yaw) > 300 && STATE == RUN) {
STATE = OUT;
}
// 类型判断*****************
if(STATE == IN) {
circle_type_judge();
type_rec_flag = 1;
}
// 中线赋值*****************
if(circle_type == 2) { // 右环岛
switch(STATE) {
case IN:
connect_line(IMG_W/2, IMG_H-10,image_par.r_cir_in ,0);
break;
case RUN:
if(up_conduct_line == 0 && !right_conduct_line) {
// 在顶部20行搜索黑白跳变点（白到黑）
uint16_t sum_x = 0;
uint16_t point_count = 0;
for(int y = 0; y < 20; y++) {
// 从右向左搜索 (100-139列)
for(int x = IMG_W-1; x >= 0; x--) {
if(bin_image[y][x] == 0 && bin_image[y][x-1] == 255) {
sum_x += x; // 记录跳变点位置
point_count++;
break; // 每行只取一个跳变点
}
}
}
if(point_count > 0) {
// 计算平均跳变点位置
uint8_t avg_x = sum_x / point_count;
// 连接底部中心到平均点
connect_line(IMG_W/2, IMG_H-1, avg_x, 10);
}
}
else if(!right_conduct_line) {
uint16_t points[20] = {0};
uint8_t point_count = 0;
// 在顶部20行扫描黑白跳变点（白到黑）
for(int y = 0; y < 20; y++) {
for(int x = IMG_W-1; x > 0; x--) {
if(bin_image[y][x] == 0 && bin_image[y][x-1] == 255) {
points[point_count++] = x;
break; // 每行只取第一个点
}
}
}
// 取中值作为坐标
if(point_count > 0) {
// 排序取中值
for(int i = 0; i < point_count-1; i++) {
for(int j = i+1; j < point_count; j++) {
if(points[j] < points[i]) {
uint16_t temp = points[i];
points[i] = points[j];
points[j] = temp;
}
}
}
uint8_t median_x = points[point_count/2];
connect_line(IMG_W/2, IMG_H-1, median_x, 10); // 使用中间行高度
}
}
else {
connect_line(IMG_W/2, IMG_H-1, IMG_W, right_y);
}
break;
case OUT:
break;
case RETURN:
break;
case NONE:
break;
}
}
if(circle_type == 1) {
switch(STATE) {
case IN:
connect_line(IMG_W/2, IMG_W-10,image_par.l_cir_in,0);
break;
case RUN:
if(up_conduct_line == 0 && !left_conduct_line) {
// 在顶部20行搜索黑白跳变点（黑到白）
uint16_t sum_x = 0;
uint16_t point_count = 0;
for(int y = 0; y < 20; y++) {
// 从左向右搜索 (0-39列)
for(int x = 0; x < IMG_W; x++) {
if(bin_image[y][x] == 255 && bin_image[y][x-1] == 0) {
sum_x += (x-1); // 记录跳变点位置
point_count++;
break; // 每行只取一个跳变点
}
}
}
if(point_count > 0) {
// 计算平均跳变点位置
uint8_t avg_x = sum_x / point_count;
// 连接底部中心到平均点
connect_line(IMG_W/2, IMG_H-1, avg_x, 10);
}
}
else if(!left_conduct_line) {
uint16_t points[20] = {0};
uint8_t point_count = 0;
// 在顶部20行扫描黑白跳变点（黑到白）
for(int y = 0; y < 20; y++) {
for(int x = 0; x < IMG_W-1; x++) {
if(bin_image[y][x] == 255 && bin_image[y][x-1] == 0) {
points[point_count++] = x-1;
break; // 每行只取第一个点
}
}
}
// 取中值作为坐标
if(point_count > 0) {
// 排序取中值
for(int i = 0; i < point_count-1; i++) {
for(int j = i+1; j < point_count; j++) {
if(points[j] < points[i]) {
uint16_t temp = points[i];
points[i] = points[j];
points[j] = temp;
}
}
}
uint8_t median_x = points[point_count/2];
connect_line(IMG_W/2, IMG_H-1, median_x, 10); // 使用中间行高度
}
}
else {
connect_line(IMG_W/2, IMG_H-1, 0, left_y);
}
break;
case OUT:
break;
case RETURN:
break;
case NONE:
break;
}
}
}陀螺仪积分：
void yaw_record(void) {
// 使用去零漂的角速度数据 GZ（单位：度/秒）
float current_gyro_z = GZ; // 使用去零漂的变量
// 固定时间间隔 10ms = 0.01s
const float dt = 0.01f;
// 第一次调用初始化
if (first_gyro_rec) {
last_gyro_z = current_gyro_z;
first_gyro_rec = 0;
return;
}
// 使用梯形法则积分角速度（度/秒 -> 度）
// 公式：角度变化 = 0.5 * (上次角速度 + 本次角速度) * 时间间隔
float delta_deg = 0.5f * (current_gyro_z + last_gyro_z) * dt;
// 累加角度变化
sum_yaw += delta_deg;
// 保存当前值用于下一次计算
last_gyro_z = current_gyro_z;
}
DAMO开发者矩阵，由阿里巴巴达摩院和中国互联网协会联合发起，致力于探讨最前沿的技术趋势与应用成果，搭建高质量的交流与分享平台，推动技术创新与产业应用链接，围绕“人工智能与新型计算”构建开放共享的开发者生态。
更多推荐
42
0- 0
已为社区贡献1条内容
所有评论(0)