基于惯性元件的实时三维室内定位系统设计*

白梦帅， 曾祥烨， 王静宜， 苏彦莽

(河北工业大学 电子信息工程学院 天津市电子材料与器件重点实验室，天津 300401)

0 引 言

随着经济社会的发展，城市中大型建筑物的数量不断增加，随之而来的是以火灾为主的突发性灾害事件的发生概率逐渐提高[1]。火灾发生时，实时的室内定位可以提供必要的位置信息，进而提高救援效率，在最大限度上保证人民的生命财产安全，因此室内定位的研究具有重要的理论意义和应用价值。本文设计了一种以惯性元件和气压计进行数据采集，然后利用树莓派进行处理，最后通过无线通信将解算后的数据发送到上位机，在上位机上利用基于粒子滤波的地图匹配来进行实时位置显示的三维室内定位系统。

1 惯性定位技术的理论基础

惯性定位技术以牛顿运动定律为理论基础，首先通过姿态传感器采集被定位目标的惯性信息，然后将载体坐标系与地理坐标系进行转换[2]，转换后对惯性数据进行位移和姿态解算，最后得到被定位目标的位置信息。

1.1 坐标系之间的转换

在惯性导航技术中，要想由原始数据得到位置信息，首先需要进行坐标系的转换。实时更新运动载体的姿态矩阵是进行坐标系转换的前提，常用的姿态矩阵更新方法有欧拉角法、方向余弦算法和四元数法[3]，与前两种方法相比，四元数算法具有计算量较小，并且可以避免死锁问题的优点[4]。所以本文采用四元数法来完成坐标系的转换。由四元数表示的坐标转换矩阵如下

(1)

q=q0+q1i+q2j+q3k

(2)

式中q0为四元数的实部，q1，q2，q3为三个相互正交的虚部单位向量。

1.2 位置解算

在位置解算方面，惯性导航技术常用的方法有二次积分法和行人航位推算(pedestrian dead reckoning，PDR)算法，本文采用PDR算法来解算位置，与二次积分法相比，PDR最大的优势是可以避免累计误差[5,6]，其基本原理如图1所示。

图1 PDR基本原理

如图1所示,建立直角坐标系OXY，其中X轴与地理坐标系的东向重合，Y轴与北向重合，假设(X0,Y0)为被定位目标的初始位置，目标从初始位置开始运动，di为每次运动的单步步长，m，ai为当前步长方向与Y轴的夹角(°)，则运动目标从第(i-1)步运动到第i步的步长在X轴方向上可由下式表示

ΔXi-(i-1)=disinαi

(3)

同理,在Y轴方向上的步长可由下式表示

ΔYi-(i-1)=dicosαi

(4)

由上式可得，被定位目标迈出第n步后的坐标(Xn,Yn)可作如下表示

(5)

(6)

2 系统框架与硬件方案设计

本系统能够完成数据的采集、处理和发送以及最终位置的解算和显示，系统框架主要分为下位机、上位机以及上位机和下位机之间的无线通信三大部分。其中下位机以树莓派3B+作为核心主控设备，可以满足本系统大量数据运算的需求。惯性模块采用JY901，气压采集模块采用GY68均通过I2C与树莓派连接，无线通信模块采用AS07-M1101S，利用自定义协议完成无线数据通信。

3 系统软件方案设计

本系统软件设计主要分为上位机和下位机以及二者之间的无线通信。下位机主要的功能为采集原始数据，然后进行处理运算，最后将处理过的位移数据发送给上位机进行基于粒子滤波的位置显示。软件总体工作流程如图2所示。

图2 系统软件设计流程

为了保证数据传输的稳定性和有效性，本系统将无线通信的波特率设置为38 400 Baud/s，每一帧数据共包含32个字节，分别是：数据类型(占1个字节)，数据序号(占1个字节)，终端地址(占1个字节)，步数(占4个字节)，位移(占24个字节)，校验位(占1个字节)。

4 定位数据的处理

由JY901和GY68采集到的数据需要进一步处理才可用于具体的定位，本文的数据处理由以树莓派为核心的下位机系统完成。

4.1 原始数据的预处理

本文所用JY901属低成本惯性元件，对抖动高度敏感[7]，经实测角速度存在静态漂移，为减小误差，采用中值均值滤波来进行预处理，处理结果如图3所示。

图3 经中值均值滤波处理后的角速度效果

由气压计GY68采集到的气压数据也存在跳变，同样采用中值均值滤波来降低数据跳变的频率，降低对高度定位效果的影响，效果如图4所示。

图4 经中值均值滤波处理后的气压数据效果

4.2 运动过程中的滤波处理

单步检测的准确率直接影响到最终定位效果，本文利用三轴合加速度来检测单步运动，经实测运动中的合加速度数据存在大量噪声，对步态检测的精度有较大影响，为了降低误差，提高精度，利用低通滤波对三轴合加速度进行平滑滤波，滤波效果如图5所示。

图5 经低通滤波处理后的合加速度效果

本系统的惯性模块JY901采样率设置为100 Hz，即每秒钟可完成100组数据的采集，每组数据包括三轴角速度和三轴加速度，实测采集15 s运动数据，共计1 500组。由图5可知，将低通滤波器的截止频率设为3.5，滤波后的运动规律更明显，可以容易的检测出单步运动的波峰以及波谷，有利于提高步态检测的精度。

5 测试结果与分析

5.1 步数检测和位移计算

本文采用设置阈值的方法来检测步态，根据行人迈步规律，单步运动可分为开始，步中，结束三个关键节点[8]，首先检测单步运动的开始，即三轴合加速度波形的波峰，然后检测单步的步中，即波谷，如果峰谷时间差大于0.3 s且小于0.8 s，便可认为检测到了一步运动，然后将检测到的单步运动的相关数据信息保存到列表中，用于后期的位移计算。每当检测到单步运动时，即在程序中调用步长计算函数，将单步内的X,Y轴位移数据进行二次积分运算，得到具体的单步位移。为了适应复杂的应用环境和进一步降低传感器的抖动误差，本文将设备佩戴于腰部右侧，X,Y,Z轴分别对应运动方向的前、右、上，步数的检测结果如表1所示。

由表1可知，步数检测的准确度可达95%以上，具有良好的检测精度。

表1 步数检测结果

5.2 楼层检测

传统的三维定位中，大多使用气压计所测得的绝对气压值来进行海拔高度的换算，此方法不仅需要提前设置参考高度，还可能受到温度变化的影响，因此通用性较差，为解决此问题，本文采用通过气压相对变化量来表示楼层变化的方法，由气压计算海拔高度的公式可知

(7)

式中h为当前海拔高度，p为气压测量值，p0为海平面所对应的气压值。高度每升高8.43 m，气压降低1 hPa。本文测试所在楼层的高度为4.3 m左右，且两层楼中间设有楼梯转向平台，实测结果如图6所示。

图6 气压值的相对变化量和楼层的对应关系

经实测可知，根据气压值的累计变化来进行楼层检测，可以达到半层楼的检测精度，具有一定的实用性。

5.3 系统定位效果验证

在上位机的位置显示部分，本系统采用基于粒子滤波的地图匹配算法，首先利用由Python编写的地图生成器生成室内建筑物的二维平面地图，预先规定的运动路线如图7所示。

图7 实际测试规画路线

通过设置一系列的特定地图标定元素，将实际的墙壁和电梯、楼梯处分别用不同元素来限定，只有当被定位目标运动到预先标定的高度可改变区域，即电梯、楼梯处，气压的变化才可以用来表示楼层变化，然后二维地图随即切换到对应楼层，二维地图中的轨迹跟踪实测效果如图8所示。

图8 实际测试运动轨迹

图7中灰线为已规划的行走路线，方向如箭头所示，地点为河北工业大学电子信息工程学院楼四楼楼道，图8为按照规划路线进行轨迹追踪的效果图，其中方块色起点，灰色粒子为运动过程中的实际轨迹。

6 结 论

本文的主要研究内容是通过姿态传感器采集惯性信息，通过气压计采集气压信息，然后由树莓派进行数据处理，把处理过的两轴位移数据和气压数据通过无线通信模块发送到上位机，由上位机进行基于粒子滤波的实时位置的显示。系统整体结构设计简单可靠，且能够实现真正的实时定位，水平定位误差达到了0.5 m以内，楼层检测精度可以达到半层楼，有一定的应用价值。