[发明专利]一种行人定位与轨迹跟踪方法和系统

说明书

技术领域

本发明属于惯性导航领域，更具体地，涉及一种行人定位与轨迹跟踪方法和系统。

背景技术

随着科学技术的快速发展和城市化的快速推进，定位技术在人们生活中变得越发重要，人们对定位产品、功能和服务的需求加大，从而催生了庞大的市场。传统定位技术基本上都是基于GPS、无线网络(Wifi、ZigBee、UWB等)，但在无GPS信号的室内，以及没有无线网络的地方，这种定位方式不能使用。因此基于惯性导航传感器的行人定位与轨迹跟踪技术逐渐兴起，其最大的特点是在无基础固联设施的已知或未知的环境下能够实现自主导航和实时定位。常规的基于惯性导航传感器的行人定位装置通常仅采用加速度计和陀螺仪，然后配合姿态解算以及零速检测算法，进行三维轨迹推算与定位。但是这种方案，由于陀螺仪的漂移，导致航向角最终会发散，在长时间导航定位情况下，定位会失败。同时，常规的基于惯性导航传感器的定位方案，有时还会采用对行人步长估计的方式进行定位，这种方案依赖特定人员的平均步长，因而对人员的适应性较差，仅适用于特定人员的定位场景，大大降低了基于惯性导航传感器的行人定位装置的应用范围。

由此可见，现有技术存在无法长时间导航、适应性较差和应用范围窄的技术问题。

发明内容

针对现有技术的以上缺陷或改进需求，本发明提供了一种行人定位与轨迹跟踪方法和系统，由此解决现有技术存在无法长时间导航、适应性较差和应用范围窄的技术问题。

为实现上述目的，按照本发明的一个方面，提供了一种行人定位与轨迹跟踪方法，包括：

(1)接收行人的加速度信息、角速度信息，对加速度信息和角速度信息进行滤波和阈值分割，得到速度状态信息，速度非0时利用角速度信息得到姿态信息，速度为0时利用加速度信息修正角速度信息得到姿态信息；

(2)接收磁力计信息，对磁力计进行椭圆校正后，再利用姿态信息进行倾角补偿，得到磁力航向角；

(3)将磁力航向角和姿态信息中的航向角融合，得到融合航向角，结合姿态信息和融合航向角，对加速度积分得速度信息，速度信息经过零速校正后，再次积分得到水平方向上的位移和垂直方向的高度，最后通过位移和高度进行行人定位与轨迹跟踪。

进一步的，步骤(1)包括：

(1-1)对加速度信息和角速度信息进行滤波和阈值分割，得到速度状态信息；

(1-2)速度非0时利用角速度信息更新四元数，利用四元数得到姿态信息；速度为0时，采用比例积分，利用加速度信息修正角速度信息的漂移，采用修正后角速度信息更新四元数，进而得到姿态信息。

进一步的，融合航向角为：

其中，0≤i≤n，c_i为i时刻的磁力航向角，y_i为i时刻的姿态信息中的航向角，h_i为i时刻的融合航向角，E_i为i时刻的航向角偏移，k_P为比例系数，k_I为积分系数。

进一步的，步骤(3)包括：

(3-1)将磁力航向角和姿态信息中的航向角融合，得到融合航向角；

(3-2)速度信息经过零速校正后，水平方向上的位移可由水平方向速度直接积分得出，垂直方向上的速度需进一步通过上、下楼判断，进行高度补偿后，得到垂直方向的高度；

(3-3)利用位移和高度进行行人定位与轨迹跟踪。

按照本发明的另一方面，提供了一种行人定位与轨迹跟踪系统，包括：加速度计、陀螺仪、磁力计和控制模块，

加速度计用于接收行人的加速度信息，传输至控制模块；

陀螺仪用于接收行人的角速度信息，传输至控制模块；

所述磁力计用于接收行人的磁力计信息，传输至控制模块；

所述控制模块用于利用加速度信息、角速度信息得到速度状态信息与姿态信息，利用磁力计信息得到磁力航向角，将磁力航向角和姿态信息中的航向角融合，得到融合航向角，利用位移和高度进行行人定位与轨迹跟踪。

进一步的，控制模块包括步态子模块、融合子模块和导航子模块。

进一步的，步态子模块用于对加速度信息和角速度信息进行滤波和阈值分割，得到速度状态信息，通过对加速度积分得到速度信息，速度信息经过零速校正后，水平方向上的位移可由水平方向速度直接积分得出，垂直方向上的速度需进一步通过上、下楼判断，进行高度补偿后，得到垂直方向的高度。