基于MPC轨迹跟踪控制算法 模型预测控制算法 基于车辆动力学模型 可以实现在不同路面附着系数跟...

基于MPC轨迹跟踪控制算法 模型预测控制算法 基于车辆动力学模型 可以实现在不同路面附着系数跟踪期望路径 车辆稳定性较好 本算法仅供学习 推荐使用carsim8.1版本matlab2016b 图示为双移线测试工况直接上干货今天咱们聊聊用模型预测控制MPC做车辆轨迹跟踪的那些事儿。这个算法最带劲的地方就是能边开车边算未来几步的动作比传统PID那种事后补救的玩法强太多了。先看车辆动力学模型的核心部分。这里用的是经典的二自由度自行车模型但咱们给它加了个料——路面摩擦系数μ直接参与轮胎力计算。这样在不同抓地力的路面上控制器自己就知道该怂还是该刚。function dx vehicle_model(t, x, u, mu) % 状态量: x[横向偏差, 偏向角偏差, 横摆角速度, 纵向速度] % 控制量: u前轮转角 delta u(1); vx x(4); % 轮胎侧偏刚度随摩擦系数变化 C_alpha mu * 80000; % 前后轴侧向力 Fyf C_alpha * (delta - (x(3)*1.2)/(vx0.1)); Fyr C_alpha * (- (x(3)*1.5)/(vx0.1)); % 动力学方程 dx zeros(4,1); dx(1) x(2)*vx; % 横向偏差变化率 dx(2) (Fyf Fyr)/(1500*vx) - x(3); % 偏向角变化率 dx(3) (1.2*Fyf - 1.5*Fyr)/2500; % 横摆角加速度 dx(4) 0; % 假设纵向速度恒定 end注意看第6行那个mu参数这就是让算法适应不同路面的关键。当我们在雪地μ0.3和干地μ0.8切换时控制器会自动调整轮胎力的预估。基于MPC轨迹跟踪控制算法 模型预测控制算法 基于车辆动力学模型 可以实现在不同路面附着系数跟踪期望路径 车辆稳定性较好 本算法仅供学习 推荐使用carsim8.1版本matlab2016b 图示为双移线测试工况MPC的核心在于滚动优化这段QP求解代码是灵魂所在for k 1:N % 构建预测模型的A,B矩阵 [A, B] linearize_model(x_pred(:,k), mu_current); % 填充QP矩阵 Q_bar((k-1)*nx1:k*nx, (k-1)*nx1:k*nx) Q; R_bar((k-1)*nu1:k*nu, (k-1)*nu1:k*nu) R; % 等式约束矩阵动力学耦合 if k N Aeq((k)*nx1:(k1)*nx, (k-1)*nx1:k*nx) -A; Aeq((k)*nx1:(k1)*nx, k*nx1:(k1)*nx) eye(nx); Beq((k)*nx1:(k1)*nx) B*U_prev(:,k); end end这里有个骚操作——第7行用到了前一步的控制量U_prev。这其实是热启动技巧把上次求解的结果作为本次迭代的初始猜测能减少30%以上的计算时间。在Carsim联合仿真时千万注意采样时间同步。实测发现用Matlab的tic-toc比定时器更靠谱sim_time 10; ts 0.05; loop_cnt 0; tic while toc sim_time % 获取Carsim实时数据 [yaw_rate, beta] cscom(get_signal, 2, 1); % 状态预测 x0 [lat_err; beta; yaw_rate; vx]; % 求解MPC [u_opt, feas] solve_mpc(x0); % 发送前轮转角 if feas cscom(send_delta, u_opt(1)); else % 触发降级控制 emergency_handler(); end % 硬核等待 while toc ts*loop_cnt pause(0.0001) end loop_cnt loop_cnt 1; end第18行的忙等待看着不优雅但实际测试比用定时器少了30%的抖动。注意这个ts要跟Carsim的仿真步长一致不然会出现控制晚点的鬼畜现象。最后说几个踩过的坑Carsim8.1的64位版本和Matlab2016b存在内存映射兼容问题建议用32位版轮胎模型线性区间外的预测会崩需要加dU约束限幅低附路面要把预测时域缩短到0.8秒以内否则容易发散跑出来的双移线轨迹跟刀切的一样直就算突然切到低附路面车子也只是稍微扭下屁股马上找回节奏。这算法虽然不能直接上车但用来理解MPC的套路绝对够劲。下次试试加上纵横向联合控制那才是真的秀