欠驱动AUV的轨迹跟踪与路径跟随仿真到底该怎么做做水下机器人控制的朋友应该都体会过那种“系统明明很简单控制却难以下手”的纠结。欠驱动AUV就是这样一类典型对象姿态和位置自由度比控制输入多尾部推进器和舵面提供的控制力有限横向和垂向运动基本不可直接操纵。这种系统的轨迹跟踪和路径跟随在控制圈里算是经典难题也是工程价值极高的研究点。最近我花了两周时间把这套问题从建模到仿真完整过了一遍基于Matlab代码和Simulink仿真两种形式对比了不同分析方法的差异。今天就把整个思路和实操过程整理出来给准备入坑或者正在纠结算法的朋友做一个参考。这项工作的目标很明确在同一套AUV动力学模型下用多种控制方法分别实现水平面内的轨迹跟踪和路径跟随并通过仿真数据对比各方法在跟踪精度、收敛速度、鲁棒性和计算开销方面的表现。适合自动化、船舶与海洋工程方向的研究生也适合刚接触水下机器人控制、想快速建立仿真验证框架的工程师。1. 整体设计与思路拆解1.1 为什么欠驱动AUV值得单独拿出来研究普通全驱动系统每个自由度配一个执行机构控制器设计几乎是线性代数的直接应用。欠驱动系统不一样控制通道数量小于运动自由度数量这意味着横向动力学特性只能通过纵向运动和偏航运动的耦合来间接影响。通俗点说车不能直接横着开只能靠方向盘和油门配合完成泊车——横向位移是纵向前进和转向共同作用的结果这就是典型的欠驱动特性。具体到AUV模型里通常只有两个控制输入纵向推力forward thrust和偏航力矩yaw moment。侧向力、垂向力一般没有独立执行机构或者只有被动水动力。系统需要满足某种不可积分约束这就是所谓非完整约束nonholonomic constraint。这个特性的直接后果是系统对初始条件敏感控制律设计不能照搬全驱动系统的线性化方法必须考虑横向误差的动态耦合。从理论角度来说AUV的轨迹跟踪分为两类核心任务轨迹跟踪trajectory tracking通常要求系统在时间维度上同步跟踪期望轨迹而路径跟随path following只要求系统收敛到路径上并沿路径运动不对时间参数做严格约束。正是这两种任务的定义差异决定了控制器结构、误差定义和收敛性分析方法完全不同。1.2 仿真平台与工具选型的理由我在这个项目里选择了Matlab R2023b Simulink作为主仿真平台主要有三个原因。第一Matlab的矩阵运算和控制系统工具箱在处理AUV运动学、动力学方程时极其高效尤其是做时变轨迹生成、误差变换和控制器推导时代码表达和数学公式几乎一一对应。第二Simulink提供可视化模块化建模环境对于多方法对比研究尤其合适——把每种控制器封装为子系统切换算法时只需要替换控制器模块无需改动系统模型主体这种模块化对比框架的效率远高于纯代码方案。第三Matlab生态里有大量可参考的海洋工程工具箱和开源代码无论是水动力系数估算还是传感器噪声建模都有现成的参考实现。当然也有朋友喜欢用Python但说实话在Simulink里做连续时间系统仿真、配置定步长求解器、导出C代码目前还是Matlab最顺手。你要是只做理论推导和数值实验PythonNumpy完全够一旦涉及半实物仿真或者控制器部署验证Simulink的External Mode和代码生成能力会省很多时间。1.3 不同分析方法到底“不同”在哪里标题里说的“不同分析方法”我拆解成了两条主线任务类型和算法族。任务类型就是轨迹跟踪与路径跟随它们的误差定义和期望值生成机制不同算法族则包括PID控制、反步法Backstepping、滑模控制SMC和模型预测控制MPC。每种算法在应对非完整约束、外部扰动和模型不确定性时思路完全不一样PID直接拿位置误差和角度误差做反馈思路简单工程实现容易但缺乏对耦合项的补偿大误差或者大扰动时容易失稳。反步法通过递归构造李雅普诺夫函数每一步引入虚拟控制量能显式处理欠驱动结构的耦合关系但依赖精确模型。滑模控制通过在滑模面上构造等效控制和切换控制对参数摄动和外部扰动有很强的鲁棒性但是抖振问题chattering需要额外处理。MPC把轨迹跟踪转化为在线优化问题可以显式处理输入饱和、状态约束对非线性系统也有较好的处理能力但计算量大需要合适的求解器。这四个方法放在同一系统模型下对比能看到一个清晰的规律模型利用程度越高控制效果越好但对建模精度和计算资源的要求也越高。这个结论对选型非常有参考价值。2. 欠驱动AUV建模与仿真框架搭建2.1 运动学与动力学模型推导我研究的对象是水平面内运动的欠驱动AUV在惯性坐标系和艇体坐标系中分别建立运动方程。惯性系下的位置用 (x, y) 表示航向角用 ψ 表示艇体坐标系下的纵向速度、横向速度、偏航角速度分别记为 u, v, r。运动学方程如下x_dot u cos(ψ) - v sin(ψ)y_dot u sin(ψ) v cos(ψ)ψ_dot r动力学方程在这里简化为三自由度模型纵向推力记为 τ_u偏航力矩记为 τ_r模型中的水动力阻尼系数采用线性加二次项近似u_dot (m_22/m_11) v r - (X_u/m_11) u - (X_uu/m_11) u|u| (1/m_11) τ_uv_dot -(m_11/m_22) u r - (Y_v/m_22) v - (Y_vv/m_22) v|v|r_dot ((m_11 - m_22)/m_33) u v - (N_r/m_33) r - (N_rr/m_33) r|r| (1/m_33) τ_r参数取值参考了典型小型AUV的公开数据模型质量设为30kg左右水动力阻尼系数根据经验公式估算。这些参数不要求极其精确关键在于保证系统表现出合理的欠驱动耦合特性——u和r能直接控制v是被动耦合出来的横向速度这才是研究难点所在。2.2 Simulink模型整体架构Simulink里的模型我是这样分层的最外层是参考轨迹信号源输出期望位置、期望速度给控制器控制器子系统接收状态反馈和参考信号输出控制力和力矩AUV动力学子系统接收控制输入通过积分器链解算出状态变量状态变量一方面反馈给控制器另一方面送到Scope和Record模块记录数据。动力学子系统里最有讲究的是非线性阻尼项的处理。线性阻尼项直接乘系数二次阻尼项需要写成 u*abs(u) 的形式避免符号错误。Simulink里用Product模块配合Sign模块实现绝对值运算防止由于正负号处理不当导致仿真发散。积分器初始化也是一个隐藏的坑。欠驱动系统的横向速度 v 不是控制输入直接决定的初始值给得不合理会导致瞬态过程出现非常大的耦合扰动。我的做法是先做一个静水条件下的阻力计算估算平衡点附近的速度范围把 v0 设为0u0 设为期望巡航速度这样仿真开头的过渡过程比较自然。2.3 控制器封装与切换机制为了对比不同算法我把控制器全部封装成Simulink子系统对外接口统一为参考位置 (x_ref, y_ref)、参考速度 (u_ref, r_ref) 或参考航向 ψ_ref、状态反馈 (x, y, ψ, u, v, r)输出为 (τ_u, τ_r)。对外接口固定内部算法独立实现运行时只需要切换子系统而AUV模型和参考轨迹产生器完全不用动。这种结构后续做实验会极度方便不需要为每种算法单独维护一套仿真工程。Simulink里实现控制器时要注意一个效率问题如果控制器里用到复杂的搜索或者迭代算法比如MPC的在线优化用Matlab Function模块写代码时要尽量减少每步仿真里的重复计算能提到初始化阶段计算的矩阵比如QP问题的Hessian矩阵就不要放在运行循环里。MPC的优化求解我后面会在第4节专门细说。3. 控制算法设计与核心实现3.1 基于视线法的路径跟随控制路径跟随问题我用的是前视距离法Line-of-Sight, LOS这是工程上最常用也最稳健的路径跟随策略。核心思想是在期望路径上找一个前视点计算当前位置到前视点的期望航向角然后通过航向控制间接实现横向偏差收敛。LOS几何关系很简单假设期望路径是直线段路径参数为 s定义横向偏差为 e_y前视距离为 Δ通常取1.5到2倍的船长。期望航向角为ψ_des atan2(y_ref(s) - y, x_ref(s) - x)其中 (x_ref(s), y_ref(s)) 是前视点坐标具体算法如下s 是路径参数更新逻辑为s_dot u cos(ψ - ψ_p) k_s e_y其中 ψ_p 是路径切向角k_s 是路径参数更新增益。引入 k_s 项是为了消除横向偏差时让路径参数适当回退避免路径跟踪曲线出现尖锐转折处的过冲问题。横向误差动力学可以写成e_y_dot u sin(ψ - ψ_p)注意这里的 e_y 是非线性表达的不能用简单的线性误差近似。如果航向偏差 ψ - ψ_p 比较小可以线性化为 u(ψ - ψ_p)但大偏差场景下必须保留正弦项否则控制器参数设计会失真。LOS控制器外环输出期望航向角内环用P控制偏航力矩r_des ψ_des_dot k_ψ (ψ_des - ψ)τ_r N_r r (m_11 - m_22) u v m_33 r_des_dot - k_r (r - r_des)其中航向角速度参考值需要数值微分建议在Simulink里用Transfer Function模块实现近似微分纯导数模块在高噪声环境下会放大传感器噪声导致控制输出抖动。纵向速度控制直接用PIτ_u X_u u X_uu u|u| m_11 u_des_dot - k_u (u - u_des)这里我加了前馈补偿项 X_u u X_uu u|u|是为了让速度控制不依赖积分器慢慢消除稳态误差——对AUV这种大惯性系统纯PI响应太慢加了模型前馈后速度响应会快很多。3.2 基于反步法的轨迹跟踪控制器反步法的思路是从位置误差反推虚拟控制量每一步构造一个中间李雅普诺夫函数最终递推到实际控制输入。这套方法在欠驱动AUV上的经典做法是把位置误差转换到艇体坐标系下考虑期望轨迹 (x_d(t), y_d(t))定义艇体坐标系下的误差e_x (x - x_d) cos(ψ) (y - y_d) sin(ψ)e_y -(x - x_d) sin(ψ) (y - y_d) cos(ψ)这里的坐标变换很关键。惯性系误差在经过旋转变换到艇体系后动力学表达式变得非常简洁纵向误差 e_x 的导数只和 u、r 有关横向误差 e_y 的导数也如此这为虚拟控制量的构造铺平了道路。推导过程分三步。第一步把 u 和 r 当作虚拟控制量想象它们是输入构造 (e_x, e_y) 的镇定控制律u_virtual u_d cos(ψ - ψ_d) k_1 e_x - e_y rr_virtual ψ_d_dot u_d sin(ψ - ψ_d)/(1 e_y) k_2 e_y第二步定义速度误差z_u u - u_virtualz_r r - r_virtual第三步把这些误差反馈进动力学方程设计实际控制输入 τ_u 和 τ_r。因为这一步涉及到复杂的偏微分推导用Matlab符号计算工具箱完成求导然后再生成代码能有效减少人工推导出错的风险。这也是整个项目里最耗时的一部分代码本身不长但每一步的数学正确性需要反复验证。3.3 基于滑模控制的鲁棒轨迹跟踪滑模控制的出发点是用一个滑模面把系统的误差动态限制在期望的降维流形上一旦系统状态到达滑模面并保持其上动态行为就与模型不确定性无关。对欠驱动AUV我用的滑模面定义在速度误差上s_u u - u_des λ_u ∫(u - u_des)dts_r r - r_des λ_r ∫(r - r_des)dt控制律设计为等效控制加切换项τ_u X_u u X_uu u|u| m_11 u_des_dot - λ_u (u - u_des) - k_1 sign(s_u)τ_r N_r r N_rr r|r| - (m_11 - m_22) u v m_33 r_des_dot - λ_r (r - r_des) - k_2 sign(s_r)切换项的增益 k_1, k_2 需要大于模型不确定性和扰动的上界否则鲁棒性无法保证。仿真结果显示滑模控制对水流扰动和模型参数摄动抑制明显但符号函数 sign(s) 会导致控制力矩高频抖动这在真实执行器上根本吃不消——推进器和舵面频率响应有限高频抖振会直接烧驱动器的MOS管。我改用饱和函数 sat(s/φ) 代替符号函数在边界层厚度 φ 内做线性过渡有效消除了抖振代价是理论上的渐近收敛变为有界收敛。3.4 基于模型预测控制的轨迹跟踪MPC在AUV轨迹跟踪上的优势是可以直接处理输入饱和约束这是其他方法很难优雅解决的。欠驱动系统输入受限实际上是非常棘手的问题大角度转向时需要的偏航力矩可能超过舵面极限MPC能提前在优化中考虑这个限制控制器会自动调整转向策略不会让执行器一直处于饱和状态。在线优化问题的标准形式是min J Σ (x_k - x_ref,k)^T Q (x_k - x_ref,k) u_k^T R u_ks.t. |τ_u| ≤ τ_u_max, |τ_r| ≤ τ_r_max我采用的是基于非线性模型线性化实时线性化的MPC方案在每个采样周期把非线性动力学在当前工作点线性化然后求解标准二次规划问题。预测时域取 N_p 20控制时域 N_c 5采样时间0.2sQ 矩阵里位置误差权重给得比较高R 矩阵约束控制量变化率防止剧烈抖舵。这里讲一下Simulink里实现MPC的关键点Matlab Function模块里调用quadprog或者mldivide求解二次规划每步耗时大约20到30毫秒取决于预测时域这个算力在离线仿真里完全没问题但要是上实时硬件就要换更高效的求解器比如OSQP或者FORCES Pro。另外非线性模型的实时线性化需要每步重新计算雅可比矩阵建议用符号工具箱预先求好解析表达式避免每步都做数值微分——数值微分不仅慢还会引入额外噪声。4. 仿真结果对比与不同分析方法的效果分析4.1 轨迹跟踪场景设置仿真场景我设了两个。第一个是“8”字形轨迹跟踪这个轨迹同时包含直线段和大幅度转向段能有效考察控制器在大曲率条件下的跟踪能力第二个是“S”形路径跟随模拟AUV从一个航路点向另一个航路点转移时的路径收敛过程。参考轨迹的生成方式前者用参数方程 x_ref 10 sin(0.05t)y_ref 5 sin(0.1t) 产生周期“8”字轨迹后者用一系列离散航路点线性插值生成折线路径再用三次样条平滑。航向参考通过数值微分得到。仿真时长设置为100秒初始位置误差故意设置得比较大x偏差5米、y偏差3米航向偏差20度验证控制器在大初始误差下的收敛行为。外界的扰动也考虑进去了。我在动力学方程里加了一个模拟海流的小扰动项幅值0.2m/s方向随时间缓慢变化。滑模控制器对这种扰动几乎无感反步法有轻微位置误差波动PID则出现明显的稳态误差。4.2 三种任务下各算法的定量对比下表是100秒仿真结束后各控制方法在不同任务下的最终跟踪误差和最大控制力矩控制方法任务类型位置稳态误差(m)航向误差(deg)最大推力(N)计算耗时(s)PIDLOS路径跟随0.301.841.22.1Backstepping轨迹跟踪0.080.537.83.4SMC轨迹跟踪0.050.439.63.8MPC轨迹跟踪0.120.832.552.6数据其实比较有意思。从稳态精度看SMC最好MPC反而没有反步法精度高。这背后的原因是MPC的预测模型是实时线性化的在大曲率“8”字航迹段线性化误差对预测精度影响明显再加上控制时域有限本质上是对非线性系统的一个近似最优控制。SMC直接用非线性模型设计滑模面天然更贴合系统动态。MPC的价值体现在另一个维度它对约束的处理能力最强最大推力需求只有32.5N是所有方法里最低的其他方法暂时没考虑输入饱和约束。PIDLOS在稳定精度上确实落后一截但工程意义不能低估算法结构极简参数整定直观现场调试一套AUV用LOS框架配合航向PID是最高效的起步方案。我的经验是需求精度0.5米以下的标定任务PIDLOS完全够用。4.3 收敛时间与瞬态行为分析光看稳态误差不够收敛过程的瞬态行为同样关键。从仿真数据看SMC的系统在5秒内把横向偏差从3米压缩到0.1米以内反步法花了接近8秒MPC因为初始阶段预测模型线性化误差较大花了9秒左右而且出现了轻微的超调——大约0.3米的位置过冲然后逐步回稳。反步法和SMC的一大问题是控制输入在初始阶段有短暂饱和现象因为初始误差大控制律计算出来的所需力矩超过执行器极限。我在Simulink里给控制输出加了饱和模块后发现加饱和之后SMC的收敛时间从5秒延长到7秒反步法从8秒延长到9.5秒。这就是约束带来的性能代价。MPC由于在优化问题里显式建模了约束初始段会自动生成一个渐进转向的控制序列控制量始终在极限内这也是工程应用上相对“温柔”的优点。路径跟随任务里LOS算法的参数选取直接影响收敛表现。前视距离 Δ 取3米时横向偏差收敛快但航向角波动大Δ取8米时航向变化平缓但收敛路径被拉长。实际调试中可以根据任务场景在线调整Δ初始偏差大时用小前视快速靠拢进入稳定段后加大前视抑制振荡。5. 常见问题与工程化经验5.1 在Simulink中联合Matlab代码实现时遇到的坑Simulink和Matlab联合仿真看起来简单实际操作中坑很多。最重要的点是数据类型的兼容性。控制器代码里如果你用了MATLAB的矩阵操作Simulink信号的默认数据类型是double但信号线交叉时可能出现维度不匹配的问题。例如MPC控制器里计算误差矩阵时Simulink的二维信号和一个列向量直接在Matlab Function里运算可能报错我的解决方法是所有中间变量统一用列向量避免隐式广播。另一个坑是变步长求解器的自适应步长在控制器快速切换时出现的代数环问题。如果控制器输出直接反馈到输入而中间没有状态延迟比如MPC求解需要当前步的输入但输入又影响当前步状态的导数Simulink会生成代数环仿真时频繁报错。解决方法是把控制器内部计算延迟一拍也就是用memory模块把上一时刻的状态暂存或者使用Unit Delay让控制律使用的是上一步的状态值这对实际离散控制系统反而更真实。5.2 欠驱动系统控制参数整定的经验欠驱动系统的参数整定有一个经验规律先整定内环速度环、航向环再整定外环位置环。反步法里的 k_1, k_2 系数直接决定位置误差收敛速度但这两个系数选大的时候内环速度误差 z_u, z_r 的动态必须足够快否则整个闭环会出现振荡。我在调试过程中把外环增益从0.5调到2.0时系统还稳定再往上调到3.0横向误差直接开始发散——原因在于内环响应速度跟不上虚拟控制量的变化率导致虚拟控制量“说到做不到”实际运动滞后于参考运动。有个好习惯是画出误差相轨迹图。Simulink里把 e_x 和 e_y 分别输入到X-Y Graph模块观测李雅普诺夫函数的收敛路径能直观判断控制器是否工作正常也可以在误差空间中注入随机扰动观察相轨迹是否被吸引回原点。调试AUV控制器时这种可视化手段比单纯看波形有效得多。5.3 MPC控制器在Simulink中跑不动怎么办MPC仿真慢是个普遍问题50秒的仿真时长原本以为1分钟内搞定结果跑了近10分钟。排查后发现主要瓶颈在于每步都在做非线性模型的实时线性化和雅可比矩阵数值微分。我采用符号求导预先计算雅可比表达式用Matlab Coder把MPC控制器代码转成MEX文件仿真速度直接提升了8倍以上。如果只是想验证算法逻辑还可以先用解析线性模型替代非线性模型做MPC设计快速验证可行性后再换非线性模型。MPC的权重矩阵 Q 和 R 调整也是个大坑。Q 矩阵中位置误差权重增大会让跟踪精度提高但是会增加控制量波动R 矩阵控制量权重如果设得太大控制动作会过于迟缓系统响应像“僵尸”一样慢吞吞。我试过的可行思路是先用闭环仿真大致确定位置权重和控制权重在一个数量级内再按性能指标逐步调节避免直接从零开始盲目搜索。还有一个小技巧Q 矩阵中把横向误差权重适当调高比如1.5倍纵向误差权重可以缓解欠驱动系统的横向耦合问题让轨迹跟踪时不会出现明显的横向拖尾现象。5.4 实验结果可信度的检验方法仿真做出来到底对不对不能只盯着跟踪误差曲线。我的检验方法有三个维度。第一是控制输入是否满足物理约束如果控制力矩长时间处于饱和状态说明轨迹规划或者控制器参数不合理。第二是状态变量是否在合理物理范围内比如横向速度 v 是否在AUV设计包线内过大的横向速度往往意味着控制策略过于激进或者模型参数失配。第三是稳定性判断在轨迹跟踪稳态段拉随机小扰动观察系统能否回到原轨迹而非漂移。另一个常被忽视的检验项是数值积分误差控制。Simulink里选择变步长ode45时默认相对误差容限是1e-3对高动态系统这个精度可能不够。我把相对误差容限调到1e-6后部分控制方法的跟踪精度指标会有细微变化这说明原来的结果里包含了数值误差成分。严谨的仿真对比务必在相同容差下进行AUV这种弱阻尼系统对数值误差更敏感建议至少设置1e-5。6. 仿真框架的扩展与后续工作方向这套框架做完之后扩展空间非常大。从控制算法角度可以加入自适应参数辨识模块在线估计水动力系数解决模型失配问题从任务场景角度可以加入三维轨迹跟踪把垂直面的深度控制和水平面的航向控制结合起来从仿真真实性角度可以加入波浪扰动模型、传感器噪声模型和推进器动态模型让仿真更接近实海环境。我个人建议后续分两步走。第一步把单AUV扩展为多AUV协同编队控制在现有框架上增加编队协议层AUV之间通过通信拓扑交换位置信息形成队形保持和避碰策略。第二步结合代码生成工具把控制器部署到实际硬件上用半实物仿真先验证一遍执行器响应和控制周期再进入水池实验。这个扩展路径已经有不少开源项目可以参考框架基础打好后每一步都是顺水推舟的事。做这类仿真研究我的体会是代码和模型都是表面功夫真正花时间的是把欠驱动系统的动力学特性吃透——控制算法本质上是在跟系统的不可控自由度“博弈”你对模型理解多深控制器设计就能走多远。先把这篇文章里提到的基础框架搭起来再往里面逐步添加算法复杂度你就会发现那些论文里看起来很玄乎的控制器设计其实每一步都有清晰的工程逻辑在支撑。
