三维定位中的最小二乘类算法：LS、WLS、NLS与WNLS

1. 概述

在三维定位中，常见任务是利用多个已知位置的锚点（基站、UWB 基站、卫星等）与目标之间的测距值，反推目标的三维坐标。设第 $i$ 个锚点坐标为：

$$\mathbf{a}_i=(x_i,y_i,z_i)$$

目标坐标为：

$$\mathbf{x}=(x,y,z)$$

第 $i$ 个测距值为 $d_i$。理想情况下满足球面方程：

$$(x-x_i)^2+(y-y_i)^2+(z-z_i)^2=d_i^2$$

或写成距离形式：

$$d_i=\|\mathbf{x}-\mathbf{a}_i\|+\varepsilon_i$$

其中 $\varepsilon_i$ 为测距误差。

LS、WLS、NLS、WNLS 都是围绕上述观测模型进行参数估计的方法，区别主要在于：

• 是否对原始非线性模型进行线性化；
• 是否对不同测量赋予不同权重；
• 是否需要迭代求解。

---

2. Least Squares（LS，最小二乘）

2.1 基本思想

原始球面方程对未知坐标 $(x,y,z)$ 是非线性的。经典 LS 方法通常通过方程相减消去二次项，将非线性方程转化为线性方程：

$$A\mathbf{x}=\mathbf{b}$$

然后最小化残差平方和：

$$\hat{\mathbf{x}}=\arg\min_{\mathbf{x}}\|A\mathbf{x}-\mathbf{b}\|_2^2$$

其闭式解为：

$$\hat{\mathbf{x}}=(A^TA)^{-1}A^T\mathbf{b}$$

在三维定位中，理论上至少需要 4 个锚点，才能使线性化后的方程组完全确定。当锚点数量多于 4 个且存在测距误差时，LS 给出使残差平方和最小的坐标估计。

2.2 线性化推导：方程相减消去二次项

2.2.1 从原始球面方程出发

第 $i$ 个锚点坐标为 $(x_i,y_i,z_i)$，目标坐标为 $(x,y,z)$，测距为 $d_i$，满足：

$$(x-x_i)^2+(y-y_i)^2+(z-z_i)^2=d_i^2$$

展开：

$$x^2-2xx_i+x_i^2+y^2-2yy_i+y_i^2+z^2-2zz_i+z_i^2=d_i^2$$

把与锚点有关的常数项移到右边，并把 $x^2+y^2+z^2$ 单独拿出来：

$$-2x_ix-2y_iy-2z_iz+(x^2+y^2+z^2) = d_i^2-(x_i^2+y_i^2+z_i^2)$$

令

$$R^2=x^2+y^2+z^2$$

则每个锚点给出一个方程：

$$-2x_ix-2y_iy-2z_iz+R^2 = d_i^2-(x_i^2+y_i^2+z_i^2) \tag{1}$$

此时未知量是 $(x,y,z,R^2)$，方程本身是线性的，但 $R^2$ 与 $x,y,z$ 不独立，直接解会多一个未知量。

2.2.2 选参考锚点做方程相减

选第 1 个锚点作为参考锚点。对 $i=2,3,\dots,N$，用第 $i$ 个方程减去第 1 个方程：

$$\big[-2x_ix-2y_iy-2z_iz+R^2\big] - \big[-2x_1x-2y_1y-2z_1z+R^2\big] = \big[d_i^2-(x_i^2+y_i^2+z_i^2)\big] - \big[d_1^2-(x_1^2+y_1^2+z_1^2)\big]$$

左边 $R^2-R^2=0$，二次项被消掉：

$$-2(x_i-x_1)x-2(y_i-y_1)y-2(z_i-z_1)z = d_i^2-d_1^2-(x_i^2+y_i^2+z_i^2)+(x_1^2+y_1^2+z_1^2)$$

两边乘 $-1$ 并整理，得到标准的线性方程：

$$2(x_i-x_1)x+2(y_i-y_1)y+2(z_i-z_1)z = d_1^2-d_i^2+(x_i^2+y_i^2+z_i^2)-(x_1^2+y_1^2+z_1^2) \tag{2}$$

这就是第 $i$ 个线性方程，未知量只有 $x,y,z$。

2.2.3 写成矩阵形式

对 $i=2,3,\dots,N$ 各写一行，得到：

$$A= \begin{bmatrix} 2(x_2-x_1) & 2(y_2-y_1) & 2(z_2-z_1)\\ 2(x_3-x_1) & 2(y_3-y_1) & 2(z_3-z_1)\\ \vdots & \vdots & \vdots\\ 2(x_N-x_1) & 2(y_N-y_1) & 2(z_N-z_1) \end{bmatrix}$$

$$\mathbf{b}= \begin{bmatrix} d_1^2-d_2^2+(x_2^2+y_2^2+z_2^2)-(x_1^2+y_1^2+z_1^2)\\ d_1^2-d_3^2+(x_3^2+y_3^2+z_3^2)-(x_1^2+y_1^2+z_1^2)\\ \vdots\\ d_1^2-d_N^2+(x_N^2+y_N^2+z_N^2)-(x_1^2+y_1^2+z_1^2) \end{bmatrix}$$

于是线性模型为：

$$A\mathbf{x}=\mathbf{b}$$

其中 $\mathbf{x}=[x,y,z]^T$。

2.2.4 求解

• $N=4$（4 个锚点）：$A$ 是 $3\times3$ 方阵，若锚点不共面（$A$ 满秩），可直接求：

$$\hat{\mathbf{x}}=A^{-1}\mathbf{b}$$

• $N>4$（锚点多于 4 个，存在测距误差）：$A$ 是 $(N-1)\times3$ 的超定矩阵，用最小二乘：

$$\hat{\mathbf{x}}=\arg\min_{\mathbf{x}}\|A\mathbf{x}-\mathbf{b}\|_2^2$$

闭式解为：

$$\hat{\mathbf{x}}=(A^TA)^{-1}A^T\mathbf{b}$$

这就是 LS 在三维定位中的标准线性化解。

2.2.5 一个 4 锚点示例

设 4 个锚点：

$$\mathbf{a}_1=(0,0,0),\quad \mathbf{a}_2=(1,0,0),\quad \mathbf{a}_3=(0,1,0),\quad \mathbf{a}_4=(0,0,1)$$

目标在 $\mathbf{x}=(0.2,0.3,0.4)$，无噪声测距：

$$d_1=\sqrt{0.2^2+0.3^2+0.4^2}=\sqrt{0.29}$$ $$d_2=\sqrt{(-0.8)^2+0.3^2+0.4^2}=\sqrt{0.89}$$ $$d_3=\sqrt{0.2^2+(-0.7)^2+0.4^2}=\sqrt{0.69}$$ $$d_4=\sqrt{0.2^2+0.3^2+(-0.6)^2}=\sqrt{0.49}$$

以锚点 1 为参考：

$$A= \begin{bmatrix} 2(1-0) & 2(0-0) & 2(0-0)\\ 2(0-0) & 2(1-0) & 2(0-0)\\ 2(0-0) & 2(0-0) & 2(1-0) \end{bmatrix} = \begin{bmatrix} 2&0&0\\0&2&0\\0&0&2 \end{bmatrix}$$

$$\mathbf{b}= \begin{bmatrix} d_1^2-d_2^2+(1^2+0+0)-(0)\\ d_1^2-d_3^2+(0+1^2+0)-(0)\\ d_1^2-d_4^2+(0+0+1^2)-(0) \end{bmatrix} = \begin{bmatrix} 0.29-0.89+1\\ 0.29-0.69+1\\ 0.29-0.49+1 \end{bmatrix} = \begin{bmatrix} 0.4\\0.6\\0.8 \end{bmatrix}$$

解得：

$$\hat{\mathbf{x}}= \begin{bmatrix} 0.2\\0.3\\0.4 \end{bmatrix}$$

与真实坐标一致。这个例子说明：无噪声、锚点几何良好时，线性化 LS 能精确恢复目标位置；有噪声时，它给出的是使线性化残差平方和最小的估计。

2.3 线性化过程中的注意点

1. 参考锚点的选择会影响结果。
   因为相减会把参考锚点的测距误差带入所有方程。工程上可以选测距质量较好的锚点作为参考，或者对多个参考锚点的结果做平均/择优。

2. $A$ 的秩条件对应几何构型。
   三维定位中 4 个锚点不能共面，否则 $A$ 不满秩，线性方程组无法唯一确定 $x,y,z$。

3. 线性化不是无损的。
   相减消去的是 $R^2$ 这个二次项，但测距误差 $d_i^2$ 在相减后会被组合放大，且原始噪声的统计特性在平方后不再保持高斯性。因此线性 LS 的解通常不如 NLS/WNLS 精确，但胜在无需初值、计算快。

4. 另一种线性化思路（保留 $R^2$）：
   不对第 1 个方程做相减，而是把 $R^2=x^2+y^2+z^2$ 当作第 4 个未知量，直接解

   $$\begin{bmatrix} -2x_1 & -2y_1 & -2z_1 & 1\\ \vdots & \vdots & \vdots & \vdots\\ -2x_N & -2y_N & -2z_N & 1 \end{bmatrix} \begin{bmatrix} x\\y\\z\\R^2 \end{bmatrix} = \begin{bmatrix} d_1^2-(x_1^2+y_1^2+z_1^2)\\ \vdots\\ d_N^2-(x_N^2+y_N^2+z_N^2) \end{bmatrix}$$

   得到 $(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}$，目标函数变为：

$$\hat{\mathbf{x}}=\arg\min_{\mathbf{x}}(A\mathbf{x}-\mathbf{b})^T\mathbf{W}(A\mathbf{x}-\mathbf{b})$$

其闭式解为：

$$\hat{\mathbf{x}}=(A^T\mathbf{W}A)^{-1}A^T\mathbf{W}\mathbf{b}$$

权重矩阵的核心含义是：测量越可靠，权重越大；测量越不可靠，权重越小。常见取法为：

$$w_i=\frac{1}{\sigma_i^2}$$

其中 $\sigma_i^2$ 为第 $i$ 个测距值的误差方差。若各测量相互独立，则 $\mathbf{W}$ 可取对角矩阵：

$$\mathbf{W}=\mathrm{diag}(w_1,w_2,\dots,w_N)$$

在 NLOS（非视距）场景中，受遮挡锚点的测距偏差通常较大，可以通过降低其权重来抑制其对最终定位结果的拉偏作用。

3.2 优点

• 保持线性闭式解结构，计算效率较高；
• 能体现不同测量的质量差异；
• 对异常测量或 NLOS 测量有一定抑制能力。

3.3 缺点

• 需要事先知道或估计测量方差/协方差；
• 权重设置不合理时可能引入新的偏差；
• 仍然基于线性化模型，存在线性化误差。

---

4. Nonlinear Least Squares（NLS，非线性最小二乘）

4.1 基本思想

NLS 不先将模型线性化，而是直接最小化原始非线性距离模型的残差平方和：

$$\hat{\mathbf{x}}=\arg\min_{\mathbf{x}}\sum_{i=1}^{N}\left(d_i-\|\mathbf{x}-\mathbf{a}_i\|\right)^2$$

由于距离函数对坐标是非线性的，NLS 通常采用泰勒展开与 Gauss–Newton 迭代求解。设当前估计为 $\mathbf{x}_k$，定义残差向量：

$$\mathbf{r}(\mathbf{x})= \begin{bmatrix} d_1-\|\mathbf{x}-\mathbf{a}_1\|\\ d_2-\|\mathbf{x}-\mathbf{a}_2\|\\ \vdots\\ d_N-\|\mathbf{x}-\mathbf{a}_N\| \end{bmatrix}$$

对距离函数在当前点做一阶泰勒近似，可得增量方程：

$$\mathbf{J}_k^T\mathbf{J}_k\Delta\mathbf{x} = \mathbf{J}_k^T\mathbf{r}(\mathbf{x}_k)$$

其中 $\mathbf{J}_k$ 为距离函数对 $\mathbf{x}$ 的雅可比矩阵。更新：

$$\mathbf{x}_{k+1}=\mathbf{x}_k+\Delta\mathbf{x}$$

重复迭代，直到增量足够小或达到最大迭代次数。

4.2 优点

• 直接处理真实非线性模型，避免线性化带来的系统误差；
• 在初值合理时，精度通常优于线性 LS；
• 不要求已知测量误差的完整统计特性。

4.3 缺点

• 需要初值；
• 距离模型通常是非凸的，初值较差时可能收敛到局部极小；
• 计算量高于闭式 LS/WLS。

---

5. Weighted Nonlinear Least Squares（WNLS，加权非线性最小二乘）

5.1 基本思想

WNLS 是 NLS 的加权版本，目标函数为：

$$\hat{\mathbf{x}}=\arg\min_{\mathbf{x}} \sum_{i=1}^{N}w_i\left(d_i-\|\mathbf{x}-\mathbf{a}_i\|\right)^2$$

矩阵形式可写为：

$$\hat{\mathbf{x}}=\arg\min_{\mathbf{x}} \left(\mathbf{m}-\mathbf{h}(\mathbf{x})\right)^T \boldsymbol{\Sigma}^{-1} \left(\mathbf{m}-\mathbf{h}(\mathbf{x})\right)$$

其中：

• $\mathbf{m}$ 为测距向量；
• $\mathbf{h}(\mathbf{x})$ 为根据目标坐标计算出的距离向量；
• $\boldsymbol{\Sigma}$ 为测量误差协方差矩阵。

若各测量相互独立，则：

$$\boldsymbol{\Sigma}=\mathrm{diag}(\sigma_1^2,\sigma_2^2,\dots,\sigma_N^2)$$

对应权重：

$$w_i=\frac{1}{\sigma_i^2}$$

WNLS 仍采用迭代求解。其 Gauss–Newton 增量方程为：

$$\mathbf{J}_k^T\mathbf{W}\mathbf{J}_k\Delta\mathbf{x} = \mathbf{J}_k^T\mathbf{W}\mathbf{r}(\mathbf{x}_k)$$

其中：

$$\mathbf{W}=\boldsymbol{\Sigma}^{-1}$$

更新方式与 NLS 相同：

$$\mathbf{x}_{k+1}=\mathbf{x}_k+\Delta\mathbf{x}$$

5.2 统计意义

当各测量误差相互独立且服从高斯分布时，WNLS 解与最大似然估计（ML）一致。在误差模型正确的前提下，WNLS 能逼近 Cramér–Rao 下界，是这几类方法中理论上精度最高的选择之一。

5.3 优点

• 同时利用非线性模型和测量质量信息；
• 高斯噪声假设下具有较好的统计最优性；
• 适合对精度要求较高的三维定位场景。

5.4 缺点

• 需要初值；
• 需要估计测量方差/协方差；
• 计算量较大，且对初值和权重模型较敏感。

---

6. 四种方法对比

方法	是否线性化	是否使用权重	是否迭代	核心优势	主要限制
LS	是	否	否	闭式解、无需初值、计算简单	线性化误差；不区分测量质量
WLS	是	是	否	仍为闭式解，可抑制差测量	需要方差/协方差信息
NLS	否	否	是	直接处理非线性模型，精度优于线性 LS	需要初值；可能局部收敛
WNLS	否	是	是	高斯噪声下逼近 ML/CRLB，精度较高	需要初值及协方差信息，计算量较大

---

7. 工程选用建议

实际三维定位项目中，可以根据计算资源、先验信息和精度要求选择方法：

1. 快速获得粗估计
   若需要快速得到定位结果，且没有可靠的误差统计信息，可优先使用 LS。

2. 存在明显坏测量或 NLOS
   若能估计不同锚点的测距质量，可使用 WLS，通过权重抑制异常测量影响。

3. 精度要求较高且可提供初值
   可使用 NLS，直接对非线性距离模型进行迭代优化。

4. 可估计测量方差/协方差
   使用 WNLS，可在高斯误差假设下获得更接近最大似然估计的结果。

5. 常见工程组合
   可先用 WLS 获得闭式初值，再将该初值送入 WNLS 进行迭代精化。这样既能缓解 WNLS 对初值的依赖，又能利用其对测量质量的建模能力。

---

8. 总结

从 LS 到 WLS、NLS、WNLS，本质上是在逐步引入更强的模型假设：

• LS：假设模型可线性化，所有测量同等重要；
• WLS：在线性模型上引入权重，区分测量可信度；
• NLS：放弃线性近似，直接优化原始非线性模型；
• WNLS：同时利用非线性模型与测量权重，在统计意义上更接近最优估计。

在实际三维定位中，方法选择取决于三个关键因素：能否提供可靠初值、能否估计测量误差统计特性，以及计算资源是否允许迭代求解。