基于到达时间量测的多传感器协同定位算法研究

方艺忠，姜浩楠，陈 旸，蔡远利，孟 刚

（1. 北京航天长征飞行器研究所，北京，100076；2. 试验物理与计算数学国家重点实验室，北京，100076；3. 西安交通大学电子与信息学部，西安，710049；4. 中国运载火箭技术研究院，北京，100076）

0 引 言

无线定位系统通常需要两个或者更多的传感器来对目标位置进行捕获。常见的定位方法通常基于到达时间（Time of Arrival，TOA）、到达方位（Angle of Arrival，AOA）、接收信号强度（Received Signal Strength，RSS）和到达时间差（Time Difference of Arrival，TDOA）中的一种或多种量测信息对目标的位置进行估计。由于在无线通信系统、检测系统以及导航系统中的广泛应用，基于TOA 等量测信息的目标定位问题得到了广泛的关注和研究。本文主要研究基于TOA 量测的多传感器协同定位算法。

TOA 协同定位的目标是根据分布在不同位置的传感器获得的目标信号到达时间量测（当信号传播速度为1 时，到达时间量测即为距离量测）来估计目标的位置，本质上是一类优化问题。不同定位方法的主要区别之一是目标函数不同，常用的目标函数包括距离最小二乘（Range Least Squares，RLS）和距离平方最小二乘（Squared Range Least Squares，SRLS）。大部分解析算法都是采用SRLS 作为目标函数，将欧氏距离转换成为更容易处理的二阶多项式的形式，保证了最优解的存在性。然而，数值迭代算法一般都采用RLS 作为目标函数，从一个给定的初始估计开始，不断逼近最优解。在量测噪声服从零均值高斯分布的假设下，RLS 得到的解和最大似然估计解一致。除此以外，还有一些优化工具箱可以给出RLS 意义下的最优解，如半正定松弛（Semidefinite Relaxation，SDR）[和多项式优化（Polynomial Optimization，POP）等。这一类方法可以以任意精度逼近最大似然估计，缺点是计算时间过长。

本文主要研究TOA 定位算法中的解析算法和数值迭代算法。解析算法通常是在原始问题上放松了一些条件或者运用一些巧妙的数学变换，推导出目标位置的直接结果；数值迭代算法则是从一个初始估计开始，不断逼近全局最优解，这通常会消耗非常多的时间，不便于实际应用。然而，数值迭代算法也有着多方面的优势，如适用于任意数量任意类型的量测信息、编程相对简单、不需要放松任何条件以及容易扩展和推广等。本文利用热图分析了两类算法在不同目标位置下的表现，随后在不同量测噪声强度条件下对比了多种定位算法的估计效果，能够对不同场景下定位算法的选择提供帮助。同时，通过对迭代算法进行适当调整，可以改善算法的鲁棒性，提升算法在量测噪声强度较大环境下的估计效果，达到与解析算法同样好的估计精度。

1 问题描述

假定为二维平面内目标的位置，s (= 1,2,…,)为第个传感器的位置，各个传感器获取的信号源到自身位置的时间量测信息依次为,… , T。TOA 定位的目标是：在传感器位置,…,和传感器量测,… , T给定的情况下，估计目标的位置。为了简便，本文假定信号的传播速度为1，则TOA 量测即为距离量测。

由此量测方程可以写为

依据极大似然准则，TOA 定位问题的解可通过下式求得：

等价于：

通过式（3）求得的最小二乘解是在方差意义下的最优解。

2 解析算法

本小节介绍一些解决TOA 定位问题的解析算法。尽管解析算法在推导方面较为复杂，但其不需要初始估计且能够保证结果的收敛性，相比于数值迭代算法得到的结果更加精确和稳定。

2.1 基线法

基线法通过计算所有传感器位置坐标的均值作为目标位置的估计：

从计算公式可以看出，基线法得到的目标位置估计不依赖于任何量测信息。在平均意义下，可以判断，基线法的估计效果较差。

2.2 Simple Intersection 算法

从几何上理解，该算法考虑−1组圆，用直线代替它们的交会部分。由于消除掉了非线性项，得到的结果等价于在少了一个量测的情况下对目标进行估计的结果。

采用Simple Intersection 算法得到的目标位置估计为

2.3 Bancroft 算法

式（7）整合到一起，可以得到：

即：

式（9）左右两边同时乘以矩阵的Moore–Penrose伪逆，可以得到：

两边同时平方可以得到：

等价于：

Bancroft 算法的具体流程为：

a）计算向量= ()1；

b）计算向量= ()；

c）求解方程（14）的两个根；

2.4 Beck 算法

Beck 等人于 2008 年提出了一种精确的least-quartic 算法。在求解的过程中，需要使用二分法寻找单变量严格单调函数的根，因此得到的并不是严格意义上的解析解，但这种算法能够保证估计效果收敛。具体算法流程如下：

3 数值迭代算法

数值迭代算法的典型代表是高斯-牛顿算法，又称为加权迭代最小二乘算法。高斯-牛顿算法在量测噪声较大的情况下无法避免估计发散的问题。为了克服这一缺陷，对传统的高斯-牛顿算法进行修正，得到正则化的高斯-牛顿算法。

3.1 高斯-牛顿算法

高斯-牛顿算法流程如下：

a）选择合理的初始估计值和算法中止参数。设置=0 。

b）计算雅可比矩阵：

c）迭代更新=+Δ，其中，

式中为量测噪声协方差矩阵。

3.2 正则化高斯-牛顿算法

相比于原始的高斯-牛顿算法，正则化高斯-牛顿算法的不同仅在于Δx的设置，其具体流程如下：

a）选择合理的初始估计值和算法中止参数。设置=0 ；

b）计算雅可比矩阵：

c）迭代更新=+Δ，其中，

式中为正则化系数； x为正则化位置。

4 热图分析法

为了更全面地刻画不同种类算法的估计效果，本小节利用热图分析解析算法（以Beck 算法为例）和数值算法（以正则化高斯-牛顿算法为例）针对不同位置目标的估计误差。热图起源于数据矩阵中数值的二维显示，颜色越浅，表明该算法对位于当前方格的目标定位效果越好；颜色越深，表明该算法对位于当前方格的目标定位效果越差。

在量测噪声标准差为100 m 的情况下，Beck 算法和正则化高斯-牛顿算法得到的均方根误差（Root Mean Square Error，RMSE）关于目标位置热图如图1和图2 所示。可以发现，解析算法和数值算法针对不同位置目标的估计效果非常相似，数值算法的估计效果丝毫不亚于解析算法。

图1 Beck 算法均方根误差（RMSE）关于目标位置的热图Fig.1 Heat Map of RMSE of Beck Algorithm with Respect to Target Position

图2 正则化高斯-牛顿算法均方根误差（RMSE）关于目标位置的热图Fig.2 Heat Map of RMSE of Regularized Gauss-Newton Algorithm with Respect to Target Position

5 仿真分析

本小节针对多传感器TOA 定位问题，对比不同解析算法和数值迭代算法的估计效果。

仿真结果如图3 所示。Michael提出的Partitioning算法能够给出R-LS 准则下的全局最优解，可以用于性能评估。

图3 TOA 算法效果对比Fig.3 Comparison of TOA Algorithms

Beck 算法、Cheung 算法和Partitioning 算法等在不同量测噪声强度下均能保持较高的定位精度，对噪声的敏感性较弱。基线法、Simple Intersection 算法和Bancroft 算法的估计效果较差。当量测噪声标准差小于300 m 时，其余几种算法几乎能够达到一致且最优的估计效果。当量测噪声标准差达到2 ×10 m 时，原始的高斯-牛顿算法得到的误差曲线开始发散，表明其在较大量测噪声强度下估计效果不能得到保证，而正则化高斯-牛顿算法在高强度噪声环境下依然能保证与优异的解析算法几乎一致的估计效果，展现了其更强的鲁棒性。

在噪声标准差达到 6 ×10~10m 时，可从图3 中看出基线法的估计效果反而是最好的，说明在量测噪声强度过大时，最好的估计策略是摒弃所有得到的量测信息，直接利用先验信息对目标的位置进行估计。

6 结 论

本文研究了基于到达时间量测的多传感器定位问题，其本质上是一类优化问题，可以通过解析算法或者数值迭代算法进行求解。以Beck 算法为代表的解析算法虽然能够保证估计结果收敛，但通常需要在多种假设和前提下才能得到闭式解，求解过程较为复杂；以高斯-牛顿算法为代表的数值迭代算法结构简单、易于扩展和推广，但其需要初始估计且无法保证结果收敛。在实际应用中，如果能够获得较为准确的先验信息同时对实时性要求不高，建议采用数值迭代算法；反之，Beck 算法等解析算法是更好的选择。本文对比了多种较为经典和有效的TOA 定位算法，同时利用热图分析了算法在不同目标位置下的估计效果，能够为TOA 定位问题的算法选择提供帮助。