可自定义期望轨迹的二自由动力学 MPC 跟踪控制 可以外部导入轨迹 知道x y s 即纵向位置 横向位置 位移量即可 请注意= 要跟踪不同的轨迹，同一参数可能效果不一样 因此需要自己调参数保证控制效果最佳： Q矩阵增大可以保证侧向位置跟踪效果变好， R 矩阵增大可以保证前轮转角曲线效果变好。 同时调整输出值边界的约束 carsim 版本 2019.0
二自由度模型这玩意儿大家都熟吧？横向运动方程写成离散状态空间就是：
% 模型参数 m = 1573; % 质量 Iz = 2873; % 转动惯量 lf = 1.34; % 前轴距 lr = 1.43; % 轮胎侧偏刚度 Ts = 0.1; % 控制周期 A = [1, Ts; -2*(Caf+Car)/(m*vx)*Ts, 1-2*(lf*Caf-lr*Car)/(Iz*vx)*Ts]; B = [0; 2*Caf/(m*vx)*Ts];重点在状态矩阵里的速度项vx——这货要是没处理好，换条轨迹准翻车。实测发现参考轨迹曲率越大，预测时域得适当缩短，不然求解器直接给你摆烂。
调参这事真得看手感，上周在秋名山实测时发现个邪乎现象：同一组Q、R参数在直道表现完美，到了发夹弯直接推头。后来悟了——Q矩阵里给横向误差的权重得跟着曲率动态变化。举个栗子：
# 动态权重调整示例 curvature = abs(ref_traj['kappa'][current_index]) Q_scale = 1 + 10 * curvature # 曲率越大，横向误差权重越高 Q = np.diag([Q_scale, 0.1]) # 状态权重矩阵 R = np.array([[0.01]]) # 控制量权重这么整之后，方向盘的修正动作明显更跟脚。不过要注意R矩阵别调太小，上次手贱设成0.001，结果前轮摆角曲线抖得跟帕金森似的。
约束边界这事容易被新手忽略。实测发现输出约束不能只设固定值：
// 根据车速调整前轮转角约束 double delta_max = 0.6 - 0.15 * fabs(current_speed/40); if(delta_max < 0.3) delta_max = 0.3; // 设置MPC约束 mpc.setConstraint(-delta_max, delta_max);速度越快，允许的最大转角越小，这招对防止高速甩尾贼管用。不过要注意别矫枉过正，有次把delta_max压到0.2，结果在U型弯直接冲出赛道。
最后给个CarSim联调的小技巧：把预测时域内的参考轨迹可视化到CarSim Scene中，效果拔群！具体操作是在Simulink里加个UDP发送模块，把预测轨迹打包发到CarSim的3D显示模块。调试时看着预测轨迹和实际轨迹慢慢重合，那成就感比通关老头环还带劲。
调完参记得做暴力测试——拿正弦波轨迹、阶跃轨迹、蛇形轨迹各跑一遍。别信仿真数据，真车测试时地面附着系数变化能让你参数当场破功。最近发现个玄学规律：Q矩阵权重开三次方后再用，鲁棒性莫名变好，原理还没搞懂，但真的好使。
好了，代码扔Github了，自己去扒拉。记住参数没有银弹，多调多翻车才是正道。下次聊聊怎么把MPC预测域玩出花，保准让你们方向盘的响应比女朋友还贴心（如果有的话）。
