简介这份资源围绕Sparus自主水下航行器AUV的建模、分析与控制展开面向计算机、电子信息工程、数学等专业的大学生与研究人员适用于课程设计、期末大作业和毕业设计等场景。内容涵盖AUV物理模型、动力学模型、环境模型以及数值仿真与控制器设计配套案例数据可在Matlab中直接运行便于从理论到实践快速上手。压缩包共16个文件以m脚本为主11个包含Simulink模型mdl、仿真缓存slxc、说明文档md及旧版模型r2015a等整体大小仅68KB结构清晰、便于检索。已有80人学习浏览适合需要深入理解水下机器人建模与控制、并希望将理论落地为可运行代码的学习者。资源采用参数化编程参数可灵活调整代码注释详细、逻辑明确能有效辅助用户完成实验验证、参数优化和二次开发。1. Sparus 自主水下航行器的建模、分析与控制一条从方程到实机的闭环链路同一套 PID 参数在水池里跟踪误差能压到 0.2 米到了开放水域就漂到 1 米以上问题通常不在控制器而在模型没校准。Sparus 自主水下航行器AUV的建模、分析与控制正是解决这类漂移的完整链路先用六自由度运动学与动力学方程把 Sparus 写成一个可用数学描述的受控对象再通过水池实验数据反推水动力系数最后在这个模型之上做控制器设计和实时部署。本文面向正在做水下机器人、无人船以及刚接触 AUV 控制系统的工程师给出每一步可复现的方程、参数表和代码片段。下面的内容围绕“建模精度决定了控制带宽上限”这一主线展开按常见工程做法从理论推到实机调试不依赖任何专有软件。2. 从运动学到水动力Sparus AUV 的六自由度数学建模Sparus 这类小型 AUV 的运动可以分解为两个层次几何层描述“船在哪、朝向哪”动力学层描述“力和力矩如何改变速度”。这两个层次分别对应运动学方程和动力学方程也是后续做参数辨识、控制器设计和仿真验证的基础。建模时如果只写刚体方程而忽略水动力附加质量与阻尼仿真结果会与实机严重偏离因此这一章会把四部分完整展开。2.1 坐标系约定与 Sparus 运动学方程AUV 建模通常使用两个坐标系全球坐标系北东地NED固定在惯性空间原点取起始位置体坐标系固定在 AUV 上原点取重心附近x 轴指向艏向y 轴指向右舷z 轴向下。用向量 η[x, y, z, φ, θ, ψ]ᵀ 表示 NED 下的位置与欧拉角ν[u, v, w, p, q, r]ᵀ 表示体坐标系下的线速度与角速度。运动学方程为η̇ J(η) · ν其中 J(η) 是分块对角矩阵左上角为线速度旋转矩阵 R(φ, θ, ψ)右下角为欧拉角速度变换阵 T(φ, θ)。用小角度近似时T 可退化为单位阵附近的形式但在真实 AUV 控制中纵倾角 θ 常达到 20 度以上小角度近似会引入明显误差建议保留完整矩阵。function eta_dot kinematics(eta, nu) phi eta(4); theta eta(5); psi eta(6); R euler_rotation(phi, theta, psi); % 3x3 旋转矩阵 T [1, sin(phi)*tan(theta), cos(phi)*tan(theta); 0, cos(phi), -sin(phi); 0, sin(phi)/cos(theta), cos(phi)/cos(theta)]; eta_dot blkdiag(R, T) * nu; end上面euler_rotation按 ZYX 顺序构造旋转矩阵blkdiag把 3 线速度和 3 角速度的运动学拼接成 6 维映射。注意 T 在 θ±90° 时存在奇异实际下潜控制中要避免在这个姿态附近做解算或者切换到四元数表示。2.2 刚体惯性、科氏力与附加质量动力学方程的组成Sparus 的六自由度动力学方程可写成M ν̇ C(ν)ν D(ν)ν g(η) τ四个物理项的来源分别是M 包含刚体质量矩阵与附加质量矩阵C(ν) 是包含科氏力和向心力的矩阵来自刚体质量与附加质量的耦合D(ν) 是水动力阻尼包括线性项和二次项g(η) 是由重力与浮力共同产生的恢复力/恢复力矩。附加质量是水下机器人与地面机器人最大的差别。AUV 加速时周围水体会被一起加速等效为在惯性矩阵上叠加一个与加速度成正比的“虚拟质量”。对 Sparus 这类 2 米以内的紧凑型 AUV附加质量可以占到刚体质量的 30% 到 80%具体数值取决于载体外形。设计控制器时若忽略附加质量低频段可能仍然稳定但在频繁加减速的轨迹跟踪任务中会出现明显的相位滞后。表某小型 AUV 仿真用刚体与附加质量参数示例参数数值单位说明m18.0kg空气中质量I_z2.4kg·m²偏航转动惯量X_u̇-6.8kg纵荡附加质量Y_v̇-12.5kg横荡附加质量N_ṙ-1.8kg·m²偏航附加惯量上面这些数值是量级参考实际应以目标 AUV 的三维模型或辨识实验为准。有一个常见误用直接把从 CAD 模型算出的质量特性代入 M却忽略附加质量这会让仿真中的带宽比实机乐观导致控制器设计偏激进。2.3 水动力阻尼与恢复力低速 AUV 的简化模型Sparus 的巡航速度通常不超过 2 m/s在这种低速工况下阻尼以粘性阻力为主可以用线性加二次的简化模型描述D(ν) diag{X_u, Y_v, X_w, K_p, M_q, N_r} diag{X_u|u|·|u|, Y_v|v|·|v|, ...}线性阻尼系数由流场计算或自由衰减实验获得二次阻尼系数对应阻力随速度平方增长。有些工程师只拟合线性项结果模型在高速段偏软控制器增益在实机上被迫调低更稳妥的做法是把实验数据按 u|u| 形式回归到二次项而不是用 u²。符号上的 0.1 差别会显著影响大速度范围下的拟合质量。恢复力 g(η) 由重心与浮心不重合导致。若重心在浮心下方AUV 天然具有纵倾和横滚方向的自恢复能力这也是大多数 AUV 的默认设计。恢复力矩可以写成 λW·GM 的形式其中 GM 是初稳性高。该值决定了纵摇和横摇的固有频率后续做陷波滤波或者控制器带宽选择时需要先通过这个频率判断是否与控制带宽耦合。2.4 推力到广义力的映射确定控制自由度分配模型右侧的 τ∈R⁶ 是作用在 AUV 上的广义力与力矩。Sparus 这种小型 AUV 通常配置 4 个水平推进器和 2 个垂直推进器或者采用带矢量倾转的推进布局。推进器产生的力向量 f∈Rⁿ 与 τ 的关系为τ B · fB 矩阵的第 i 列是第 i 个推进器的推力作用位置与方向在体坐标系下的映射。以 4 个水平推进器为例若它们位于 x 方向前后两排、距中线 y 偏移则 B 中对应行的元素是纵荡力cos(α_i)偏航力矩-x_i sin(α_i) y_i cos(α_i)推进器安装角度 α_i 和力臂取值直接决定 B 矩阵。建模时最容易出错的是符号约定偏好正方向与体坐标系 x 轴正向不一致时B 中对应元素为负。一个快速验证方法是令 B 的第 i 列等于单位向量仿真观察 τ 是否指向预期方向比对着图纸手工推导更可靠。3. 参数辨识与模型分析用实验数据校准 Sparus 模型第 2 章的模型写完后如果所有系数都靠估算那么仿真与实机仍然对不上。工程上通用的做法是设计一组实验用可测量的推力、速度和加速度反推刚体质量与阻尼系数这个过程称为系统辨识。这一章围绕“如何把实验数据变成可用的模型参数”展开并介绍校验模型的一致性和可信度的两个方法。3.1 水池实验设计与最小二乘辨识流程辨识 Sparus 的纵荡方向参数可以施加一组已知幅值和频率的推力指令同时记录速度信号 resp。把动力学方程改写成线性回归形式X_u̇ u̇ X_u u X_u|u| u|u| τ_thrust令回归矩阵 Φ [u̇, u, u·|u|]待辨识参数 θ[X_u̇, X_u, X_u|u|]ᵀ则目标方程 Φ·θ τ_thrust_vec。用最小二乘求解即可。% 输入u, u_dot, tau_thrust 均为时间序列列向量 Phi [u_dot, u, u .* abs(u)]; theta Phi \ tau_thrust; % MATLAB 左除最小二乘解 X_udot theta(1); X_u theta(2); X_uu theta(3);左除运算符\在 MATLAB 中会自动选择 QR 分解或 Cholesky 分解数值稳定性优于直接求伪逆。注意输入数据必须经过低通滤波后再微分否则速度测量噪声会被差分放大导致回归矩阵病态。我一般会先对 u 做 2 Hz 截止频率的低通滤波再用中心差分计算 u̇。实验方案上推荐使用多阶跃与正弦扫频的组合阶跃响应用来估计稳态阻尼正弦扫频用来估计附加质量与阻尼的频变特性。单次阶跃无法同时辨识惯性与阻尼这是新手最容易踩的坑。3.2 分步辨识策略先阻尼后惯性避免多参数耦合如果同时辨识 M 和 D 的所有参数目标函数通常存在多个局部最小点解不稳定。更稳妥的做法是分步进行保持 AUV 匀速直线航行此时 u̇≈0方程退化为 X_u u X_u|u| u|u| τ先拟合阻尼系数在已知阻尼的前提下对 AUV 做定力启动实验从 0 加速到巡航速度拟合附加质量 X_u̇对偏航通道施加力矩辨识 N_r 与 N_r|r|并用横摇方向自由衰减实验验证恢复力模型。每一步的辨识结果都要独立验证。如果第二步拟合出的附加质量为负说明第一步的阻尼模型没有覆盖推进器尾流对船体阻力的影响或者速度测量存在恒定偏置。3.3 模型验证的误差指标与时域对比辨识完成后不能只看拟合曲线是否贴近训练数据还需要用一组未参与辨识的“验证序列”进行交叉验证。我常用的指标是时域上的相对均方根误差 RMSE 和最大绝对误差RMSE sqrt(mean((u_pred - u_meas).^2)) / (max(u_meas) - min(u_meas)); max_err max(abs(u_pred - u_meas));仿真时用四阶龙格库塔积分动力学方程把实测推力作为输入对比速度输出。如果 RMSE 在低速段连续超过 5%优先检查摩擦系数是不是用错了速度方向符号再检查推进器的推力到力的增益是否标定准确。另有一个容易被忽略的因素推进器在低转速时的死区会让微小推力信号完全失效需要在模型中增加到死区的静态非线性映射。3.4 线性化与频域分析确定控制器的可用带宽控制设计之前把非线性模型在工作点处做雅可比线性化。选择典型巡航点 u₀1.0 m/s忽略纵向与横荡的耦合项得到线性状态方程δẋ A δx B δu对小型 AUV纵荡、横荡和偏航三个通道的耦合较弱可以近似解耦。用 MATLAB 的linmod或解析求导获得 A、B 矩阵后计算可控性矩阵的秩确认系统不完全可控或者不可观的通道Co ctrb(A, B); rank_Co rank(Co); % 应等于状态维数 bode(ss(A, B, eye(size(A)), 0));Bode 图上关注两个点幅频特性跌落至 -3 dB 的频率以及相频特性穿越 -180° 的位置。-3 dB 频率大致决定了控制器只能在这个带宽以内有效之后加入再高的增益也无法追踪参考速度只会激发模型未建模的动态。这一结论直接决定了后续 PID 增益能调到多高以及是否需要引入陷波滤波器。4. 控制器设计与仿真从级联 PID 到滑模控制的递进路径模型分析给出结论后接下来是控制器设计。小型 AUV 最常见的起点是级联 PID 控制外环控制位置/深度内环控制速度/角速度。级联结构的优点是物理意义清晰、参数整定直觉化当任务要求更高的鲁棒性时可以再切换到滑模控制。这一章讲两种控制器的搭建方法和参数选择不讨论某一种“万能”控制器。4.1 级联 PID 控制结构的环间耦合与带宽分配以 Sparus 的深度控制为例级联结构为深度 PID外环输出期望垂向速度 w_ref垂向速度 PID内环输出控制力 τ_z。内环带宽要高于外环带宽 3 到 5 倍否则两个环会互相激励形成低频振荡。一个常见的参数分配是外环深度带宽 0.3 rad/s内环垂直速度带宽 1.2 rad/s相位裕度保持在 45° 以上。表深度控制级联 PID 参数示例控制器环KpKiKd采样周期深度外环0.80.020.120 ms垂向速度内环12.00.51.520 ms采样周期统一设为 20 ms50 Hz这是大多数 AUV 控制器的常规配置。内环 Kp 为什么可以取到 12因为深度环输出的 w_ref 变化平缓内环只需要保证快速跟随即可较大的比例增益能压缩速度误差只要相位裕度足够即可。4.2 在 Simulink 中搭建带抗积分饱和的 PID 控制器工程实现中Simulink 的 PID Controller 模块可以直接设置积分分离和抗饱和参数。若是手写代码常用形式如下function tau pid_antiwindup(ref, meas, e_int_prev, dt, Kp, Ki, Kd, tau_max) e ref - meas; e_dot (e - e_prev) / dt; % e_prev 由外部保存 e_int e_int_prev Ki * e * dt; % 积分分离大误差时冻结积分 if abs(e) 0.5 e_int e_int_prev; end tau_raw Kp * e e_int Kd * e_dot; tau max(min(tau_raw, tau_max), -tau_max); end注意上面的 Ki 已在 e_int 中提前乘过积分项直接使用 e_int。tau_max必须根据推进器的最大推力设定若小于未饱和输出需要在积分项上做反算补偿否则退出饱和后积分仍然很大产生明显超调。简单做法是饱和后再限制 e_int 不超过 tau_max 的 80%。4.3 滑模控制解决水动力不确定性的常见工程做法滑模控制在 AUV 上的优势是抵抗水动力系数摄动。以纵荡速度控制为例定义滑模面 s u_error λ·∫u_error控制律τ M_hat · (u_ref_dot - λ·u_error) D_hat·u - K·sat(s/Φ)其中 M_hat、D_hat 是标称模型参数K 是切换增益Φ 是边界层厚度。饱和函数 sat(s/Φ) 代替符号函数 sign(s)是为了抑制抖振。Φ 越大控制越平滑但鲁棒性下降Φ 太小推进器会持续高频开合。s u_error lambda * (cumtrapz(t, u_error)); tau_smc M_hat * (u_ref_dot - lambda * u_error) D_hat * u_ref - K * sat(s / Phi);滑模控制参数选择的经验是K 略大于水动力系数误差的上界Φ 取跟踪误差标准差的 2~3 倍。还有一个细节u_ref_dot 需要由参考轨迹解析求导或跟踪微分器提供直接对离散参考差分会产生噪声导致滑模面毛刺。4.4 仿真中的推进器饱和与模型不确定性注入闭环仿真时必须在控制输出后串联饱和环节而不是让控制器输出直接进入动力学模型。推进器饱和会让实际控制力小于期望力如果饱和特性未建模仿真中的稳定裕度会虚高。更严格的验证办法是在模型中同时注入参数摄动比如把附加质量在原值基础上拉偏 30%观察滑模控制的跟踪误差是否仍在可接受范围。5. 实时部署推力分配、状态估计与控制器离散化仿真通过后控制器要部署到 Sparus 实机。这一章处理的不是控制器设计本身而是从 Simulink 模型到实机代码的衔接问题如何把 6 维期望力映射到推进器指令如何融合传感器数据得到可靠的姿态与速度以及代码里采样周期和执行周期不一致怎么处理。5.1 推进器个数多于自由度时的分配策略Sparus 有 6 个自由度但推进器往往只有 4~6 个因此无法独立控制全部自由度。对于水平面 3 个自由度纵荡、横荡、偏航配 4 个水平推进器的构型B 矩阵是 3×4 的欠定方程。最小范数解为伪逆分配f B⁺ · τ_des但伪逆解可能在单个推进器饱和时把多余力矩分配给其他推进器导致隐性饱和。更稳妥的做法是带约束的最小二乘或二次规划把推进器上下限作为约束条件% 伪逆分配示例B 为 3x4 推力映射矩阵 B_pinv pinv(B); f_cmd B_pinv * tau_des(1:3); % 对 f_cmd 做线性缩放到安全区间 f_max 25; % 单推进器最大推力 N scale min(1, f_max / max(abs(f_cmd))); f_cmd f_cmd * scale;线性缩放策略在只有一个推进器饱和时可行但多个方向同时接近饱和时会损失精度。工程上另一种做法是忽略横荡方向的控制要求放宽 B 矩阵中横荡行优先保证纵荡和偏航这在路径跟踪中更符合实际任务需求。5.2 状态估计DVL、IMU 与深度计的组合控制器需要的状态往往不是直接测量值。Sparus 通常配备 IMU姿态、角速度、深度计z 和 w以及 DVL对地速度缺失时退化为零速假设。数据融合的常见做法是误差状态卡尔曼滤波ES-EKF状态量取位置、速度、姿态误差和传感器偏置。实现时有一个容易忽视的点DVL 与 IMU 的坐标系安装角度偏差必须标定否则融合后的速度在长时间航行中会缓慢漂移。滤波更新频率与控制器频率匹配IMU 100 Hz 做惯性递推DVL 和深度计 5~10 Hz 做测量更新。控制器读取的状态量必须是最新滤波值而不是直接读传感器原始值否则原始值的噪声会通过控制器增益放大。5.3 从 Simulink 到 C 的三条常见移植路径控制器部署有三条路径第一条用 MATLAB Coder 把 PID 或滑模控制函数直接生成 C 代码第二条按同一递归方程手写 C把控制器写成只依赖上一次状态的纯函数第三条在 ROS 2 中实现控制器节点通过话题接收状态估计并发布推力指令。我一般选择第二条因为手写代码便于阅读、没有许可证依赖也容易在无 MATLAB 的现场环境修改参数。手写时注意把控制器的所有状态误差积分、上一拍误差、滤波状态封装到一个结构体避免在回调函数中使用静态变量。静态变量在控制循环被打断时会引入难以排查的隐含状态。5.4 控制周期与执行器指令时序约束AUV 的推进器通常采用 CAN 总线或串口接收指令指令频率有限制。例如推进器驱动板支持 50 Hz 指令而控制器运行在 200 Hz就要求控制器只按 20 ms 周期对外发送多出的周期只做状态更新但不改变输出。这样一个简单的时序约束能避免总线拥塞和推进器响应不一致。还有一点推进器的启停响应有滞后通常 50~150 ms 量级控制律设计时需要考虑一个 Pade 一阶滞后模型否则闭环仿真会比实机乐观很多。6. 模型不确定性下的控制器整定技巧陷波滤波与增益调度模型参数拉偏 30% 之后还能不能稳定决定了控制器在开放水域是否可靠。最后这部分给出两个实战技巧用扫频实验确定实际对象的共振点再通过自适应频率控制思路对控制器增益做实时调整。先罚后奖地说第二步不会完全替代模型辨识但能明显提升鲁棒性。Sparus 在低速直航时纵倾和横滚通道的恢复力会形成一个固有振荡频率这个频率可以通过实验测得给垂直推进器一个短脉冲记录纵倾角的自由振荡响应再对响应做 FFT 求峰值频率。若该频率落在深度控制环带宽附近深度环会与纵倾振荡耦合表现为深度跟踪出现周期性波纹。处理方法是在深度控制回路中串联一个陷波滤波器H(s) (s² 2ζ_z ω_n s ω_n²) / (s² 2ζ_p ω_n s ω_n²)其中 ω_n 是振荡频率ζ_z 设为 1ζ_p 设为 0.1~0.3。离散化采用双线性变换加入陷波器后在 Bode 图上确认振荡频率处增益跌落 15 dB 以上同时保证 0.5 rad/s 以下的低频段不受影响。增益调度的思路是在不同的航速工作点上水动力阻尼差别明显一套固定的 PID 增益难以兼顾低速操纵与高速巡航。做法是先分别针对 0.5 m/s、1.0 m/s、1.5 m/s 三个工作点整定出一组 PID 参数再拟合出一条增益随时间变化的查表曲线控制器运行时按当前估计速度实时插值。实现时要注意调度频率不能太快否则增益突变会激发瞬态响应可以在 PID 参数更新时增加一阶低通让参数变化速度限制在 0.05 倍/秒以内。验证增益调度是否有效的办法是设置一个速度阶梯变化的参考轨迹让 AUV 从 0.5 m/s 加速到 1.5 m/s观察纵荡速度跟踪误差在切换点前后是否出现持续振荡。若振荡频率与某个已知共振点重合把陷波器中心频率同步调度就能在不让全局增益妥协的情况下压住振荡这比直接调低 PID 增益更高效且保持了动态响应。把扫频结果和陷波器状态写进调试日志排查耦合振荡时能直接定位到是模型共振还是推进器死区导致而不是靠猜。本文还有配套的精品资源点击获取 
长河君 · 2026/9/10 8:13:31 · 阅读 1.2k 自主水下航行器建模与控制：从六自由度方程到实机部署
简介这份资源围绕Sparus自主水下航行器AUV的建模、分析与控制展开面向计算机、电子信息工程、数学等专业的大学生与研究人员适用于课程设计、期末大作业和毕业设计等场景。内容涵盖AUV物理模型、动力学模型、环境模型以及数值仿真与控制器设计配套案例数据可在Matlab中直接运行便于从理论到实践快速上手。压缩包共16个文件以m脚本为主11个包含Simulink模型mdl、仿真缓存slxc、说明文档md及旧版模型r2015a等整体大小仅68KB结构清晰、便于检索。已有80人学习浏览适合需要深入理解水下机器人建模与控制、并希望将理论落地为可运行代码的学习者。资源采用参数化编程参数可灵活调整代码注释详细、逻辑明确能有效辅助用户完成实验验证、参数优化和二次开发。1. Sparus 自主水下航行器的建模、分析与控制一条从方程到实机的闭环链路同一套 PID 参数在水池里跟踪误差能压到 0.2 米到了开放水域就漂到 1 米以上问题通常不在控制器而在模型没校准。Sparus 自主水下航行器AUV的建模、分析与控制正是解决这类漂移的完整链路先用六自由度运动学与动力学方程把 Sparus 写成一个可用数学描述的受控对象再通过水池实验数据反推水动力系数最后在这个模型之上做控制器设计和实时部署。本文面向正在做水下机器人、无人船以及刚接触 AUV 控制系统的工程师给出每一步可复现的方程、参数表和代码片段。下面的内容围绕“建模精度决定了控制带宽上限”这一主线展开按常见工程做法从理论推到实机调试不依赖任何专有软件。2. 从运动学到水动力Sparus AUV 的六自由度数学建模Sparus 这类小型 AUV 的运动可以分解为两个层次几何层描述“船在哪、朝向哪”动力学层描述“力和力矩如何改变速度”。这两个层次分别对应运动学方程和动力学方程也是后续做参数辨识、控制器设计和仿真验证的基础。建模时如果只写刚体方程而忽略水动力附加质量与阻尼仿真结果会与实机严重偏离因此这一章会把四部分完整展开。2.1 坐标系约定与 Sparus 运动学方程AUV 建模通常使用两个坐标系全球坐标系北东地NED固定在惯性空间原点取起始位置体坐标系固定在 AUV 上原点取重心附近x 轴指向艏向y 轴指向右舷z 轴向下。用向量 η[x, y, z, φ, θ, ψ]ᵀ 表示 NED 下的位置与欧拉角ν[u, v, w, p, q, r]ᵀ 表示体坐标系下的线速度与角速度。运动学方程为η̇ J(η) · ν其中 J(η) 是分块对角矩阵左上角为线速度旋转矩阵 R(φ, θ, ψ)右下角为欧拉角速度变换阵 T(φ, θ)。用小角度近似时T 可退化为单位阵附近的形式但在真实 AUV 控制中纵倾角 θ 常达到 20 度以上小角度近似会引入明显误差建议保留完整矩阵。function eta_dot kinematics(eta, nu) phi eta(4); theta eta(5); psi eta(6); R euler_rotation(phi, theta, psi); % 3x3 旋转矩阵 T [1, sin(phi)*tan(theta), cos(phi)*tan(theta); 0, cos(phi), -sin(phi); 0, sin(phi)/cos(theta), cos(phi)/cos(theta)]; eta_dot blkdiag(R, T) * nu; end上面euler_rotation按 ZYX 顺序构造旋转矩阵blkdiag把 3 线速度和 3 角速度的运动学拼接成 6 维映射。注意 T 在 θ±90° 时存在奇异实际下潜控制中要避免在这个姿态附近做解算或者切换到四元数表示。2.2 刚体惯性、科氏力与附加质量动力学方程的组成Sparus 的六自由度动力学方程可写成M ν̇ C(ν)ν D(ν)ν g(η) τ四个物理项的来源分别是M 包含刚体质量矩阵与附加质量矩阵C(ν) 是包含科氏力和向心力的矩阵来自刚体质量与附加质量的耦合D(ν) 是水动力阻尼包括线性项和二次项g(η) 是由重力与浮力共同产生的恢复力/恢复力矩。附加质量是水下机器人与地面机器人最大的差别。AUV 加速时周围水体会被一起加速等效为在惯性矩阵上叠加一个与加速度成正比的“虚拟质量”。对 Sparus 这类 2 米以内的紧凑型 AUV附加质量可以占到刚体质量的 30% 到 80%具体数值取决于载体外形。设计控制器时若忽略附加质量低频段可能仍然稳定但在频繁加减速的轨迹跟踪任务中会出现明显的相位滞后。表某小型 AUV 仿真用刚体与附加质量参数示例参数数值单位说明m18.0kg空气中质量I_z2.4kg·m²偏航转动惯量X_u̇-6.8kg纵荡附加质量Y_v̇-12.5kg横荡附加质量N_ṙ-1.8kg·m²偏航附加惯量上面这些数值是量级参考实际应以目标 AUV 的三维模型或辨识实验为准。有一个常见误用直接把从 CAD 模型算出的质量特性代入 M却忽略附加质量这会让仿真中的带宽比实机乐观导致控制器设计偏激进。2.3 水动力阻尼与恢复力低速 AUV 的简化模型Sparus 的巡航速度通常不超过 2 m/s在这种低速工况下阻尼以粘性阻力为主可以用线性加二次的简化模型描述D(ν) diag{X_u, Y_v, X_w, K_p, M_q, N_r} diag{X_u|u|·|u|, Y_v|v|·|v|, ...}线性阻尼系数由流场计算或自由衰减实验获得二次阻尼系数对应阻力随速度平方增长。有些工程师只拟合线性项结果模型在高速段偏软控制器增益在实机上被迫调低更稳妥的做法是把实验数据按 u|u| 形式回归到二次项而不是用 u²。符号上的 0.1 差别会显著影响大速度范围下的拟合质量。恢复力 g(η) 由重心与浮心不重合导致。若重心在浮心下方AUV 天然具有纵倾和横滚方向的自恢复能力这也是大多数 AUV 的默认设计。恢复力矩可以写成 λW·GM 的形式其中 GM 是初稳性高。该值决定了纵摇和横摇的固有频率后续做陷波滤波或者控制器带宽选择时需要先通过这个频率判断是否与控制带宽耦合。2.4 推力到广义力的映射确定控制自由度分配模型右侧的 τ∈R⁶ 是作用在 AUV 上的广义力与力矩。Sparus 这种小型 AUV 通常配置 4 个水平推进器和 2 个垂直推进器或者采用带矢量倾转的推进布局。推进器产生的力向量 f∈Rⁿ 与 τ 的关系为τ B · fB 矩阵的第 i 列是第 i 个推进器的推力作用位置与方向在体坐标系下的映射。以 4 个水平推进器为例若它们位于 x 方向前后两排、距中线 y 偏移则 B 中对应行的元素是纵荡力cos(α_i)偏航力矩-x_i sin(α_i) y_i cos(α_i)推进器安装角度 α_i 和力臂取值直接决定 B 矩阵。建模时最容易出错的是符号约定偏好正方向与体坐标系 x 轴正向不一致时B 中对应元素为负。一个快速验证方法是令 B 的第 i 列等于单位向量仿真观察 τ 是否指向预期方向比对着图纸手工推导更可靠。3. 参数辨识与模型分析用实验数据校准 Sparus 模型第 2 章的模型写完后如果所有系数都靠估算那么仿真与实机仍然对不上。工程上通用的做法是设计一组实验用可测量的推力、速度和加速度反推刚体质量与阻尼系数这个过程称为系统辨识。这一章围绕“如何把实验数据变成可用的模型参数”展开并介绍校验模型的一致性和可信度的两个方法。3.1 水池实验设计与最小二乘辨识流程辨识 Sparus 的纵荡方向参数可以施加一组已知幅值和频率的推力指令同时记录速度信号 resp。把动力学方程改写成线性回归形式X_u̇ u̇ X_u u X_u|u| u|u| τ_thrust令回归矩阵 Φ [u̇, u, u·|u|]待辨识参数 θ[X_u̇, X_u, X_u|u|]ᵀ则目标方程 Φ·θ τ_thrust_vec。用最小二乘求解即可。% 输入u, u_dot, tau_thrust 均为时间序列列向量 Phi [u_dot, u, u .* abs(u)]; theta Phi \ tau_thrust; % MATLAB 左除最小二乘解 X_udot theta(1); X_u theta(2); X_uu theta(3);左除运算符\在 MATLAB 中会自动选择 QR 分解或 Cholesky 分解数值稳定性优于直接求伪逆。注意输入数据必须经过低通滤波后再微分否则速度测量噪声会被差分放大导致回归矩阵病态。我一般会先对 u 做 2 Hz 截止频率的低通滤波再用中心差分计算 u̇。实验方案上推荐使用多阶跃与正弦扫频的组合阶跃响应用来估计稳态阻尼正弦扫频用来估计附加质量与阻尼的频变特性。单次阶跃无法同时辨识惯性与阻尼这是新手最容易踩的坑。3.2 分步辨识策略先阻尼后惯性避免多参数耦合如果同时辨识 M 和 D 的所有参数目标函数通常存在多个局部最小点解不稳定。更稳妥的做法是分步进行保持 AUV 匀速直线航行此时 u̇≈0方程退化为 X_u u X_u|u| u|u| τ先拟合阻尼系数在已知阻尼的前提下对 AUV 做定力启动实验从 0 加速到巡航速度拟合附加质量 X_u̇对偏航通道施加力矩辨识 N_r 与 N_r|r|并用横摇方向自由衰减实验验证恢复力模型。每一步的辨识结果都要独立验证。如果第二步拟合出的附加质量为负说明第一步的阻尼模型没有覆盖推进器尾流对船体阻力的影响或者速度测量存在恒定偏置。3.3 模型验证的误差指标与时域对比辨识完成后不能只看拟合曲线是否贴近训练数据还需要用一组未参与辨识的“验证序列”进行交叉验证。我常用的指标是时域上的相对均方根误差 RMSE 和最大绝对误差RMSE sqrt(mean((u_pred - u_meas).^2)) / (max(u_meas) - min(u_meas)); max_err max(abs(u_pred - u_meas));仿真时用四阶龙格库塔积分动力学方程把实测推力作为输入对比速度输出。如果 RMSE 在低速段连续超过 5%优先检查摩擦系数是不是用错了速度方向符号再检查推进器的推力到力的增益是否标定准确。另有一个容易被忽略的因素推进器在低转速时的死区会让微小推力信号完全失效需要在模型中增加到死区的静态非线性映射。3.4 线性化与频域分析确定控制器的可用带宽控制设计之前把非线性模型在工作点处做雅可比线性化。选择典型巡航点 u₀1.0 m/s忽略纵向与横荡的耦合项得到线性状态方程δẋ A δx B δu对小型 AUV纵荡、横荡和偏航三个通道的耦合较弱可以近似解耦。用 MATLAB 的linmod或解析求导获得 A、B 矩阵后计算可控性矩阵的秩确认系统不完全可控或者不可观的通道Co ctrb(A, B); rank_Co rank(Co); % 应等于状态维数 bode(ss(A, B, eye(size(A)), 0));Bode 图上关注两个点幅频特性跌落至 -3 dB 的频率以及相频特性穿越 -180° 的位置。-3 dB 频率大致决定了控制器只能在这个带宽以内有效之后加入再高的增益也无法追踪参考速度只会激发模型未建模的动态。这一结论直接决定了后续 PID 增益能调到多高以及是否需要引入陷波滤波器。4. 控制器设计与仿真从级联 PID 到滑模控制的递进路径模型分析给出结论后接下来是控制器设计。小型 AUV 最常见的起点是级联 PID 控制外环控制位置/深度内环控制速度/角速度。级联结构的优点是物理意义清晰、参数整定直觉化当任务要求更高的鲁棒性时可以再切换到滑模控制。这一章讲两种控制器的搭建方法和参数选择不讨论某一种“万能”控制器。4.1 级联 PID 控制结构的环间耦合与带宽分配以 Sparus 的深度控制为例级联结构为深度 PID外环输出期望垂向速度 w_ref垂向速度 PID内环输出控制力 τ_z。内环带宽要高于外环带宽 3 到 5 倍否则两个环会互相激励形成低频振荡。一个常见的参数分配是外环深度带宽 0.3 rad/s内环垂直速度带宽 1.2 rad/s相位裕度保持在 45° 以上。表深度控制级联 PID 参数示例控制器环KpKiKd采样周期深度外环0.80.020.120 ms垂向速度内环12.00.51.520 ms采样周期统一设为 20 ms50 Hz这是大多数 AUV 控制器的常规配置。内环 Kp 为什么可以取到 12因为深度环输出的 w_ref 变化平缓内环只需要保证快速跟随即可较大的比例增益能压缩速度误差只要相位裕度足够即可。4.2 在 Simulink 中搭建带抗积分饱和的 PID 控制器工程实现中Simulink 的 PID Controller 模块可以直接设置积分分离和抗饱和参数。若是手写代码常用形式如下function tau pid_antiwindup(ref, meas, e_int_prev, dt, Kp, Ki, Kd, tau_max) e ref - meas; e_dot (e - e_prev) / dt; % e_prev 由外部保存 e_int e_int_prev Ki * e * dt; % 积分分离大误差时冻结积分 if abs(e) 0.5 e_int e_int_prev; end tau_raw Kp * e e_int Kd * e_dot; tau max(min(tau_raw, tau_max), -tau_max); end注意上面的 Ki 已在 e_int 中提前乘过积分项直接使用 e_int。tau_max必须根据推进器的最大推力设定若小于未饱和输出需要在积分项上做反算补偿否则退出饱和后积分仍然很大产生明显超调。简单做法是饱和后再限制 e_int 不超过 tau_max 的 80%。4.3 滑模控制解决水动力不确定性的常见工程做法滑模控制在 AUV 上的优势是抵抗水动力系数摄动。以纵荡速度控制为例定义滑模面 s u_error λ·∫u_error控制律τ M_hat · (u_ref_dot - λ·u_error) D_hat·u - K·sat(s/Φ)其中 M_hat、D_hat 是标称模型参数K 是切换增益Φ 是边界层厚度。饱和函数 sat(s/Φ) 代替符号函数 sign(s)是为了抑制抖振。Φ 越大控制越平滑但鲁棒性下降Φ 太小推进器会持续高频开合。s u_error lambda * (cumtrapz(t, u_error)); tau_smc M_hat * (u_ref_dot - lambda * u_error) D_hat * u_ref - K * sat(s / Phi);滑模控制参数选择的经验是K 略大于水动力系数误差的上界Φ 取跟踪误差标准差的 2~3 倍。还有一个细节u_ref_dot 需要由参考轨迹解析求导或跟踪微分器提供直接对离散参考差分会产生噪声导致滑模面毛刺。4.4 仿真中的推进器饱和与模型不确定性注入闭环仿真时必须在控制输出后串联饱和环节而不是让控制器输出直接进入动力学模型。推进器饱和会让实际控制力小于期望力如果饱和特性未建模仿真中的稳定裕度会虚高。更严格的验证办法是在模型中同时注入参数摄动比如把附加质量在原值基础上拉偏 30%观察滑模控制的跟踪误差是否仍在可接受范围。5. 实时部署推力分配、状态估计与控制器离散化仿真通过后控制器要部署到 Sparus 实机。这一章处理的不是控制器设计本身而是从 Simulink 模型到实机代码的衔接问题如何把 6 维期望力映射到推进器指令如何融合传感器数据得到可靠的姿态与速度以及代码里采样周期和执行周期不一致怎么处理。5.1 推进器个数多于自由度时的分配策略Sparus 有 6 个自由度但推进器往往只有 4~6 个因此无法独立控制全部自由度。对于水平面 3 个自由度纵荡、横荡、偏航配 4 个水平推进器的构型B 矩阵是 3×4 的欠定方程。最小范数解为伪逆分配f B⁺ · τ_des但伪逆解可能在单个推进器饱和时把多余力矩分配给其他推进器导致隐性饱和。更稳妥的做法是带约束的最小二乘或二次规划把推进器上下限作为约束条件% 伪逆分配示例B 为 3x4 推力映射矩阵 B_pinv pinv(B); f_cmd B_pinv * tau_des(1:3); % 对 f_cmd 做线性缩放到安全区间 f_max 25; % 单推进器最大推力 N scale min(1, f_max / max(abs(f_cmd))); f_cmd f_cmd * scale;线性缩放策略在只有一个推进器饱和时可行但多个方向同时接近饱和时会损失精度。工程上另一种做法是忽略横荡方向的控制要求放宽 B 矩阵中横荡行优先保证纵荡和偏航这在路径跟踪中更符合实际任务需求。5.2 状态估计DVL、IMU 与深度计的组合控制器需要的状态往往不是直接测量值。Sparus 通常配备 IMU姿态、角速度、深度计z 和 w以及 DVL对地速度缺失时退化为零速假设。数据融合的常见做法是误差状态卡尔曼滤波ES-EKF状态量取位置、速度、姿态误差和传感器偏置。实现时有一个容易忽视的点DVL 与 IMU 的坐标系安装角度偏差必须标定否则融合后的速度在长时间航行中会缓慢漂移。滤波更新频率与控制器频率匹配IMU 100 Hz 做惯性递推DVL 和深度计 5~10 Hz 做测量更新。控制器读取的状态量必须是最新滤波值而不是直接读传感器原始值否则原始值的噪声会通过控制器增益放大。5.3 从 Simulink 到 C 的三条常见移植路径控制器部署有三条路径第一条用 MATLAB Coder 把 PID 或滑模控制函数直接生成 C 代码第二条按同一递归方程手写 C把控制器写成只依赖上一次状态的纯函数第三条在 ROS 2 中实现控制器节点通过话题接收状态估计并发布推力指令。我一般选择第二条因为手写代码便于阅读、没有许可证依赖也容易在无 MATLAB 的现场环境修改参数。手写时注意把控制器的所有状态误差积分、上一拍误差、滤波状态封装到一个结构体避免在回调函数中使用静态变量。静态变量在控制循环被打断时会引入难以排查的隐含状态。5.4 控制周期与执行器指令时序约束AUV 的推进器通常采用 CAN 总线或串口接收指令指令频率有限制。例如推进器驱动板支持 50 Hz 指令而控制器运行在 200 Hz就要求控制器只按 20 ms 周期对外发送多出的周期只做状态更新但不改变输出。这样一个简单的时序约束能避免总线拥塞和推进器响应不一致。还有一点推进器的启停响应有滞后通常 50~150 ms 量级控制律设计时需要考虑一个 Pade 一阶滞后模型否则闭环仿真会比实机乐观很多。6. 模型不确定性下的控制器整定技巧陷波滤波与增益调度模型参数拉偏 30% 之后还能不能稳定决定了控制器在开放水域是否可靠。最后这部分给出两个实战技巧用扫频实验确定实际对象的共振点再通过自适应频率控制思路对控制器增益做实时调整。先罚后奖地说第二步不会完全替代模型辨识但能明显提升鲁棒性。Sparus 在低速直航时纵倾和横滚通道的恢复力会形成一个固有振荡频率这个频率可以通过实验测得给垂直推进器一个短脉冲记录纵倾角的自由振荡响应再对响应做 FFT 求峰值频率。若该频率落在深度控制环带宽附近深度环会与纵倾振荡耦合表现为深度跟踪出现周期性波纹。处理方法是在深度控制回路中串联一个陷波滤波器H(s) (s² 2ζ_z ω_n s ω_n²) / (s² 2ζ_p ω_n s ω_n²)其中 ω_n 是振荡频率ζ_z 设为 1ζ_p 设为 0.1~0.3。离散化采用双线性变换加入陷波器后在 Bode 图上确认振荡频率处增益跌落 15 dB 以上同时保证 0.5 rad/s 以下的低频段不受影响。增益调度的思路是在不同的航速工作点上水动力阻尼差别明显一套固定的 PID 增益难以兼顾低速操纵与高速巡航。做法是先分别针对 0.5 m/s、1.0 m/s、1.5 m/s 三个工作点整定出一组 PID 参数再拟合出一条增益随时间变化的查表曲线控制器运行时按当前估计速度实时插值。实现时要注意调度频率不能太快否则增益突变会激发瞬态响应可以在 PID 参数更新时增加一阶低通让参数变化速度限制在 0.05 倍/秒以内。验证增益调度是否有效的办法是设置一个速度阶梯变化的参考轨迹让 AUV 从 0.5 m/s 加速到 1.5 m/s观察纵荡速度跟踪误差在切换点前后是否出现持续振荡。若振荡频率与某个已知共振点重合把陷波器中心频率同步调度就能在不让全局增益妥协的情况下压住振荡这比直接调低 PID 增益更高效且保持了动态响应。把扫频结果和陷波器状态写进调试日志排查耦合振荡时能直接定位到是模型共振还是推进器死区导致而不是靠猜。本文还有配套的精品资源点击获取