📚 课程目录第3章海洋无人装备运动学建模基础

3. 海洋无人装备运动学建模基础

运动学建模是海洋无人装备控制与仿真的第一块基石。它回答一个核心问题：装备的位置、姿态如何随时间变化，以及这种变化与速度、角速度之间是什么关系。本节围绕无人艇（USV）与水下机器人（ROV/AUV）两类典型对象，先厘清坐标系约定，再推导三自由度与六自由度运动学方程，最后用 ROS2 tf2 把坐标系关系组织成可查询的变换树。

一、两套坐标系：北东地与船体坐标系

海洋装备建模通常采用两套坐标系。第一套是北东地坐标系（NED），也称导航坐标系或世界坐标系，原点固定在地面或水面某点，x 轴指向正北、y 轴指向正东、z 轴指向地心（向下为正）。第二套是船体坐标系（body frame），原点取在装备重心或浮心，x 轴指向船首（纵向），y 轴指向右舷（横向），z 轴指向下方（垂向）。

这里必须强调符号约定的一致性：NED 中 z 向下为正，因此深度增加对应 z 增大；船体坐标系同样取 z 向下，使垂荡、纵摇、艏摇的正方向与 NED 保持直观对应。全文与图示均采用这一约定，切换参照系时会明确说明。无人艇与水下机器人的差别主要在于：无人艇通常约束在水面附近，垂向运动弱，常用三自由度（纵荡、横荡、艏摇）描述；水下机器人需要完整六自由度（三平移加三转动）。

二、六自由度运动学方程

设船体坐标系下的速度向量为 ν = [u, v, w, p, q, r]ᵀ，分别对应纵荡、横荡、垂荡线速度与横滚、纵摇、艏摇角速度。设 NED 下的位置姿态为 η = [x, y, z, φ, θ, ψ]ᵀ。两者通过一个 6×6 变换矩阵 J(η) 联系：

η̇ = J(η) · ν

J(η) = [ R(φ,θ,ψ)   0₃ₓ₃ ]
       [ 0₃ₓ₃       T(φ,θ,ψ) ]

R = Rz(ψ)·Ry(θ)·Rx(φ)   （旋转顺序 Z-Y-X）
T = [1  sinφ·tanθ  cosφ·tanθ]
    [0  cosφ       -sinφ    ]
    [0  sinφ/cosθ  cosφ/cosθ]

其中 R 是线速度从船体系到 NED 的旋转矩阵，T 是角速度的欧拉角变换矩阵。注意 T 在 θ = ±90° 处奇异，实际仿真中应避免纵摇接近垂直，或改用四元数表示姿态。展开后，位置导数为：

ẋ = u·cosψ·cosθ + v·(cosψ·sinθ·sinφ − sinψ·cosφ) + w·(cosψ·sinθ·cosφ + sinψ·sinφ)
ẏ = u·sinψ·cosθ + v·(sinψ·sinθ·sinφ + cosψ·cosφ) + w·(sinψ·sinθ·cosφ − cosψ·sinφ)
ż = −u·sinθ + v·cosθ·sinφ + w·cosθ·cosφ

姿态导数则由 p、q、r 经 T 矩阵得到。以 ψ̇ 为例：ψ̇ = (q·sinφ + r·cosφ)/cosθ。当纵摇 θ 较小时，cosθ ≈ 1，可近似 ψ̇ ≈ r，这正是许多水面无人艇简化模型直接令艏摇角速度等于 r 的依据。

三、三自由度简化模型

对水面无人艇，忽略垂荡、横滚、纵摇，取状态 [x, y, ψ] 与速度 [u, v, r]，运动学方程退化为：

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

这一形式在路径跟踪、航向控制中极为常用。需要提醒的是，横荡速度 v 在欠驱动无人艇上通常不可直接控制，它由侧向水动力与转向耦合产生，因此建模时不能简单令 v = 0，否则会丢失漂角信息。教学示例中可设 u = 1.2 m/s、v = 0.15 m/s、r = 0.1 rad/s、初始 ψ = 0，用 0.1 s 步长积分 10 s，可观察到艇体沿近似圆弧轨迹前进，且航向角累计约 1 rad——这属于合理假设下的演示数据，并非实测。

四、用 tf2 建立坐标变换树

在 ROS2 中，上述坐标系关系应组织为一棵 tf2 变换树。典型结构为：world（NED 原点）→ base_link（船体坐标系）→ 各传感器坐标系（如 imu_link、gps_link、sonar_link）。world 到 base_link 的变换由运动学积分结果发布，传感器到 base_link 的变换是固定安装偏移，用静态变换发布。

实现上，world 到 base_link 的动态变换可在节点中按积分结果构造 geometry_msgs/msg/TransformStamped，用 tf2_ros::TransformBroadcaster 发布；传感器安装偏移用 StaticTransformBroadcaster 一次性发布。查询时用 tf2_ros::Buffer 配合 lookupTransform("world", "sonar_link", tf2::TimePointZero) 即可得到声呐在 NED 下的位姿，无需手工串联矩阵。

调试建议：用 ros2 run tf2_tools view_frames 生成变换树 PDF，确认没有断链；用 ros2 run tf2_ros tf2_echo world base_link 实时观察位姿数值是否与积分结果一致。若出现 "Lookup would require extrapolation into the future"，通常是时间戳未对齐，检查发布端与查询端是否使用同一时钟源。

把运动学方程与 tf2 变换树对应起来，是后续控制器设计、传感器融合与仿真可视化的共同前提。方程给出数值演化规律，tf2 给出这些数值在空间中的组织方式，两者一致，装备的位姿才在系统中"说得清、查得到"。