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

如识别出直线时，我们将图像远端的权值加大，而图像中端和近端的权值减小，在远端出现弯道时，会让车辆高速情况下提前出现打角倾向。
当贴近弯道时，我们将图像中端的权值加大，就可以减少底端较小偏差值的干扰，保证车的打角力度。
识别出小S弯时，我们将权值情况如同直线处理，高权值放在远端，那么就可以惊奇的发现，车在跑小S弯时几乎无左右摆动情况，直线路径穿过小S弯，在提高车速的同时也减少了打角消耗的时间。
在处理十字和环岛时，我们可以对多个状态设定权值，可以优化在元素不同状态时的转向效果。

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

2.给偏差值加动态权

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

cpp 复制代码

            //相遇点滑动均值滤波
            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值是2

            if(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 +