简介面向水下无人自主航行器AUV研发与仿真学习这份MATLAB/Simulink程序包提供了完整的建模与控制系统示例尤其适合船舶与海洋工程、自动化等相关专业的研究生和工程师。资源共62个文件以C源码.c/.h、M脚本和Simulink模型.mdl为核心辅以TXT说明、PDF文档与MAT数据文件C头文件与源码便于查看底层算法实现M脚本与MDL模型则可直接打开运行TXT、PDF与MAT文件提供说明文档与数据支撑。压缩包仅441KB目录层次清晰阅读和二次开发都很方便。目前已有1750人浏览学习属于实用型参考代码库。通过研读源码可快速掌握基于Simulink搭建水下航行器仿真环境的方法并针对自身课题修改参数与控制器结构尤其适合课程设计、毕业设计或科研预研阶段使用既能学习S函数的编写方法也能由此扩展实现不同运动控制策略是一份难易适中、可直接运行学习的参考资料。1. 做水下无人自主航行器仿真卡住的往往不是控制器而是模型参数在水下无人自主航行器AUV控制算法验证里我最常遇到的情况是控制律写完了Simulink 却跑不出一个可信的深度剖面。问题通常不在控制参数而在于六自由度运动模型、推进器作用方式和仿真步长没有对齐。这套基于 MATLAB/Simulink 的 AUV 仿真程序提供了一套可以直接落地的运动模型载体附带 S 函数、M 文件和说明文档适合做定深、定航向、路径跟踪的前期验证也适合当作研究生课题的对比基线。使用者只需具备基础的 Simulink 操作能力把重心放在理解参数含义和仿真设置上而不是从零推导整机运动方程。2. 自主航行器建模vehicle.m 里的载体参数与六自由度运动学2.1 先理清 AUV 仿真常用的两组坐标系打开模型之前需要先明确 vehicle.m 中状态量的坐标定义。AUV 仿真普遍采用北东地NED坐标系作为惯性系载体坐标系固连在航行器重心附近x 轴指向艏向y 轴指向右舷z 轴指向底部。状态向量一般写成 eta [x, y, z, phi, theta, psi]ᵀ速度向量写成 nu [u, v, w, p, q, r]ᵀ其中前三项是线速度后三项是角速度。vehicle.m 参数文件中所有与浮心、重心、转动惯量有关的参数都按这个约定书写改动时要对应检查符号否则模型会出现“看起来在自动下沉”的假象。常见做法是预先定义一个结构体 vehicle字段包括长度、质量、排水体积、重心位置、浮心位置、转动惯量主对角线项、流体动力导数和推进器推力系数。这份资源里的 vehicle.m 返回的就是这样一个结构体后续所有模块都通过这个结构体读取参数。% vehicle.m 的典型结构用于后续 Simulink S-Function 读取 function veh vehicle() veh.name shark-auv; veh.g 9.81; veh.rho 1025; % 海水密度 kg/m^3 veh.m 220; % 质量 kg veh.volume 0.215; % 排水体积 m^3 veh.rg [0; 0; 0]; % 重心相对载体坐标系原点位置 veh.rb [0; 0; -0.02]; % 浮心位置通常略低于重心 veh.I diag([15 16 18]); % 转动惯量 kg*m^2 veh.Xu -70; % 纵向线性阻尼系数 veh.Yv -80; % 横向线性阻尼系数 veh.Zw -85; % 垂向线性阻尼系数 veh.Kp -8; % 横滚阻尼 veh.Mq -15; % 俯仰阻尼 veh.Nr -16; % 偏航阻尼 end这里需要说明的是阻尼系数在不少 AUV 模型里被简化成与速度线性相关真实潜艇运动模型还会加入二次阻尼项。作为 Simulink 仿真演示保留线性项便于观察控制器作用与参数敏感性S-Function 读入 vehicle 结构体后每一步用当前速度向量计算阻尼力和附加质量力输出到积分器模块。2.2 六自由度运动学与欧拉角更新的取舍运动学方程部分S-Function 的核心任务是把载体系速度转换到 NED 系位置变化率。欧拉角表示简单直观但在俯仰角接近 ±90° 时会出现万向锁。AUV 一般不会长期保持大俯仰角因此这里用欧拉角更新是合理选择如果想做全姿态机动再在后半段替换成四元数更新即可。% 从当前状态向量 x 中提取欧拉角 phi x(4); theta x(5); psi x(6); % 线速度变换矩阵 J1把载体系速度转到 NED 系 J1 [cos(psi)*cos(theta), -sin(psi)*cos(phi)cos(psi)*sin(theta)*sin(phi), ... sin(psi)*sin(phi)cos(psi)*cos(phi)*sin(theta); sin(psi)*cos(theta), cos(psi)*cos(phi)sin(phi)*sin(theta)*sin(psi), ... -cos(psi)*sin(phi)sin(theta)*sin(psi)*cos(phi); -sin(theta), cos(theta)*sin(phi), cos(theta)*cos(phi)]; deta(1:3) J1 * nu(1:3);代码中 J1 是线速度变换矩阵实际调试时不需要把整个矩阵手工写到 Simulink 模块里通常封装进 Level-2 M-file S-Function 的 Derivatives 部分。仿真过程中如果发现深度输出有高频抖动先检查这里的角度变换是否拿错了旋转顺序因为 MATLAB 的旋转正方向习惯与部分文献不一致。2.3 vehicle 参数表从实验数据到仿真输入的映射参数含义在 vehicle.m 中字段调整影响m航行器质量veh.m决定加减速响应快慢volume排水体积veh.volume影响净浮力和下潜稳性rb(3)浮心 z 向坐标veh.rb决定纵向恢复力矩大小I(3,3)偏航转动惯量veh.I影响转艏响应速度Xu纵向线性阻尼veh.Xu影响最大航速和稳态误差Nr偏航阻尼veh.Nr影响航向保持稳定性很多使用者拿到程序后第一件事是修改质量或阻尼让模型“更好控制”这样会让后续滑模控制、自适应控制的结果失去可解释性。我建议只微调浮心位置和推进器推力增益把原模型视为标称模型其他参数先不改。提示修改 vehicle.m 后一定要清空 Simulink 的缓存或者在初始化脚本里强制刷新否则 S-Function 会继续使用旧参数。3. 动力学模型与 shark.mS-Function 内部在算什么3.1 从 Level-2 S-Function 的端口结构入手当 Simulink 模型开始运行shark.m 以 Level-2 M-file S-Function 的形式被调用。每个仿真步长依次执行初始化、计算导数、输出三个阶段。初始化阶段读取 vehicle 参数把位置、姿态和速度共 12 个状态做成连续状态Derivatives 阶段计算载体坐标系下的加速度Output 阶段输出位置和姿态供控制器、Scope 和记录模块使用。这种设计把仿真内核放在一个函数文件里调试时可以直接在 MATLAB 编辑器里设置断点观察每一步中间变量比把公式全画在 Simulink 模块图里更容易排查问题。% shark_sfun.m 的框架对应资源中 shark.m 的 S-Function 封装 function shark_sfun(block) setup(block); end function setup(block) block.NumInputPorts 3; block.NumOutputPorts 1; block.InputPort(1).Dimensions 6; % 推进器控制力/力矩 block.InputPort(2).Dimensions 6; % 海流等外部扰动力/力矩 block.InputPort(3).Dimensions 1; % 控制器开关 block.OutputPort(1).Dimensions 6; % 输出的位置和姿态 block.NumContStates 12; % 位置3姿态3速度6 block.SampleTimes [0 0]; % 连续采样时间 block.RegBlockMethod(InitializeConditions, InitConditions); block.RegBlockMethod(Derivatives, Derivatives); block.RegBlockMethod(Outputs, Outputs); end上面的 setup 函数定义了端口维数和连续状态数量。输入端口 1 和 2 都是 6 维信号这在 Simulink 连线时容易混淆常见错误是把控制力接到扰动端口导致开环仿真结果异常。建议在模型里给两条信号线分别命名 tau 和 tau_dist减少接错的可能。3.2 动力学方程在 Derivatives 里的落法动力学方程采用经典的刚体加流体形式M·ν̇ C(ν)ν D(ν)ν g(η) τ。其中 M 包含刚体惯量和附加质量C(ν) 是科氏力和向心力矩阵D(ν) 是阻尼矩阵g(η) 是重力与浮力产生的恢复力。shark.m 中通常把这些子矩阵写成单独的函数方便替换和单独测试。% Derivatives 回调中的核心计算 function Derivatives(block) x block.ContStates; u block.InputPort(1).Data; veh get_param(block.BlockHandle, UserData); eta x(1:6); nu x(7:12); M inertia_matrix(veh) added_mass(veh); C coriolis_matrix(veh, nu); D damping_matrix(veh, nu); g restoring_forces(veh, eta); dnu M \ (u - C * nu - D * nu - g); deta kinematic_transform(eta) * nu; block.Derivatives [deta; dnu]; end这段代码中dnu 是载体坐标系下的加速度deta 是 NED 系下的位置变化率。注意最后一行 block.Derivatives 的拼接顺序必须与 setup 里状态向量定义一致否则输出的轨迹曲线会乱掉。实际调试时可以在这一行设置条件断点比如当 z 大于 500 时暂停快速定位是控制器发散还是推进器模型写错。3.3 mex5、mex6 文件与 Simulink C Function 的关系资源里出现 mex5、mex6 文件这是作者按 MATLAB 版本区分编译出来的 S-Function MEX 文件。MEX 文件在 MATLAB 中执行速度快于纯 M 文件适合把 S-Function 函数体编译成本地代码。如果本地 MATLAB 版本是 64 位旧版 32 位 MEX 文件往往无法直接加载需要重新编译。常见做法是先把 shark.m 按 Level-2 S-Function 规范写成模板再用 mex 命令编译 C 版本或直接使用 Simulink 内置的 C Function 模块。在较新的 MATLAB 版本中C Function 模块可以直接指定 C 源码文件并填写函数签名省去手工编译 MEX 的复杂性。对于这套 AUV 仿真资源我建议先跑 M 文件版本确认模型行为符合预期后再尝试编译 C 版本这样能把数学错误与编译问题分开排查。S-Function 类型速度适合场景主要问题Level-1 M-file慢简单演示无状态端口扩展Level-2 M-file中本资源 shark.m步长较小时性能不足C MEX快长时间仿真、硬件在环跨版本需重编译C Function 模块快新版本 Simulink需要手动管理代码文件3.4 附加质量对响应曲线的实际影响附加质量矩阵在 shark.m 中用 added_mass(veh) 函数计算一般取对角阵只保留 u、v、w、p、q、r 六个方向的主项。对于细长型 AUV纵向附加质量大约是自身质量的 5%~15%横向和垂向的附加质量更大。实际调试时可以比较开环响应曲线来判断取值是否合理如果阶跃响应上升速度明显偏离水池实验数据优先调整附加质量而不是控制增益。注意mex5 与 mex6 这类文件名没有官方标准含义通常是作者为区分 MATLAB 版本生成的。一切以 source 目录下的 .c 和 .m 源码为准。4. 运行 shark demos.m 与构建 Simulink 仿真环境4.1 先处理文件名和路径问题原资源中有一个文件叫 shark demos.m文件名里带空格。MATLAB 对带空格的脚本支持不友好直接键入文件名会被解析为命令语句。我建议先复制一份并重命名为 shark_demos.m然后放到已加载的路径下。% shark_demos.m 的推荐初始化头部 function shark_demos() % 添加 source 和 doc 子目录到路径 addpath(fullfile(pwd, source)); addpath(fullfile(pwd, doc)); % 加载 AUV 参数到 base workspace veh vehicle(); assignin(base, veh, veh); % 打开并运行主模型 open_system(Shark_AUV_Demo.slx); sim(Shark_AUV_Demo.slx); end这段代码先把 source 子目录加入 MATLAB 搜索路径再把 vehicle 结构体写入 base workspace。这样 Simulink 模型内部如果通过 eval 或 .m 脚本读取 veh 变量就能直接拿到参数。注意 assignin 写入 base workspace 是必要操作因为 S-Function 初始化阶段默认在 base workspace 查找参数如果只在函数内部定义 veh模型初始化会报“未定义变量”。4.2 Simulink 模型内部结构拆解Shark_AUV_Demo.slx 的典型信号链路是 Reference → Controller → tau → S-Function → eta → Sensor → Scope。模型里至少包含五个模块组Motion PlantS-Function 模块对应 shark.m输入控制力和扰动力输出位置姿态ControllerMATLAB Function 或另一路 S-Function计算推进器推力Reference阶跃信号或深度剖面生成器用来给定目标值Sensor测量模块把真实状态转换为带噪声的测量值Scope / To Workspace记录仿真数据保存到 tout 和 yout在模型中调整控制器参数时不要直接在条件子系统里改常量而应把 PID 系数放到 MATLAB 工作区变量里用 set_param 或模型回调统一管理。这样后期做批量参数扫描时只需要修改一个脚本不需要反复打开模型窗口。4.3 求解器、步长和模型回调参数AUV 仿真里的主要时间常数来自推进器响应和浮力恢复力矩一般比飞行器慢得多步长可以适当放大。但加入高频传感器噪声或滑模控制器后模型会变刚需要切换到隐式求解器。求解器步长设置适用阶段ode450.01~0.1模型初调、理论验证ode23t自适应加入非线性扰动后ode4固定 0.01硬件在环、实时仿真ode15s自适应模型出现刚性发散如果仿真停在某个固定时间点不前进或输出曲线出现锯齿先看是否使用了固定步长配合连续 S-Function。更稳妥的做法是在模型属性回调中使用 set_param 统一配置停止时间和求解器。% 模型回调或脚本中设置仿真参数 set_param(Shark_AUV_Demo.slx, SolverType, Fixed-step); set_param(Shark_AUV_Demo.slx, Solver, ode4); set_param(Shark_AUV_Demo.slx, FixedStep, 0.01); set_param(Shark_AUV_Demo.slx, StopTime, 120);这段命令把模型切换为固定步长 ode4每步 0.01 秒。固定步长仿真虽然精度相对自适应求解器低一些但结果可以离线回放也能直接用 Simulink 的代码生成功能导出 C 代码后续做控制器在环测试时不需要改变模型结构。5. 仿真实战定深控制、航向保持与传感器噪声注入5.1 定深控制的推进器映射AUV 定深控制的最终控制量是垂向推力推进器模型通常包含饱和限幅。可以写成 MX 函数输入深度误差和当前俯仰角输出垂向推力。function tau_z depth_controller(depth_ref, depth_now, theta, Ts) persistent err_int err_prev if isempty(err_int) err_int 0; err_prev 0; end err depth_ref - depth_now; err_int err_int err * Ts; tau_z 180 * err 40 * err_int 25 * (err - err_prev) / Ts; tau_z max(-320, min(320, tau_z)); % 推进器饱和限幅 err_prev err; end这个控制器是带饱和限幅的 PI-D 结构。比例 180 控制响应速度积分 40 消除稳态误差微分 25 抑制超调。输出限幅 320 N 对应推进器最大推力。仿真时如果把深度目标从 20 m 阶跃到 40 m观察俯仰角是否先抬头再下潜这是 AUV 定深的标准动作如果模型只平移下潜说明动力学耦合里缺少了俯仰恢复力矩。5.2 航向保持控制与解耦航向保持用偏航力矩 tau_r 控制。AUV 在定深模式下偏航控制与深度控制耦合较小可以独立调试。把航行器转向看作一阶惯性环节加积分器便可用比例控制加前馈补偿转向阻尼。function tau_r heading_controller(psi_ref, psi_now, r_now, veh) err wrapToPi(psi_ref - psi_now); tau_r 45 * err - veh.Nr * r_now; endwrapToPi 把角度误差限制在 [-pi, pi] 区间避免误差在 ±180° 附近跳变。veh.Nr * r_now 是阻尼前馈项能补偿偏航阻尼。调试这段代码时建议把参考航向从 0° 改成 90°观察是否有明显超调如果超调超过 20°在模型里把微分项加回或在控制律中加入偏航角速度反馈。5.3 传感器噪声与海流扰动注入纯仿真跑通后要在 Sensor 模块中加入噪声否则控制器在真实环境中会因测量噪声过大而失稳。可以采用零均值高斯白噪声深度噪声标准差 0.1 m航向角噪声标准差 0.5°并通过 band-limited white noise 模块实现。场景扰动形式设置方法定深深度测量噪声 0.1 mSensor 模块加离散白噪声航向航向角噪声 0.5°Angle 测量后加噪声航行海流 x 向 0.2 m/s扰动端口输入常值抗扰海流 y 向脉动使用正弦叠加海流扰动建议加到第二输入端口即 6 维扰动信号。用常值海流测试系统鲁棒性用正弦海流测试控制器的动态响应。加上扰动后再跑一百秒仿真能看出积分项是否存在饱和饱和后是否需要 anti-windup。6. 批量仿真加速与数值稳定性验证6.1 用 SimulationInput 做参数扫描定深控制器和航向控制器调好后下一步验证模型在不同阻尼系数下是否稳定。可以在脚本里用 Simulink.SimulationInput 批量设置 vehicle 参数循环调用 sim。% 对 Xu 阻尼系数做扫描 for i 1:length(xu_list) simIn(i) Simulink.SimulationInput(Shark_AUV_Demo.slx); simIn(i) simIn(i).setVariable(veh.Xu, xu_list(i)); end simOut sim(simIn, ShowProgress, off);把参数放在 SimulationInput 中而不是直接 assignin能避免循环中工作区变量被覆盖。电脑核心多时可以把 sim 换成 parsim并行执行所有扫描任务但要注意 vehicle 结构体里如果有随机噪声种子需为每次仿真设定不同随机数避免结果完全相同。仿真完成后用最小二乘拟合深度响应曲线计算超调量和调整时间并把结果汇总到表格中就能快速判断阻尼变化对控制品质的影响。6.2 数值稳定性验证策略在做下一组仿真前先跑一段 5 秒开环仿真观察速度导数和位置导数是否出现 NaN 或 Inf。出现数值发散时优先怀疑力矩矩阵 M 是否奇异。把 M 矩阵输出到 MATLAB用 cond() 检查条件数如果条件数大于 1e12说明附加质量设置不合理需要减小附加质量或改用伪逆求解。另一个常用验证方法是能量检查。把仿真结果中的动能和势能提取出来加上耗散功总能量应当随时间缓慢下降绝不应该持续增长。能量曲线持续上升大概率是阻尼矩阵符号写反维修时把对手的阻尼系数取反即可。把这些检查项写进模型的 InitFcn 回调下次改完参数重新跑一遍就能立刻看出哪个设定值在破坏仿真。本文还有配套的精品资源点击获取
基于MATLAB/Simulink的AUV六自由度运动建模与仿真实现
简介面向水下无人自主航行器AUV研发与仿真学习这份MATLAB/Simulink程序包提供了完整的建模与控制系统示例尤其适合船舶与海洋工程、自动化等相关专业的研究生和工程师。资源共62个文件以C源码.c/.h、M脚本和Simulink模型.mdl为核心辅以TXT说明、PDF文档与MAT数据文件C头文件与源码便于查看底层算法实现M脚本与MDL模型则可直接打开运行TXT、PDF与MAT文件提供说明文档与数据支撑。压缩包仅441KB目录层次清晰阅读和二次开发都很方便。目前已有1750人浏览学习属于实用型参考代码库。通过研读源码可快速掌握基于Simulink搭建水下航行器仿真环境的方法并针对自身课题修改参数与控制器结构尤其适合课程设计、毕业设计或科研预研阶段使用既能学习S函数的编写方法也能由此扩展实现不同运动控制策略是一份难易适中、可直接运行学习的参考资料。1. 做水下无人自主航行器仿真卡住的往往不是控制器而是模型参数在水下无人自主航行器AUV控制算法验证里我最常遇到的情况是控制律写完了Simulink 却跑不出一个可信的深度剖面。问题通常不在控制参数而在于六自由度运动模型、推进器作用方式和仿真步长没有对齐。这套基于 MATLAB/Simulink 的 AUV 仿真程序提供了一套可以直接落地的运动模型载体附带 S 函数、M 文件和说明文档适合做定深、定航向、路径跟踪的前期验证也适合当作研究生课题的对比基线。使用者只需具备基础的 Simulink 操作能力把重心放在理解参数含义和仿真设置上而不是从零推导整机运动方程。2. 自主航行器建模vehicle.m 里的载体参数与六自由度运动学2.1 先理清 AUV 仿真常用的两组坐标系打开模型之前需要先明确 vehicle.m 中状态量的坐标定义。AUV 仿真普遍采用北东地NED坐标系作为惯性系载体坐标系固连在航行器重心附近x 轴指向艏向y 轴指向右舷z 轴指向底部。状态向量一般写成 eta [x, y, z, phi, theta, psi]ᵀ速度向量写成 nu [u, v, w, p, q, r]ᵀ其中前三项是线速度后三项是角速度。vehicle.m 参数文件中所有与浮心、重心、转动惯量有关的参数都按这个约定书写改动时要对应检查符号否则模型会出现“看起来在自动下沉”的假象。常见做法是预先定义一个结构体 vehicle字段包括长度、质量、排水体积、重心位置、浮心位置、转动惯量主对角线项、流体动力导数和推进器推力系数。这份资源里的 vehicle.m 返回的就是这样一个结构体后续所有模块都通过这个结构体读取参数。% vehicle.m 的典型结构用于后续 Simulink S-Function 读取 function veh vehicle() veh.name shark-auv; veh.g 9.81; veh.rho 1025; % 海水密度 kg/m^3 veh.m 220; % 质量 kg veh.volume 0.215; % 排水体积 m^3 veh.rg [0; 0; 0]; % 重心相对载体坐标系原点位置 veh.rb [0; 0; -0.02]; % 浮心位置通常略低于重心 veh.I diag([15 16 18]); % 转动惯量 kg*m^2 veh.Xu -70; % 纵向线性阻尼系数 veh.Yv -80; % 横向线性阻尼系数 veh.Zw -85; % 垂向线性阻尼系数 veh.Kp -8; % 横滚阻尼 veh.Mq -15; % 俯仰阻尼 veh.Nr -16; % 偏航阻尼 end这里需要说明的是阻尼系数在不少 AUV 模型里被简化成与速度线性相关真实潜艇运动模型还会加入二次阻尼项。作为 Simulink 仿真演示保留线性项便于观察控制器作用与参数敏感性S-Function 读入 vehicle 结构体后每一步用当前速度向量计算阻尼力和附加质量力输出到积分器模块。2.2 六自由度运动学与欧拉角更新的取舍运动学方程部分S-Function 的核心任务是把载体系速度转换到 NED 系位置变化率。欧拉角表示简单直观但在俯仰角接近 ±90° 时会出现万向锁。AUV 一般不会长期保持大俯仰角因此这里用欧拉角更新是合理选择如果想做全姿态机动再在后半段替换成四元数更新即可。% 从当前状态向量 x 中提取欧拉角 phi x(4); theta x(5); psi x(6); % 线速度变换矩阵 J1把载体系速度转到 NED 系 J1 [cos(psi)*cos(theta), -sin(psi)*cos(phi)cos(psi)*sin(theta)*sin(phi), ... sin(psi)*sin(phi)cos(psi)*cos(phi)*sin(theta); sin(psi)*cos(theta), cos(psi)*cos(phi)sin(phi)*sin(theta)*sin(psi), ... -cos(psi)*sin(phi)sin(theta)*sin(psi)*cos(phi); -sin(theta), cos(theta)*sin(phi), cos(theta)*cos(phi)]; deta(1:3) J1 * nu(1:3);代码中 J1 是线速度变换矩阵实际调试时不需要把整个矩阵手工写到 Simulink 模块里通常封装进 Level-2 M-file S-Function 的 Derivatives 部分。仿真过程中如果发现深度输出有高频抖动先检查这里的角度变换是否拿错了旋转顺序因为 MATLAB 的旋转正方向习惯与部分文献不一致。2.3 vehicle 参数表从实验数据到仿真输入的映射参数含义在 vehicle.m 中字段调整影响m航行器质量veh.m决定加减速响应快慢volume排水体积veh.volume影响净浮力和下潜稳性rb(3)浮心 z 向坐标veh.rb决定纵向恢复力矩大小I(3,3)偏航转动惯量veh.I影响转艏响应速度Xu纵向线性阻尼veh.Xu影响最大航速和稳态误差Nr偏航阻尼veh.Nr影响航向保持稳定性很多使用者拿到程序后第一件事是修改质量或阻尼让模型“更好控制”这样会让后续滑模控制、自适应控制的结果失去可解释性。我建议只微调浮心位置和推进器推力增益把原模型视为标称模型其他参数先不改。提示修改 vehicle.m 后一定要清空 Simulink 的缓存或者在初始化脚本里强制刷新否则 S-Function 会继续使用旧参数。3. 动力学模型与 shark.mS-Function 内部在算什么3.1 从 Level-2 S-Function 的端口结构入手当 Simulink 模型开始运行shark.m 以 Level-2 M-file S-Function 的形式被调用。每个仿真步长依次执行初始化、计算导数、输出三个阶段。初始化阶段读取 vehicle 参数把位置、姿态和速度共 12 个状态做成连续状态Derivatives 阶段计算载体坐标系下的加速度Output 阶段输出位置和姿态供控制器、Scope 和记录模块使用。这种设计把仿真内核放在一个函数文件里调试时可以直接在 MATLAB 编辑器里设置断点观察每一步中间变量比把公式全画在 Simulink 模块图里更容易排查问题。% shark_sfun.m 的框架对应资源中 shark.m 的 S-Function 封装 function shark_sfun(block) setup(block); end function setup(block) block.NumInputPorts 3; block.NumOutputPorts 1; block.InputPort(1).Dimensions 6; % 推进器控制力/力矩 block.InputPort(2).Dimensions 6; % 海流等外部扰动力/力矩 block.InputPort(3).Dimensions 1; % 控制器开关 block.OutputPort(1).Dimensions 6; % 输出的位置和姿态 block.NumContStates 12; % 位置3姿态3速度6 block.SampleTimes [0 0]; % 连续采样时间 block.RegBlockMethod(InitializeConditions, InitConditions); block.RegBlockMethod(Derivatives, Derivatives); block.RegBlockMethod(Outputs, Outputs); end上面的 setup 函数定义了端口维数和连续状态数量。输入端口 1 和 2 都是 6 维信号这在 Simulink 连线时容易混淆常见错误是把控制力接到扰动端口导致开环仿真结果异常。建议在模型里给两条信号线分别命名 tau 和 tau_dist减少接错的可能。3.2 动力学方程在 Derivatives 里的落法动力学方程采用经典的刚体加流体形式M·ν̇ C(ν)ν D(ν)ν g(η) τ。其中 M 包含刚体惯量和附加质量C(ν) 是科氏力和向心力矩阵D(ν) 是阻尼矩阵g(η) 是重力与浮力产生的恢复力。shark.m 中通常把这些子矩阵写成单独的函数方便替换和单独测试。% Derivatives 回调中的核心计算 function Derivatives(block) x block.ContStates; u block.InputPort(1).Data; veh get_param(block.BlockHandle, UserData); eta x(1:6); nu x(7:12); M inertia_matrix(veh) added_mass(veh); C coriolis_matrix(veh, nu); D damping_matrix(veh, nu); g restoring_forces(veh, eta); dnu M \ (u - C * nu - D * nu - g); deta kinematic_transform(eta) * nu; block.Derivatives [deta; dnu]; end这段代码中dnu 是载体坐标系下的加速度deta 是 NED 系下的位置变化率。注意最后一行 block.Derivatives 的拼接顺序必须与 setup 里状态向量定义一致否则输出的轨迹曲线会乱掉。实际调试时可以在这一行设置条件断点比如当 z 大于 500 时暂停快速定位是控制器发散还是推进器模型写错。3.3 mex5、mex6 文件与 Simulink C Function 的关系资源里出现 mex5、mex6 文件这是作者按 MATLAB 版本区分编译出来的 S-Function MEX 文件。MEX 文件在 MATLAB 中执行速度快于纯 M 文件适合把 S-Function 函数体编译成本地代码。如果本地 MATLAB 版本是 64 位旧版 32 位 MEX 文件往往无法直接加载需要重新编译。常见做法是先把 shark.m 按 Level-2 S-Function 规范写成模板再用 mex 命令编译 C 版本或直接使用 Simulink 内置的 C Function 模块。在较新的 MATLAB 版本中C Function 模块可以直接指定 C 源码文件并填写函数签名省去手工编译 MEX 的复杂性。对于这套 AUV 仿真资源我建议先跑 M 文件版本确认模型行为符合预期后再尝试编译 C 版本这样能把数学错误与编译问题分开排查。S-Function 类型速度适合场景主要问题Level-1 M-file慢简单演示无状态端口扩展Level-2 M-file中本资源 shark.m步长较小时性能不足C MEX快长时间仿真、硬件在环跨版本需重编译C Function 模块快新版本 Simulink需要手动管理代码文件3.4 附加质量对响应曲线的实际影响附加质量矩阵在 shark.m 中用 added_mass(veh) 函数计算一般取对角阵只保留 u、v、w、p、q、r 六个方向的主项。对于细长型 AUV纵向附加质量大约是自身质量的 5%~15%横向和垂向的附加质量更大。实际调试时可以比较开环响应曲线来判断取值是否合理如果阶跃响应上升速度明显偏离水池实验数据优先调整附加质量而不是控制增益。注意mex5 与 mex6 这类文件名没有官方标准含义通常是作者为区分 MATLAB 版本生成的。一切以 source 目录下的 .c 和 .m 源码为准。4. 运行 shark demos.m 与构建 Simulink 仿真环境4.1 先处理文件名和路径问题原资源中有一个文件叫 shark demos.m文件名里带空格。MATLAB 对带空格的脚本支持不友好直接键入文件名会被解析为命令语句。我建议先复制一份并重命名为 shark_demos.m然后放到已加载的路径下。% shark_demos.m 的推荐初始化头部 function shark_demos() % 添加 source 和 doc 子目录到路径 addpath(fullfile(pwd, source)); addpath(fullfile(pwd, doc)); % 加载 AUV 参数到 base workspace veh vehicle(); assignin(base, veh, veh); % 打开并运行主模型 open_system(Shark_AUV_Demo.slx); sim(Shark_AUV_Demo.slx); end这段代码先把 source 子目录加入 MATLAB 搜索路径再把 vehicle 结构体写入 base workspace。这样 Simulink 模型内部如果通过 eval 或 .m 脚本读取 veh 变量就能直接拿到参数。注意 assignin 写入 base workspace 是必要操作因为 S-Function 初始化阶段默认在 base workspace 查找参数如果只在函数内部定义 veh模型初始化会报“未定义变量”。4.2 Simulink 模型内部结构拆解Shark_AUV_Demo.slx 的典型信号链路是 Reference → Controller → tau → S-Function → eta → Sensor → Scope。模型里至少包含五个模块组Motion PlantS-Function 模块对应 shark.m输入控制力和扰动力输出位置姿态ControllerMATLAB Function 或另一路 S-Function计算推进器推力Reference阶跃信号或深度剖面生成器用来给定目标值Sensor测量模块把真实状态转换为带噪声的测量值Scope / To Workspace记录仿真数据保存到 tout 和 yout在模型中调整控制器参数时不要直接在条件子系统里改常量而应把 PID 系数放到 MATLAB 工作区变量里用 set_param 或模型回调统一管理。这样后期做批量参数扫描时只需要修改一个脚本不需要反复打开模型窗口。4.3 求解器、步长和模型回调参数AUV 仿真里的主要时间常数来自推进器响应和浮力恢复力矩一般比飞行器慢得多步长可以适当放大。但加入高频传感器噪声或滑模控制器后模型会变刚需要切换到隐式求解器。求解器步长设置适用阶段ode450.01~0.1模型初调、理论验证ode23t自适应加入非线性扰动后ode4固定 0.01硬件在环、实时仿真ode15s自适应模型出现刚性发散如果仿真停在某个固定时间点不前进或输出曲线出现锯齿先看是否使用了固定步长配合连续 S-Function。更稳妥的做法是在模型属性回调中使用 set_param 统一配置停止时间和求解器。% 模型回调或脚本中设置仿真参数 set_param(Shark_AUV_Demo.slx, SolverType, Fixed-step); set_param(Shark_AUV_Demo.slx, Solver, ode4); set_param(Shark_AUV_Demo.slx, FixedStep, 0.01); set_param(Shark_AUV_Demo.slx, StopTime, 120);这段命令把模型切换为固定步长 ode4每步 0.01 秒。固定步长仿真虽然精度相对自适应求解器低一些但结果可以离线回放也能直接用 Simulink 的代码生成功能导出 C 代码后续做控制器在环测试时不需要改变模型结构。5. 仿真实战定深控制、航向保持与传感器噪声注入5.1 定深控制的推进器映射AUV 定深控制的最终控制量是垂向推力推进器模型通常包含饱和限幅。可以写成 MX 函数输入深度误差和当前俯仰角输出垂向推力。function tau_z depth_controller(depth_ref, depth_now, theta, Ts) persistent err_int err_prev if isempty(err_int) err_int 0; err_prev 0; end err depth_ref - depth_now; err_int err_int err * Ts; tau_z 180 * err 40 * err_int 25 * (err - err_prev) / Ts; tau_z max(-320, min(320, tau_z)); % 推进器饱和限幅 err_prev err; end这个控制器是带饱和限幅的 PI-D 结构。比例 180 控制响应速度积分 40 消除稳态误差微分 25 抑制超调。输出限幅 320 N 对应推进器最大推力。仿真时如果把深度目标从 20 m 阶跃到 40 m观察俯仰角是否先抬头再下潜这是 AUV 定深的标准动作如果模型只平移下潜说明动力学耦合里缺少了俯仰恢复力矩。5.2 航向保持控制与解耦航向保持用偏航力矩 tau_r 控制。AUV 在定深模式下偏航控制与深度控制耦合较小可以独立调试。把航行器转向看作一阶惯性环节加积分器便可用比例控制加前馈补偿转向阻尼。function tau_r heading_controller(psi_ref, psi_now, r_now, veh) err wrapToPi(psi_ref - psi_now); tau_r 45 * err - veh.Nr * r_now; endwrapToPi 把角度误差限制在 [-pi, pi] 区间避免误差在 ±180° 附近跳变。veh.Nr * r_now 是阻尼前馈项能补偿偏航阻尼。调试这段代码时建议把参考航向从 0° 改成 90°观察是否有明显超调如果超调超过 20°在模型里把微分项加回或在控制律中加入偏航角速度反馈。5.3 传感器噪声与海流扰动注入纯仿真跑通后要在 Sensor 模块中加入噪声否则控制器在真实环境中会因测量噪声过大而失稳。可以采用零均值高斯白噪声深度噪声标准差 0.1 m航向角噪声标准差 0.5°并通过 band-limited white noise 模块实现。场景扰动形式设置方法定深深度测量噪声 0.1 mSensor 模块加离散白噪声航向航向角噪声 0.5°Angle 测量后加噪声航行海流 x 向 0.2 m/s扰动端口输入常值抗扰海流 y 向脉动使用正弦叠加海流扰动建议加到第二输入端口即 6 维扰动信号。用常值海流测试系统鲁棒性用正弦海流测试控制器的动态响应。加上扰动后再跑一百秒仿真能看出积分项是否存在饱和饱和后是否需要 anti-windup。6. 批量仿真加速与数值稳定性验证6.1 用 SimulationInput 做参数扫描定深控制器和航向控制器调好后下一步验证模型在不同阻尼系数下是否稳定。可以在脚本里用 Simulink.SimulationInput 批量设置 vehicle 参数循环调用 sim。% 对 Xu 阻尼系数做扫描 for i 1:length(xu_list) simIn(i) Simulink.SimulationInput(Shark_AUV_Demo.slx); simIn(i) simIn(i).setVariable(veh.Xu, xu_list(i)); end simOut sim(simIn, ShowProgress, off);把参数放在 SimulationInput 中而不是直接 assignin能避免循环中工作区变量被覆盖。电脑核心多时可以把 sim 换成 parsim并行执行所有扫描任务但要注意 vehicle 结构体里如果有随机噪声种子需为每次仿真设定不同随机数避免结果完全相同。仿真完成后用最小二乘拟合深度响应曲线计算超调量和调整时间并把结果汇总到表格中就能快速判断阻尼变化对控制品质的影响。6.2 数值稳定性验证策略在做下一组仿真前先跑一段 5 秒开环仿真观察速度导数和位置导数是否出现 NaN 或 Inf。出现数值发散时优先怀疑力矩矩阵 M 是否奇异。把 M 矩阵输出到 MATLAB用 cond() 检查条件数如果条件数大于 1e12说明附加质量设置不合理需要减小附加质量或改用伪逆求解。另一个常用验证方法是能量检查。把仿真结果中的动能和势能提取出来加上耗散功总能量应当随时间缓慢下降绝不应该持续增长。能量曲线持续上升大概率是阻尼矩阵符号写反维修时把对手的阻尼系数取反即可。把这些检查项写进模型的 InitFcn 回调下次改完参数重新跑一遍就能立刻看出哪个设定值在破坏仿真。本文还有配套的精品资源点击获取
