基于UWB的室内移动机器人定位算法研究

基于UWB的室内移动机器人定位算法研究

王娟,雷铭,蔡洪炜

柳州工学院 广西柳州 545000

摘要：本文针对移动机器人在室内环境中因运动和传感器测量噪声导致定位精度不高的问题，采用基于TDOA测距算法的超宽带（UWB）技术获得移动机器人在室内环境中的全局位置信息，对比研究了基于扩展卡尔曼滤波（EKF）和无迹卡尔曼滤波（UKF）算法实现的多传感器数据融合。实验结果表明，两种方法均能有效提高运动目标的定位精度，在系统非线性条件下，UKF算法的定位精度更高。

关键词：移动机器人 UWB定位 卡尔曼滤波

0 引言

近年来，随着“中国制造2025”战略的不断推进，智能工厂中对各种机器人的需求量和智能化程度要求都在增加，移动机器人作为自主智能机器人的一种，广泛应用在智能工厂的仓储物流、巡线、生产搬运等环节。机器人定位和自主导航是移动机器人区别于其他智能机器人的主要特征功能，实时精确定位是其自主导航的先决条件。同步定位与地图构建（Simultaneous Localization and Mapping，SLAM）是指机器人对周围未知环境信息进行组合感知，在移动的同时完成环境地图的构建并不断对自身的位姿进行修正的过程，已成为移动机器人领域的研究热点。

SLAM问题的研究方法众多，主要可分为基于概率论的方法和模拟生物神经激励系统的方法。前者以卡尔曼滤波算法、粒子滤波算法为代表，通过各种传感器的测量数据不断估计机器人状态和参数。这些方法主要依赖各种满足工程实用性的传感器和高性能的计算能力，在假设机器人所处环境是静止不变的情况下，取得了很好的实用效果。

如果能获取绝对位置信息，移动机器人在室内已知场景中的定位将更简单，但是室内环境无法使用全球卫星系统（Global Navigation Satellite System，GNSS）信号，仅通过搭载的雷达、相机无法获得准确的全局绝对位置信息，因此研究者们开始研究各种无线通信技术在室内定位中的应用。近年来，超宽带（Ultra Wide-Band，UWB）定位技术凭借其高精度、低成本优势获得研究者青睐，被广泛应用在移动机器人室内定位中。

本文在研究UWB定位的基础上，采用卡尔曼滤波将UWB测距信息进行滤波，实现移动机器人的协同精确定位，针对移动机器人运行状态和环境的非线性特性，采用EKF和UKF滤波算法进行对比分析。

1 UWB定位算法

利用UWB进行移动机器人定位分为测距和解算两步，首先通过基站与移动机器人的UWB模块通信获取定位解算相关的基础参数数据，然后根据测距参数解算出移动机器人的位置。无线测距方法有很多种类，UWB定位中常用的是基于到达时间差（Time Difference Of Arrival，TDOA）和基于飞行时间（Time Of TOF）的定位。

本文采用基于到达时间差的TDOA算法，利用不同锚点与移动标签的TOA差值来计算机器人位置，能减轻由于室内环境因素对测量的影响。具体过程如图1所示，假设有三个锚点（xi，yi）（i=1,2,3），根据TDOA定位原理，可以得到：

       (1)

其中d1，d2，d3分别表示锚点到标签的距离，d21，d31表示锚点到标签的距离差值，由于锚点间时钟同步，有：

                                 (2)

根据标签信号到达锚点的时间差，可求出d21和d31，根据公式1确定标签位置（x,y）。

图1二维空间TDOA定位算法模型（理想情况）

2无线时钟同步的实现

TDOA定位中，标签将数据包发送到基站覆盖区域，附近基站收到信号后无需应答，单次通信即可完成标签位置解算所需的时间差，各基站时钟同步是准确位置解算的前提条件。

本文通过选择温补型晶振和PTP协议从硬件和软件上完成主基站和从基站之间的无线时钟同步，根据IEEE1588标准中的的延迟请求-响应机制，完成主基站和从基站的时间同步。具体过程如图2所示，主基站在t1时刻发送sync，从基站在t2时刻收到报文，之后从基站在t3时刻发送Delay_Req，主基站在t4时刻收到Delay_Req，Delay_Resp里包含了从基站时钟同步需要的时间。

图2 延迟请求-响应机制

从以上的7个步骤中，从基站获取到精确时间戳t1~t4，用于同步计算，可列出方程组：

                           (3)

式中，toffset：表示主时钟与从时钟之间的时间偏差；

tdelay：表示网络延迟，假设网络延迟是不变的。

根据式（3）可求得：

                        (4)

根据时间偏移toffset，即可调整从时钟时间，实现各基站间的时钟同步。

3.基于EKF和UKF的滤波定位算法

UWB在LOS（Line-Of-Sight）环境下定位精度可以达到厘米级，但是在复杂工业环境下常常会受到障碍物阻挡，使得UWB信号发送和接收到达时间差比实际飞行时间更长，产生NLOS（None-Line-Of-Sight）误差，影响定位精度

[1]。减少NLOS误差，除了改善UWB传输的物理环境和增加基站数量，还可以通过与其他传感器组合使用来获得更好的定位效果。

本文采用两种卡尔曼滤波算法进行多传感器数据融合，融合算法有EKF和UKF。两种方案的区别主要在于融合算法，系统模型和观测方程是相同的。具体的思路如下：首先UWB通过TDOA测距方式解算得到UWB与各基站的距离信息，IMU子系统通过航迹推算算法得到IMU的定位信息（），将IMU定位点与各基站的距离信息与UWB的距离信息进行关联，经过EKF数据融合，得到坐标偏差的最优估计值（），用于修正IMU的位置坐标[2]。

将IMU航迹解算得到的速度误差()和位置误差()作为机器人的状态向量，有：

状态方程为：

                               (5)

式中，为系统的状态噪声，且；

F为转移状态矩阵，设系统采样周期为，有：

以k时刻移动机器人分别通过IMU和UWB与各基站的距离的平方差作为观测向量，建立观测方程：

                                 (6)

式中，为系统的测量噪声，且。

建立机器人的运动模型和观测模型后，分别使用EKF算法和UKF算法使用UWB观测数据对IMU测量数据进行修正，预测并更新移动机器人的位置，两种算法流程如图3和图4。

图3 EKF算法流程图              图4 UKF算法流程图

4.实验结果及分析

为验证融合算法的有效性，本文选择一个5m×2.5m的场地测试UWB组合定位算法，，UWB测距系统由5个UWB基站和1个标签组成，在Matlab中导入实际运行轨迹数据和UWB测距信息，然后使用两种滤波算法实现数据融合。图5展示了移动机器人实际运动轨迹、UWB定位轨迹和EKF、UKF组合定位轨迹，绿色线表示机器人实际运行路径，黑色线（+）表示UWB测距解算路径，红色线（*）表示采用EKF融合估计路径，蓝色先表示采用UKF融合估计路径，对比仿真结果可以看到，采用EKF和UKF滤波后的路径与实际路径更加一致，基于UKF的滤波算法对理想路径的估计偏差远小于基于EKF的滤波算法，从图6和图7两种算法对机器人估计误差的对比更能直观看出修正效果。

图5 基于UWB和EKF/UKF的移动机器人定位及轨迹仿真

图6 X轴误差                        图7 Y轴误差

5 结束语

本文中设计的基于UWB的室内移动机器人定位系统，以移动机器人IMU速度和位置信号为状态变量，以UWB采集的标签和基站距离差为观测量，在Matlab中对移动机器人IMU和UWB定位数据进行EKF和UKF算法的融合仿真，仿真结果与真实轨迹对比可以明显看出融合算法能有效提高定位精度，而UKF比EKF算法在提高定位精度、减小误差方面表现更优，能更好的适用于非线性系统。

[参考文献]

[1]周琳. 基于UWB DR的室内移动机器人组合定位方法研究与实现[D].浙江理工大学,2020.

[2]李义. 基于UWB与IMU组合的室内移动机器人定位方法研究[D].电子科技大学,2021.

基金项目：2019年度广西高校中青年教师科研基础能力提升项目（2019KY1103）

作者简介：王娟（1983-），女，讲师/工程师，主要研究方向：物联网技术、智能控制。