基于人体运动学辅助的可穿戴式行人导航系统

张 苗，许建新，黄 欣，殷德全，熊 智

（南京航空航天大学，南京211106）

0 引言

目前，较为成熟的个人移动定位服务系统主要依赖于卫星导航与无线通信定位技术。而当卫星信号不良时，系统面临瘫痪的风险，无线通信信号易受到干扰，系统的稳定性与可靠性不强。尤其是在特种行业领域，如消防救援、野外搜救、井下勘探等复杂应用环境下，GPS以及无线通信信号往往失效不可用，给救援任务和特种作业带来了极大的困难，甚至导致巨大的财产损失和人员伤亡。同时，特种作业人员在执行任务时，可能出现匍匐、跳跃、跑步等多种复杂运动模态，与行人正常的行走运动模态有较大差别，对传感器的测量范围、抗高温、抗震动等能力，以及对算法的适应性具有较高的要求。

本文结合特种行业的个人定位要求，主要研究了基于人体运动学辅助的行人导航定位技术，并研制了可穿戴式行人导航系统。针对行人的多种运动模态进行了测试验证，同时该系统可在上位机界面实时显示行人的运动状态和二维轨迹信息，便于外部人员对行人的实时监控和指挥。

1 系统算法原理

1.1 基于人体运动学模型的步态检测算法

将可穿戴式MEMS惯性传感测量元件固定于人体足部脚面位置，通过检测运动时人体足部输出的加速度信息和角速度信息来判断人体处于何种运动模态，并使用判断条件检测出对应的零速时刻，进行速度和姿态的更新。图1和图2分别为人体正常行走时IMU的三轴加速度计和三轴陀螺仪原始数据输出。

由图1和图2可看出，三轴加速度计与陀螺仪数据具有明显的周期性。基于人体正常行走时的步态变化规律，可以对应到三轴加速度计和陀螺仪的数据变化规律。经过分析得知，足部与地面实际接触时，陀螺三轴值和加速度计三轴值都较为平稳，且接近于零，认为这段时间为人体运动时的 “零速时刻”，即此时的速度为零。

由于零速检测的准确性直接影响着导航精度，因此选择合适的零速检测方法至关重要。常用的零速检测条件包括加速度计三轴模值与三轴标准差、陀螺仪三轴模值与三轴标准差。检测条件具体如下：

综合利用三轴加速度计和陀螺仪的输出信息特征，本文设计了采用加速度计三轴模值、加速度计三轴标准差与陀螺仪三轴模值组合判断零速时的判别方法。

a1和σ1是加速度计三轴模值与标准差，g1和σ2是陀螺仪三轴模值与标准差，当a1小于阈值εa1、σ1小于阈值εa2且g1小于阈值εω1、σ2小于阈值εω2时，零速检测标志置为1，即成功检测到零速区间。 其中，阈值 εa1、εa2与 εω1、εω2是根据多组实验的加速度计三轴模值曲线和三轴标准差曲线、陀螺仪三轴模值曲线和三轴标准差曲线与对应周期的人体步行零速区间对比分析得到。

1.2 基于Kalman滤波的零速修正方法

Kalman滤波是由Kalman于1960年提出的一种滤波方法，其优点为高效率、可实时估计且具有递推性，该方法由前一时刻的估计值结合当前时刻的测量值递推得到当前时刻的估计值。本文将惯性传感器固定于足部脚面上，当脚与地面实际接触时速度为零，位置不变，可以将速度误差、位置误差作为观测量，经Kalman滤波估计惯导系统误差并进行反馈校正，从而抑制惯导系统的误差，提高导航定位精度。

基于MEMS的惯性导航系统虽是连续系统，但只需线性化就可以采用离散Kalman滤波算法进行误差状态估计。Kalman滤波器状态量为18维状态变量，具体如下：

其中，φe、φn、φu为惯导平台误差角，δVe、δVn、δVu为速度误差，δL、δλ、δh为位置误差，εbx、εby、εbz为陀螺三轴零偏，εrx、εry、εrz为陀螺1阶Markov过程随机误差，▽rx、▽ry、▽rz为加速度计1阶Markov过程随机误差。

在检测到零速区间时，认为这段时间人体运动的速度为零，位置保持不变，可以将惯导解算的速度信息与零速作差作为量测量，同时将惯导解算的位置信息与上一时刻惯导解算的位置信息作差作为量测量，所构成的系统量测方程为：

式中，[LINSλINShINS]为惯导解算的位置信息；[LZUPTλZUPThZUPT]为上一时刻惯导解算的位置信息；[veINSvnINSvuINS]为惯导解算的速度信息；[veZUPTvnZUPTvuZUPT]为速度零速，即[0 0 0]（单位：m/s）。 式中，量测矩阵 H（t）与量测噪声矩阵NZUPT（t）分别为：

2 系统硬件架构

2.1 硬件平台设计

本文所设计的可穿戴式行人导航系统硬件部分主要由导航计算机树莓派3B、MTI-700 MEMS惯性传感器、手机控制端、远程监控端和电源模块构成。硬件部分各模块介绍如下：

1）树莓派3B：本文中所使用的导航计算机是树莓派3B，树莓派（Raspberry Pi）是一款基于Linux的单板机电脑，由英国的树莓派基金会所开发，树莓派3B实物如3图所示。

树莓派3B硬件平台的各项参数如下：

①1.2GHz四核博通（Broadcom）BCM2837 64位ARMv8处理器。4核处理器满足了多线程行人导航设备对处理器的计算需求，可以大幅提高程序的运行效率，保证了算法的实时精确解算。

②板载BCM43143 WiFi模块。行人导航设备可以通过该WiFi模块连接网络，为手机控制端的控制功能，以及远程监控端的网络通信提供了极大便利性。

③板载低功耗（BLE）蓝牙模块。可以通过蓝牙模块进行手机app与行人导航设备之间的蓝牙通信，方便后续工作的可扩展性。

④1GB RAM。大内存保证了程序的稳定运行，保证了行人导航程序的长时稳定运行。

⑤4个USB端口。该行人导航设备的处理器与传感器之间通过USB串口进行通信，方便传感器的扩展。

⑥40针扩展GPIO。引脚可以根据特殊需求进行配置，提供了更加丰富的接口以备使用。

⑦HDMI和RCA视频输出。外接显示器，可以在处理器上直接调试导航设备程序，为野外工作提供了便利。

⑧微型SD端口。用于下载操作系统以及存储数据。

2）MTI-700 MEMS惯性传感器：MTI-700惯性传感器主要是向行人导航系统提供IMU数据，是整个行人导航解算中最重要的传感模块，其精度直接决定了行人导航系统的性能，如图4所示。

MTI-700由三轴加速度计、三轴陀螺仪、三轴磁强计、16位的AD、内部DSP、非易失性存储器和串口收发器组成。各个传感模块的频率可以根据需求进行配置，以满足导航解算对数据采集频率的需要。MTI-700系统主要功能是实时采集行人的运动信息，并将采集到的数据传输给处理器进行解算。

3）手机控制端：手机控制端由智能手机构成，智能手机通过Vnc Server登录树莓派3B的界面系统，通过智能手机即可控制导航程序的开始和终止，同时行人可以通过智能手机实时观察到自己的位置及运动信息。手机登录树莓派界面系统如图5所示。

4）远程监控端：本文研究的远程监控端是通过Qt软件开发平台开发的实时人机交付操作系统，该操作系统通过TCP网络与行人导航终端进行网络通信，实时获取行人的运动和位置信息。同时，该界面还可以实时显示行人的运动轨迹，并将运动信息保存下来，以支持事后轨迹回放和数据分析。行人在进行特种作业时，监控端可以根据需要发出特定的指令，以保证作业的顺利完成。远程监控界面如图6所示。

2.2 具体使用流程

本文研制的基于人体运动学辅助的可穿戴式行人导航系统具体使用流程如图7所示。

首先，用户使用手机通过Vnc Server登录处理器界面，同时在手机控制端启动行人导航程序；用户在原地静止约5s时间进行初始对准；然后用户可以开始运动，足部固定的传感器可以实时采集人体运动数据，并实时发送到处理器进行导航信息解算；用户通过手机控制端可以看到自己的行走轨迹，远程监控端也可以实时显示出行人的运动轨迹；用户可以通过手机控制端终止行人导航程序，终止行程。用户的运动信息被保存下来，以支持事后轨迹回放和数据分析。

3 系统测试与验证

为验证本文所设计系统的可靠性和适应性，测试人员穿戴该系统进行了多场景测试，测试场地有操场、足球场、篮球场、办公楼等。测试人员穿戴效果如图8所示。

图9为部分测试的二维/三维轨迹图，表1为行人导航系统测试结果。

表1 行人导航系统测试结果Table 1 Test results of pedestrian navigation system

在实际测试过程中，测试人员运动较为随意，不受行人导航设备的影响，行走的路线涉及直线、多边形、半圆、上下楼梯等多种场景。在操场绕行3圈后，行走误差仅为7m；在上下楼梯1圈后，误差仅为0.17m。同时，高度方向解算较为精确，可以辨识出楼层的变化。由以上可知，本文研制的行人导航系统在15min运动时间内可保持高精度定位，行走路线长度在1.2km内定位精度保持不变，行人导航系统的误差平均在路线总长度的1.1%左右，最大不超过行走距离的1.7%，行人导航的精度不受具体行走路线复杂程度的限制。

4 结论

本文首先基于人体运动学原理研究了步态检测算法，并引入基于Kalman滤波的零速修正辅助算法，估计惯导误差并进行反馈校正，从而抑制惯导系统的误差，提高导航定位精度。研制了一种集信息采集、数据传输、导航解算与监控显示于一体的可穿戴式行人导航系统，体积小、佩戴便携、使用简易、续航时间长，可满足行人长时、多动态、多环境下的高精度三维定位需求，具有较高的工程应用价值。