3. 导航坐标系与运动学基础

USV（无人水面艇）的导航与任务规划，第一步不是写算法，而是把“位置、姿态、速度”这些量放进一个明确的参照系里。坐标系定义不清，后续的航迹跟踪、避障判断、控制律设计都会出现方向反号、角度跳变、位置漂移等难以排查的问题。本节围绕三个常用坐标系、它们之间的转换关系，以及USV平面运动学模型与操纵性参数展开，目标是让读者能独立完成一次“给定经纬度与航向，算出船体在局部平面内的位置与速度分量”的完整计算。

三个常用坐标系及其定义

WGS84 大地坐标系是全局参照。它用经度 λ、纬度 φ 和大地高 h 描述地球表面附近的位置，配合椭球参数（长半轴 a≈6378137 m，扁率 f≈1/298.257223563）确定几何关系。USV 的 GPS/RTK 输出通常就是 WGS84 下的经纬度，它是绝对定位的源头，但直接用于局部规划并不方便：经纬度的“1 度”在不同纬度对应的地面距离不同，且航向、速度在经纬度空间里不是线性量。

ENU 局部切平面坐标系解决了局部计算问题。以某个参考点（如任务起点或码头）为原点，E 轴指向东、N 轴指向北、U 轴指向天，构成右手直角坐标系。在小范围（通常几公里内）作业时，可把地球局部近似为平面，经纬度到 ENU 的转换误差可控制在厘米到分米级，足以支撑内河、湖泊、港区等典型 USV 场景。ENU 是任务规划、航迹生成、避障几何计算最常用的工作坐标系。

船体坐标系固定在船体上，随船运动。按船舶与海洋工程惯例，取 x 轴指向船首、y 轴指向左舷、z 轴指向天，原点可取在重心或船体几何中心。船体坐标系下的速度分量 (u, v) 分别称为纵荡速度和横荡速度，r 为艏摇角速度。控制、推进、水动力建模几乎都在船体坐标系下描述，因为推力、阻力、舵效都是相对船体方向作用的。

坐标转换的实际计算

从 WGS84 到 ENU 的转换分两步。第一步把经纬度差换算为局部平面位移。设参考点 (φ₀, λ₀)，目标点 (φ, λ)，则近似有：

E ≈ (λ - λ₀) · cos(φ₀) · R
N ≈ (φ - φ₀) · R
其中 R 为地球平均曲率半径，取 6371000 m 量级；
角度需先由度转为弧度。

举例：参考点取 φ₀=31.2300°、λ₀=121.4700°（示例坐标），目标点 φ=31.2305°、λ=121.4708°。纬度差 0.0005° 对应 N≈0.0005×π/180×6371000≈55.6 m；经度差 0.0008° 对应 E≈0.0008×π/180×6371000×cos(31.23°)≈77.6 m。可见同样 0.0001° 的经度差在上海纬度约合 9.5 m，而在赤道约合 11.1 m，这正是必须做 cos(φ₀) 修正的原因。若任务范围超过十几公里或对精度要求高，应改用完整的椭球投影（如横轴墨卡托）而非上述平面近似。

ENU 与船体坐标系之间的转换是旋转关系。设航向角为 ψ（北向顺时针为正），则船体速度到 ENU 速度的映射为：

V_E = u·sin(ψ) - v·cos(ψ)
V_N = u·cos(ψ) + v·sin(ψ)
r   = ψ̇（艏摇角速度，逆时针为正时需注意符号）

反过来，已知 ENU 速度求船体分量，用同一旋转矩阵的转置即可。实际实现时建议把旋转矩阵写成函数，输入 ψ 和向量，避免在多个模块里手写正负号。一个常见做法是：规划层全部在 ENU 下工作，控制层把期望速度旋转到船体坐标系后再分配给推进器，这样各层职责清晰、符号不易混乱。

USV 平面运动学模型

对大多数内河、湖泊、港区作业的 USV，可忽略垂荡、横摇、纵摇，只保留平面三自由度：位置 (x, y)、航向 ψ、纵荡速度 u、横荡速度 v、艏摇角速度 r。运动学方程（在 ENU 下描述位置，在船体系下描述速度）为：

ẋ = u·cos(ψ) - v·sin(ψ)
ẏ = u·sin(ψ) + v·cos(ψ)
ψ̇ = r

注意这里的 ẋ、ẏ 是 ENU 下的东、北向速度，与上一小节公式一致，只是把 V_E、V_N 换成了位置导数。若采用“北东天”且 ψ 从北顺时针为正，上式成立；若你的 ψ 从东逆时针为正，则需把 sin/cos 互换并调整符号。运动学模型只描述几何关系，不含力与质量；要描述“打舵后多久转过来”，需要动力学模型，但任务规划与航迹跟踪的几何层通常先用运动学模型验证可行性。

一个实用的教学示例：设某 USV 以 u=2.0 m/s、v=0、r=0.1 rad/s 航行，ψ=30°。则 ẋ=2.0×cos30°≈1.73 m/s（东向），ẏ=2.0×sin30°=1.0 m/s（北向），10 秒后位置约东移 17.3 m、北移 10.0 m，航向变为 30°+0.1×10×180/π≈87.3°。这个手算过程可用于校验仿真或实船日志中的速度分解是否正确。

操纵性参数及其工程意义

规划层最关心的操纵性参数有三类。第一是最大航速与巡航速度，决定任务时间估算与能耗预算；第二是最小回转半径，决定航迹转弯段能否跟踪、避障时是否需要提前转向；第三是艏摇响应时间常数，即打舵后航向角速度达到稳态值约 63% 所需时间，反映船的“跟舵快慢”。这些参数通常通过实船回转试验、Z 形试验获取，也可由厂家提供或由简化动力学模型估算。

以回转半径为例：若某 USV 在巡航速度 2 m/s 下最小回转半径约 8 m，则规划航迹的转弯半径不应小于 8 m，否则控制器会持续饱和、实际航迹外扩。避障时若障碍物距离小于“回转半径 + 安全余量”，就应提前减速或选择绕行而非急转。艏摇时间常数较大（如 3～5 s）的船，在狭窄水道中应降低航速、增大转弯提前量，否则会出现“指令已打满、船头还没转过来”的滞后。

从坐标到可执行规划的最小闭环

把本节内容串起来，一次完整的规划前处理应包含：读取任务点的 WGS84 经纬度，选定参考点并转换为 ENU 平面坐标；根据当前航向 ψ 把期望速度旋转到船体坐标系，检查 u、v 是否在推进器能力范围内；用最小回转半径约束航迹曲率，用艏摇时间常数估算转弯提前量；最后把规划结果再旋转回 ENU 下发给控制器。每一步都应有明确的输入、输出与单位，建议在代码中用带单位的变量名（如 psi_rad、u_mps）减少混用错误。完成这套转换后，后续的航迹跟踪与避障算法才有可靠的几何基础。