陈新东 蔡 鹏 高大远 王 超 朱海荣
(海军潜艇学院 青岛 266000)
全球定位系统(GPS)能提供三维位置和速度信息,已经广泛应用于人们的日常生活中,但GPS信号容易受卫星信号的衰减以及干扰信息的影响,在室内、地下建筑以及城市街道等环境下信号弱,信息可用性差,GPS不能进行准确的定位[1]。目前有很多学者对室内定位技术进行了研究,如WiFi定位技术、超宽带技术、超声波技术、视觉定位、红外技术、FRID定位、蓝牙技术等定位传感技术[2~7]。然而多传感器的室内定位技术是比较热门的一种定位手段。当前,国内外有不少学者对多传感器定位技术进行了研究。Foxlin等开发了”NavSheo”的定位系统[8];邓林坤等设计了低成本多传感器的室内行人航位推算(PDR)系统,并且在ipad上进行了实验[9];Ruiz和 Madrid 等利用惯性导航系统与RFID定位系统进行结合实现了室内行人的定位[10]。
本文提出了一种基于足绑式的多传感器PDR算法,并采用磁角速率更新(MARU)很好地克服了磁干扰的影响,利用惯性测量单元(IMU)对行人的线运动以及角运动等信息进行采集,同时利用三轴磁力计来计算得到对应的磁感应电势信息,采用EKF算法对各个传感器的误差进行补偿与修正,最终推算得到行人的位置。
静止状态时,利用加速度计与陀螺仪的输出进行初始对准。加速度计测量重力加速度,可据此来计算俯仰角和横滚角。陀螺仪测量地球自转角速度,可计算得到航向角。但是对于低成本的MEMS IMU来说,MEMS IMU陀螺仪的漂移非常大,用陀螺仪的输出来计算航向角,其误差非常大。因此本文采用静止条件下加速度计和磁力计获得初始IMU的姿态角,导航坐标系(n系)为北西天坐标系,载体坐标系(b系)固连在传感器上。一般情况下,由加速度计和磁力计的输出可得:
行人导航系统状态方程与状态向量呈非线性关系,可以进行线性化处理[11-13]。系统误差状态向量可以表示为
式中:δΨ 、Δω 、δr、δṙ、Δa分别代表k时刻的姿态误差信息,陀螺零偏信息,位置偏差信息,速度偏差信息,加计零偏信息,总共15维信息当做系统的误差状态向量。
经过线性化处理得状态方程:
式中:Xk-1代表k-1时刻的误差状态向量后验估计,Xk代表k时刻误差状态向量先验估计,Wk-1代表k-1时刻的过程噪声,它的协方差矩阵是Qk=E(WkWkT),Φk/k-1则代表状态转移矩阵。其表达式如下:
其中:
ab(k)=Acc(k)-Δa为EKF修正后的比力,为k时刻加计的量测值,是k时刻b系经过坐标变换到n系方向余弦矩阵的先验估计。
当足部处于静止或者速度为0时,此时相对于地面,速度接近于0,ZUPT算法把速度误差Δvk,k-1=vk,k-1-[0,0,0]T作为EKF的观测量。
在室内这种环境下,磁的干扰不容易估计,很容易会受到室内建筑等设施的干扰从而引入更多的误差,为了避免外界磁干扰的影响,引入MARU来进行航向修正,即行人足部处于零速或静止时,磁感应强度恒定不变。而MEMS转动时,三轴磁力计分量会发生旋转,且旋转速率与陀螺仪角速率ω(k)变化有关,所以将磁感应强度变化作为量测矢量,对ω(k)进行误差估计补偿来抑制航向角的发散。
当MEMS转动时,磁增量为
式中:Bb(k)和Bb(k-1)分别表示k时刻和k-1时刻的在载体坐标系下磁感应强度。
行人行走时,走的步态是周期性变化的;本实验对行人行走的数据进行采集,每个传感器的三个轴都具有周期性变化的特征。本文利用三个传感器,分别是加速度计、陀螺仪以及磁力计,通过三者结合对行人行走进行检测测定。
其中γα加速度幅值阈值,γω为角速度幅值阈值,Δt为采样时间间隔。
通过上述判定条件,行人步态检测如图1所示。
图1 步态检测
文中仅仅截取了一小段步态检测数据用来进行描述与分析,图中X轴代表传感器采样样本数,Y轴代表数值大小,无固定含义。从图中可以看出,行人行走步态呈现出周期性变化规律,步态静止时,行人静止,步态移动时,行人则处于移动状态。
实验采用的IMU是XSENS公司的MTI产品,实验在室内完成,本次实验将IMU绑在实验人员的足部,依据已经设定好的路线行走,总共做了两次实验,分别沿着不同路线行走,采样频率设置为100HZ,图2和图3分别为Matlab软件下仿真的实验结果对比图。
图2 总路程28.8m测试对比结果
图3 总路程110.7m测试对比结果
为了更加清晰地看出文中算法的效果,对两种方法产生的定位误差进行相应的统计,如表1所示。定位误差采用实验路线终点值减去路线真实终点值,得到的绝对差值占总路程的比值来进行表示。表1记录了两种不同方法的对比,其中初始对准时间为120s。从表中定位误差结果对比可得:在EKF基础上的ZUPT+MARU融合的方法,对位置误差具有改善作用,定位精度可以得到一定的提高。
表1 100HZ两种算法IMU数据的计算结果比较
文中提出了一种多传感器行人航位推算方法,该方法可以减少加计零偏,同时修正行人步态误差;对陀螺仪角速率进行误差估计补偿,抑制了外界磁干扰,增强了对行人航向跟踪性能;通过引入ZUPT+MARU融合构建量测模型,有效地修正行人行走时航向的偏差,实验结果表明:在短时间内,该系统能够满足行人在室内的定位需求,可以实现短距离的导航,位置误差能够控制在0.56%范围内,定位效果良好。