利用UWB校正LiDAR SLAM的室内组合定位方法*

王昭东，秦文虎

(东南大学 仪器科学与工程学院，江苏 南京 210096)

0 引 言

定位与导航技术是室内自主移动机器人的关键技术之一。在室外良好的情况下，以全球定位系统(global positioning system,GPS)、北斗系统(BDS)为代表的全球卫星导航系统(global navigation satellite system,GNSS)能够提供良好的定位效果[1]。但在室内环境下，由于建筑物对信号的遮挡，GNSS的信号会严重衰减，无法满足室内高精度定位的需要。在这种情况下，室内定位技术朝多方向的发展。按照使用的传感器分类，常见的室内定位技术有WiFi[2]、射频识别(radio frequency identification,RFID)、超宽带(ultra-wideband,UWB)、惯性导航系统(inertial navigation system,INS)、蓝牙(Bluetooth)、激光雷达(LiDAR)、工业相机等[3]。

近年来，学者们趋向于采用多传感器融合的方法来解决室内定位的问题。文献[4]提出利用粒子滤波器融合加速度计、指南针和惯性测量单元(inertial measurement unit,IMU)的数据实现室内定位的方法，在不考虑实时性的情况下具有比较高的精度；文献[5]提出了一种融合了智能手机中方向传感器、环境中的WiFi和蓝牙的多源信息融合的定位算法。文献[6]提出的将双目立体视觉和IMU数据紧耦合的组合定位导航算法，具有较好的精度，鲁棒性较好。文献[7]提出了一种将里程计、IMU和视觉相结合的轮式机器人定位的辅助导航系统，实验证明该系统具有较高的定位精度。

相比于其他技术，LiDAR具有分辨率高、抗干扰能力强的优点，而SLAM技术是解决LiDAR定位的热门技术[8]。但LiDAR即时定位与地图构建(simultaneous localization and mapping,SLAM)要求工作的环境有丰富的特征。在特征缺乏的环境下，LiDAR SLAM算法会发生特征匹配错误的问题，造成误差的累积，这将导致定位结果出现较大误差。UWB具有低功耗、高时间分辨率和保密性好的优点[9,10]，并且具有较大的覆盖范围。但UWB容易受到多径效应和非视距误差的影响，导致定位的结果发散。

本文提出了一种利用UWB观测值来校正LiDAR SLAM预测结果的组合定位方法。首先，通过里程计预测模型得到下一时刻的位姿信息；之后，利用粒子滤波的方法融合雷达观测值对局部定位结果进行修正，并通过坐标变换将局部定位结果转换到全局坐标系中；最后，利用UWB观测值对全局坐标系中的定位结果进行修正。该组合方法的优势在于，既有效地通过UWB来消除LiDAR SLAM中的累积误差，又抑制了定位结果较为分散的现象。

1 UWB与LiDAR SLAM定位原理

1.1 UWB定位原理

基于测距信息进行定位是UWB定位的常用方法。双向测距(two-way ranging,TWR)算法[11]是一种典型的测距方法，这种测距方法不需要信标之间保持时间同步。

图1 DS-TWR算法原理

1.2 LiDAR SLAM定位原理

常用的LiDAR SLAM算法中，目前Gmapping算法[12]融合了里程计信息，同时，使用了Rao-Blackwellized 粒子滤波(Rao-Blackwellized particle filtering,RBPF)算法，在构建小场景地图时所需的计算量较小、精度较高，且相对于Hector算法[13]来说鲁棒性更好；Cartographer算法[14]由于需要大量的粒子且有回环检测的部分，需要较大的计算量，适用于构建大场景并需要部署在算力较高的设备上。综合考虑，本文选用Gmapping算法作为LiDAR SLAM的基本算法。

Gmapping算法中采用的RBPF算法将SLAM问题中同时定位与建图的问题拆分成了2个问题：一个问题是已知预测值和观测值求位姿；另一个问题是已知机器人位姿进行地图构建，如式(1)所示

p(x1︰t,m|z1︰t,u1︰t)=p(m|x1︰t,z1︰t)p(x1︰t|z1︰t,u1︰t)

(1)

式中x1︰t为1到t时刻的位姿，m为地图，z1︰t为LiDAR的观测值，u0︰t为里程计得到的输入值。本文关心的问题为两个问题中求位姿的问题。通过贝叶斯准则可以对p(x1︰t|z1︰t,u0︰t)进行分解得到

p(x1︰t|z1︰t,u1︰t)=ηp(zt|xt)p(xt|xt-1,ut)p(x1︰t-1|u1︰t-1,

z1︰t-1)

(2)

通过推导，求位姿的过程可以分解成3个过程：p(x1︰t-1|u1︰t-1,z1︰t-1)代表获取上一时刻的位置估计；p(xt|xt-1,ut)代表在上时刻位姿的基础上，通过里程计预测模型求得当前位置的估计值；ηp(zt|xt)代表通过LiDAR观测模型，对各个预测的粒子进行权重归一化，得到当前时刻的位姿。

2 UWB/LiDAR融合定位算法

融合算法的流程如图2。

在语言学大师索绪尔的眼中，符号是由 “能指”（signifier）与“所指”（signified）构成的二元实体。[8]商标犯罪侵犯的是社会主义市场经济秩序。细言之，以商业利用的方式而假冒他人的注册商标应用在自己的商品或者服务上，造成消费者对相关商品或者服务的混淆，从而无法清晰分辨各个商标所施指的商品或服务，致使商标指示市场的混乱。

图2 融合算法流程

首先利用上一时刻的粒子群信息来初始化位姿信息xt-1，粒子权重wt-1和地图mt-1，如式(3)

(3)

之后利用里程计预测模型得到位置的估计值

(4)

在LiDAR观测模型的基础上通过极大似然估计求得局部极值

(5)

如果这里没有找到局部极值，使用提议分布，更新粒子的位姿状态，并使用观测模型对位姿的权重进行更新，并从初始化阶段重新开始

(6)

(7)

如果找到了局部极值，在局部极值附近取k个位姿

(8)

假设k个位姿分布服从高斯分布，对k个位姿xj∈{x1,…,xk}求均值和归一化参数

(9)

(10)

对均值进行归一化处理

(11)

计算k个位姿的方差

(12)

方差归一化处理

(13)

这样可以用正态分布的形式来表示新的位姿

(14)

更新该位姿粒子的权重

(15)

引入UWB的观测值，对每个粒子的均值和方差进行修正，设UWB在t时刻的位置信息为

(16)

(17)

根据更新后的粒子权重、位置信息加权求和，可以得到修正后的当前位置的信息。最后通过离子权重和各粒子的均值加权平均得到当前位置的定位结果。

3 实验与结果分析

3.1 实验机器人的搭建

搭建机器人平台，如图3所示。

图3 实验小车的左视图和俯视图

本文实验选取的UWB模块为研创物联基于Decawave1000的UWB Mini 3s Plus模块，选用的LiDAR模块为思岚科技的RPLiDAR A1模块。UWB传感器的参数：发射功率为110 kW/6.8 M，覆盖半径为300 m(110 kW)，测距误差小于10 cm，定位误差小于15 cm。LiDAR参数：测量半径为12 m，测量角度为360°，测量频率为8 000次/s，扫描频率为2～10 Hz，角度误差为1°。

安装时要将服务端的UWB信标的天线置于LiDAR的正上方，便于两者坐标系的统一。小车的ROS系统部署在一块树莓派4B上。为了缓解树莓派的运算压力，UWB的测距信息首先被一块STMF103C8接收，将得到的坐标解算结果通过串口发送给树莓派。同时为了便于监控系统的运行状态，安装一个无线串口，将小车位姿数据实时发送给电脑。

3.2 实验场景布置

实验场地布置在一个11.7 m×8.5 m的房间中进行，在房间中央布置场地作为试验场所。场地中布置4个UWB基站，并在地面贴设地标作为参考标识。场景的布置效果如图4所示。

图4 实验场地布置效果

UWB基站与地标的位置关系如图5所示。

图5 UWB与地标位置关系

图5中A0，A1，A2，A3为布置在场地中的4个UWB基站，线框代表地标线贴设的位置。各距离测量值通过激光测距仪得到，误差为±2 mm。实验时操控小车从A点开始按照A→B→C→D→A的顺序沿着地标线运动，比较通过传感器定位得到的结果和地标参考位置之间的差距。

3.3 实验结果与分析

分别让机器人在UWB单独定位，LiDAR SLAM单独定位和组合定位的条件下，沿着地标线运行1周，得到的结果如图6所示。

图6 定位实验结果

图6(a)中散点构成的轨迹为UWB单独定位得到的结果分布情况，虚线框代表由地标线标识的参考轨迹。由图可见，在UWB单独定位的情况下，小车的定位轨迹分布在参考轨迹周围，定位结果比较分散。图6(b)中散点代表单独LiDAR SLAM得到的定位结果分布的情况，虚线框代表地标参考轨迹。可见，在LiDAR单独定位的情况下，小车的轨迹比较平滑，但由于实验环境中缺少特征，随着时间的推进，累积误差逐渐变大。同时可以观察到由于定位点缺失导致的轨迹中断现象。图6(c)中散点代表UWB/LiDAR融合方法的定位结果分布情况。由图可见，在组合定位下，小车的轨迹较为平滑，并消除了累积误差，具有比较好的定位效果。

统计小车在A，B，C，D四点坐标的均方根误差如表1。

表1 均方根误差统计 m

4 结 论

本文提出的一种利用UWB观测值来校正LiDAR SLAM的组合定位方法，在LiDAR SLAM的基础上通过高斯滤波的方法融合UWB的观测值，并通过实验对单独使用UWB和LiDAR SLAM两种定位方式进行了比较。实验结果表明：本文提出的组合定位方法能够有效地消除LiDAR SLAM算法的累积误差，同时改善了单独用UWB进行定位时结果分散的现象。因此，相对于单一传感器，本文提出的组合定位系统更适用于室内高精度定位。