AUV仿真从零搭建：MATLAB/Simulink水下航行器建模与参数辨识指南

简介面向水下无人自主航行器仿真学习与开发的压缩包提供基于MATLAB和Simulink的完整建模与仿真工程适合水下机器人、自动化与控制专业的学生及工程师对照研究可用于验证控制算法、分析机动性能或作为毕业设计的基础代码。包内包含Simulink模型、S函数、M脚本、C/C源文件以及动态库覆盖航行器动力学建模、控制律设计、坐标系转换、水动力参数计算等关键环节通过S函数和M文件可深入理解自定义模块的编写与调用方式结合PDF文档与文本说明可快速理清代码逻辑不同m文件和c文件分别承担参数初始化、核心算法与接口封装等职责模块划分清晰。资源共61个文件压缩包仅406KB轻量紧凑便于下载后在MATLAB中直接运行和调试也适合作为课程设计或课题预研的参考模板。已有676人学习尤其适合希望从零搭建水下航行器仿真环境、掌握Simulink模块化建模或进一步扩展导航滤波算法的读者。1. 为什么说 AUV 仿真的第一关在 MATLAB/Simulink 建模一条水下航行器要从项目文档走向水池试验中间隔着电池、密封与回收好几个坎很多团队选择先把控制算法和路径规划在虚拟环境里验证完再动硬件因此 AUV 仿真几乎成了课题组的必修前置。像标题中“shark.rar”这样的压缩包解压后大多是一套 .slx 模型加多个 .m 初始化脚本不管它来自研究生师兄还是公开课能把它跑成一条可信的、可复现的水下航行轨迹取决于你是否清楚背后的运动学方程、动力学参数、求解器设置和数据处理链路。下面按“建立数学模型 → Simulink 建模 → 排除仿真发散 → 可视化验证 → 参数辨识”的顺序推进覆盖 MATLAB、Simulink 与水下航行三者的交叠部分。已有经验的工程师可以直接跳到第 4 章和第 6 章看调参边界。2. 建立 AUV 运动学与动力学模型仿真前要把这 3 类方程写清楚2.1 水下航行器运动学方程推导坐标变换是模型的骨架即使从压缩包里拿到了模型也要先在纸上把 AUV 模型的数学结构理顺。所有的 Simulink 模块都只是在数值层面复现同一条物理关系NED 大地坐标系负责描述位置与姿态载体坐标系负责描述速度与角速度。二者靠旋转矩阵 $J(\eta)$ 联系起来运动学方程写作 $\dot{\eta} J(\eta) \cdot \nu$其中 $\eta [x, y, z, \phi, \theta, \psi]^T$$\nu [u, v, w, p, q, r]^T$。自由度体坐标系速度NED 位置/姿态纵荡ux横荡vy垂荡wz横摇pphi纵摇qtheta艏摇rpsi把旋转矩阵写成 MATLAB 函数后面在 Simulink 的 MATLAB Function 块里可以直接引用function eta_dot auv_kinematics(eta, nu) % AUV 运动学体坐标速度 - NED 坐标速度 % eta [x; y; z; phi; theta; psi] % nu [u; v; w; p; q; r] phi eta(4); theta eta(5); psi eta(6); Rz [cos(psi) -sin(psi) 0; sin(psi) cos(psi) 0; 0 0 1]; Ry [cos(theta) 0 sin(theta); 0 1 0; -sin(theta) 0 cos(theta)]; Rx [1 0 0; 0 cos(phi) -sin(phi); 0 sin(phi) cos(phi)]; J1 Rz * Ry * Rx; J2 [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 zeros(6,1); eta_dot(1:3) J1 * nu(1:3); eta_dot(4:6) J2 * nu(4:6); end这段代码里J1负责把线速度从载体坐标系转到 NED 坐标系J2负责把角速度转成欧拉角速率。新手容易踩的坑是直接用J1去变换角速度那会让偏航速率被错误投影最终在 Scope 里看到欧拉角乱跳。另外J2在俯仰角接近 ±90° 时会发散若 AUV 要做倒扣或大俯仰机动建议把姿态表示换成四元数否则仿真一到边界就退化。2.2 刚体动力学与水动力系数决定仿真“像不像真的”的参数运动学只解决方向问题真正决定加速度的是动力学方程。水下航行器六自由度动力学常写成 Fossen 形式$M \dot{\nu} C(\nu)\nu D(\nu_r)\nu_r g(\eta) \tau$。其中 $M M_{RB} M_A$ 包含刚体惯性矩阵和附加质量矩阵$C$ 是科氏力与向心力矩阵$D$ 是水动力阻尼$g$ 是重力与浮力恢复项$\tau$ 是推进器与舵面产生的力和力矩。小型 AUV 很难把全部水动力导数辨识出来工程上常用水平面三自由度模型只保留 u、v、r。此时 $M diag(m X_{\dot u}, m Y_{\dot v}, I_z N_{\dot r})$$D diag(X_u, Y_v, N_r)$。下面是一组典型小型实验 AUV 参数参数数值单位质量 m12.5kg纵荡附加质量 -X_u_dot2.5kg横荡附加质量 -Y_v_dot12kg惯性矩 I_z0.35kg·m^2纵荡阻尼 X_u1.2kg/s横荡阻尼 Y_v8kg/s艏摇阻尼 N_r1.5kg·m^2/s文献中这些参数常以无因次形式给出必须乘上水密度与特征长度后才能用于仿真。还有一个容易被忽略的细节阻尼矩阵里的交叉耦合项如果缺失AUV 在做原地回转时会绕几何中心旋转而实际模型中如果浮心与重心不重合转角轨迹会有明显偏移。仿真曲线越看越别扭时优先检查交叉阻尼。2.3 Simulink 中把动力学方程落地的常见写法实现上述方程的常见做法是用 MATLAB Function 块计算 $\dot{\nu}$对 $\dot{\nu}$ 积分得到速度再把速度传入运动学子系统得到 $\dot{\eta}$积分后得到位置与姿态。下面这个函数把简化动力学与运动学合并方便直接接积分器function [nu_dot, eta_dot] auv_dynamics_and_kinematics(eta, nu, tau) % 简化水平面 AUV 模型只考虑 u/v/r m 12.5; Iz 0.35; Xu_dot 2.5; Yv_dot 12; Nr_dot 1.2; Xu 1.2; Yv 8.0; Nr 1.5; M diag([m Xu_dot, m Yv_dot, Iz Nr_dot]); C [0 0 -m*nu(2); 0 0 m*nu(1); m*nu(2) -m*nu(1) 0]; D diag([Xu, Yv, Nr]); nu_dot_3d M \ (tau - C*nu(1:3) - D*nu(1:3)); nu_dot [nu_dot_3d; 0; 0; 0]; eta6 [eta(1:3); 0; 0; eta(6)]; nu6 [nu(1:3); 0; 0; nu(6)]; eta_dot6 auv_kinematics(eta6, nu6); eta_dot eta_dot6; end注意这里用的是M \而不是inv(M) *。inv在矩阵元素量级差异大时容易放大舍入误差M \内部走数值求解在大多数 Simulink 模型里数值稳定性更好。水动力矩阵若接近奇异M \也会报错这比“跑出 NaN 再回头找”要容易定位得多。3. 从 shark.rar 到可运行模型Simulink 建模的最小复现路径3.1 打开压缩包后先分清四类文件拿到标题里这类 shark.rar 压缩包不要急着双击 .slx。先看文件后缀常见的 AUV 仿真工程通常包含下面四类内容文件类型典型用途打开模型前要做什么.m参数初始化、后处理脚本先运行或确认被模型回调自动运行.slx主仿真模型用 get_param 查看 InitFcn 回调.mat实验数据、初始状态load 后核对变量名是否与模型一致.sldd数据字典检查模型是否绑定字典避免和 init 脚本冲突检查模型初始化回调的命令是mdl shark_sim; % 以压缩包里实际模型名为准 get_param(mdl, PreLoadFcn) get_param(mdl, InitFcn)如果InitFcn返回空说明参数需要手动运行对应的 init_auv.m返回字符串里有run命令说明压缩包作者已经把初始化嵌进模型回调。换机器跑不起模型大多是因为回调被清除或 aux 脚本依赖的绝对路径失效。遇到这种情况第一步先把 .m 和 .slx 路径都加到 MATLAB 的路径里再执行一次clear; close_system; load_system(mdl)。3.2 用基本模块搭一个最小 AUV 仿真闭环假设压缩包里没有现成模型需要从零搭一个可运行最小系统。新建空白模型后拖入一个 MATLAB Function 块作为动力学两个 Integrator 分别积分 $\dot{\nu}$ 和 $\dot{\eta}$一个 Inport 作为推力指令 tau一个 To Workspace 记录输出。把上一节的auv_dynamics_and_kinematics写入 MATLAB Function 块积分器的初始条件设为[0;0;0]和[0;0;0]表示 AUV 从静止开始。连线时注意$nu_dot$ 必须进入 nu 积分器$nu$ 返回动力学函数的同时也传入运动学部分$eta_dot$ 进入 eta 积分器$eta$ 再反馈回动力学函数。积分器天然切断直接馈通所以这套结构不会产生代数环。如果把积分器去掉直接让 nu_dot 反馈到 nu 端口Simulink 会报 “direct feedthrough” 错误这正是初学者最容易卡住的地方。3.3 用 MATLAB 脚本装载参数并批量仿真模型建好后用脚本驱动仿真比每次手动点 Run 更可控。下面的脚本假设模型名为auv_sim.slx且模型中的 MATLAB Function 块从基础工作区读取M_A和Dclear; clc; close all; % 初始化水动力参数 M_A diag([2.5, 12.0, 1.2]); D diag([1.2, 8.0, 1.5]); assignin(base, M_A, M_A); assignin(base, D, D); % 配置仿真并运行 simOut sim(auv_sim, ... StopTime, 200, ... MaxStep, 0.01, ... SaveOutput, on, ... ReturnWorkspaceOutputs, on); % 提取输出并绘制三维轨迹 y simOut.get(yout); data y{1}.Values.Data; plot3(data(:,1), data(:,2), data(:,3)); xlabel(x (m)); ylabel(y (m)); zlabel(z (m)); grid on;MaxStep对变步长求解器是上限约束设置太大会丢掉推进器阶跃响应中的动态设置太小又会把 200 秒仿真拖到几十秒。建议先从0.01起步观察姿态曲线没有毛刺后再逐步放大。simOut.get(yout)的写法适配老模型新模型如果使用logsout需要改成simOut.logsout.getElement方式后面第 5 章会专门讲。3.4 模型回调和初值不一致的排查参数变量都设好但结果仍然不对先检查模型内部是否有硬编码的 Constant 块覆盖了初始化脚本里的初值。判断方法是运行前执行clear eta0 nu0再跑一次模型如果轨迹不随变量清空而改变说明初值被写死在 Constant 里。更规范的做法是让模型通过Parameter类型的 Simulink.Parameter 对象读取初值在 InitFcn 回调里统一赋值。这样后续做蒙特卡洛仿真时只需要循环修改工作区里的eta0和nu0就能批量生成不同初始状态下的轨迹数据。4. 仿真发散先按表排查AUV Simulink 中的三个诱发点4.1 仿真发散的表现与定位思路Simulink 里的“仿真发散”通常指输出数值在很短时间内变成 NaN 或 ±Inf。打开 Scope 会看到曲线直接冲出坐标系控制器输出饱和成直线。排查时不要急于加限幅先看发散发生在哪一步第一步就发散多半与参数矩阵奇异或初值错误有关运行几十秒后才发散则要检查状态是否越过欧拉角奇异点、控制器指令是否让阻尼变成负值。发散时刻典型原因排查方向第 1 步发散参数矩阵奇异或初值错误检查 M 矩阵特征值中间段发散状态超出欧拉角适用范围改用四元数表示姿态高频振荡后发散求解器步长过大调小相对误差或最大步长外部指令跳变时发散执行器模型产生过大加速度给力指令加斜率约束4.2 求解器与步长的匹配关系AUV 仿真的时间常数跨度很大位置响应是秒级推进器和舵机的响应是毫秒到百毫秒级。变步长求解器遇到信号切换会自动缩小步长但缩得不够仍会丢转折点。一套保守配置是纯连续 AUV 模型用ode45相对误差设1e-4最大步长取0.01 秒模型中如果包含离散控制器或 PWM 输出改用固定步长求解器步长取控制器采样周期的四分之一。用命令行设置固定步长set_param(auv_sim, SolverType, Fixed-step, ... Solver, ode4, ... FixedStep, 0.0025);固定步长 0.0025 秒意味着仿真周期是控制周期的四分之一能够把每拍控制信号之间的响应拆成 4 个插值点。不要因为模型“看起来简单”就把步长调到 0.05 秒艏摇角速度 r 的振荡常被混叠最终得到一条平滑但完全错误的航向响应曲线。4.3 附加质量矩阵病态造成的“静步原地爆炸”只给 AUV 一个纵向推力横荡方向却突然冒出 NaN常见原因是附加质量矩阵与刚体惯性矩阵叠加后接近奇异。当总惯性矩阵某个对角线元素接近于零$M \setminus \tau$ 会算出极大加速度。先用rcond检查条件M_RB diag([12.5, 12.5, 0.35]); M_A diag([2.5, 12.0, 1.2]); M M_RB M_A; if rcond(M) 1e-8 warning(M 矩阵接近奇异检查附加质量符号或增加交叉耦合项); end如果确实存在弱惯性轴工程上会在仿真模型里给对应方向加一个小阻尼项比如1e-4 * nu(i)让加速度在初始瞬间不至于爆炸。这种做法只能用于数值稳定不能替代真实 AUV 的动力学建模。4.4 高频振荡的围堵限幅、滤波与仿真停止检测控制器输出的力矩指令有阶跃时先在执行器端口加 Saturation 饱和块上限和下限分别取推进器最大正反推力。若微分项带来的抖振无法消除用一阶低通滤波器滤掉 10 Hz 以上分量。调试期间建议在脚本里加入有限性检查防止仿真把一堆无效数据写进结果simOut sim(auv_sim, StopTime, 100); y simOut.logsout.getElement(eta).Values.Data; if ~all(isfinite(y), all) error(仿真结果包含 NaN/Inf按第 4 节流程排查); end也可以在模型中添加 Model Verification 库的 Assertion 块把状态量接到检测目标范围超出边界就中止仿真。这对批量参数寻优很有用单个发散工况会立即跳出而不是拖着整批计算跑完。5. 让 AUV 仿真动起来外部模式、3D 可视化与数据回放5.1 Simulink 外部模式把仿真和真实控制器接起来AUV 控制算法常在 Simulink 里验证后再部署到 MCU 上。Simulink 外部模式支持在模型运行期间在线修改参数并将数据实时传回上位机配合 Arduino、STM32 等硬件支持包可以做成低成本 HIL 台架。外部模式的接线方式是算法模型保留把执行器模块替换为真实电机驱动把传感器模块替换为 IMU 和深度计读数。对于只关心轨迹复现的 AUV 仿真外部模式不是必须但做半物理验证时它是从纯仿真过渡到水池试验最省力的中间层。5.2 从 Simulink 模型到 SDF/URDF进入 Gazebo 的常用路线如果要把 AUV 放进 Gazebo 做多机协同或环境交互仿真Simulink 通常不直接导出 SDF而是先导出 URDF。常见做法是在 Simscape Multibody 里搭出推进器、舱体和浮力模型用 MATLAB 的 URDF 导出功能生成中间文件再用 Gazebo 命令把 URDF 转成 SDFgz sdf -p model.urdf model.sdf转换后重点检查三处惯性张量单位是否已转为 SI浮力中心坐标有没有被当作 link 原点推进器作用力是否映射成正确的 joint 或 link 上的力。URDF 本身不包含流体阻尼标签因此仿真水下运动时需要在 SDF 的link里补充阻尼参数或者挂上 Gazebo 水下插件把第 2 章辨识出的水动力系数填进去。5.3 用 logsout 快速绘制三维轨迹和状态曲线Scope 适合边跑边看批量仿真之后的对比更适合用 logsout 数据。只要在模型中把坐标转换模块的输出信号启用日志记录仿真结束后就可以直接提取logs simOut.logsout; eta logs.getElement(eta).Values.Data; nu logs.getElement(nu).Values.Data; figure; subplot(2,1,1); plot3(eta(:,1), eta(:,2), eta(:,3), LineWidth, 1.5); xlabel(x); ylabel(y); zlabel(z); grid on; title(AUV 三维轨迹); subplot(2,1,2); t logs.getElement(eta).Values.Time; plot(t, rad2deg(eta(:,6))); xlabel(t (s)); ylabel(偏航角 (deg)); grid on;这段脚本把空间轨迹与艏摇响应放在同一张图里适合写技术报告。数据点特别多时先降采样再绘图比如eta(1:10:end,:)否则图形窗口可能卡顿。另一个常用技巧是把仿真结果存成 .mat再在 App Designer 里做一个回放面板用按钮驱动轨迹重放做组会汇报时直观很多。5.4 注入传感器噪声模拟真实水下环境闭环控制器验证到后期只给理想状态反馈不够。常见做法是在 eta 输出路径上叠加高斯白噪声再串联一个低通滤波器模拟 IMU 带宽。噪声方差可以从实际传感器数据里估计没有实测数据时先取位置噪声 0.1 m、姿态噪声 0.5° 试跑观察控制器是否仍能稳定收敛。注意噪声要加在反馈信号上不能加在积分器内部否则会污染真实状态量控制率也会失去比较基准。6. 让 AUV 仿真贴合实测数据用 lsqnonlin 反演水动力系数水池试验做完后第 2 章参数表里那组水动力系数往往经不起检验。水动力系数随速度变化且存在交叉耦合手调很难同时命中纵荡和转艏响应。更快的做法是让 MATLAB 优化工具箱基于实测数据反演核心参数。下面脚本假设模型从工作区读取M_A和D输入 tau 序列与实测工况相同function res sim_residuals(p, t_exp, eta_exp) % p 包含 [Xu_dot, Yv_dot, Nr_dot, Xu, Yv, Nr] assignin(base, M_A, diag(p(1:3))); assignin(base, D, diag(p(4:6))); simOut sim(auv_sim, StopTime, num2str(t_exp(end))); eta_sim simOut.logsout.getElement(eta).Values.Data; res [eta_sim(:,1) - eta_exp(:,1); eta_sim(:,2) - eta_exp(:,2); eta_sim(:,6) - eta_exp(:,6)]; end p0 [2.5, 12, 1.2, 1.2, 8, 1.5]; pOpt lsqnonlin((p) sim_residuals(p, t_exp, eta_exp), ... p0, [], [], ... optimset(Display, iter, MaxIter, 20));这段代码把仿真输出与实测的 x、y、偏航角之差作为残差lsqnonlin会通过迭代修改附加质量和阻尼系数来最小化残差。注意初值p0不要偏离真值太远否则优化可能落到一个数值上稳定但物理意义错误的局部解。更稳妥的做法是先固定纵向自由度只辨识横向阻尼再扩展到三自由度联动。优化完成后把pOpt写回init_auv.m重新跑一次与实测相同的工况对比轨迹和姿态曲线是否落在允许误差带内这样整条 AUV 仿真模型才算真正闭环。本文还有配套的精品资源点击获取