1. NMPC：当控制理论遇上贪吃蛇策略
非线性模型预测控制（NMPC）本质上是一场与未来博弈的数学游戏。就像高手玩贪吃蛇时既要紧盯当前移动方向，又要预判三秒后蛇尾的走向。这种"滚动优化+反馈校正"的机制，让NMPC在自动泊车、无人机控制等领域大放异彩。最近在复现四个经典案例时，我发现教科书从不会告诉你：当优化求解器突然报错时，很可能只是因为初始猜测不够"优雅"。
2. 自动泊车：用自行车模型驯服铁皮怪兽
2.1 车辆运动学建模的魔鬼细节
定义自行车模型时，那个0.5的魔术系数其实暗藏玄机——这是前轮转向角到等效自行车把手的转换因子。假设轴距L=2.1米，前轮中心到转向主销距离为0.5L时，转向几何会满足阿克曼原理。在Python中实现时，这个beta角计算必须用arctan而非直接除法，否则大角度转向时会引入模型误差：
python复制def vehicle_model(x, u):
beta = np.arctan(0.5 * np.tan(u[1])) # 关键！前轮转角到等效自行车把手的转换
dx = x[3] * np.cos(x[2] + beta) # x方向速度
dy = x[3] * np.sin(x[2] + beta) # y方向速度
dtheta = x[3] * np.sin(beta) / 2.1 # 横摆角速度
return vertcat(dx, dy, dtheta, u[0]) # 状态导数
警告：使用CasADi符号运算时，务必开启自动微分选项
Opti.jacobian_approximation('exact')，否则数值近似雅可比矩阵会导致优化速度下降10倍以上。
2.2 目标函数设计的艺术
好的目标函数就像科目二的评分标准——既要位置准，又要姿态帅。建议采用分层加权策略：
- 首要惩罚终端状态误差（停车到位）
- 次优考虑航向角偏差（车头方向）
- 最后约束控制量变化率（避免方向盘抽风）
在CasADi中实现时，采用二次型代价函数：
python复制# 状态权重矩阵
Q = diag([10, 10, 5, 1]) # x,y,θ,v权重
# 控制量权重矩阵
R = diag([0.1, 1]) # 加速度/转向角速度权重
cost = 0
for k in range(N):
cost += (x[:,k]-x_ref[:,k]).T @ Q @ (x[:,k]-x_ref[:,k])
cost += u[:,k].T @ R @ u[:,k]
2.3 实测避坑指南
- 预测时域陷阱：20步预测时，步长0.1秒效果最佳。超过1.5秒的预测会导致优化问题病态
- 曲率突变处理：在路径曲率突变点添加松弛变量，避免求解器崩溃
- 热启动技巧：用上一时刻的解作为初始猜测，计算耗时降低40%
3. 倒立摆上翻：从躺平到直立的暴力美学
3.1 动力学方程里的隐藏参数
倒立摆的微分方程看似简单，但那个b*x(2)阻尼项经常被忽视。实验室用的摆杆阻尼系数b≈0.001N·m·s/rad，而仿真时若设为0，会导致NMPC设计出"抽搐式"控制策略：
matlab复制function dxdt = pendulum(~,x,u)
g = 9.81; l = 0.3; % 摆长30cm
m = 0.2; J = 0.006; % 质量200g，转动惯量
b = 0.001; % 实测阻尼系数
dxdt = [x(2);
(m*g*l*sin(x(1)) - b*x(2) + u)/J];
end
3.2 能量成型法的神来之笔
初始猜测决定NMPC的成败。采用能量成型法计算初始控制序列：
- 计算当前机械能：
E = 0.5*J*θ_dot^2 + m*g*l*(1-cosθ) - 目标能量：
E_des = m*g*l*2（直立位置） - 生成虚拟控制量：
u_guess = k*(E_des - E)*sign(θ_dot*cosθ)
这套方法能让求解成功率从30%飙升至85%，特别是当摆杆处于水平位置时。
3.3 实时调试观察窗
| 现象 | 可能原因 | 解决方案 |
|---|---|---|
| 摆杆高频抖动 | 权重矩阵Q中角度权重过大 | 降低q_θ，增加q_θ_dot |
| 无法突破水平位置 | 预测时域太短 | 增加到1.5秒，步数30步 |
| 优化耗时过长 | 雅可比矩阵未自动生成 | 检查Opti的jacobian_approximation设置 |
4. 车辆轨迹跟踪：Frenet坐标系降维打击
4.1 直角坐标到Frenet的华丽转身
在直角坐标系下描述路径跟踪，约束条件会多到爆炸。转换到Frenet坐标系后，问题瞬间清爽：
python复制s = MX.sym('s') # 路径进度
d = MX.sym('d') # 横向偏移
theta_e = MX.sym('theta_e') # 航向角偏差
# 转换关系
x = x_path(s) - d*sin(psi_path(s))
y = y_path(s) + d*cos(psi_path(s))
theta = psi_path(s) + theta_e
其中x_path(s), y_path(s)是路径参数方程，psi_path(s)是路径切线角。这种表示法将横向误差和纵向运动解耦，约束条件减少50%。
4.2 曲率突变点的正则化处理
当路径曲率突变时（如直角弯），在目标函数中添加曲率变化率惩罚项：
python复制kappa = (dpsi_ds(s) + dtheta_e_ds) / (1 - d*curvature(s)) # 实际曲率
cost += 0.1 * dot(kappa, kappa) # 曲率变化惩罚
实测表明，在曲率突变点将预测时域压缩到0.5秒，同时将控制频率提升到50Hz，可避免车辆画出"蛇形走位"。
5. 无人机三维跟踪：四元数与欧拉角的相爱相杀
5.1 姿态表示的陷阱与救赎
虽然欧拉角+小角度假设能让动力学方程简洁明了：
cpp复制// 简化的姿态动力学
Eigen::Vector3d angular_acc = J.inverse() * (tau - omega.cross(J*omega));
但实际飞控必须使用四元数，因为当俯仰角接近90°时，欧拉角会出现万向节死锁。正确的四元数动力学方程为：
cpp复制Quaterniond q_dot = 0.5 * q * Quaterniond(0, omega.x(), omega.y(), omega.z());
Matrix3d R = q.toRotationMatrix(); // 旋转矩阵
5.2 耦合效应补偿技巧
无人机偏航控制滞后的根本原因，是转动惯量矩阵J的非对角项引入了滚转-偏航耦合。解决方法是在权重矩阵中添加交叉项：
python复制Q_att = np.array([
[10, 0, 0, 0.1], # 滚转
[0, 10, 0, 0.1], # 俯仰
[0, 0, 8, 0 ], # 偏航
[0.1,0.1,0, 5 ]]) # 角速度
调整后，在快速滚转时偏航跟踪误差减小60%。
6. NMPC调参心法：次优解的艺术
经过四个案例的锤炼，我总结出NMPC的黄金法则：在85%的次优解和2秒延迟的最优解之间，永远选择前者。三个关键调参原则：
- 预测时域：从系统主导时间常数的1/3开始调试
- 权重矩阵：先确定状态量纲，再用Bryson法则初始化
- 求解精度：相对容忍度设为1e-4是速度与精度的甜蜜点
最后记住：NMPC不是数学家的玩具，而是工程师的工具。当你在凌晨三点盯着发散的求解器时，不妨试试把终端约束改成软约束——这招救过我五篇论文的 deadline。