3. 基于最小二乘的SBL单点定位解算

问题设定与观测数据

本节以船底换能器阵列对海底多个应答器进行斜距测量为背景，完成一次SBL单点三维坐标的最小二乘解算。设换能器阵列在船体坐标系下的坐标为已知量，目标（应答器或水下目标）的概略坐标为 X0 = (x0, y0, z0)，通过测量得到目标到各换能器的斜距观测值 ρi。解算任务即：由这些斜距观测值反求目标的三维坐标估值，并给出残差以评估拟合质量。

为便于复现，采用一组教学示例数据（示例值，非实测）：4 个换能器在船体坐标系下的位置与目标概略坐标如下表，斜距观测值由“真实坐标 + 微小观测噪声”构造，用于演示解算流程与残差判读。

目标概略坐标取 X0 = (30.000, 40.000, 5.000)（m）。真实坐标设为 (30.000, 40.000, 5.000) 附近，观测噪声量级约 1 cm，用于检验解算能否收敛到合理估值。

最小二乘平差模型

斜距观测方程为非线性形式：

ρi = sqrt((x - Xi)^2 + (y - Yi)^2 + (z - Zi)^2)

在概略坐标 X0 处线性化，令 δ = (δx, δy, δz) 为坐标改正数，则观测方程可写为：

v_i = A_i · δ - l_i

其中 A_i 为第 i 个观测的系数行向量（单位视线方向），l_i = ρi - ρi(X0) 为观测值与概略值之差（闭合差）。等权最小二乘解为：

δ = (AᵀA)⁻¹ Aᵀ l

迭代更新 X = X0 + δ，直至 |δ| 小于阈值。本节采用单次迭代演示（概略坐标已较接近真值），并输出残差 v = A·δ - l。

解算实现（Python）

以下代码完整实现上述流程：构造观测、线性化、法方程求解、坐标更新与残差输出。所有输入均在本例中定义，可直接运行。

import numpy as np

# 换能器坐标（船体坐标系，m）
T = np.array([
    [0.0, 0.0, 0.0],
    [2.0, 0.0, 0.0],
    [0.0, 2.0, 0.0],
    [0.0, 0.0, 1.5],
])

# 斜距观测值（示例数据，m）
rho_obs = np.array([50.012, 50.048, 50.043, 50.034])

# 概略坐标
X0 = np.array([30.0, 40.0, 5.0])

def solve_ls(T, rho_obs, X0, max_iter=10, tol=1e-4):
    X = X0.copy()
    for it in range(max_iter):
        # 概略斜距
        d = X[None, :] - T            # (n,3)
        rho0 = np.linalg.norm(d, axis=1)
        # 系数矩阵 A：单位视线方向
        A = d / rho0[:, None]         # (n,3)
        # 闭合差 l
        l = rho_obs - rho0            # (n,)
        # 法方程
        N = A.T @ A
        w = A.T @ l
        delta = np.linalg.solve(N, w)
        X = X + delta
        # 残差
        v = A @ delta - l
        print(f"iter {it}: delta={delta}, |delta|={np.linalg.norm(delta):.6f}")
        if np.linalg.norm(delta) < tol:
            break
    return X, v, A, l

X_hat, v, A, l = solve_ls(T, rho_obs, X0)
print("坐标估值 X_hat =", X_hat)
print("残差 v =", v)
print("残差平方和 =", float(v @ v))

结果判读

运行上述代码（示例数据）可得到坐标估值与残差。判读要点如下：

• 坐标增量：若 |δ| 在首次迭代后已小于 1e-4 m，说明概略坐标足够接近真值，单次迭代即可收敛；否则应继续迭代。
• 残差量级：残差 v 应处于观测噪声量级（本例约 1 cm）。若残差显著偏大（如分米级），说明几何构型不良或存在粗差观测。
• 法方程条件：若 AᵀA 接近奇异（换能器共面且目标位于该平面附近），解算不稳定，需检查阵列构型。