基于超宽带和航位推算的室内机器人UKF定位算法

王 芳,李 楠,刘汝佳,吕 翀

(航天科工智能机器人有限责任公司,北京 100074)

基于超宽带和航位推算的室内机器人UKF定位算法

王 芳,李 楠,刘汝佳,吕 翀

(航天科工智能机器人有限责任公司,北京 100074)

超宽带是一种传输速率快、功耗低的新型无线通信技术,可提供亚米级定位精度,近年来超宽带定位在机器人领域的应用日益广泛。在超宽带信号有效区域边缘或信号受到遮挡时,超宽带定位精度急剧下降。为此提出了一种基于超宽带定位和航位推算的UKF组合定位方法,可有效克服上述问题,从而为室内机器人定位提供一种稳定可靠的解决方案。

机器人；定位；UKF；超宽带；航位推算

0 引言

2013年比尔·盖茨预测机器人与自动化技术将成为科技发展的一大趋势,机器人或将成为后移动时代一件改变世界的大事。继机器人在工业现场获得大规模应用后,服务机器人日渐增多,机器人与我们的生活日益密切。服务机器人主要工作在室内环境,在室内的自主定位和导航是服务机器人关键技术之一。

目前应用于机器人的室内定位技术包括：基于视觉传感器或激光雷达的SLAM技术,基于Wi-Fi、蓝牙、ZigBee的定位技术,以及超宽带(Ultra Wide Band,UWB)定位技术等。SLAM技术不依赖外部信息源,具有较强的自主性,对环境改动最小,但在人员密集的动态环境下SLAM技术难以奏效。超宽带定位是一种基于TOF(Time of Flight)原理的定位方式,相比于Wi-Fi、蓝牙、ZigBee等需要检测信号强度的方法,定位更为可靠,近年来在室内定位导航应用方面获得关注[1-3]。对于超宽带信号受到遮挡时,或机器人超出超宽带有效定位区域时,超宽带定位误差会急剧增大,此时可通过与航位推算(Dead Reckoning,DR)、惯性等信息进行组合获得较为精确的定位结果。为此,本文提出了一种基于超宽带和航位推算的UKF定位算法,从而为室内机器人导航提供一种较为精确可靠的定位方式。

1 超宽带定位

超宽带技术是一种新型的无线通信技术,它通过对急剧变化的冲击脉冲进行直接调制,使信号具有GHz量级的带宽。超宽带技术解决了困扰传统无线技术多年的有关传播方面的重大难题,具有对信道衰落不敏感、发射信号功率谱密度低、抗截获能力强等特点,能提供分米级定位精度。

本文采用清研讯科公司研制的LocalSense®自解算超宽带定位系统,该系统工作原理如图1所示。4个定位基站是定位系统的基准锚点,安装在试验区域的固定位置,为系统确定了空间坐标基准。一般情况下,坐标原点选在墙角或者走廊入口,或者具有明显标志性的节点；坐标系的X轴一般与墙体或走廊的延伸方向一致；Y轴则与X轴垂直,由此建立机器人定位导航坐标系。自解算标签安装在移动机器人上,是定位系统的计算核心,该标签实时获取周围基站的位置,并根据反馈结果计算本身的位置,亦即机器人的位置。在信号无遮挡的条件下,该系统定位精度可达10～15cm。

图1 超宽带定位原理Fig.1 UWB localization system

超宽带定位系统是基于电磁脉冲飞行时间的系统,基本算法采用了三点定位曲线相交的原理。在室内环境下,由于空间尺寸较小,无线系统多径效应强,高精度定位部署困难；而由于建筑支柱、隔断墙体、玻璃、家具等遮挡物体的存在,更加恶化了无线定位的工作环境。因此,机器人在室内环境下自主移动不能单纯依靠超宽带定位系统,应与其他定位方式进行融合。

2 航位推算

本文采用的机器人平台如图2所示。该机器人由2个直流伺服电机驱动左右主动轮,通过左右轮差速实现转向运动,前后2个万向轮仅起支撑作用。本文以安装在2个伺服电机轴端的光电编码器输出进行航位推算。

图2 机器人平台Fig.2 Robot platform

记2个编码器精度,即每转脉冲数(PPR)为P,驱动电机的减速比为η,车轮直径为D(单位：m),两车轮轮间距为w(单位：m)。令L=π·D·η/P,为航位推算的比例系数。设k-1至k时刻左右车轮编码器输出的脉冲增量为Nl和Nr,则k时刻根据DR机器人的位置(xdk,ydk)和航向φdk的递推公式如式(1)～式(3)所示。

(1)

(2)

(3)

初始装订时,以机器人当前位置的超宽带定位数据作为航位推算的初始位置,以当前航向下电子罗盘输出的航向作为初始航向。该电子罗盘集成了三轴磁传感器、三轴MEMS陀螺、三轴加速度计,可以对航向、水平姿态进行测量。已事先将电子罗盘航向进行校准,以便将电子罗盘的航向统一到超宽带定位系统坐标系下,即导航所用航向为与X轴正方向夹角,取值范围0°～360°。

由于2个电机输出的转速经减速器减速时存在间隙,而且车轮与地面之间不可避免地存在打滑现象,尤其在转弯时滑动更为明显。因此航位推算的轨迹为理想的输出轨迹,全程连续光滑,在转弯时航向误差有明显增大的趋势。机器人在室内运行时同时采集超宽带数据和航位推算数据,将两组数据在同一坐标系输出,如图3所示。因此,在本文研究中不能以航位推算的结果作为导航的基准,仅作为组合定位的参考,用以预测机器人运行轨迹的趋势。

图3 超宽带定位与航位推算对比Fig.3 The comparison of UWB localization and DR

3 UKF组合定位算法

无迹Kalman滤波(Unscented Kalman Filter,UKF)、扩展Kalman滤波(Extended Kalman Filter,EKF)、粒子滤波(Particle Filter,PF)都可以解决非线性滤波问题。EKF通过将非线性模型线性化后进行滤波,因此不可避免地存在模型误差,导致滤波器次优甚至发散。UKF则在Kalman滤波算法的框架上,通过UT(Unscented Transform)变换,以一组Sigma点表示随机状态的先验分布,经过非线性变换以后获得后验分布。UKF直接基于非线性模型进行解算,近似非线性函数的概率密度分布,而非对非线性模型近似,避免了模型误差。粒子滤波同样通过采样方法,近似随机样本在状态空间传播的概率密度函数。粒子滤波基于蒙特卡罗方法(Monte Carlo methods),是一种顺序重要性采样方法,状态变量无需满足高斯分布,但计算量较大,粒子数过少将导致估计精度下降。UKF基于状态变量满足高斯分布的假设,为确定性采样,样本数与状态维数相关,与粒子滤波器相比采样点数少,同时又能保证逼近精度。

近年来随着UKF技术引入组合导航领域,基于UKF的组合定位方法研究取得了较大进展[4-8],尤其是在车辆或轮式机器人DR导航中。从式(1)～式(3)可见航位推算模型是非线性的,若采用EKF方法,需要对模型进行线性化, 即在当前状态估值处进行Taylor 级数展开,并忽略高阶项,从而导致模型信息丢失, 而且噪声的存在使之进一步恶化。为此本文选用UKF方法进行组合定位。

3.1 状态方程和观测方程

基于式(1)～式(3),UWB/DR组合定位算法的状态方程如下：

cos (φ(k)+Δφ(k))+ωx(k)

(4)

sin (φ(k)+Δφ(k))+ωy(k)

(5)

wφ(k)

(6)

Δφ(k)=Δφ(k-1)+ωΔφ(k)

(7)

滤波状态X=[x,y,φ,Δφ],其中(x,y)为机器人的位置坐标；φ+Δφ为机器人当前航向,包括根据航位推算获得的机器人航向φ,和由于传动间隙和打滑造成的航位推算的航向偏差Δφ。ω=[ωx,ωy,ωφ,ωΔφ]是高斯白噪声,Cov[w(k),w(l)]=Q·δkl。NR、NL分别为左右轮驱动电机的编码器脉冲,可看作状态方程的控制变量。由式(4)～式(7)可以看出状态方程是非线性的,状态方程可记为

X(k)=f(X(k-1),NR(k),NL(k))+w(k)

(8)

选取观测方程如下

Z(k)=H·X(k)+v(k)

(9)

测量值Z=[z1,z2,z3]分别为超宽带测量的XY轴坐标,和电子罗盘测量的航向(已转换到超宽带定位坐标系下)。矩阵H定义如下

(10)

测量噪声v=[v1,v2,v3]为高斯白噪声,Cov[v(k),v(l)]=R·δkl。

3.2UKF方法

将UKF计算过程分为Sigma点计算、一步预测和测量更新三部分,滤波的过程就是这三部分运算的反复迭代[9-10]。

1)初始化

(11)

(12)

根据上述公式,在导航初始位置采集超宽带坐标、电子罗盘航向的均值、方差,作为状态初值和对应的误差方差,航向偏差Δφ的初值和方差可取较小值。

系统噪声矩阵Q可通过航位推算实验统计得到,量测噪声R可根据超宽带定位系统及电子罗盘产品参数及测量统计得到。

由于UKF算法避免了模型线性化造成的高阶截断误差,收敛速度和稳态精度也优于EKF,对初始条件和系统噪声的不确定性具有较好的鲁棒。

2)Sigma点计算

(11)

其中,λ为尺度因子,其计算公式如下

λ=α2(n+β)-n

(12)

利用各采样点计算的权重设定如下：

(13)

3)一步预测

将上述Sigma点代入非线性状态方程(8),得到更新后的Sigma点

(14)

根据更新后的Sigma点计算状态、状态方差和测量的一步预测值,公式如下：

(15)

(16)

(17)

(18)

4)测量更新

(19)

(20)

(21)

(22)

(23)

4 实验验证

将上述UKF组合定位算法在机器人平台上编程实现,并在室内环境下进行试验验证。UKF滤波周期选为200ms,超宽带、里程计、电子罗盘等的采样周期为50ms。

在图3所示的实验中,航位推算由于齿轮间隙、轮子打滑等原因,导致在转弯后航向误差增大较快。采用UKF组合定位算法,实验结果如图4所示,蓝色曲线为UKF结果。可以看出滤波后能够很好修正航位推算的航向误差,从而保证了机器人定位精度。

图4 实验数据1Fig.4 Experimental data Ⅰ

图5所示实验中,在坐标(900,450)附近存在较大金属物体,超宽带信号被遮挡,导致超宽带定位误差急剧增大。通过对比2次采样间超宽带位置增量和里程增量,很容易判别超宽带数据异常。在超宽带数据异常期间以航位推算位置作为滤波输出。由于UKF补偿航位推算的航向误差,因此采用短时间采用航位推算可以保持定位精度。

图5 实验数据2Fig.5 Experimental data Ⅱ

5 结论

本文提出的UKF组合定位方法能够有效融合超宽带定位、航位推算、电子罗盘等多源信息,从而为室内移动机器人提供了一种较为稳定、可靠的方案。通过UKF可根据超宽带定位信息、罗盘航向信息估计出航位推算的航向失准角,从而在超宽带信号异常时,可通过航位推算、维持机器人定位精度。

[1]LiuB,WuX,ReddyV,etal.ARTLS/DRbasedlocalizationsystemarchitectureforindoormobilerobots[C]//IEEEInternationalSymposiumonaWorldofWireless,MobileandMultimediaNetworks, 2010:1-6.

[2] 何贵斌. 超宽带导航技术及应用构想[J]. 中国新通信, 2012(8): 43-48.

[3] 张令文, 杨刚. 超宽带室内定位关键技术[J]. 数据采集与处理, 2013, 28(6): 706-713.

[4] 张红梅,邓正隆.UKF方法在陆地车辆组合导航中的应用[J]. 中国惯性技术学报, 2004, 12(4):20-23.

[5]ZhouZ,GaoY,ChenJ.UnscentedKalmanfilterforSINSalignment[J].JournalofSystemsEngineeringandElectronics, 2007, 18(2): 327-333.

[6] 陆海勇. 捷联惯性导航系统中UKF滤波技术的应用研究[D]. 南京: 南京航空航天大学, 2009.

[7] 王维锋, 徐万里, 聂挥宇. 基于UKF的北斗/INS组合导航[J]. 四川兵工学报, 2013, 34(8): 96-99.

[8] 郭泽, 缪玲娟, 赵洪松. 一种改进的强跟踪UKF算法及其在SINS大方位失准角初始对准中的应用[J]. 航空学报, 2014, 35(1): 203-214.

[9]ZhouZhanxin,GaoYannan,ChenJiabin.UnscentedKalmanfilterforSINSalignment[J].JournalofSystemsEngineeringandElectronics, 2007, 18(2): 327-333.

[10] 申文斌,裴海龙.改进的UnscentedKalman滤波算法[J]. 计算机工程与科学, 2011,33(4):192-197.

Indoor Robot UKF Localization Algorithm Based on UWB and DR

WANG Fang, LI Nan, LIU Ru-jia, LV Chong

(Aerospace Science & Industry Intelligent Robot Company Limited,Beijing 100074,China)

UWB (Ultra Wide Band) is a new type of wireless communication technology with characteristics of rapid transmission, low power consumption and so on. By means of UWB, sub-meter level positioning can be achieved. Recently, positioning based on UWB has been widely used in the field of robots. Given that the positioning accuracy decreases rapidly at the edge of effective area of UWB, or in the situation of the signal block, a new method of UKF(Unscented Kalman Filter) combined positioning based on UWB and dead reckoning is proposed. It affords a stable and reliable approach for indoor robot localization for the sake of overcoming the shortcomings of UWB positioning mentioned above.

Robot; Localization; UKF; UWB; DR

2016-09-07；

2016-10-09

王芳(1977-),女,高级工程师,主要从事移动机器人定位导航和环境感知方面的研究。 E-mail: zhwfang765@sohu.com

10.19306/j.cnki.2095-8110.2017.02.005

TP242

A

2095-8110(2017)02-0026-05