SINS/GPS/PDR室内外无缝导航定位算法

张鹏，王树森，李孟委

(1.中北大学 电子测试技术国家重点实验室，山西 太原 030051；2.中北大学 南通智能光机电研究院，江苏 南通 226000)

0 引 言

随着通信技术与传感器技术的飞速发展，位置服务(location-based services，LBS)的应用范围逐渐扩大，不同环境下的定位服务需求不断增长，人们对室内外无缝定位的需求更加迫切[1-2]。当卫星信号良好时，基于卫星的导航技术可满足大部分情况下的连续导航定位服务需求,然而在卫星信号被遮蔽的情况下，尤其是在城市峡谷和封闭的室内环境中，卫星导航信号将无法提供精确的连续导航服务[3]。

捷联惯导(strapdown inertial navigation system，SINS)具有可靠性高、隐蔽性好、体积小等优势，但是SINS的定位误差会随时间进行积累[4]；航位推算系统(dead reckoning，DR)受环境影响小，具有良好的定位精度，但是也存在误差积累的问题[5]；射频识别(radio frequency identification，RFID)等定位功能的实现必须借助外部基础设施，限制了其应用范围[6]。各种导航定位系统都有自身的缺陷，在这种情况下,依靠单源定位系统无法满足室内外无缝导航定位需求。ZHU R等[7]提出一种用于城市和室内环境的RTK/Pseudolite/LAHDE/IMU-PDR集成行人导航系统；E.A.KHATIB等[8]提出一种基于PDR/GPS/WiFi指纹的无缝室内室外人群定位系统；LI I等[9]提出一种基于低成本视觉/惯性传感器进行多传感器融合的室内/室外环境的移动机器人的导航系统。上述研究表明，开展室内外多源信息融合无缝导航定位算法研究是必然趋势[10]。

本文提出一种基于GPS(global positioning system)、MIMU(micro inertial measurement unit)、表面肌电信号(surface electromyogram，SEMG)传感器、三维电子罗盘的SINS/GPS/PDR无缝导航定位算法，依靠捷联导航(SINS)改善GPS在复杂环境中的抗干扰能力[11]，通过GPS位置和速度信息校正SINS累积误差，同时行人航位推算系统(pedestrian dead reckoning，PDR)通过表面肌电信号(SEMG)传感器能够直接准确地获取行人运动信息，可以利用SEMG信号的长期稳定性补偿SINS误差[12]。

本文采用自适应联邦滤波器将不同导航源的数据进行融合，依据子滤波器滤波质量动态调整信息分配因子[13]，充分利用各导航系统优势，得到最优融合定位结果，从而为行人提供复杂环境下的实时精确定位服务。

1 多源信息融合体系设计

1.1 多源信息融合的无缝定位原理

本文设计的SINS/GPS/PDR无缝导航定位系统，主要包括传感器部分、数据采集和解算部分。其中，数据采集和解算部分主要包括SINS/GPS紧组合解算、SINS/PDR解算及自适应联邦滤波算法。在室外有GPS信号等环境中，主要采用SINS/GPS紧组合，依靠GPS观测及SINS加星历计算所得伪距与伪距率之差作观测量[14]，通过无迹卡尔曼滤波UKF对SINS反馈修正，辅助SINS/PDR模式。在室内GPS信号被完全遮挡等区域，由于GPS信号失锁，采用SINS/PDR模式，依靠行人航迹推算系统(PDR)输出载体的位置信息与MIMU捷联解算输出的位置信息之差作为观测量，通过UKF估计出位置误差，对SINS进行实时误差补偿；通过自适应联邦卡尔曼滤波对不同导航源数据进行融合，实现室内外无缝定位。室内外无缝导航定位系统原理如图1所示。

图1 室内外无缝导航定位系统示意图

1.2 基于SEMG传感器的行人航迹推算算法

针对SINS系统定位误差会随时间进行累积的问题，本文提出一种基于表面肌电信号(SEMG)传感器和三维电子罗盘的行人航位推算(PDR)算法。SEMG传感器能够以非侵入方式测量肌肉在收缩过程中产生的微弱电流，当行人行走时，从腿部测得的肌电信号是周期性变化的，因而可以反映行人的行动意图，可有效避免因参考外界环境物理量变化而造成的误差。从解剖学角度和文献[15]中的一些实验结果看,当行人行走时，小腿腓肠肌的肌电信号比其他肌肉的信号明显，因此选取小腿腓肠肌进行SEMG信号采集，选用非行走区间SEMG平稳而行走区间SEMG强度变化明显的肌电能量信号积分值特征法(IEMGp)进行SEMG特征提取，以此进行活动分类，其中，

(1)

选用峰值探测法进行步进发生检测，迈步计数和步频计算；依据所得步频与SEMG特征通过BP神经网络估计出步长；最后，通过SEMG信号步频和步长结合三维电子罗盘提供的航向角和姿态角，计算出行人的运动轨迹，从而实现行人航位推算。基于SEMG传感器的行人航位推算系统原理如图2所示。

图2 基于SEMG的行人航位推算系统示意图

假设行人初始位置为(E0,N0,U0)，第k步的迈步步长为SLk，航向角为φk，运动坡度角(俯仰角)为αk，则行人东北天向的位移表示为

(2)

步行者的当前位置(En,Nn,Un)可以通过下式求出:

(3)

2 多源信息融合的数学模型

2.1 自适应联邦卡尔曼滤波器的结构设计

本文以GPS、SINS、行人航迹推算PDR系统为导航源,设计包含SINS/GPS、SINS/PDR子滤波器与主滤波器两级过滤结构的SINS/GPS/PDR自适应联邦卡尔曼滤波器[16]。利用导航精度高、工作稳定的SINS作参考系统，将SINS测量值与子滤波器局部最优估计送入主滤波器进行融合，得到定位结果的最优估计值，并将其反馈给SINS系统和各滤波器以调整滤波器，输出滤波结果以校正定位误差，同时依据子滤波器滤波质量动态调节信息分配因子[17]。自适应联邦卡尔曼滤波器结构如图3所示。

图3 自适应联邦卡尔曼滤波器

2.2 参考系统SINS及其状态方程

捷联惯导系统(SINS)作为一个自主系统，能够在各种条件下全天候工作，因此，本文选取自主性强、精度较高、工作较为稳定的SINS系统作为参考系统。SINS参考系统基本状态方程为

(4)

系统状态误差矢量为

XI(t)=[φE,φN,φU,δνE,δνN,δνU,δL,δλ,δh,
εbx,εby,εbz,εrx,εry,εrz,∇x,∇y,∇z]T,

(5)

式中：φE,φN,φU为姿态误差角；δνE,δνN,δνU为东北天向速度误差；δL,δλ,δh为经纬高度误差；εbx,εby,εbz,εrx,εry,εrz为陀螺仪随机误差及一阶马尔科夫过程；∇x,∇y,∇z为加速度计零偏。

系统状态转移矩阵为

(6)

(7)

FN为9阶惯导误差方阵，

(8)

式中，Tgi,Tai(i=x,y,z)分别为陀螺仪和加速度计所对应一阶马尔可夫相关时间常数。

惯导系统白噪声为，

(9)

包括陀螺仪3个轴向随机噪声、白噪声及加速度计随机噪声。其系数矩阵为

(10)

2.3 SINS/GPS子滤波器

SINS/GPS紧组合的GPS卫星误差状态一般选用GPS等效距离误差δtu和等效距离率误差δtru。

GPS的误差状态方程为

式中:

(12)

根据SINS和GPS误差状态方程，SINS/GPS紧组合状态方程表示为

(14)

其中，

XI(t)=[φE,φN,φU,δνE,δνN,δνU,δL,δλ,δh,

εbx,εby,εbz,εrx,εry,εrz,∇x,∇y,∇z,δtu,δtru]T。(15)

选取SINS计算的值及GPS提供的伪距差和伪距率分别作差作紧组合观测量，

(16)

式中，ρI为SINS计算所得载体与第j颗卫星之间伪距。

GPS所得载体与第j颗卫星间伪距

ρG=ρ+δtu+vρ，

(17)

式中，vρ为伪距的量测噪声。

(18)

2.4 SINS/PDR子滤波器

SINS/PDR子滤波器状态方程为

(19)

X2=[φE,φN,φU,δνE,δνN,δνU,δL,δλ,δh,εbx,

εby,εbz,εrx,εry,εrz,∇x,∇y,∇z,δLPDR,δλPDR,

δhPDR]T,

(20)

(21)

式中，δLPDR,δλPDR,δhPDR为行人航位推算的位置误差。

设SINS输出的位置信息为

(22)

航迹推算PDR输出的位置信息为

(23)

选择SINS及PDR位置误差作观测量，则SINS/PDR量测方程为

h2[X2(t),t]+v2(t),

(24)

式中，v2(t)为量测噪声。

2.5 主滤波器设计与信息分配原则

(1)设置各子滤波器Pi,0与Qi,0,以确保它们互不相关，即

(25)

(26)

(2)各滤波器分别进行时间更新，得

子滤波器量测更新，得

主滤波器量测更新，即

(29)

(4)在主滤波器中进行一次最优信息融合，共18维公共误差状态，

(30)

(31)

(5)主滤波器依据信息分配因子进行信息反馈：

(32)

(6)自适应信息分配因子的确定。

在联邦滤波中各子滤波器和主滤波器中所含有的状态信息和噪声信息的总量是不变的，只是每个滤波器中的比例不一致，需要根据子滤波器的滤波效果实时调节。在子滤波器中，协方差矩阵Pi描述了子滤波器的滤波质量。Pi越大，说明该子滤波器精度越低，反之亦然。在联邦滤波器中，信息分配因子βi决定着联邦滤波器的性能。大量实验证明，βi越小，其所对应的子滤波器的输出对整个联邦滤波器的状态估计值影响就越小。

为了提高子系统的估计精度，进而使全局估计保持较高的精度，本文采用一种基于协方差矩阵的迹进行信息分配因子动态分配的算法，βi(k)的计算公式为

式中，trale(·)为求矩阵的迹运算。

3 系统试验及分析

本次试验的目的是在城市高楼隧道、室内外等多种复杂环境下，验证本文所设计的SINS/GPS/PDR室内外无缝定位算法性能和可靠性。研究人员于2019年12月在中北大学进行了室内外无缝试验，在测试过程中，测试人员在腿部捆绑肌电传感器，鞋上捆绑三维电子罗盘，腰间佩戴MEMS惯性测量单元和GPS接收机，肩膀上捆绑GPS天线。其中MIMU数据输出频率100 Hz，MIMU/GPS子系统数据输出频率是1 Hz，MIMU/PDR子系统数据输出频率是10 Hz，自适应联邦卡尔曼滤波器数据输出频率是1 Hz。采用高精度GNSS/SINS系统作为基准设备，并配合MS800A-ED基站使用，采用网络RTK模式，RTK平面精度为(10 mm+1×10-6mm)，高程量程精度为(20 mm+1×10-6mm)，对于开放天空场景，位置精度约为1 cm，即使GNSS卫星的信号不可用，也可以提供包括位置、速度和姿态在内的连续测量，位置精度会逐渐降低，最大位置误差30 cm。

试验数据采集地为中北大学行知广场及地下通道部分，测试人员在测试过程中沿着室外路线行走，从起点出发，沿着中北大学行知广场四周行走，走到地下通达的入口处。室内测试的路线，从地下通道的入口处开始，经过一个斜坡进入地下通道，穿过地下通道，在地下通道出口处结束测试。数据采集时长约7 min，其中室外持续5 min；步行距离320 m：其中地下通道持续2 min，步行140 m；下坡距离40 m；直行(40+ 100)m。真实步行路线如图4所示。

图4 真实步行路线图

步行试验轨迹如图5所示，图5中:红色曲线为由高精度GNSS/SINS系统输出的基准轨迹;蓝色曲线为SINS/GPS/PDR无缝定位系统输出的实时轨迹;绿色曲线为低精度SINS/GPS紧组合输出的实时轨迹。

步行试验轨迹简单分为5段，在A,B,C 3段GPS信号完备，在D段，由室外进入隧道内，此时，GPS信号逐渐减弱，进入隧道内E段后，GPS信号完全消失。由图5分析SINS/GPS紧组合、SINS/GPS/PDR组合与基准轨迹的定位误差如表1所示。

图5 步行试验轨迹图

表1 位置误差比较

从表1可以看出，在GPS信号完备的ABC区域，SINS/GPS紧组合的定位误差在2.5 m以内，而SINS/GPS/PDR组合由于加入了SINS/PDR子系统，修正SINS累积误差，定位误差在1.5 m以内。在D区域，由室外进入隧道，GPS信号减弱，SINS/GPS紧组合定位精度降低，但仍然能够实现导航定位，而SINS/PDR子系统能改善定位结果，提高定位精度。在E区域，GPS信号完全消失，SINS/GPS本质上变成纯SINS导航，导航结果发散，定位误差18.7 m，而SINS/GPS/PDR系统本质上变为SINS/PDR导航，由于PDR系统修正SINS系统累积误差，在短时间内，导航结果一直保持在2 m以内。步行试验东北天向位置误差曲线如图6所示。

图6 位置误差曲线图

本次步行路线包括广场部分和地下通道部分，0～320 s时沿行知广场行走，此时3种定位系统在320～420 s时由行知广场进入地下通道，GPS信号被完全遮蔽，此时，SINS/PDR组合能够抑制纯SINS发散，从误差曲线能够看出，本文所设计的SINS/GPS/PDR无缝定位算法室外位置误差优于1.5 m，室内位置误差优于2 m，系统的定位精度较高，连续性较好。

4 结 语

本文针对城市高楼、隧道、室内外等多种复杂环境下单源导航定位系统的定位精度不高、可靠性低和不连续的问题，设计了基于GPS、MIMU、表面肌电信号(SEMG)传感器、三维电子罗盘的一种SINS/GPS/PDR室内外无缝定位算法，建立了多源信息融合模型，设计了自适应联邦卡尔曼滤波算法。实际行走试验结果表明，该方法能够满足室内外无缝导航定位的需求。