📚 课程目录第2章 AUV运动学与动力学建模基础

2. AUV运动学与动力学建模基础

要为AUV设计路径跟踪制导律，第一步不是写控制器，而是把“艇在哪里、朝哪里、以多快速度运动”用一套自洽的数学语言描述清楚。运动学回答几何关系——位置、姿态与速度之间的映射；动力学回答力的关系——推力、水动力、重力浮力如何改变速度。二者合起来，才构成制导律可以依赖的模型。本节先建立六自由度框架，再聚焦水平面三自由度模型，因为绝大多数路径跟踪任务（巡航、巡检、测绘）主要发生在水平面内。

一、坐标系与状态量定义

建模前必须先固定参照约定，否则后续所有正负号都会混乱。本课程统一采用两类坐标系：

惯性坐标系 {I}：原点取任务起点水面某点，X轴指向正北，Y轴指向正东，Z轴指向地心（北-东-地，NED）。AUV位置用 (x, y, z) 表示，姿态用横滚角 φ、纵倾角 θ、艏向角 ψ 表示。
艇体坐标系 {B}：原点取艇体重心，x轴指向艇艏，y轴指向右舷，z轴指向艇底。线速度 u（纵荡）、v（横荡）、w（垂荡），角速度 p（横滚）、q（纵倾）、r（艏摇）均在此系下定义。

六自由度速度向量记为 ν = [u, v, w, p, q, r]ᵀ，惯性系位置姿态记为 η = [x, y, z, φ, θ, ψ]ᵀ。两者通过运动学方程 η̇ = J(η)ν 联系，其中 J(η) 是由欧拉角构成的分块变换矩阵。这个矩阵的作用是把艇体系速度“旋转”到惯性系，是后续所有轨迹推算的基础。

二、六自由度运动学与动力学方程

完整六自由度动力学采用Fossen提出的刚体+水动力形式：

M ν̇ + C(ν)ν + D(ν)ν + g(η) = τ + τ_env

其中 M 为包含附加质量的惯性矩阵，C(ν) 为科氏-向心力矩阵，D(ν) 为阻尼矩阵（含线性与二次阻尼），g(η) 为恢复力（重力与浮力），τ 为推进器与舵产生的控制力/力矩，τ_env 为海流等环境扰动。对教学与制导设计而言，不必一开始就展开全部36个水动力系数，而应理解每一项的物理含义：M 决定加速响应快慢，D 决定速度衰减特性，g(η) 决定静稳定性，τ 是控制器唯一能直接施加的量。

建模精度与任务匹配：路径跟踪制导主要关心水平面低速运动，垂向与横滚通道可先解耦简化；若做定深或姿态控制，则需保留对应自由度。不要为“完整”而堆砌无法辨识的系数。

三、水平面三自由度模型

假设AUV在恒定深度航行，忽略垂荡、横滚与纵倾（w = p = q = 0），状态简化为 η = [x, y, ψ]ᵀ，ν = [u, v, r]ᵀ。此时运动学方程展开为：

ẋ = u·cosψ − v·sinψ
ẏ = u·sinψ + v·cosψ
ψ̇ = r

动力学方程在水平面内写为三个耦合方程。以纵荡、横荡、艏摇为例，忽略高阶耦合项后的常用简化形式为：

(m − X_u̇)u̇ = X_u·u + X_|u|u·|u| + T
(m − Y_v̇)v̇ = Y_v·v + Y_r·r − (m − X_u̇)·u·r
(I_z − N_ṙ)ṙ = N_v·v + N_r·r + N_δ·δ

这里 T 为纵向推力，δ 为舵角，X_u̇、Y_v̇、N_ṙ 为附加质量项，X_u、Y_v、N_r 等为阻尼导数。注意横荡方程中的 −(m − X_u̇)·u·r 项：它表明前进速度 u 与艏摇角速度 r 会“制造”侧向速度，这正是AUV转弯时产生侧滑的数学根源，也是LOS制导中前视距离不能取得过小的原因之一。

四、从模型到制导：哪些项真正影响路径跟踪

制导律设计并不需要完整辨识所有水动力系数，但必须清楚哪些项会显著影响跟踪品质。第一，阻尼项 D(ν) 决定AUV对舵角和推力的响应滞后，若忽略二次阻尼，仿真中会出现“转向过快”的假象。第二，附加质量项使有效惯性大于刚体质量，转弯时艏摇响应变慢，前视距离需相应加大。第三，横荡-艏摇耦合项意味着即使舵角为零，转弯后AUV仍会带侧向速度，LOS制导若只跟踪位置误差而不考虑侧滑，会在弯道出现持续外偏。

一个实用的教学示例：设某小型AUV质量 m = 45 kg，长度 1.6 m，附加质量 X_u̇ ≈ −4.5 kg、Y_v̇ ≈ −38 kg、N_ṙ ≈ −2.8 kg·m²，线性阻尼 X_u ≈ −12 N·s/m、Y_v ≈ −95 N·s/m、N_r ≈ −18 N·m·s。这些数值属于合理假设的示例参数，用于说明量级关系而非实测。代入纵荡方程可见，时间常数约为 (45+4.5)/12 ≈ 4.1 s，说明推力变化后速度需要数秒才稳定；而艏摇通道时间常数约为 (I_z+2.8)/18，若 I_z 取 6 kg·m²，则约 0.49 s，转向明显快于变速。这一快慢差异直接决定了制导律中“位置修正”和“艏向修正”不能使用同一增益。

常见建模错误：把艇体坐标系速度直接当作惯性系速度积分，导致航迹在转弯时系统性偏转；或忽略附加质量，使仿真中的转向响应比实艇快一个量级。建模阶段务必先核对坐标系定义与单位（弧度/度、牛/千克力）。

五、建模成果的检验方式

模型建立后应做两项基本检验。其一是直航检验：给定恒定推力 T、舵角 δ = 0，从静止积分方程，观察 u 是否单调趋近稳态值 T/|X_u|，v 与 r 是否保持为零；若 v 不为零，说明耦合项符号或初值有误。其二是定舵回转检验：给定恒定 u 与固定舵角 δ，观察 r 是否收敛到稳态回转角速度，且回转半径 R ≈ u/r 是否合理。这两项检验不需要实测数据，仅靠方程自洽性即可发现大多数符号与量级错误，是进入LOS制导设计前必须通过的关卡。

完成本节建模后，读者应能写出水平面三自由度状态方程，说明每个状态量的物理意义与坐标系归属，并判断哪些水动力项会影响后续制导精度。这套模型将作为LOS制导律推导与仿真的被控对象基础。