基于超宽带阵列与里程计的多机器人相对定位

刘 冉 曹志强 邓天睿 邓忠元 肖宇峰

(西南科技大学信息工程学院 绵阳 621000)

1 引 言

近年来，随着多机器人应用需求的增加，例如，分布式编队、多机器人建图等。这些应用对相对定位有较高的需求，因此，面向多智能体间的高精度定位已成为国内外研究的热点[1]。目前，在室外环境可以通过全球定位系统(Global Positioning System, GPS)或北斗卫星导航系统为机器人提供较高的定位精度。然而由于建筑物对GPS信号的阻挡，室内环境无法较好地接收卫星信号，给机器人室内定位带来了巨大挑战[2]。针对视距下的室内环境，国内外学者提出了很多成熟的相对定位技术和方法[3,4]。但针对非视距等室内场景，实现机器人间相对定位仍存在很大的挑战。

目前，多机器人定位方法主要分为全局定位和相对定位。运动捕捉系统[5]、超宽带(Ultra Wideband,UWB)定位系统[6]、WiFi定位系统[7]等为常用的全局定位系统。上述定位系统对基础设备的安装精度有着极高的要求，否则将会影响定位的准确性。全局定位系统需要事先构建环境地图或标签的分布情况，且每个移动机器人需额外配置通信设备，以获得其他机器人在环境中的位置，极大增加了硬件开销。另外，在紧急情况下，环境中的基础设施结构可能已被破坏，事先安装的摄像头或标签均可能无法正常工作，导致全局定位系统失效，不能为多机器人在未知环境中提供定位服务[8]。

相对定位系统相较全局定位系统而言，在实现多机器人定位上更具灵活性和抗干扰性。在相对定位系统中，无需提前安装基础设备，机器人可通过自身传感器获得测量范围内的其他机器人的相对位置。Scaramuzza等人[9]提出了一种基于同时定位与地图构建(Simultaneous Localization And Mapping, SLAM)技术的相对定位方法，在建图期间实现目标自主定位，通过通信网络分享各自估计位置。Saska等人[10]提出一种基于视觉在线标定的方法实现相对定位，但该方法易受光照影响，且视角小、计算复杂度高，在非视距环境下表现欠佳。Wang等人[11]设计了一款具有旋转激光发射功能和接收功能的硬件装置，并提出事件触发 (event-trigged)算法实现相对位姿估计，该方法对硬件设施要求高，且不适用于非视距环境。与激光和视觉传感器相比，无线传感器在非视距的情况下有更好的表现[12,13]。UWB技术因具有成本低、体积小、功耗低和出色的穿透力，在行业中得到了广泛的应用。Ghanem等人[14]利用每个车辆配备的两个UWB传感器实现两个车辆间的相对定位，但存在定位奇异性问题。另外，吴玉秀等人[15]提出了一种基于声音的分布式多机器人定位方法，通过麦克风阵列获得附近机器人在自身坐标系下的相对位置，但该方法仅在小范围内且机器人静止状态下进行了验证。

航迹推算在机器人定位中起着至关重要的作用，其根据里程计或惯性测量单元，在短时间内能够提供精准的姿态估计[16—18]。但随着长时间运行，会出现不可避免的累计误差，影响姿态估计的精度。国内外研究人员致力于将航迹推算与其他传感器源融合，提高定位精度。针对UWB与里程计数据融合，San Martín等人[19]和Magnago等人[20]分别提出扩展卡尔曼滤波(Extended Kalman Filter, EKF)和无迹卡尔曼滤波(Unscented Kalman Filter,UKF)等不同的数据融合方案。此外，针对相对定位，Guo等人[21]提出了一种基于UWB的协作相对定位方法，该方法基于UWB测量距离值和IMU位移测量值来估计多个无人机的位置，但该文中仅讨论了与静态无人机间的相对定位问题。Liu等人[22]提出了一种融合惯性测量单元(Inertial Measurement Unit, IMU)和UWB的相对定位方法，但未讨论相对角度的估计问题。Hardy等人[23]融合了IMU、磁强计、点对点测距和俯视摄像头实现无人机组高精度姿态估算。该方法计算复杂度较高，因此不适用于复杂环境中多机器人的应用。

本文针对室内场景的多机器人相对定位问题，提出了多UWB节点与里程计融合的相对定位方法。该方法需在每个机器人上安装多个UWB节点，如图1所示。UWB节点安装在移动机器人上的特定位置，通过最小化多组UWB节点的距离测量值来实现相对姿态估计 (相对位置和航向)。由于UWB距离测量值存在误差，因此相对姿态估计值不可避免地产生不规则波动。虽然里程计长时间运行存在累计误差，但短期内精度较高，且轨迹平滑。因此，利用相邻时刻里程计测量值对非线性优化所得相对姿态进行约束，提高定位精度。另外，为保证系统实时性利用滑动窗口的思想仅优化特定时间段的测量数据。图优化过程中不能确定UWB位姿估计误差的大小，对优化结果造成一定的影响，因此，通过粒子滤波融合里程计和滑动窗口优化后的相对姿态，实现准确的姿态估计。本文的创新之处在于：(1) 提出一种基于多UWB节点距离测量值，通过非线性优化实现移动机器人之间相对姿态估计的方法。(2) 设计滑动窗口，并基于图优化算法对滑动窗口内的数据进行优化，进一步提升位姿估计精度，并保证算法实时性。(3) 利用粒子滤波融合里程计和滑动窗口优化后的相对位姿，获得更准确的相对姿态估计。(4) 本文在真实环境中验证算法，实验结果表明，在12 m×6 m的环境中3个移动机器人能到达0.312 m的定位误差以及4.903°的角度误差，且满足系统的实时性。

2 系统概述

如图1所示，移动机器人在未知的、无基础设施的场景中实现相对定位。每个机器人携带里程计和4个UWB节点，UWB节点构成一个正方形矩形阵列。每个机器人上的UWB节点的收发频率与UWB阵列的距离配置相同。另外，移动机器人底盘均为两轮差分驱动。本文所提方法中，每个移动机器人携带处理单元，将测量数据 (UWB距离和里程计信息) 上传至服务器，以便数据集中处理。

3 算法设计

本文所提基于多UWB节点的机器人相对定位算法由3部分组成，分别为：基于UWB的机器人间相对位姿估计、融合里程计约束的机器人间相对位姿估计和基于粒子滤波的机器人间相对位姿估计。此3部分紧密相关、层层递进：(1) 基于UWB的机器人间相对位姿估计，仅利用UWB距离测量值通过非线性优化算法实现相对姿态估计。(2) 融合里程计约束的机器人间相对位姿估计，通过图优化算法对滑动窗口进行优化，利用里程计相邻时刻的相对位移对相对姿态估计进行约束，获得更为准确的估计。(3) 基于粒子滤波的机器人间相对位姿估计，通过粒子滤波器融合里程计和滑动窗口优化后的相对位姿，进一步提升定位精度。整体算法框架如图2所示。

3.1 基于UWB的机器人间相对位姿估计

3.2 融合里程计约束的机器人间相对姿态估计

通常情况下，移动机器人的尺寸较小，因此机器人上UWB节点之间距离受到了限制。另外，由于UWB距离测量值的误差，对非线性优化结果造成很大的影响。本文中所有移动机器人均携带里程计，将一段时间内的里程计数据以及非线性优化得到的相对位姿构成滑动窗口。利用里程计在短时间内相对位移准确的特性，对滑动窗口中的相对姿态估计进行约束，提升定位精度。图优化算法[24]中的图由顶点和边构成，本文中将相邻时刻里程计的相对位移以及基于非线性优化算法所得相对姿态估计值作为图的边，移动机器人不同时刻的位姿作为顶点，利用边约束顶点，构建待优化的位姿图。随着边与顶点数量的增加，优化所需时间也相应地变长，因此本文通过滑动窗口来减少算法的计算开销。图优化算法中的整体约束表述为

3.3 基于粒子滤波的机器人间相对姿态估计

在更新步骤之后，将执行重采样以获得新的粒子集，这对于避免粒子群退化至关重要。理论上，粒子的权重越大，在重采样过程中被选中的概率越大。

4 实验与分析

4.1 实验环境搭建

本文设计数据采集平台如图4所示。每个移动机器人携带4个UWB节点(nooploop linktrack)，构成正方形矩阵阵列，且每个UWB节点的最大测量距离为100 m，数据采集频率被设置为50 Hz。两轮差分式移动机器人底盘的里程计输出频率为20 Hz，但由于地面不规整，轮子将出现不同程度的打滑及空转现象，影响里程计精度。为验证本文算法的精度，每台机器人搭载一台Hokuyo激光雷达，用于建图(gmapping)以及实现自适应蒙特卡洛定位(Adaptive Monte Carlo Localization, AMCL)[26]，国内外研究者常将AMCL所得姿态作为机器人在室内环境中的真实轨迹，将其与估计值做比较，详情参考文献[27]。另外，每个移动机器人搭载一台笔记本电脑用于运行机器人操作系统(Robot Operation System, ROS)，并记录各个模块采集的数据。

为验证本文提出的算法的性能，共设置以下两组实验。实验1-a中，仅使用两个移动机器人，机器人2静止不动，机器人1以0.2 m/s的速度沿着7×6 m的矩形轨迹运动，在运动过程中，各组模块实时记录数据，如图5(a)所示；实验1-b为验证算法在非视距环境不规则运行路径下的表现，机器人2前方被远高于机器人的木板遮挡，机器人1沿着不规则路径运动，如图5(b)、图6所示。实验2：在该实验中，3个相互独立的移动机器人，在12 m×6 m的环境中沿着3条不同路径以0.2 m/s的速度同时运动，如图5(c)所示，两个机器人沿着不同的矩形轨迹运动，另一个机器人沿着T型轨迹运动。

4.2 实验1

在实验1-a中，机器人1围绕机器人2沿着矩形轨迹运动数圈，如图5(a)所示。该实验设置不同的UWB节点距离配置，检验不同算法在不同配置下的性能。分别设置0.3 m, 0.5 m和0.7 m的UWB间距。在每组测试中，机器人1和机器人2的UWB节点距离配置以及各传感器输出频率始终保持一致。

由于UWB传感器自身存在测距误差及数据波动，因此不同UWB距离配置对非线性优化的结果有较大影响。如表1所示，对比不同UWB配置对相对定位性能的影响。其中，UWB节点距离配置为0.7 m时，获得0.37 m的位置误差且具有0.22 m的标准差，角度误差为5.05°以及标准差为3.63°；对比距离配置为0.3 m，位置误差降低22%，角度误差降低33%，且具有更小的标准差。表明位置误差和角度误差随着UWB间距的增加逐渐降低，且具有更小的数据波动。实际应用中，通常根据移动机器人体积大小选择不同的UWB节点距离配置。

不同的滑动窗口长度，对图优化算法的时间消耗以及定位精度有一定的影响，ω长度越大所需时间消耗越多。不同窗口长度ω的定位精度如表1所示，由此表可见位置误差和角度误差随长度ω增加而降低。

对比UWB节点距离0.3 m时，ω=80相较于ω=5的位置误差和角度误差分别下降21%和57%。但当ω=160时位置误差和角度误差反而增加，可能由于ω过大，导致算法执行时间太久，影响系统的实时性。

为保证估计精度和系统实时性，在后续算法中选择滑动窗口长度ω=30。本文提出的粒子滤波算法与文献[22]所提的基于粒子滤波融合IMU和UWB距离测量数据的相对定位方法对比，两个粒子滤波中，粒子数量均为500。实验结果表明，本文所提算法具有更优的表现，如表1所示。与文献[22]方法相比，位置精度提升55%，角度精度提升56%，且估计轨迹更加平滑，误差分布波动更稳定，证明算法性能更稳定可靠，如图7所示。

表1 实验1：不同估计方法在不同的UWB节点配置下，位置误差以及角度误差的评估结果

为验证算法在非视距、非规则路径条件下的性能。设置实验1-b，机器人1与机器人2之间被木板遮挡，且机器人1在实验场地中沿不规则路径移动，如图6所示。该实验中，设置UWB节点距离配置为0.5 m，滑动窗口长度为ω=30以及粒子数量为500。估计轨迹如图6所示，里程计在长期运行过程中出现明显的累计误差，位置误差和角度误差分别为0.48 m和8.61°。通过本文所提出的算法，轨迹得到明显的提升，与真实轨迹相比，获得0.29 m的位置误差和4.39°的角度误差，该结果证明了该算法在上述条件下的有效性。

4.3 实验2

为充分验证本文所提方法的实用性，设置实验2，3个移动机器人被控制沿不同的轨迹同时运动，且每个机器人起点不同，如图5(c)所示。综合实验1分析结果，在实验2中，确保定位精度和系统实时性，设置滑动窗口长度为ω=30，UWB节点距离为0.5 m。表2给出不同定位算法的精度。如表2所示，3个移动机器人在仅使用里程计的情况下，获得相对位置误差和相对角度误差分别为1.466 m和22.076°。3个机器人里程计估计轨迹如图8(a)所示，显而易见，随着时间的累计3个移动机器人的里程计都发生了不同程度的漂移，其中机器人1的里程计漂移最为严重。与里程计相比，仅利用UWB数据通过非线性优化估计得到的相对姿态，精度得到提高，且不存在累计误差。如图8(b)所示，假设机器人1姿态已知，估计机器人2和机器人3相对于机器人1的姿态，获得位置误差0.528 m和角度误差7.413°。由于UWB距离测量数据的误差，造成估计值产生无规则的波动，且在个别位置误差较大。

表2 实验2：不同估计方法在w=30及UWB间距0.5 m情况下评估结果

利用图优化算法对滑动窗口中的数据进行优化，通过相邻时刻里程计的相对位移对非线性优化结果进行约束，进一步提升定位精度，被优化后的滑动窗口组成的运动轨迹如图8(c)所示。在假设机器人1姿态已知的情况下，获得位置误差0.424 m和角度误差5.648°。与非线性优化相比，被优化后的滑动窗口在位置误差和角度误差上分别下降了19%和23%。

文献[22]所提的基于粒子滤波融合IMU和UWB距离测量值的相对定位方法的结果与本文所提粒子滤波算法对比，其估计轨迹如图8(d)和图8(e)所示。粒子数量均设为500。本文的粒子滤波算法，融合滑动窗口当前时刻估计值和里程计测量值，获得位置误差0.312 m和角度误差4.903°。相较滑动窗口优化结果，精度分别提升了26%和13%。采用文献[22]的方法，将UWB距离值和里程计通过粒子滤波融合，取得位置误差0.534 m和角度误差7.234°。本文所提方法在位置精度和角度精度上都得到极大提升，精度分别提升41%和32%。另外，与实验1相比，由于移动机器人数量的增加以及实验空间的扩大，UWB的测距误差也随之增加，因此整体表现稍差。

本文涉及的相关算法，在一款内存为12.0 GB搭载Intel i5-6300HQ 2.30 GHz CPU的装有ROS系统的笔记本电脑上运行。相关算法所消耗的计算时间如图9所示，本文提出的基于粒子滤波融合滑动窗口和里程计的方法需消耗时间86 ms，其中包括：非线性优化、ω=30的滑动窗口以及粒子数为500的粒子滤波器总共花费的计算时间，如图9所示。对比文献[22]所提粒子滤波算法，时间消耗大约为其2倍。本文所提方法能够达到大约12 Hz的输出频率，亦能满足大多数的机器人应用。

5 结束语

本文提出一种基于对UWB与里程计融合的移动机器人相对定位方法。在仅采用UWB传感器的情况下，通过非线性优化算法能够实现机器人间的相对姿态估计。利用相邻时刻里程计的相对位移对非线性优化结果进行约束，提升定位精度。另外，为确保系统实时性，利用滑动窗口的思想仅对特定时间内的数据进行优化。由于UWB相对姿态约束中的信息矩阵的不确定，对优化结果造成影响，因此基于粒子滤波融合滑动窗口优化后的机器人间的相对姿态和里程计，进一步提升相对姿态估计精度。通过3个配置 (UWB距离配置和各传感器收发频率) 相同的移动机器人在12 m×6 m的室内场地进行实地测试，实验结果表明，本算法在满足实时定位的基础上，能够达到0.312 m的定位误差以及4.903°的角度误差。为多机器人相对定位提供了一种具有参考意义的方法。在后续的研究中，探索其他更有效的方式来提升精度以及减少时间消耗，并投入到多机器人协作与编队控制中。