2. 无人船坐标系定义与航向角约定一致性检查

坐标系与符号约定

无人船航向相关计算最容易出错的地方不是算法本身，而是各模块对“正方向”的理解不一致。本例统一采用以下约定，并在后续所有模块中强制遵守：

• NED 导航坐标系：原点取任务起点，X_N 指向正北，Y_E 指向正东，Z_D 垂直向下。航向角 ψ 从正北起算，顺时针为正，范围 [−180°, 180°)，即 ψ=90° 表示船头朝东。
• 船体坐标系：X_B 指向船头，Y_B 指向右舷，Z_B 向下。船体坐标系相对 NED 的旋转角即偏航角 ψ，绕 Z_D 轴顺时针为正。
• 偏航角速率 r：r = dψ/dt，单位 rad/s 或 °/s，右转（顺时针）为正。
• 舵角 δ：规定右舵为正（舵叶使船右转），左舵为负，范围通常 [−35°, 35°]。

关键点：NED 是右手系但 Z 轴向下，因此绕 Z 轴的“右手正旋转”在俯视图中表现为顺时针。很多模块（尤其是从 ENU 移植的代码）默认逆时针为正，这正是航向偏差的常见来源。

角度约定一致性检查脚本

检查思路：构造一组已知真值的航向序列，分别送入“制导模块”“控制器模块”“状态估计模块”的角度处理函数，比较各模块输出的 ψ、r、δ 符号与真值是否一致。下面脚本用纯 Python 实现，不依赖第三方库，可直接运行。

import math

D2R = math.pi / 180.0
R2D = 180.0 / math.pi

def wrap_pi(angle_rad):
    """把角度归一化到 [-pi, pi)，NED 约定：顺时针为正"""
    return (angle_rad + math.pi) % (2 * math.pi) - math.pi

# ---------- 三个模块各自的角度处理实现 ----------
def guidance_heading(xn, xe):
    """制导模块：由 NED 位置差求期望航向，顺时针为正"""
    return wrap_pi(math.atan2(xe, xn))   # atan2(E, N) 即从北顺时针

def controller_yaw_rate(psi_ref, psi_now, kp=0.8):
    """控制器模块：航向误差 -> 期望偏航角速率，右转为正"""
    err = wrap_pi(psi_ref - psi_now)
    return kp * err

def estimator_yaw_rate(psi_prev, psi_now, dt):
    """状态估计模块：差分求偏航角速率，需处理 ±pi 跳变"""
    return wrap_pi(psi_now - psi_prev) / dt

def rudder_from_rate(r_cmd, k_r=1.2, delta_max=35.0):
    """舵角映射：右舵为正，限幅"""
    delta = k_r * r_cmd * R2D
    return max(-delta_max, min(delta_max, delta))

# ---------- 一致性检查 ----------
def check_consistency():
    cases = [
        # (说明, 起点NED, 终点NED, 期望航向deg)
        ("正北", (0, 0), (100, 0), 0.0),
        ("正东", (0, 0), (0, 100), 90.0),
        ("正南", (0, 0), (-100, 0), 180.0),
        ("正西", (0, 0), (0, -100), -90.0),
        ("东北", (0, 0), (100, 100), 45.0),
    ]
    print(f"{'场景':<6}{'期望ψ':>8}{'制导ψ':>8}{'误差':>8}{'r符号':>8}{'δ符号':>8}{'结论':>6}")
    all_ok = True
    for name, p0, p1, psi_exp_deg in cases:
        dxn, dxe = p1[0] - p0[0], p1[1] - p0[1]
        psi_g = guidance_heading(dxn, dxe) * R2D
        err = wrap_pi((psi_g - psi_exp_deg) * D2R) * R2D
        # 假设当前航向为 0，期望航向 psi_g，检查 r 与 δ 符号
        r_cmd = controller_yaw_rate(psi_g * D2R, 0.0)
        delta = rudder_from_rate(r_cmd)
        # 期望：psi_g>0 时 r>0、δ>0；psi_g<0 时 r<0、δ<0
        sign_ok = (psi_g == 0) or (math.copysign(1, r_cmd) == math.copysign(1, psi_g)
                                   and math.copysign(1, delta) == math.copysign(1, psi_g))
        ok = abs(err) < 1e-6 and sign_ok
        all_ok = all_ok and ok
        print(f"{name:<6}{psi_exp_deg:>8.1f}{psi_g:>8.1f}{err:>8.3f}"
              f"{'+' if r_cmd>=0 else '-':>8}{'+' if delta>=0 else '-':>8}"
              f"{'通过' if ok else '失败':>6}")
    # 估计模块：跨越 ±pi 的差分
    psi_a, psi_b = 179.0 * D2R, -179.0 * D2R
    r_est = estimator_yaw_rate(psi_a, psi_b, 0.1) * R2D
    print(f"\n跨±180°差分: ψ 179°→-179°, dt=0.1s, r_est={r_est:.1f}°/s (期望 +20°/s)")
    if abs(r_est - 20.0) > 1e-6:
        all_ok = False
    print("总体一致性:", "通过" if all_ok else "存在不一致")
    return all_ok

if __name__ == "__main__":
    check_consistency()

预期输出（示例运行结果，非实测）：

场景        期望ψ    制导ψ      误差   r符号  δ符号  结论
正北       0.0     0.0   0.000      +      +   通过
正东      90.0    90.0   0.000      +      +   通过
正南     180.0   180.0   0.000      +      +   通过
正西     -90.0   -90.0   0.000      -      -   通过
东北      45.0    45.0   0.000      +      +   通过

跨±180°差分: ψ 179°→-179°, dt=0.1s, r_est=20.0°/s (期望 +20°/s)
总体一致性: 通过

定位坐标系混用的典型偏差

若某模块误用 ENU 约定（逆时针为正），最直接的表现是航向角符号翻转：期望向东（ψ=90°）时，该模块输出 −90°，控制器据此打左舵，船实际向西偏转。检查脚本中“正东”用例的误差会变成 −180°，r 与 δ 符号同时为负，一眼即可定位。另一类隐蔽问题是差分求 r 时未做 ±180° 环绕处理，跨越正北时 r 会突变为约 −3580°/s（dt=0.1s 时），导致舵角瞬间打满。

检查清单