【智能车Code review】——坡道图像与控制处理

坡道处理概述

坡道可分为大坡和小坡。
坡道图像遵循状态机的原则，分为上坡、在坡顶、下坡；
由于摄像头限制，坡道顶部的图像能用的范围很少且图像很乱，所以在坡顶采取锁中线或者是重新找中线。

图像处理部分

清除标志位函数clear_po()

清除坡道保护标志位：po_protect
清除坡道标志位：podao_flag
清除上坡标志位：podaoup_flag
清除坡顶标志位：podaoding_flag
清除下坡标志位：podaodown_flag
在判断成下坡时调用

void clear_po()
{
    po_protect = 0;
    podao_flag = 0;
    podaoup_flag = 0;
    podaoding_flag = 0;
    podaodown_flag = 0;
}

坡道判断函数juge_po()

上坡分为斜入坡道、正入坡道判断；
long_turn_flag_left ：左线连续程度
long_turn_flag_right ：右线连续程度
rou_of_left ：左线方差
rou_of_right ：右线方差
fiv_width[i]:第i行的赛道宽度
k_left：最小二乘法拟合出来的左线斜率
k_right：最小二乘法拟合出来的左线斜率
lostright_times:右线丢失行数
l_start：左线第一个不为白的点
break_hangshu：中线break行数
fps_up_and_on_po ：上坡和在坡顶的帧数，用于强制出坡状态
must_out_po_fps ：人工设置的出坡状态的帧数

#region[坡道]
void juge_po()
{
    byte jugetime = 0;
    //正入坡道
    //大坡
    if (po_protect == 0 && podaodown_flag == 0 && podao_flag == 0 && long_turn_flag_left >= 55 && long_turn_flag_right >= 55 && rou_of_left >= 5 && rou_of_left <= 440 && rou_of_right >= 5 && rou_of_right <= 440)
    {
        if (fiv_width[40] >= 90 && fiv_width[45] >= 70 && fiv_width[50] >= 75 && fiv_width[55] >= 67)
        {
            jugetime = 1;
        }
        if (jugetime >= 1 && k_left >= -1.1 && k_left <= 0.15 && k_right >= -0.15 && k_right <= 1.1)
        {
            podao_flag = 1;
            podaoup_flag = 1;
            SetText("往坡上走");
        }
    }
    //右斜入坡道
    if (po_protect == 0 && podaodown_flag == 0 && podao_flag == 0 && l_start <= 10 && lostright_times >= 48 && break_hangshu >= 63 && long_turn_flag_left >= 60 && long_turn_flag_right >= 60 && rou_of_left >= 5 && rou_of_left <= 1000 && rou_of_right >= 0 && rou_of_right <= 220)
    {

        if (fiv_width[45] >= 60 && fiv_width[50] >= 55)
        {
            jugetime = 1;
        }
        if (jugetime >= 1 && k_left >= -2 && k_left <= -0.35)
        {
            podao_flag = 1;
            podaoup_flag = 1;
            SetText("往坡上走(右斜入)");
        }
    }
    //左斜入坡道
    if (po_protect == 0 && podaodown_flag == 0 && podao_flag == 0 && r_start <= 10 && lostleft_times >= 48 && break_hangshu >= 63 && long_turn_flag_left >= 60 && long_turn_flag_right >= 60 && rou_of_right >= 5 && rou_of_right <= 1000 && rou_of_left >= 0 && rou_of_left <= 220)
    {

        if (fiv_width[45] >= 60 && fiv_width[50] >= 55)
        {
            jugetime = 1;
        }
        if (jugetime >= 1 && k_left >= -2 && k_left <= -0.35)
        {
            podao_flag = 1;
            podaoup_flag = 1;
            SetText("往坡上走(左斜入)");
        }
    }
    if (podao_flag == 1 && long_turn_flag_left < 40 && long_turn_flag_right < 40)
    {
        podaoup_flag = 0;
        podaoding_flag = 1;
        old = 93;
        SetText("在坡道顶部");
    }
    if (street_flag == 17 || street_flag == 18)
    {
        fps_up_and_on_po++;
        setText用户自定义("坡道状态第" + fps_up_and_on_po + "帧");
    }
    if (fps_up_and_on_po >= must_out_po_fps || (podaoding_flag == 1 && break_hangshu >= 45 && (long_turn_flag_left >= 40 || long_turn_flag_right >= 40) && fiv_width[10] >= 110 && fiv_width[20] >= 100 && fiv_width[25] >= 90))
    {
        podao_flag = 0;
        podaoup_flag = 0;
        podaoding_flag = 0;
        podaodown_flag = 1;
        not_rush_flag = 1;
        fps_up_and_on_po = 0;
        SetText("往坡下走");
    }
    po_protect_timer();
}
#endregion

坡道保护函数po_protect_timer()（多少帧之内不判坡道）

这是由于上坡图像和下坡图像比较相似造成的，为了不在下坡的时候再次进入上坡状态，特加入此函数用于保护。

void po_protect_timer()
{
    if (podaoding_flag == 1) po_protcectflag = 1;
    if (po_protcectflag == 1)
    {
        timer++;
        po_protect = 1;
        if (timer >= fps_po_protect)
        {
            po_protcectflag = 0;
            timer = 0;
            po_protect = 0;
        }
    }

}

坡顶与下坡寻找赛道函数search_track_function()

用于上坡时车身没有摆正导致下坡图像紊乱时用于重新找寻赛道。

uint8_t center_ramp_we_set = 0;
uint8_t search_track_function(uint8_t thread_rANDl)
{
    uint8_t centerline_we_set = 0;
    if (fiv_width[1] == 2) //图像1行以下紊乱
    {
        //扫第一行的线寻找可能的赛道
        byte i = 93;
        uint8_t right_start_point = 0;
        uint8_t right_end_point = 0;
        uint8_t left_start_point = 0;
        uint8_t left_end_point = 0;
        //从黑色区域带寻找白色区域带
        //【1】先找区域所在位置
        //向右找
        for (i = 93; i >= 93 - thread_rANDl; i--)
        {
            if (BinPixels[0][i] == 0 && BinPixels[0][i - 1] != 0)   //赛道跳变
            {
                right_start_point = (uint8_t)(i - 1);
                break;
            }
        }
        //向左找
        for (i = 93; i <= 93 + thread_rANDl; i++)
        {
            if (BinPixels[0][i] == 0 && BinPixels[0][i + 1] != 0)   //赛道跳变
            {
                left_start_point = (uint8_t)(i + 1);
                break;
            }
        }
        //【2】计算白色区域像素点长度，观察是否是正确的！
        if (right_start_point != 0)   //从右点往右寻找，直到遇到黑色像素，或者到0
        {
            for (i = right_start_point; i > 0; i--)
            {
                if (BinPixels[0][i] == 0)   //黑色像素
                {
                    right_end_point = i;
                    break;
                }
                if (i <= 1) right_end_point = 0;
            }

        }
        if (left_start_point != 0)   //从左点往左寻找，直到遇到黑色像素，或者到185
        {
            for (i = left_start_point; i < 185; i++)
            {
                if (BinPixels[0][i] == 0)   //黑色像素
                {
                    left_end_point = i;
                    break;
                }
                if (i >= 184) left_end_point = 185;
            }
        }
        //计算左右两边寻找后得到的两个赛道宽度（取比较大的为真的赛道边界）
        int width_right = right_start_point - right_end_point;
        int width_left = left_end_point - left_start_point;
        if (width_right >= 5 || width_left >= 5)    //当赛道宽度较大的时候认为是真的赛道
        {
            if (width_right >= width_left) centerline_we_set = (uint8_t)((right_start_point + right_end_point) * 0.5f);
            else centerline_we_set = (uint8_t)((left_start_point + left_end_point) * 0.5f);
        }
        //setText用户自定义("左start点"+ left_start_point+ "左end点" + left_end_point);
        //setText用户自定义("右start点" + right_start_point + "右end点" + right_end_point);
    }
    //图像没有紊乱
    else centerline_we_set = 1;
    //setText用户自定义("新的中线值" + centerline_we_set);
    return centerline_we_set;
}

调用方法：

//在坡道顶部时，纠正中线
if (street_flag == 18)
{
    center_ramp_we_set = search_track_function(20);
    //当找到中线值且线紊乱的时候赋值
    if (center_ramp_we_set != 0 && center_ramp_we_set != 1)
    {
        setText用户自定义("lalalalla");	//用于显示是否有效，无特殊意义
        //中线归线
        for (j = 0; j < 70; j++)
        {
            LCenter[j] = center_ramp_we_set;
        }
    }
}

效果如下：

控制部分

//当普通扫线失败，通过搜索找到中线值且线紊乱的时候，采用中线值赋值
//或者当普通扫线成功（break_hangshu>20）
if((center_ramp_we_set != 0 && center_ramp_we_set!=1) || (break_hangshu>20))
{
}
//否则锁舵机,直接锁中线值
else
{
	centerline[0]=93;
}
//计算舵机打角
feedback=LocPIDcalc_new(3,(centerline[0]));	 //得到的直道反馈值
//舵机pwm限幅
steer_limit();
//舵机pwm输出
pwm_duty(steer_FTM,steer_middle_pwm-feedback);//steer_middle_pwm+feedback
//计算电机输出
if(ramp_speedup==0)
{
	qurve_PID_OUTPUT(max_ramp_peak,max_ramp_peak,GO);
}
else
{
	qurve_PID_OUTPUT(max_ramp_peak+20,max_ramp_peak+20,GO);
}

计算连续程度的函数可以到这篇文章寻找：
https://blog.csdn.net/qq_42604176/article/details/105336462