基于双向测距的协同定位系统误差分析

徐 望， 陈恒智， 阚 磊

(1.海军上海航保修理厂，上海 200083； 2.中国空间技术研究院 载人航天总体部，北京 100094；3.陆装驻重庆地区第六军代室，重庆 400010)

协同定位的概念早在20世纪60年代就已被提出，该思想早期主要被用于卫星组网的自主导航系统。随后物理学家哈肯又创立了协同理论，该理论的含义主要是在一个庞大的系统中，若其中各个子系统都能够相互分享信息并协调配合，就可使整体系统获得更强的功能。该理论的提出使多节点协同定位技术作为新的研究方向受到了广泛的关注[1-3]，其应用的平台包括无人机、无人水下潜器以及其他各种陆用平台等。此外，各相关平台通过信息的交互协同使工作范围和工作稳定性得到大幅提升。例如，在战争环境下，部队的通信、指挥、调度系统时刻面临着高时变、高动态和高随机性的挑战，而部队的大部分传感器依赖于GNSS(Global Navigation Satellite System，全球导航卫星系统)的辅助，恶劣的战场环境很有可能导致GNSS信号拒止或不可靠。此时，协同定位系统通过组网结合各传感器信息，从而提升信息的冗余性，进而提高了导航传感器定位精度和可靠性。正是由于协同定位系统具有这一优点，相关研究者们希望其在自身的研究领域中有所应用。多节点协同定位系统有以下诸多优势[4]：① 系统中部分节点的高精度定位信息可以传递至定位精度较低的节点，用以提高低精度节点的定位性能；② 如果系统中的部分节点的定位误差有界，那么通过组网信息共享能够使系统的每个节点的定位误差都有界；③ 当系统中的部分节点由于不同因素丧失自主定位能力时，通过多节点协同定位能够在一定程度上恢复这些节点的导航定位能力。

为获得更高的协同定位精度，国内外学者进行了有针对性的研究。Tang等[5]提出了一种采用激光雷达和视觉传感器的协同导航方案，实现了较高的定位精度，但其设备高昂的价格限制了该方案的实际推广。Lim等[6]通过组合低成本GPS和基于超宽带的双向测距(Two-Way Ranging,TWR)进行协同定位，实现了1 m以内的定位精度，具有较好的大规模工程应用前景，因此本文在该方案的硬件配置基础上进行研究分析。

协同定位系统的基础组成单元为移动节点，移动节点根据系统控制信号可分别作为锚节点[7]和标签节点，其中锚节点是自身位置准确已知的移动节点，标签节点是待定位移动节点，该类节点可利用与锚节点间的距离通过三角定位等方法来完成自身定位。另外，协同定位系统可以通过改变移动节点工作模式来扩展锚节点分布和定位范围，例如当一个标签节点通过锚节点或GPS获取其准确位置后，该标签节点的工作模式即可更改为锚节点。

锚节点工作模式下的移动节点在测量与标签节点间的距离进行定位的过程中，会将自身定位误差传播给其定位的标签节点。而即使忽略测距噪声的影响，锚节点的位置误差仍然会在双向测距过程中带来额外的误差。因此，研究如何降低基于双向测距的协同定位网络中标签节点的定位误差必须考虑精度因子(Dilution of Precision，DOP)和锚节点几何分布的影响。同时，由于协同定位系统中锚节点的几何分布一般是时变的，因而集群编队控制[8-9]也是协同定位的关键问题之一。

本文主要分析了基于双向测距的协同定位系统误差传播机理。首先，对距离量测的误差源进行了总结。另外，分析推导了由测距误差带来的协同定位系统中的定位误差。最后，通过在不同场景下进行仿真展现了锚节点几何分布对系统误差传播的影响。

1 协同定位系统误差源

以无人机集群编队飞行需确定精确的相对位置关系为背景，对基于双向测距的协同定位系统的误差进行了研究。由于二维空间条件下误差问题的分析可以较为容易地推广至三维空间，因此文中的分析主要从二维空间展开。不同场景下协同定位系统示意图如图1所示。

图1 不同场景下协同定位系统示意图

从图1场景1中可以看出，所有移动节点均通过GPS修正自身位置，随后场景2中移动节点5进入GPS拒止区域。在场景3中，移动节点5通过与其他已知确切位置的移动节点完成双向距离测量，并确定自身位置成为锚节点。在场景4中，移动节点1进入GPS拒止区域，并利用锚节点5及其他节点的测距信息定位自身位置。

1.1 测距误差

基于无线电测距方式的实现一般包括单向测距和双向测距。假设需要完成移动节点A和移动节点B之间的距离测量，单向测距方式是通过移动节点A将数据包发送到移动节点B，同时计算信号飞行时间，实现该方法需要全局同步高精度晶振，因此价格较为高昂。而双向测距方式具体的实现流程如图2所示。定位标签会在定位开始时发送一个数据包，同时记录下此刻的发送时间T1，安装在某一确定位置的定位基站在正常的工作模式下会接收到这个数据包，等待一段时间后会发送应答数据包，该数据包包含了这段等待时间T_replyB，标签在接收到应答包后，记录此时的时间T2，得到T_RoundA=T2-T1，然后经过等待时间T_replyA发送最终的数据，进行一次与前一阶段相同的通信过程。该测距方式利用两次通信的时间来对时钟偏移引入的误差进行补偿，解决了时钟不统一的问题。

图2 双向测距方式示意图

另外，锚节点的定位精度也会对带定位标签节点的定位精度产生影响。若锚节点定位存在误差，那么即使是无测量噪声的测距值，也与标签节点真实位置和锚节点估计位置之间的标称距离不相等，这一误差是协同定位系统中测距误差的必要组成部分。Lim[10]将测距误差的协方差矩阵Q表示为：

(1)

式中，Pi为第i个锚节点的协方差矩阵；σ2为距离量测的方差；IN为N×N维单位矩阵；hi为从标签节点到第i个节点的方向余弦向量，可以表示为

(2)

其中，(xi,yi,zi)为第i个锚节点的位置；(x,y,z)为标签节点位置；ri为两节点间的距离。由式(1)可以看出，测距精度主要取决于测距噪声以及锚节点的位置估计误差这两个部分。

1.2 位置误差

标签节点的定位过程主要是通过结合测距信息及锚节点的位置信息来实现，其定位误差的协方差矩阵P可由以下公式推出：

P=cov(Δx)

=(HTQ-1H)-1HTQ-1cov(Δr){Q-1HT(HTQ-1H)-1}T

=(HTQ-1H)-1

(3)

式中，Δx=(HTQ-1H)-1HTQ-1Δr，H为标签节点的观测矩阵，Q为测距误差协方差矩阵。在协同定位系统中，Q矩阵通常不是单位矩阵，这与有关精度因子的假设有所区别。将P引用为加权精度因子，与定位误差协方差矩阵取值相同。

1.3 移动场景下误差

在对标签节点的位置进行估计的过程中，锚节点是瞬态静止的，直至完成对标签节点间的距离量测与定位后方可自由移动，而这一节点静止和移动状态的转换过程即移动场景。在移动场景下，移动标签节点借助锚节点完成了自身定位。同时，由锚节点移动带来的锚节点定位范围移动将使加权精度因子发生变化。为更好地定量描述这一变化，在此将这一变化定义为移动精度因子，该定义值可通过测距误差与移动节点定位误差之比计算得到。

范数的均方根增益可以表示为Frobenius范数，且每一个范数均是凸函数。由于均方根误差的最小值点一般落在锚节点定位范围的内部，移动标签节点的定位误差会随着与锚节点定位范围之间的距离增加而线性增加。从这一重要特性可以看出，由于移动标签定位误差的非线性增长， 当锚节点的定位范围固定不变时，将移动标签节点直接移至最终目标位置不是最优的方案。

2 仿真分析

为验证基于双向测距的协同定位系统相对于传统基于双程测距定位系统的优势，下面分别在两种场景下进行仿真分析并对比，具体场景设计如图3所示。在场景1中，待定位标签节点5初始时位于锚节点1～锚节点4的定位范围内，随后标签节点5按照黑色实线方向移动出先前锚节点的定位范围。在场景2中，首先节点6和节点7从初始位置移动至锚节点1～锚节点4所构成的定位范围之外，随后标签节点5移动出锚节点1、锚节点2、锚节点6、锚节点7构成的定位范围，其中锚节点3和锚节点4在此过程中未被标签节点5使用。

图3 协同定位系统动态场景对比

在此基础上，设置仿真条件如下：场景区域为40 m×40 m方形范围，锚节点具体分布位置坐标如图4所示，节点间测距标准差为0.3 m，通过节点的真实位置可以计算出标签节点与4个锚节点的真实距离，并将此距离加上给定的误差 0.3 m作为定位系统的原始测量距离。标签节点的定位解算方法采用CHAN算法[11]，迭代次数设为5000次，该算法能够以很小的计算量解算出高精度的位置坐标，理论上可达到克拉美罗下限。此外，锚节点的位置用空心点表示，解算出的标签节点位置在图4中用蓝点表示。

图4 移动场景下移动节点的定位结果对比

从图4中可以看出，场景2中标签节点移动出锚节点定位范围时的定位误差与场景1中相比较小。其中，场景1中标签节点定位标准差为0.76 m，而场景2中定位标准差为0.49 m。因此，基于双向测距的协同定位系统在对移动节点的定位精度上优于传统基于双程测距定位系统。

在此基础上，为考察基于双向测距的协同定位系统中几何分布因素对移动节点定位误差的影响，下面在仿真实验中加入距离因子d来进一步对比分析，距离因子d会直接影响标签节点到锚节点的方向余弦向量，如图5所示。

图5 考虑距离因子d的协同定位系统

标签节点定位均方根误差随距离因子的变化图如图6所示。从图6中可以看出，当距离因子d较小时标签节点定位均方差较差，同时标签节点的定位均方根误差随着距离因子d减小而增大，其根本原因是距离因子d较小时系统的精度因子较差。

图6 标签节点定位均方根误差随距离因子的变化

另外，如果继续增大距离因子d，如图7所示，可以明显看到标签节点的定位均方根误差随着距离因子增大至125后大幅增加。这是由于即使增大距离因子d而带来较好的精度因子，仍存在双程测距方式误差传播的因素对标签节点定位误差的影响。

图7 协同定位系统中均方根误差随距离因子的变化

最后，总结对比传统基于双向测距的定位系统与协同定位系统的误差源，如表1所示。

表1 不同定位系统的误差源对比

3 结束语

本文主要分析了协同定位系统中与传统基于双向测距的定位系统相比需额外考虑的测距误差传播和移动场景误差影响。此外，为了更好地评估移动场景的定位效果，提出了移动精度因子概念。移动精度因子实质上是加权精度因子与误差传播的结合。同时，通过仿真分析了锚节点几何分布的距离因子对标签节点定位误差的影响，从仿真结果中可以看出，若将标签节点移动的过程控制在合理的移动精度因子范围内，可以得到更优的移动标签节点定位精度。