智能车摄像头控制：动态权、模糊 PID、速度决策与路径优化

智能车摄像头控制系统的核心算法，包括动态权值偏差计算、模糊 PID 控制策略、基于曲率和相遇点的速度决策以及路径优化方法。通过引入动态权重解决弯道打角力度问题，利用模糊 PID 调整转向环参数以适应不同工况。速度控制结合中线曲率与图像有效行数据实现自适应调节，并包含防误判缓冲机制。文章提供了相关 C 语言代码示例及原理说明，旨在提升车辆行驶速度与路径稳定性。

指针猎手发布于 2026/3/25更新于 2026/4/174 浏览

一、前言

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

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

二、动态权

1.概述

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

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

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

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

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

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

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);
(Y_Meet_Average > ) {
    Y_Meet_Average = ;
}  (Y_Meet_Average < ) {
    Y_Meet_Average = ;
}

 Temp_Float = ()(Y_Meet_Average - );

(Temp_Float > ) 
{
    Temp_Float = ;
}
(Temp_Float < )
{
    Temp_Float = ;
}

uint8 Base_Weight[] = {, , , , , , , , , , , , , , , , , , , , , , , , , , , , , , , , , , , , , , , , , , , , , , , , , , , , , , , };

uint8 Trends_Weight[] = {, , , , , , , , , , , , , , , , , , };

uint8 Start_Line = (uint8)(Temp_Float /  *  + );

(i = ; i < ; i ++) {
    Base_Weight[Start_Line - i] = Trends_Weight[i];
}
(i = Y_Meet_Average + ; i < ; i++) 
{
    Sum += (int32)Base_Weight[i] * (int32)C_Line[i];
    Weight_Sum += (int32)Base_Weight[i];
}

相关免费在线工具

加密/解密文本

使用加密算法（如AES、TripleDES、Rabbit或RC4）加密和解密文本明文。在线工具，加密/解密文本在线工具，online

Base64 字符串编码/解码

将字符串编码和解码为其 Base64 格式表示形式即可。在线工具，Base64 字符串编码/解码在线工具，online

Base64 文件转换器

将字符串、文件或图像转换为其 Base64 表示形式。在线工具，Base64 文件转换器在线工具，online

Markdown转HTML

将 Markdown（GFM）转为 HTML 片段，浏览器内 marked 解析；与 HTML转Markdown 互为补充。在线工具，Markdown转HTML在线工具，online

HTML转Markdown

将 HTML 片段转为 GitHub Flavored Markdown，支持标题、列表、链接、代码块与表格等；浏览器内处理，可链接预填。在线工具，HTML转Markdown在线工具，online

JSON 压缩

通过删除不必要的空白来缩小和压缩JSON。在线工具，JSON 压缩在线工具，online

//除了注释了（需要改）的地方，其他地方直接照抄就可以 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 值 }

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; //返回拟合好的速度 }

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 等于手动设定一个曲率最小阈值，小于这个阈值时直接将曲率设为 0 if(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 { //手动控制速度部分的代码相必就很简单了，我这里比较乱，需读者自行编写了 } }

智能车摄像头控制：动态权、模糊 PID、速度决策与路径优化

一、前言

二、动态权

1.概述

2.给偏差值加动态权

更多推荐文章

相关免费在线工具

三、模糊 PID

四、速度决策

1.曲率计算

2.速度拟合

3.速度控制

五、路径

智能车摄像头控制：动态权、模糊 PID、速度决策与路径优化

一、前言

二、动态权

1.概述

2.给偏差值加动态权

微信扫一扫，关注极客日志

更多推荐文章

相关免费在线工具

三、模糊 PID

四、速度决策

1.曲率计算

2.速度拟合

3.速度控制

五、路径