导航定位服务具有很好的实用价值和应用前景^[1]. 当前常用的定位产品, 大部分采用非自主式定位技术, 这类技术主要包括卫星导航、WiFi、蓝牙、超宽带、蜂窝移动网络. 在复杂的建筑物或矿井里, 卫星导航信号易被遮挡或缺失, 提供的定位服务精准度较低; WiFi、蓝牙都是利用接收端与信号源的信号衰减强度换算成两者之间的距离, 再采用三角定位法对位置进行估算, 但在复杂多变的室内环境中很难建立准确的信号衰减模型; 超宽带利用脉冲信号在接收端与发射端的时间差来测量它们的距离, 再采用三角定位法估算位置, 在视距范围内可达厘米级定位精度, 但超宽带设备价格昂贵, 部署成本较高; 随着5G网络技术的大力发展, 蜂窝移动网络可以便捷使用搭建的基础设施, 利用移动通信系统的体系结构和传输信息实现用户的位置坐标推算, 但受基站密度的影响, 目前蜂窝移动网络定位精度较低, 无法实现精确的定位. 当人员处于火灾救援、矿井抢险等极端或未知环境时, 无法获得非自主式定位技术所需的有效信息, 这就对导航方式的自主性提出了更高要求^[2]. 惯性导航技术不依赖于外部信息, 是当前实现室内人员定位的主要研究方向.

惯性导航技术依靠自身的惯性传感器, 能够敏感载体运动的角速度和加速度, 根据牛顿运动定律, 通过3个方向的加速度数据积分计算出三维速度和位置进行导航推算. 该技术能够实现连续定位, 且不受外界环境干扰, 但是由于惯性传感器的器件误差, 长时间的误差积累严重影响定位的精度, 需要采用主动或被动的方式来抑制误差发散^[3]. 零速修正技术是进行误差抑制的一种简单而且有效的手段, 利用人员行走过程中的零速作为观测量, 通过周期性的误差修正以提高定位精度.

人员在运动过程中, 脚部与地面保持相对静止的时间十分短暂, 一种准确率高、鲁棒性强的零速检测方法是基于惯性导航技术的室内定位算法的核心. 目前典型的零速检测方法有: (1)固定阈值法^[4], 通过对单一传感器或某种组合观测量设定阈值, 满足阈值要求即检测得到零速区间, 这种方法仅在正常行走模式下检测效果较好, 但是对于快走、跑步等运动模式, 该方法的检测效果并不理想; (2)外部传感器检测法^[5], 利用压力传感器或超声波数据辅助检测得到零速区间, 外部传感器需要绑在脚上或腿上, 这使得穿戴更加繁琐, 增加外部传感器使得数据处理量增加且成本较高, 在实际应用中并不常见.

本研究提出了基于改进零速检测方法的室内定位系统, 适用于正常行走、小跑、上下楼等多种运动模式, 经多次试验验证, 该方法可以控制导航误差在1%里程以内, 实时将位置信息传送至服务端, 图形界面显示人员轨迹, 为一些安全生产和紧急救援提供了重要的技术支撑.

1 室内系统定位设计 1.1 系统架构设计

本系统由设备端、手机端、云端3部分组成, 主要完成惯性传感器数据的采集、处理及导航解算, 解算得到的位置数据上传到云端保存后, 可视化显示人员的实时位置与航迹, 具体如图1所示.

(1)设备端: 由三轴陀螺仪、三轴加速度计、蓝牙等组成^[6], 负责采集运动时的数据, 并通过蓝牙通信协议将导航解算后的数据传送到手机端;

(2)手机端: 安装对应的APP, 主要是通过手机蓝牙, 接受设备端的数据通信, 再通过WiFi、4G等网络通信方式, 将数据实时上传到云端.

(3)云端: 包括数据存储模块和应用模块^[7]. 数据存储模块负责保存上传的原始数据, 应用模块是对位置数据进行处理、分析和显示, 比如根据定位数据推算确定人员的位置和方向, 显示在已导入的建筑模型中, 以及安全轨迹预警等.

1.2 数据协议

传感器收集的数据处理后, 再经导航解算, 按规定的协议格式通过蓝牙通信协议, 传输到手机APP. 协议具体如表1所示.

表1中, 数据类型以及其对应数据长度如表2所定义.

2 室内人员定位惯性导航算法 2.1 基本原理

人员在运动过程中, 脚部会周期性出现离地、摆动、触地、静止4个阶段, 也就是一个步态周期, 如图2所示. 当脚部处于静止状态时, 此时人员的理论速度为零. 惯性传感器固定在人员脚部, 采集x、y、z三个方向的角速度(单位为度/秒, 即deg/s), 当3个方向角速度在某段时间内均为零时, 则把此段称为静止区间, 如图3所示.

把速度为零作为卡尔曼滤波的虚拟观测, 可以建立卡尔曼滤波方程, 修正惯性导航误差^[8]. 以常用的9状态滤波器为例, 设模型的系统状态为导航系统误差, 如式(1)所示:

$\delta x = {\left[ {\delta \varphi \; \; \delta v\; \; \delta r} \right]^{\rm{T}}}$

(1)

式中, $\delta \varphi = \left[ {{\varphi _E}\; \;{\varphi _N}\; \;{\varphi _U}} \right]$ 为姿态角误差, $\delta v = \left[ {{v_E}\; \;{v_N}\; \;{v_U}} \right]$ 为速度误差, $\delta r = \left[ {{r_E}\; \;{r_N}\; \;{r_U}} \right]$ 为位置误差.

建立卡尔曼滤波状态方程和量测方程^[9]:

$\delta \dot x = F\delta x + \omega $

(2)

${\textit{z}} = Hx + \upsilon $

(3)

系统的观测量z如下:

${\textit{z}} = \delta {v_k} = {v_k} - \left[ {0,0,0} \right]$

(4)

观测矩阵为:

$H = \left[ {{0_{3 \times 3}}\; \;{I_{3 \times 3}}\; \;{0_{3 \times 3}}} \right]$

(5)

这就是基于惯性导航技术的室内定位算法基本原理, 其中零速检测的准确性是影响系统定位精度的重要原因之一.

2.2 改进的零速检测算法

零速检测算法是基于零速检测的人员定位算法中重要部分, 当惯性传感器被固定在鞋上时, 三轴陀螺仪和三轴加速度计可以敏感到脚部的运动状态, 实时检测脚部离地、摆动、触地、静止的周期性变化. 在目前的应用中, 常采用加速度计和陀螺仪固定阈值法, 利用统计学特性来完成静止区间的检测.

$\left\| {{f_k}} \right\| = \sqrt {f_x^2 + f_y^2 + f_{\textit{z}}^2} $

(6)

$\left\| {{\omega _k}} \right\| = \sqrt {\omega _x^2 + \omega _y^2 + \omega _{\textit{z}}^2} $

(7)

式中, $||{f_k}||$ 表示加速度计模值, $||{f_{x/y/{\textit{z}}}}||$ 表示加速度计测量值, $||{w_k}||$ 表示陀螺仪模值, $||{w_{x/y/{\textit{z}}}}||$ 表示陀螺仪测量值.

当 $||{f_k}||$ 和 $||{w_k}||$ 分别小于其设定阈值, 同时满足判断条件时, 即判定此时间段为静止区间, 但是在实际应用中, 由于人员脚部运动的随机性, 在一个步态周期内常常出现漏检, 如图4所示. 由于x轴陀螺仪数据较大, 会造成大量数据漏检, 但此时脚部仍然处于静止阶段.

针对以上漏检的情况, 本文对基于固定阈值的零速检测算法进行改进. 由于人员运动属于低速运动, 毫秒级的延迟不会影响定位精度, 故采用滑动窗的方式, 缓存需判断时刻及其前后各3个时刻的传感器数据, 设滑动窗缓存陀螺仪测量值为 ${\omega _{{x_i}}},{\omega _{{y_i}}},{\omega _{{{\textit{z}}_i}}},i = 1,2, \cdots, 7$ , 缓存加速度计测量值为 ${a_{{x_i}}},{a_{{y_i}}},{a_{{{\textit{z}}_i}}},i = 1,2, \cdots, 7$ , 分别设定陀螺仪判断阈值Thre_Gyro和加速度计判断阈值Thre_Acc, 判断条件如式(8)和式(9):

$\sum\limits_{i = 1}^{i = 7} {\sqrt {\omega _{{x_{_i}}}^2 + \omega _{{y_{_i}}}^2 + \omega _{{{\textit{z}}_{_i}}}^2} } \le Thre\_Gyro$

(8)

$\sum\limits_{i = 1}^{i = 7} {\sqrt {a_{{x_i}}^2 + a_{{y_i}}^2 + a_{{{\textit{z}}_i}}^2} } \le Thre\_Acc$

(9)

当式(8)和式(9)满足逻辑与的条件下, 判断此时刻为静止区间, 当式(8)和式(9)满足逻辑或的条件下, 进行重新判断.

由图4可以看出, 出现漏检的原因是由于x轴陀螺仪变化较大, 造成此现象的原因是脚踝有轻微的抖动, 但脚部实际仍处于静止状态. 针对此干扰项, 选取当前时刻与缓存区其它时刻数据进行冗余判断, 若当前时刻与其它时刻的变化较小, 即认为该时刻为静止区间. 分别设定陀螺仪冗余判断阈值ThreDiff_Gyro和加速度计冗余判断阈值ThreDiff_Acc, 判断条件如式(10)和式(11)所示.

$\begin{split} & \sum\limits_{i = 1}^{i = 7} {\sqrt {{{(a_{{x_i}}^{} - a_{{x_4}}^{})}^2} + {{(a_{{y_i}}^{} - a_{{y_4}}^{})}^2} + {{(a_{{{\textit{z}}_i}}^{} - a_{{{\textit{z}}_4}}^{})}^2}} }\\ &\le ThreDiff\_Gyro \end{split} $

(10)

$\begin{split} & \sum\limits_{i = 1}^{i = 7} {\sqrt {{{(\omega _{{x_i}}^{} - \omega _{{x_4}}^{})}^2} + {{(\omega _{{y_i}}^{} - \omega _{{y_4}}^{})}^2} + {{(\omega _{{{\textit{z}}_i}}^{} - \omega _{{{\textit{z}}_4}}^{})}^2}} } \\ & \le ThreDiff\_Acc \end{split} $

(11)

当式(10)和式(11)同时满足条件情况下, 判断此时刻为静止区间, 改进零速检测方法流程图如图5.

利用以上改进的零速检测方法后, 测出结果如图6所示, 可以看出图4中出现的漏检数据被正确检测判断为静止区间.

在水平地面, 选择步行、跑步两种状态进行测试, 结果分别如图7、图8所示. 从这两个图可以看出, 改进零速检测算法比原零速检测算法定位更准确, 在走路和跑步情况下均提高了定位精度, 达到了预期效果.

3 室内定位系统实现

室内定位系统由设备端、手机端和云端组成. 设备端是集成硬件于一体的装置, 人员可随身携带; 手机端安装对应的APP, 打开蓝牙, 选用4G网络用于数据的通信; 云端是部署在服务器上, 有固定的IP, 用户可随时访问.

将室内建筑模型坐标保存到系统, 设备上的位置数据上传之后可以实时绘制人员轨迹, 也可以浏览历史轨迹, 方便在特殊情况下进行任务调配和危险处置, 如图9所示. 还支持划定安全区电子围栏、警报区和禁区电子围栏, 进入则触发相应级别的报警, 有益于安全生产和紧急救援.

4 实验与分析

实验人员佩戴此系统装置进入测试建筑物, 从A点出发, 穿越一层长廊到达北侧消防楼梯间(B点), 向上爬行至3层, 到达C点, 再穿越长廊到达南侧防烟楼梯间(D点), 从此处消防楼梯间下至1层, 回到起始A点结束, 完成一个闭环测试路径, 期间包括正常行走、小跑、上下楼等运动方式.

从图10可以看出, 测试全程约为330 m, 闭合误差为1 m, 闭合误差为0.4%里程, 满足1%里程的设计精度, 证明该室内定位系统在不依赖于外界信息的情况下具有良好的实时自主导航性能.

5 结论

基于惯性导航技术的室内定位系统为人员定位提供了新的解决方案, 作为一种自主式导航方式, 不需要外界基站的辅助, 提高了环境适用性, 在消防救援、抢险救灾等未知环境下具有不可替代的作用.

基于零速修正的惯性导航算法中航向角是不可观的, 故无法对航向角误差进行修正, 后期需继续针对航向角的误差修正进行研究, 同磁力计^[10]、雷达^[11]、Slam^[12]、UWB^[13]等多种导航信息源进行数据融合是进一步提高室内定位精度、适用性和可靠性的有效途径, 本文的研究也为室内定位多传感器融合技术奠定了研究基础.