MotorCar_design_Prelearning

Posted on
字数统计: 10.2k 字 | 预估阅读时间 ≈ 45 分钟
这篇文章主要记录大二学期的平衡摩托车项目的预习工作

在不侧重于硬件配置情况下，给出逐飞给出的部分外设重要函数：

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
//zf_common_clock.c：设置、获取系统频率，关闭看门狗，时钟初始化，等待所有核心初始化完毕
*****************************************************************************************
//zf_device_absolute.encoder.c:编码器部分：
uint8 absolute_encoder_init();         //绝对值编码器初始化
int16 absolute_encoder_get_offset();   //绝对值编码器获取相较上次位置的偏移值
int16 absolute_encoder_get_location(); //绝对值编码器获取当前角度值
****************************************************************************************
//zf_device_bluetooth_ch9141.c: 蓝牙转串口模块
uint32 bluetooth_ch9141_send_byte(const uint8 data);   //蓝牙转串口模块 发送数据
uint32 bluetooth_ch9141_send_buffer(const uint8 *buff, uint32 len); //蓝牙转串口模块 发送数组
uint32 bluetooth_ch9141_send_string (const char *str); //蓝牙转串口模块 发送字符串
void bluetooth_ch9141_send_image (const uint8 *image_addr, uint32 image_size);
//蓝牙转串口模块 发送摄像头图像至上位机查看图像
uint32 bluetooth_ch9141_read_buffer (uint8 *buff, uint32 len);//蓝牙转串口模块 读取函数
void bluetooth_ch9141_uart_callback (void); //蓝牙转串口模块 串口中断回调函数
uint8 bluetooth_ch9141_init (void)// 蓝牙转串口模块 初始化；使用的波特率为115200 为蓝牙转串口模块的默认波特率 如需其他波特率请使用上位机修改模块参数 *****************************************************************************************
//zf_device_imu660ra.c: IMU660ra模块
uint8 imu660ra_init (void); //初始化 IMU660RA
void imu660ra_get_acc (void); //获取 IMU660RA 加速度计数据imu660ra_acc_x/y/z
void imu660ra_get_gyro (void); //获取 IMU660RA 陀螺仪数据imu660ra_gyro_x/y/z
//还有imu963ra,mpu6050的配置代码，结构与IMU660ra相似
**********************************************************************************
//zf_device_ips200.c: 屏幕显示
void ips200_clear (void)； //IPS200 清屏函数，将屏幕清空成背景颜色
void ips200_full (const uint16 color)；//IPS200 屏幕填充函数，颜色格式 RGB565 或者可以使用 zf_common_font.h 内 rgb565_color_enum 枚举值或者自行写入
void ips200_set_dir (ips200_dir_enum dir)；//设置显示方向，只有在初始化屏幕之前调用才生效
void ips200_set_font (ips200_font_size_enum font)；//设置显示字体
void ips200_set_color (const uint16 pen, const uint16 bgcolor)；//设置显示颜色
void ips200_draw_point (uint16 x, uint16 y, const uint16 color)；//IPS200 画点
void ips200_draw_line (uint16 x_start, uint16 y_start, uint16 x_end, uint16 y_end, const uint16 color)；//画线
void ips200_show_char (uint16 x, uint16 y, const char dat)；//显示字符
void ips200_show_string (uint16 x, uint16 y, const char dat[])；//IPS200 显示字符串
void ips200_show_int (uint16 x, uint16 y, const int32 dat, uint8 num);//IPS200 显示32位有符号 (去除整数部分无效的0)
void ips200_show_uint (uint16 x, uint16 y, const uint32 dat, uint8 num);//IPS200 显示32位无符号 (去除整数部分无效的0)
void ips200_show_float (uint16 x, uint16 y, const double dat, uint8 num, uint8 pointnum)
//IPS200 显示浮点数(去除整数部分无效的0)
void ips200_show_binary_image (uint16 x, uint16 y, const uint8 *image, uint16 width, uint16 height, uint16 dis_width, uint16 dis_height);//IPS200 显示二值图像 数据每八个点组成一个字节数据,不可以用来直接显示总钻风的未压缩的二值化图像    
void ips200_show_gray_image (uint16 x, uint16 y, const uint8 *image, uint16 width, uint16 height, uint16 dis_width, uint16 dis_height, uint8 threshold);//IPS200 显示 8bit 灰度图像 带二值化阈值,如果要显示二值化图像 直接修改最后一个参数为需要的二值化阈值即可
void ips200_show_rgb565_image (uint16 x, uint16 y, const uint16 *image, uint16 width, uint16 height, uint16 dis_width, uint16 dis_height, uint8 color_mode);//IPS200 显示 RGB565 彩色图像,用于显示凌瞳的 RGB565 的图像,如果要显示低位在前的其他 RGB565 图像 修改最后一个参数即可
void ips200_show_wave (uint16 x, uint16 y, const uint16 *wave, uint16 width, uint16 value_max, uint16 dis_width, uint16 dis_value_max);//IPS200 显示波形
void ips200_show_chinese (uint16 x, uint16 y, uint8 size, const uint8 *chinese_buffer, uint8 number, const uint16 color);//汉字显示
void ips200_init (ips200_type_enum type_select);//  2寸 IPS液晶初始化   
//IPS114，oled,tft180同理。
**********************************************************************************
//zf_device_key.c:按键状态扫描; 获取按键状态; 清除指定按键状态;按键初始化
**********************************************************************************
//zf_device_mt9v03x.c:
//定义 MT9V03X 基本配置,初始化    
//注：小钻风摄像头OV7725以及scc8660同理
**********************************************************************************

例程中的重要部分：

GPIO演示例程 本例程主要作用是，展示如何使用GPIO模块
串口演示例程 本例程主要作用是，展示如何使用串口的发送与中断接收
ADC采集例程 本例程主要作用是，展示如何使用ADC模块
PWM输出例程 本例程主要作用是，展示如何使用GTM模块输出PWM以及设定占空比
周期中断演示例程 本例程主要作用是，展示如何使用定时器完成周期中断
GPIO中断演示例程 本例程主要作用是，展示如何使用GPIO触发中断
编码器例程 本例程主要作用是，展示如何使用正交解码编码器以及带方向编码器
Flash存取例程 本例程主要作用是，展示如何使用片内EEPROM(Dflash)存取参数
延时例程 本例程主要作用是，展示如何使用滴答定时器进行延时
调试信息输出例程 本例程主要作用是，展示如何使用底层库进行调试信息输出
中断优先级例程 本例程主要作用是，展示如何使用中断优先级的设置
核心1处理中断例程 本例程主要作用是，展示如何使用核心1来处理中断
双核使用例程 本例程主要作用是，展示如何使用双核同时运行程序
指定变量或代码位置 本例程主要作用是，展示如何控制变量或者代码位置，来提高性能
FFT使用例程 本例程主要作用是，展示如何使用英飞凌SDK自带的软件FFT计算

E1:配置IO口，点灯（略）

E2:利用串口发送/接受信息，注意中断的配置

E3:ADC,主要函数有adc_convert(); adc_mean_filter_convert();参数是ADC通道等

E4：pwm制作呼吸灯

E5:利用中断控制灯的亮灭

E6:中断嵌套的应用

E7:利用中断读取编码器的输出值

E8:利用串口输出数据至串口助手

E9:利用system_getval_ms();来获取系统从运行到现在的时间，也可以us

...双核的使用建议看E14,详细解释了分配规则：

  • CPU0是一级流水线，CPU1是三级流水线，因此CPU1更适合做运算，速度会比CPU0快一些

  • CPU0 psram 程序RAM 大小为16KB CPU0 dsram 数据RAM 大小为72KB

  • CPU1 psram 程序RAM 大小为32KB CPU1 dsram 数据RAM 大小为120KB

  • TC264D中两个核心都有自己的程序缓存， CPU0 Cache大小为8KB CPU1 Cache大小为16KB

  • 程序RAM用来放代码，将代码放在RAM中，可以提高执行速度，特别是当周期运行的代码大小超过了cache之后效果比较显著。数据RAM用来放变量，任意CPU都可以访问dsram0和dsram1，也就是说CPU0可以访问dsram1，这种行为叫做跨核访问，带来的问题就是访问延迟有所增大；最好的是CPU0访问dsram0，CPU1访问dsram1，这样速度最快。 默认情况下定义的全局变量都是保存在dsram1中的，这就会导致在CPU0中跑的代码对RAM读取会有一定的延迟。

    1
    2
    3
    4
    5
    #pragma section all "cpu0_dsram"  // 将变量放在cpu0_dsram区域内
    uint8 test_value = 0;
    // 如果将cpu0_dsram改为cpu1_dsram  则变量放在cpu1_dsram区域内
    // 我们建议程序由哪个CPU执行，则将所使用的变量放置在哪个区域内，默认不指定的情况下变量被放置在cpu1_dsram区域
    #pragma section all restore
    1
    2
    3
    4
    5
    6
    7
    8
    9
    10
    11
    12
    13
    // 使用#pragma section all "cpu0_psram" 和 #pragma section all restore可以将函数加载到cpu0_psram
    // 我们只需要将函数定义放在上面两句话中间即可，使用示例如下
    #pragma section all "cpu0_psram"  // 将函数加载到cpu0_psram区域内执行
    void delay_tset(void)
    {
        uint32 i;
        i = 16000000;
        while(i--);
    }
    // 如果将cpu0_psram改为cpu1_psram  则函数加载到cpu1_psram区域内
    // 我们建议程序由哪个CPU执行，则将函数加载到在哪个区域内，默认不指定的情况下，程序都是从flash加载并执行
    // 通常我们都不需要将函数放在RAM运行，一般在周期执行的程序大小超过了cache后，会出现程序执行速度降低，这个时候我们就可以将部分代码放入RAM执行，可以提高运算速度
    #pragma section all restore
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
float I_ex, I_ey, I_ez;  //误差积分
float param_Kp = 50.0;   //加速度计的收敛速率比例增益 50
float param_Ki = 0.20;   //陀螺仪收敛速率的积分增益 0.2
//姿态解算融合，是Crazepony和核心算法。使用的是互补滤波算法
static void IMU_AHRSupdate_noMagnetic(float gx, float gy, float gz, float ax, float ay, float az)
{
    float halfT = 0.5 * delta_T;//采样周期一半
    float vx, vy, vz;           //当前的机体坐标系上的重力单位向量
    float ex, ey, ez;           //四元数计算值与加速度计测量值的误差
    //四元数
    float q0 = Q_info.q0;
    float q1 = Q_info.q1;
    float q2 = Q_info.q2;
    float q3 = Q_info.q3;
    //四元数相乘，方便后面的计算
    float q0q0 = q0 * q0;
    float q0q1 = q0 * q1;
    float q0q2 = q0 * q2;
    float q0q3 = q0 * q3;
    float q1q1 = q1 * q1;
    float q1q2 = q1 * q2;
    float q1q3 = q1 * q3;
    float q2q2 = q2 * q2;
    float q2q3 = q2 * q3;
    float q3q3 = q3 * q3;

    //对加速度数据进行归一化，得到单位加速度
    float norm = invSqrt(ax*ax + ay*ay + az*az);
    ax = ax * norm;
    ay = ay * norm;
    az = az * norm;
    //载体坐标系下重力在三个轴上的分量
    vx = 2*(q1q3 - q0q2);
    vy = 2*(q0q1 + q2q3);
    vz = q0q0 - q1q1 - q2q2 + q3q3;
    //g^b和a^b做向量叉乘，得到陀螺仪的校正补偿向量e的系数
    ex = ay * vz - az * vy;
    ey = az * vx - ax * vz;
    ez = ax * vy - ay * vx;

    /*
    用叉乘误差来做PI修正陀螺零偏，
    通过调节 param_Kp，param_Ki 两个参数，
    可以控制加速度计修正陀螺仪积分姿态的速度。
    */

    //误差累加
    I_ex += delta_T * ex;
    I_ey += delta_T * ey;
    I_ez += delta_T * ez;
    //使用PI控制器消除向量积误差（陀螺仪漂移误差）
    gx = gx+ param_Kp*ex + param_Ki*I_ex;
    gy = gy+ param_Kp*ey + param_Ki*I_ey;
    gz = gz+ param_Kp*ez + param_Ki*I_ez;
    /*数据修正完成，下面是四元数微分方程*/
    //四元数微分方程，其中halfT为测量周期的1/2，gx gy gz为陀螺仪角速度，以下都是已知量，这里使用了一阶龙格库塔求解四元数微分方程
    q0 = q0 + (-q1*gx - q2*gy - q3*gz)*halfT;
    q1 = q1 + ( q0*gx + q2*gz - q3*gy)*halfT;
    q2 = q2 + ( q0*gy - q1*gz + q3*gx)*halfT;
    q3 = q3 + ( q0*gz + q1*gy - q2*gx)*halfT;
    //保存上一次四元数的值
    norm = invSqrt(q0*q0 + q1*q1 + q2*q2 + q3*q3);  //归一化系数
    Q_info.q0 = q0 * norm;
    Q_info.q1 = q1 * norm;
    Q_info.q2 = q2 * norm;
    Q_info.q3 = q3 * norm;
}
//上述的函数invSqrt是求导数的平方根：
float invSqrt(float x)
{
    float halfx = 0.5f * x;
    float y = x;
    long i = *(long*)&y;
    i = 0x5f3759df - (i>>1);
    y = *(float*)&i;
    y = y * (1.5f - (halfx * y * y));
    return y;
}


//把四元数转换成欧拉角
void IMU_quaterToEulerianAngles(void)
{
    IMUGetValues(values);   //基础数据处理
    IMU_AHRSupdate_noMagnetic(values[3], values[4], values[5], values[0], values[1], values[2]);    //融合滤波解算四元数
    //四元数
    float q0 = Q_info.q0;
    float q1 = Q_info.q1;
    float q2 = Q_info.q2;
    float q3 = Q_info.q3;
    //换算欧拉角
    eulerAngle.pitch = asin(-2*q1*q3 + 2*q0*q2) * 180/M_PI;                        // pitch
    eulerAngle.roll = atan2(2*q2*q3 + 2*q0*q1, -2*q1*q1 - 2*q2*q2 + 1) * 180/M_PI; // roll
    eulerAngle.yaw = atan2(2*q1*q2 + 2*q0*q3, -2*q2*q2 - 2*q3*q3 + 1) * 180/M_PI;  // yaw
}
//当然也可以作姿态限度限制，但没什么必要：
//    if(eulerAngle.roll>90 || eulerAngle.roll<-90)
//    {
//        if(eulerAngle.pitch > 0)
//        {
//            eulerAngle.pitch = 180-eulerAngle.pitch;
//        }
//        if(eulerAngle.pitch < 0)
//        {
//            eulerAngle.pitch = -(180+eulerAngle.pitch);
//        }
//    }
//    if(eulerAngle.yaw > 180)
//    {
//        eulerAngle.yaw -=360;
//    }
//    else if(eulerAngle.yaw <-180)
//    {
//        eulerAngle.yaw +=360;
//    }
//由此，我们得到了姿态角。

附加滤波算法：

注意，对于各个部件选用同一种滤波方式时要区分，也即写多个同样的函数（但不同名）

一阶滞后滤波算法

1
2
3
4
5
6
7
8
9
10
#define FIRST_LAG_P 0.8     //一阶滞后滤波系数，越小滞后效果越强
float FirstOrderLagFilter(float value)
{
    static float last_value;  //上一次滤波结果

    value=FIRST_LAG_P*value+(1-FIRST_LAG_P)*last_value; //一阶滞后滤波
    last_value=value;   //保存此次滤波结果为上一次滤波结果

    return value;
}

二阶滞后滤波算法与卡尔曼滤波

第八篇，滤波：二阶低通滤波、卡尔曼滤波-CSDN博客

1
2
3
4
5
6
7
8
9
10
11
int16 SecondOrderLagFilter(int16 value)
{
    static int16 last_value,last_2_value;

    value=0.2*value+0.4*last_value+0.4*last_2_value;     //二阶滞后滤波

    last_2_value=last_value;
    last_value=value;

    return value;
}
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
typedef struct {
    int16 x;        // state
    float A;        // x(n)=A*x(n-1)+u(n),u(n)~N(0,q)
    float H;        // z(n)=H*x(n)+w(n),w(n)~N(0,r)
    float q;        // process(predict) noise convariance
    float r;        // measure noise(error) convariance
    float p;        // estimated error convariance
    float gain;     // kalman gain
} kalman1_filter_t;

kalman1_filter_t kalman_gyro;       //定义卡尔曼结构体
//写一个初始化函数，便于初始参数设定

void kalman1_init(kalman1_filter_t* state, float q, float r)
{
    state->x = 0;
    state->p = 0.0f;
    state->A = 1.0f;
    state->H = 1.0f;
    state->q = q;
    state->r = r;
}
//滤波算法
int16 kalman1_filter(kalman1_filter_t* state, float z_measure)
{
    /* Predict */
    // 时间更新(预测): X(k|k-1) = A(k,k-1)*X(k-1|k-1) + B(k)*u(k)
    state->x = state->A * state->x;
    // 更新先验协方差: P(k|k-1) = A(k,k-1)*A(k,k-1)^T*P(k-1|k-1)+Q(k)
    state->p = state->A * state->A * state->p + state->q;

    /* Measurement */
    // 计算卡尔曼增益: K(k) = P(k|k-1)*H(k)^T/(P(k|k-1)*H(k)*H(k)^T + R(k))
    state->gain = state->p * state->H / (state->p * state->H * state->H + state->r);
    // 测量更新(校正): X(k|k) = X(k|k-1)+K(k)*(Z(k)-H(k)*X(k|k-1))
    state->x = state->x + state->gain * (z_measure - state->H * state->x);
    // 更新后验协方差: P(k|k) =（I-K(k)*H(k))*P(k|k-1)
    state->p = (1 - state->gain * state->H) * state->p;
    return state->x;
}

模糊PID算法:

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135

//对数据模糊化的宏定义{NB,NM,NS,ZO,PS,PM,PB},不用数组是因为方便后面规则表的编写
//******************模糊子集******************************
#define NB -3
#define NM -2
#define NS -1
#define ZO  0
#define PS  1
#define PM  2
#define PB  3
//*******************************************************

//隶属值的单位值：FuzzyArray[]*这个=隶属值，该值需要调参使用，(MAX-MIN)/8得到
/*******************物理论域宏定义****************************/
//*******************************************************
#define BiasMembership 60 //偏差
#define BiasCMembership 10//偏差于上一次偏差的差
#define SteerKP 1
#define SteerKd 1
#define MotorKP 1
#define MotorKI 1
//********************************************************

/*************************模糊规则***********************/
uint8 KPFuzzyRule[7][7]={{PB,PB,PM,PM,PS,ZO,ZO},
                         {PB,PB,PM,PS,PS,ZO,NS},
                         {PM,PM,PM,PS,ZO,NS,NS},
                         {PM,PM,PS,ZO,NS,NM,NM},
                         {PS,PS,ZO,NS,NS,NM,NM},
                         {PS,ZO,NS,NM,NM,NM,NB},
                         {ZO,ZO,NM,NM,NM,NB,NB}};//KP参数的模糊规则表
uint8 KIFuzzyRule[7][7]={{NB,NB,NM,NM,NS,ZO,ZO},
                         {NB,NB,NM,NS,NS,ZO,ZO},
                         {NB,NM,NS,NS,ZO,PS,PS},
                         {NM,NM,NS,ZO,PS,PM,PM},
                         {NM,NS,ZO,PS,PS,PM,PB},
                         {ZO,ZO,PS,PS,PM,PB,PB},
                         {ZO,ZO,PS,PM,PM,PB,PB}};//KI参数的模糊规则表
uint8 KDFuzzyRule[7][7]={{PS,NS,NB,NB,NB,NM,PS},
                         {PS,NS,NB,NM,NM,NS,ZO},
                         {ZO,NS,NM,NM,NS,NS,ZO},
                         {ZO,NS,NS,NS,NS,NS,ZO},
                         {ZO,ZO,ZO,ZO,ZO,ZO,ZO},
                         {PB,NS,PS,PS,PS,PS,PB},
                         {PB,PM,PM,PM,PS,PS,PB}};//KD参数的模糊规则表
/**********************************************************/

//求出偏差的隶属值与隶属度
//float E: 偏差/偏差论域宏定义（把他们限制在-3~3之间）
//float Membership[2]: 用于存储传进来参数两个隶属度的数组，M[0]是比较小的模糊子集的隶属度
//int Index[2]: 索引数组，用于索引规则表，跟Membership同理M[0]是比较小的模糊子集的索引值
void ClacMembership(float E,float Membership[2],int Index[2])
{
    /*三角隶属度函数*/
    if(E>=NB && E<=NM)
    {
        Index[0]=0;//NB
        Index[1]=1;//NM
        Membership[0]=NM-E;//隶属于NB的隶属度(NM-E)/(NM-NB)，因为分母都是1
        Membership[1]=E-NB;//隶属于NM的隶属度(E-NB)/(NM-NB)
    }
    else if(E>=NM && E<=NS)
    {
        Index[0]=1;
        Index[1]=2;
        Membership[0]=NS-E;//隶属于NM的隶属度
        Membership[1]=E-NM;//隶属于NS的隶属度
    }
    else if(E>=NS && E<=ZO)
    {
        Index[0]=2;
        Index[1]=3;
        Membership[0]=ZO-E;//隶属于NS的隶属度
        Membership[1]=E-NS;//隶属于ZO的隶属度
    }
    else if(E>=ZO && E<=PS)
    {
        Index[0]=3;
        Index[1]=4;
        Membership[0]=PS-E;//隶属于ZO的隶属度
        Membership[1]=E-ZO;//隶属于PS的隶属度
    }
    else if(E>=PS && E<=PM)
    {
        Index[0]=4;
        Index[1]=5;
        Membership[0]=PM-E;//隶属于PS的隶属度
        Membership[1]=E-PS;//隶属于PM的隶属度
    }
    else if(E>=PM && E<=PB)
    {
        Index[0]=5;
        Index[1]=6;
        Membership[0]=PB-E;//隶属于PS的隶属度
        Membership[1]=E-PM;//隶属于PM的隶属度
    }
}

//根据模糊规则表以及重心法求出输出
// 参    数: int IndexE[2]: 偏差的索引数组
//float MSE[2]: 偏差对于两个隶属值的隶属度数组
//int IndexEC[2]: 偏差变化率的索引数组
//float MSEC[2]: 偏差变化率对于两个隶属值的隶属度数组
//int type: 1：KP 2：KI 3：KD
//返 回 值: K的增量K=K0+detaK*比例系数
int  SolutionFuzzy(int IndexE[2],float MSE[2],int IndexEC[2],float MSEC[2],int type)
{
    int temp=0;
    switch(type)
    {
        case 1:
            //重心法求解
            temp=KPFuzzyRule[IndexE[0]][IndexEC[0]]*MSE[0]*MSEC[0]+
                 KPFuzzyRule[IndexE[0]][IndexEC[1]]*MSE[0]*MSEC[1]+
                 KPFuzzyRule[IndexE[1]][IndexEC[0]]*MSE[1]*MSEC[0]+
                 KPFuzzyRule[IndexE[1]][IndexEC[1]]*MSE[1]*MSEC[1];
            break;
        case 2:
            temp=KIFuzzyRule[IndexE[0]][IndexEC[0]]*MSE[0]*MSEC[0]+
                 KIFuzzyRule[IndexE[0]][IndexEC[1]]*MSE[0]*MSEC[1]+
                 KIFuzzyRule[IndexE[1]][IndexEC[0]]*MSE[1]*MSEC[0]+
                 KIFuzzyRule[IndexE[1]][IndexEC[1]]*MSE[1]*MSEC[1];
            break;
        case 3:
            temp=KDFuzzyRule[IndexE[0]][IndexEC[0]]*MSE[0]*MSEC[0]+
                 KDFuzzyRule[IndexE[0]][IndexEC[1]]*MSE[0]*MSEC[1]+
                 KDFuzzyRule[IndexE[1]][IndexEC[0]]*MSE[1]*MSEC[0]+
                 KDFuzzyRule[IndexE[1]][IndexEC[1]]*MSE[1]*MSEC[1];
            break;
        default:
            break;
    }
    return temp;
}

图像处理部分：

图像阈值二值化

首先进行图像的二值化：

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
#define IMAGE_BLACK 0//二值化的黑点
#define IMAGE_WHITE 255//二值化的白点，用在二值化图像以及扫线那里
#define IMAGE_COMPRESS_W    80//压缩之后的图像宽度
#define IMAGE_COMPRESS_H    60//压缩之后的图像高度
#define IMAGECOMPRESS 0//是否开启图像压缩的二值化，1：是 0：否 压缩图像帧率记得调成230
uint8 CompressImage[IMAGE_COMPRESS_H][IMAGE_COMPRESS_W] = {0}; //存储压缩之后的灰度图
#if IMAGECOMPRESS
uint8 BinaryImage[IMAGE_COMPRESS_H][IMAGE_COMPRESS_W] = {0}; //压缩图像之后的二值化图像
#else
uint8 BinaryImage[MT9V03X_H][MT9V03X_W] = {0};                                     //二值化图像
#endif

//上述宏定义中，用到了：
#define MT9V03X_W               160  //图像宽度 范围1-188 //对封装库的修改：适配LCD屏幕的大小
#define MT9V03X_H               120             	//图像高度	范围1-120

对应的处理函数有（自适应建议用，opencv经典算法）：

1
2
3
4
5
6
uint8 otsuThreshold(uint8 *image, uint16 width, uint16 height);     //计算二值化阈值: 大津法 3ms
uint8 GuDiThreshold(uint16 width, uint16 height);     //计算二值化阈值：谷底最小值 2ms
uint8 OneDimensionalThreshold(uint16 width, uint16 height); //计算二值化阈值 4ms
void adaptiveThreshold(uint8 *img_data, uint8 *output_data, int width, int height, int block, uint8 clip_value);//自适应阈值二值化
void Get_Compress_Image(void); //压缩原始灰度图像再进行二值化，速度是之前的一半之前二值化一张图5ms，现在1.5ms
void ImageBinary(void); //根据阈值二值化图像
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253

/*
 *  @brief  大津法二值化0.8ms程序（实际测试4ms在TC264中）
 *  *image ：图像地址
 *  width:  图像宽
 *  height：图像高
 */
uint8 otsuThreshold(uint8 *image, uint16 width, uint16 height)
{
#define GrayScale 256
    int pixelCount[GrayScale] = {0}; //每个灰度值所占像素个数
    float pixelPro[GrayScale] = {0}; //每个灰度值所占总像素比例
    int i, j;
    int Sumpix = width * height; //总像素点
    uint8 threshold = 0;
    uint8 *data = image; //指向像素数据的指针

    //统计灰度级中每个像素在整幅图像中的个数
    for (i = 0; i < height; i++)
    {
        for (j = 0; j < width; j++)
        {
            pixelCount[(int)data[i * width + j]]++; //将像素值作为计数数组的下标
                                                    //   pixelCount[(int)image[i][j]]++;    若不用指针用这个
        }
    }
    float u = 0;
    for (i = 0; i < GrayScale; i++)
    {
        pixelPro[i] = (float)pixelCount[i] / Sumpix; //计算每个像素在整幅图像中的比例
        u += i * pixelPro[i];                        //总平均灰度
    }

    float maxVariance = 0.0;      //最大类间方差
    float w0 = 0, avgValue = 0;   //w0 前景比例 ，avgValue 前景平均灰度
    for (int i = 0; i < 256; i++) //每一次循环都是一次完整类间方差计算 (两个for叠加为1个)
    {
        w0 += pixelPro[i]; //假设当前灰度i为阈值, 0~i 灰度像素所占整幅图像的比例即前景比例
        avgValue += i * pixelPro[i];

        float variance = pow((avgValue / w0 - u), 2) * w0 / (1 - w0); //类间方差
        if (variance > maxVariance)
        {
            maxVariance = variance;
            threshold = (uint8)i;
        }
    }

    return threshold;
}
/***************************************************************************
 ** 函数功能: 谷底最小值二值化算法
 ** 参    数: uint16 width : 图像宽度
 **           uint16 height: 图像高度
 ** 返 回 值: 二值化阈值
 ** 注    意：运用了摄像头采集的灰度图像的全局变量
 *************************************************************************/
uint8 GuDiThreshold(uint16 width, uint16 height) //计算二值化阈值：谷底最小值
{
    uint16 graynum[256] = {0};
    for (int i = 0; i < height; ++i)
    {
        for (int j = 0; j < width; ++j)
        {
            uint8 pix = mt9v03x_image[i][j];
            graynum[pix]++;
        }
    }
    uint8 Max[2] = {0};
    uint8 index[2] = {0};
    for (int i = 0; i < 2; ++i)
    {
        for (uint16 j = 0; j < 256; ++j)
        {
            if (i == 0)
            {
                if (graynum[j] > Max[i])
                {
                    Max[i] = graynum[j];
                    index[i] = j;
                }
            }
            else
            {
                if (graynum[j] > Max[i] && graynum[j] != Max[0])
                {
                    Max[i] = graynum[j];
                    index[i] = j;
                }
            }
        }
    }
    if (index[0] > index[1])
    {
        uint8 temp = 0;
        temp = index[0];
        index[0] = index[1];
        index[1] = temp;
    }

    uint8 Min = 255, index_Min = 0;
    for (uint8 i = index[0]; i < index[1]; ++i)
    {
        if (graynum[i] < Min)
        {
            Min = graynum[i];
            index_Min = i;
        }
    }

    return index_Min;
}

/*
 *******************************************************************************************
 ** 函数功能: 类似一维Means的二值化阈值计算
 ** 参    数: width：图像宽度
 **           height：图像高度
 ** 返 回 值: 二值化阈值********************************************************************************************
 */
uint8 OneDimensionalThreshold(uint16 width, uint16 height)
{
    int row, cloum;
    int G1, G2;
    int g1, g2;
    uint8 threshold = 160, threshold_last = 0; //阈值与上一次阈值，初始化为不同的值，第一个阈值是认为随机设定的

    while (abs(threshold - threshold_last) > 10) //只有当连续两次计算的阈值相等时才会跳出while
    {
        //初始化数据
        G1 = 0;
        G2 = 0;
        g1 = 0;
        g2 = 0;
        //进行G1和G2的分类
        for (row = 0; row < height; row++)
        {
            for (cloum = 0; cloum < width; cloum++)
            {
                if (mt9v03x_image[row][cloum] > threshold)
                {
                    G1 += mt9v03x_image[row][cloum];
                    g1++;
                }
                else
                {
                    G2 += mt9v03x_image[row][cloum];
                    g2++;
                }
            }
        }
        //进行新阈值的计算
        threshold_last = threshold;              //保存上一次的阈值
        threshold = ((G1 / g1) + (G2 / g2)) / 2; //阈值=（G1平均值+G2平均值）/ 2
    }
    return threshold;
}

//根据场地条件调用大津法或谷底最小值得到二值化阈值然后根据灰度图得到黑白图像
void ImageBinary()
{
#if IMAGECOMPRESS
    //压缩图像的二值化
    uint8 Image_Threshold = otsuThreshold(CompressImage[0], IMAGE_COMPRESS_W, IMAGE_COMPRESS_H); //使用大津法得到二值化阈值
    for (int i = 0; i < IMAGE_COMPRESS_H; ++i)
    {
        for (int j = 0; j < IMAGE_COMPRESS_W; ++j)
        {
            if (CompressImage[i][j] <= Image_Threshold) //进行二值化之前只是得到阈值
                BinaryImage[i][j] = IMAGE_BLACK;        //0是黑色  //图像原点不变
            else
                BinaryImage[i][j] = IMAGE_WHITE; //1是白色  //图像原点不变
        }
    }
#else
    uint8 Image_Threshold = otsuThreshold(mt9v03x_image[0], MT9V03X_W, MT9V03X_H);
    for (int i = 0; i < MT9V03X_H; ++i)
    {
        for (int j = 0; j < MT9V03X_W; ++j)
        {
            if (mt9v03x_image[i][j] <= Image_Threshold) //进行二值化之前只是得到阈值
                BinaryImage[i][j] = IMAGE_BLACK;        //0是黑色  //图像原点不变
            else
                BinaryImage[i][j] = IMAGE_WHITE; //1是白色  //图像原点不变
        }
    }
#endif
}

/********************************************************************************************
 ** 函数功能: 自适应阈值二值化图像
 ** 参    数: uint8* img_data：灰度图像
 **           uint8* output_data：二值化图像
 **           int width：图像宽度
 **           int height：图像高度
 **           int block：分割局部阈值的方块大小例如7*7
 **           uint8 clip_value: 局部阈值减去的经验值一般为（2~5）
 ** 返 回 值: 无
 ** 作    者: 上海交大16届智能车智能视觉组SJTUAuTop
 **           https://zhuanlan.zhihu.com/p/391051197
 ** 注    意：adaptiveThreshold(mt9v03x_image[0],BinaryImage[0],MT9V03X_W,MT9V03X_H,5,1);//但是没d用跟大津法一样
 *********************************************************************************************/
void adaptiveThreshold(uint8 *img_data, uint8 *output_data, int width, int height, int block, uint8 clip_value)
{
    //  assert(block % 2 == 1); // block必须为奇数
    int half_block = block / 2;
    for (int y = half_block; y < height - half_block; y++)
    {
        for (int x = half_block; x < width - half_block; x++)
        {
            // 计算局部阈值
            int thres = 0;
            for (int dy = -half_block; dy <= half_block; dy++)
            {
                for (int dx = -half_block; dx <= half_block; dx++)
                {
                    thres += img_data[(x + dx) + (y + dy) * width];
                }
            }
            thres = thres / (block * block) - clip_value;
            // 进行二值化
            output_data[x + y * width] = img_data[x + y * width] > thres ? 255 : 0;
        }
    }
}

/*************************************************************************
 *  函数名称：void Get_Use_Image (void)
 *  功能说明：把摄像头采集到原始图像，缩放到赛道识别所需大小
 *  参数说明：无
 *  函数返回：无
 *  备    注： IMAGE_COMPRESS_H:为图像压缩之后的高度120/2=60
 *             IMAGE_COMPRESS_W:为图像压缩之后的高度160/2=80
 *             帧率230的条件下压缩完之后二值化搜线找拐点采集图像的时间为2.3~2.5ms，未压缩：8.0ms，帧率400的时候为5ms
 *             顺便在压缩的时候进行了个均值滤波，对灰度图滤波这个像素点和旁边那个滤去的求均值
 *             看起来跟原来没太大差别，因为环境不恶劣，可以删去，耗时加了0.2ms
 *************************************************************************/
void Get_Compress_Image(void)
{
    short i = 0, j = 0, row = 0, line = 0;

    for (i = 0; i < MT9V03X_H; i += 2) //神眼高 120 / 2  = 60，
    {
        for (j = 0; j <= MT9V03X_W; j += 2) //神眼宽188 / 2  = 94，
        {
            CompressImage[row][line] = mt9v03x_image[i][j];
            //            CompressImage[row][line] = (mt9v03x_image[i][j]+mt9v03x_image[i][j-1])/2;
            line++;
        }
        line = 0;
        row++;
    }
}

识别边界拐点等：

1
2
3
4
5
6
7
8
9
10
#define BORDER_BIAS 1   //扫线误差
#define INFLECTION_WIDTH  10    //拐点赛道宽度
uint8 Mid=MT9V03X_W/2;                        //初始化扫线的中点为图像中点
uint8 Lost_Row=0;                             //中线丢失的行坐标(扫线到赛道外)
uint8 LostNum_LeftLine=0,LostNum_RightLine=0; //记录左右边界丢线数
typedef struct Point
{
    int X;
    int Y;
}Point; //点坐标的结构体
1
2
3
4
5
6
7
void GetImagBasic(int *LeftLine, int *CentreLine, int *RightLine ,char path);  //扫线提取左中右三线
void GetDownInflection(int startline,int endline,int *LeftLine,int *RightLine,Point *InflectionL,Point *InflectionR);//根据左右边界线来得到下拐点（十字、三岔、环岛）
void GetUpInflection(char Choose,int startline,int endline,Point *UpInflection);//根据遍历左右线得到行与行之间的列坐标的差值，大于设定的阈值则判断为是上拐点
void GetRightangleUPInflection(char Choose,Point DowmInflection,Point *UpInflection,int ROWTHR,int CLOUMNTHR);//根据二值化图像找直角上拐点
void Bin_Image_Filter(void);//图像腐蚀
void MeasureWidth(int startline,int endline);//测量赛道宽度
void GetImagBasic_Garage(int *LeftLine, int *CentreLine, int *RightLine ,char path);
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
258
259
260
261
262
263
264
265
266
267
268
269
270
271
272
273
274
275
276
277
278
279
280
281
282
283
284
285
286
287
288
289
290
291
292
293
294
295
296
297
298
299
300
301
302
303
304
305
306
307
308
309
310
311
312
313
314
315
316
317
318
319
320
321
322
323
324
325
326
327
328
329
330
331
332
333
334
335
336
337
338
339
340
341
342
343
344
345
346
347
348
349
350
351
352
353
354
355
356
357
358
359
360
361
362
363
364
365
366
367
368
369
370
371
372
373
374
375
376
377
378
379
380
381
382
383
384
385
386
387
388
389
390
391
392
393
394
395
396
397
398
399
400
401
402
403
404
405
406
407
408
409
410
411
412
413
414
415
416
417
418
419
420
421
422
423
424
425
426
427
428
429
430
431
432
433
434
435
436
437
438
439
440
441
442
443
444
445
446
447
448
449
450
451
452
453
454
455
456
457
458
459
460
461
462
463
464
465
466
467
468
469
470
471
472
473
474
475
476
477
478
479
480
481
482
483
484
485
486
487
488
489
490
491
492
493
494
495
496
497
498
499
500
501
502
503
504
505
506
507
508
509
510
511
512
513
514
515
516
517
518
519
520
521
522
523
524
525
526
527
528
529
530
531
532
533
534
535
536
537
538
539
540
541
542
543
544
545
546
547
548
549
550
551
552
553
554
555
556
557
558
559
560
561
562
563
564
565
566
567
568
569
570
571
572
573
574
575
576
577
578
579
580
581
582
583
584
585
586
587
588
589
590
591
592
593
594
595
596
597
598
599
600
601
602
603
604
605
606
607
608
609
610
611
612
613
614
615
616
617
618
619
620
621
622
623
624
625
626
627
628
629
630
631
632
633
634
635
636
637
638
639
640
641
642
643
644
645
646
647
648
649
650
651

/*
 ** 函数功能: 扫线提取左中右三线的坐标
 ** 参    数: *LeftLine：左线数组
 **           *CentreLine：中线数组
 **           *RightLine：右线数组
 **           path：默认扫线方向
 ** 返 回 值: 无
 ** 作    者: WBN
 */
void GetImagBasic(int *LeftLine, int *CentreLine, int *RightLine ,char path)
{
    uint8 row,cloum;              //行,列
    uint8 flag_l=0,flag_r=0;    //记录是否丢线flag，flag=0：丢线
    uint8 num=0;                //记录中线连续丢失的次数
    LostNum_LeftLine=0;LostNum_RightLine=0; //把丢线的计数变量清零

    //开始扫线(从下往上,从中间往两边),为了扫线的严谨性,我们做BORDER_BIAS的误差处理，即扫线范围会小于图像大小
    for(row=MT9V03X_H-1;row>0;row--) //从下往上，遍历整幅图像
    {
        //在赛道外，优先按path扫线方向寻找赛道
        if(BinaryImage[row][Mid]==IMAGE_BLACK)  //扫线中点是黑色的（中点在赛道外）
        {
            num++;    //中线连续丢失，+1
            if(path=='L')   //默认向左扫线
            {
                //先向左边扫线，寻找右边界点
                for(cloum=Mid;cloum-BORDER_BIAS>0;cloum--)    //向左扫
                {
                    if(BinaryImage[row][cloum]==IMAGE_WHITE && BinaryImage[row][cloum-BORDER_BIAS]==IMAGE_WHITE)  //找到白点（从赛道外扫到赛道内：黑-白）
                    {
                        RightLine[row]=cloum;    //记录右边界点（向左找到的是右边界点）
                        flag_r=1;               //flag做无丢线标记
                        break;
                    }
                }
                //根据上面扫线的结果判断丢失的赛道是在左边还是右边从而决定继续向哪边扫线
                if(flag_r==1)   //找到了右边界（丢失的赛道在左边）
                {
                    //继续向左寻找左边界
                    for(;cloum-BORDER_BIAS>0;cloum--)    //继续向左扫
                    {
                        if(BinaryImage[row][cloum]==IMAGE_BLACK && BinaryImage[row][cloum-BORDER_BIAS]==IMAGE_BLACK)    //找到黑点：（从赛道内扫到赛道外：白-黑）
                        {
                            LeftLine[row]=cloum;   //记录左边界点
                            flag_l=1;              //flag做无丢线标记
                            break;
                        }
                    }
                }
                else            //没有找到右边界（丢失的赛道在右边）
                {
                    for(cloum=Mid;cloum+BORDER_BIAS<MT9V03X_W-1;cloum++)    //向右扫
                    {
                        if(BinaryImage[row][cloum]==IMAGE_WHITE && BinaryImage[row][cloum+BORDER_BIAS]==IMAGE_WHITE)
                        {
                            LeftLine[row]=cloum;   //记录左边界点（向右找到的是左边界点）
                            flag_l=1;              //flag做无丢线标记
                            break;
                        }
                    }
                    if(flag_l==1)   //找到了左边界（丢失的赛道在右边）
                    {
                        for(;cloum+BORDER_BIAS<MT9V03X_W-1;cloum++) //继续向右扫
                        {
                            if(BinaryImage[row][cloum]==IMAGE_BLACK && BinaryImage[row][cloum+BORDER_BIAS]==IMAGE_BLACK)
                            {
                                RightLine[row]=cloum;   //记录右边界点
                                flag_r=1;               //flag做无丢线标记
                                break;
                            }
                        }
                    }
                }
            }
            else if(path=='R')  //默认向右扫线
            {
                //先向右边扫线，寻找左边界点
                for(cloum=Mid;cloum+BORDER_BIAS<MT9V03X_W-1;cloum++)    //向右扫
                {
                    //判断左边界点
                    if(BinaryImage[row][cloum]==IMAGE_WHITE && BinaryImage[row][cloum+BORDER_BIAS]==IMAGE_WHITE)  //找到白点（从赛道外扫到赛道内：黑-白）
                    {
                        LeftLine[row]=cloum;    //记录左边界点（向右找到的是左边界点）
                        flag_l=1;               //flag做无丢线标记
                        break;
                    }
                }
                //根据上面扫线的结果判断丢失的赛道是在左边还是右边从而决定继续向哪边扫线
                if(flag_l==1)   //找到了左边界（丢失的赛道在右边）
                {
                    //继续向右寻找右边界
                    for(;cloum+BORDER_BIAS<MT9V03X_W-1;cloum++) //继续向右扫
                    {
                        //判断右边界点
                        if(BinaryImage[row][cloum]==IMAGE_BLACK && BinaryImage[row][cloum+BORDER_BIAS]==IMAGE_BLACK)    //找到黑点（从赛道内扫到赛道外：白-黑）
                        {
                            RightLine[row]=cloum;   //记录左边界点
                            flag_r=1;               //flag做无丢线标记
                            break;
                        }
                    }
                }
                else            //没有找到左边界（丢失的赛道在左边）
                {
                    for(cloum=Mid;cloum-BORDER_BIAS>0;cloum--)    //向左扫
                    {
                        //判断右边界点
                        if(BinaryImage[row][cloum]==IMAGE_WHITE && BinaryImage[row][cloum-BORDER_BIAS]==IMAGE_WHITE)
                        {
                            RightLine[row]=cloum;   //记录右边界点（向左找到的是右边界点）
                            flag_r=1;               //flag做无丢线标记
                            break;
                        }
                    }
                    if(flag_r==1)   //找到了右边界（丢失的赛道在左边）
                    {
                        //继续向左寻找左边界
                        for(;cloum-BORDER_BIAS>0;cloum--)   //继续向左扫
                        {
                            //判断左边界点
                            if(BinaryImage[row][cloum]==IMAGE_BLACK && BinaryImage[row][cloum-BORDER_BIAS]==IMAGE_BLACK)  //判断右边界点
                            {
                                LeftLine[row]=cloum;    //记录左边界点
                                flag_l=1;               //flag做无丢线标记
                                break;
                            }
                        }
                    }
                }
            }
        }
        //在赛道中，正常扫线
        else
        {
            num=0;  //打断中线连续丢失，=0
            //左边扫线
            for(cloum=Mid;cloum-BORDER_BIAS>0;cloum--)              //向左扫
            {
                if(BinaryImage[row][cloum]==IMAGE_BLACK && BinaryImage[row][cloum-BORDER_BIAS]==IMAGE_BLACK)  //判断左边界点（BORDER_BIAS防偶然因素）
                {
                    LeftLine[row]=cloum;    //记录左边界点
                    flag_l=1;               //flag做无丢线标记
                    break;
                }
            }
            //右边扫线
            for(cloum=Mid;cloum+BORDER_BIAS<MT9V03X_W-1;cloum++)    //向右扫
            {
                if(BinaryImage[row][cloum]==IMAGE_BLACK && BinaryImage[row][cloum+BORDER_BIAS]==IMAGE_BLACK)  //判断右边界点（BORDER_BIAS防偶然因素）
                {
                    RightLine[row]=cloum;   //记录右边界点
                    flag_r=1;               //flag做无丢线标记
                    break;
                }
            }
        }
        //1.29晚上重新写的数据处理
        if(flag_l==0)   //左边界丢线
        {
            LeftLine[row]=0;            //左边界点等于图像的左边界
            LostNum_LeftLine++;         //左丢线数+1
        }
        if(flag_r==0)   //右边界丢线
        {
            RightLine[row]=MT9V03X_W-1; //右边界点等于图像的右边界
            LostNum_RightLine++;        //右丢线数+1
        }
        CentreLine[row]=(LeftLine[row]+RightLine[row])/2;   //计算中线点
        //为下一次扫线做准备
        Mid=CentreLine[row];    //以上一次的中线值为下一次扫线的中间点
        flag_l=0;               //左边界丢线flag置0
        flag_r=0;               //右边界丢线flag置0

//        //LCD绘制图像
//        lcd_drawpoint(LeftLine[row],row,GREEN);
//        lcd_drawpoint(CentreLine[row],row,RED);
//        lcd_drawpoint(RightLine[row],row,BLUE);
//        systick_delay_ms(STM0,50);
    }
}

/*
 ** 函数功能: 根据左右边界线来得到下拐点（十字、三岔、环岛的判断会用上）
 ** 参    数: int starline:     起始行
 **           int endline:      结束行
 **           int *LeftLine：     左线数组
 **           int *RightLine：   右线数组
 **           Point *InflectionL: 左边拐点
 **           Point *InflectionR: 右边拐点
 ** 返 回 值: 无
 ** 说    明: - 用指针带入进来函数，最后得到的点可以两点确定直线进行补线
 **           - 由于图像原点在左上角，所以起始行是大于结束行，左右线数组从下往上遍历
 ** 作    者: LJF
 */
void GetDownInflection(int startline,int endline,int *LeftLine,int *RightLine,Point *InflectionL,Point *InflectionR)
{
    int i=0;
    InflectionL->X=0;InflectionL->Y=0;InflectionR->X=0;InflectionR->Y=0;//左右拐点置零

    for(i=startline;i>endline;i--)
    {
        //遍历左线，求出先变小大后变小的点，多加三个点的判断是为了误判，左边丢线为0
        /*注意：这里如果判断条件是和相邻的1,3的值对比的话，会区间太小从而如果有相等的列数存在的话，会影响判断，所以需要改大比较的区间*/
        //加了个判断InflectionL->Y==0是为了从下往上遍历，找到了之后就不再继续往上找了，这样遍历时候的截距图片就不用刚刚好了
        //2022年5月27日：加多一层条件，即：比>=相邻的点，随后再取筛选它是不是直线：大于比较远的点
        if(InflectionL->Y==0 && LeftLine[i]>=LeftLine[i-1] && LeftLine[i]>=LeftLine[i-3] && LeftLine[i]>=LeftLine[i+1] && LeftLine[i]>=LeftLine[i+3])
        {
            if(LeftLine[i]>LeftLine[i-5] && LeftLine[i]>LeftLine[i-7] && LeftLine[i]>LeftLine[i+5] && LeftLine[i]>LeftLine[i+7])
            {
                InflectionL->X=LeftLine[i];//存入拐点的（x,y）坐标
                InflectionL->Y=i;
                /*debug*/
//                lcd_showint32(0,6,InflectionL->X,3);
//                systick_delay_ms(STM0, 1000);
            }
        }

        //遍历右线，求出列数最小的点就是右边的拐点，右边线丢线为MT9V03X_W-1
        //加了个判断InflectionR->Y==0是为了从下往上遍历，找到了之后就不再继续往上找了，这样遍历时候的截距图片就不用刚刚好了
        if(InflectionR->Y==0 && RightLine[i]<=RightLine[i-1] && RightLine[i]<=RightLine[i-3] && RightLine[i]<=RightLine[i+1] && RightLine[i]<=RightLine[i+3])
        {
            if(RightLine[i]<RightLine[i-5] && RightLine[i]<RightLine[i-7] && RightLine[i]<RightLine[i+5] && RightLine[i]<RightLine[i+7])
            {
                InflectionR->X=RightLine[i];//存入拐点的（x,y）坐标
                InflectionR->Y=i;
               /*打印被判断为拐点的列坐标，用于调试*/
//                lcd_showint32(TFT_X_MAX-50,6,InflectionR->X,3);
//                systick_delay_ms(STM0, 1000);
            }
        }

        /*打印被判断为拐点的列坐标，用于调试*/
//        lcd_drawpoint(RightLine[i],i,RED);
//        lcd_showint32(0,0,LeftLine[i],3);
//        systick_delay_ms(STM0, 800);
    }
}
#define UPINFLECTION_DOWM_MIN_THRESHOLD  15  //上拐点列坐标与下面行数的差值最小阈值
#define UPINFLECTION_UP_MAX_THRESHOLD  5  //上拐点列坐标与上面行数的差值最大阈值
#define UPINFLECTION_COMPARE_INTERVAL 3 //上拐点两点之间比较间隔

/************************************************************************
 ** 函数功能: 根据左右边界得到上拐点
 ** 参    数: char Choose：选择遍历左线还是右线
 **           int startline：起始行
 **           int endline：结束行
 **           Point *UpInflection：上拐点
 ** 返 回 值: 无
 ** 说    明: 起始行要小于结束行，从上往下遍历左右线
 ** 作    者: LJF
 ***********************************************************************/
void GetUpInflection(char Choose,int startline,int endline,Point *UpInflection)
{
    int row=0;
    switch(Choose)
    {
        case 'L':
        {
            //从上往下遍历
            for (row = startline; row < endline; row++)
            {
                //下三行的列坐标-这行列坐标大于阈值，不用ABS是为了速度更快
                if (LeftLine[row] - LeftLine[row+UPINFLECTION_COMPARE_INTERVAL] >= UPINFLECTION_DOWM_MIN_THRESHOLD
                 && LeftLine[row - UPINFLECTION_COMPARE_INTERVAL] - LeftLine[row + UPINFLECTION_COMPARE_INTERVAL] >= UPINFLECTION_DOWM_MIN_THRESHOLD
                 && LeftLine[row-1]-LeftLine[row]<=UPINFLECTION_UP_MAX_THRESHOLD)
                {
                    UpInflection->X = LeftLine[row]; UpInflection->Y = row;
                    /**************debug***********/
//                    lcd_drawpoint(UpInflection->X,UpInflection->Y,PURPLE);
//                    lcd_showint32(TFT_X_MAX-50,0,UpInflection->X,3);
//                    lcd_showint32(TFT_X_MAX-50,1,UpInflection->Y,3);
//                    systick_delay_ms(STM0, 800);
                    /*****************************/
                    break;
                }
                /**************debug***********/
//                lcd_showint32(0,0,LeftLine[row],3);
//                lcd_showint32(0,1,LeftLine[row+UPINFLECTION_COMPARE_INTERVAL],3);
//                systick_delay_ms(STM0, 800);
                /*****************************/
            }
            break;
        }
        case 'R':
        {
            for(row = startline; row < endline; row++)
            {
                //这行列坐标-下三行的列坐标大于阈值，不用ABS是为了速度更快
                if (RightLine[row+UPINFLECTION_COMPARE_INTERVAL] - RightLine[row] >= UPINFLECTION_DOWM_MIN_THRESHOLD
                 && RightLine[row + UPINFLECTION_COMPARE_INTERVAL] - RightLine[row - UPINFLECTION_COMPARE_INTERVAL] >= UPINFLECTION_DOWM_MIN_THRESHOLD
                 && RightLine[row]-RightLine[row-1]<=UPINFLECTION_UP_MAX_THRESHOLD)
                {
                    UpInflection->X=RightLine[row];UpInflection->Y=row;
                   /**************debug***********/
//                   lcd_drawpoint(UpInflection->X,UpInflection->Y,PURPLE);
//                   lcd_showint32(TFT_X_MAX-50,0,UpInflection->X,3);
//                   lcd_showint32(TFT_X_MAX-50,1,UpInflection->Y,3);
//                   systick_delay_ms(STM0, 800);
                   /*****************************/
                    break;
                }
            }
            break;
        }
        default:break;
    }
}
/************************************************************************
 ** 函数功能: 根据图像黑白跳变寻找上直角拐点
 ** 参    数: char Choose：选择是左上还是右上
 **           Point DowmInflection：基准点
 **           Point *UpInflection：找到的上拐点
 **           int RowThr:遍历图像行的阈值（找跳变点到哪里停下）
 **           int CloumnThr:遍历图像列的阈值
 ** 返 回 值: 无
 ** 说    明: 起始行要小于结束行，从上往下遍历左右线
 ** 作    者: LJF
 ***********************************************************************/
void GetRightangleUPInflection(char Choose,Point DowmInflection,Point *UpInflection,int ROWTHR,int CLOUMNTHR)
{
    int row=0,cloumn=0;//起始行,列
    UpInflection->X=0;UpInflection->Y=0;//左上拐点置零
    //从下往上找白跳黑
    for(row=DowmInflection.Y;row>ROWTHR;row--)
    {
#if FINE_RIGHT_ANGLE_INFLECTION_DEBUG
        lcd_drawpoint(DowmInflection.X, row, PURPLE);
#endif
        if(BinaryImage[row][DowmInflection.X]==IMAGE_WHITE&&BinaryImage[row-1][DowmInflection.X]==IMAGE_BLACK)
        {
            row=row-3;//多往上面跳点
            switch(Choose)
            {
               case 'L':
               {
                   //左往右找到黑跳白
                   for(cloumn=DowmInflection.X;cloumn<CLOUMNTHR;cloumn++)
                   {
#if FINE_RIGHT_ANGLE_INFLECTION_DEBUG
                       lcd_drawpoint(cloumn, row, PURPLE);
#endif
                       if(BinaryImage[row][cloumn]==IMAGE_BLACK&&BinaryImage[row][cloumn+1]==IMAGE_WHITE)
                       {
                           UpInflection->X=cloumn;UpInflection->Y=row;
                           break;
                       }
                   }
                   break;
               }
               case 'R':
               {
                   //右往左找到黑跳白
                   for(cloumn=DowmInflection.X;cloumn>CLOUMNTHR;cloumn--)
                   {
                       if(BinaryImage[row][cloumn]==IMAGE_BLACK&&BinaryImage[row][cloumn-1]==IMAGE_WHITE)
                       {
                           UpInflection->X=cloumn;UpInflection->Y=row;
                           break;
                       }
                   }
                   break;
               }
               default:break;
            }
            break;//跳出行循环,没必要继续行循环下去
        }
    }
}
/*---------------------------------------------------------------
 【函    数】Bin_Image_Filter
 【功    能】过滤噪点
 【参    数】无
 【返 回 值】无
 【注意事项】
 ----------------------------------------------------------------*/
void Bin_Image_Filter(void)
{
    for (int nr=1; nr < MT9V03X_H-1; nr++)
    {
        for (int nc=1; nc < MT9V03X_W-1; nc++)
        {
            if ((BinaryImage[nr][nc] == IMAGE_BLACK)
                    &&(BinaryImage[nr-1][nc]+BinaryImage[nr+1][nc]+BinaryImage[nr][nc+1]+BinaryImage[nr][nc-1]>510))
            {
                BinaryImage[nr][nc] = IMAGE_WHITE;
            }
            else if ((BinaryImage[nr][nc] == IMAGE_WHITE)
                    && (BinaryImage[nr-1][nc]+BinaryImage[nr+1][nc]+BinaryImage[nr][nc+1]+BinaryImage[nr][nc-1]<510))
            {
                BinaryImage[nr][nc] = IMAGE_BLACK;
            }
        }
    }
}

/************************************************************************
 ** 函数功能: 测量赛道宽度函数
 ** 参    数: int startline:要测量的起始行
 **           int endline:要测量的结束行
 ** 返 回 值: 无
 ** 说    明: 用于赛前测试使用，赛道宽度可以给拐弯的时候单边巡线
 ** 作    者: LJF
 ***********************************************************************/
void MeasureWidth(int startline,int endline)
{
    int row=0,width=0;
    for(row=startline;row>endline;row--)
    {
        width=RightLine[row]-LeftLine[row];
//        printf("%d\r\n",width);//测到赛道宽度与行的函数关系为y=135-(119-nowline)*1.1
    }
}

/*
 ** 函数功能: 扫线提取左中右三线的坐标：车库专属
 ** 参    数: *LeftLine：左线数组
 **           *CentreLine：中线数组
 **           *RightLine：右线数组
 **           path：默认扫线方向
 ** 返 回 值: 无
 ** 作    者: WBN
 */
void GetImagBasic_Garage(int *LeftLine, int *CentreLine, int *RightLine ,char path)
{
    uint8 row,cloum;              //行,列
    uint8 flag_l=0,flag_r=0;    //记录是否丢线flag，flag=0：丢线
    uint8 num=0;                //记录中线连续丢失的次数
    LostNum_LeftLine=0;LostNum_RightLine=0; //把丢线的计数变量清零
    uint8 black_num=0;

    //开始扫线(从下往上,从中间往两边),为了扫线的严谨性,我们做BORDER_BIAS的误差处理，即扫线范围会小于图像大小
    for(row=MT9V03X_H-1;row>0;row--) //从下往上，遍历整幅图像
    {
        //在赛道外，优先按path扫线方向寻找赛道
        if(BinaryImage[row][Mid]==IMAGE_BLACK)  //扫线中点是黑色的（中点在赛道外）
        {
            num++;    //中线连续丢失，+1
            if(path=='L')   //默认向左扫线
            {
                //先向左边扫线，寻找右边界点
                for(cloum=Mid;cloum-BORDER_BIAS>0;cloum--)    //向左扫
                {
                    if(BinaryImage[row][cloum]==IMAGE_WHITE && BinaryImage[row][cloum-BORDER_BIAS]==IMAGE_WHITE)  //找到白点（从赛道外扫到赛道内：黑-白）
                    {
                        RightLine[row]=cloum;    //记录右边界点（向左找到的是右边界点）
                        flag_r=1;               //flag做无丢线标记
                        break;
                    }
                }
                //根据上面扫线的结果判断丢失的赛道是在左边还是右边从而决定继续向哪边扫线
                if(flag_r==1)   //找到了右边界（丢失的赛道在左边）
                {
                    black_num=0;
                    //继续向左寻找左边界
                    for(;cloum-BORDER_BIAS>0;cloum--)    //继续向左扫
                    {
                        if(BinaryImage[row][cloum]==IMAGE_BLACK)    //找到黑点：（从赛道内扫到赛道外：白-黑）
                        {
                            black_num++;
                        }
                        else    //保证连续扫点
                        {
                            black_num=0;
                        }
                        if(black_num>=BASIC_GARAGE)   //连续扫到十个黑点
                        {
                            LeftLine[row]=cloum+BASIC_GARAGE;   //记录左边界点
                            flag_l=1;              //flag做无丢线标记
                            black_num=0;
                            break;
                        }
                    }
                }
                else            //没有找到右边界（丢失的赛道在右边）
                {
                    for(cloum=Mid;cloum+BORDER_BIAS<MT9V03X_W-1;cloum++)    //向右扫
                    {
                        if(BinaryImage[row][cloum]==IMAGE_WHITE && BinaryImage[row][cloum+BORDER_BIAS]==IMAGE_WHITE)
                        {
                            LeftLine[row]=cloum;   //记录左边界点（向右找到的是左边界点）
                            flag_l=1;              //flag做无丢线标记
                            break;
                        }
                    }
                    if(flag_l==1)   //找到了左边界（丢失的赛道在右边）
                    {
                        black_num=0;
                        for(;cloum+BORDER_BIAS<MT9V03X_W-1;cloum++) //继续向右扫
                        {
                            if(BinaryImage[row][cloum]==IMAGE_BLACK)
                            {
                                black_num++;
                            }
                            else
                            {
                                black_num=0;
                            }
                            if(black_num>=BASIC_GARAGE)   //连续扫到十个黑点
                            {
                                RightLine[row]=cloum-BASIC_GARAGE;   //记录右边界点
                                flag_r=1;               //flag做无丢线标记
                                black_num=0;
                                break;
                            }
                        }
                    }
                }
            }
            else if(path=='R')  //默认向右扫线
            {
                //先向右边扫线，寻找左边界点
                for(cloum=Mid;cloum+BORDER_BIAS<MT9V03X_W-1;cloum++)    //向右扫
                {
                    //判断左边界点
                    if(BinaryImage[row][cloum]==IMAGE_WHITE && BinaryImage[row][cloum+BORDER_BIAS]==IMAGE_WHITE)  //找到白点（从赛道外扫到赛道内：黑-白）
                    {
                        LeftLine[row]=cloum;    //记录左边界点（向右找到的是左边界点）
                        flag_l=1;               //flag做无丢线标记
                        break;
                    }
                }
                //根据上面扫线的结果判断丢失的赛道是在左边还是右边从而决定继续向哪边扫线
                if(flag_l==1)   //找到了左边界（丢失的赛道在右边）
                {
                    black_num=0;
                    //继续向右寻找右边界
                    for(;cloum+BORDER_BIAS<MT9V03X_W-1;cloum++) //继续向右扫
                    {
                        //判断右边界点
                        if(BinaryImage[row][cloum]==IMAGE_BLACK)    //找到黑点（从赛道内扫到赛道外：白-黑）
                        {
                            black_num++;
                        }
                        else
                        {
                            black_num=0;
                        }
                        if(black_num>=BASIC_GARAGE)
                        {
                            RightLine[row]=cloum-BASIC_GARAGE;   //记录左边界点
                            flag_r=1;               //flag做无丢线标记
                            black_num=0;
                            break;
                        }
                    }
                }
                else            //没有找到左边界（丢失的赛道在左边）
                {
                    for(cloum=Mid;cloum-BORDER_BIAS>0;cloum--)    //向左扫
                    {
                        //判断右边界点
                        if(BinaryImage[row][cloum]==IMAGE_WHITE && BinaryImage[row][cloum-BORDER_BIAS]==IMAGE_WHITE)
                        {
                            RightLine[row]=cloum;   //记录右边界点（向左找到的是右边界点）
                            flag_r=1;               //flag做无丢线标记
                            break;
                        }
                    }
                    if(flag_r==1)   //找到了右边界（丢失的赛道在左边）
                    {
                        black_num=0;
                        //继续向左寻找左边界
                        for(;cloum-BORDER_BIAS>0;cloum--)   //继续向左扫
                        {
                            //判断左边界点
                            if(BinaryImage[row][cloum]==IMAGE_BLACK)  //判断右边界点
                            {
                                black_num++;
                            }
                            else
                            {
                                black_num=0;
                            }
                            if(black_num>=BASIC_GARAGE)
                            {
                                LeftLine[row]=cloum-BASIC_GARAGE;    //记录左边界点
                                flag_l=1;               //flag做无丢线标记
                                black_num=0;
                                break;
                            }
                        }
                    }
                }
            }
        }
        //在赛道中，正常扫线
        else
        {
            num=0;  //打断中线连续丢失，=0
            //左边扫线
            black_num=0;
            for(cloum=Mid;cloum-BORDER_BIAS>0;cloum--)              //向左扫
            {
                if(BinaryImage[row][cloum]==IMAGE_BLACK)  //判断左边界点（BORDER_BIAS防偶然因素）
                {
                    black_num++;
                }
                else
                {
                    black_num=0;
                }
                if(black_num>=BASIC_GARAGE)
                {
                    LeftLine[row]=cloum+BASIC_GARAGE;    //记录左边界点
                    flag_l=1;               //flag做无丢线标记
                    black_num=0;
                    break;
                }
            }
            //右边扫线
            black_num=0;
            for(cloum=Mid;cloum+BORDER_BIAS<MT9V03X_W-1;cloum++)    //向右扫
            {
                if(BinaryImage[row][cloum]==IMAGE_BLACK)  //判断右边界点（BORDER_BIAS防偶然因素）
                {
                    black_num++;
                }
                else
                {
                    black_num=0;
                }
                if(black_num>=BASIC_GARAGE)
                {
                    RightLine[row]=cloum-BASIC_GARAGE;   //记录右边界点
                    flag_r=1;               //flag做无丢线标记
                    black_num=0;
                    break;
                }
            }
        }

        if(flag_l==0)   //左边界丢线
        {
            LeftLine[row]=0;            //左边界点等于图像的左边界
            LostNum_LeftLine++;         //左丢线数+1
        }
        if(flag_r==0)   //右边界丢线
        {
            RightLine[row]=MT9V03X_W-1; //右边界点等于图像的右边界
            LostNum_RightLine++;        //右丢线数+1
        }
        CentreLine[row]=(LeftLine[row]+RightLine[row])/2;   //计算中线点
        //为下一次扫线做准备
        Mid=CentreLine[row];    //以上一次的中线值为下一次扫线的中间点
        flag_l=0;               //左边界丢线flag置0
        flag_r=0;               //右边界丢线flag置0

    }
}