室内行人自主定位算法的研究

张 博，贾浩男，赵 皎

(卫星导航系统与装备技术国家重点实验室，河北 石家庄050081)

室内行人自主定位算法的研究

张 博，贾浩男，赵 皎

(卫星导航系统与装备技术国家重点实验室，河北 石家庄050081)

室内定位与导航在军事单兵系统、消防定位和大型场馆引导领域都拥有非常大的需求。由于GNSS导航在复杂的室内环境下会出现衰减、多径等问题，因此针对行人的基于低成本MEMS惯性传感器的行人航位推算(Personal Dead-reckoning，PDR)导航系统成为该领域的研究热点。以捷联惯性导航原理为基础，设计了室内行人自主定位算法并实现了该算法实时地解算行人位置，该系统模块佩戴于行人脚部，可以完成在室内环境中行人所在位置的实时定位。实验结果表明，该室内行人自主定位算法的精度在行进距离的1%以内，达到了室内定位功能的要求。

室内定位；惯性传感器；零速检测；姿态更新

Abstract The fields of military pedestrian system,firefighting and large venues have a very large demand of indoor positioning and navigation.GNSS navigation will be chanllenged by attenuation,multipath and other issues in a complex indoor environment.The Personal Dead-reckoning navigation system based on low-cost MEMS inertial sensors become a hot topic in the field.In this paper,the algorithm of pedestrian autonomous positioning is designed and used to solve pedestrian location in real time based on the strapdown inertial navigation system.The system module is worn on the pedestrian foot,and can complete the pedestrian location in indoor and outdoor environment.Experiment results show that the accuracy of the indoor pedestrian localization algorithm is within 1% of the travel distance,which meets the requirement of indoor positioning function.

Key words indoor positioning;inertial sensor;zero velocity detection;attitude update

0 引言

随着现代化建设的进程不断深入以及大型场馆建筑日益增多，对于室内的和一些未接收GPS信号的区域，自主的定位与导航技术的需求会越来越大[1]。在军事方面，在一些GPS拒止的环境中，士兵可以通过穿戴能够自主定位的单兵系统，来完成地面作战任务或者协助记录训练任务[2]。对于相关问题的研究，目前已有部分学者取得了一定的成果。Ruiz、Madrid等人提出了将基于惯性导航原理的系统与RFID定位系统结合一起实现行人室内的定位与跟踪[3]，具体来说是将集成了IMU的模块固连在脚部采集运动信息，融合预安置在室内的若干个主动式电子标签发送的RSS信号，采用基于卡尔曼滤波的方法，将INS估计的位置与RSS路径损失模型估计出的位置进行整合。Eric Foxlin等人开发了称为“NavSheo”的定位系统[4]，其中包含有微型惯性、磁力计等传感器，将其佩戴在行人脚部以在具有和不具有GPS支持的任意环境中提供导航，密歇根大学的Lauro Ojeda、Johann Borenstein等人针对使用惯性传感器实现行人自主定位出现的误差漂移提出了地图匹配启发式方法(MAPHDE)，验证了该方法在一定程度上减小了方向的漂移，进而降低了位置误差[5]。

为了实现室内行人的高精度定位，本文主要研究采用惯性传感器、磁力计组合导航原理，优化姿态融合解算算法，进而通过行人航迹推算实现实时化的室内定位。在最初通过惯性传感器进行室内定位时，行人仅仅步行几分钟后便会出现较大的位置误差，针对这一问题，找到了解决问题的思路并提出行人自主定位算法。

1 行人自主定位算法设计

行人自主定位系统的信息采集与处理模块将行人脚部运动信息(包括三轴加速度值、三轴角速度值与三轴磁场强度值)输出，通过标定系数对传感器输出进行处理，并选用低通滤波器对传感器输出值进行滤波，以便得到较为精确的原始数值信息。

然后设计基于梯度下降的姿态解算算法对加速度计、陀螺仪和磁力计的导航作用进行组合[6]，综合各传感器优点获得行人脚部姿态，尤其是获得偏航角信息[7]；以四元数形式来表示姿态变化，可以避免出现奇点现象；以实现3个传感器的最佳融合效果。通过四元数实现加速度数据由载体系向地理系转换，积分后得到行人速度与位置[8]。关键算法为基于梯度下降的姿态解算算法，推导过程主要涉及4个算法。

1.1 角速率计算载体姿态算法

三轴陀螺仪可以分别测量MEMS-IMUx，y，z三个方向角速率ωx、ωy和ωz[9]，单位为rads-1，扩展为四元数Sω记录为：

(1)

(2)

(3)

(4)

1.2 加速度计、磁力计计算载体姿态算法

地理系中重力加速度是垂直向下的，磁感线方向是地理南极指向地理北极方向，在无干扰的环境下，地理系中重力与磁强方向是已知的[10]。传感器系中三轴加速度与磁强是可以测量出的，由此可以计算出传感器系至地理系姿态变化矩阵，且姿态变换矩阵不是唯一的，具有无穷多解，与真姿态角具有周期关系的角度均是解。通过对目标函数最优计算可以得到姿态矩阵：

(5)

(6)

最优化求解技巧有许多，比如共轭梯度法、梯度下降法、牛顿法和拟牛顿法[11]，其中梯度下降法是最简单、精度高的一种办法，式(7)中给出n次迭代的梯度下降计算方法：

(7)

(8)

上述公式中描述了通用的计算形式，然而对于行人自主定位系统中，地理系的加速度与磁强方向往往具有1～2个分量，那么上述形式可以进行简化。

(9)

(10)

(11)

(12)

单独地使用重力的测量或地球磁场的测量不能提供准确的方向[12]，因此，将加速度与磁场强度的测量进行合并处理，如式(13)和式(14)所示；由式(9)和式(11)建立的解的集合具有一个全局最小值，由方程式(13)定义的解具有一个单个点定义的最小值，条件是bx=0。

(13)

(14)

(15)

(16)

(17)

1.3 磁场扰动补偿算法

行人行走环境中磁场扰动会严重影响姿态计算精度，例如包括电器、金属家具和建筑结构内金属结构都可以引入实质的误差[14]。磁场扰动根据干扰源分为硬磁干扰与软磁干扰，其中硬磁干扰可以通过传感器校准校正掉，而软磁干扰只能消减无法消除，除非有其他参考[15]。加速度计提供了另外的参考姿态，因此可以用于消除软磁偏差。

地理系t时刻磁场强度描述为：

(18)

(19)

以这种方式补偿磁性失真来确保磁干扰被限制为仅影响航向分量[16]。该方法还消除了对预定义的地球磁场的参考方向的需要。

1.4 数据融合算法

(20)

(21)

(22)

(23)

(24)

将式(23)、式(24)和式(25)代入式(20)中，可得通过融合算法之后的姿态计算公式，式中由陀螺仪推算出的姿态中使用的融合权重比为γt≈0。

(25)

式中，滤波增益β为陀螺仪零输入误差的平均值，零输入误差包括非线性误差、安装误差、校准误差、噪声误差以及信号混叠误差，通过其微分方程描述[21]为：

(26)

通过上述算法可以由基于梯度的姿态解算算法得到行人的姿态变化四元数，可以将加速度信息转换为地理系下的信息[22]，进行双重积分后即可得出行人的速度和位置信息。

2 行人自主定位系统实验

行人自主定位系统采用室内场地，地面由边长60cm的正方形地板铺成，方便规划路径以及行走时基准路线的确定，且场地内2个区域中间有4个台阶的高度差，可以分析行人自主定位算法对于垂直位移的定位精度。

测试过程为：置采集频率为123Hz，初始对准时间为20s，在每次采集数据之前都应将传感器预热10min，行走距离水平方向50.3m，垂直方向1.4m。重复上述步骤做10组实验。

通过航位推算得行人的X、Y和Z轴方向上的矢量位移估计，从而实现行人的定位，如图1所示，通过终点与起始点的距离获得行人位置定位的误差，水平误差为0.154 m，高度误差为0.6 cm，由此可知，行人在室内行走的定位精度比较高。

图1 行人位置坐标的估计

根据对行人速度、姿态的估计，获得对行人航迹的估计，分别为水平位置的轨迹和高度变化的轨迹，如图2和图3所示。

图2 行人的水平位置轨迹

图3 行人的高度变化轨迹

为了更具体地验证算法定位效果，表1给出了以均方根误差衡量的统计结果，由表中数据可以分析出行走总长度50.3 m，高度变化0.7 m的定位中水平方向误差平均值为0.201 m，为行走总长度的0.39%，垂直方向上误差平均值为0.010 2 m，为上升总高度的0.73%。

表1 实验结果统计

行人自主定位系统中信息采集与处理模块将实时解算出的信息通过串口蓝牙模块发送至显示终端，显示终端接收显示行人行走轨迹与所在室内的具体位置。

该行人自主定位系统在行人线性位移的测量中较为精确，在行进速度为1～2.5 m/s的情况下，误差小于行进距离的1%。并且行人自主定位系统对步行中的暂停、快走和上下楼梯等行为均能够保持较好的精度。

3 结束语

为室内行人提供了一个简单的个人航位推算定位系统，该系统不需要任何外部参考，例如WiFi或RFID等基站，能够实现室内较长时间自主、精确的定位功能。通过对行人自主定位系统进行设计，并针对室内行人实验中存在的问题，提出了基于梯度下降的姿态融合算法，经过多次实验验证满足预期目标并且算法比较稳定，具有很好的应用前景。

[1] 尹杭.使用MEMS惯性测量单元和拓扑地图的室内定位与导航技术研究[D].成都:电子科技大学,2015.

[2] 朱娟,周尚伟,马启平.基于RFID的室内定位算法[J].微计算机信息,2009,25(8):160-162.

[3] 邓仲哲.基于惯性传感器和地图匹配的行人室内定位算法[D].哈尔滨:哈尔滨工业大学,2015.

[4] FOXLIN E.Inertial Head-tracker Sensor Fusion by a Complementary Separate-bias Kalman Filter[C]∥Virtual Reality Annual International Symposium the IEEE,1996:185-194.

[5] Ronan Alves da Paixao,Jacy Montenegro Magalhaes Neto,Paulo Fernando Ferreira Rosa.Prototyping a Basic MEMS Attitude Heading and Reference System[C]∥2011 IEEE International Conference on Industrial Technology,2011:13-38.

[6] GLUECKM,BUHMANN A,MANOLI Y.Autocalibration of MEMS Accelerometers[C]∥Graz,Austria.IEEE Inertional Instrumentation and Measurement Technology Conference,2012:45-48.

[7] 张浩,任芊.四旋翼飞行器航姿测量系统的数据融合方法[J].兵工自动化,2004,32(1):28-31.

[8] 陈静燕.深海环境惯性导航系统误差补偿方法的研究[D].杭州:浙江大学,2009.

[9] 黄艳辉.MTi中微机械陀螺误差测试、建模及补偿研究[D].哈尔滨:哈尔滨工程大学,2009.

[10] 白晶,马燕飞.无源交叉定位系统中的最优交会角[J].无线电通信技术,2015,41(1):31-34.

[11] 梁健.目标定位精度评估方法研究[J].无线电工程,2015,45(4):65-69.

[12] 洪雁,王培康.基于稀疏表示多分类的室内定位算法[J].无线电工程,2014,44(2):46-49.

[13] 江城.基于北斗终端的室内外无缝定位系统设计[J].无线电工程.2016,46(8):38-42.

[14] 刘烜,胡倩影.航空光电平台机动目标跟踪定位技术应用[J].无线电通信技术,2014,40(3):82-85.

[15] 刘欣然.老人跌倒监测定位装置的研究[J].无线电通信技术,2016,42(1):82-85.

[16] 袁晓峰,陈颀.一种基于RFID的定位方法[J].无线电通信技术,2016,42(6)：81-85.

[17] 段同乐,张冬宁,刘文展.基于模糊逻辑的WSN节点自定位算法[J].无线电通信技术,2016,42(2):48-50.

[18] 任绍俊,于惠海.基于GPS的水下动态目标定位方法研究[J].无线电工程,2015,45(7):87-90.

[19] 后茜,刘韦韦,丁进.LTE定位中非视距误差的鉴别方法[J].无线电工程,2016,46(1):8-11.

[20] 江城.基于北斗终端的室内外无缝定位系统设计[J].无线电工程,2016,46(8):38-42.

[21] 赵凯.神经网络和RFID相融合的室内定位算法[J].激光杂志,2015,36(8):138-141.

[22] 王远哲,毛陆虹,刘辉.基于参考标签的射频识别定位算法研究与应用[J].通信学报,2010,31(2):86-92.

Research on Indoor Pedestrian Autonomous Location Algorithm

ZHANG Bo，JIA Hao-nan，ZHAO Jiao

(StateKeyLaboratoryofSatelliteNavigationSystemandEquipmentTechnology,ShijiazhuangHebei050081,China)

10.3969/j.issn.1003-3106.2017.08.08

张博，贾浩男，赵皎.室内行人自主定位算法的研究[J].无线电工程,2017,47(8):31-35.[ZHANG Bo，JIA Haonan，ZHAO Jiao.Research on Indoor Pedestrian Autonomous Location Algorithm[J].Radio Engineering,2017,47(8):31-35.]

2017-03-24

国家自然科学基金重大研究计划基金资助项目(91638203)；国家重点研发计划基金资助项目(2016YFB0502402，2016YFB0502101)。

TN961

A

1003-3106(2017)08-0031-05

张 博 男，(1986—)，硕士,工程师。主要研究方向：地面运控系统和定位算法。

贾浩男 男，(1990—)，硕士,助理工程师。主要研究方向：信号处理和信息处理。