移动机器人室内外无缝切换定位方法研究

刘慧敏，李国丽，董 翔，张 鑫，汪 鹏

（安徽大学，合肥 230601）

0 引言

随着科学技术的不断发展和移动机器人涉及领域的逐渐扩展，对既能实现室内定位也能实现室外定位机器人的需求日趋强烈[1]。为了实现室内外环境下定位的连续性和平滑性，使其能够无缝衔接，无缝定位技术应运而生[2]。GPS具有定位误差较小、覆盖范围广、用户投入成本低等优点，广泛应用于室外定位技术，但其单独使用时会存在抗干扰能力较差以及信号易被遮挡等不足。惯性导航系统（Inertial Navigation System，INS）作为基于航位推算原理的非视距定位传感器，抗干扰能力强，短时间内的定位精度较高，但长时间的定位容易产生累积误差造成漂移。因此本文选择GPS和INS组合作为室外环境定位系统。Wi-Fi、蓝牙、UWB等都可以实现基本的室内定位，但在定位精度，投入成本，复杂度上有所不同。Wi-Fi指纹库需要时常更新维护，人工成本较大；蓝牙虽然体积小，造价低，但易受多信道噪声干扰，稳定性差；超宽带（ultrawide band，UWB）可靠性高，信号穿透性强不需要载波，其定位帧可以在多噪声无线信道环境下进行可靠地数据传输，信号传输距离远且传输速度较快，不易受外界干扰，定位精度可达到厘米级、系统较简单。因此本文选择UWB和INS组合作为室内环境定位系统。文献[3，4]提出室外采用北斗与GPS的双模定位，室内以及卫星信号缺失的情况下采用基于IMU的航位推算算法，无法满足长时间室内复杂环境下的定位精度要求。文献[5]采用赫尔默特方差估计分类法对两种定位系统定权，没有设置阈值来避免运算浪费。

本文实现了基于阈值机制和多传感器定位精度的室内外定位无缝切换，使系统在运动过程中进行有效的定位方式切换以实现当前区域下最精确的定位，同时避免传感器间反复不必要的切换造成运算量的浪费。在室外环境下使用GPS/IMU组合定位来提供较精确的定位服务，在如遮挡处、室内等无法感知GPS卫星信号的环境下，系统切换至UWB/IMU组合定位，从而实现室内外的无缝精准定位。具体工作如下：

1）构建机器人全局地图；

2）使用容积卡尔曼（CKF）实现室外GPS/IMU以及室外UWB/IMU融合定位；

3）利用基于阈值机制和多传感器定位精度的室内外定位的无缝切换算法实现室内外定位的无缝切换。通过实验平台验证了该算法提高了室内外切换的中间区域定位的精度，使切换更加平滑稳定，优于传统的算法。

1 全局地图的构建

为了方便组合定位，需要将各个定位系统输出的移动机器人位置坐标等信息转换至统一的坐标系中，这是实现室内外无缝定位的基础和前提。地理坐标系又叫做东-北-天坐标系，该坐标系以载体中心为坐标原点，x轴、y轴、z轴分别指向东、北、天方向。本文选择该坐标系作为全局坐标系（n系）。

GPS测得的经纬度坐标需要先转换至地心地固坐标系下再转换至移动机器人全局坐标系（n系）。选取机器人定位区域内的某一点为全局坐标系原点，并测得原点的经纬度坐标（lon0，lat0，alt0）。地心地固坐标系（ECEF系）下的某一点经过旋转矩阵Cn

e可以得到全局坐标系下的坐标。

机器人采用的是捷联式惯性导航系统（SINS，Strapdown Inertial Navigation System），陀螺仪、加速度计以及磁力计直接固定在载体上。在运动过程中，磁力计可以测得载体相对于真北方向的角度，陀螺仪可以测得载体相对于惯性参考系的角速度，从而得到载体运动过程中的横滚角、俯仰角、偏航角。惯性导航输出的载体坐标系（b系）下的坐标通过旋转矩阵转换至移动机器人全局坐标系（n系）下，旋转矩阵为Cnb。将加速度计测量得到的载体坐标系下的加速度以及陀螺仪测量得到的载体坐标系下的角速度经过旋转矩阵变换至移动机器人全局坐标系下是惯导解算的前提。

其中c代表三角函数cos，s代表三角函数sin，γ、θ、ψ分别为载体横滚角、俯仰角、偏航角。

2 室内外定位方法

2.1 室外环境下的GPS定位

在定位区域内设定一点为导航坐标系（enu坐标系）的原点P0，经纬度坐标为(lon0，lat0，alt0)，P0点在ECEF系下对应坐标为(x0，y0，z0)。机器人运动过程中某一点Pi(loni，lati，alti)，可以通过如下过程转换至导航坐标系下。其中，(xi，yi，zi)为该点在ECEF系中对应的坐标，(ei，ni，ui)为该点在导航坐标系下的坐标。

其中，e为椭球体偏心率，N为基准椭球体曲率半径。

图1 地图坐标系示意图

2.2 室内环境下的UWB定位

目前的室内定位方案主要有基于RSSI（接收信号强度）的蓝牙定位技术、基于反射式测距法的超声波定位技术、基于TOF（飞行时间）的UWB定位技术、基于指纹定位的WIFI定位技术等。本文选用UWB作为室内定位传感器，采用TWR测距。

UWB的定位帧中的的测距信息由定位请求信息、应答信息以及最终信息组成。以测得标签与基站1之间定位信息的飞行时间TOF为例，需测得标签接收到基站1应答信息的时间TRR与标签发送定位请求信息时间TTP之差、基站1接收到标签发送的最终信息时间TBF与基站1发送应答信息时间TTR之差、标签发送最终信息时间TTF与标签接收到基站1应答消息时间TRR之差、基站1发送应答消息时间TTR与基站1接收到标签发送定位请求信息时间TRP之差。标签与各个基站的距离d可以用TOF与光速c的乘积得到。

图2 双边测距原理图

实现测距后，首先确定各个UWB基站在导航坐标系中的位置坐标，基站坐标的精确度直接影响定位的精度。在得到移动机器人距离基站的距离后，可对移动节点进行定位[6]。已知距离机器人最近的三个基站及坐标为B1(x1，y1)、B2(x2，y2)、B3(x3，y3)。R(x，y)为待求的机器人坐标，标签距三个基站的距离分别为d1、d2、d3。B1与B2之间距离为l12，B1与B2之间距离为l13。以B1、B2为焦点，作过(x，y)的双曲线，再以B1、B3为焦点，作过(x，y)的双曲线，如图3所示。

图3 双曲线定位原理图

其中：

根据余弦定理，可得：

整理可得：

同理可得：

可得移动机器人的位置坐标。

2.3 惯性导航系统定位

惯性导航系统（Inertial Navigation System，INS）由加速度计（Accelerometer）、陀螺仪（Gyro）、磁力计（magnetometer）构成。通过磁力计的指北功能可以得到惯性导航模块相对于地理坐标系的偏转角度。通过对线加速度和角加速度的量测，进行积分运算从而推导出载体的位置、姿态和速度信息。

加速度计可以测出三轴的比力，将所测比力信息通过坐标转换，去除地球自转角速度以及重力加速度的影响后得到速度微分进而得到三轴的速度信息。同样，已知载体上一时刻的位置坐标及惯性导航系统的采样周期为T，将已经解算出的速度进行周期性的积分即可得到这一时刻的位置坐标。

3 基于容积卡尔曼滤波器的融合定位

当系统模型是非线性的，需要非线性滤波方法来进行数据的融合，容积卡尔曼滤波器通过对非线性系统的概率分布进行近似输出，这要比通过复杂的雅可比矩阵求解将非线性系统近似为线性系统要容易且精度更高。CKF利用一组等权值的容积点来进行状态和协方差的更新[8]。

图4 容积卡尔曼融合定位系统框架

惯导系统的误差值作为状态变量，维数为15，ΔR为位置误差，ΔV为速度误差，Δϖ为姿态角误差，Δ为加速度计零偏误差，ε为陀螺仪零偏误差[9]。

将INS和GPS/UWB解算得到的位置、速度信息作差作为滤波器的观测量Z，维数为6，RINS和VINS为n系下INS解算出的位置和速度，RGPS/UWB和VGPS/UWB为n系下GPS/UWB解算出的位置和速度。

由IMU运动学方程可得系统的非线性状态方程。

由GPS量测数据和UWB量测数据可得系统的非线性观测方程。

其中αk和βk分别为系统过程噪声和系统测量噪声。容积卡尔曼滤波器通过对后验概率密度进行高斯近似来实现系统的状态估计，对一组等权值的cubature采样点的加权计算来近似求解积分，得到状态变量的均值和方差。Fk为估计协方差的一个平方根，。cubature采样点的个数为2n，n为状态变量的维数。η为cubature采样点的点集，点集的第i个元素为ηi，该点对应的权值为εi[10]。

其中[1]代表将n维单位向量e=[1，0，...，0]T的元素进行全排列并改变符号产生的完整全对称点集的第i个元素。

1）时间更新

非线性状态方程变换后的采样点：

2）测量更新

非线性量测方程变换后的采样点：

4 室内外无缝切换定位方法

无缝切换定位技术是指机器人在复杂环境内运动过程中可以根据室内外环境的特征，选择最佳的定位方式以保证定位的精准性，同时在进行室内外定位方式切换时需要满足定位的无缝衔接、平滑稳定，最终实现机器人在室内室外以及室内外中间区域全覆盖高精度的定位。GPS的定位精度不仅仅与可视卫星数量有关，如果只根据GPS搜索卫星数量是否大于4来作为定位切换的条件，无法达到无缝切换的要求。本文提出一种基于阈值机制和多传感器定位精度的室内外定位切换算法，可以通过传感器实时定位精度来决定各传感器系统的置信度在切换区域内实现室内外两种定位系统的充分融合，避免出现定位跳变和由于在临界区域内反复无效的定位切换产生乒乓效应导致的大量运算浪费。

定位系统间切换时基于对两套定位传感器的精度的判断，采用多个门限阈值同时也会降低切换出错的概率。在GPS定位中，几何精度因子（Geometric Dilution Precision，GDOP）是衡量定位质量的好坏的重要参数，这意味着各种延迟和噪声造成的距离误差会被几何放大。GDOP的大小受所能观测到的卫星在空间中几何分布的影响，若当前可见卫星数量不少于4且在空间中分布较为分散定位结果可以获得更高的准确率。GDOP的数值越大，当前时刻的定位精度越差。用户等效测距误差（User Equivalent Range Error，UERE）包含用户测距误差（User Range Error，URE）和用户设备误差（User Equipment Error，UEE）[11]。URE主要取决于卫星钟差、卫星的位置和星历误差，和机器人平台的位置无关。UEE与机器人所处环境有关。通过GDOP和UERE可以评估GPS定位的精度PGPS。

由于UWB定位误差与信标和基站之间的几何分布关系、信号强度的大小、标签当前处于静止状态还是运动状态以及磁场干扰等因素有关，所以机器人在实验场景的不同区域的UWB定位精度各不相同。对实验场景中的室内外中间区域的UWB的定位数据进行多次采样，并计算出该区域内标签与基站不同距离对应的UWB定位精度PUWB。

在室内与室外区域间有一个中间区域，在这个区域内两套定位传感器的定位信息都能接收到，当机器人从其中一个定位系统服务区域运行到中间区域时，接收到了另一个定位系统的信号，如果此时直接采取定位切换，最终的定位结果就会与无缝衔接、平滑稳定的需求相违背。考虑到两套定位系统的定位精度不同以及定位切换算法不能过于繁琐，可以对两个定位系统的定位结果进行加权融合。

式(32)中，(x，y)为加权融合后的定位坐标，(xGPS，yGPS)为GPS定位坐标，(xUWB，yUWB)为UWB定位坐标，k1为GPS融合权重，k2为UWB融合权重。

GPS系统的定位误差（δxGPS,δyGPS）在二维空间中服从二维正态分布。UWB系统的定位误差（δxUWB,δyUWB）同理。两个系统在室内外中间区域融合后的误差为（δx,δy）。ς1、ς2分别为GPS、UWB定位误差均方差。

融合的目的是为了让定位误差最小化，所以需要满足下式：

由此可确定符合融合目的的权值范围：

图5 室内外无缝切换算法流程图

无缝切换算法中引入计数值n、计数阈值N、减小率R（0＜R＜1，R的大小需要根据传感器频率设定）、UWB定位精度的两个门限阈值α1和α2（其中α1＜α2）。启动定位后，首先对GDOP的大小进行判断，若GDOP小于3，说明此时GPS模块可以提供较为精准的定位，当前可观测卫星空间分布状况良好，机器人处于无遮挡的室外环境，选择使用GPS/IMU组合定位；若3＜GDOP＜7，且UWB的定位精度满足条件α1＜PUWB＜α2，说明此时机器人可能处在室内外切换的中间区域，满足触发切换的条件，此时计数值加1，当大于计数阈值N时，进行切换判决阶段，确定机器人处于中间区域，此时采用基于定位精度的加权平均的算法融合GPS和UWB的定位结果，计数阈值的使用可以有效避免乒乓效应的产生；若GDOP＞7且满足PUWB＜α1，机器人可能已经运行到了室内，此时GPS基本为无效定位，如果继续采用GPS进行定位，会使定位结果产生漂移，这时将计数值乘上一个减小率R，如果定位精度一直满足当前的条件，直到小于计数阈值N时，即可确定移动机器人处于室内，系统切换到UWB/IMU组合方式进行定位。

5 实验及分析

为了对本文的定位方法进行验证，在安徽大学校磬苑校区内使用机器人小车搭载的模块进行数据采集，由于机器人实验平台的车轮对地面光滑程度要求较高，所以选择由室外马路至一楼车库作为实验场地。实验轨迹如图6所示，实验平台实物和结构简图分别如图7(a)、图7(b)所示。

图6 实验轨迹示意图

图7 移动机器人平台

实验采用SIM868模块进行GPS数据的采集，MPU9250进行角度、角速度、线加速度的采集，DWM1000实现UWB信标和基站之间距离的采集。实验使用的GPS模块数据采集频率为1Hz，UWB模块的数据采集频率为3Hz，使用插值的方法实现室内外数据在时间上的对应。通过UWB定位精度测量发现机器人在运行到UWB基站附近时和运行到室内外中间区域时，定位误差较机器人处于基站连线以内的区域时高。

图8中，虚线为实测轨迹，点为实测定位点，实线为滤波后并使用不同切换算法的定位轨迹，左侧区域为室外区域，中间浅色区域为室内外切换的中间区域，右侧深色区域为室内区域。可以看出在UWB基站分布良好且没有遮挡物的情况下UWB的定位精度比GPS定位精度高很多，机器人在UWB基站旁转向，此时的定位误差较大，精度受到影响。图8(a)中标注轨迹为单基于UWB定位精度进行室内外定位切换的轨迹，当机器人从室外向室内运行时，定位系统只在UWB的定位误差小于阈值α1时切换到UWB进行定位。图8(b)中标注轨迹为单基于GPS定位精度进行室内外传感器切换的定位轨迹，当移动机器人从室外向室内运动时，定位传感器只在GPS的GDOP大于7时切换到UWB进行定位。图8(c)中标注轨迹是使用了基于计数阈值和双定位精度的无缝切换算法进行切换的定位轨迹。由实验结果分析得到使用无缝切换算法可以实现室内外定位切换的平滑稳定。

图8 不同切换算法下的定位轨迹图

图9中，区域Ⅰ为室外区域，区域Ⅱ为室内外切换的中间区域，区域Ⅲ为室内区域。

图9 不同切换算法下的定位误差

表1为在使用CKF的基础上使用各种切换算法在中间区域的误差对比。由图9和表1得到，使用无缝切换算法可以有效减小室内外切换中间区域的定位误差。

表1 各切换算法在中间区域的误差

图10中，由于遮挡导致GPS的几何精度因子会发生跳变，使用未采用计数阈值的切换算法会导致定位系统在中间区域时反复切换造成不必要的运算浪费。

图10 采用计数阈值和未采用计数阈值切换对比

6 结语

本文开发设计了一种基于阈值机制和多传感器定位精度的室内外无缝定位切换算法，通过室内和室外两种定位系统实时精度来分配置信度从而提高室内外中间区域定位精度并保证定位轨迹平滑，同时采用计数阈值的方法避免反复切换造成乒乓效应，增加了系统稳定性。搭建实验平台完成对该方法的验证，实现了室内外定位的无缝切换，完成了同一坐标系下室内外的全局定位。室内外无缝切换定位在巡检机器人、消防机器人上都有广阔的应用前景。