简介面向水下机器人研究者的AUV三维运动仿真工程包基于MATLAB/Simulink实现重点解决AUV动力学建模与三维回旋机动仿真问题。压缩包共26个文件包含17个mat数据文件、4个l库文件、2个m脚本分别对应动力学与运动学模型、1个slx仿真模型及1个fig界面图整体仅223KB目录结构清晰便于二次开发与调试。源码通过S函数构建推进系统、姿态控制及水动力模型可模拟横向、纵向及垂直方向上的回旋运动用于避障、定位、目标捕获等任务场景下的操纵性分析。已有2029人学习下载适合具备一定Simulink基础、希望深入AUV控制策略仿真与性能验证的工程师和研究生。解压后可直接运行仿真调整模型参数并观察不同控制律下的三维轨迹变化也可对照m脚本学习AUV建模思路为实际水下试验提供参考。1. 从回旋仿真切入AUV三维运动仿真到底在仿什么做过 AUV 实航试验的人都有一个共识靠航向闭环把轨迹拉直是基本功真正让人睡不着觉的是回转。回旋仿真做透了AUV 操纵性仿真才算拿到了 50 分。标题里同时出现“三维运动仿真”和“回旋仿真”说明需求方关心的不只是垂直面的潜浮与定深而是把水平面回转和三维空间运动放在同一个模型里统一考察。这个需求在工程上对应的是一套完整的六自由度建模、水动力系数整定、Simulink 仿真与数据后处理流程。这套流程的价值在于它能把实艇试验中“不敢轻易做”的大舵角机动先在数字空间里推演一遍评估回转半径、漂角、速度损失和姿态耦合再决定是否值得下水。适合谁用一是做 AUV 总体与操纵性设计的工程师二是做控制算法验证的研究人员三是对 Marine Systems Simulator 或 NSRDC 标准方程有基础、想落地成可复现模型的从业者。下面的方案是我在工程中验证过的一套做法从坐标变换讲到 Simulink 实现再落到回旋仿真的数据判据。2. 坐标系与运动学方程三维仿真先解决“坐标怎么转”2.1 AUV 本体坐标系与地面坐标系的约定做 AUV 三维运动仿真最容易在第一步就埋雷的是坐标系约定。不同文献里x 轴可以指向艇艏也可以指向右侧欧拉角的旋转顺序可能取 z-y-x也可能取 z-x-y。约定不一致仿真结果天差地别而且查错极其困难。我一般沿用 SNAME造船与轮机工程师学会的约定地面坐标系或称为惯性系取 NED 框架即 x 向北、y 向东、z 向下体坐标系固连于 AUV原点取重心x 沿艇艏方向y 指向右舷z 指向艇底。这个约定的好处是NED 坐标系与 GPS/罗经输出的北东地数据直接对应实航数据灌入仿真模型时不需要额外转换。体坐标系的线速度 u, v, w 分别对应纵向、横向、垂向速度角速度 p, q, r 分别为横摇、纵摇、偏航角速度。欧拉角方面沿用 z-y-x 的旋转顺序即先偏航角再俯仰角最后横滚角。这一顺序在航空航天和船舶领域都通行表达式也最简洁。2.2 旋转矩阵与欧拉角奇异性的处理方式从体坐标系到地面坐标系的旋转矩阵 R 在 z-y-x 约定下写为三个基本旋转矩阵的乘积。给定偏航角、俯仰角、横滚角旋转矩阵 R 的数值在 MATLAB 里可以用示例代码构造。同时地面坐标系下的位置导数与速度的换算关系是位置增量等于旋转矩阵乘以体速度向量。需要注意的是欧拉角在俯仰角接近 ±90° 时会出现万向锁奇异性导致旋转矩阵退化。2.2.1 奇异性规避仿真中采用四元数中间变量解决欧拉角奇异性的常用做法是在 Simulink 内部用四元数做姿态递推只在输出端把四元数转回欧拉角用于显示。四元数不存在奇异性且避免三角函数反复计算积分效率高。MATLAB 提供了 quatnormalize 和 quatmultiply 等工具函数但在 C MEX 级的 S-Function 里一般自己实现乘法规则。具体四元数的更新方程为 q 对时间的导数等于一个关于角速度的叉乘式这一步是姿态解算的核心。另一个容易忽略的细节是四元数积分前必须归一化否则模长漂移会随着时间的推移把姿态角越带越偏。在实际仿真中每步积分后做一次单位化即可代价很低。输出欧拉角时注意角度的范围约定我的做法是把横滚角限制在 [-π, π]偏航角限制在 [-π, π]这样示波器上不会被 2π 跳变干扰。2.3 Simulink 下的运动学实现结构在 Simulink 里组织这一块的常见做法是把它与动力学模块分离开动力学模块输出线速度 u, v, w 和角速度 p, q, r运动学模块负责把这些量转换成地面坐标系下的位置和姿态角。分离的原因是动力学方程的更新频率由系统刚性决定而运动学部分不存在刚性可以合并处理但分开后模型更清晰排查问题时能直接看中间变量。2.3.1 MATLAB Function 实现旋转矩阵以下是一段在 MATLAB Function 模块中使用的代码输入为欧拉角输出为地面系到体坐标系的旋转矩阵。代码里使用角度制输入方便与船级社规范中的舵角标示习惯对接。function R eulerToRotMat(roll, pitch, yaw) % 输入角度单位为度输出为 3x3 旋转矩阵 cr cosd(roll); sr sind(roll); cp cosd(pitch); sp sind(pitch); cy cosd(yaw); sy sind(yaw); R [cy*cp, cy*sp*sr - sy*cr, cy*sp*cr sy*sr; sy*cp, sy*sp*sr cy*cr, sy*sp*cr - cy*sr; -sp, cp*sr, cp*cr]; end该矩阵的用途是将体坐标系下的速度向量转换到地面坐标系。逻辑上矩阵的列表示体坐标轴在地面坐标系中的方向余弦使用 cosd/sind 可以避免在 MATLAB Function 中频繁做度到弧度的换算。如果后续要嵌入 C 代码生成建议直接改用 cos/sin 并以弧度传递减少一次乘除运算。3. 刚体动力学与水动力模型回旋仿真可信度的分水岭3.1 六自由度运动方程的标准形式AUV 的动力学方程在 NED 坐标系下的标准向量形式为质量矩阵乘以加速度向量与刚体科氏项之和等于外力向量。这个方程在 Fossen 的著作中有完整推导工程中一般直接采用其矩阵形式。质量矩阵包含刚体质量和转动惯量以及附加质量流体加速效应的贡献。在实际六自由度方程中横向v、垂向w、横摇p和偏航r之间存在强耦合回旋运动正是这种耦合的中心体现。更细一点说方程右侧的外力项包括水动力阻尼、静力重力和浮力、螺旋桨推力以及舵力。水动力阻尼项本身又包括线性项和非线性二次项。在线性模型中阻尼矩阵只保留与速度正比的项这在低速仿真时勉强可用但回旋运动幅度大、漂角大线性阻尼的线性适用区间很快被突破因此工程上至少用线性加二次阻尼的组合。真实流体中AUV 的粘性阻力通常按平方律建模数据表格中给出来的系数大多是标准试验条件下的结果。3.2 NSRDC 水动力系数的参数化选型工程上做 AUV 仿真常见的做法是参考 NSRDC美国海军船舶研究与发展中心的 Suboff 模型数据或者使用 REMUS 100 实艇辨识系数再按目标艇型的缩尺比或艇体主尺度做无量纲化换算。NSRDC 数据的优势是公开、完备、包含了从 -10° 到 30° 漂角范围的试验值恰好为回旋仿真提供了可信度基础。3.2.1 无量纲化规则与常用初始参数表系数无量纲化的规则为力除以 0.5ρV²L²力矩除以 0.5ρV²L³。其中ρ为海水密度V 为合速度。无量纲化后的系数才可以在不同尺度艇型之间对比。下表给出常见的初始参数取值以 REMUS 100 量级、艇体长 1.6 m、质量约 30 kg 为例实际建模请根据具体艇型整定。参数名变量符号取值单位纵向附加质量Xu_dot-2.0kg横向附加质量Yv_dot-35.5kg偏航附加转动惯量Nr_dot-0.1kg·m²横摇附加转动惯量Kp_dot-0.02kg·m²纵向线性阻尼X_u-2.5kg/s横向线性阻尼Y_v-30.0kg/s偏航线性阻尼N_r-4.0kg·m²/s纵倾线性阻尼M_q-6.0kg·m²/s垂向线性阻尼Z_w-30.0kg/s偏航二阶阻尼N_rr-8.0kg·m²需要注意上表只是启动仿真的初始猜测值。回旋运动对这组参数中的横向阻尼和偏航阻尼极其敏感而这两个参数恰恰又是辨识中误差率最高的两个量。3.2.2 具体水动力系数查表法工程做法是先按漂角 β 查表获得位置导数再对漂角做插值处理。要在 Simulink 里实现查表可以直接用 Lookup Table 模块断点设为漂角和舵角两维。如果嫌查表模块在模型里穿插太多信号线可以把整个水动力函数写成 MATLAB Function内部用 interp2 做二维插值。下表是漂角 β 在 -10°、0°、10° 三个典型工况下横向力系数 Cy 的原始数据无量纲值参考 NSRDC 试验结果漂角 β侧向力系数 Cy偏航力矩系数 Cn-10°-0.320.0120°0.000.00010°0.32-0.012要注意这些量纲为一的值在 Model 中需要乘以动压、参考面积和特征长度后才转化为实际的力和力矩。3.3 附加质量与科氏力为什么回旋仿真不能忽略这两项刚体在水下加速运动时周围流体会被带动并产生反作用力这就是附加质量效应。回旋运动中横向速度 v 和偏航角速度 r 的变化率很大附加质量项贡献的力占比甚至可能接近总的惯性力的一半。忽略附加质量的直接后果是回转半径计算值偏大、回转周期偏长仿真轨迹比实际“飘得更远”。科氏力与向心力项同样是六自由度方程的组成部分其作用为耦合纵荡与横荡以及纵荡与偏航之间的运动。在定常回转中向心项与舵力、水动力阻尼达到平衡形成稳定的回转半径因此这一项的建模错误会直接破坏整体平衡。回旋仿真的本质就是验证这一平衡是否存在以及平衡点在什么舵角、什么速度下达成。一般工程思路是将科氏项写成矩阵乘向量形式而不要手工展开成十几个标量公式检查起来更好维护。4. Simulink 建模实现从方程到可跑通的最小模型4.1 选型理由S-Function 还是基础模块拼接搭建 AUV 动力学模型有两种主流路线。一是全部用 Simulink 基础模块增益、积分器、求和、函数模块拼装优点是每个信号都能直接用 Scope 观察适合教学展示缺点是方程复杂时连线密集、改一个系数要翻多层子系统。二是把动力学方程写进 S-Function用一个模块封装整个右侧函数优点是层次清爽、参数集中、代码可移植仿真速度也更快。我自己的工程实践是采用后者理由很简单后续要改水动力系数只需要打开一个参数对话框或改一行参数结构体不用在模型图里搜索纵横交错的总线信号。4.2 一个可跑的六自由度 S-Function 框架下面这段 S-FunctionLevel-2 M S-Function实现了刚体动力学与静力的核心部分水动力系数以结构体参数传入。该代码可以直接放进 Simulink 的 MATLAB S-Function 模块中运行。function auvDynamics(block) setup(block); end function setup(block) block.NumInputPorts 2; block.NumOutputPorts 1; block.SetPreCompInpPortInfoToDynamic; block.SetPreCompOutPortInfoToDynamic; block.InputPort(1).Dimensions 6; % 速度状态 [u v w p q r] block.InputPort(1).SamplingMode sample; block.InputPort(2).Dimensions 3; % 控制输入 [δr δs n] block.InputPort(2).SamplingMode sample; block.OutputPort(1).Dimensions 6; % 状态导数 block.OutputPort(1).SamplingMode sample; block.NumDialogPrms 1; block.DialogPrmsTunable {Nontunable}; block.SampleTimes [0 0]; block.SimStateCompliance DefaultSimState; block.RegBlockMethod(Outputs, Outputs); end function Outputs(block) state block.InputPort(1).Data; ctrl block.InputPort(2).Data; p block.DialogPrms(1).Data; % 参数结构体 u state(1); v state(2); w state(3); p1 state(4); q1 state(5); r1 state(6); delta_r ctrl(1); delta_s ctrl(2); thrust_n ctrl(3); % 合速度与动压 U sqrt(u^2 v^2 w^2); dynP 0.5 * p.rho * U^2; % 静力重力和浮力简化假设浮心在重心正上方 Gravity_Force (p.W - p.B) * [0; 0; 1]; % z 方向分量 % 此处实际应根据姿态旋转矩阵计算 % 水动力阻尼线性项 二次项 X_damp p.X_u * u p.X_uu * u*abs(u); Y_damp p.Y_v * v p.Y_vv * v*abs(v); Z_damp p.Z_w * w p.Z_ww * w*abs(w); N_damp p.N_r * r1 p.N_rr * r1*abs(r1); % 舵力简化线性化模型 Y_rudder 0.5 * p.rho * p.U_ref^2 * p.A_r * p.Cy_delta * delta_r; N_rudder 0.5 * p.rho * p.U_ref^2 * p.A_r * p.Cn_delta * delta_r; % 状态导数俯仰和滚转项在这里省略实际需要完整矩阵求逆 udot (thrust_n X_damp) / (p.m - p.Xudot); vdot (Y_damp Y_rudder) / (p.m - p.Yvdot); wdot (Z_damp Gravity_Force(3)) / (p.m - p.Zwdot); rdot (N_damp N_rudder) / (p.Izz - p.Nrdot); block.OutputPort(1).Data [udot; vdot; wdot; 0; 0; rdot]; end代码逻辑说明该 S-Function 只实现了纵向、横向、垂向和偏航四个自由度的主体项横摇与纵倾自由度的完整方程需要扩展工程中不能直接省略。注释中的“需要根据姿态旋转矩阵计算”提示静力项需要由初始姿态推算实际应用时以状态向量中的四元数或欧拉角作为输入参与计算。参数结构体 p 的字段与仿真脚本保持同名便于批处理。这种写法的核心好处是把矩阵求逆放在方程中一次性计算后续修改系数不需要触碰模型图上的连线。缺点是初期搭建时矩阵求逆的正确性必须仔细验证验证方法可以单独跑一个简化工况与解析解对照。4.3 仿真参数配置定步长、求解器与外部模式回旋仿真的数值刚性整体不高但舵角阶跃输入会在一开始激励出较快的水动力瞬态因此建议采用定步长四阶龙格库塔法。算法参数配置表如下配置项推荐值说明求解器类型Fixed-step避免变步长在机动段加密时间步长导致回放困难求解器算法ode4 (RK4)稳定性好计算量适中固定步长0.01 s仿真时长 100 s 时约需要 1 万步外部模式关闭回旋仿真不需要实时调参日志记录更高效状态保存勾选完整状态便于后处理提取中间变量固定步长的另外一层考虑是后续如果要走 Simulink Coder 生成 C 代码并部署到实时环境固定步长是前提。另外参数 p 的初始化应使用 MATLAB 脚本统一赋值严禁在模型里用常量模块散落赋值否则批处理不同工况时容易漏改。5. 回旋仿真的实施与数据后处理验证模型可信度的实战技巧5.1 回旋仿真的初始条件与输入信号设计回旋仿真的标准工况设定为AUV 以恒定航速直航待运动稳定后给舵一个固定舵角阶跃。常见做法是先给直航阶段 20 秒让速度场与姿态场收敛然后在第 20 秒给方向舵一个 15° 阶跃持续 80 秒。航速建议取设计巡航速度通常为 1.5 m/s 到 2.0 m/s 之间过低的航速会因舵效不足而无法形成定常回转。直航阶段中的纵倾与横摇耦合在回旋过程中会体现出来这部分响应正是三维运动仿真的观察重点。水平面回旋期间AUV 会产生内倾并伴随纵倾变化若不记录这两个自由度就失去了三维仿真的价值。信号输入用 Signal Builder 或 Step 模块均可关键是在后处理时输入时刻必须与控制器切换时刻严格对齐否则计算回转周期会产生相位偏差。5.2 从数据中提取回转半径与漂角的准确方法回旋完成后处理的第一步是将地面坐标系下的轨迹投影到水平面然后拟合圆心。具体的 MATLAB 数据后处理代码示例如下该代码读取 Simulation Data Inspector 导出的日志文件。% 加载仿真日志数据格式为 [time, x, y, z, yaw, v, u] data load(turn_sim.mat); t data.time; x data.x; y data.y; u data.u; v data.v; % 只取定常回转段从舵角输入后 40 s 开始 idx t 60 t 100; xc x(idx); yc y(idx); % 最小二乘圆拟合 A [-2*xc, -2*yc, ones(length(xc),1)]; b -xc.^2 - yc.^2; sol A \ b; center_x sol(1); center_y sol(2); radius sqrt(center_x^2 center_y^2 - sol(3)); % 漂角计算atan2(v, u) 随时间变化取定常段平均值 beta atan2(v(idx), u(idx)); beta_deg mean(beta) * 180/pi; fprintf(回转半径: %.2f m\n, radius); fprintf(平均漂角: %.2f deg\n, beta_deg);该代码的核心逻辑是最小二乘圆拟合通过构造线性方程组求解圆心坐标与半径。漂角则用横向速度与纵向速度之比的反正切得到。值得留意的是数据段长度的选取直接影响拟合圆的精度太短的段会因初始瞬态还未完全衰减而偏小工程上应保证用于拟合的弧长至少覆盖一个完整的 360°。5.3 三维轨迹可视化的两个实用技巧三维轨迹的常用绘制方法是在 MATLAB 中使用 plot3 函数。回旋段的轨迹会呈现螺旋状下沉或上浮这取决于艇的浮力调节状态。在绘制前建议先将不同阶段的数据进行颜色区分直航段用蓝色回旋段用红色这样报告图片直观性更好。另一个技巧是将轨迹投影到三个正交平面水平面、纵剖面、横剖面上分别观察操纵耦合关系。若纵剖面内的投影显示潜深周期性振荡说明纵倾与垂向通道存在耦合可能是水动力导数的耦合参数有误。利用视频输出函数将三维图保存为动画可以直接观察模型在舵命令扰动下的鲁棒性。回旋仿真的一个高阶验证技巧是给定反向舵角观察回旋方向是否反转、轨迹是否关于原点对称。不对称现象常指向附加质量矩或科氏力的符号错误排查思路是回到第三部分的参数表中检查 Yv_dot 与 Nr_dot 的取值方向。最后用五分钟不到的时间跑完仿真后直接检查回转半径与漂角就可以判断模型是否有硬伤而省去大段时间在实艇上试错。本文还有配套的精品资源点击获取