徐庆坤，王天皓，宋中越

（天津中德应用技术大学新能源系，天津300350）

1 引言

随着传感器与智能技术的发展，室内自主移动机器人定位算法也被持续地提出与改进[1]。近年来，室内定位中应用比较广泛的传感器有UWB、激光雷达、摄像头、IMU、里程计、超声波等。UWB（Ultra Wideband）[2，3]，即超宽带技术，此技术可追溯至19世纪，如今已发展为一种新兴的、全新的无载波无线通信技术。它集无线通信与实时感知定位于一体，与其他室内定位技术相比，它具有传输速率高、带宽极宽、功耗低、辐射小、抗干扰能力强、定位精度高、安全可靠性高等优点[4-5]，已成为性价比较高定位方法。但由于其具有超高的时间分辨率，因此，往往会受到非视距（non-line of sight，简称NLOS）[6-8]误差的影响，它是影响UWB定位精度与可靠性的主要来源。

为了有效抑制UWB定位的非视距误差，提高室内移动机器人的定位精度与可靠性，众多学者提出了很多算法。文献[9]提出一种以Chan算法为基础的非视距误差鉴别和消除的TDOA定位算法，首先利用Chan算法定位获得定位目标位置的初始估计，再通过TDOA测量误差鉴别非视距，最后通过将修正后的测量值代入Chan算法并获得定位目标的精确位置。文献[10]提出了一种基于UWB与激光雷达组合的室内定位方法，充分利用UWB的超高的分辨率、良好的抗多径能力以及激光雷达高效、高精度等特点，在复杂环境下将两种传感器数据进行EKF融合，从而获得厘米级定位。

综上所述，基于UWB与里程计两种不同类型的传感器，充分利用UWB定位精度高以及里程计短时定位效果好等特点，将二者进行融合，各取所长，从而获得高可靠性、高精度的定位效果。视距（line of sight，简称LOS）环境下，利用UWB高精度定位数据消除里程计长距离长时间所产生的累积误差；非视距环境下，利用里程计航迹推演算法获得机器人的定位信息来弥补UWB非视距引起的中断区域影响。

2硬件系统设计

本系统硬件由定位基站与定位单元两部分组成，其中定位基站由A0、A1、A2、A3组成，定位基站采用DecaWave公司出品的DW1000芯片作为UWB的收发器，它是一款厘米级的低功耗低成本的室内定位芯片，可用于双向测距或时差定位系统。定位单元安装于移动机器人平台上，主要由主控单元、编码器、标签T0与标签T1组成。编码器选用增量式编码器，脉冲数为1024，集电极输出方式。标签T0、T1与定位基站（A0、A1、A2、A3）的硬件组成完全一致，仅在软件上有所区别。主控单元选用32位STM32F103系列ARM微控制器作为定位系统的处理单元，它采用Cortex-M3内核，CPU最高速度可达72MHz。移动机器人定位系统硬件结构，如图1所示。

图1定位系统硬件框图Fig.1 Hardware Diagram of Positioning System

3定位技术分析

3.1 UWB定位分析

UWB定位相比于蓝牙或WIFI定位的方法不同，它不是基于信号强度（RSSI）方法实现定位，而是通过对无线信号的发送时刻与接收时刻进行精确捕获，进而计算得出基站与标签之间的精准距离，并通过相应算法解算获得定位信息。目前比较流行的两种定位方法为到达时间差法（TDOA，Time Difference of Arrival）与飞行时间测距法（TOF，Time Of Flight）[11-13]。TDOA利用到达时间差方法进行定位，定位标签仅需对外发射一次UWB信号，根据各基站接收到信号的时间差即可计算出距离信息，这种方法要求各基站必须为严格的时间同步，若不同步，此方法无效。

本系统采用TOF测距方法实现室内移动机器人的定位。TOF是一种双向测距技术，其双向测距算法流程，如图2所示。从图中可看出，标签定时发送带有时间戳Tsp的轮询信息，基站收到信息后记录时间Trp，并将带有时间戳Tsr的响应信息发送至标签，标签接收到信息后记录时间Trr，并将带有时间戳Tsf以及以上时间戳的信息发送至基站，基站接收到信息后通过式（1）与式（2）计算得出标签与基站间的距离。

式中：Tof—单程飞行时间；（Tsr-Trp）—基站的反应时间；（Tsf-Trr）—标签的反应时间。

式中：D—标签与基站间的距离；C—光速。

TOF测距方法的误差来源主要为随机误差与NLOS误差，其中随机误差服从高斯分布，影响不大，可通过滤波算法进行有效滤除；而NLOS误差具有偶发性，常常由于人为遮挡或者穿墙等因素的影响，致使测量数据较大偏离数据真值，甚至在受到严重遮挡时，会造成数据丢失而无法定位，因此，为了提高UWB定位的精度与可靠性，必须对NLOS误差进行抑制。

图2双向测距算法流程图Fig.2 Flow Chart of Two-way Ranging Algorithm

3.2里程计定位分析

里程计（Odometry）是一种根据传感器所检测到的数据以估计物体的位置与时间的关系的方法[14-16]，目前已广泛应用于移动机器人定位系统。目前里程计的航迹推演定位方法主要基于光电编码器来实现，通过在采样周期内采集光电编码器脉冲的变化量计算车轮的转动圈数，已知车轮的周长，进而间接地计算机器人的移动距离与方向角的变化量。

这种方法实现相对简单，但是这种定位方法存在以下缺点：（1）当移动机器人如遇到比较光滑的特殊地形，会造成车轮打滑空转现象，导致里程计出现无效的累积情况；（2）里程计航迹推演算法由于存在累积误差，不适合移动机器人长距离的定位场合。

3.3 UWB与里程计融合定位机制

针对上述室内移动机器人定位传感器存在可靠性差，随时间推移误差累积增大，复杂环境下非视距效应等问题，提出了一种基于UWB与里程计融合的定位机制，融合定位算法流程，如图3所示。

图3 融合定位算法流程图Fig.3 Flow Chart of Fusion Location Algorithm

（1）为了解决经纬度、环境障碍物等现场环境因素对UWB测距值的干扰影响，首先需要对UWB基站与标签进行一次校准标定；针对TOF测距法存在的随机误差问题，采用卡尔曼滤波算法对测距值进行滤波处理；

（2）针对TOF测距法存在的非视距误差问题，采用融合里程计方法来消除非视距对移动机器人定位的影响。首先利用NLOS误差鉴别方法进行鉴别，如鉴别结果为非视距场景，将采用里程计航迹推演的定位结果对UWB测距进行补偿；

（3）为解决里程计所产生的固然累积误差，通过NLOS误差鉴别方法鉴别现场环境，如鉴别结果为视距场景，将UWB高精度定位结果对组合里程计进行累积误差矫正。

4 融合算法

4.1 卡尔曼滤波算法

卡尔曼（Kalman）滤波是最佳的线性滤波器[17，18]，实现简单，广泛应用于导航、定位、传感器数据融合等领域。本文采用其对UWB测距值进行处理，有效地过滤了随机误差。卡尔曼滤波器的状态方程与观测方程分别为：

式中：k—离散时刻，A与H分别为系统的转移矩阵与观测矩阵；x→k—k时刻输出的状态；x→k-1—k-1时刻输出的状态；B—系统的控制矩阵；u→k—k时刻的控制量；ω→—系统的过程噪声；z→k—k时刻的测量值；v→k—系统的观测噪声。卡尔曼滤波算法的迭代过程为：

式中：P k|k-1—k-1时刻预测误差协方差矩阵；P k-1|k-1—k-1时刻估计误差的协方差矩阵；Q—系统的过程噪声的协方差，可由误差方差代替；K—卡尔曼增益，用于修正预测值；R—测量噪声对应的协方差矩阵，它反映了传感器的精度；I—单位矩阵。

4.2 里程计航迹推演算法

在做移动机器人定位导航或者方向控制时，通常会用到航迹推演。根据机器人轮子的编码器测得四个轮子的速度以及运动底盘的运动模型，可解算出机器人的线速度（Vx，Vy）与角速度ω，再将其进行积分处理获得机器人的坐标。

航迹推演算法，如图4所示。将机器人的起始点作为坐标系的原点，起始航向作为坐标系的Y方向，在较小的时间间隔d t下，根据几何算法可近似得出此时机器人的坐标（x1，y1）与航向角β。推导公示，如式（10）与式（11）所示。

图4 航迹推演示意图Fig.4 Diagram of Odometry

将公示化简得：

4.3 里程计累积误差矫正

里程计航迹推演算法的一个缺点是推导出的位置值误差与不确定性是累积的，会随着时间增加而增长，因此其定位精度取决于初始位置与航向估计，而UWB定位属于绝对定位方法，在视距情况下它具有较高的定位精度。对于位置累积误差，本文采用累积误差矫正方法来清除里程计的位置累积误差，首先根据NLOS鉴别方法鉴别环境情况，如为视距场景，那么将UWB的定位坐标作为里程计的初始位置。对于航向估计累积误差，本文采用双标签航向解析方法来清除里程计的航向误差累积，首先求解UWB标签T0与标签T1的定位坐标，然后对坐标进行航向解析，最后将航向结果作为里程计的初始航向估计。

4.4 UWB非视距补偿方法

卡尔曼滤波迭代算法可有效地抑制随机误差对测距值的影响，但对于较大的NLOS误差，如标签与基站之间存在障碍物等干扰时，其抑制效果明显降低，因此，在对UWB基站进行环境布局时应充分做好测试环节，保证UWB定位标签不能出现长时间丢失信号状态。在此前提下，本文提出一种非视距补偿方法。

通过引入NLOS误差鉴别法来鉴别现场环境。首先，常态下取，式中，→zk—实际观测值—状态向量的预测值。通过将二者的差值e与阈值TH进行比较，如果e小于阈值TH，可认为此时为LOS场景；如果e大于阈值TH，或者丢失的时间大于设定的失效时间时可认为此时为NLOS场景，此时将里程计获得的定位坐标经进行反解算，从而得到标签与各基站的距离信息，再将各距离信息作为卡尔曼滤波的测距值。

5 定位实验

本文实验安排在工业实训中心E1-5楼车间进行。实验平台选用自主开发的仓储物流机器人平台，如图5所示。它选用四个麦克纳姆轮作为机器人的车轮，每个车轮分别安装了电机与编码器；选用UWB Mini 3s模块作为本实验的定位产品，它采用DW1000芯片作为UWB收发芯片，通讯距离在无遮挡情况下可达80m，实验中使用三个基站（Anchor）与两个标签（Tag）组成，测试时将基站固定在三脚架上，距离地面1.5m以上。测试实验场地布局，如图6所示。各基站分别固定在距离地面1.5m的三脚架上，并布置在测试区域的角落。

图5 移动机器人平台Fig.5 Mobile Robot Platform

图6 实验场地Fig.6 Experiment Site

5.1 UWB测距数据标定

由于现场环境不同，受经纬度、海拔等因素的干扰，须对UWB模块测距值进行校准，校准只需在现场对1个基站（Anchor）与1个标签（Tag）进行校准一次即可。具体标定方法为：基站A0固定不动，将安装有标签T0的移动机器人通过手动遥控方式从1m位置移动至10m位置，步长为1m，共选取10个点，在每个点的位置处测量100组测距数据，再经求平均值得到该点的测距值，以此类推，共得到10组UWB测距值，再利用线性方程y=k x+b进行数据拟合，最终获得修正系数，测距数据拟合直线，如图7所示。

图7 测距数据拟合图Fig.7 Fitting Diagram of Experiment Data

5.2 组合定位实验

实验选取测试场地中P1、P2、P3三点进行运动测试，测试路线，如图6所示。实验过程中，通过手动控制机器人以0.5m∕s的速度从P1点匀速前进至P2点位置，之后右转弯前进至P3点位置。实验的测试结果，如图8所示。

图8 融合定位实验Fig.8 Fusion Positioning Experiment

红色加号为UWB原始定位轨迹，黑色圆圈为实际位置轨迹，如图8（a）所示。从图中可看出：UWB定位在视距情况下，定位精度较高，当经过立柱障碍物时，由于受到NLOS影响，致使图8（a）中A与B区域点位严重偏离实际位置，由表可看出其最大误差达到1.77m，平均误差达到了0.38m。蓝色叉号为里程计定位轨迹，如图8（b）所示。从图中可看出：从起点P1开始至P2位置这段轨迹定位效果较好，但在经过一段时间拐弯后，累积误差开始显现出来，由表可看出最大误差0.49m，平均误差为0.29m，如果路线更长时间更久误差会随之加大。粉色方框为利用本文融合UWB与里程计算法计算的轨迹，如图8（c）所示。在LOS环境下的轨迹与UWB定位精度相当，但在NLOS环境下，通过NLOS误差鉴别与融合算法有效地滤除了NLOS误差的影响，即使出现严重的测距信息丢失现象，仍能通过里程计进行航迹推演，另外在LOS环境下，UWB测距对里程计进行实时修正，有效地抑制了航迹推演的累积误差，静态点测试数据，如表1所示。由表可看出最大误差为0.31m，平均误差为0.2m。因此，本文所采用的融合算法有效地抑制了NLOS误差的影响，提高了室内移动机器人定位精度与可靠性。

表1 静态点测试数据（mm）Tab.1 Experiments Data of Static Point

6 结语

设计了一种基于UWB与里程计融合的定位系统，通过在传统的UWB定位算法基础上引入NLOS误差鉴别方法，如鉴别为LOS场景，将根据UWB测距值进行卡尔曼滤波处理，滤除掉随机干扰误差，再经最小二乘法获得机器人的定位坐标，而机器人的位姿本文采用双标签坐标解算法，最终获得机器人的坐标及位姿，同时将获得的高精度定位结果对里程计进行累积误差修正；如鉴别为NLOS场景，将里程计获得的定位坐标通过反解算获得测距值对UWB定位进行补偿。实验结果表明，设计的定位系统有效地提高了基于UWB定位方法的定位精度与可靠性，尤其是当UWB信号受到严重影响无法获得测距值时，仍可解算出机器人的定位数据，对于室内移动机器人定位系统的设计场合具有一定的参考意义。