三维定位中的最小二乘类算法：LS、WLS、NLS与WNLS
三维定位中的最小二乘类算法：LS、WLS、NLS与WNLS
1. 概述
在三维定位中，常见任务是利用多个已知位置的锚点（基站、UWB 基站、卫星等）与目标之间的测距值，反推目标的三维坐标。设第 $i$ 个锚点坐标为：
目标坐标为：
第 $i$ 个测距值为 $d_i$。理想情况下满足球面方程：
或写成距离形式：
其中 $\varepsilon_i$ 为测距误差。
LS、WLS、NLS、WNLS 都是围绕上述观测模型进行参数估计的方法，区别主要在于：
- 是否对原始非线性模型进行线性化；
- 是否对不同测量赋予不同权重；
- 是否需要迭代求解。
2. Least Squares（LS，最小二乘）
2.1 基本思想
原始球面方程对未知坐标 $(x,y,z)$ 是非线性的。经典 LS 方法通常通过方程相减消去二次项，将非线性方程转化为线性方程：
然后最小化残差平方和：
其闭式解为：
在三维定位中，理论上至少需要 4 个锚点，才能使线性化后的方程组完全确定。当锚点数量多于 4 个且存在测距误差时，LS 给出使残差平方和最小的坐标估计。
2.2 线性化推导：方程相减消去二次项
2.2.1 从原始球面方程出发
第 $i$ 个锚点坐标为 $(x_i,y_i,z_i)$，目标坐标为 $(x,y,z)$，测距为 $d_i$，满足：
展开：
把与锚点有关的常数项移到右边，并把 $x^2+y^2+z^2$ 单独拿出来：
令
则每个锚点给出一个方程：
此时未知量是 $(x,y,z,R^2)$，方程本身是线性的，但 $R^2$ 与 $x,y,z$ 不独立，直接解会多一个未知量。
2.2.2 选参考锚点做方程相减
选第 1 个锚点作为参考锚点。对 $i=2,3,\dots,N$，用第 $i$ 个方程减去第 1 个方程：
左边 $R^2-R^2=0$，二次项被消掉：
两边乘 $-1$ 并整理，得到标准的线性方程：
这就是第 $i$ 个线性方程，未知量只有 $x,y,z$。
2.2.3 写成矩阵形式
对 $i=2,3,\dots,N$ 各写一行，得到：
于是线性模型为：
其中 $\mathbf{x}=[x,y,z]^T$。
2.2.4 求解
- $N=4$（4 个锚点）：$A$ 是 $3\times3$ 方阵，若锚点不共面（$A$ 满秩），可直接求：
- $N>4$（锚点多于 4 个，存在测距误差）：$A$ 是 $(N-1)\times3$ 的超定矩阵，用最小二乘：
闭式解为：
这就是 LS 在三维定位中的标准线性化解。
2.2.5 一个 4 锚点示例
设 4 个锚点：
目标在 $\mathbf{x}=(0.2,0.3,0.4)$，无噪声测距：
以锚点 1 为参考：
解得：
与真实坐标一致。这个例子说明：无噪声、锚点几何良好时，线性化 LS 能精确恢复目标位置；有噪声时，它给出的是使线性化残差平方和最小的估计。
2.3 线性化过程中的注意点
参考锚点的选择会影响结果。
因为相减会把参考锚点的测距误差带入所有方程。工程上可以选测距质量较好的锚点作为参考，或者对多个参考锚点的结果做平均/择优。$A$ 的秩条件对应几何构型。
三维定位中 4 个锚点不能共面，否则 $A$ 不满秩，线性方程组无法唯一确定 $x,y,z$。线性化不是无损的。
相减消去的是 $R^2$ 这个二次项，但测距误差 $d_i^2$ 在相减后会被组合放大，且原始噪声的统计特性在平方后不再保持高斯性。因此线性 LS 的解通常不如 NLS/WNLS 精确，但胜在无需初值、计算快。另一种线性化思路（保留 $R^2$）：
不对第 1 个方程做相减，而是把 $R^2=x^2+y^2+z^2$ 当作第 4 个未知量，直接解得到 $(x,y,z,R^2)$ 的线性最小二乘解后，再施加约束 $R^2=x^2+y^2+z^2$ 进行修正（例如把解投影回二次曲面，或用约束最小二乘再优化一次）。这种做法避免了“选参考锚点”带来的偏差传播，但多了一个需要事后修正的约束。
2.4 优点
- 不需要初值；
- 计算简单，有闭式解；
- 实现方便，适合作为基础定位解或初值估计。
2.5 缺点
- 线性化过程会引入额外误差；
- 对所有测量一视同仁，不区分测量质量；
- 若用于方程相减的参考锚点误差较大，误差可能传播到整个解中。
3. Weighted Least Squares（WLS，加权最小二乘）
3.1 基本思想
WLS 在 LS 的线性模型基础上引入权重矩阵 $\mathbf{W}$，目标函数变为：
其闭式解为：
权重矩阵的核心含义是：测量越可靠，权重越大；测量越不可靠，权重越小。常见取法为：
其中 $\sigma_i^2$ 为第 $i$ 个测距值的误差方差。若各测量相互独立，则 $\mathbf{W}$ 可取对角矩阵：
在 NLOS（非视距）场景中，受遮挡锚点的测距偏差通常较大，可以通过降低其权重来抑制其对最终定位结果的拉偏作用。
3.2 优点
- 保持线性闭式解结构，计算效率较高；
- 能体现不同测量的质量差异；
- 对异常测量或 NLOS 测量有一定抑制能力。
3.3 缺点
- 需要事先知道或估计测量方差/协方差；
- 权重设置不合理时可能引入新的偏差；
- 仍然基于线性化模型，存在线性化误差。
4. Nonlinear Least Squares（NLS，非线性最小二乘）
4.1 基本思想
NLS 不先将模型线性化，而是直接最小化原始非线性距离模型的残差平方和：
由于距离函数对坐标是非线性的，NLS 通常采用泰勒展开与 Gauss–Newton 迭代求解。设当前估计为 $\mathbf{x}_k$，定义残差向量：
对距离函数在当前点做一阶泰勒近似，可得增量方程：
其中 $\mathbf{J}_k$ 为距离函数对 $\mathbf{x}$ 的雅可比矩阵。更新：
重复迭代，直到增量足够小或达到最大迭代次数。
4.2 优点
- 直接处理真实非线性模型，避免线性化带来的系统误差；
- 在初值合理时，精度通常优于线性 LS；
- 不要求已知测量误差的完整统计特性。
4.3 缺点
- 需要初值；
- 距离模型通常是非凸的，初值较差时可能收敛到局部极小；
- 计算量高于闭式 LS/WLS。
5. Weighted Nonlinear Least Squares（WNLS，加权非线性最小二乘）
5.1 基本思想
WNLS 是 NLS 的加权版本，目标函数为：
矩阵形式可写为：
其中：
- $\mathbf{m}$ 为测距向量；
- $\mathbf{h}(\mathbf{x})$ 为根据目标坐标计算出的距离向量；
- $\boldsymbol{\Sigma}$ 为测量误差协方差矩阵。
若各测量相互独立，则：
对应权重：
WNLS 仍采用迭代求解。其 Gauss–Newton 增量方程为：
其中：
更新方式与 NLS 相同：
5.2 统计意义
当各测量误差相互独立且服从高斯分布时，WNLS 解与最大似然估计（ML）一致。在误差模型正确的前提下，WNLS 能逼近 Cramér–Rao 下界，是这几类方法中理论上精度最高的选择之一。
5.3 优点
- 同时利用非线性模型和测量质量信息；
- 高斯噪声假设下具有较好的统计最优性；
- 适合对精度要求较高的三维定位场景。
5.4 缺点
- 需要初值；
- 需要估计测量方差/协方差；
- 计算量较大，且对初值和权重模型较敏感。
6. 四种方法对比
| 方法 | 是否线性化 | 是否使用权重 | 是否迭代 | 核心优势 | 主要限制 |
|---|---|---|---|---|---|
| LS | 是 | 否 | 否 | 闭式解、无需初值、计算简单 | 线性化误差；不区分测量质量 |
| WLS | 是 | 是 | 否 | 仍为闭式解，可抑制差测量 | 需要方差/协方差信息 |
| NLS | 否 | 否 | 是 | 直接处理非线性模型，精度优于线性 LS | 需要初值；可能局部收敛 |
| WNLS | 否 | 是 | 是 | 高斯噪声下逼近 ML/CRLB，精度较高 | 需要初值及协方差信息，计算量较大 |
7. 工程选用建议
实际三维定位项目中，可以根据计算资源、先验信息和精度要求选择方法：
快速获得粗估计
若需要快速得到定位结果，且没有可靠的误差统计信息，可优先使用 LS。存在明显坏测量或 NLOS
若能估计不同锚点的测距质量，可使用 WLS，通过权重抑制异常测量影响。精度要求较高且可提供初值
可使用 NLS，直接对非线性距离模型进行迭代优化。可估计测量方差/协方差
使用 WNLS，可在高斯误差假设下获得更接近最大似然估计的结果。常见工程组合
可先用 WLS 获得闭式初值，再将该初值送入 WNLS 进行迭代精化。这样既能缓解 WNLS 对初值的依赖，又能利用其对测量质量的建模能力。
8. 总结
从 LS 到 WLS、NLS、WNLS，本质上是在逐步引入更强的模型假设：
- LS：假设模型可线性化，所有测量同等重要；
- WLS：在线性模型上引入权重，区分测量可信度；
- NLS：放弃线性近似，直接优化原始非线性模型；
- WNLS：同时利用非线性模型与测量权重，在统计意义上更接近最优估计。
在实际三维定位中，方法选择取决于三个关键因素：能否提供可靠初值、能否估计测量误差统计特性，以及计算资源是否允许迭代求解。