基于MPC的智能车运动预测和控制算法 Motion predication； Kinemati...

基于MPC的智能车运动预测和控制算法 Motion predication; Kinematic model //. MATLAB coding //. 加入求解步骤进而得到自定义成本函数的可扩展MPC控制器; //. 模型状态空间方程线性化和离散化; //. 可与风险场/人工势场/决策/轨迹跟踪等算法集成; //. 需要用到车辆运动学模型的算法

把方向盘交给代码之前，得先让车知道自己是谁。在停车场倒库的场景里，我们给智能车构建了一个自行车模型——别误会，这可不是两轮自行车，而是把四个轮子简化为前后两轮的数学模型。MATLAB里这么定义状态向量：

states = @(x) [x(1); x(2); x(3); x(4)]; % X坐标,Y坐标,航向角,车速 controls = @(u) [u(1); u(2)]; % 前轮转角,加速度

这个简化模型暗藏玄机：前轮负责转向，后轮只管驱动。实际调试时会发现，当车速超过30km/h，模型误差开始明显变大——这时候就得换动力学模型了，但那是另一个故事。

要让MPC跑起来，得把连续时间模型切成时间片。用前向欧拉法离散化时，采样时间Δt的选择直接决定控制器是"近视"还是"远视"。举个栗子：

dt = 0.1; % 100ms控制周期 A_discrete = eye(4) + A_continuous*dt; B_discrete = B_continuous*dt;

代码里的A_continuous来自雅可比矩阵线性化。有趣的是，在高速场景下，有些团队会给Δt做速度自适应，但这会让预测时域变得不均匀，增加QP求解难度。

成本函数就像驾校教练的评分标准。下面这个函数既惩罚偏离路径，又限制急刹猛打方向：

function J = custom_cost(X, U, ref) path_error = sum((X(1:2,:) - ref(1:2,:)).^2); control_smooth = sum(diff(U).^2); J = 0.8*path_error + 0.2*control_smooth; end

权重的调整堪称玄学——某自动驾驶公司工程师透露，他们曾用强化学习自动调参，结果训练出疯狂点头的加减速策略，活像驾校新手。

把风险场引入MPC时，可以在成本项里加势能梯度。比如遇到突然窜出的行人：

pedestrian_risk = exp(-norm(X(1:2)-ped_pos)/(2*sigma^2)); J = J + 50*pedestrian_risk;

这个50的系数需要实际路测校准：太高会导致车辆在空旷路段也蛇形走位，太低则可能反应不足。

完整MPC求解流程在代码里呈现为循环优化问题。注意看qp求解器的输入如何随时间窗滑动：

for k = 1:N % 构建QP矩阵 H = ... % 二次型矩阵 f = ... % 线性项 % 添加道路边界约束 A_ineq = [road_constraints; dynamics_constraints]; b_ineq = [road_bounds; zeros(dynamics_dim,1)]; [U_opt, fval] = quadprog(H, f, A_ineq, b_ineq); % 执行第一控制量 apply_control(U_opt(1:2)); end

实际工程中，90%的bug出在约束矩阵的维度对齐问题。有团队曾因索引偏移导致车辆画龙，排查三天后发现是b_ineq少了个转置。

当集成轨迹跟踪时，会发现纯运动学模型在急弯处容易"画地图"——预测轨迹和实际轨迹偏差太大。这时需要引入路径曲率前馈：

delta_feedforward = atan(wheelbase * curvature); U(1) = delta_feedforward + delta_feedback;

某自动驾驶测试视频里，没有前馈项的车过弯时像醉汉左右摇摆，加上后立刻变得丝滑，可见底层控制的小细节决定用户体验。

最后留个思考题：当GPS信号丢失时，如何用MPC的预测状态做dead reckoning（航位推算）？答案藏在状态观测器的设计里，下次我们可以聊聊卡尔曼滤波如何与MPC暗通款曲。