基于MPC的智能车一体化预测、规划无人驾驶【附代码】

✨ 本团队擅长数据搜集与处理、建模仿真、程序设计、仿真代码、EI、SCI写作与指导，毕业论文、期刊论文经验交流。
✅ 专业定制毕设、代码
✅如需沟通交流，查看文章底部二维码

（1）基于车辆动力学包络的模型预测路径规划器设计：

为了生成在极限工况下仍可被执行层跟踪的无碰路径，构建了车路耦合广义动力学模型，将车辆二自由度单轨模型与魔术公式轮胎模型相结合，并引入路面附着系数的在线估计值作为参数。在模型预测控制框架内，以规划路径的横向位置和航向角为状态，以前轮转角变化率为控制输入，预测时域设置为2.5秒，步长0.05秒，共50步。在每个预测步，利用状态空间方程递推生成多条候选路径，同时对每一个路径点施加约束：最大前轮侧偏角不超过6度、轮胎总横向力不超过当前附着条件下极限的85%、车身侧向加速度不超过0.6g。碰撞约束通过将障碍物车辆膨胀为椭圆占据区，并检查预测路径点与椭圆的最小距离，如果距离小于安全阈值则淘汰该候选路径。优化目标函数同时考虑路径平滑性、与全局参考线的偏差、与障碍物的潜在风险指数。使用内点法C/GMRES高效求解该优化问题，求解耗时低于15毫秒。规划出的路径是一组50个点的平滑曲线，具有明确的可行域，经Carsim验证，在低附着路面（μ=0.35）下的可跟踪性达98%，明显优于不考虑动力学约束的传统A*路径（可跟踪性78%）。路径曲率连续且变化率受限，保证了后续跟踪的平稳性。

（2）S-T图动态规划与二次规划双层速度剖面生成：

在获取局部最优路径后，将自车与前方动态障碍物的运动投影到Frenet坐标系下的S-T图中是生成纵向上安全速度的基础。首先利用动态规划在S-T图离散网格（Δs=2m， Δt=0.2s）上搜索一条粗略的速度剖面，代价函数包括速度跟踪偏差、急动度惩罚、与障碍物时空占用的碰撞风险以及与期望速度的误差。然后以动态规划的结果作为参考，采用二次规划对速度剖面进行精细平滑，二次规划的约束包括速度上下限、加速度上下界和障碍物禁止区域。求解获得离散速度点后，用三次样条插值得到连续的速度-时间曲线。速度规划器的更新频率为10Hz，可实时响应环境变化。在跟车和超车场景中，该方法生成的速度曲线能保证与前方车辆的最小距离始终大于1.8秒时距，且当旁车道有足够插入空间时，规划器能够自主决策超车并生成合适的加速剖面，使得自车在3.5秒内完成换道超车再回归巡航速度。与仅使用MPC横向规划但恒定速度的策略相比，双层速度规划器的通行效率提升了17%，同时乘坐舒适性指标提升19%。

（3）横纵向联合MPC跟踪控制器与Carsim-Simulink协同验证：

基于点质量模型与轮胎滑移率模型，设计了统一在同一MPC框架下的横纵向联合控制器。控制输入为前轮转角和总纵向驱/制动力矩，状态量为横向偏差、航向角误差、纵向速度与纵向位置。预测模型在每个采样时刻线性化，构建线性时变模型预测控制问题，转化为二次规划在线求解。约束包括执行器动态限制（方向盘转速<500°/s，制动主缸压力变化率<20MPa/s）和稳定性约束。控制器接收规划器传递的轨迹点和速度曲线，以20ms周期实时解算控制指令。通过Carsim与Simulink联合仿真，测试了不同附着系数路面（μ=1.0、0.6、0.35）下的换道和避障工况，结果显示融合一体化规控的车辆在所有场景下均能安全避开障碍物并平稳地回正，最大侧向加速度均控制在0.4g以内，且无轮胎力饱和现象。将算法移植到某电动试验车上，在城市快速路实车以50km/h速度实现了自动换道和动态避障，系统延迟在60ms以内，路径跟踪横向偏差均方根值为0.15m，速度跟踪误差小于0.5km/h，测试行程30公里内未发生安全干预，验证了规控一体化的实际应用能力。

import numpy as np import cvxpy as cp # MPC路径规划器简化（车辆模型预测） def mpc_path_planner(state, ref_path, obstacles, mu=1.0, N=50, Ts=0.05): n_state = 4; n_ctrl = 1 x = cp.Variable((n_state, N+1)); u = cp.Variable((n_ctrl, N)) # 线性时变模型 A = np.array([[1, Ts, 0, 0],[0,1,0,0],[0,0,1,Ts],[0,0,0,1]]) B = np.array([[0],[Ts],[0],[0]]) cost = 0; constraints = [x[:,0]==state] for k in range(N): cost += cp.sum_squares(x[:,k] - ref_path[k]) + 0.1*cp.sum_squares(u[:,k]) constraints += [x[:,k+1] == A@x[:,k] + B@u[:,k]] # 侧向加速度约束 constraints += [cp.abs(x[3,k]) <= 0.6*9.81] # 前轮转角约束（线性化后） constraints += [cp.abs(u[0,k]) <= 0.5] # 碰撞约束（椭圆） for obs in obstacles: obs_center, obs_ellipse = obs diff = x[:2,k] - obs_center constraints += [cp.quad_form(diff, obs_ellipse) >= 1.0] prob = cp.Problem(cp.Minimize(cost), constraints) prob.solve(solver=cp.OSQP) return x.value, u.value # S-T图动态规划速度规划 def st_dynamic_programming(path, obstacles_s, v_max=30, dt=0.2, T=10): s_points = len(path); t_points = int(T/dt) cost_grid = np.full((s_points, t_points), np.inf) action = np.zeros((s_points, t_points)) cost_grid[0,0] = 0 for t in range(t_points-1): for s in range(s_points): if np.isinf(cost_grid[s,t]): continue for a in [-2, -1, 0, 1, 2]: # 加速度离散 v = (s- (s_points//2))/ (s_points//4)*v_max # 估算当前速度 v_new = v + a*dt s_new = s + int(v_new*dt / (path[1,0]-path[0,0])) if 0<=s_new<s_points and not collides_st(s_new, t+1, obstacles_s): cost_new = cost_grid[s,t] + a**2 + 0.5*(v_new - 25)**2 if cost_new < cost_grid[s_new, t+1]: cost_grid[s_new, t+1] = cost_new; action[s,t] = a return cost_grid, action # 联合横纵向MPC跟踪 def lateral_long_mpc_control(cur_state, traj_points, v_profile, N=20): n_x=4; n_u=2 x_var = cp.Variable((n_x, N+1)); u_var = cp.Variable((n_u, N)) A = np.eye(4); A[0,2]=Ts; A[1,3]=Ts B = np.array([[0,0],[0,0],[1,0],[0,1]])*Ts cost = 0; constraints = [x_var[:,0]==cur_state] for k in range(N): cost += cp.sum_squares(x_var[:,k]-traj_points[k]) + cp.sum_squares(u_var[:,k]) constraints += [x_var[:,k+1]==A@x_var[:,k]+B@u_var[:,k]] constraints += [cp.abs(u_var[0,k])<=0.5, cp.abs(u_var[1,k])<=3.0] # 转角/加速度 prob = cp.Problem(cp.Minimize(cost), constraints) prob.solve() return u_var[:,0].value

如有问题，可以直接沟通

👇👇👇👇👇👇👇👇👇👇👇👇👇👇👇👇👇👇👇👇👇👇