0 引言

    移动机器人定位是其自主导航中的最基本环节，也是移动机器人完成任务必须解决的问题。对定位的要求是定位精度高（亚米级精度），实时性好。目前移动机器人室内定位主要分为两大类：(1)相对定位法，即航迹推算法^[1]。利用机器人所装备的各种传感器获取机器人的运动动态信息，通过递推累计公式获得机器人相对初始状态的估计位置。使用的传感器主要是码盘和惯性传感器。但是他们都有一个共同的缺点：存在累积误差，随着行驶时间、距离的不断增加，误差也不断增大，不适合长时间长距离的精确定位。(2)绝对定位法，即机器人通过获取外界一些位置等已知的参照信息，通过计算自己与参照信息之间的相互关系解算出自己的位置。绝对定位法主要采用同步定位与建图^[2]（Simultaneous Localization and Mapping，SLAM）、视觉定位方法以及基于信标定位等方法。其中SLAM定位法和视觉定位数据量大，价格昂贵,目前只适宜试验研究使用。而且，这两种方法只适用于一些结构简单的环境，对移动机器人一般工作的复杂室内环境并不能提供很好的定位精度。因此，本系统选用基于UWB的信标定位的方法来实现复杂室内环境下的移动机器人室内定位。

    UWB信号具有超高分辨率、抗多径效应、穿透力强以及结构简单的优点，成为目前室内高精度定位的最佳技术^[3]。常用的UWB定位方式为基于到达时间法(Time of Arrival，TOA)以及到达时间差法(Time Difference of Arrival，TDOA)。但是，这TOA法需要标签与基站之间时钟同步，TDOA法需要基站与基站之间的时钟同步，这增大了系统设计难度。

    本文基于UWB技术，采用decaWave公司生产的DWM1000模块，应用非对称双边双向测距(Asymmetric Double Sided Two-Way Range，ADS-TWR)技术进行定位，无需基站与标签之间和基站与基站之间时钟同步，大大缩减系统设计难度。针对实际应用中由非视距传播引起的测量误差，采用卡尔曼滤波算法对测距进行优化，提高定位精度。最终实现一种高精度、高实时性的移动机器人室内定位系统。

1 系统总体设计

    移动机器人室内定位系统主要由UWB无线传感器网络和上位机显示软件两部分组成，系统示意图如图1所示。系统硬件部分包括基站和安装在移动机器人顶端的标签。其中基站分为普通基站(基站2、基站3和基站4)和通信基站(基站1)。标签和基站均由单片机和DWM1000组成的通信模块构成，并由软件配置模块的角色(标签或者基站)。DWM1000可以精确地测量UWB信号发送和接收的时间点，通过ADS-TWR 技术测得标签到各个基站之间的距离，利用UWB通信功能，各基站将距离信息发送给通信基站，最终通过WiFi将距离信息传送给上位机进行定位并显示。通信基站的硬件结构图如图2所示（标签和普通基站没有WiFi模块）。

2 基于ADS-TWR技术的测距及优化

    无线定位系统定位的准确性取决于测距的精度。UWB定位系统测距误差的来源除了非视距引起的误差外，还包括晶振的时钟漂移引起的误差。晶振时钟漂移会影响对信号发送和接收时间点的测量，继而影响测距的准确性。UWB定位系统最简单的测距方式是单程测距（One Way Ranging，OWR），但是这对节点之间的时钟同步要求极其严格，而双程测距（Two Way Ranging，TWR）虽能消除节点之间未能完全同步的影响，但无法消除晶振时钟漂移的影响。而对称双边双向测距（Symmetry Double Sided Two-Way Range，SDS-TWR）可以消除晶振时钟漂移的影响，但是要求信号回复时间严格相等，这样大大降低了定位的实时性^[4]。在此采用ADS-TWR测距技术。

    ADS-TWR测距过程如图3所示，图中poll_TX、poll_RX、answer_TX、answer_RX、final_TX、final_RX代表UWB信号离开标签和基站天线的时间点。测距过程如下：首先标签向基站请求帧；基站收到请求帧后启动计时，经过延时T_reply1后向标签发送应答帧；标签收到应答帧后启动计时，并将发送和接收信号时的时间点写入终止帧，经延时T_reply2后发送给基站；基站收终止帧后表示测距结束。

    距离计算公式如式(1)和(2)所示：

其中，k_t和 k_a为标签和基站时钟偏移系数，二者都接近于1。对于20 ppm的时钟(最坏规格的时钟)，则k_t和k_a都可以是0.999 98或1.000 02。对于相对较大的测距范围如100 m，T_tof仅为333 ns，飞行时间测量误差为6.7 ps，换算为距离误差仅为2.2 mm。因此，ADS-TWR测距能很好地抑制时钟漂移的影响。

    ADS-TWR测距不要求应答时间T_reply1和T_reply2相等。因此，在标签与多个基站进行通信时，可以通过设定各节点的应答时间来减少标签测距的时间，保证定位系统的实时性。

    图4为本文所采用的多基站测距机制。标签向4个基站发送请求帧，基站接收到请求帧后按照设定的应答时间依次向标签发送应答帧，标签接收到应答帧后，将4个基站用于计算距离参数写入终止帧，并发送给所有基站；各基站接收到终止帧后测距结束。基站利用式(1)和(2)计算出距离，然后通过UWB发送给通信基站。实际应用中，对于四基站定位系统，通过优化各基站应答时间，可使单轮测距时间能控制在2 ms左右，完全可以满足移动机器人定位对实时性的要求。

3 基于卡尔曼滤波的定位算法

    移动机器人在室内活动，不可避免地受到遮挡的影响。标签与基站之间由于存在人或座椅等遮挡物，这时UWB信号不能进行直线传播，而是利用衍射、透射和反射的方式到达接收端，就是非视距传播（NLOS）。这时系统对飞行时间的测量就是会出现误差，而定位精度也就有了误差。非视距误差受到室内环境影响，是一个实时变化的值。由于非视距传播增加了信号的传播时间和传播距离，因此非视距误差服从一个正均值的随机过程^[5]。

    假设t时刻标签到基站的距离为d_i(t)，r_i(t)表示二者之间的真实距离，N_i(t)表示观测时由环境因素引起的非视距误差，n_i(t)为零均值高斯噪声，则可用下式表示它们之间的关系：

    本文采用卡尔曼滤波算法对距离进行优化，对NLOS误差值进行迭代处理，以此来减小定位误差。首先利用状态向量方程对距离信号进行卡尔曼滤波，估计出NLOS误差值N_i(t)，然后从最初的测距值d_i(t)中除去非视距误差，得到精确地距离值^[6]。系统的状态方程和测量方程如下：

其中，Δt为采样间隔；ω_d(t)和ω_N(t)分别为测量过程中的噪声误差分量；β为实验参数；v_i(t)为测量误差。再给出t时刻的状态向量和估计误差的协方差的初始值之后，就可以通过迭代运算对不同时刻的状态向量做出估计。由于非视距误差具有非负性，因此在迭代过程中如N_i(t)出现负值就强制置零。

    根据基站坐标和标签到基站的距离可以得到以下方程组：

其中(x_i，y_i，z_i)为基站i的坐标，d_i为经过卡尔曼滤波优化后标签到基站i的距离。用第i个式子减去第j个式子得：

4 系统测试

    实验地点为12 m×8 m×6 m的南开大学微纳加工实验室，该环境下NLOS干扰现象比较严重。为减小人工对真实路径测量所带来的误差，本文实验结果与实验室内QUALISYS视频式三维运动采集系统的定位结果进行对比。通过在标签上粘贴被动标记获得QUALISYS系统的定位结果，其定位精度可达到亚毫米级。试验场景照片如图5所示，标签安装在移动机器人的顶端。

    系统测试分为两部分，分别对静态点和动态点进行定位实验。在试验场地内随机选择10个点作为定点实验位置，分别做定位实验 ，每个点采集500次定位数据。实验结果如图6所示，其中方框代表 QUALISYS系统测出的结果，小黑点为本系统定位结果。

    利用均方根误差（RSME）对实验数据进行分析，计算结果如表1所示。可以看出，在室内NLOS环境下，基于卡尔曼滤波的定位方法在定点定位中误差能控制在13 cm以内。

    进一步根据移动机器人实际定位的需求，随机选择一个标签以1 m/s的速度在沿着设定路线进行移动，测试结果如图7所示，其中深色轨迹为QUALISYS系统的定位结果，浅色轨迹为所设计系统定位结果。可以看到标签偏离目标估计的最大距离为20 cm，保证了移动机器人移动时的定位精度。

5 结语

    本文针对目前移动机器人室内定位方式灵活性差和精度不高的问题，利用UWB技术，设计了一种高精度移动机器人室内定位系统。一方面采用ADS-TWR测距技术保证定位系统的实时性和测距精度；另一方面采用卡尔曼滤波方法进行定位，滤除非视距误差，保证系统的定位精度。实验表明，系统具有高精度、高稳定性的特点，满全能满足移动机器人室内定位的需求。

参考文献

[1] CHUNG H，OJEDA L，BORENSTEIN J.Accurate mobile robot dead-reckoning with a precision-calibrated fiber-optic gyroscope[J].Robotics & Automation IEEE Transactions on，2001，17(1)：80-84.

[2] 王炜强.基于视觉定位的地图构建方法研究[D].浙江：浙江大学，2010.

[3] 房秉毅.基于超宽带技术的室内定位系统[J].电子技术应用，2006，32(7)：124-127.

[4] 陶偲.基于UWB的室内SDS-TWR测距算法优化和定位算法融合的研究[D].武汉：华中师范大学，2016.

[5] 张宴龙.室内定位关键技术研究[D].合肥：中国科学技术大学，2014.

[6] ZHANG L，ZHANG H，CUI X R，et al.Ultra wideband indoor positioning using Kalman filters[C].Advanced Materials Research，2012，433-440：4207-4213.

作者信息:

卢靖宇1，2，余文涛1，2，赵  新1，2，孙广毅1，2

（1.南开大学 计算机与控制工程学院，天津300350；2.天津市智能机器人技术重点实验室，天津300350）