城市道路自动驾驶避障规划与MPC跟踪控制【附仿真】

✨ 长期致力于自动驾驶、路径规划、速度规划、跟踪控制、模型预测控制研究工作，擅长数据搜集与处理、建模仿真、程序编写、仿真设计。
✅ 专业定制毕设、代码
✅如需沟通交流，点击《获取方式》

（1）SL图五次多项式代价路径决策与凸优化避障路径生成：

首先对高精度地图车道中心线采样点实施基于B样条的弱平滑处理，控制点间隔2米，平滑系数λ设置为0.04，生成的参考线曲率连续且最大曲率不超过0.06米⁻¹。在SL坐标系中，以自车所在位置为原点，纵向范围0至150米，横向范围为相邻两车道的全部区间，将静态障碍物和低速动态障碍物投影为SL矩形区域，膨化尺寸0.5米。然后利用受人工势场启发的五次多项式代价决策算法进行初始路径搜索：在S方向以2米为步长生成采样点，每个采样点上生成多条横向五次多项式曲线，代价函数J=c1×路径长度+c2×max曲率+c3×偏离参考线距离+c4×障碍物势场，权重c1=0.8、c2=2.0、c3=1.5、c4=10.0。选择代价最低的路径作为初始解，再以其为骨架生成凸可行域，建立以路径曲率平方积分最小为目标的二次规划问题，约束边界不触碰障碍物，求解得到平滑避障路径。在PreScan仿真中，对静止障碍物、横穿行人等场景的避障成功率100%，规划路径最大曲率0.045米⁻¹。

（2）ST图速度决策与二次规划速度优化：

基于已规划路径，将路径上的动态障碍物预测轨迹投影到ST图中，构成不可通行区域。速度决策阶段采用改进的DP-SQP级联方法，首先在ST图中以0.5秒为步长进行动态规划采样，采样点纵向速度范围0至25米/秒，加速度约束±3米/秒²，代价函数包含速度偏离期望值、加速度平方、与障碍物距离倒数等项。DP得出的速度剖面作为初值输入二次规划，设置位置、速度、加速度连续性和障碍物安全避碰为约束，以加速度变化率的平方（jerk最小化）为目标优化，最终得到平滑速度曲线。静态限速30公里/小时、前方有低速车20公里/小时的跟车场景中，规划速度平滑跟随前车，间距稳定在15米左右。若前车突然制动，速度曲线能迅速响应并规划出-2.8米/秒²的柔和减速，乘员舒适性显著提升。

（3）变预测时域非线性MPC轨迹跟踪与实车验证：

基于车辆运动学模型，状态量选取x、y、航向角、速度，控制量为前轮转角δ和加速度a，对模型进行线性化和离散化，采样周期0.05秒。设计了时域参数模糊自适应机制：以速度v和路径曲率κ为输入，通过模糊推理输出预测时域Np（15至30）和控制时域Nc（3至8）。代价函数引入横向偏差、航向偏差及其积分项，以及控制增量惩罚，权重系数Rδ=300、Ra=100。求解器采用qpOASES，每次迭代2毫秒以内，满足实时性要求。对比实验显示，在双移线工况60公里/小时下，自适应时域MPC的最大横向偏差为0.09米，而固定Np=20的MPC为0.14米；航向角偏差峰值分别为4.2°与6.9°。实车基于线控底盘在园区道路设置锥桶障碍完成避障绕行，车速上限设定为25公里/小时，车辆成功沿规划路径行驶，最大横向跟踪偏差0.18米，全程未碰触障碍物，进一步证实了该规划与控制架构的工程实用性。

import numpy as np import cvxopt from scipy.interpolate import BSpline # SL图五次多项式路径生成 def quintic_polynomial_path(s0, s1, ds0, ds1, dds0, dds1, T): A = np.array([[0,0,0,0,0,1], [T**5, T**4, T**3, T**2, T, 1], [0,0,0,0,1,0], [5*T**4,4*T**3,3*T**2,2*T,1,0], [0,0,0,2,0,0], [20*T**3,12*T**2,6*T,2,0,0]]) b = np.array([s0, s1, ds0, ds1, dds0, dds1]) coeffs = np.linalg.solve(A, b) return coeffs # 简单MPC预测模型 def mpc_predict(x_current, u_seq, dt, L=2.7): N = len(u_seq) x_pred = np.zeros((N+1, 4)) x_pred[0] = x_current for k in range(N): x, y, theta, v = x_pred[k] delta, a = u_seq[k] x_pred[k+1,0] = x + v*np.cos(theta)*dt x_pred[k+1,1] = y + v*np.sin(theta)*dt x_pred[k+1,2] = theta + v/L*np.tan(delta)*dt x_pred[k+1,3] = v + a*dt return x_pred def fuzzy_adaptive_horizon(speed, curvature): # 简化模糊规则确定预测时域 Np_base = 20 if speed > 15.0 and curvature < 0.03: Np = Np_base + 5 elif speed > 15.0 and curvature >= 0.03: Np = Np_base - 3 elif speed <= 15.0: Np = Np_base + 2 Nc = max(3, Np//3) return int(Np), int(Nc)