复现一篇水下机器人方向的顶刊论文最折磨人的往往不是算法本身而是你明明照着论文搭好了模型仿真曲线却跟论文里的对不上。这段时间我刚把“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; % 行 - y2.2 路径平滑与冗余点剔除A*生成的折线路径在转折处会产生很大的曲率如果直接把折线点作为MPC参考路径控制器为了硬追折点必然输出剧烈变化的控制量看起来像是在抖舵。所以平滑是必不可少的一步。我的做法分两步剔除共线冗余点如果相邻三个点近似共线删除中间点减少路径点数量。用累积弧长作为参数对 ( (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在实时性上比较吃亏而且容易陷入局部最优。所以复现顶刊论文时我更推荐用线性时变MPCLTV-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(k1) A X(k) B U(k) C ]这里的 ( C ) 是线性化余项保证在工作点处模型和原始非线性模型完全一致。很多复现代码只算了 ( A ) 和 ( B )忘记算 ( C )导致预测轨迹偏掉。这是我调试时发现的第一个高频错误。3.2 代价函数跟踪误差、控制增量与松弛变量MPC每一拍的优化目标是一个带约束的二次规划QP。我设计的代价函数包含三项预测时域内状态与参考轨迹的偏差控制增量的大小也就是控制量的变化率一个松弛变量用来吸收约束不可行问题用数学式子表达[ J \sum_{i1}^{N_p} e_{ki}^T Q e_{ki} \sum_{i0}^{N_c-1} \Delta U_{ki}^T R \Delta U_{ki} \lambda_\epsilon \epsilon^2 ]其中 ( e_{ki} X_{ki} - X_{ref,ki} )( \Delta U_{ki} U_{ki} - U_{ki-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); endexitflag 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结果控制量计算出来的数值巨大因为栅格索引动辄几十而实际位置只有几米。这个坑听起来低级但真的很容易犯。避免办法是显式定义“坐标变换层”模块坐标系单位栅格地图行列坐标像素栅格路径后处理全局坐标米MPC控制器全局坐标米AUV运动学全局坐标米在规划模块输出路径点后立刻乘以grid_size转换为米制之后所有模块只使用米制坐标。调试时如果发现某一级的数值量级跟预期不符第一件事就是检查单位。5. 仿真结果与评价指标以及还能怎么玩5.1 跟踪误差怎么算MAE、RMSE和横向偏差仿真跑通后不能只看曲线重合。我用三个指标评估跟踪效果平均绝对误差MAE反映整体跟踪精度。均方根误差RMSE对个别大的偏差更敏感。最大横向偏差反映最危险工况下的偏差极限。在一张10m × 10m栅格地图、起点(1,1)、终点(9,9)、参考速度 ( 0.5m/s ) 的测试场景下我复现的参数组合得到的结果大约如下指标数值平均绝对跟踪误差0.032 mRMSE0.058 m最大横向偏差0.148 m线速度平均变化率0.06 m/s²角速度平均变化率0.12 rad/s²这个量级的误差对于简化运动学模型来说已经相当不错。如果你的误差比这个大一个数量级先检查前面提到的坐标系和线性化点问题大概率能找到原因。5.2 不同参数下的对比实验为了验证MPC的预测特性我做了两组对比。第一组固定 ( Q, R )只改变预测时域 ( N_p )预测时域平均误差最大横向偏差控制量变化剧烈程度Np 100.047 m0.210 m较剧烈Np 200.032 m0.148 m适中Np 300.030 m0.153 m更平滑但有切弯第二组固定 ( 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状态定义统一成长度单位、角度单位、时间步长完全一致的系统后面所有环节都会顺畅很多反过来接口不对齐调参调到怀疑人生都救不回来。这个坑我踩了整整一个周末写出来也是希望后来人能少走点弯路。