室内环境下UWB与LiDAR融合定位算法研究

李中道，刘元盛，常飞翔，张 军，路 铭

1.北京联合大学 智慧城市学院，北京 100101

2.北京联合大学 机器人学院，北京 100101

3.北京联合大学 北京市信息服务工程重点实验室，北京 100101

4.北京联合大学 应用科技学院，北京 100101

无人驾驶技术被广泛认为是避免道路碰撞和改善交通状况的一个有意义，且具有实用性价值的研究方向。为了使无人驾驶汽车在城市环境中安全行驶，汽车需要获取自身的准确位姿。因此，定位技术在无人驾驶汽车的应用中起着关键的作用[1]。目前几乎所有的无人驾驶车辆定位方案中都使用绝对定位（如Global Navigation Satellite System，GNSS）与相对定位（如带里程表的Laser-Simultaneous Localization and Mapping，Laser-SLAM）相结合的方法来获取车辆更精确的位置信息[2-3]。但GNSS系统仅适用于室外环境，在室内环境下由于卫星信号遮挡导致定位失败，然而95%的汽车大多时间都停在停车场或者室内环境[3]。由此可见，构建与GNSS相对应的室内定位系统从而实现无人驾驶车辆的室内精确定位已成为亟待解决的问题。

当前成熟的室内定位方法中，基于诸如超宽带（Ultra-Wideband，UWB）、蓝牙、Zigbee、可见光、超声波、WiFi、射频识别设备、激光雷达（Light Detection and Ranging，LiDAR）、视觉设备等传感器；并采用到达角（Angle of Arrival，AOA）、飞行时间（Time of Flight ，TOF）、飞行返回时间（Roundtrip Time of Flight，RTOF）、往返飞行时间（Two Way-Time of Flight，TW-TOF）、接收信号强度（Received Signal Strength，RSS）等方法实现室内定位[4-5]。但目前成熟的室内定位方案中，由于不同的传感器自身特性，没有任何一个单一的传感器能够同时满足无人驾驶车辆的定位精度、实时性、鲁棒性需求。故需要采用多类型传感器融合的方式实现无人驾驶车辆的定位与导航，如绝对定位类型的传感器与相对定位类型的传感器相融合的定位导航方式。

近年来，学者对于多传感器数据融合定位算法的研究近年来趋向于基于滤波的数据融合方法，文献[6]分析了LiDAR和UWB测距的定位问题，提出了一种将惯性测量单元（Inertial Measurement Unit，IMU）、LiDAR和UWB 相结合的基于卡尔曼滤波的传感器融合方法，并在无人机设备连续定位状态取得了较好的效果，然而在文中没有涉及因动态障碍物干扰造成激光雷达失配后重新匹配的研究。文献[7-8]利用UWB超高的时间分辨率、良好的抗多径能力和较强的穿透能力，以及LiDAR在构图与定位方面所表现出主动、高效、精确的特点，提出了基于扩展卡尔曼滤波的UWB/LiDAR 组合定位系统。该系统能够稳定维持较高的定位精度，然而在定位失效后无法迅速重定位，因此无法满足无人驾驶定位稳定性的需求。此外，文献[9]中无人驾驶定位导航系统自适应地使用来自互补传感器（如GNSS，LiDAR 和IMU）的信息，以在挑战性场景（如市区、高速公路和隧道）中实现较高的定位精度和弹性，并采用误差状态卡尔曼滤波器将来自不同来源的定位测量结果与新的不确定性估计融合在一起，实现了5～10 cm 均方根精度。

然而在复杂的室内环境中，如动态障碍物较多，以及由于长时间停车造成的LiDAR SLAM时间累积误差导致定位失效问题，在该研究中并没有给出解决方案。

为了更好地解决室内定位的问题，本文采用UWB绝对定位技术和LiDAR SLAM相对定位技术相融合的定位方法。UWB 技术是一种基于信号的定位方式，类比于GNSS的定位方式，在视距（Line of Sight，LOS）环境的情况下可以提供可靠的绝对定位，但也会受到室内环境复杂性的制约，在非视距（Non Line of Sight，NLOS）环境下以及多路径效应会使得定位精度下降。另外LiDAR SLAM 定位技术可以提供可靠的空间特性，而且其定位精度较高，但单独使用LiDAR SLAM 定位技术存在起始点定位搜索时间长，以及激光点云匹配失效后无法快速重定位问题。为此提出基于粒子滤波的多传感器融合算法，将UWB和LiDAR量测数据互补融合解算。一方面，LiDAR 给定平面图的精确性可以补偿UWB 在非视距环境下定位误差，另一方面，UWB 定位高实时性可以弥补LiDAR SLAM 定位速率低的问题。通过这种方法可以满足无人驾驶车辆定位系统的低延时，定位精度高以及稳定性的需求。

1 定位技术原理

1.1 UWB定位原理

UWB 定位通常使用TOF 测距技术，测距至少需要三个锚点（空间基站）和一个标签（车载标签）两种设备，通过计算信号在锚点和标签间的传输时间差来确定二者的相对距离，这与全球定位系统中的伪距定位技术类似。其优点是TOF 测距技术不依赖基站与标签的时间同步，故没有时钟同步偏差带来的误差，但TOF测距方法的时间取决于时钟精度。为此本文采用异步TW-TOF方法，其测量模型如图1所示。该方法不需要锚点和标签通信双方的时间同步，根据数据交互产生的时间差，可以计算出锚点和标签之间的距离，从而消除TOF测距方式时钟精度偏移引起的误差[10-11]。

图1 TW-TOF测量模型

根据TW-TOF测量原理，可以求出锚点与标签之间的距离dTW-TOF如式（1）所示：

其中，c是电磁波在真空中的传播速度的近似常数3×108m/s；t1和t2分别表示请求时钟起始时刻和接收应答终止时刻；δ为响应时钟周期，一般为固定参数；δe表示总的系统时延（多由温度，传播介质等因素决定），其误差分布符合高斯白噪声N(0,Q)。

由于环境的复杂性，UWB 在测距过程中经常由于障碍物遮挡等原因引起的NLOS 误差dNLOS，NLOS 误差呈现非高斯分布。故可得出基于TW-TOF 测距模型的表达式为式（2）：

其中，di表示标签距第i个基站的距离。

由上所述，本文采用三个坐标已知的UWB 基站和一个UWB 车载标签实现车辆的定位，通过测得的标签到基站距离di,(i=1,2,3)后，通过三点定位法画三个圆，所得的交点即为标签的位置。

其原理如图2 所示，UWB 基站的坐标分别为A1(x1,y1,z1)、A2(x2,y2,z2) 和A3(x3,y3,z3) ，基站在安装部署时位置固定且坐标已知，所求UWB 车载标签的坐标为T0(x0,y0,z0)，其中z0为标签高度且已知。设r1、r2和r3分别为三个基站与标签之间的相对距离，计算公式如式（3）：

图2 UWB定位原理示意图

每个基站以相对距离为半径画一个圆形轨迹。利用三个圆形方程能够计算出唯一的交点，计算公式如式（4）：

即可求解出T0在平面坐标系上的坐标(x0,y0)。

由于非视距误差为非高斯模型，涉及到基站与标签的位置分布、障碍物的物质介质因素等难以估计，故用LiDAR 与UWB 数据融合算法消除。该算法适用于室内NLOS环境下运动目标的精确定位和动态跟踪。

1.2 LiDAR SLAM定位原理

即时定位与地图构建（Simultaneous Localization and Mapping，SLAM）算法是目前解决无人驾驶车辆LiDAR或者视觉定位所常用的方法[12]。

本文采用Google Cartographer[13]算法地图构建功能，生成车辆可行驶的局部点云地图。其可以生成分辨率为r=5 cm 的二维栅格地图，系统结合了单独的局部和全局方法来实现二维SLAM。并且两种方法都优化了由激光雷达观测值（又称为扫描）组成的由(x,y)平移和旋转γ组成的姿态δ=( )δx δy δγ。其中激光点云的坐标由激光雷达以固定的频率与角度旋转扫描所得，如其中一个扫描点数据为Pi=(ρi,θi)，其中ρi为当前激光点到原点的距离，θi为当前激光束与起始激光束之间的夹角。则可得其i点坐标如式（5）所示：

其中，xi和yi分别表示第i个激光点距离激光雷达原点的坐标，δγi为当前激光束与上一时刻激光束的转角增量。

其主要建图与定位步骤如下：

（1）把空间划分成边长r为5 cm的二维栅格。

（2）根据每个栅格内t时刻是否有激光点，给每个栅格分配一个占栅格值p(mi)：

式（6）中，p(mi)为第i个栅格占用的概率，栅格值为1代表被占用，栅格值为0代表空闲。

（3）计算当前帧和局部地图的占栅格值。

（4）对当前一帧设置一个搜索区间，即一个平移(δx,δy)和旋转δγ的区间。

（5）把搜索区间划分成多个选择项，每一个选择项对应一个位姿，即(δx δy δγ)，其中δx,δy为当前激光雷达对上一时刻的平移量，δγ为航向角旋转增量。

（6）用当前帧备选位姿计算相应的指标，判断其位姿相对局部地图的匹配程度，匹配度最高的位姿，即为当前位姿的最优估计。

但LiDAR SLAM 技术在实际应用中，需要提供初始位姿以提高匹配速率，在复杂环境中由于动态障碍物等因素的影响导致匹配失败以至于无法重定位。

2 组合定位系统模型

2.1 UWB与LiDAR组合定位模型

通过第1 章分别对UWB 和LiDAR SLAM 定位技术的介绍，可以得知每一种传感器在室内独立定位时都存在自身固有的技术壁垒，例如：UWB定位技术存在的非视距环境下误差，以及LiDAR SLAM 技术存在的误差累积和时延问题。这些问题都会影响到无人驾驶车辆在室内环境下的定位精度以及稳定性。为此有效的数据融合可以对两者的劣势进行互补，从而获得更加准确和稳定的定位结果。

无人驾驶车辆室内环境下UWB 和LiDAR 融合定位算法框架主要分为两大部分，第一步对UWB与LiDAR组合定位系统在时间和空间系统上进行统一。首先在待测地图中根据合理布局对UWB基站进行布局以及测量地图，并利用LiDAR SLAM 算法对待测地图进行地图构建，最终将地图坐标系通过坐标变换进行统一；对于融合系统的时间输出，使用解算设备的系统时间作为统一标准，从而达到组合定位模型在时间和空间上的统一。对于超宽带和激光雷达组合定位算法模型，将UWB和LIDAR量测信息用于基于粒子滤波的多传感器融合算法中。该算法是基于蒙特卡洛积分的最佳非线性滤波器的估计算法，其适用于解决非线性非高斯的动态模型问题[14-16]；针对UWB 与LiDAR 融合定位系统的非线性非高斯的动态模型问题，可以发挥不错的效果。如图3为无人驾驶车辆室内环境下UWB 和LiDAR 融合定位算法框架。

2.2 基于粒子滤波的数据融合算法

根据第1 章中UWB 和LiDAR 定位算法，为了得到高精度、高稳定性的定位结果，本文采用基于粒子滤波的超宽带和激光雷达融合定位误差模型，将UWB 和LiDAR 量测信息互补融合，其中使用Xt表示移动车辆在t时刻的位置与姿态，使用Zt表示UWB和LiDAR传感器在t时刻的观测值，则可以构建基于粒子滤波的融合算法模型。公式（7）和公式（8）分别表示该模型的状态方程和量测方程。

状态方程：

量测方程：

式（7）、（8）中，函数f(⋅)为基于时间t的状态函数，g(⋅)为传感器非线性量测函数。Wt-1表示离散时域中的随机白噪声序列，Vt表示测量噪声序列。在该模型中，将Xt=[ ]

xt yt θtT 作为状态信息向量，xt和yt分别表示UWB 和LiDAR 组合系统在t时刻的横纵坐标，θt表示在t时刻的航向角增量，且有：

式（9）中，γt-1为系统在t-1 时刻的航向角。将Zt=作为量测信息输入向量，其中分别表示UWB 设备在t时刻横纵坐标的量测值，分别表示LiDAR 设备在t时刻的横纵坐标和航向角增量的量测值。

本文基于粒子滤波的UWB 和LiDAR 定位算法的主要步骤如下，如图4 所示为基于粒子滤波的UWB 和LiDAR融合定位算法流程图。

图3 室内环境下UWB和LiDAR融合定位算法框架图

图4 基于粒子滤波的UWB和LiDAR融合定位算法

（1）Initialization Step：在初始化步骤中，根据UWB坐标输入估算位置，估算位置是存在噪声的，但是可以提供一个范围约束。通过UWB 提供的初始位置，提高粒子滤波运算效率。

公式（10）中，和分别表示UWB 定位系统在t时刻的测量值，ε为 UWB 定系统最大测量误差，rand(⋅)为生成(0,1)区间的随机函数，xi和yi分别表示生成第i个粒子的横纵坐标，N为随机粒子样本数，对应每个粒子的初始化权值为ωi，设为1/N。

（2）Prediction Step：在预测过程中，对所有粒子增加车辆的控制输入（速度、角速度等），预测所有粒子的下一步位置。

公式（11）中，xt和yt分别为当前粒子状态预测横纵坐标，xt-1和yt-1分别为上一时刻横纵状态坐标，ΔT为系统运行时间增量，vt为当前时刻车辆速度，γt-1为上一时刻车辆航向角，θt为当前时刻航向角增量，和为系统状态噪声，由车辆运动的横纵向标准偏差确定。

（3）Update Step：在更新过程中，计算此时由UWB观测生成的每个粒子的位置和LiDAR量测得到的车辆位置之间的几何距离，来更新所有粒子的权重。从而通过超宽带和激光雷达交互更新粒子权重，消除UWB 引起的NLOS误差，同时消除LiDAR时间累积误差。

其权值的更新公式如式（12）所示：

其中，为根据UWB 量测值生成的粒子中第i个粒子的横纵坐标集合为时刻t+1 的 LiDAR横纵坐标量测集合为两者集合的2范数求解，意为求解两个集合之间的距离。R为定位系统测量误差。

再利用式（13）对权值进行归一化：

（4）Resample Step：根据粒子的权重，对所有粒子进行重采样，权重越高的粒子有更大的概率生存下来，权重越小的例子生存下来的概率就越低，从而达到优胜劣汰的目的。利用后验概率密度P(Xt|Zt)（式（14））产生N个新粒子解决粒子退化问题。

即对不同粒子进行加权后的概率密度，同时，t时刻状态Xt的最终估计值如式（15）所示：

式（13）中，Xt表示粒子在t时刻的车辆状态，Zt表示车辆在t时刻的观测车辆位置，式（15）中，为车辆t时刻的车辆状态最优估计值，表示t时刻第i个粒子的状态函数，表示t时刻重采样之后的第i个粒子权重值。

（5）再回到（2），进入下一次循环，在此过程中，遇到车辆状态突变，以当前UWB 定位结果作为初始位置回到（1），重新搜索车辆位置。

3 实验分析

3.1 实验平台搭建及预处理

为了验证UWB 与LiDAR 的室内组合定位算法的效果，在地下车库内搭建测试场景，并选用长530 mm，宽310 mm 的智能驾驶小车作为无人驾驶车辆。同时，该车还配有车载处理器NVIDIA TX2，并运行ROS机器人操作系统。定位传感器UWB 和LiDAR 分别使用布科思科技有限公司的BooStar-T和思岚科技有限公司的RPLIDAR-A2。其中，UWB的工作频率为6.2～6.7 GHz，无遮挡通信距离为130 m；LIDAR的测距范围为0.2～8 m，扫描角度为360°，单次测距时间0.125 ms。为保证UWB 和LiDAR 在空间解算上的统一性，将UWB 标签和LiDAR 安装在智能驾驶小车平台的同一轴线上，分别通过RS232串口协议将数据传输至TX2平台。

此外，为了保证定位精度，使用激光水平仪与激光测距仪测量车库的空间距离以及UWB 基站的相对位置，如图5 所示。测量结果为：长7.0 m，宽7.0 m，环境左下角为坐标原点；其中UWB 基站的坐标分别为(0.0,1.0,2.0),(7.0,1.0,2.0)和(3.5,7.0,2.0)（坐标单位：m）。

图5 实验环境及其参数

在地库测试环境中，智能驾驶小车平台的运行路径为A、B、C、D 围成的矩形区域（图5 所示），其坐标分别为(1.0,1.0),(6.0,1.0),(6.0,6.0),(1.0,6.0)（坐标单位：m），其中A→B→C路段为LOS和无动态障碍物路段，C→D→A路段为NLOS和有动态障碍物路段。

图6 为智能驾驶小车平台使用cartographer 激光雷达SLAM算法建立的点云地图，采集地图运行路径如图5 所示，从A 点出发，按照既定路线（A→B→C→D→A）运行智能驾驶小车平台，回到A点。采集所得的地图通过坐标变换后用于匹配激光雷达定位。

图6 激光雷达点云地图

3.2 实验结果及分析

实验1单独使用UWB 定位方法在LOS 路段A→B→C和NLOS路段C→D→A进行定位实验。图7所示为UWB 在室内环境下定位实验结果。可以得出X轴（A→B）LOS 最大误差为30.5 cm，Y轴为（B→C）LOS最大误差为24.5 cm，X轴（C→D）NLOS 最大误差为33.0 cm，Y轴（D→A）NLOS最大误差为93.5 cm。

图7 UWB在室内环境下定位实验结果

实验2使用LiDAR-SLAM方法在无动态障碍物路段A→B→C 和有动态障碍物路段C→D→A 进行定位实验。图8 所示为LiDAR 在室内环境下定位实验结果。结果表明，在无动态障碍物路段LiDAR 的X轴（A→B）最大误差为33.2 cm，Y轴（B→C）最大误差为31.4 cm；在有动态障碍物路段LiDAR的X轴（C→D）最大误差为42.8 cm，Y轴（D→A）最大误差为77.9 cm。同时可以看出LiDAR单一传感器SLAM算法容易产生较大的累积误差。

图8 LiDAR在室内环境下定位实验结果

实验3使用本文提出的UWB 和LiDAR 融合定位算法在LOS 和无动态障碍物路段A→B→C 和在NLOS和有动态障碍物的路段C→D→A，进行实验。图9所示为本文算法在室内环境下定位实验结果。实验表明，本文算法在LOS和无动态障碍物环境下，X轴（A→B）最大误差为12.8 cm，Y轴（B→C）最大误差为18.5 cm；在NLOS 和有动态障碍物环境下，X轴（C→D）最大误差为22.1 cm，Y轴（D→A）最大误差为37.9 cm。

图9 本文算法在室内环境下定位实验结果

图10 给出了 UWB 定位、LiDAR SLAM 定位以及本文算法的定位对比结果，图中实线轨迹为本文算法，可以看出本文算法定位结果在最大定位误差上得到了很大的改善，其均方根误差实验结果如表1 所示，结果表明UWB 和LiDAR 组合定位的方式在NLOS 和有动态障碍物环境下仍然可以提供良好的定位精度。

表1 给出了三次动态实验的数据统计，分别为UWB定位、LiDAR定位和融合定位的均方根误差对比。

实验4分别对LiDAR 定位实验和融合定位实验中，添加人为遮挡因素，使其LiDAR传感器点云匹配失效，并记录重新匹配定位时长。表2中给出了本文融合定位算法与室内LiDAR定位算法在点云地图失配后重定位时长，结果表明，本文融合算法解决了LiDAR SLAM失配后的重定位时间长的问题，重定位平均时长较LiDAR SLAM算法降低了15.7倍。

图10 本文算法与单一传感器定位实验对比

表1 定位结果实验对比 m

表2 重定位结果实验对比 ms

4 结束语

针对现有无人驾驶车辆在室内环境（如地下车库，仓库）中由于卫星信号的丢失导致车辆定位精度低甚至无法定位问题，本文采用超宽带（UWB）定位技术与激光雷达（LiDAR）即时定位与地图构建（SLAM）技术并提出一种基于粒子滤波的多传感器互补误差消除融合定位算法。通过将UWB 和LiDAR 的量测数据进行基于粒子滤波的互补融合解算，提升了多传感器融合定位系统的定位精度以及稳定性，并提供良好的定位实时性。在室内环境下实验结果表明：相比单独使用UWB或LiDAR的定位技术来说，其定位精度被全面提升，平均误差控制在15 cm以内，同时该方法还解决了无人驾驶车辆起始点搜索以及点云匹配失效后的快速匹配的问题，从而提高了无人驾驶车辆定位的实时性和稳定性。综上，融合定位系统能够同时满足无人驾驶车辆需求的定位精度，实时性以及稳定性。