简介资源围绕自主水下航行器AUV的建模与Matlab仿真展开面向计算机、电子信息工程等专业学生用于课程设计、期末大作业和毕业设计也适合刚开始接触Matlab仿真的读者快速上手。压缩包共二十三个文件大小约一百五十KB包含Simulink模型文件slx、工程文件prj、Matlab脚本m、说明文档md及十九个XML配置结构紧凑能从模型搭建覆盖到仿真运行的完整流程。内含可直接运行的Matlab程序与案例数据支持matlab2014、2019a、2021a版本代码采用参数化编程参数便于修改思路清晰且注释详细便于理解与二次开发。已有147人学习下载作者为资深算法工程师在Matlab算法仿真领域有十余年经验内容在智能优化、神经网络预测、信号处理等方向有一定实践参考价值可帮助读者掌握AUV建模与仿真方法。1. 自主水下航行器建模与matlab仿真先解决“模型从哪来”的问题拿到“自主水下航行器的建模和matlab仿真”这类 zip 包时最容易卡住的往往不是 Simulink 连线而是运动方程里那几十个水动力系数到底从哪来、正负号对不对、加在哪一项里。自主水下航行器AUV的建模与 matlab 仿真核心是把牛顿-欧拉运动方程、附加质量、浮力恢复力矩和海流扰动用一组可积分的状态方程表达出来之后控制算法才有地方闭环。适合两类人一类做 AUV 总体与控制设计需要在水池实验前验证深度、航向控制逻辑另一类做课程设计或数学建模需要把报告里“单自由度鱼雷模型”升级成六自由度可展示的仿真结果。下文按“建方程、写函数、接 Simulink、闭环调参”的顺序走所有代码都带参数说明。2. 建坐标系与六自由度运动方程从牛顿欧拉到水动力系数表AUV 建模的第一步不是打开 MATLAB而是定义坐标系。常见做法是同时使用惯性系与随体系位置和姿态在惯性系north-east-down, NED中描述线速度和角速度在随体系body frame中描述。这样选有两个直接原因刚体在随体系中的惯性矩是常量而推进器推力、舵力和水动力也天然按船体轴分解比在惯性系里表达省去大量时变项。2.1 两个坐标系与欧拉角变换设惯性系下位置向量为 (\eta_1[x,y,z]^T)姿态角为 (\eta_2[\phi,\theta,\psi]^T)随体系下速度为 (\nu_1[u,v,w]^T)角速度为 (\nu_2[p,q,r]^T)。运动学关系写成[ \dot{\eta}J(\eta)\nu ]其中 (J(\eta)) 是 6×6 的块对角矩阵左上角是欧拉角旋转矩阵右下角是角速度传递矩阵。实际编程时不需要写出完整的 6×6 矩阵可以拆成两步先用 z-y-x 顺序的旋转矩阵把 ([u,v,w]) 投影到惯性系再用下面的传递矩阵把 ([p,q,r]) 映射到欧拉角速率[ \begin{bmatrix}\dot{\phi}\\dot{\theta}\\dot{\psi}\end{bmatrix}\begin{bmatrix} 1 \sin\phi\tan\theta \cos\phi\tan\theta\ 0 \cos\phi -\sin\phi\ 0 \sin\phi/\cos\theta \cos\phi/\cos\theta \end{bmatrix} \begin{bmatrix}p\q\r\end{bmatrix} ]这个矩阵在俯仰角接近 ±90° 时会出现奇异所以六自由度欧拉角模型一般只适用于俯仰角在 ±80° 以内的航行工况。如果你的 AUV 需要做大俯仰角机动就要把姿态部分换成四元数代价是状态维数增加、显示不直观。2.2 六自由度运动方程的矩阵形式AUV 动力学方程的习惯写法是[ (M_{RB}M_A)\dot{\nu}C(\nu)\nuD(\nu_r)\nu_rg(\eta)\tau_{env}\tau_{ctrl} ]其中 (M_{RB}) 是刚体质量矩阵(M_A) 是附加质量矩阵(C(\nu)) 包含刚体科氏力与向心力(D(\nu_r)) 是阻尼矩阵(g(\eta)) 是重力与浮力产生的恢复力/恢复力矩(\tau_{env}) 是海流等环境力(\tau_{ctrl}) 是推进器与舵产生的控制力。(\nu_r) 是 AUV 相对海流的速度如果忽略海流则 (\nu_r\nu)。工程中有一个经常被忽略的简化当 AUV 巡航速度低于 2 m/s 时(C(\nu)) 中的向心力项通常比阻尼项小一个数量级可以并入阻尼建模误差里不单独列项。多数课程级仿真也采用这个近似不会对深度控制结论产生方向性影响但论文里必须写明“忽略科氏力/向心力项”这个假设。恢复力项 (g(\eta)) 很容易写错。它的形式是[ g(\eta) \begin{bmatrix} (W-B)\sin\theta\ -(W-B)\cos\theta\sin\phi\ -(W-B)\cos\theta\cos\phi\ -(y_g W-y_b B)\cos\theta\cos\phi(z_g W-z_b B)\cos\theta\sin\phi\ (z_g W-z_b B)\sin\theta-(x_g W-x_b B)\cos\theta\cos\phi\ (x_g W-x_b B)\cos\theta\sin\phi-(y_g W-y_b B)\sin\theta \end{bmatrix} ]其中 (Wmg)(B\rho g \nabla)。多数小型 AUV 设计成微正浮力或零浮力(W\approx B)此时前三个力项接近零而重浮力力偶产生的恢复力矩是横摇和纵摇自稳定的关键。人为把浮心放在重心正上方会得到正反馈力矩仿真曲线会直接发散。2.3 水动力系数怎么取没有风洞时用细长体近似水动力系数是最容易让新人放弃建模的一道坎。没有循环水槽和拖曳实验时常见做法是用细长体理论slender-body theory加经验修正。基本思想是把 AUV 近似为回转椭球或圆柱头尾锥的组合体用切片法估算每个自由度上的附加质量和阻尼。附加质量系数沿主轴方向最小横截面方向最大。一个总长 1.8 m、直径 0.25 m、质量约 42 kg 的典型小型 AUV合理的系数量级如下表符号含义典型值含符号单位(X_{\dot{u}})纵向附加质量-1.8kg(Y_{\dot{v}})横向附加质量-42kg(Z_{\dot{w}})垂向附加质量-42kg(N_{\dot{r}})偏航附加惯性矩-2.5kg·m²(X_{uu})纵向二次阻尼(Y_{vv})横向二次阻尼(N_{rr})偏航二次阻尼(x_g)重心距随体系原点0.02m(z_b-z_g)浮心相对重心高度0.015m这里需要注意附加质量的符号习惯。水动力学的定义是 (M_A-diag{X_{\dot{u}},Y_{\dot{v}},...})而 (X_{\dot{u}}) 本身是负值因此总质量阵中纵向项为 (m-X_{\dot{u}})最终大于 (m)。初学时常把负号重复计算导致仿真里 AUV 一推就高速振荡。没有实验数据时横向附加质量可以先按 (Y_{\dot{v}} \approx -0.8m) 到 (-1.0m) 的范围估纵向取 (X_{\dot{u}} \approx -0.03m) 到 (-0.05m)。这比随便填一个对角矩阵要可信得多也方便后续用系统辨识结果替换。3. 用MATLAB函数把AUV模型封装成可复用的状态方程从实现角度看六自由度模型最终就是一个函数输入当前状态 (x)、控制力矩 (\tau)、结构体参数par输出状态导数 (\dot{x})。写成函数后有两条使用路径在 MATLAB 脚本里用ode45做开环验证或者把函数放到 Simulink 的 MATLAB Function 块里做闭环仿真。3.1 12维状态向量与参数结构体状态向量按下述顺序排列% x(1:3): 惯性系位置 [x; y; z] % x(4:6): 欧拉角 [phi; theta; psi] % x(7:9): 随体系线速度 [u; v; w] % x(10:12): 随体系角速度 [p; q; r]参数统一放进结构体好处是函数签名不变只需要替换par就能切换不同 AUV 型号。初始化代码par.m 42.0; % 质量, kg par.rho 1025; % 海水密度, kg/m^3 par.Vol 0.042; % 排水体积, m^3 par.g 9.81; par.Iy 0.85; % 纵摇转动惯量, kg*m^2 par.Iz 0.95; % 偏航转动惯量 par.xg 0.02; par.yg 0; par.zg 0.005; par.xb 0.02; par.yb 0; par.zb -0.01; % 附加质量按 m - X_udot 形式组合 par.Xudot -1.8; par.Yvdot -42.0; par.Zwdot -42.0; par.Kpdot -0.05; % 横摇附加惯性矩通常很小 par.Mqdot -2.5; par.Nrdot -2.5; % 线性二次阻尼 par.Xu -2.0; par.Xuu -8.5; par.Yv -15.0; par.Yvv -120.0; par.Zw -15.0; par.Zww -120.0; par.Kp -0.1; par.Kpp -0.5; par.Mq -3.0; par.Mqq -8.0; par.Nr -3.0; par.Nrr -10.0;这组参数代表一艘排水量 42 L、艇体较细长的试验型 AUV。横向阻尼远大于纵向阻尼符合细长体特征横摇附加惯性矩很小因为旋转轴接近艇体轴线。3.2 编写auv_dynamics.m从力与力矩到状态导数核心函数如下。这里省略科氏力项并用对角矩阵近似质量阵和阻尼阵适合速度低于 2 m/s 的常规巡航仿真function xdot auv_dynamics(t, x, tau, par) % t: 时间, x: 12维状态, tau: 随体系控制力/力矩(6x1) % 输出xdot为12维状态导数 eta x(1:6); nu x(7:12); phi eta(4); theta eta(5); psi eta(6); % 1) 旋转矩阵R(3x3): 随体系 - 惯性系 cphi cos(phi); sphi sin(phi); cth cos(theta); sth sin(theta); cps cos(psi); sps sin(psi); R [cps*cth, cps*sth*sphi-sps*cphi, cps*sth*cphisps*sphi; sps*cth, sps*sth*sphicps*cphi, sps*sth*cphi-cps*sphi; -sth, cth*sphi, cth*cphi]; % 2) 角速度传递矩阵T(3x3) T [1, sphi*sth/cth, cphi*sth/cth; 0, cphi, -sphi; 0, sphi/cth, cphi/cth]; % 3) 质量矩阵(对角近似) M diag([par.m-par.Xudot, par.m-par.Yvdot, par.m-par.Zwdot, ... par.Ix-par.Kpdot, par.Iy-par.Mqdot, par.Iz-par.Nrdot]); % 4) 阻尼力(二次项取绝对值乘速度, 保证耗散方向正确) u nu(1); v nu(2); w nu(3); p nu(4); q nu(5); r nu(6); D_nu [par.Xu*u par.Xuu*abs(u)*u; par.Yv*v par.Yvv*abs(v)*v; par.Zw*w par.Zww*abs(w)*w; par.Kp*p par.Kpp*abs(p)*p; par.Mq*q par.Mqq*abs(q)*q; par.Nr*r par.Nrr*abs(r)*r]; % 5) 重浮力恢复力/力矩 W par.m * par.g; B par.rho * par.Vol * par.g; xG par.xg; yG par.yg; zG par.zg; xB par.xb; yB par.yb; zB par.zb; g_eta zeros(6,1); g_eta(1) (W - B) * sin(theta); g_eta(2) -(W - B) * cos(theta) * sin(phi); g_eta(3) -(W - B) * cos(theta) * cos(phi); g_eta(4) (yG*W - yB*B)*cos(theta)*cos(phi) - (zG*W - zB*B)*cos(theta)*sin(phi); g_eta(5) -(zG*W - zB*B)*sin(theta) - (xG*W - xB*B)*cos(theta)*cos(phi); g_eta(6) (xG*W - xB*B)*cos(theta)*sin(phi) (yG*W - yB*B)*sin(theta); % 6) 合并: M * nu_dot tau - D_nu - g_eta nu_dot M \ (tau - D_nu - g_eta); % 7) 运动学 eta_dot [R * nu(1:3); T * nu(4:6)]; xdot [eta_dot; nu_dot]; end逻辑上这段代码分成三个层次。前两步是运动学坐标变换只依赖当前姿态第三到五步计算动力学广义力其中阻尼和恢复力都基于当前速度与姿态第六步用质量阵的逆求解加速度。注意M \而不是inv(M)乘原因是逆矩阵求解在数值上更稳定尤其当附加质量项接近零时不会放大舍入误差。如果你把科氏力项加回来需要在tau - D_nu - g_eta前面插入-C_mat * nu。科氏力矩阵的速度耦合项很多最简单的方式是用mss_vehutils之类工具箱里的m2c(MRB, nu)自动生成。手动写容易漏掉交叉项反而破坏 M 矩阵与 C 矩阵的匹配关系。3.3 Simulink接入MATLAB Function块与代数环处理在 Simulink 里接入上述模型最直接的方法是用 MATLAB Function 块。新建模型后拖入一个 MATLAB Function 块双击打开编辑器把auv_dynamics的函数体完整粘贴进去函数签名保持不变。输入端连一个 6 维控制力矩向量输出端是 12 维xdot后面接一组积分器Integrator得到x再通过 Demux 分别引出位置、姿态、速度供控制器和示波器使用。这里有一个常见坑如果你在同一时刻既从x提取速度反馈给控制器又把控制器输出tau送回动力学块Simulink 可能报代数环Algebraic Loop错误。原因是控制力矩直接依赖x而xdot又依赖tau逻辑上形成瞬时闭环。解决办法有两个。第一个是在动力学块输出后接一个 Memory 块做一步延迟打破代数环第二个是给控制器反馈路径加单位延迟Unit Delay让控制量总是基于上一拍的测量值这更接近真实嵌入式控制器的执行时序。建议在模型里加一个常量sample_time 0.1所有控制器和延迟模块都以这个步长运行而动力学积分器继续使用连续时间。这样能模拟离散控制周期也方便后续直接生成 C 代码部署到单片机。编译时如果出现仿真发散先看这一步采样时间是否设置成了inf或与固定步长不匹配。4. Simulink仿真平台搭建海流环境、推力分配与PID闭环有了动力学核心里函数下一步是搭一个可复用的 Simulink 仿真平台。完整闭环至少需要四个子系统环境干扰模块、控制器、推力分配/执行器模块、AUV六自由度模型。四个模块按“外环指令→控制器→推力图→动力学→状态反馈”的顺序连接缺一不可。4.1 仿真总框图与模块划分推荐把模型分成四层方便不同成员并行开发子系统输入输出主要内容环境模块仿真时间、AUV位置/速度海流速度向量、波浪干扰力定常流、剪切流、白噪声扰动控制器指令值、状态反馈期望力/力矩PID、LQR、滑模等控制律推力分配控制力/力矩各推进器转速、舵角分配矩阵、饱和限制、变化率限制AUV模型推进器力、舵力12维状态上文auv_dynamics封装Simulink 里建议新建一个AUV Model子系统内部只放 MATLAB Function 块和 Integrator不给外部留修改动力学方程的口子。这样后续换控制律时不需要动模型内部结构。子系统输入是 6 维tau输出是 12 维状态再在子系统外部用 Demux 拆出位置、姿态、速度。求解器选择上开环快速验证用ode45闭环定步长仿真用ode4固定步长 0.01 s。定步长的原因是后面的 PID 参数扫描与嵌入式代码生成都要求确定性的执行时序而ode45的变步长会让每次仿真的有效采样周期不同做批量扫描时结果可比性差。4.2 海流建模定常流与随机流怎么叠加到相对速度最简单但实用的海流模型是定常均匀流设定惯性系下的流速向量 (v_n[V_c^x,V_c^y,0]^T)把它转换到随体系后用相对速度计算阻尼力。因为海流速度远低于 AUV 航行速度时附加质量项受海流加速度的影响可以忽略只修正阻尼项即可。时变海流可以用一阶马尔可夫过程生成延续性比白噪声好。离散形式写成[ V_c(k1)e^{-\Delta t/T}V_c(k)\sqrt{1-e^{-2\Delta t/T}},\sigma_w \varepsilon ]其中 (T) 是海流相关时间常数(\sigma_w) 是流速标准差(\varepsilon) 是标准正态随机数。在 Simulink 中实现时用离散状态空间模块或 MATLAB Function 块存状态变量。把海流接到动力学模型中的代码片段function xdot auv_model_with_current(t, x, tau, par, Vn) % Vn: 惯性系恒定流速 [Vx; Vy; 0] eta x(1:6); nu x(7:12); phi eta(4); theta eta(5); psi eta(6); % 旋转矩阵与auv_dynamics中相同 % 计算随体系下的相对速度 R euler_to_R(phi, theta, psi); vc_body R * Vn; % 惯性系流速转到随体系 nu_rel nu(1:3) - vc_body; % 相对速度用于水动力计算 % 后续阻尼项用nu_rel计算, 舍去相对加速度耦合 end这里容易犯的错误是直接在随体系里加减绝对流速。海流上通常定义在惯性系必须先乘旋转矩阵的转置。另一个常见问题是把海流速度也加到角速度上但平流海流不产生涡量对 AUV 角速度不产生直接作用这一项保持为零就行。4.3 执行器饱和与推力分配小型 AUV 的常见配置是尾部两个水平推进器、中部一个垂直推进器。水平面内通过左右推进器的差速产生偏航力矩不再单独设置垂直舵垂直面内靠垂直推进器或俯仰舵控制深度。这种配置的推力分配矩阵写出来是% tau_cmd(6x1) 来自控制器 % th(4x1) 为推进器命令: [左水平; 右水平; 垂直; 俯仰舵] B_thrust [1 1 0 0; 0 0 1 0; 0 0 0 0; 0 0 0 0; 0 0 0 1; -L L 0 0]; % L为水平推进器到中线的距离 th pinv(B_thrust) * tau_cmd; % 最小范数分配 th min(max(th, -thrust_max), thrust_max); % 幅值饱和分配矩阵直接用伪逆pinv处理因为多数工况下控制力维度大于执行器数量伪逆给出最小能量解。仿真里如果出现推进器输出高频抖动还需要对th加变化率限制否则真实推进器的响应时间跟不上控制器输出仿真结论会偏乐观。用 Simulink 的 Rate Limiter 模块实现变化率限制上限设为推进器最快转速变化率例如 200 rpm/s。4.4 PID参数整定深度环与航向环的分层设计深度控制和航向控制不建议直接对推力做单环 PID。深度与俯仰角强耦合正确做法是外环深度 PID 生成俯仰角指令内环俯仰角 PID 生成俯仰力矩航向控制类似外环航向 PID 生成偏航角速度指令内环偏航角速度 PID 生成偏航力矩。% 深度外环: 深度误差 - 期望俯仰角 e_z z_ref - z; theta_ref saturate(e_z * Kp_z integral_e_z * Ki_z, [-0.4 0.4]); % 俯仰内环: 俯仰角误差 - 升降舵/垂推 e_theta theta_ref - theta; tau_theta Kp_theta * e_theta Kd_theta * (-q); % 航向外环: 航向误差 - 期望偏航角速度 e_psi wrapToPi(psi_ref - psi); r_ref Kp_psi * e_psi; % 偏航内环: 角速度误差 - 差速力矩 e_r r_ref - r; tau_psi Kp_r * e_r;wrapToPi这一步很关键不做角度归一化时航向从 170° 转到的 -170° 会产生 340° 的伪误差控制器会错误地反向打舵。深度环的积分项建议加抗饱和限制因为垂直推进器推力有限积分饱和会让 AUV 在到达目标深度后仍然保持一个偏置舵角。一组能直接跑通初仿真的 PID 参数参考如下回路KpKiKd说明深度外环0.80.020输出限制 ±0.4 rad俯仰内环3.001.2微分项取-q而非θ的差分航向外环1.500输出为偏航角速度指令偏航内环2.50.10积分用于消除稳态静差调参顺序固定为先内环后外环。把深度外环断开给俯仰阶跃指令调内环直到超调小于 15%再接上深度外环调 Kp_z 和 Ki_z。如果深度曲线出现等幅振荡优先减小外环 Kp而不是调内环。5. 仿真结果验证与批量参数扫描模型建完后第一件该做的事情是验证符号方向是否正确而不是直接调 PID。方向错误在视觉上表现为“给正纵倾力矩AUV 低头”这在某些数据格式混乱的模型里非常容易出现。5.1 用自由衰减曲线验证模型符号方向写一个零控制输入的开环脚本x0 zeros(12,1); x0(7) 0.8; % 初始纵向速度 0.8 m/s tau zeros(6,1); % 无控制输入 [t, X] ode45((t,x) auv_dynamics(t,x,tau,par), [0 100], x0); plot(t, X(:,7)); xlabel(time (s)); ylabel(u (m/s));正常结果应该是纵向速度从 0.8 m/s 呈指数衰减趋势缓慢下降不会越过零轴变成负值如果有俯仰角初值AUV 会以阻尼振荡方式回到水平姿态。如果曲线发散、或初始横摇角对应错误方向的恢复力矩优先检查 (W-B) 和浮心位置的正负号。常见错误是重浮心坐标用惯性系下的值代替随体系下的值导致恢复力矩方向反了。5.2 用parsim并行扫描PID参数单次闭环仿真很快但 PID 参数空间扫描次数一多串行跑会浪费大量等待时间。MATLAB 的parsim可以在并行池里批量运行 Simulink 模型前提是把每次仿真的参数写入SimulationInput对象KpSet linspace(0.4, 1.2, 8); KdSet linspace(0.2, 1.5, 8); [KP, KD] ndgrid(KpSet, KdSet); simIn(1:numel(KP)) Simulink.SimulationInput(auv_depth_control); for i 1:numel(KP) simIn(i) simIn(i).setVariable(Kp_z, KP(i)); simIn(i) simIn(i).setVariable(Kd_theta, KD(i)); end out parsim(simIn, ShowProgress, on);扫描完成后用循环从每个out(i)里取出深度曲线计算上升时间、超调量和稳态误差再画成参数热力图。这里一个实用技巧是给SimulationInput的setVariable指定变量作用域为数据字典或模型工作区避免修改基础工作区变量影响并行池的随机数流。批量仿真时把StopTime设为固定值并用离散求解器否则不同参数的仿真步数差异会影响对比公平性。若机器是多核 CPUparsim默认按核数分配任务比手写parfor加sim()的写法更简洁出错时还会保留下标对应的完整日志定位崩溃参数组很快。本文还有配套的精品资源点击获取
AUV六自由度建模与MATLAB仿真：从水动力系数到PID闭环
简介资源围绕自主水下航行器AUV的建模与Matlab仿真展开面向计算机、电子信息工程等专业学生用于课程设计、期末大作业和毕业设计也适合刚开始接触Matlab仿真的读者快速上手。压缩包共二十三个文件大小约一百五十KB包含Simulink模型文件slx、工程文件prj、Matlab脚本m、说明文档md及十九个XML配置结构紧凑能从模型搭建覆盖到仿真运行的完整流程。内含可直接运行的Matlab程序与案例数据支持matlab2014、2019a、2021a版本代码采用参数化编程参数便于修改思路清晰且注释详细便于理解与二次开发。已有147人学习下载作者为资深算法工程师在Matlab算法仿真领域有十余年经验内容在智能优化、神经网络预测、信号处理等方向有一定实践参考价值可帮助读者掌握AUV建模与仿真方法。1. 自主水下航行器建模与matlab仿真先解决“模型从哪来”的问题拿到“自主水下航行器的建模和matlab仿真”这类 zip 包时最容易卡住的往往不是 Simulink 连线而是运动方程里那几十个水动力系数到底从哪来、正负号对不对、加在哪一项里。自主水下航行器AUV的建模与 matlab 仿真核心是把牛顿-欧拉运动方程、附加质量、浮力恢复力矩和海流扰动用一组可积分的状态方程表达出来之后控制算法才有地方闭环。适合两类人一类做 AUV 总体与控制设计需要在水池实验前验证深度、航向控制逻辑另一类做课程设计或数学建模需要把报告里“单自由度鱼雷模型”升级成六自由度可展示的仿真结果。下文按“建方程、写函数、接 Simulink、闭环调参”的顺序走所有代码都带参数说明。2. 建坐标系与六自由度运动方程从牛顿欧拉到水动力系数表AUV 建模的第一步不是打开 MATLAB而是定义坐标系。常见做法是同时使用惯性系与随体系位置和姿态在惯性系north-east-down, NED中描述线速度和角速度在随体系body frame中描述。这样选有两个直接原因刚体在随体系中的惯性矩是常量而推进器推力、舵力和水动力也天然按船体轴分解比在惯性系里表达省去大量时变项。2.1 两个坐标系与欧拉角变换设惯性系下位置向量为 (\eta_1[x,y,z]^T)姿态角为 (\eta_2[\phi,\theta,\psi]^T)随体系下速度为 (\nu_1[u,v,w]^T)角速度为 (\nu_2[p,q,r]^T)。运动学关系写成[ \dot{\eta}J(\eta)\nu ]其中 (J(\eta)) 是 6×6 的块对角矩阵左上角是欧拉角旋转矩阵右下角是角速度传递矩阵。实际编程时不需要写出完整的 6×6 矩阵可以拆成两步先用 z-y-x 顺序的旋转矩阵把 ([u,v,w]) 投影到惯性系再用下面的传递矩阵把 ([p,q,r]) 映射到欧拉角速率[ \begin{bmatrix}\dot{\phi}\\dot{\theta}\\dot{\psi}\end{bmatrix}\begin{bmatrix} 1 \sin\phi\tan\theta \cos\phi\tan\theta\ 0 \cos\phi -\sin\phi\ 0 \sin\phi/\cos\theta \cos\phi/\cos\theta \end{bmatrix} \begin{bmatrix}p\q\r\end{bmatrix} ]这个矩阵在俯仰角接近 ±90° 时会出现奇异所以六自由度欧拉角模型一般只适用于俯仰角在 ±80° 以内的航行工况。如果你的 AUV 需要做大俯仰角机动就要把姿态部分换成四元数代价是状态维数增加、显示不直观。2.2 六自由度运动方程的矩阵形式AUV 动力学方程的习惯写法是[ (M_{RB}M_A)\dot{\nu}C(\nu)\nuD(\nu_r)\nu_rg(\eta)\tau_{env}\tau_{ctrl} ]其中 (M_{RB}) 是刚体质量矩阵(M_A) 是附加质量矩阵(C(\nu)) 包含刚体科氏力与向心力(D(\nu_r)) 是阻尼矩阵(g(\eta)) 是重力与浮力产生的恢复力/恢复力矩(\tau_{env}) 是海流等环境力(\tau_{ctrl}) 是推进器与舵产生的控制力。(\nu_r) 是 AUV 相对海流的速度如果忽略海流则 (\nu_r\nu)。工程中有一个经常被忽略的简化当 AUV 巡航速度低于 2 m/s 时(C(\nu)) 中的向心力项通常比阻尼项小一个数量级可以并入阻尼建模误差里不单独列项。多数课程级仿真也采用这个近似不会对深度控制结论产生方向性影响但论文里必须写明“忽略科氏力/向心力项”这个假设。恢复力项 (g(\eta)) 很容易写错。它的形式是[ g(\eta) \begin{bmatrix} (W-B)\sin\theta\ -(W-B)\cos\theta\sin\phi\ -(W-B)\cos\theta\cos\phi\ -(y_g W-y_b B)\cos\theta\cos\phi(z_g W-z_b B)\cos\theta\sin\phi\ (z_g W-z_b B)\sin\theta-(x_g W-x_b B)\cos\theta\cos\phi\ (x_g W-x_b B)\cos\theta\sin\phi-(y_g W-y_b B)\sin\theta \end{bmatrix} ]其中 (Wmg)(B\rho g \nabla)。多数小型 AUV 设计成微正浮力或零浮力(W\approx B)此时前三个力项接近零而重浮力力偶产生的恢复力矩是横摇和纵摇自稳定的关键。人为把浮心放在重心正上方会得到正反馈力矩仿真曲线会直接发散。2.3 水动力系数怎么取没有风洞时用细长体近似水动力系数是最容易让新人放弃建模的一道坎。没有循环水槽和拖曳实验时常见做法是用细长体理论slender-body theory加经验修正。基本思想是把 AUV 近似为回转椭球或圆柱头尾锥的组合体用切片法估算每个自由度上的附加质量和阻尼。附加质量系数沿主轴方向最小横截面方向最大。一个总长 1.8 m、直径 0.25 m、质量约 42 kg 的典型小型 AUV合理的系数量级如下表符号含义典型值含符号单位(X_{\dot{u}})纵向附加质量-1.8kg(Y_{\dot{v}})横向附加质量-42kg(Z_{\dot{w}})垂向附加质量-42kg(N_{\dot{r}})偏航附加惯性矩-2.5kg·m²(X_{uu})纵向二次阻尼(Y_{vv})横向二次阻尼(N_{rr})偏航二次阻尼(x_g)重心距随体系原点0.02m(z_b-z_g)浮心相对重心高度0.015m这里需要注意附加质量的符号习惯。水动力学的定义是 (M_A-diag{X_{\dot{u}},Y_{\dot{v}},...})而 (X_{\dot{u}}) 本身是负值因此总质量阵中纵向项为 (m-X_{\dot{u}})最终大于 (m)。初学时常把负号重复计算导致仿真里 AUV 一推就高速振荡。没有实验数据时横向附加质量可以先按 (Y_{\dot{v}} \approx -0.8m) 到 (-1.0m) 的范围估纵向取 (X_{\dot{u}} \approx -0.03m) 到 (-0.05m)。这比随便填一个对角矩阵要可信得多也方便后续用系统辨识结果替换。3. 用MATLAB函数把AUV模型封装成可复用的状态方程从实现角度看六自由度模型最终就是一个函数输入当前状态 (x)、控制力矩 (\tau)、结构体参数par输出状态导数 (\dot{x})。写成函数后有两条使用路径在 MATLAB 脚本里用ode45做开环验证或者把函数放到 Simulink 的 MATLAB Function 块里做闭环仿真。3.1 12维状态向量与参数结构体状态向量按下述顺序排列% x(1:3): 惯性系位置 [x; y; z] % x(4:6): 欧拉角 [phi; theta; psi] % x(7:9): 随体系线速度 [u; v; w] % x(10:12): 随体系角速度 [p; q; r]参数统一放进结构体好处是函数签名不变只需要替换par就能切换不同 AUV 型号。初始化代码par.m 42.0; % 质量, kg par.rho 1025; % 海水密度, kg/m^3 par.Vol 0.042; % 排水体积, m^3 par.g 9.81; par.Iy 0.85; % 纵摇转动惯量, kg*m^2 par.Iz 0.95; % 偏航转动惯量 par.xg 0.02; par.yg 0; par.zg 0.005; par.xb 0.02; par.yb 0; par.zb -0.01; % 附加质量按 m - X_udot 形式组合 par.Xudot -1.8; par.Yvdot -42.0; par.Zwdot -42.0; par.Kpdot -0.05; % 横摇附加惯性矩通常很小 par.Mqdot -2.5; par.Nrdot -2.5; % 线性二次阻尼 par.Xu -2.0; par.Xuu -8.5; par.Yv -15.0; par.Yvv -120.0; par.Zw -15.0; par.Zww -120.0; par.Kp -0.1; par.Kpp -0.5; par.Mq -3.0; par.Mqq -8.0; par.Nr -3.0; par.Nrr -10.0;这组参数代表一艘排水量 42 L、艇体较细长的试验型 AUV。横向阻尼远大于纵向阻尼符合细长体特征横摇附加惯性矩很小因为旋转轴接近艇体轴线。3.2 编写auv_dynamics.m从力与力矩到状态导数核心函数如下。这里省略科氏力项并用对角矩阵近似质量阵和阻尼阵适合速度低于 2 m/s 的常规巡航仿真function xdot auv_dynamics(t, x, tau, par) % t: 时间, x: 12维状态, tau: 随体系控制力/力矩(6x1) % 输出xdot为12维状态导数 eta x(1:6); nu x(7:12); phi eta(4); theta eta(5); psi eta(6); % 1) 旋转矩阵R(3x3): 随体系 - 惯性系 cphi cos(phi); sphi sin(phi); cth cos(theta); sth sin(theta); cps cos(psi); sps sin(psi); R [cps*cth, cps*sth*sphi-sps*cphi, cps*sth*cphisps*sphi; sps*cth, sps*sth*sphicps*cphi, sps*sth*cphi-cps*sphi; -sth, cth*sphi, cth*cphi]; % 2) 角速度传递矩阵T(3x3) T [1, sphi*sth/cth, cphi*sth/cth; 0, cphi, -sphi; 0, sphi/cth, cphi/cth]; % 3) 质量矩阵(对角近似) M diag([par.m-par.Xudot, par.m-par.Yvdot, par.m-par.Zwdot, ... par.Ix-par.Kpdot, par.Iy-par.Mqdot, par.Iz-par.Nrdot]); % 4) 阻尼力(二次项取绝对值乘速度, 保证耗散方向正确) u nu(1); v nu(2); w nu(3); p nu(4); q nu(5); r nu(6); D_nu [par.Xu*u par.Xuu*abs(u)*u; par.Yv*v par.Yvv*abs(v)*v; par.Zw*w par.Zww*abs(w)*w; par.Kp*p par.Kpp*abs(p)*p; par.Mq*q par.Mqq*abs(q)*q; par.Nr*r par.Nrr*abs(r)*r]; % 5) 重浮力恢复力/力矩 W par.m * par.g; B par.rho * par.Vol * par.g; xG par.xg; yG par.yg; zG par.zg; xB par.xb; yB par.yb; zB par.zb; g_eta zeros(6,1); g_eta(1) (W - B) * sin(theta); g_eta(2) -(W - B) * cos(theta) * sin(phi); g_eta(3) -(W - B) * cos(theta) * cos(phi); g_eta(4) (yG*W - yB*B)*cos(theta)*cos(phi) - (zG*W - zB*B)*cos(theta)*sin(phi); g_eta(5) -(zG*W - zB*B)*sin(theta) - (xG*W - xB*B)*cos(theta)*cos(phi); g_eta(6) (xG*W - xB*B)*cos(theta)*sin(phi) (yG*W - yB*B)*sin(theta); % 6) 合并: M * nu_dot tau - D_nu - g_eta nu_dot M \ (tau - D_nu - g_eta); % 7) 运动学 eta_dot [R * nu(1:3); T * nu(4:6)]; xdot [eta_dot; nu_dot]; end逻辑上这段代码分成三个层次。前两步是运动学坐标变换只依赖当前姿态第三到五步计算动力学广义力其中阻尼和恢复力都基于当前速度与姿态第六步用质量阵的逆求解加速度。注意M \而不是inv(M)乘原因是逆矩阵求解在数值上更稳定尤其当附加质量项接近零时不会放大舍入误差。如果你把科氏力项加回来需要在tau - D_nu - g_eta前面插入-C_mat * nu。科氏力矩阵的速度耦合项很多最简单的方式是用mss_vehutils之类工具箱里的m2c(MRB, nu)自动生成。手动写容易漏掉交叉项反而破坏 M 矩阵与 C 矩阵的匹配关系。3.3 Simulink接入MATLAB Function块与代数环处理在 Simulink 里接入上述模型最直接的方法是用 MATLAB Function 块。新建模型后拖入一个 MATLAB Function 块双击打开编辑器把auv_dynamics的函数体完整粘贴进去函数签名保持不变。输入端连一个 6 维控制力矩向量输出端是 12 维xdot后面接一组积分器Integrator得到x再通过 Demux 分别引出位置、姿态、速度供控制器和示波器使用。这里有一个常见坑如果你在同一时刻既从x提取速度反馈给控制器又把控制器输出tau送回动力学块Simulink 可能报代数环Algebraic Loop错误。原因是控制力矩直接依赖x而xdot又依赖tau逻辑上形成瞬时闭环。解决办法有两个。第一个是在动力学块输出后接一个 Memory 块做一步延迟打破代数环第二个是给控制器反馈路径加单位延迟Unit Delay让控制量总是基于上一拍的测量值这更接近真实嵌入式控制器的执行时序。建议在模型里加一个常量sample_time 0.1所有控制器和延迟模块都以这个步长运行而动力学积分器继续使用连续时间。这样能模拟离散控制周期也方便后续直接生成 C 代码部署到单片机。编译时如果出现仿真发散先看这一步采样时间是否设置成了inf或与固定步长不匹配。4. Simulink仿真平台搭建海流环境、推力分配与PID闭环有了动力学核心里函数下一步是搭一个可复用的 Simulink 仿真平台。完整闭环至少需要四个子系统环境干扰模块、控制器、推力分配/执行器模块、AUV六自由度模型。四个模块按“外环指令→控制器→推力图→动力学→状态反馈”的顺序连接缺一不可。4.1 仿真总框图与模块划分推荐把模型分成四层方便不同成员并行开发子系统输入输出主要内容环境模块仿真时间、AUV位置/速度海流速度向量、波浪干扰力定常流、剪切流、白噪声扰动控制器指令值、状态反馈期望力/力矩PID、LQR、滑模等控制律推力分配控制力/力矩各推进器转速、舵角分配矩阵、饱和限制、变化率限制AUV模型推进器力、舵力12维状态上文auv_dynamics封装Simulink 里建议新建一个AUV Model子系统内部只放 MATLAB Function 块和 Integrator不给外部留修改动力学方程的口子。这样后续换控制律时不需要动模型内部结构。子系统输入是 6 维tau输出是 12 维状态再在子系统外部用 Demux 拆出位置、姿态、速度。求解器选择上开环快速验证用ode45闭环定步长仿真用ode4固定步长 0.01 s。定步长的原因是后面的 PID 参数扫描与嵌入式代码生成都要求确定性的执行时序而ode45的变步长会让每次仿真的有效采样周期不同做批量扫描时结果可比性差。4.2 海流建模定常流与随机流怎么叠加到相对速度最简单但实用的海流模型是定常均匀流设定惯性系下的流速向量 (v_n[V_c^x,V_c^y,0]^T)把它转换到随体系后用相对速度计算阻尼力。因为海流速度远低于 AUV 航行速度时附加质量项受海流加速度的影响可以忽略只修正阻尼项即可。时变海流可以用一阶马尔可夫过程生成延续性比白噪声好。离散形式写成[ V_c(k1)e^{-\Delta t/T}V_c(k)\sqrt{1-e^{-2\Delta t/T}},\sigma_w \varepsilon ]其中 (T) 是海流相关时间常数(\sigma_w) 是流速标准差(\varepsilon) 是标准正态随机数。在 Simulink 中实现时用离散状态空间模块或 MATLAB Function 块存状态变量。把海流接到动力学模型中的代码片段function xdot auv_model_with_current(t, x, tau, par, Vn) % Vn: 惯性系恒定流速 [Vx; Vy; 0] eta x(1:6); nu x(7:12); phi eta(4); theta eta(5); psi eta(6); % 旋转矩阵与auv_dynamics中相同 % 计算随体系下的相对速度 R euler_to_R(phi, theta, psi); vc_body R * Vn; % 惯性系流速转到随体系 nu_rel nu(1:3) - vc_body; % 相对速度用于水动力计算 % 后续阻尼项用nu_rel计算, 舍去相对加速度耦合 end这里容易犯的错误是直接在随体系里加减绝对流速。海流上通常定义在惯性系必须先乘旋转矩阵的转置。另一个常见问题是把海流速度也加到角速度上但平流海流不产生涡量对 AUV 角速度不产生直接作用这一项保持为零就行。4.3 执行器饱和与推力分配小型 AUV 的常见配置是尾部两个水平推进器、中部一个垂直推进器。水平面内通过左右推进器的差速产生偏航力矩不再单独设置垂直舵垂直面内靠垂直推进器或俯仰舵控制深度。这种配置的推力分配矩阵写出来是% tau_cmd(6x1) 来自控制器 % th(4x1) 为推进器命令: [左水平; 右水平; 垂直; 俯仰舵] B_thrust [1 1 0 0; 0 0 1 0; 0 0 0 0; 0 0 0 0; 0 0 0 1; -L L 0 0]; % L为水平推进器到中线的距离 th pinv(B_thrust) * tau_cmd; % 最小范数分配 th min(max(th, -thrust_max), thrust_max); % 幅值饱和分配矩阵直接用伪逆pinv处理因为多数工况下控制力维度大于执行器数量伪逆给出最小能量解。仿真里如果出现推进器输出高频抖动还需要对th加变化率限制否则真实推进器的响应时间跟不上控制器输出仿真结论会偏乐观。用 Simulink 的 Rate Limiter 模块实现变化率限制上限设为推进器最快转速变化率例如 200 rpm/s。4.4 PID参数整定深度环与航向环的分层设计深度控制和航向控制不建议直接对推力做单环 PID。深度与俯仰角强耦合正确做法是外环深度 PID 生成俯仰角指令内环俯仰角 PID 生成俯仰力矩航向控制类似外环航向 PID 生成偏航角速度指令内环偏航角速度 PID 生成偏航力矩。% 深度外环: 深度误差 - 期望俯仰角 e_z z_ref - z; theta_ref saturate(e_z * Kp_z integral_e_z * Ki_z, [-0.4 0.4]); % 俯仰内环: 俯仰角误差 - 升降舵/垂推 e_theta theta_ref - theta; tau_theta Kp_theta * e_theta Kd_theta * (-q); % 航向外环: 航向误差 - 期望偏航角速度 e_psi wrapToPi(psi_ref - psi); r_ref Kp_psi * e_psi; % 偏航内环: 角速度误差 - 差速力矩 e_r r_ref - r; tau_psi Kp_r * e_r;wrapToPi这一步很关键不做角度归一化时航向从 170° 转到的 -170° 会产生 340° 的伪误差控制器会错误地反向打舵。深度环的积分项建议加抗饱和限制因为垂直推进器推力有限积分饱和会让 AUV 在到达目标深度后仍然保持一个偏置舵角。一组能直接跑通初仿真的 PID 参数参考如下回路KpKiKd说明深度外环0.80.020输出限制 ±0.4 rad俯仰内环3.001.2微分项取-q而非θ的差分航向外环1.500输出为偏航角速度指令偏航内环2.50.10积分用于消除稳态静差调参顺序固定为先内环后外环。把深度外环断开给俯仰阶跃指令调内环直到超调小于 15%再接上深度外环调 Kp_z 和 Ki_z。如果深度曲线出现等幅振荡优先减小外环 Kp而不是调内环。5. 仿真结果验证与批量参数扫描模型建完后第一件该做的事情是验证符号方向是否正确而不是直接调 PID。方向错误在视觉上表现为“给正纵倾力矩AUV 低头”这在某些数据格式混乱的模型里非常容易出现。5.1 用自由衰减曲线验证模型符号方向写一个零控制输入的开环脚本x0 zeros(12,1); x0(7) 0.8; % 初始纵向速度 0.8 m/s tau zeros(6,1); % 无控制输入 [t, X] ode45((t,x) auv_dynamics(t,x,tau,par), [0 100], x0); plot(t, X(:,7)); xlabel(time (s)); ylabel(u (m/s));正常结果应该是纵向速度从 0.8 m/s 呈指数衰减趋势缓慢下降不会越过零轴变成负值如果有俯仰角初值AUV 会以阻尼振荡方式回到水平姿态。如果曲线发散、或初始横摇角对应错误方向的恢复力矩优先检查 (W-B) 和浮心位置的正负号。常见错误是重浮心坐标用惯性系下的值代替随体系下的值导致恢复力矩方向反了。5.2 用parsim并行扫描PID参数单次闭环仿真很快但 PID 参数空间扫描次数一多串行跑会浪费大量等待时间。MATLAB 的parsim可以在并行池里批量运行 Simulink 模型前提是把每次仿真的参数写入SimulationInput对象KpSet linspace(0.4, 1.2, 8); KdSet linspace(0.2, 1.5, 8); [KP, KD] ndgrid(KpSet, KdSet); simIn(1:numel(KP)) Simulink.SimulationInput(auv_depth_control); for i 1:numel(KP) simIn(i) simIn(i).setVariable(Kp_z, KP(i)); simIn(i) simIn(i).setVariable(Kd_theta, KD(i)); end out parsim(simIn, ShowProgress, on);扫描完成后用循环从每个out(i)里取出深度曲线计算上升时间、超调量和稳态误差再画成参数热力图。这里一个实用技巧是给SimulationInput的setVariable指定变量作用域为数据字典或模型工作区避免修改基础工作区变量影响并行池的随机数流。批量仿真时把StopTime设为固定值并用离散求解器否则不同参数的仿真步数差异会影响对比公平性。若机器是多核 CPUparsim默认按核数分配任务比手写parfor加sim()的写法更简洁出错时还会保留下标对应的完整日志定位崩溃参数组很快。本文还有配套的精品资源点击获取
