基于航迹推算的低成本北斗与mems紧耦合定位系统及方法
【技术领域】
[0001] 本发明涉及信号处理技术、数据融合技术、无线定位和传感器定位技术领域，具体 涉及一种基于航迹推算的低成本北斗与MEMS紧耦合定位系统及方法。
【背景技术】
[0002] 随着可移动设备得到越来越广泛的应用以及无线网络的普及，用户对信息即时性 和就地性的需求越来越强烈，因此基于位置的服务也备受关注。目前较为流行的无线定位 系统有全球定位系统（GPS)、蜂窝定位系统、蓝牙定位系统、射频识别（RFID)定位系统、 ZigBee定位系统以及WLAN定位系统。其中GPS定位系统目前应用最为广泛，室外定位精度较 高，然而在障碍物较多的遮蔽或室内环境下，如高楼林立的城市街道或室内停车场，卫星信 号会急剧减弱，导致GPS定位系统难以工作;蜂窝无线定位系统的定位精度较低，室外定位 误差通常大于50米，对于定位精度要求较高的室内场景则无法适用；蓝牙、RFID和ZigBee等 定位技术一般较为适用于近距离定位环境。因此，上述定位技术不具有较好的普适性，而需 要寻找一种新的定位技术来代替或弥补各自的不足。同时，无线局域网的不断普及给了 WLAN定位技术很大的发展机会。
[0003] 在WLAN定位系统中，位置指纹定位方法的精度较高且不需要添加额外的设备，从 而得到了较为广泛的应用。基于位置指纹的定位方法主要分为两个阶段:离线阶段和在线 阶段。离线阶段，在目标区域内选择合适的参考点，并在参考点处测量来自每个AP的信号强 度值，建立位置指纹数据库。在线阶段，通过利用定位算法对接收端实时测量得到的信号值 与位置指纹数据库中已保存的信号数据进行匹配，来估计终端位置。
[0004] 由于室内环境的复杂多变，因此位置指纹定位方法的精度性能受多种因素的影 响。通过分析对比影响位置指纹定位方法精度性能的因素，AP摆放位置对于定位精度有较 大影响。在现有方法中，为了优化AP摆放位置，其主要关注的有信号覆盖范围，服务质量，网 络吞吐量，信号传输速率以及覆盖范围内的信号差异性等因素。针对提高信号差异性这一 因素的各项研究中，现有方法都集中在使用某种搜索算法，去搜索一个使得与信号差异性 相关的目标函数达到最大或最小值的组合解，这类方法的主要缺陷是对搜索算法的依赖程 度较高且时间开销较大。
【发明内容】
[0005] 本发明针对现有室外定位技术存在的缺陷和不足，利用北斗卫星信息和MEMS传感 器信息，提出一种基于航迹推算的北斗与MEMS(Micr〇-Electr〇-Mechanical Systems，即微 电机系统)融合定位系统，实现从室外进入室内时，或定位目标从室外开阔环境下至室外建 筑物遮蔽环境下的连续定位。
[0006] 本发明所述的一种基于航迹推算的低成本北斗与MEMS紧耦合定位系统，包括北斗 接收机模块、MEMS传感器模块、北斗与MEMS融合算法模块和ARM-Linux嵌入式系统模块，所 述ARM-Linux嵌入式系统模块分别与北斗接收机模块、MEMS传感器模块、北斗与MEMS融合算 法模块连接；
[0007] 北斗接收机模块用于对卫星信号的捕获与跟踪，并输出伪距、伪距率和卫星星历 信息；
[0008] MEMS传感器模块包括处理器，以及分别与处理器连接的三轴加速度计、三轴陀螺 仪和三轴磁力计，处理器根据三轴加速度计、三轴陀螺仪和三轴磁力计所输出的九轴传感 器信息，估算出行人的航向角和速度信息，所述速度信息即为单位时间内的步长信息；
[0009] 北斗与MEMS融合算法模块将北斗接收机模块所输出的伪距、伪距率，以及MEMS传 感器模块所输出的航向角、步长信息，采用联邦卡尔曼滤波数据融合算法，输出最优定位结 果，即是用户位置信息；
[0010] ARM-Linux嵌入式系统模块用于实现北斗接收机模块与MEMS传感器模块之间的数 据同步、数据处理、定位算法的实施以及定位结果的显示。
[0011]所述ARM-Linux嵌入式系统模块利用嵌入式外部中断来检测北斗接收机模块的秒 脉冲信号是否到来，当检测到有秒脉冲信号到来时，则开启北斗接收机串口数据通道，同时 锁定MEMS传感器串口数据通道，当北斗接收机模块的数据写入完毕，将北斗接收机串口数 据通道切换至MEMS传感器串口数据，至到检测到下一个秒脉冲信号的到来。
[0012] 本发明所述的一种基于航迹推算的低成本北斗与MEMS紧耦合定位方法，采用本发 明所述的基于航迹推算的低成本北斗与MEMS紧耦合定位系统，包括以下步骤：
[0013] 步骤1、从北斗接收机模块中提取出伪距、伪距率、卫星星历信息，利用修正算法对 伪距和伪距率信息进行修正处理；同时利用初始位置和速度解算算法从北斗接收机模块中 得到北斗与MEMS紧耦合系统的初始位置和速度信息，并将该初始位置和速度信息用于PDR 算法的初始化；
[0014] 步骤2、利用MEMS传感器模块所测量的三轴加速度数据、三轴磁力计数据，通过PI 控制以及互补滤波算法对三轴陀螺仪数据进行校正，然后将校正后的九轴传感器数据送入 基于四元数的卡尔曼姿态更新算法进行姿态解算，估算出行人的航向角信息；
[0015] 利用步态以及步频检测算法对三轴加速度计数据进行分析，从而估算出行人的步 长信息，再利用每秒的步长值估算出行人的速度；
[0016] 将估算出的行人的航向角和速度，结合北斗接收机模块所输出的卫星星历信息， 利用roR算法估算出基于行人的伪距和伪距率；
[0017] 步骤3、将所述步骤2计算出的基于行人的伪距、伪距率分别与所述步骤1修正后的 基于北斗接收机模块的伪距、伪距率--对应作差，得到伪距差、伪距率差，并将该伪距差、 伪距率差作为联邦卡尔曼滤波算法的量测输入值，同时将东向、北向位置误差、垂直误差， 东向、北向速度误差、高度速度误差，等效时钟误差相对应的距离，等效时钟误差相对应的 距离率，航向角偏差误差量作为联邦卡尔曼滤波算法的状态变量，利用联邦卡尔曼滤波数 据融合算法对北斗与MEMS紧耦合系统的位置和速度误差进行最优估计；
[0018] 步骤4、将位置和速度误差反馈到PDR解算模块，从而对位置和速度信息进行相应 的校正，得到最优的定位结果，即是用户位置信息。
[0019] 所述状态变量如下：
[0020] Χ=[δΕ δΝ 5h 5Ve δνΝ 5Vh 5tu 5tru]T (1)
[0021] 其中变量分别为：东向北向位置误差δΕ、δΝ，垂直误差δ?ι，东向和北向速度误差δ νΕ、δνΝ，垂直速度误差SVh，等效时钟误差相应的距离Stu，等效时钟误差相应的距离率St ru， 状态方程为：
[0022]
(2)
[0023] 其中ω k为相应的噪声向量，ω Ε、ω n、ω h、_气、％、ω u、ω ru分别为状态量 的噪声值，系统的观测方程为：
[0024] (3)
[0025]
[0026] (4)
[0027] (5)
[0028]其中Pn为MEMS信息与第i颗卫星信息计算得到的伪距，pBl*BD接收机测得第i颗 卫星的伪距，e^(j = l，2,3)为接收机相对于第i颗卫星的三维方向余弦，δ为相应位置速度 误差，V为量测噪声矢量，Vp1、 Vp2、Vp3Vp4分别为量测伪距噪声、％、％、々、％分别为量测 伪距率噪声。
[0029]所述PDR算法主要是根据每个时刻由MEMS模块解算出的航向角信息和速度信息， 递推出每个时刻的行人位置信息，具体步骤如下：
[0030] 利用MEMS传感器模块所检测的三轴加速度计、三轴磁力计和三轴陀螺仪数据，估 算出行人的步长信息cU和航向角信息Θ,，结合从北斗接收机模块直接获取到的行人位置信 息，即为初始位置Ρ〇(Νο，Εο)，则可以递推出第η时刻的行人位置?"(化而），可由下式确定：
[0031] (6)
[0032]
[0033] 其中，和di为第i次位移方向和位移距离，Nk为第k时刻的北向位置值，Ek为第k时 刻的东向位置值;将滤波估计误差结果对PDR解算出来的位置信息进行相应坐标轴上的校 正。
[0034]所述步长的估算方法，定义stride (m)为步长公式，则根据三轴加速度计数据计算 得到的步苌彳古彳彳太加下·
[0035]
(7)
[0036]其中：ax，ay，az分别为三轴加速度的X轴、y轴、z轴输出，a maJPamir^v别为三轴加速 度计输出加速度模值的最大值max和最小值min，即是从抬脚时刻到落脚时刻之间的最大值 和最小值;tup和tdmm对应抬脚up、落脚down时刻。
[0037]利用MEMS传感器模块所测量的三轴加速度数据、三轴磁力计数据，通过PI控制以 及互补滤波算法对三轴陀螺仪数据进行校正，具体校正过程如下：
[0038] (a)互补滤波模型与PI控制模型：
[0039]假设经过校正后得到的姿态角度为Θ，则在s域上有如下等式成立
[0040]
.(8)
[0041] 其中，域的低通滤波器，與4 = ^7为s域的高通滤波器，且有A 、丁k s+k (s)+B(s) = 1，k为角度补偿参数，0rrf为解算出来的姿态角，L表示拉普拉斯变换， t表示测量 的起始时刻，At表示测量间隔，wm为陀螺仪测量值;式(8)的时域表达式为：
[0042]
(9)
[0043]其中，0ref-0为通过磁力计和加速度计计算得到的姿态角度的误差量，用其校正陀 螺仪的角速度输出，校正后的姿态角是需要利用PI调节器控制权重K的大小；
[0044] PI控制器的模型如下：
[0045] u〇(t) = Kie(t)+TiJe(t)dt (10)
[0046] 其中，e(t)为第t时刻系统的误差，Uci(t)为第t时刻PI调节器的输出，通过调节Ti和 I两个参数即能获得较好的误差估计量；
[0047] (