3. DVL 安装角标定与速度投影补偿
安装角误差的物理来源与观测补偿思路
DVL 的换能器基阵在安装时，其测量坐标系（记为 d 系）与 INS 的载体系（记为 b 系）之间通常存在一个小的固定旋转，称为安装角。若忽略它，DVL 输出的速度 v_d^d 被直接当作 v^b 使用，就会在观测方程中引入与航向、横滚相关的系统性残差。本节只处理其中最主要的两个分量：绕载体系 z 轴的航向安装角 ψ_m 与绕 y 轴的俯仰安装角 θ_m，并假设横滚安装角 φ_m 已通过机械安装保证足够小。
设 DVL 在自身坐标系下测得速度 v^d = [u_d, v_d, w_d]^T，则补偿到载体系的速度为：
v^b = C_d^b · v^d
C_d^b ≈ [ 1 ψ_m -θ_m ]
[ -ψ_m 1 φ_m ]
[ θ_m -φ_m 1 ] （小角度近似，φ_m 取 0）
展开后得到用于观测方程的两个水平分量：
v_x^b ≈ u_d + ψ_m·v_d − θ_m·w_d
v_y^b ≈ v_d − ψ_m·u_d
可见 ψ_m 主要耦合在 u_d 与 v_d 的交叉项上，而 θ_m 通过垂向速度 w_d 影响纵向速度。当载体做纯直航（v_d≈0）时，ψ_m 几乎不可观测；因此标定必须包含转向或斜航机动，使 u_d 与 v_d 同时非零。
标定数据构造与最小二乘求解
标定的核心是把安装角当作未知常量，用一段机动数据构造线性方程组。设第 k 个采样时刻 INS 输出的载体系速度为 v_INS,k^b，DVL 原始速度为 v_k^d，则残差模型为：
r_k = v_INS,k^b − v_k^d = H_k · [ψ_m, θ_m]^T + n_k
H_k = [ v_d,k −w_d,k ]
[ −u_d,k 0 ]
把所有时刻堆叠成 r = H·x + n，最小二乘解为 x = (HᵀH)⁻¹Hᵀr。下面给出可直接运行的 Python 实现，包含示例数据生成、求解与残差验证。示例数据为教学假设值，用于演示流程，不代表实测结果。
import numpy as np
# 教学示例：真实安装角（假设值）
psi_true = np.deg2rad(1.5) # 航向安装角
theta_true = np.deg2rad(0.8) # 俯仰安装角
rng = np.random.default_rng(42)
N = 400
# 构造机动段：u_d 与 v_d 同时非零，w_d 小幅变化
u_d = 2.0 + 0.5 * rng.standard_normal(N)
v_d = 0.8 * np.sin(np.linspace(0, 4*np.pi, N)) + 0.1 * rng.standard_normal(N)
w_d = 0.05 * rng.standard_normal(N)
# 由真实安装角生成“INS 速度”（含小噪声）
v_ins_x = u_d + psi_true * v_d - theta_true * w_d + 0.01 * rng.standard_normal(N)
v_ins_y = v_d - psi_true * u_d + 0.01 * rng.standard_normal(N)
# 构造观测矩阵与残差
H = np.zeros((2*N, 2))
r = np.zeros(2*N)
H[0::2, 0] = v_d
H[0::2, 1] = -w_d
r[0::2] = v_ins_x - u_d
H[1::2, 0] = -u_d
H[1::2, 1] = 0.0
r[1::2] = v_ins_y - v_d
x_hat, *_ = np.linalg.lstsq(H, r, rcond=None)
psi_hat, theta_hat = x_hat
print("psi_hat = %.4f deg" % np.rad2deg(psi_hat))
print("theta_hat = %.4f deg" % np.rad2deg(theta_hat))
# 补偿后残差 RMS 对比
r_before = np.sqrt(np.mean(r**2))
r_after = r - H @ x_hat
r_after_rms = np.sqrt(np.mean(r_after**2))
print("补偿前残差 RMS = %.5f m/s" % r_before)
print("补偿后残差 RMS = %.5f m/s" % r_after_rms)
运行后应观察到 psi_hat 与 theta_hat 接近 1.5° 与 0.8°，且补偿后残差 RMS 明显小于补偿前。若 psi_hat 偏差大，优先检查机动段中 u_d 是否足够大、v_d 是否长期接近零。
把安装角写入滤波观测方程
标定完成后有两种使用方式。第一种是离线标定后作为常量补偿，观测方程直接使用 C_d^b·v^d。第二种是把 ψ_m、θ_m 作为滤波状态量在线估计，此时观测矩阵需要增加对安装角的偏导项。以水平速度观测为例，观测矩阵中对应安装角的两列为：
∂v_x^b/∂ψ_m = v_d ∂v_x^b/∂θ_m = −w_d
∂v_y^b/∂ψ_m = −u_d ∂v_y^b/∂θ_m = 0
在 15 维误差状态滤波中，把这两列追加到原有速度观测矩阵的对应位置即可。需要注意：安装角状态的过程噪声应设得很小（视为常值），否则会与姿态误差状态产生耦合，导致航向误差与安装角互相“吸收”，收敛变慢。
u_d 与 v_d 的乘积项在数据中充分激励；纯直航数据只能标定 θ_m，无法可靠分离 ψ_m。ψ_m 估计出现随航向变化的偏差。建议先完成时间同步，再做本节标定。结果判读与常见问题
| 现象 | 可能原因 | 处理 |
|---|---|---|
| ψ_m 估计值随航向段变化 | 时间同步未对齐或存在未建模的杆臂效应 | 先做时间对齐，检查 DVL 与 INS 安装位置差 |
| θ_m 收敛但 ψ_m 不收敛 | 机动段 v_d 长期接近零，ψ_m 不可观测 | 增加转向或斜航机动，保证 u_d、v_d 同时非零 |
| 补偿后残差 RMS 反而增大 | 安装角符号约定与投影方向不一致 | 核对 C_d^b 的旋转方向与 b/d 系定义 |
| 在线估计时安装角漂移 | 安装角状态过程噪声过大，与姿态误差耦合 | 减小安装角过程噪声，或改为离线标定后固定 |
完成本节后，应能得到一组稳定的 ψ_m、θ_m 估计值，并验证补偿后速度残差 RMS 下降。该结果将作为后续刻度因数估计与异常值检测的输入基础。