1. 单应答器距离观测方程建立与线性化

单应答器斜距观测方程

在SBL（短基线）系统中，船底换能器阵列中的某个换能器 i 向海底单个应答器发射询问脉冲，应答器接收后回传应答信号。换能器测得双程传播时间 t，结合声速 c 得到斜距观测值：

ρ_i = c · t / 2

设换能器 i 在船体坐标系（或当地水平坐标系）下的已知坐标为 P_i = (x_i, y_i, z_i)，应答器待求坐标为 X = (x, y, z)，则斜距观测方程为：

ρ_i = √[(x − x_i)² + (y − y_i)² + (z − z_i)²] + ε_i

其中 ε_i 为观测噪声。该方程关于 X 是非线性的（含平方根），无法直接用于最小二乘，需要先线性化。

在概略坐标处的线性化推导

设应答器概略坐标为 X₀ = (x₀, y₀, z₀)，改正数向量 δX = (δx, δy, δz)，即 X = X₀ + δX。将观测方程在 X₀ 处作一阶泰勒展开：

ρ_i ≈ ρ_i(X₀) + (∂ρ_i/∂x)·δx + (∂ρ_i/∂y)·δy + (∂ρ_i/∂z)·δz

记概略斜距 ρ_i⁰ = √[(x₀ − x_i)² + (y₀ − y_i)² + (z₀ − z_i)²]，各偏导数为：

∂ρ_i/∂x = (x₀ − x_i) / ρ_i⁰，  ∂ρ_i/∂y = (y₀ − y_i) / ρ_i⁰，  ∂ρ_i/∂z = (z₀ − z_i) / ρ_i⁰

这三个偏导数构成从应答器指向换能器 i 的单位方向余弦向量 u_i。将观测方程改写为残差形式：

v_i = u_i^T · δX − l_i，  其中 l_i = ρ_i^obs − ρ_i⁰

这里 l_i 为观测斜距与概略斜距之差，即残差向量（闭合差）的元素；u_i^T 为线性化系数矩阵（设计矩阵）的第 i 行。

数值示例：系数矩阵与残差向量计算

取一组教学示例数据（单位：米）。换能器 i 坐标 P_i = (10.0, 5.0, −2.0)，应答器概略坐标 X₀ = (100.0, 80.0, −50.0)，实测斜距 ρ_i^obs = 121.35 m。

import numpy as np

# 已知量
Pi = np.array([10.0, 5.0, -2.0])      # 换能器 i 坐标
X0 = np.array([100.0, 80.0, -50.0])   # 应答器概略坐标
rho_obs = 121.35                       # 实测斜距 (m)

# 概略斜距
d = X0 - Pi
rho0 = np.linalg.norm(d)
print("概略斜距 rho0 =", round(rho0, 4))

# 单位方向余弦（线性化系数行）
u = d / rho0
print("方向余弦 u =", np.round(u, 6))

# 残差元素
l = rho_obs - rho0
print("残差 l =", round(l, 4))

# 单应答器设计矩阵 A (1x3) 与残差向量 L (1x1)
A = u.reshape(1, 3)
L = np.array([l])
print("A =", np.round(A, 6))
print("L =", np.round(L, 4))

按上述输入可直接推出：ρ_i⁰ = √(90² + 75² + (−48)²) = √(8100 + 5625 + 2304) = √16029 ≈ 126.606 m；方向余弦 u ≈ (0.7109, 0.5924, −0.3791)；残差 l = 121.35 − 126.606 ≈ −5.256 m。设计矩阵 A = [0.7109, 0.5924, −0.3791]，残差向量 L = [−5.256]。

线性化有效性检查

一阶展开的截断误差量级约为 (‖δX‖/ρ⁰)² 乘以 ρ⁰。以上例为例，若真实位置与概略位置相差 5 m，则相对偏差约 5/126.6 ≈ 4%，二阶项贡献约 0.04² × 126.6 ≈ 0.2 m，对厘米级定位不可忽略，因此实际解算中应在每次迭代后用更新坐标重新计算 A 与 L，直至改正数收敛。判断收敛的常用条件是 ‖δX‖ 小于给定阈值（如 1 mm）。