水下机器人非线性MPC路径规划实战解析

1. 无人潜水艇路径优化问题概述

水下机器人路径规划一直是个让人头秃的领域，特别是当你要在布满珊瑚礁的复杂地形里穿行时。去年我在做一个水下探测项目时，就深刻体会到了传统PID控制在三维空间避障时的无力感——那感觉就像用算盘解微分方程。

模型预测控制(MPC)之所以成为水下避障的终极武器，核心在于它独特的"预判-修正"机制。想象一下你在玩贪吃蛇游戏：普通控制是看到障碍才转向，而MPC是提前计算未来5步的走位，主动避开所有可能的碰撞。这种基于模型的前馈控制，特别适合水下这种传感器反馈延迟严重的场景。

潜水艇的运动学模型本质上是个六自由度(6DOF)的刚体动力学问题。不同于地面车辆，水下环境还要考虑流体动力学效应。最要命的是，推进器产生的力矩会与横滚角、俯仰角强烈耦合——这就注定了我们必须跟非线性方程死磕到底。

2. 非线性动力学建模实战

2.1 状态空间方程构建

潜水艇的完整状态变量通常包含：

• 位置(x,y,z) ∈ R³
• 欧拉角(φ,θ,ψ) ∈ SO(3)
• 线速度(vx,vy,vz) ∈ R³
• 角速度(p,q,r) ∈ R³

用CasADi构建模型时，我习惯把角速度也纳入状态变量，这样总共需要12维状态向量。不过实际项目中可以根据运动特性做简化，比如忽略垂荡运动时就降到9维。

旋转矩阵的推导是第一个难点。以Z-Y-X欧拉角为例，完整的旋转矩阵应该是：

python复制R = cs.vertcat(
    cs.horzcat(cosθ*cosψ, sinφ*sinθ*cosψ-cosφ*sinψ, cosφ*sinθ*cosψ+sinφ*sinψ),
    cs.horzcat(cosθ*sinψ, sinφ*sinθ*sinψ+cosφ*cosψ, cosφ*sinθ*sinψ-sinφ*cosψ),
    cs.horzcat(-sinθ, sinφ*cosθ, cosφ*cosθ)
)

这个非线性变换会导致两个致命问题：

1. 万向节锁(Gimbal Lock)：当俯仰角θ接近±90°时，系统会丢失一个自由度
2. 三角函数耦合：使得雅可比矩阵的计算复杂度指数级上升

2.2 流体阻力建模技巧

水下阻力系数不是常数！我的经验公式是：

python复制drag_coeff = cs.vertcat(
    0.1*(1 + 0.5*cs.fabs(x[6])),  # x方向阻力与速度相关
    0.2*(1 + 0.3*cs.fabs(x[7])),
    0.3*(1 + 0.2*cs.fabs(x[8])),
    0.05, 0.05, 0.05  # 旋转阻尼
)

这种速度相关的阻力模型，能更好地模拟实际水下运动的非线性阻尼特性。不过会增加雅可比矩阵的计算负担，需要在精度和效率之间权衡。

3. MPC控制器设计与实现

3.1 线性时变近似方法

虽然系统本质是非线性的，但在每个采样周期内可以做局部线性化：

python复制# 在当前操作点(x_k, u_k)处线性化
A = cs.jacobian(model(x,u), x)
B = cs.jacobian(model(x,u), u)
C = A @ x_k + B @ u_k - model(x_k, u_k)  # 偏移项

这样得到的线性时变(LTV)模型，在预测时域内可以表示为：

code复制x_{k+1} = A_k x_k + B_k u_k + C_k

关键技巧：线性化时最好在期望轨迹附近多取几个操作点，避免单一线性化点导致的模型失配

3.2 混合约束处理策略

障碍物约束的处理很有讲究，我的方案是：

python复制# 球型障碍物约束
for obs in obstacles:
    dist = cs.norm_2(x[0:3,k] - obs.pos)
    opti.subject_to(dist > obs.radius + safe_margin)
    
# 洋流场约束
current_effect = current_field(x[0:3,k])
opti.subject_to(x[6:9,k] <= max_speed + current_effect)

这种将非线性约束直接嵌入QP问题的做法，虽然增加了求解难度，但能保证避障的可靠性。实测表明，相比完全线性化的方案，避障成功率能提升15%以上。

3.3 代价函数调参经验

代价函数设计是MPC的灵魂，我的黄金法则是：

1. 位置误差权重 > 姿态误差权重 > 控制量权重
2. 终端代价要占整体代价的20%-30%
3. 加入控制量变化率惩罚项避免抖动

典型配置如下：

python复制Q_pos = np.diag([10, 10, 15])  # z方向权重更高
Q_angle = np.diag([1, 1, 0.5])  # yaw角权重较低
R = np.diag([0.1, 0.1, 0.1, 0.05])  # 推进器功耗惩罚

for k in range(prediction_horizon):
    cost += (x[:,k]-x_ref[:,k]).T @ Q @ (x[:,k]-x_ref[:,k])
    cost += u[:,k].T @ R @ u[:,k]
    if k>0:
        cost += (u[:,k]-u[:,k-1]).T @ Rd @ (u[:,k]-u[:,k-1])

4. 工程实现中的坑与解决方案

4.1 实时性优化技巧

当预测时域N=20时，单次求解时间可能超过100ms。我采用的加速方案：

1. 热启动(Hot-start)：用上一周期的解作为初始猜测
2. 代码生成：将CasADi问题编译为C代码
3. 并行计算：使用ACADO工具箱的并行QP求解器

实测可以将求解时间从120ms压缩到35ms，满足50Hz的控制频率需求。

4.2 数值稳定性问题

欧拉角参数化会导致奇异点问题，我的解决方案是：

1. 当俯仰角接近±80°时，切换为四元数表示
2. 在代价函数中加入姿态误差的余弦相似度项：

python复制cost += 1 - cs.dot(R[:,2], R_des[:,2])  # z轴对齐程度

4.3 传感器噪声处理

水下声呐的噪声标准差可能达到0.3m，必须采用：

1. 移动 horizon估计(MHE)与MPC结合
2. 在状态约束中增加缓冲带：

python复制opti.subject_to(x[0:3,k] >= x_min + 0.5*noise_std)
opti.subject_to(x[0:3,k] <= x_max - 0.5*noise_std)

5. 实测性能对比分析

在南海某测试场进行的对比实验数据：

这个结果印证了非线性MPC的优越性，但也暴露出计算量大的问题。我的折中方案是：

• 正常巡航时使用线性MPC(50Hz)
• 进入障碍区切换非线性MPC(30Hz)

最后分享一个调试心得：MPC控制器的参数不能只看仿真结果，一定要做海试。我在水池里调得完美的参数，到真实海洋环境中会因为洋流扰动完全失效。后来发现需要在代价函数中加入"抗扰动项"：

python复制cost += 0.5*cs.norm_2(x[6:9,k] - current_estimate)**2

这样才能让潜水艇在洋流中保持稳定。