一种行人定位与轨迹跟踪方法和系统与流程

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

背景技术：

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

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

技术实现要素：

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

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

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

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

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

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

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

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

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

其中，0≤i≤n，ci为i时刻的磁力航向角，yi为i时刻的姿态信息中的航向角，hi为i时刻的融合航向角，ei为i时刻的航向角偏移，kp为比例系数，ki为积分系数。

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

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

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

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

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

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

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

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

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

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

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

进一步的，融合子模块用于在行人速度非0时利用角速度信息得到姿态信息，行人速度为0时利用加速度信息修正角速度信息进而得到姿态信息，对磁力计进行椭圆校正后，利用姿态信息对其进行倾角补偿得到磁力航向角，将磁力航向角和姿态信息中的航向角融合，得到融合航向角。

进一步的，导航子模块用于利用位移和高度进行行人定位与轨迹跟踪。

总体而言，通过本发明所构思的以上技术方案与现有技术相比，能够取得下列有益效果：

(1)速度非0时，加速度信息受外力的影响，并不能较准确的反应姿态信息，此时利用加速度信息和角速度信息获取姿态信息的话，将降低姿态信息的解算精度；速度为0时，加速度信息可以较准确的反应姿态信息，此时用加速度信息来修正角速度信息的偏移，能够有效地提高姿态信息的解算精度，本发明速度非0时利用角速度信息得到姿态信息，速度为0时利用加速度信息修正角速度信息进而得到姿态信息，本发明的姿态信息精度高。

(2)本发明将磁力航向角和姿态信息中的航向角融合，得到的融合航向角，是特征级的融合，由于磁力计信息容易受干扰，若在数据层，将磁力计信息与加速度信息、角速度信息一起融合计算姿态角，易将磁力计信息的误差扩散至俯仰角与横滚角，从而降低姿态信息的精度，而采取本发明特征级融合方案后，在不降低俯仰角与横滚角精度的前提下，能够有效地用磁力计信息来改善融合航向角的漂移，从而进一步提高融合航向角的精度。

(3)本发明不依赖于特定人员的步行平均步长，算法计算量小，可满足大部分应用的要求，本发明可以长时间导航、适应性较强且应用范围宽。

(4)优选的，速度信息经过零速校正后，水平方向上的位移可由水平方向速度直接积分得出，垂直方向上的速度需进一步通过上、下楼判断，再融合高度补偿，以补偿高度的漂移，这样在保证二维定位精度的情况下提升三维定位的精度。

附图说明

图1是本发明实施例提供的一种行人定位与轨迹跟踪方法的流程图；

图2是本发明实施例1提供的一种行人定位与轨迹跟踪系统的结构图；

图3是本发明实施例1提供的一种行人定位与轨迹跟踪系统的结构设计和拓扑关系；

图4是本发明实施例1提供的姿态解算算法流程图。

具体实施方式

为了使本发明的目的、技术方案及优点更加清楚明白，以下结合附图及实施例，对本发明进行进一步详细说明。应当理解，此处所描述的具体实施例仅仅用以解释本发明，并不用于限定本发明。此外，下面所描述的本发明各个实施方式中所涉及到的技术特征只要彼此之间未构成冲突就可以相互组合。

如图1所示，一种行人定位与轨迹跟踪方法，包括：

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

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

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

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

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

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

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

其中，0≤i≤n，ci为i时刻的磁力航向角，yi为i时刻的姿态信息中的航向角，hi为i时刻的融合航向角，ei为i时刻的航向角偏移，kp为比例系数，ki为积分系数，t1与t2分别为比例限幅幅值与积分限幅幅值。

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

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

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

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

实施例1

如图2所示，本发明实施例1提供一种行人定位与轨迹跟踪系统的结构示意图，其主要由电源、加速度计、陀螺仪、磁力计、控制模块以及蓝牙组成，其连接方式是：电源与加速度计、陀螺仪、磁力计、控制模块以及蓝牙连接，控制模块与加速度计、陀螺仪、磁力计以及蓝牙双向连接。

如图3所示，本发明实施例1提供的一种行人定位与轨迹跟踪系统的结构设计和拓扑关系：优选的，磁力计为三轴磁力计，加速度计为三轴加速度计，陀螺仪为三轴陀螺仪，三轴磁力计经磁力计校正后，计算磁力航向角；三轴加速度计与三轴陀螺仪，通过ahrs姿态解算，不仅解算出姿态角还解算出世界坐标系下的行人步行的加速度；姿态角包括航向角、俯仰角与横滚角，姿态解算出的航向角与磁力航向角做融合后，更新世界坐标系下的行人步行加速度；利用三轴陀螺仪与世界坐标系下的行人步行加速度，通过步态提取，对行人步行状态进行分割，分割结果反馈至ahrs姿态解算以及用于零速校正；经过零速校正后，水平方向上的位移可由水平方向速度直接积分得出，垂直方向上的速度需进一步通过上、下楼判断，进行高度补偿后，再计算垂直方向的高度。

如图4所示，姿态解算采取四元数法进行计算，首先初始化四元数q，当步态判断为非零速时，采用陀螺仪更新四元数；而当步态判断为零速时，采用pi(比例积分)算法，利用加速度计修正陀螺仪的漂移，并采用修正后的陀螺仪更新四元数，最后通过四元数解算三个姿态角：姿态航向角y、滚转角r和俯仰角p，利用磁力计计算磁力航向角c，而航向角融合分为实时融合和定时融合两个阶段。实时融合阶段使用磁力计来实时修正航向角，定时融合阶段定期采用融合后的航向角更新四元数，以使陀螺仪的积分漂移定期清零。

本发明与现有技术相比，无需基础固联设施，没有外界的辅助设备，采用全自主式定位方式，能够方便地用于行人定位与轨迹跟踪；而在相同的硬件方案中，采用改进的算法结构，不依赖于特定人员的步行平均步长，算法计算量小，可满足大部分应用的要求。

采用在特征级融合航向角的算法，在有效地改善航向角漂移的同时，并没有影响俯仰角以及横滚角的解算精度，并利用零速判断的结果反馈至ahrs姿态解算，避免加速度在受外力干扰的情况下，依然用于修正陀螺仪的零漂，从而进一步提高了姿态角的解算精度。

采用零速校正方式，不仅对水平速度做了二次平滑估计，对于其垂直方向利用步行速度特征，做了可变参的二次平滑估计，进一步的提高了本装置的三维定位精度。

本领域的技术人员容易理解，以上所述仅为本发明的较佳实施例而已，并不用以限制本发明，凡在本发明的精神和原则之内所作的任何修改、等同替换和改进等，均应包含在本发明的保护范围之内。