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

✨ 本团队擅长数据搜集与处理、建模仿真、程序设计、仿真代码、EI、SCI写作与指导毕业论文、期刊论文经验交流。✅ 专业定制毕设、代码✅如需沟通交流查看文章底部二维码1基于车辆动力学包络的模型预测路径规划器设计为了生成在极限工况下仍可被执行层跟踪的无碰路径构建了车路耦合广义动力学模型将车辆二自由度单轨模型与魔术公式轮胎模型相结合并引入路面附着系数的在线估计值作为参数。在模型预测控制框架内以规划路径的横向位置和航向角为状态以前轮转角变化率为控制输入预测时域设置为2.5秒步长0.05秒共50步。在每个预测步利用状态空间方程递推生成多条候选路径同时对每一个路径点施加约束最大前轮侧偏角不超过6度、轮胎总横向力不超过当前附着条件下极限的85%、车身侧向加速度不超过0.6g。碰撞约束通过将障碍物车辆膨胀为椭圆占据区并检查预测路径点与椭圆的最小距离如果距离小于安全阈值则淘汰该候选路径。优化目标函数同时考虑路径平滑性、与全局参考线的偏差、与障碍物的潜在风险指数。使用内点法C/GMRES高效求解该优化问题求解耗时低于15毫秒。规划出的路径是一组50个点的平滑曲线具有明确的可行域经Carsim验证在低附着路面μ0.35下的可跟踪性达98%明显优于不考虑动力学约束的传统A*路径可跟踪性78%。路径曲率连续且变化率受限保证了后续跟踪的平稳性。2S-T图动态规划与二次规划双层速度剖面生成在获取局部最优路径后将自车与前方动态障碍物的运动投影到Frenet坐标系下的S-T图中是生成纵向上安全速度的基础。首先利用动态规划在S-T图离散网格Δs2m Δt0.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, mu1.0, N50, Ts0.05): n_state 4; n_ctrl 1 x cp.Variable((n_state, N1)); 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[:,k1] Ax[:,k] Bu[:,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(solvercp.OSQP) return x.value, u.value # S-T图动态规划速度规划 def st_dynamic_programming(path, obstacles_s, v_max30, dt0.2, T10): 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 0s_news_points and not collides_st(s_new, t1, obstacles_s): cost_new cost_grid[s,t] a**2 0.5*(v_new - 25)**2 if cost_new cost_grid[s_new, t1]: cost_grid[s_new, t1] cost_new; action[s,t] a return cost_grid, action # 联合横纵向MPC跟踪 def lateral_long_mpc_control(cur_state, traj_points, v_profile, N20): n_x4; n_u2 x_var cp.Variable((n_x, N1)); 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[:,k1]Ax_var[:,k]Bu_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如有问题可以直接沟通