惯导系统辅助CSS的室内定位方法*

黄金鹏,尚俊娜*,岳克强,张 波,李蕊江

(1.杭州电子科技大学通信工程学院,杭州 310018;2.杭州电子科技大学电子信息学院,杭州 310018)

室内导航定位技术的研究已成为导航中的热门领域,特别是在众多公共室内场所的高精度导航要求日益强烈,如大型车库、商场、飞机场以及地铁等等[1]。多种室内定位技术包括惯性导航系统、WiFi、Zigbee以及UWB等,在定位精度、系统成本、鲁棒性及适用的环境范围各有不同。当前单一的定位方式很难同时满足高精度、高实时性、高可靠性、低成本、低复杂度的要求。因此选择多个室内定位方法相融合,即选择多种传感器网络组合导航的方式,成为当前室内导航定位的主要研究方向。

室内组合导航定位有效的提高了定位系统的整体性能,如文献[2]有效地融合了4种定位技术,包括GPS、WiFi、计步器以及RFID,定位结果融合了各种定位技术的优点,精度与稳定性都得到很大的提高。目前,WiFi作为公共通信接入热点,已经广泛部署于众多公共室内场所中,因此选择与WiFi组合导航是比较普遍的,文献[3]中提出WiFi与行人航迹推算PDR(Pedestrian Dead Reckoning)进行融合定位,定位精度平均误差可达1.24 m。文献[4]提出一种基于WiFi和手持设备上的微机电系统传感器的室内行人导航定位的方法,最大定位误差为2.5 m。但是随着室内组合导航技术的发展,基于WiFi的定位精度愈加满足不了室内定位高精度的要求。从而更多的研究选择与具有独特定位优势的超宽带UWB(Ultra Wide Band)系统进行组合导航,文献[5]提出一种基于9轴惯性导航与UWB的联合定位方法,定位误差控制在0.2 m以内,实验环境仅为8 m×5 m的场所,需铺设4个UWB锚节点才能覆盖定位。文献[6]提出一种融合PDR、RFID与UWB的室内定位技术,定位精度达亚米级,实验环境为15 m×25 m的场所,但需铺设6个UWB锚节点满足定位需求。虽然基于UWB相组合的定位方法定位精度基本可达到亚米级别,满足高精度定位的需求,但是UWB基站成本相对较高,很多大面积的室内导航定位环境的预算无法支持UWB的全面覆盖。因此本文选择线性调频扩频CSS(Chirp Spread Spectrum)技术作为组合导航中主要的定位技术,虽然CSS系统的定位精度会低于UWB系统,但是CSS技术对于其他无线通信系统干扰较小,且通信距离更远,故而性价较UWB系统会有很大的提高[7]。当前国内基于CSS技术室内定位的研究多为其单一系统的室内定位方法,文献[8]中在CSS定位系统下提出的非视距抑制算法,定位精度在视距情况下可达1 m;文献[9]中设计并实现了基于CSS信号的化工生产区定位与跟踪系统,理想实验环境下定位误差为亚米级。国外学者率先提出了将CSS技术进行室内组合导航定位的方法,文献[10]利用超声波装置辅助CSS系统进行室内组合定位,精度可达分米级别。

本文提出一种惯导系统辅助CSS的室内定位方法,惯性导航系统INS(Inertial Navigation System)利用由陀螺仪和加速度计构成的惯性测量单元IMU(Inertial Measurement Unit)来实时监测运动载体的运动状态,是一种独立自主的导航系统[11]。本文中设计并实现了基于MPU9250多轴传感器的IMU,并与CSS定位系统进行组合定位,利用扩展卡尔曼滤波(EKF)算法融合惯导系统和CSS系统的定位结果,实验环境为52 m×20 m的地下车库里,CSS锚节点铺设数目为4个,实验结果表明定位结果的平均误差为0.158 1 m,达到亚米级别,满足复杂的室内环境下高精度定位的要求。

1 惯导系统定位原理及IMU的设计

1.1 惯导系统定位原理

惯导系统定位采用基本的航位推算原理[12],即在得知当前时刻位置的条件下,通过测量移动过程中的航向角变化值及运动载体的位移量,来推算下一时刻的位置。图1中,假设运动载体在A点的位置坐标为(x0,y0),运动速度为v,偏航角为θ0,则可推算经过时间T后到达B点的坐标(x1,y1):

(1)

图1 航位推算原理图

假设t0时刻运动载体初始位置(x0,y0),则tn时刻的位置(xn,yn)及偏航角θn可由式(2)计算得到:

(2)

在实际测试中,航位角θi可由IMU提供,而位移量di则可由载体的行驶距离或者行人的步距来提供。在本文实验环境中位移量采用智能测速系统中常用的脉冲计数法[13]获得;只要转轴每旋转一周,产生一个或固定多个脉冲,并将脉冲送入微处理器中进行计数,即可获得转速的信息,从而可得到小推车在运动时间中的位移量,即小推车在实验中曲线运行的实时距离值。如图2所示,将霍尔传感器A44E安装于实验小推车的转轴上,当车轮的扇叶经过时,即可产生脉冲信号,实现脉冲计数。

本文的惯性导航系统包括了IMU单元及霍尔测速单元,通过IMU输出航向角,再借助霍尔传感器提供的系统当前的测速值,从而可完成惯导系统的定位导航。但是想获得精度较高的定位结果,必然需要精确稳定的航向角,为此本文设计了IMU,以提高惯导系统的定位性能。

图2 霍尔传感器安装示意图

图3 IMU设计框图

1.2 IMU设计

本文设计的IMU包括硬件平台设计及软件环境的搭建,相应设计框图如图3(a)所示。其中硬件设计包括两个模块,分别是数字信号处理DSP(Digital Signal Processor)核心板模块和传感器模块。姿态解算算法中存在大量非线性运算及浮点运算,因此需要运算能力较强的处理器。这里选择了恩智浦ARM Cortex-M3的核心处理器LPC1788。传感器模块采用的是InvenSence公司生产的型号为MPU9250的9轴传感器芯片,集成了3轴陀螺仪、3轴加速度计、3轴电子罗盘,特点是体积小、功耗低且分辨率高[14]。陀螺仪、加速度计、磁罗盘均为数字的I2C接口,与DSP的I2C总线相连。IMU设计的软件部分设计流程如图3(a)所示。

本文所设计的IMU实物图如图3(b)所示,由于MPU9250集成了3轴磁罗盘,有效的规避了IMU航向角存在累计误差的问题[15]。如图4所示,当IMU保持静止状态1 h来观测其姿态值,可以看出各个姿态角精度较高并且角度偏移量很小,从而为惯导系统的导航定位奠定了高精度航向角的基础。

图4 IMU静止时输出的姿态角

图5 CSS定位流程

2 惯导系统辅助CSS定位方法

2.1 CSS定位及选星方法

CSS的测距方式是基于飞行时间的,其定位算法可基于常用的无线定位算法:TOA(Timeof Arrival)定位算法中基于数值处理的最小二乘定位算[16]。CSS定位实现流程如图5所示。

在复杂的室内环境下,移动节点与锚节点之间不可避免的会出现非视距的情况;为了检测非视距对测距值精度的影响,本文将移动节点放置于与锚节点相距5 m的位置,在视距的路径上分别放置不同的物品,具体的实验结果由图6可知:行人、座椅等物品及3 cm厚的铝合金门对于CSS节点之间视距传输基本没有影响,但是0.3 m厚的墙壁和6 cm厚的铁制门对于测距值的影响严重,基于上述非视距情况下的定位结果必然会存在很大的偏差,因此本文提出了一种基于三角形三边准则的选星算法以克服非视距情况下的定位偏差问题。

图6 不同非视距情况下的测距结果

图7 选星示意图及其算法流程

如图7所示,锚节点的数目与彼此间的距离已知;三角形三边准则的选星算法具体步骤如下所示:

①CSS定位开始后,通过移动节点获取距离各锚节点的测距信息。最小的测距值对应的锚节点为中心锚节点。图7中,测距值l1最小,相应的锚节点1位中心锚节点。因为在室内环境下,面积较小,所以该测距值基本认为在视距环境下得到的。

②其次找到距离中心锚节点最近的邻锚节点,中心锚节点、邻锚节点与移动节点构成三角形,通过三角形三边准则来判断邻锚节点与移动节点是否处于视距情况下。图7中,中心锚节点1最近的邻锚节点2,判断;l2-l1

③第2步的判断若满足三角形三边准则,则进行第4步操作;否则从所有的锚节点中剔除当前邻锚节点,使其不参与TOA定位算法。

④判断当前邻锚节点是否为最后一个锚节点,成立则完成CSS选星,算法结束;否则,将当前邻锚节点作为新的中心锚节点,之后回到上述的第2步操作。

通过该选星方法可以选出与移动节点处于视距或类视距情况下的锚节点,之后再根据TOA算法中的最小二乘定位算法求解出移动节点的坐标。

2.2 惯导系统辅助CSS定位原理

本文所设计的IMU包含磁罗盘,解决了航向角累积误差问题;通过霍尔传感器A44E来读取惯导系统当前状态的速度值,与应用零速修正的可穿戴性惯性测量器件比较[17],不必担心行人行走的姿态、速度及步伐大小对IMU测量精度的影响。但是惯导系统的航位推算仅仅实现的是相对定位结果,即相对于前一个运动状态的定位结果,所以必须设置定位的初始点;并且在室内的复杂环境下,往往其磁场的环境比较复杂,导致航向角会出现的突变情况,如果没有其他定位系统进行校准,会对定位结果造成较大的误差。基于TOA定位算法的CSS定位系统采用精度较高的飞行时间测距,结合本文提出的基于三角形三边准则的选星方法,可为行人提供较准确的绝对定位坐标。但是室内复杂的环境下,多径及非视距问题仍很难避免,特别在楼道拐点、支柱周围及房门的阻隔,这些影响将会使得定位结果出现较大偏差。

图8 惯导系统辅助CSS定位系统原理图

本文提出的惯导系统辅助CSS的室内定位方法系统原理如图8所示,CSS定位结果来设定滤波启动值和PDR定位的初始值,IMU提供航向角,霍尔测速单元作为当前运动状态的判断。当导航定位开始时,PDR处理模块与CSS处理模块同时工作,启动测量更新和状态更新的迭代计算。两定位系统可以互相补偿,当惯导系统由于磁场的影响出现突变情况下,可以被CSS定位系统的定位结果修正;同时在室内的角落里或者拐角处,可以通过惯导系统的定位结果来修正由于非视距问题导致的CSS较大的定位偏差。

2.3 扩展卡尔曼滤波

惯导系统辅助CSS定位方法中最后需对两种定位方式的定位结果进行融合,以输出最终的定位结果。本文的实验环境均为非线性环境,从而选择扩展卡尔曼滤波器EKF(Extended Kalman filte)[18],考虑一般的非线性系统:

(3)

式中:Fk和Hk为雅可比矩阵:

(4)

式中:Xk为状态量,Yk为量测量,Wk和Vk分别为系统的过程噪声和测量噪声。

这里定义系统EKF的状态量如式(5)所示:

X=[δgyroδacceδqδvδrbR]

(5)

式中:δgyro和δacce分别为陀螺仪和加速度计的偏差随机分量,δq为求解姿态四元数的误差,δv为通过霍尔传感器解算的速度误差,δr为CSS定位节点之间的测距误差;除了上述包含的误差之外,为进一步提高组合之后的定位精度,这里将CSS移动节点接收到锚节点的时钟偏差bR(等效测距值)也作为EKF的扩展状态量,

下面为主要误差状态量的误差模型建立过程:

①INS误差方程

δgyro和δacce代表陀螺仪和加速度计的偏差,通常被认作是一个随机变量与一个随机误差之和,建模成一阶高斯马尔科夫过程如式(6)所示:

(6)

δq为姿态四元数的误差,姿态四元数的误差方程由式(7)得:

(7)

式中:Ω为反对称矩阵,其形式如下:

(8)

式中:ωx、ωy和ωz是陀螺仪角速度的三轴分量值。

δv为霍尔传感器速度误差模型,设车轮旋转一周速度霍尔器件输出的脉冲个数为n,0.2 s内计数器测得的脉冲数为f,则车轮旋转圈数N=f/n;设车轮的半径为R,则在0.2 s内小推车行驶的距离为:L=2πRN,所以1 s内小车行驶的距离为:L1=5L,最后可得速度计算公式:V=10πRN。从而速度的误差方程可由式(9)得到:

(9)

②CSS系统误差方程

(10)

CSS节点接收机的时钟偏差:

(11)

式中:wg为高斯白噪声。

最后整理可得惯导系统辅助CSS定位滤波器的误差状态方程:

(12)

EKF的量测量为:

Z=[δrδθδδvδxδy]

(13)

式中:δr、δθ、δφ为当前时刻IMU输出的姿态角与前一时刻的姿态角差值,δv为当前时刻霍尔器件输出速度值与前一时刻的速度差值,δx、δy为当前时刻惯导系统解算的定位位置坐标与CSS系统定位的位置坐标的差值。最后经过测量更新和状态更新的迭代计算,输出导航定位结果。

3 实验验证及结果分析

本文提出的惯导系统辅助CSS定位的方法进行可行性验证,实验地点选在杭州电子科技大学图书馆楼下的地下车库,具体实验环境如图9(a)所示,地下车库的面积为52×20 m2。实验系统平台如图9(b)所示,其中包括设计的惯性测量单元(IMU)、测速的霍尔传感器器件以及CSS测距节点。

图9 实验环境及测试系统平台

图10 直线行走实验结果

3.1 单纯惯导系统实验结果

①直线行走

该实验在车库中沿行车道直线行走,行走的距离为40 m,直线行走的真实轨与推车行走的PDR轨迹如图10(a)所示,行走过程中IMU的航向角变化情况如图10(b);图10(b)中可看到,由于室内磁场复杂多变,IMU输出的航向角会出现突变,导致直线行走定位结果出现偏差。直线行走的误差分布情况如图10(c)所示,行走过程中最大误差值为0.635 4 m,平均误差值为0.294 1 m。可见,基于本文设计的IMU,直线行走时能基本满足室内定位的需要。

②Z字形行走

如图11(a)所示为推车按Z字形行走时的真实轨迹与其PDR轨迹,行走过程中IMU输出的航向角变化情况如图11(b)所示。Z字行走的误差分布情况如图11(c)所示,行走过程中最大误差值为0.743 1 m,平均误差值为0.302 3 m。

图11 Z字形行走实验结果

3.2 惯导系统辅助CSS定位的实验结果

①3种定位方式的比较

本实验环境为52 m×20 m的长方形视距区域,推车的真实轨迹如图12(a)中所示,行走路线:①→②→③→④→①。地图中“o”为CSS锚节点悬挂位置。图12(a)中显示了3种定位方法的定位轨迹,单一的定位方式得到的轨迹路线会有所偏差,而惯导辅助CSS定位的轨迹与真实的轨迹更加吻合。

由图12(b)表示3种定位方式的误差比较,单一定位方式的定位误差较大,惯导的PDR平均误差为0.345 6 m,CSS系统定位的平均误差为0.365 9 m,最后惯导辅助CSS定位的平均误差为0.158 1 m。

图12 惯导、CSS定位与组合定位实验结果比较

定位方式平均值/m标准差/m平均耗时/msPDR0.34560.215220.000CSS0.36590.2305943.375PDR+CSS0.15810.1103505.432

由表1可看出惯导辅助CSS定位方法可以提高50%的定位精度,惯导系统输出频率为50 Hz,而CSS定位系统由于环境及距离影响会出现丢包现象,因此这里取20个定位点耗时的平均值;组合方法的平均耗时减少了一半的时间,同时定位精度达到0.1 m,使用惯导辅助CSS定位方法更能满足室内定位精度和实时性要求。

图13 惯导辅助CSS车库定位轨迹

②实际应用定位结果

基于本文的惯导辅助CSS定位方法进行了6次推车进出车位的操作,如图13所示,地图中定位的车位与推车实际进入的车位是一致的。由于在CSS定位中采用了基于三角形三边准则的选星方法,使定位结果尽可能规避了非视距的影响,从而在地图中右下角落里的车位也得以精准定位。经验证,即便在较复杂的室内环境下,本文设计的定位方法实际定位的平均误差均小于1 m。

4 结语

本文选择了惯导与CSS定位相组合的方式进行室内定位,实验结果证明了两者的组合定位继承了各自的优点,获得了比单一定位方式精度更高的定位结果。本文的贡献包括:①设计了基于MPU9250传感器的惯性测量单元,得到精度较高的航向角,规避了累计误差的问题,与霍尔传感器A44E组成惯导系统,为PDR轨迹的稳定性与准确性提供了基础保障;②改进了CSS定位,提出基于三角形三边准则的选星方法,有效地降低了室内复杂环境下非视距问题的影响;③提出通过扩展卡尔曼滤波来融合PDR和CSS的定位结果,得到更稳定和精确的定位结果。通过实验结果证明惯导辅助CSS定位结果较两种单一的定位方式性能提高了50%以上。

[1] Peng T,Liu Q,Wang G. Enhanced Location Privacy Preserving Scheme in Location-Based Services[J]. IEEE Systems Journal,2017,11(1):219-230.

[2] Daotk,Nguyenhl,Phamtt,et a1.User Localization in Complex Environments by Multimodal Combination of GPS,WiFi,RFID and PedometerTechnologies[J]. The Scientific World Journal. 2014:Article ID 814538.

[3] 陈国良,张言哲,汪云甲,等. WiFi-PDR室内组合定位的无迹卡尔曼滤波算法[J]. 测绘学报,2015,44(12):1314-1321.

[4] Zhuang Y,El-Sheimy N. Tightly-Coupled Integration of WiFi and MEMS Sensors on Handheld Devices for Indoor Pedestrian Navigation[J]. IEEE Sensors Journal,2016,16(1):224-234.

[5] 孙璧文,樊启高,武亚恒,等. 基于PDR/UWB紧耦合的足绑式行人导航技术[J]. 传感器与微系统,2017,36(3):43-47,50.

[6] Zampella F,Jiménez A R R,Seco F. Robust Indoor Positioning Fusing PDR and RF Technologies:The RFID and UWB case,International Conference on Indoor Positioning and Indoor Navigation,Montbeliard-Belfort,2013:1-10.

[7] 张骁耀,王玫. 基于CSS技术的室内定位通信系统的研究与实现[J]. 微型机与应用,2012,31(13):49-52.

[8] 殷学强. CSS无线定位系统设计及非视距抑制算法[J]. 现代电子技术,2016,39(7):5-9.

[9] 段翠翠,王瑞荣,王建中,等. 基于无线传感器网络的高危生产区人员定位系统[J]. 传感技术学报,2012,25(11):1599-1602.

[10] Nam Y J,Park Y G. Efficient Indoor Localization and Navigation with a Combination of Ultrasonic and CSS-Based IEEE 802.15.4a[C]//Proceedings of the 4th International Conference on Ubiquitous Information Technologies and Applications,Fukuoka,2009:1-6.

[11] Liu Chunyang,Chen Fan,Sui Xin,et al. Gesture Detection and Data Fusion Based on MPU9250 Sensor,2015 12th IEEE International Conference on Electronic Measurement and Instruments(ICEMI),Qingdao,2015:1612-1615.

[12] 王亚娜,蔡成林,李思民,等. 基于行人航迹推算的室内定位算法研究[J/OL]. 电子技术应用,2017,43(4):86-89,93.

[13] 杨继生,刘芬. 霍尔传感器A44E在车轮测速中的应用研究[J]. 电子测量技术,2009,32(10):100-102.

[14] 刘明亮,崔宇佳,张一迪,等. 基于多传感器数据融合的姿态控制与应用[J]. 微电子学与计算机,2017,34(7):65-69.

[15] 张承岫,李铁鹰,王耀力,等. 基于MPU6050和互补滤波的四旋翼飞控系统设计[J]. 传感技术学报,2016,29(7):1011-1015.

[16] 王沁,何杰,张前雄,等. 测距误差分级的室内TOA定位算法[J]. 仪器仪表学报,2011,32(12):2851-2856.

[17] 李超,苏中,朱嘉林,等. 可穿戴式自主定位技术的零速触发算法研究[J]. 传感技术学报,2014,(5):627-632.

[18] 金仁成,赵伟,石小培,等. 基于EKF算法的无线传感器网络定位技术研究[J]. 传感技术学报,2010,23(12):1810-1814.