nmpc非线性模型预测控制从原理到代码实践 含4个案例 自动泊车轨迹优化； 倒立摆上翻控制； 车辆运动学轨迹跟踪； 四旋翼无人机轨迹跟踪。
自动泊车这事挺反直觉，车屁股挪动的轨迹比考科目二还玄乎。先定义个自行车模型：
def vehicle_model(x, u): beta = np.arctan(0.5 * np.tan(u[1])) # 转向角换算 dx = x[3] * np.cos(x[2] + beta) dy = x[3] * np.sin(x[2] + beta) dtheta = x[3] * np.sin(beta) / 2.1 # 轴距2.1米 return vertcat(dx, dy, dtheta, u[0]) # 状态导数目标函数得惩罚位置偏差、车头朝向偏差，还得防止方向盘扭成麻花。在CasADi里搭优化问题时，特别要注意雅可比矩阵是否自动生成，否则求解速度能让你等到地老天荒。
倒立摆暴力上翻的代码更有意思。设置MPC的预测时域时，太短了算不出骚操作，太长了CPU扛不住：
% 倒立摆动力学方程 function dxdt = pendulum(~,x,u) dxdt = [x(2); (m*g*l*sin(x(1)) - b*x(2) + u)/J]; end这里有个坑：初始猜测选不好，求解器直接摆烂。实测用能量成型法给个粗略猜测，成功率能从30%飙升到85%。仿真时看到摆杆突然抽搐式发力，别慌，那是NMPC在尝试突破局部最优。
nmpc非线性模型预测控制从原理到代码实践 含4个案例 自动泊车轨迹优化； 倒立摆上翻控制； 车辆运动学轨迹跟踪； 四旋翼无人机轨迹跟踪。
车辆轨迹跟踪的核心在于路径参数化。别傻乎乎地用直角坐标，改用Frenet坐标系后，约束条件直接减半：
s = MX.sym('s') # 路径进度 d = MX.sym('d') # 横向偏移 theta_e = MX.sym('theta_e') # 航向角偏差处理曲率突变点时，记得在目标函数里加正则项，否则车辆容易走出六亲不认的步伐。实测在急弯处把预测时域压缩到0.5秒，控制频率提高到50Hz能避免翻车。
无人机三维跟踪最烧脑，四元数动力学方程能写满整个屏幕。简化方案是用欧拉角+小角度假设：
// 姿态动力学近似 Eigen::Vector3d angular_acc = J.inverse() * (tau - omega.cross(J*omega));但真到飞控代码里还得切回四元数，否则大机动时控制器直接懵圈。调试时发现偏航角控制总滞后，原来是权重矩阵没考虑耦合效应，调整后轨迹跟踪精度立马上个档次。
搞完这四个案例的最大收获：NMPC就像带预测功能的PID，只不过把玄学调参变成了数学优化。但小心别掉进完美主义的坑——有时候85%的次优解比等2秒求最优解更实用。
