在工业物联网、仓储机器人与室内导航场景中，GPS 信号往往无法穿透建筑结构，而视觉定位又受光照与遮挡限制。此时，基于距离观测的定位方案 —— 通过测量目标与多个固定锚点之间的距离来推算位置 —— 成为了一种低成本且可靠的替代方案。本文将系统阐述如何仅用距离观测与运动向量实现二维定位，涵盖几何约束求解、卡尔曼滤波融合以及多假设追踪的工程实现细节。
几何约束求解：从三边测量到最小二乘
三边测量（Trilateration）是距离定位的核心算法。假设有三个锚点分别位于坐标 $(x_1, y_1)$、$(x_2, y_2)$、$(x_3, y_3)$，测得到目标的距离分别为 $d_1$、$d_2$、$d_3$，则目标位置 $(x, y)$ 满足以下约束方程组：
$$ (x - x_i)^2 + (y - y_i)^2 = d_i^2, \quad i = 1, 2, 3 $$
直接求解这个二次方程组较为复杂。工程上通常采用两两相减的方式消去二次项，将其转化为线性系统。以前两个方程为例，相减后得到：
$$ 2(x_2 - x_1)x + 2(y_2 - y_1)y = d_1^2 - d_2^2 - (x_1^2 - x_2^2) - (y_1^2 - y_2^2) $$
同理可得第二组线性方程。将两式联立，即可求解出目标位置。这种方法在三个锚点不共线时具有唯一解。
然而，实际工程环境中往往存在超过三个锚点，且距离测量不可避免地带有噪声。此时应采用最小二乘法进行求解。对于 $N$ 个锚点（$N \geq 3$），构建矩阵方程 $Ap = b$，其中 $p = [x, y]^T$ 为待求位置，矩阵 $A$ 和向量 $b$ 由锚点几何位置与测量距离构造。最小二乘解为：
$$ p = (A^T A)^{-1} A^T b $$
这种处理方式能够有效平滑测量噪声，提高定位精度。需要注意的是，锚点的几何布局对求解稳定性至关重要 —— 当锚点接近共线或目标远离锚点集群时，矩阵 $A^T A$ 可能接近奇异，导致误差放大。
卡尔曼滤波融合：运动模型与观测的权重博弈
单纯依赖距离观测进行定位存在两个局限：一是测量频率受限，二是单次测量误差可能导致位置跳变。引入卡尔曼滤波器可以将运动模型与观测数据融合，实现平滑且鲁棒的轨迹追踪。
状态空间定义
对于二维平面运动，状态向量通常定义为：
$$ s = [x, y, v_x, v_y]^T $$
其中 $(x, y)$ 为位置，$(v_x, v_y)$ 为速度。采用常速度模型（Constant Velocity Model），离散时间下的状态转移方程为：
$$ \begin{aligned} x_k &= x_{k-1} + v_{x,k-1} \cdot \Delta t \ y_k &= y_{k-1} + v_{y,k-1} \cdot \Delta t \ v_{x,k} &= v_{x,k-1} \ v_{y,k} &= v_{y,k-1} \end{aligned} $$
对应的状态转移矩阵 $F$ 为：
$$ F = \begin{bmatrix} 1 & 0 & \Delta t & 0 \ 0 & 1 & 0 & \Delta t \ 0 & 0 & 1 & 0 \ 0 & 0 & 0 & 1 \end{bmatrix} $$
过程噪声与观测噪声
过程噪声协方差矩阵 $Q$ 反映了运动模型的不确定性。若系统配备加速度计，可将加速度作为控制输入，此时 $Q$ 通过控制输入矩阵 $G$ 计算：
$$ G = \begin{bmatrix} \Delta t^2/2 & 0 \ 0 & \Delta t^2/2 \ \Delta t & 0 \ 0 & \Delta t \end{bmatrix}, \quad Q = G \cdot W \cdot G^T $$
其中 $W = \text {diag}(\sigma_{ax}^2, \sigma_{ay}^2)$ 为加速度噪声协方差。观测噪声协方差 $R$ 则取决于距离测量精度，通常需要通过标定实验确定。
滤波流程
每个时间步的滤波流程包含预测与更新两个阶段：
预测阶段： $$ \hat{x}{k|k-1} = F \hat{x}{k-1|k-1}, \quad P_{k|k-1} = F P_{k-1|k-1} F^T + Q $$
更新阶段： $$ K_k = P_{k|k-1} H^T (H P_{k|k-1} H^T + R)^{-1} \ \hat{x}{k|k} = \hat{x}{k|k-1} + K_k (z_k - H \hat{x}{k|k-1}) \ P{k|k} = (I - K_k H) P_{k|k-1} $$
观测矩阵 $H$ 取决于观测类型：仅观测位置时 $H = \begin {bmatrix} I_2 & 0_2 \end {bmatrix}$，同时观测位置与速度时 $H = I_4$。
卡尔曼滤波器的一个显著优势是具备 "预测 - 更新" 的容错机制。当距离观测暂时丢失时，系统可仅依靠预测步骤继续估算轨迹，虽然误差会随时间累积，但不会出现定位跳变或系统崩溃。
多假设追踪：应对歧义的工程策略
距离观测存在一个固有的几何歧义问题：仅通过距离无法区分目标位于锚点连线的哪一侧。当目标穿越锚点构成的几何边界时，三边测量可能产生镜像解。多假设追踪（Multiple Hypothesis Tracking, MHT）是应对这一挑战的有效策略。
工程实现中，可以为每个时刻维护多个候选假设，每个假设包含一个状态估计及其置信度。当新的距离观测到达时，计算各假设与新观测的匹配程度，保留概率较高的假设分支，剪枝低概率分支。具体而言，可采用以下启发式规则：
- 连续性约束：目标在短时间内不可能发生大幅度位置跳变，剔除与上一时刻估计距离过远的假设
- 速度合理性：剔除导致超物理速度（如超过最大运动速度）的假设
- 几何一致性：检查假设位置与各锚点距离测量的一致性，残差过大的假设予以剔除
当歧义无法通过单帧观测消除时，保留多个假设并等待后续观测提供更多信息。这种延迟决策策略在目标穿越复杂几何区域时尤为重要。
可落地参数清单与调优指南
以下参数配置经过实际工程验证，可作为初始调参基准：
| 参数 | 符号 | 推荐值 | 调优方向 |
|---|---|---|---|
| 采样周期 | $\Delta t$ | 0.05–0.1s | 根据传感器刷新率设定 |
| 初始位置不确定度 | $\sigma_{pos,0}$ | 1.0–5.0m | 先验知识越精确，取值越小 |
| 初始速度不确定度 | $\sigma_{vel,0}$ | 0.5–2.0m/s | 目标静止时取小值 |
| 加速度噪声 | $\sigma_a$ | 0.3–0.5 m/s² | 运动越剧烈，取值越大 |
| 距离测量噪声 | $\sigma_d$ | 0.1–0.5m | 取决于测距硬件精度 |
| 最大假设数 | $N_{hyp}$ | 3–5 | 计算资源充足时可增大 |
调优原则：
- 增大 $Q$ 表示降低对运动模型的信任，允许状态更自由地变化，适用于目标运动模式不确定的场景
- 增大 $R$ 表示降低对观测的信任，滤波结果更依赖预测，适用于测量噪声大的场景
- 若定位结果滞后明显，可适当增大过程噪声或减小观测噪声，提高观测权重
总结
仅用距离观测实现二维定位是一个经典的传感器融合问题。三边测量提供了几何约束求解的基础，最小二乘法扩展使其能够处理多锚点与噪声场景，卡尔曼滤波器引入运动模型实现平滑追踪，而多假设追踪则解决了距离观测固有的歧义性问题。
这套技术栈的价值在于其硬件无关性 —— 无论是 UWB、蓝牙 RSSI、超声波还是激光测距，只要能够获得距离信息，都可以接入同一套算法框架。对于资源受限的嵌入式系统，可以进一步简化：仅保留位置状态（$[x, y]^T$）的卡尔曼滤波器，或将二维问题分解为两个独立的一维滤波器，显著降低计算开销。
参考资料
- Cookie Robotics. "Kalman Filter for 2D Motion." 提供了完整的 Python 实现与测试用例，涵盖静止、匀速、加速三种场景的仿真验证。
- Perplexity Research. "Distance-based 2D localization with trilateration and Kalman filter." 汇总了最小二乘三边测量与卡尔曼滤波的理论框架及矩阵推导。
内容声明：本文无广告投放、无付费植入。
如有事实性问题，欢迎发送勘误至 i@hotdrydog.com。