AUV轨迹跟踪控制器复现：反步法与LMPC的Matlab仿真实现

把AUV的轨迹跟踪代码从论文变成能跑的Matlab仿真我前后折腾了两周多。第一次成功跟上一组螺旋线时画面里的误差曲线掉下去的那一瞬间确实很解压。这篇内容不是教科书翻译而是我复现“基于Lyapunov的非线性模型预测控制LMPC与反步法”这套AUV轨迹跟踪控制器时觉得最值得写下来的东西模型怎么简化、反步法怎么递推、LMPC怎么和Lyapunov函数结合、Matlab代码怎么搭、参数怎么调才不炸。适合正在做水下机器人运动控制课程设计、准备控制方向复现实验或者想从仿真往实船过渡的读者参考。1. 先理清要复现的控制问题轨迹跟踪不是“追着点跑”1.1 AUV的运动本质欠驱动、非线性、强耦合自主水下航行器AUV在水下的运动理论上说是一个六自由度问题前后、左右、上下三个平动加上横摇、纵摇、偏航三个转动。但绝大多数轨迹跟踪研究不会一上来就碰六自由度原因是模型参数多、执行机构少、耦合强直接上六自由度会让控制器设计和调参都变成一场灾难。更实际的做法是先把问题限制在水平面三自由度也就是只考虑前后速度、横漂速度和偏航角速度忽略垂荡、纵摇、横摇。水平面模型已经很能说明AUV控制的难点了。首先它属于典型欠驱动系统很多AUV只有尾部推进器和垂直舵/水平舵横向没有直接推力。这意味着你无法像操纵一个全向移动机器人那样直接命令它“向左平移”只能通过偏航和前进速度的组合来实现横向运动。其次模型里的科氏力项和水动力阻尼项都是状态相关的非线性函数速度一旦变化系统动态就跟着变。最后纵向速度、横漂速度、偏航角速度三者之间互相耦合想要快速转向纵向速度和横漂速度就会发生变化反过来又影响航向响应。这类系统用线性控制理论设计出的PID在小范围工作点附近可能还能工作一旦参考轨迹变化剧烈或者需要高速转向线性模型失配带来的误差就很明显。所以学术界和工程界普遍转向非线性控制方法反步法就是其中一种非常经典的递推设计工具。1.2 轨迹跟踪和点镇定、路径跟踪的区别我在复现之前也犯过一个概念错误把轨迹跟踪和路径跟踪当成一回事。实际做仿真时才发现控制目标不一样误差定义不一样控制器设计难度也不一样。下面这个表是我后来整理给自己看的比较直观。控制模式参考输入误差定义控制难点典型应用场景点镇定固定点当前位置与目标点之差欠驱动下存在不能稳定到任意点的约束悬停、定点作业路径跟踪几何路径无时间要求到路径的最短距离路径参数化与前进速度规划管道巡检、海底测绘轨迹跟踪时变轨迹当前状态与轨迹点的时间同步误差需要同时保证空间跟踪和时间同步编队、对接、避障后的轨迹重规划轨迹跟踪最难的地方是“时间同步”。你看参考轨迹时不能只看AUV离轨迹有多近还要看它在某一时刻是否到达了轨迹上对应的位置。如果AUV早到了它可能需要减速等待如果晚到了它需要加速追赶。这种要求让控制器必须同时处理空间误差和速度误差反步法里虚拟速度的设计本质上就是在给AUV规划一条“追得上”的速度路径。1.3 复现控制器的输入输出关系做仿真前第一步是把被控对象和控制器的接口定义清楚。我简化后的AUV状态向量取为η [x, y, ψ]^T // 大地坐标系下的位置和偏航角 ν [u, v, r]^T // 载体坐标系下的纵向速度、横漂速度、偏航角速度控制器输出是推进器推力和偏航力矩τ [τ_u, τ_r]^T其中 τ_u 控制纵向速度τ_r 控制偏航角速度横漂方向没有控制输入这是欠驱动特性的直接体现。整个闭环系统可以写成η_dot R(ψ) ν ν_dot M^{-1} ( -C(ν)ν - D(ν)ν τ )R(ψ) 是载体坐标系到大地坐标系的旋转矩阵M 是包含附加质量的惯性矩阵C(ν) 是科氏力矩阵D(ν) 是阻尼矩阵。后文我会给出一套具体的参数和Matlab实现这样整个复现过程才有抓手。2. 为什么把反步法和LMPC放在一起用一个兜底一个远视2.1 反步法的优势与短板反步法的核心思想很简单把一个高阶非线性系统拆成低阶子系统从最外层误差开始逐层设计虚拟控制量每一步都构造一个李雅普诺夫函数来保证误差收敛最后推导出真正的控制输入。这个方法的优点是很自然地兼容非线性项不需要做线性化稳定性证明也相对完整。但反步法的短板同样明显。第一经典反步法在推导控制律时需要对虚拟控制量求导也就是需要对参考轨迹求高阶导数。如果参考轨迹本身不平滑或者存在噪声微分项会被放大造成“微分爆炸”。第二反步法在设计过程中一般不考虑输入饱和约束可实际AUV的推进器推力、舵面偏转角都有物理上限。第三它没有预测能力控制器只能基于当前误差做出反应面对变化剧烈的参考轨迹时响应容易滞后。2.2 LMPC的优势与短板模型预测控制的核心是滚动优化。每个控制周期基于当前状态和预测模型在线求解一个有限时域优化问题得到未来N步的最优输入序列但只执行第一步下一周期重复。这种机制让LMPC可以非常自然地处理输入约束、状态约束甚至可以把“收益函数”“避障函数”写进优化目标里。LMPC的问题在于普通MPC的稳定性分析不太直观往往需要加终端约束或者终端代价函数才能给出理论保证。而且非线性MPC每个周期都要在线求解优化问题计算量比反步法大很多。在Matlab里用fmincon跑非线性MPC如果预测时域太长或者决策变量太多一个周期可能要好几百毫秒甚至几秒这对需要实时控制的AUV来说是很难接受的。2.3 组合思路LMPC给期望动态反步法管稳定跟踪在我复现的方案里不是把两个控制器并联然后平均也不是简单二选一而是让它们各管一层上层LMPC 在每一步读取当前状态结合参考轨迹滚动优化出未来一段时间内“最优且不违反约束”的期望速度增量与期望偏航角速度增量并在代价函数中加入李雅普诺夫函数衰减项保证优化解不会把系统推向不稳定区域。下层反步法把LMPC给出的期望动态当作新的参考输入通过递推设计计算出AUV实际需要的推进器推力和偏航力矩。这样做的好处是反步法保证了对非线性模型的稳定跟踪能力LMPC则负责在更大的时间尺度上考虑约束和未来趋势。代价函数里的李雅普诺夫项相当于给MPC加了一道“安全护栏”不让它为了短时最优而激进地输出离谱的控制量。我在仿真里也做了一个对照组单独用反步法和单独用LMPC分别跑同一组参考轨迹。结果符合预期单独反步法在输入约束较强时表现变差单独LMPC在模型参数失配时误差收敛慢而组合方案在约束、噪声、模型失配同时存在时仍然保持了可接受的跟踪精度。3. 复现前的数学准备模型、误差和约束3.1 水平面三自由度运动模型我用的AUV模型参考了常见的水下机器人模型做了如下假设忽略纵摇、横摇、垂荡只研究水平面运动浮力与重力平衡且浮心与重心在同一垂线上水动力系数取常数不考虑波浪、海流等环境扰动扰动在后面作为附加测试项加入推进器只提供纵向推力和偏航力矩。完整的运动学方程如下x_dot u cos(ψ) - v sin(ψ) y_dot u sin(ψ) v cos(ψ) ψ_dot r动力学方程如下u_dot (m22 / m11) * v * r - (d11 / m11) * u (1 / m11) * τ_u v_dot -(m11 / m22) * u * r - (d22 / m22) * v r_dot ((m11 - m22) / m33) * u * v - (d33 / m33) * r (1 / m33) * τ_r这里 m11、m22、m33 是包含附加质量的惯性项d11、d22、d33 是阻尼系数。横向速度 v 没有控制输入它由纵向速度和偏航角速度耦合产生这正是欠驱动系统的核心特征。3.2 仿真参数怎么选参数取值我直接参考了某公开仿真案例中的一组经典参数然后根据我的仿真步长和控制周期做了微调。具体如下表。参数数值含义m1125.0 kg纵向惯性项m2230.0 kg横向惯性项m3310.0 kg·m²偏航惯性项d115.0 kg/s纵向阻尼d226.0 kg/s横向阻尼d334.0 kg·m²/s偏航阻尼τ_u_max10.0 N最大纵向推力τ_r_max3.0 N·m最大偏航力矩dt0.05 s模型积分步长T_ctrl0.20 s控制周期控制周期比模型积分步长稳定这个差别很关键。模型积分用较小的步长模拟连续系统动态控制器每隔一个控制周期才计算一次输入这样更接近真实AUV的机载计算机工作模式。如果你在仿真里让控制器每个积分步都工作后期换到实物或者半实物仿真时计算负荷会给你一个非常大的“惊喜”。3.3 参考轨迹设计参考轨迹我选了两条。一条是水平面的螺旋线x_d(t) 2 * sin(0.2 * t) y_d(t) 2 * cos(0.2 * t) ψ_d(t) atan2(y_dot_d(t), x_dot_d(t))另一条是组合正弦轨迹用来测试控制器对突变速度的响应x_d(t) t y_d(t) 1.5 * sin(0.3 * t)螺旋线的优点是曲率连续变化速度方向变化平稳适合看基础跟踪效果。组合正弦轨迹则会让偏航角速度变化更剧烈用来考验LMPC预测时域和反步法虚拟速度的配合是否到位。我在后面的仿真结果中默认展示螺旋线跟踪效果。4. 反步法控制器设计递推出来的李雅普诺夫函数4.1 从位置误差到虚拟速度第一层反推反步法设计的第一步是定义位置误差。这里我没有直接用大地坐标系下的位置差而是把位置误差旋转到载体坐标系下这样后续控制器设计更贴合AUV的物理感知。位置误差定义为e_x cos(ψ) * (x - x_d) sin(ψ) * (y - y_d) e_y -sin(ψ) * (x - x_d) cos(ψ) * (y - y_d)对它们求导整理后可以得到误差动力学。为了让位置误差收敛我设计第一个虚拟控制量也就是期望的纵向速度和期望的“漂角补偿速度”u_d x_d_dot * cos(ψ) y_d_dot * sin(ψ) - k_x * e_x v_d -x_d_dot * sin(ψ) y_d_dot * cos(ψ) - k_y * e_y注意这里的 v_d 不是横漂速度而是为了消除位置误差而构造的虚拟量。k_x 和 k_y 是正增益它们的取值决定了位置误差的收敛速度。这一步我把李雅普诺夫函数取为V1 0.5 * (e_x^2 e_y^2)如果后续速度误差能追得上虚拟速度那么 V1 的导数就是负定的位置误差会指数收敛。4.2 从速度误差到控制力第二层反推第二层定义速度误差e_u u - u_d e_v v - v_d // 这里的v_d是辅助量不是实际横漂控制目标 e_r r - r_d由于横漂方向没有控制输入e_v 不能直接通过 τ_v 消除只能通过纵向推力和偏航力矩间接影响。这给反步法带来了一点麻烦但也是欠驱动系统设计时必须面对的现实。设计李雅普诺夫函数V2 V1 0.5 * m11 * e_u^2 0.5 * m22 * e_v^2 0.5 * m33 * e_r^2对 V2 求导代入模型方程然后选择控制输入使得导数负定。经过整理控制律可以写成τ_u m11 * ( -k1 * e_u u_d_dot - (m22/m11) * v * r (d11/m11) * u - e_x ) τ_r m33 * ( -k2 * e_r r_d_dot - ((m11-m22)/m33) * u * v (d33/m33) * r - e_y )这里 k1、k2 是速度误差反馈增益。公式最后面的 -e_x 和 -e_y 是从 V2 交叉项里得到的作用是让速度误差和位置误差建立耦合避免出现“位置误差小了但速度误差很大”的尴尬状态。4.3 稳定性结论和参数整定在上述控制律作用下V2 的导数近似为V2_dot -k_x * e_x^2 - k_y * e_y^2 - k1 * e_u^2 - k2 * e_r^2只要所有增益都大于零误差系统在理论上就是渐近稳定的。实际调参时要注意k_x、k_y 决定位置误差收敛速度过大会导致虚拟速度变化太剧烈反步法里需要求导的项就会很大k1、k2 决定速度误差收敛速度过大会导致控制输入接近饱和增益之间最好保持“位置环比速度环慢半拍”的原则即 k1、k2 略大于 k_x、k_y这样控制输入比较平滑。我在仿真中最终取的是 k_x 0.8k_y 0.8k1 1.2k2 1.0。这套参数在螺旋线和组合正弦轨迹上都有不错的表现但换到更激进的参考轨迹时需要重新微调。5. LMPC控制器设计把李雅普诺夫函数写进滚动优化5.1 预测模型与离散化LMPC的预测模型可以继续用连续模型然后在每个控制周期内做离散化。我选的仿真积分步长 dt 是0.05秒控制周期 T_ctrl 是0.2秒也就是每个控制周期内包含4个积分步。为了方便LMPC计算我直接用一个离散递推函数来表示模型给定当前状态和输入输出下一个控制周期结束时的状态。这一步有两个选择一是用非线性模型直接做预测二是先把模型线性化再用线性MPC。我优先选择了非线性模型配合fmincon求解因为标题里的LMPC强调非线性控制。如果计算资源紧张也可以在每个工作点做在线线性化用quadprog求解一个QP问题速度会快很多但调约束时要更小心。5.2 代价函数与约束LMPC的核心是一个带约束的有限时域优化问题。我设计的代价函数包括四部分状态跟踪误差、控制输入幅值惩罚、控制输入变化率惩罚、终端李雅普诺夫函数惩罚。参考轨迹在未来 N 步的值记为 x_ref(k)预测状态记为 x(k)控制输入记为 tau(k)。代价函数为J Σ [ (x(k) - x_ref(k))^T Q (x(k) - x_ref(k)) ] Σ [ tau(k)^T R tau(k) ] Σ [ (tau(k) - tau(k-1))^T S (tau(k) - tau(k-1)) ] V_f(x(N))其中 V_f 是终端李雅普诺夫函数惩罚项我取为误差终端的二次型。这里最值得说明的是“如何体现Lyapunov”在每一步优化目标中加入了一个软约束要求当前计算得到的代价函数不超过上一时刻代价函数乘以衰减系数即 J(k) ≤ γ J(k-1)终端惩罚项实质上就是反步法里 V2 的离散版本用误差状态二次型表示。这样做之后MPC不再是一个“只顾短时最优”的优化器而是在优化每一步输入时都会考虑系统稳定性衰减趋势。这也是LMPC和普通MPC的明显区别。5.3 MATLAB中的实现形式非线性MPC我在Matlab里直接用fmincon求解决策变量取为未来 N 步的输入序列z [tau_u(1), tau_r(1), tau_u(2), tau_r(2), ... , tau_u(N), tau_r(N)]预测状态由模型函数递推得到不需要把它们也作为决策变量这样可以显著减少优化变量数量。虽然无法处理复杂状态约束但对于轨迹跟踪来说足够了。预测时域 N 我取10到20之间N太小时预测效果不明显N太大时求解速度太慢而且约束过多容易导致无解。6. 从零搭Matlab仿真主循环、代码骨架与踩坑记录6.1 主程序结构整个仿真程序我分成三个文件主脚本、AUV模型函数、控制器函数。主脚本负责初始化参数、生成参考轨迹、跑闭环循环、画图。模型函数负责状态递推。控制器函数里同时放了反步法和LMPC两种实现方便切换对照。主程序的核心逻辑如下%% 初始化 clear; clc; close all; dt 0.05; % 模型积分步长 T_ctrl 0.2; % 控制周期 t_end 30; % 仿真时长 t 0:dt:t_end; N_ctrl round(T_ctrl / dt); % 每个控制周期内的积分步数 % AUV参数 params.m11 25; params.m22 30; params.m33 10; params.d11 5; params.d22 6; params.d33 4; params.tau_u_max 10; params.tau_r_max 3; % 初始状态 x0 [0.5; -0.5; 0; 0.1; 0.05; 0]; state x0; tau [0; 0]; % 存储变量 state_log zeros(length(x0), length(t)); tau_log zeros(2, length(t)); ref_log zeros(2, length(t)); for i 1:length(t) t_now t(i); % 计算参考轨迹 [xd, yd, psid, xd_dot, yd_dot, psid_dot] ref_trajectory(t_now); % 每隔N_ctrl步计算一次控制输入 if mod(i-1, N_ctrl) 0 % 当前量测状态 x_meas state 0.01 * randn(6,1); % 模拟测量噪声 % 调用控制器 tau lmpc_backstepping_controller(x_meas, t_now, params); end % 模型递推 state auv_dynamics(state, tau, dt, params); % 记录 state_log(:,i) state; tau_log(:,i) tau; ref_log(:,i) [xd; yd]; end主循环里我故意加了一行测量噪声模拟代码量测噪声方差取0.01。很多入门复现不加噪声控制效果看起来完美但一加噪声就露馅。加入噪声后反步法里对虚拟量求导的部分会变得特别敏感这是我在复现过程中学到的非常重要的一课。6.2 AUV模型函数模型函数负责一步递推。我用的是最简单的欧拉积分步长0.05秒时精度足够而且代码直观。核心代码如下function x_next auv_dynamics(x, tau, dt, params) x_pos x(1); y_pos x(2); psi x(3); u x(4); v x(5); r x(6); tau_u max(min(tau(1), params.tau_u_max), -params.tau_u_max); tau_r max(min(tau(2), params.tau_r_max), -params.tau_r_max); x_dot u * cos(psi) - v * sin(psi); y_dot u * sin(psi) v * cos(psi); psi_dot r; u_dot (params.m22 / params.m11) * v * r - (params.d11 / params.m11) * u tau_u / params.m11; v_dot -(params.m11 / params.m22) * u * r - (params.d22 / params.m22) * v; r_dot ((params.m11 - params.m22) / params.m33) * u * v - (params.d33 / params.m33) * r tau_r / params.m33; x_next x dt * [x_dot; y_dot; psi_dot; u_dot; v_dot; r_dot]; end注意动力学方程里的输出饱和。控制器计算出的tau可能超过执行机构能力模型递推时必须先做饱和处理否则仿真里会出现“控制器以为给了10N推力但物理上根本实现不了”的问题。如果你后面要画控制输入曲线建议分别保存饱和前后的值能清楚看到控制器到底有没有在约束边界上工作。6.3 反步法控制器代码反步法控制器接收当前状态、参考轨迹信息和控制参数输出控制输入。核心代码结构如下function tau backstepping_controller(x, ref, params) x_pos x(1); y_pos x(2); psi x(3); u x(4); v x(5); r x(6); xd ref.xd; yd ref.yd; psid ref.psid; xd_dot ref.xd_dot; yd_dot ref.yd_dot; psid_dot ref.psid_dot; % 控制增益 kx 0.8; ky 0.8; k1 1.2; k2 1.0; % 位置误差旋转到载体坐标系 ex cos(psi) * (x_pos - xd) sin(psi) * (y_pos - yd); ey -sin(psi) * (x_pos - xd) cos(psi) * (y_pos - yd); % 虚拟速度 xd_dot_body xd_dot * cos(psi) yd_dot * sin(psi); yd_dot_body -xd_dot * sin(psi) yd_dot * cos(psi); u_d xd_dot_body - kx * ex; v_d yd_dot_body - ky * ey; % 这里 r_d 取参考偏航角速度 r_d psid_dot; % 速度误差 eu u - u_d; ev v - v_d; er r - r_d; % 需要虚拟速度的导数实际实现中可用数值微分或者解析表达式 u_d_dot (ref.xd_dot2 * cos(psi) - xd_dot * sin(psi) * r ... ref.yd_dot2 * sin(psi) yd_dot * cos(psi) * r) - kx * (u - u_d); r_d_dot ref.psid_dot2; % 反步控制律 tau_u params.m11 * (-k1 * eu u_d_dot - (params.m22/params.m11) * v * r ... (params.d11/params.m11) * u - ex); tau_r params.m33 * (-k2 * er r_d_dot - ((params.m11-params.m22)/params.m33) * u * v ... (params.d33/params.m33) * r - ey); tau [tau_u; tau_r]; end这段代码里最麻烦的是 u_d_dot 的计算。如果你直接对 u_d 的解析表达式求导里面会同时出现位置误差、速度误差和偏航角速度稍不注意就少项。所以我强烈建议在写代码之前先在纸上把 u_d 的导数完整推一遍。如果嫌麻烦也可以用一阶低通滤波器对 u_d 做平滑数值微分这样对噪声抑制更好但会引入相位滞后。6.4 LMPC控制器代码LMPC控制器我核心用的是fmincon。为了让优化问题不过于复杂我在每个控制周期把预测状态用模型函数递推出来把代价和目标约束写成函数句柄。关键代码如下function tau lmpc_controller(x, t_now, params) N 10; % 预测时域 Q diag([10, 10, 5, 1, 1, 1]); R diag([0.1, 0.5]); S diag([0.2, 0.3]); % 上一时刻输入这里用持久变量保存 persistent tau_prev; if isempty(tau_prev) tau_prev [0; 0]; end % 预测时域内的参考轨迹 ref_seq zeros(6, N); for k 1:N t_k t_now k * T_ctrl; ref ref_trajectory(t_k); ref_seq(:,k) [ref.xd; ref.yd; ref.psid; ... ref.xd_dot; ref.yd_dot; ref.psid_dot]; end % 决策变量初值上一时刻输入不断重复 z0 repmat(tau_prev, 1, N); z0 z0(:); lb []; ub []; A []; b []; Aeq []; beq []; % 输入约束 lb -repmat([params.tau_u_max; params.tau_r_max], N, 1); ub repmat([params.tau_u_max; params.tau_r_max], N, 1); options optimoptions(fmincon, Display, off, Algorithm, sqp); % 求解优化问题 z fmincon((z) lmpc_cost(z, x, ref_seq, params, Q, R, S, tau_prev), ... z0, A, b, Aeq, beq, lb, ub, (z) lmpc_constraints(z, x, ref_seq, params), options); tau z(1:2); tau_prev tau; endlmpc_cost函数负责递推预测状态并计算代价如下function cost lmpc_cost(z, x, ref_seq, params, Q, R, S, tau_prev) N length(ref_seq) / 6; cost 0; x_pred x; tau_last tau_prev; for k 1:N tau_k z(2*(k-1)1 : 2*k); x_pred auv_dynamics(x_pred, tau_k, T_ctrl, params); e_state x_pred - ref_seq(:,k); cost cost e_state * Q * e_state; cost cost tau_k * R * tau_k; cost cost (tau_k - tau_last) * S * (tau_k - tau_last); tau_last tau_k; end % 终端李雅普诺夫惩罚 e_term x_pred - ref_seq(:,end); cost cost e_term * (10 * Q) * e_term; end这里有一个细节auv_dynamics内部积分步长本来是 dt但LMPC预测时希望输出的是一个控制周期之后的状态所以我在调用时把 dt 参数换成 T_ctrl也就是说模型函数实际上是在做一个T_ctrl的“大步长递推”。这会引入离散化误差。如果你希望预测更精确可以在auv_dynamics里做4次小步长积分再把最终状态返回。6.5 我踩过的四个坑复现过程中最折磨人的不是控制器推导而是各种细节问题。我把印象最深的四个坑记录一下。第一坑是fmincon的初值问题。如果不把上一时刻最优输入序列作为本轮初值而是每次都用零向量初始化前几个控制周期的求解时间会明显偏长而且容易陷入局部最优。把tau_prev扩展成序列作为初值之后求解速度几乎快了一倍。第二坑是代价函数里状态误差的单位不一致。位置误差是米速度误差是米每秒偏航角误差是弧度如果Q矩阵不按物理单位调整位置误差绝对值小但偏航误差大优化器会“偏心”。我最后采用diag([10, 10, 5, 1, 1, 1])实际上就是对不同物理量做归一化保证位置误差优先级最高但又不会完全忽视速度误差。第三坑是输入变化率惩罚的权重。刚开始我把S设成零结果控制输入剧烈抖振轨迹虽然跟踪上了但执行机构完全没法接受。加上S矩阵之后控制量平滑很多代价是快速响应稍慢一些。第四坑是模型函数里的饱和必须在积分之前做而不是积分之后。如果你先递推再限幅仿真结果会出现“状态越界后被强行拉回”的虚假动态看起来像是控制器出了问题实际是模型实现有误。这个逻辑顺序问题非常隐蔽但影响很大。7. 仿真结果怎么看轨迹、误差和输入7.1 轨迹跟踪效果我先把螺旋线跟踪的轨迹画出来AUV从初始位置 (0.5, -0.5) 出发参考轨迹是半径2米的螺旋线。前2秒AUV在追赶参考点之后基本稳定在轨迹附近。需要特别看的是转弯阶段螺旋线曲率不断变化AUV的偏航角速度需求不是恒定值反步法能给出平滑的力矩而LMPC的预测作用让AUV在进入大曲率段时提前调整航向角所以实际轨迹与参考轨迹的偏差在转弯段并没有明显放大。如果单独看某一段时间内的轨迹细节你会发现LMPC反步法的轨迹不是“锯齿状”的。纯反步法在高增益下容易出现小幅抖动而组合方案中LMPC限制住输入变化率轨迹更平滑。这一点在从仿真曲线转成实物运动时非常关键因为机械系统对高频抖动的损耗是肉眼可见的。7.2 误差收敛与输入约束位置误差和速度误差的收敛曲线是评估控制器性能的重要依据。我的仿真中初始位置误差大约0.7米经过约3秒后降到0.05米以内稳态阶段误差保持在0.02米左右波动。这个数值在纯仿真里不算惊艳但考虑到我加了测量噪声而且模型参数并未完全等于控制器设计时使用的参数这个表现足够说明方案是有效的。控制输入方面纵向推力在启动阶段短暂接近10N的约束上限之后稳定在2到4N之间。偏航力矩在转弯段会周期性冲到2N·m以上但没有超过3N·m的约束边界。如果你看到控制输入长时间贴在上限波动说明控制器设计或参数整定有问题不是在“努力工作”而是在“挣扎”。7.3 参数敏感性与海流扰动为了测试鲁棒性我在模型里加入了定常海流扰动等效为在大地坐标系下增加了一组恒定速度分量。具体做法是在运动学方程的输出端加上一个常值偏移。扰动加入后反步法在位置环的积分作用不够稳态误差明显增大到0.15米左右而LMPC因为每个周期都在基于预测模型重新求解能更快估计出扰动带来的偏移稳态误差只增大到0.06米左右。这组对比说明了一个结论如果你只在干净环境里做仿真反步法已经足够好但如果你要应对海流、测量噪声、模型失配LMPC的预测优化能力确实能提供反步法不具备的鲁棒性。两者结合不是炫技而是工程上的互补。8. 从仿真到水池实验的血泪建议8.1 先加测量噪声再谈性能很多仿真复现为了图省事直接在理想状态下跑通闭环就结束。我的建议是从第一天开始就在状态反馈里加入高斯噪声。AUV的定位通常来自惯导和深度计组合速度来自多普勒测速仪这些设备都有噪声和时间延迟。仿真不加噪声控制器增益可以调得很高看起来误差曲线非常漂亮一旦到水池实验同样的控制器可能直接发散。我在仿真里加的噪声方差并不大已经足够让反步法的虚拟速度导数项产生明显抖动。解决办法有两种一是提高量测数据的滤波程度二是把反步法里的虚拟速度导数用低通滤波后的数值微分代替。后者在工程中更常用。8.2 控制器周期与离散化控制器周期 T_ctrl 的选择直接影响LMPC的求解时间和跟踪效果。T_ctrl 太小N步预测覆盖的总时间太短LMPC看不到远处的轨迹趋势退化为一个局部优化器T_ctrl 太大预测总时间够长但每个优化步的离散化误差也变大而且控制更新的滞后性更明显。我在多次试验后发现对于水平面轨迹跟踪T_ctrl 0.2秒、N 10到15是一个比较稳妥的组合。这样预测总时长为2到3秒既能看到轨迹变化趋势又不会让优化问题复杂到无法实时求解。8.3 个人体会这套控制方案从理论上看很顺理成章但真正把它变成能跑的代码需要大量细节推敲。反步法部分的虚拟速度求导、LMPC部分的李雅普诺夫衰减约束、输入饱和的执行顺序、控制周期与模型积分的解耦每一个单独拎出来都不难放在一起却足够让人折腾好几个星期。我的建议是不要急着追求复杂的控制器先把反步法在一个简化模型上跑通再逐步加上LMPC、噪声、扰动、输入约束。每一步改动都保留一份能跑的归档版本这样出了问题可以快速回退对比。我自己就是因为刚开始贪多把LMPC和反步法一次性全写进主循环结果仿真发散后根本不知道问题出在控制器推导还是代码实现上。最后一个小技巧仿真里的参考轨迹生成函数一定要尽量平滑尤其是参考加速度项。反步法的虚拟速度求导和LMPC的终端代价都对参考轨迹的平滑度很敏感轨迹不光滑会让两个控制器同时产生额外的抖振。如果参考轨迹本身带转折先做一次轨迹平滑预处理再让控制器跟踪效果会比硬着头皮直接跟踪好很多。