智能车摄像头开源—9 动态权、模糊PID、速度决策、路径优化

一、前言

二、动态权

1.概述

2.偏差值加动态权

三、模糊PID

四、速度决策

1.曲率计算

2.速度拟合

3.速度控制

五、路径

六、国赛视频

一、前言

在前中期通过识别直道、弯道等元素可进行加减速操作实现速度的控制，可进一步缩减一圈的运行速度，但会出现刹车不及时、加速不及时、车路径较差等问题，反而导致处理后运行一圈时间反而多于之前。由此我们引入多个方法，使车辆能自行解算自行控制速度、转向力度等，以跑出最好效果。

当然有部分代码为了提高摩托运行稳定性的，对于四轮车模反而是一种限制，需读者使用时自行删减。

二、动态权

1.概述

如果图像有六十行，那我们可以得到六十行有效偏差值（弯道时有效行数会减少），我们在计算偏差值时，常用的方法是将所有有效行的偏差值相加，再取均值。

但调过车的都知道，当所有行取均值时，会导致弯道打角力度不够，直线打角力度过大导致车辆摆动的情况。

参考我们平时开车或者骑车时的情形，车快时我们眼睛就看的远，车速慢时我们眼睛就看的近。由此我们可以优化上述情况，引入加权。

最简单且繁琐的方法是引入固定权值（提前录入权值数组），即提前设定好不同元素的加权情况：

1. 如识别出直线时，我们将图像远端的权值加大，而图像中端和近端的权值减小，在远端出现弯道时，会让车辆高速情况下提前出现打角倾向。

2. 当贴近弯道时，我们将图像中端的权值加大，就可以减少底端较小偏差值的干扰，保证车的打角力度。

3. 识别出小S弯时，我们将权值情况如同直线处理，高权值放在远端，那么就可以惊奇的发现，车在跑小S弯时几乎无左右摆动情况，直线路径穿过小S弯，在提高车速的同时也减少了打角消耗的时间。

4. 在处理十字和环岛时，我们可以对多个状态设定权值，可以优化在元素不同状态时的转向效果。

但上述方法存在弊端：一方面元素与元素之间权值会切换的很生硬，使车辆出现摆动；另一方面当我们需要加速时，所有的元素权值都需要调整，十分繁琐。由此我们引入动态权。

2.给偏差值加动态权

控制动态权我们使用一个数据，为爬线相遇点的Y值（一个可以反映偏差值有效行的数据），那么在直线和小S弯等地方，Y贴近图像最远端，那么高权值就自动滑动到最远端。在由直线靠近弯道时，相遇点Y值不断下压，那么高权值就随着Y值向下滑动到中间区域，整个过程很丝滑，不会出现强硬的切换，并且十分易于修改。下面上代码：

            //相遇点滑动均值滤波uint8 Y_Meet_Max = 0;uint8 Y_Meet_Min = 0;uint16 Y_Meet_Sum = 0;uint8 Y_Meet_Average = 0;Y_Meet_Min = Y_Meet_Array[0];Y_Meet_Max = Y_Meet_Array[0];for(i = 0; i < 19; i++){Y_Meet_Array[i + 1] = Y_Meet_Array[i];}Y_Meet_Array[0] = start;for(i = 0; i < 20; i++){if(Y_Meet_Array[i] < Y_Meet_Min){Y_Meet_Min = Y_Meet_Array[i];}if(Y_Meet_Array[i] > Y_Meet_Max){Y_Meet_Max = Y_Meet_Array[i];}}for(i = 0; i < 20; i++){Y_Meet_Sum += (uint16)Y_Meet_Array[i];}Y_Meet_Sum = Y_Meet_Sum - (uint16)Y_Meet_Min - (uint16)Y_Meet_Max;Y_Meet_Average = (uint8)(Y_Meet_Sum / 18);if(Y_Meet_Average > 20){Y_Meet_Average = 20;}else if(Y_Meet_Average < 2){Y_Meet_Average = 2;}   //一直到这里都是华东均值滤波，滤的比较狠，防止数据跳动//        float Temp_Float = (0.9f * (float)(Y_Meet_Average - 2) + 1.8f * Sensitivity);float Temp_Float = (float)(Y_Meet_Average - 2);     //当直线时，我的Y_Meet值是2if(Temp_Float > 18.0f)      //限幅{Temp_Float = 18.0f;}if(Temp_Float < 0){Temp_Float = 0;}//固定权值uint8 Base_Weight[60] = {0, 0, 2, 0, 2, 0, 2, 0, 2, 0,      2, 0, 2, 0, 2, 0, 2, 0, 2, 0,      2, 0, 2, 0, 2, 0, 2, 0, 2, 0,2, 0, 2, 0, 2, 0, 2, 0, 2, 0,      2, 0, 2, 0, 2, 0, 2, 0, 2, 0,      2, 0, 2, 0, 2, 0, 2, 1, 0, 0};//局部动态权值，可根据车速自行调整区间和数值uint8 Trends_Weight[19] = {4, 5, 5, 6, 6, 7, 7, 8, 9, 10, 9, 8, 7, 7, 6, 6, 5, 5, 4};//动态权重加权起始行只能落在24 - 40行之间(由下往上)，相遇点最大值为20（减2为Temp_Float最大值为18）uint8 Start_Line = (uint8)(Temp_Float / 18.0f * 16.0f + 24.0f);//将动态权值赋值给固定权值数组for(i = 0; i < 19; i ++){Base_Weight[Start_Line - i] = Trends_Weight[i];}for(i = Y_Meet_Average + 1; i < 57; i++)    //加权，可直接加权给中线之后再算偏差值，也可直接加权给每行偏差值{Sum += (int32)Base_Weight[i] * (int32)C_Line[i];Weight_Sum += (int32)Base_Weight[i];}

识别出元素时也可适当给固定权保证车辆行驶的稳定性。

三、模糊PID

模糊PID，是对P, I, D其中一个变量或几个变量进行模糊，每模糊一个参数，就需要写一个对应的模糊表。当模糊多个参数时，调参工作量会增大。因此我们组别只模糊了专项环的P值

模糊PID重要的是原理，先上一张图：

（自己手绘潦草勿喷）

模糊的原理即查表映射，上述图中，a为偏差值；b网络上多为偏差值的差值，即偏差值的变化率（反映偏差值的变化情况，如直线时变化率很小，进入弯道时变化率会很大）；c就是对应的Kp值了。举例理解：我的偏差值区间为 -1 到 1 （其中负数向左转，正数向右转），那么我将（-1,1）等分为六个区间（七个值），作为模糊表的横向变量。列向变量也是同理。

那我们映射的时候，是获得一个偏差值，再获得一个偏差值的差值，然后在表中一查找直接对应到一个值作为转向环的P吗？

是也不是。我们会发现我们很少会获得一个a参数和b参数，其中a参数和 a1~a7中的某一个值正好对应，b参数也正好和b1~b7中的某一个值正好对应，绝大多数情况下两者是都无法正好相对的，那么我们就需使用占比对应的方式。

正如上述图中的式子，假设我们获得一个偏差a位于区间a5~a6，偏差的差值b位于区间b3~b4，那我们可列出如下式子：

Kp = [(b - b3）/ (b4 - b3)] * [(a - a5) / (a6 - a5)] * c1 +

[(b - b3）/ (b4 - b3)] * [(a6 - a) / (a6 - a5)] * c2 +

[(b4 - b）/ (b4 - b3)] * [(a - a5) / (a6 - a5)] * c3 +

[(b4 - b）/ (b4 - b3)] * [(a6 - a) / (a6 - a5)] * c4

不要觉得式子很长很繁琐，就是初中的知识，一眼就能看懂。理解了上述原理后，就懂了模糊PID查表的核心思路。然后阅读这篇文章（其中隶属度通俗点讲就是所占的比例，代码可看可不看）：

【智能车】模糊PID控制原理详解与代码实现

然后我们再细说下c，即如何在表格中填入Kp的值：

1. 首先我们让车以一个恒定速度行进，测出弯道转向时需要的Kp值，即PB（尽量使路径最好）；

2. 然后给一个较快的速度，测出直线时车身比较平稳的Kp值，即NB（尽量使路径最好）；

3. 然后将最大~最小值均分为七个值，对应上述文章里的NB ~ PB；

4. 最后将Kp的七个值，按照既定的规则表填入表格内（注意不要自己乱填，当自己能力足够时可自行优化规则表）

到这一步，相必已经完全了解模糊PID的原理和使用方法了，那么接下来就是代码的实现了。这里代码与上述方法相似但不相同，但核心原理与方法是完全一致的，只是实现的方式进行了改进。此处我的b使用的并不是偏差值的差值，而是爬线相遇点的Y值：

//除了注释了（需要改）的地方，其他地方直接照抄就可以float Dif_Effictive_Line = 40.0f;    //偏差值最大值
int16 View_Effictive_Line = 20;      //相遇点Y坐标的最大值（需要改）float P_Value_L[7] = {11.0f, 12.0f, 12.5f, 13.0f, 14.5f, 15.5f, 17.2f};    //这里没有等分，自己调一调车可摸出规律（需要改）float Vague_Array[4][4] = { {0, 1, 2, 3},{1, 2, 3, 4},{3, 4, 5, 6},{5, 6, 6, 6}};    //这个表原样抄下来float f_Get_H_approximation(int16 i16_ViewH)     
{float H_approximation;if (i16_ViewH < 0){i16_ViewH = 0;}H_approximation = ((float)i16_ViewH * 3.0f / (float)View_Effictive_Line);    //*3.0是为了将结果放大三倍return H_approximation;
}float f_Get_E_approximation(float i16_E)
{float E_approximation;if (i16_E < 0){i16_E = -i16_E;}E_approximation = (float)i16_E * 40.0f * 3.0f/ Dif_Effictive_Line;    //*3.0与上述同理，还多乘了个四十，与Dif_Effictive_Line 的值对应上return E_approximation;
}int16 Off_Line = 0;
int16 Now_Off_Line = 0;    //用于滤波
int16 Last_Off_Line = 0;    //用于滤波第一个参数输入相遇点Y坐标
第二个参数输入归一化后的偏差值
float Get_P(int16 off_line, float dif_value)
{Last_Off_Line = Now_Off_Line;Now_Off_Line = off_line;if(((Now_Off_Line - Last_Off_Line) >= 10) || ((Now_Off_Line - Last_Off_Line) <= -10) || Now_Off_Line >= 45)    //需要改{Off_Line = Last_Off_Line;}else{Off_Line = (int16)(0.3f * (float)(Now_Off_Line) + 0.7f * (float)(Last_Off_Line));}//下面这部分代入几个数据，结合整段代码计算几遍即可理解，只看的话比较吃力float VH = f_Get_H_approximation(off_line - 2);float VE = f_Get_E_approximation(dif_value);float X2Y = 0;float X1Y = 0;float Y2X = 0;float Y1X = 0;int8 VH1 = (int)VH;if (VH1 > VH){VH--;}int8 VH2 = VH1 + 1;int8 VE1 = (int8)VE;if (VE1 > VE){VE1--;}int8 VE2 = VE1 + 1;if (VH1 > 3){VH1 = 3;}if (VH2 > 3){VH2 = 3;}if (VE1 > 3){VE1 = 3;}if (VE2 > 3){VE2 = 3;}X2Y = (Vague_Array[VH1][VE2] - Vague_Array[VH1][VE1]) * (VE - VE1) + Vague_Array[VH1][VE1];X1Y = (Vague_Array[VH2][VE2] - Vague_Array[VH2][VE1]) * (VE - VE1) + Vague_Array[VH2][VE1];Y2X = (Vague_Array[VH2][VE1] - Vague_Array[VH1][VE1]) * (VH - VH1) + Vague_Array[VH1][VE1];Y1X = (Vague_Array[VH2][VE2] - Vague_Array[VH1][VE2]) * (VH - VH1) + Vague_Array[VH1][VE2];float P_approximation = (X2Y + X1Y + Y2X + Y1X) / 4.0;int8 P1 = (int8)P_approximation;if (P1 > P_approximation){P1--;}int8 P2 = P1 + 1;return (P_Value_L[P2] - P_Value_L[P1]) * (P_approximation - P1) + P_Value_L[P1]; //返回p值
}

四、速度决策

1.曲率计算

速度要变化起来，就需要有参考，即根据一个或两个实时求出的数据去控制权值的滑动。那么第一个变量我使用的是中线曲率：

曲率我尝试了很多算法，都不是很稳定，由此大道至简，直接将中线每两行之间的X坐标的差值相加求均值再归一化（归一化所除的系数在弯道处实测得出），就可以反映中线的弯曲程度，也近似可看为曲率。

注：这里每两行之间X坐标的差值不可取绝对值后再相加。在弯道时，中线是向一侧弯的，那么X坐标的差值的符号都是一样的，可以得到一个比较大的均值。而在小S弯处，X坐标的差值有正有负，那么相加之后求均值会获得一个比较小的值，这是将小S弯当直线处理的关键所在。

这里我的代码参照动态权的思想给曲率的计算也加了个小型化的动态权，即突出重点区域，使用爬线相遇点Y坐标（即图像最大有效行）控制权值移动，可自行修改这部分，根据实际情况选用是否需要这部分处理。

下面上代码：

float Last_Curvature_Value = 0;     //防止
/**
* 函数功能：      计算中线曲率，作为控制速度的一个数据
* 特殊说明：      无
* 形  参：        uint8 *line      中线
*                 uint8 start      爬线相遇点的Y值，或最大有效行
*                 uint8 end        爬线起始行（或图像最底端）
*
* 示例：          Calculate_Curvature_2(C_Line, Y_Meet, L_Start_Point[1]);
* 返回值：        Last_Curvature_Value      所得曲率
*/
float Calculate_Curvature_2(uint8 *line, uint8 start, uint8 end)
{int16 Sum_Difference_Value = 0;     //用于X坐标差值的累积int16 Sum_Weight = 0;               //用于权值相加uint8 i = 0;uint8 Base_Curvature_Weight[60] = {0, 0, 1, 0, 1, 0, 1, 0, 1, 0,1, 0, 1, 0, 1, 0, 1, 0, 1, 0,1, 0, 1, 0, 1, 0, 1, 0, 1, 0,1, 0, 1, 0, 1, 0, 1, 0, 1, 0,1, 0, 1, 0, 1, 0, 1, 0, 1, 0,1, 0, 1, 0, 1, 0, 1, 0, 1, 0};       //固定权值，0,1,0,1间隔加权uint8 Trends_Curvature_Weight[19] = {4, 5, 5, 6, 6, 7, 7, 8, 8, 8, 8, 8, 7, 7, 6, 6, 5, 5, 4};      //动态权值，高权值放中间uint8 Start_Line = (uint8)((float)(Y_Meet - 2) / 18.0f * 16.0f + 24.0f);    //动态权替换固定权起始行，我图像为60行，最低从24行向上替换，最高为40行向上替换，Y_Meet最大值为20//将动态权值赋值给固定权值数组for(i = 0; i < 19; i ++){Base_Curvature_Weight[Start_Line - i] = Trends_Curvature_Weight[i];}for(i = start; i < end - 1; i++)    //求差值和并求出权值{Sum_Difference_Value += (int16)(My_ABS((int)line[i] - (int)line[i + 1]) * (int)Base_Curvature_Weight[i]);Sum_Weight += (int16)Base_Curvature_Weight[i];}if(Sum_Weight != 0)     //防止权值为0，但好像不会出现，当时不知道为啥要写这个{Last_Curvature_Value = (float)Sum_Difference_Value / (float)Sum_Weight;     //求加权平均值，归一化放在了另一个函数里return Last_Curvature_Value;}else{return Last_Curvature_Value;}
}

2.速度拟合

除了曲率，速度拟合还需引入爬线相遇点Y值作为第二个参考数据，下面这部分代码思想有一定深度：

float Speed_Value[7] = {0.0f, 0.166f, 0.333f, 0.5f, 0.666f, 0.833f, 1.0f};  //速度采用了归一化，将1.0均分为六段float Speed_Vague_Array[4][4] = { {0, 1, 2, 3},{1, 2, 3, 4},{3, 4, 5, 6},{5, 6, 6, 6}};    //映射数组int16 Y_Meet_Count = 0;     //累积满足相遇点条件的次数，比如由弯道进入直道时，车身未转正时Y_Meet已经趋近于2了，此时加速会导致车轨迹不稳//那我们就进行一定的缓冲，当Y_Meet满足我们设定的条件一定次数后，再去执行相应的加速程序，可确保车身转正再加速
/**
* 函数功能：      使用相遇点和曲率映射速度，参照模糊PID思想
* 特殊说明：      无
* 形  参：        float sensitivity                //曲率
*                 float speed_min_proportion       //速度最小比率（设定最小速度与最大速度的比值），为0~1之间
*                 float Speed_max_proportion       //速度最大比率，通常为1
*                 uint8 y_meet                     //爬线相遇点的Y坐标
*                 uint8 min_y_meet                 //爬线相遇点的最小值，看得越远值越小
*                 uint8 max_y_meet                 //爬线相遇点的最大值，看得越远值越大
*
* 示例：          Calculate_Curvature_2(C_Line, Y_Meet, L_Start_Point[1]);
* 返回值：        Last_Curvature_Value      所得曲率
*/
float Speed_Mapping(float sensitivity, float speed_min_proportion, float Speed_max_proportion, uint8 y_meet, uint8 min_y_meet, uint8 max_y_meet)
{uint16 i = 0;float VH = 0, VE = 0;float X2Y = 0;float X1Y = 0;float Y2X = 0;float Y1X = 0;uint8 View_Effictive_Line = max_y_meet - min_y_meet;//弯道、环岛、十字时设定最大速度为35（这是我们组控制位给的一个值，换成自己组的）if(Element_State == L_Turn ||  Element_State == R_Turn || Element_State == L_Circle || Element_State == R_Circle || Element_State == Cross){Y_Meet_Count = 0;Max_Speed = 35;}//下面就是所说的缓冲代码，弯道到直道切换时，爬线相遇点Y趋近于2，当满足一千次以后（计算一千张图像），将最大速度设定为40//另一方面更重要的是为了防止误判，导致乱加减速//冲！冲！冲！冲！冲！冲！冲！冲！冲！冲！冲！冲！冲！冲！冲！冲！冲！冲！冲！冲！冲！冲！冲！冲！//冲！冲！冲！冲！冲！冲！冲！冲！冲！冲！冲！冲！冲！冲！冲！冲！冲！冲！冲！冲！冲！冲！冲！冲！//冲！冲！冲！冲！冲！冲！冲！冲！冲！冲！冲！冲！冲！冲！冲！冲！冲！冲！冲！冲！冲！冲！冲！冲！else if(Y_Meet <= 4 && Max_Speed == 35){Y_Meet_Count ++;if(Y_Meet_Count >= 1000){Y_Meet_Count = 0;Max_Speed = 40;}}//一次不满足就重新来，比较严格，可以不用或者判定放缓一些else if(Y_Meet != 2 && Max_Speed == 35){Max_Speed = 35;Y_Meet_Count = 0;}//直道到弯道的缓冲（作用于弯道在图像最远端，还未识别到弯道时），这个缓冲时间短一些，减速快些，这两个计数值根据自己情况调整else if(Y_Meet != 2 && Max_Speed == 40){Y_Meet_Count --;if(Y_Meet_Count <= -350){Y_Meet_Count = 0;Max_Speed = 35;}}//获取左右两侧边线落在边框上的个数Get_L_Border_Point_Num();Get_R_Border_Point_Num();//当识别出元素是直道，爬线相遇点位于顶端，左右两侧几乎没有落在边框上的点，并且中线曲率很小时，直接返回最大速度开冲！//此处判定十分严格，不会误判，不满足时才执行下方速度拟合部分代码if(Element_State == 2 && Y_Meet <= 3 && L_Border_Point_Num <= 2 && R_Border_Point_Num <= 2 && sensitivity <= 0.2f){return Max_Speed;}//下面使用曲率和爬线相遇点Y坐标拟合速度//与模糊PID原理相同，不过多注释VH = ((float)(Y_Meet - 2) * 3.0f / (float)View_Effictive_Line);VE = sensitivity * 3.0f;int8 VH1 = (int8)VH;if (VH1 > VH){VH--;}int8 VH2 = VH1 + 1;int8 VE1 = (int8)VE;if (VE1 > VE){VE1--;}int8 VE2 = VE1 + 1;if (VH1 > 3){VH1 = 3;}if (VH2 > 3){VH2 = 3;}if (VE1 > 3){VE1 = 3;}if (VE2 > 3){VE2 = 3;}X2Y = (Speed_Vague_Array[VH1][VE2] - Speed_Vague_Array[VH1][VE1]) * (VE - VE1) + Speed_Vague_Array[VH1][VE1];X1Y = (Speed_Vague_Array[VH2][VE2] - Speed_Vague_Array[VH2][VE1]) * (VE - VE1) + Speed_Vague_Array[VH2][VE1];Y2X = (Speed_Vague_Array[VH2][VE1] - Speed_Vague_Array[VH1][VE1]) * (VH - VH1) + Speed_Vague_Array[VH1][VE1];Y1X = (Speed_Vague_Array[VH2][VE2] - Speed_Vague_Array[VH1][VE2]) * (VH - VH1) + Speed_Vague_Array[VH1][VE2];float Speed_approximation = (X2Y + X1Y + Y2X + Y1X) / 4.0;int8 Speed_1 = (int8)Speed_approximation;if (Speed_1 > Speed_approximation){Speed_1--;}int8 Speed_2 = Speed_1 + 1;return (1.0f - ((Speed_Value[Speed_2] - Speed_Value[Speed_1]) * (Speed_approximation - Speed_1) + Speed_Value[Speed_1])) * (float)(Max_Speed - Min_Speed) + (float)Min_Speed;   //返回拟合好的速度
}

3.速度控制

我们需要一个整体的函数进行速度的调控，减速拉伸部分有一定难度（也可以不使用），多读几次就好，下面上代码：

float Last_Sensitivity = 0;
float Temp_Sensitivity = 0;
float Speed_Proportion_Array[100] = {0};    //用于滑动均值滤波
/**
* 函数功能：      进行速度控制使用相遇点和曲率映射速度，参照模糊PID思想
* 特殊说明：      无
* 形  参：        uint8 Auto_Control_Flag           是否要车辆自行控制速度
*                                                   //当自动控制标志位为1时，速度根据中线曲率和相遇点进行拟合
*                                                   //当自动控制标志位为0时，速度由手动输入                              
*                 uint16 Manual_Control_Value       //手动控制速度
*
* 示例：          Fitting_Speed_Control(1, 0);
* 返回值：        无
*/
void Fitting_Speed_Control(uint8 Auto_Control_Flag, uint16 Manual_Control_Value, uint8 element, uint8 element_state, uint8 start, uint8 end)
{uint8 i = 0;if(Auto_Control_Flag == 1){float Temp_Speed_Proportion = 0;//计算中线曲率，用于速度控制Last_Sensitivity = Sensitivity;Temp_Sensitivity = Calculate_Curvature_2(C_Line, Y_Meet, L_Start_Point[1]) / 1.0f;//计算最小速度比率Speed_Min_Proportion = (float)Min_Speed / (float)Max_Speed;//Stretch_Coefficient：减速拉伸系数，此参数越大，减速距离越短，最大值为1//Stretch_Coefficient 等于手动设定一个曲率最小阈值，小于这个阈值时直接将曲率设为0if(Temp_Sensitivity <= Stretch_Coefficient){Sensitivity = 0.0f;}else    //将（阈值~1.0）之间的数进行拉伸，可以代入几个实际的数理解下这部分原理{float Temp_Float_Num = (Temp_Sensitivity - Stretch_Coefficient) * (1.0f / (1.0f - Stretch_Coefficient));Sensitivity = Limit_Float(Temp_Float_Num * 0.3f + Last_Sensitivity * 0.7f, 0.01f, 1.0f);    //曲率值滤波}Temp_Speed_Proportion = Speed_Mapping(Sensitivity, Speed_Min_Proportion, 1.0f, Y_Meet, 2, 12);  //计算速度Speed_Proportion = Limit_Float(Temp_Speed_Proportion, (float)Min_Speed, (float)Max_Speed);  //速度限幅//滑动均值滤波，防止速度突变float Speed_Proportion_Sum = 0;for(i = 0; i < 99; i ++){Speed_Proportion_Array[i + 1] = Speed_Proportion_Array[i];}Speed_Proportion_Array[0] = Speed_Proportion;for(i = 0; i < 100; i ++){Speed_Proportion_Sum += Speed_Proportion_Array[i];}Speed_Proportion = Speed_Proportion_Sum / 100.0f;}    else{//手动控制速度部分的代码相必就很简单了，我这里比较乱，需读者自行编写了}
}