AUV路径规划与MPC跟踪控制的Matlab复现全流程解析

复现一篇水下机器人方向的顶刊论文，最折磨人的往往不是算法本身，而是你明明照着论文搭好了模型，仿真曲线却跟论文里的对不上。这段时间我刚把“AUV路径规划 + MPC（模型预测控制）跟踪控制”的Matlab复现代码完整跑通，从全局路径生成到局部轨迹跟踪，最终误差控制在厘米级。这篇文章想把整个复现过程从头到尾说清楚：怎么搭AUV运动学模型、怎么用A*生成全局路径、怎么把路径点变成MPC可用的参考轨迹、MPC控制器内部每一步在算什么，以及调试过程中最容易翻车的地方。内容偏实战，适合正在做无人系统控制、机器人导航相关研究，或者想快速把论文方法落地到Matlab仿真里的朋友。看过之后照着操作，至少能省掉几个月的摸索时间。

1. 复现思路：先搞清楚论文里的“规划+跟踪”到底怎么串联

1.1 这个系统要解决的实际问题

水下机器人（AUV）作业时通常要经历两个阶段：一是根据任务起点和终点，在已知或部分已知的海底环境中生成一条安全、平滑的全局路径；二是沿着这条路径，在受到水流扰动、模型误差的情况下精确跟踪，不能偏离太远。

很多入门者会把“路径规划”和“轨迹跟踪”当成两个独立模块分别去复现，结果两个模块单独跑都没问题，连起来却完全失控。实际上，顶刊论文通常把全局规划结果作为MPC跟踪的参考轨迹，形成一个“规划–跟踪”闭环。复现时最关键的第一步，就是把这个闭环的接口协议定义清楚。

这个系统到底解决了什么问题？往大了说是提高AUV的自主作业能力；往具体说，是让机器人从A点走到B点的过程中，既能避开已知障碍物，又能在不违反自身执行器物理限制的前提下，把实际位置和参考路径之间的偏差压到最小。MPC是为了处理“带约束的跟踪问题”，而路径规划则是为MPC提供一条可行、平滑的“期望走廊”。两者不是替代关系，而是上下游关系。

1.2 坐标系与运动学模型的选择

我复现的论文背景是水平面内的AUV路径跟踪，所以只用到两个坐标系：

• 全局坐标系（惯性系）：固定在地球上，用 ( (x, y, \psi) ) 表示AUV的北向位置、东向位置和航向角。
• 随体坐标系：固定在AUV上，通常用 ( (u, v, r) ) 表示纵向速度、横向速度和偏航角速度。

很多AUV动力学简化模型会忽略横漂速度 ( v )，因为常规水下机器人在水平面内以小侧滑角航行时，( v ) 对运动轨迹的影响很小。于是三自由度水平面运动学模型可以写成：

[ \dot{x} = u \cos\psi, \quad \dot{y} = u \sin\psi, \quad \dot{\psi} = r ]

在Matlab里离散化就是：

function [x_next, y_next, psi_next] = update_kinematics(x, y, psi, u, r, Ts) x_next = x + u * cos(psi) * Ts; y_next = y + u * sin(psi) * Ts; psi_next = psi + r * Ts; end

这个模型没有考虑升沉、纵倾和横滚，对于水平面定深直航控制的复现来说足够用。如果你是第一次接触AUV建模，不要一上来就考虑六自由度模型，状态变量一多，MPC的预测矩阵规模会迅速膨胀，调参难度也会指数级上升。先把平面模型复现活，再往里面加扰动和动力学项，是更务实的一条路线。

1.3 路径规划模块和MPC模块的接口设计

这是我最想强调的一点。路径规划器输出的是“路径点数组”，MPC需要的是“带时间戳的参考状态序列”。如果不把这两个数据结构对齐，后续代码一定乱。

我的做法是定义参考状态结构体：

ref.x = interp1(t_path, x_smooth, t_mpc)'; ref.y = interp1(t_path, y_smooth, t_mpc)'; ref.psi = psi_smooth; % 由路径切向角换算得到 ref.u = u_ref * ones(length(t_mpc), 1); % 期望前向速度 ref.r = zeros(length(t_mpc), 1);

其中t_mpc是仿真控制周期的时间轴，t_path是路径规划模块根据期望速度生成的时间戳。接口统一之后，MPC在每个控制周期只要取ref中从当前时刻开始的 ( N_p ) 个点，就能直接构造代价函数，完全不需要关心路径是怎么生成的。

2. 全局路径规划的Matlab实现：A*搜索到平滑轨迹生成

2.1 栅格地图上的A*寻路

我复现时选用的是最经典的A*算法，原因有两个：一是实现简单，Matlab代码量小；二是它的确定性好，同样的地图、同样的起点终点，路径结果完全一致，方便排查跟踪问题。

核心函数骨架是这样的：

function path = astar_grid(map, start, goal) % map: 逻辑矩阵，true表示障碍物 % start, goal: 栅格坐标 [row, col] [rows, cols] = size(map); openList = []; closeList = []; gScore = inf(rows, cols); gScore(start(1), start(2)) = 0; parent = zeros(rows, cols, 2); % 启发函数用欧氏距离 hScore = @(r,c) sqrt((r - goal(1))^2 + (c - goal(2))^2); openList = [start, hScore(start(1),start(2))]; % 主循环省略... end

实际复现时我建议把启发式函数设计成“对角线距离”而不是“曼哈顿距离”，因为AUV栅格地图允许八方向移动，对角线距离能减少搜索节点的数量。当然，如果考虑AUV最小转弯半径，八方向仍然过于理想，但作为全局路径规划的第一步已经够用，后续可以用平滑算法弥补。

A*跑完得到的路径是栅格行列坐标，注意这一步千万不要把它当成米制坐标直接喂给MPC，否则后面控制量计算会完全错乱。统一做法是：

x_path = (path(:,2) - 1) * grid_size; % 列 -> x y_path = (path(:,1) - 1) * grid_size; % 行 -> y

2.2 路径平滑与冗余点剔除

A*生成的折线路径在转折处会产生很大的曲率，如果直接把折线点作为MPC参考路径，控制器为了硬追折点必然输出剧烈变化的控制量，看起来像是在抖舵。所以平滑是必不可少的一步。

我的做法分两步：

1. 剔除共线冗余点：如果相邻三个点近似共线，删除中间点，减少路径点数量。
2. 用累积弧长作为参数，对 ( (x,y) ) 分别做插值。我实际用的是Matlab的interp1配合pchip方法，因为它不会像普通三次样条那样在障碍物附近产生过度振荡。

t_raw = linspace(0, 1, size(path_raw, 1)); t_fine = linspace(0, 1, 500); x_smooth = interp1(t_raw, path_raw(:, 1), t_fine, 'pchip'); y_smooth = interp1(t_raw, path_raw(:, 2), t_fine, 'pchip');

注意插值之后路径点变密了，需要按弧长重新采样，使相邻参考点间距近似等于u_ref * Ts。这一步如果不做，MPC的时间参数化会出现严重偏差，后面会具体说。

2.3 从路径到参考轨迹：时间参数化与期望航向

路径是几何信息，跟踪需要时间信息。我采用的方案是：先计算路径累积弧长s，然后用期望巡航速度u_ref生成时间轴：

s = cumsum([0; sqrt(diff(x_smooth).^2 + diff(y_smooth).^2)]); t_path = s / u_ref;

这样每个路径点都对应一个时间戳，表示AUV“应当在什么时候到达哪里”。期望航向psi_ref则由路径切向角计算：

psi_ref = atan2(diff(y_smooth), diff(x_smooth)); psi_ref = unwrap(psi_ref); % 防止角度跳变

unwrap非常重要。如果不处理角度跳变，比如航向从-179°跳到179°，MPC里的误差项会突然变成358°，控制器会误以为发生了巨大误差，进而输出夸张的控制量。

3. MPC跟踪控制器的设计与Matlab落地

3.1 预测模型：在当前状态点线性化

MPC的核心是“利用模型预测未来，在线求解优化问题”。AUV运动学方程是非线性的，直接做非线性MPC当然可以，但Matlab里求非线性优化问题（fmincon）在实时性上比较吃亏，而且容易陷入局部最优。所以复现顶刊论文时，我更推荐用线性时变MPC（LTV-MPC）：每个控制周期以“当前状态”为工作点，对运动学方程做一阶泰勒展开，得到一个近似的线性状态空间模型。

具体推导如下：

状态量 ( X = [x, y, \psi]^T )，控制量 ( U = [u, r]^T )。

在当前工作点 ( (X_k, U_{k-1}) ) 处对运动学方程求雅克比矩阵：

A = eye(3) + Ts * [0, 0, -u_lin * sin(psi_lin); 0, 0, u_lin * cos(psi_lin); 0, 0, 0]; B = Ts * [cos(psi_lin), 0; sin(psi_lin), 0; 0, 1]; C = [u_lin * cos(psi_lin); u_lin * sin(psi_lin); r_lin] - A * X_lin - B * U_lin;

于是离散预测模型可以写成：

[ X(k+1) = A X(k) + B U(k) + C ]

这里的 ( C ) 是线性化余项，保证在工作点处模型和原始非线性模型完全一致。很多复现代码只算了 ( A ) 和 ( B )，忘记算 ( C )，导致预测轨迹偏掉。这是我调试时发现的第一个高频错误。

3.2 代价函数：跟踪误差、控制增量与松弛变量

MPC每一拍的优化目标是一个带约束的二次规划（QP）。我设计的代价函数包含三项：

1. 预测时域内状态与参考轨迹的偏差
2. 控制增量的大小，也就是控制量的变化率
3. 一个松弛变量，用来吸收约束不可行问题

用数学式子表达：

[ J = \sum_{i=1}^{N_p} e_{k+i}^T Q e_{k+i} + \sum_{i=0}^{N_c-1} \Delta U_{k+i}^T R \Delta U_{k+i} + \lambda_\epsilon \epsilon^2 ]

其中 ( e_{k+i} = X_{k+i} - X_{ref,k+i} )，( \Delta U_{k+i} = U_{k+i} - U_{k+i-1} )。

最后一项 ( \lambda_\epsilon \epsilon^2 ) 是松弛项，后面避坑章节会详细说它为什么必须存在。

在实际代码中，我会把预测状态写成初始状态和控制增量的线性函数，然后通过批量矩阵运算把代价函数整理成标准的QP形式：

[ J = \frac{1}{2} \Delta U^T H \Delta U + f^T \Delta U ]

这一步最大，也最容易出错。我的建议是先在草稿纸上把递推关系写清楚，再对照代码检查矩阵维度。Matlab里维度错一个位置，运行起来就是一片NaN。

3.3 调用quadprog求解控制量

用Matlab内置的quadprog求解QP问题。每次滚动的伪代码如下：

options = optimoptions('quadprog', 'Display', 'off', 'Algorithm', 'interior-point-convex'); % 构建H矩阵和f向量 H = 2 * (Mu' * Q_bar * Mu + R_bar + lambda_eps_penalty); f = 2 * ((Mx * x_now + Mc - Ref_seq)' * Q_bar * Mu); % 线性不等式约束（控制量和松弛变量的上下界） Aineq = [...]; bineq = [...]; lb = [...]; ub = [...]; % 求解 [U_opt, ~, exitflag] = quadprog(H, f, Aineq, bineq, [], [], lb, ub, [], options); if exitflag > 0 u_applied = U_prev(1) + U_opt(1); r_applied = U_prev(2) + U_opt(2); else % 如果无解，退化为只施加第一个控制量 warning('MPC solver failed, use previous control'); u_applied = U_prev(1); r_applied = U_prev(2); end

exitflag > 0的判断非常重要。实际仿真中，偶尔会遇到因为约束冲突导致QP无解的情况，如果不对exitflag做处理，程序会在仿真跑到一半时崩溃。退化到上一拍控制量是简单粗暴但有效的兜底方案。

3.4 参数调优：预测时域、控制时域和权重矩阵

MPC参数没有绝对标准，但我的经验是有一个安全起点：

• 采样周期 ( T_s = 0.1s )。如果路径曲率很大，建议减小到 ( 0.05s )。
• 预测时域 ( N_p = 20 \sim 30 )，对应预测未来 ( 2 \sim 3s ) 的运动。
• 控制时域 ( N_c = 5 \sim 10 )，不需要超过预测时域的一半。
• 状态权重矩阵 ( Q = \text{diag}([1, 1, 0.5]) )。位置误差权重给1，航向误差权重给0.5，不要给太高，否则控制器会为了追航向而大幅牺牲位置。
• 控制增量权重 ( R = \text{diag}([0.1, 0.5]) )。线速度增量权重小一点，角速度增量权重大一点，因为AUV偏航执行机构响应相对慢，过大的角速度增量会让舵效显得很“冲”。

调参的时候先固定场景，跑一组Np为10、20、30的对比曲线。你会发现预测时域越长，路径跟踪越平滑，但转弯时“切弯”现象越明显；预测时域太短，则跟踪误差大，甚至发散。这是MPC“前瞻性”的直接体现。

4. 复现中踩过的坑：从仿真发散到曲线贴合

4.1 线性化点选错导致预测全偏

我第一次跑通MPC时，用的是“参考路径点作为线性化点”，也就是在每个控制周期把模型在参考点 ( X_{ref} ) 处线性化，然后让预测轨迹从当前实际状态出发。结果在路径曲率大的地方，预测轨迹严重偏离真实运动，仿真曲线直接发散。

后来我才意识到一个关键问题：MPC的预测模型是用来描述“实际系统从现在状态出发将如何演化”的，而不是描述“期望系统的演化”。所以线性化点必须选在当前实际状态，参考轨迹只参与代价函数，不参与模型线性化。改掉这一点之后，控制器在整条路径上的表现立刻稳定下来。

4.2 约束不可行：松弛变量是必需品

AUV在跟踪大角度转弯路径时，如果要硬性满足所有控制量约束和状态约束，就可能出现“当前系统状态无论如何都无法在预测时域内回到参考轨迹上”的情况。此时QP求解器会返回无解，整段仿真会断掉。

我的解决方案是在优化变量里增加一个松弛变量 ( \epsilon )，把所有硬约束改写成软约束：

% 扩展决策变量维度，最后一个变量是epsilon n_theta = Nc * 2 + 1; lb = [lb_u; -1e3]; ub = [ub_u; 1e3]; % 在代价函数中给epsilon一个很大的权重 H(end, end) = 1e6;

这样即使约束暂时无法满足，松弛变量也会以很大的代价吸收这部分冲突，保证QP一定有解。实际操作中，只要epsilon权重足够大，它在正常跟踪过程中会接近0，不会影响控制精度。但如果发现松弛变量长时间不为0，说明路径规划或MPC权重设计有问题，需要回头调整而不是盲目加权重。

4.3 路径插值的时间戳与MPC参考信号不匹配

这个坑很隐蔽。我的路径规划模块一开始输出的是固定数量路径点，比如500个点，然后直接把这些点的序号当作时间轴传给MPC。结果MPC的参考速度完全错乱，因为路径点不是均匀分布的，用序号当时间等价于假设AUV在所有路径间移动耗时相同。

正确做法是按弧长生成时间轴：

arc_length = cumsum([0; sqrt(diff(x_smooth).^2 + diff(y_smooth).^2)]); time_stamp = arc_length / u_ref;

然后MPC在控制周期t = k * Ts处通过插值取参考状态。这样无论路径点如何分布，参考轨迹在时间上都是均匀的。这个问题不解决，误差指标会完全失真。

4.4 栅格坐标与全局坐标单位不统一

还有一次，我把A*输出的栅格坐标直接当成米制坐标传入MPC，结果控制量计算出来的数值巨大，因为栅格索引动辄几十，而实际位置只有几米。这个坑听起来低级，但真的很容易犯。

避免办法是显式定义“坐标变换层”：

在规划模块输出路径点后，立刻乘以grid_size转换为米制，之后所有模块只使用米制坐标。调试时，如果发现某一级的数值量级跟预期不符，第一件事就是检查单位。

5. 仿真结果与评价指标，以及还能怎么玩

5.1 跟踪误差怎么算：MAE、RMSE和横向偏差

仿真跑通后不能只看曲线重合。我用三个指标评估跟踪效果：

• 平均绝对误差（MAE）：反映整体跟踪精度。
• 均方根误差（RMSE）：对个别大的偏差更敏感。
• 最大横向偏差：反映最危险工况下的偏差极限。

在一张10m × 10m栅格地图、起点(1,1)、终点(9,9)、参考速度 ( 0.5m/s ) 的测试场景下，我复现的参数组合得到的结果大约如下：

这个量级的误差对于简化运动学模型来说已经相当不错。如果你的误差比这个大一个数量级，先检查前面提到的坐标系和线性化点问题，大概率能找到原因。

5.2 不同参数下的对比实验

为了验证MPC的预测特性，我做了两组对比。第一组固定 ( Q, R )，只改变预测时域 ( N_p )：

第二组固定 ( N_p )，改变 ( Q(3,3) )（航向权重）。航向权重从0.5提升到2后，航向误差明显减小，但位置误差反而增加，因为控制器更像是在“追航向”而不是“追轨迹”。这再次印证了前面说的：航向误差权重不宜过高。

仿真对比最大的价值不是找到一个“最优参数”，而是帮助你理解MPC各参数之间是如何博弈的。论文里给出的参数通常是在文章特定场景下调的，拷贝过来不一定适配你的问题，这也是复现论文时最容易产生挫败感的地方。

5.3 从复现到扩展：避碰、海流扰动与更智能的规划器

复现只是第一步。接下来可以做的扩展方向很多，我列几个自己觉得有价值的：

• 在MPC中直接加入障碍物约束，把已知障碍物转化为线性不等式约束或代价函数中的排斥项。这样AUV不仅能跟踪路径，还能在偏离全局路径时重新规划局部绕行动作。
• 加入海流扰动模型。AUV真实作业环境中海流是绕不开的干扰，把海流速度当作可测扰动前馈进预测模型，跟踪误差会进一步降低。
• 把A换成RRT或基于采样的方法，处理高维状态空间和非完整约束问题。全局路径的规划质量直接影响MPC控制难度。

我在实际复现中试过加入海流扰动，用的最简单方法是把运动学方程改成：

[ \dot{x} = u \cos\psi + v_{cx}, \quad \dot{y} = u \sin\psi + v_{cy} ]

其中 ( v_{cx}, v_{cy} ) 是海流速度在全局坐标系下的分量。这样一来，MPC的预测模型需要加入扰动估计，代价函数中也要增加抵抗扰动的控制能力。虽然只是很小的改动，但仿真结果更能反映真实水下环境。

最后再分享一点个人体会。这套复现看起来是“路径规划”和“MPC控制”两条技术线，实际上最核心的是“系统建模的一致性”。你花时间把AUV运动学模型、路径规划输出、MPC状态定义统一成长度单位、角度单位、时间步长完全一致的系统，后面所有环节都会顺畅很多；反过来，接口不对齐，调参调到怀疑人生都救不回来。这个坑我踩了整整一个周末，写出来也是希望后来人能少走点弯路。