[发明专利]一种行人主航向更新方法、行人航迹获取方法及系统

权利要求书

1.一种行人主航向更新方法，其特征在于，应用于在行人腰部同时设置陀螺仪、腰部三轴加速度计和磁力计的场景，包括：

基于行走起点腰部三轴加速度计和磁力计的输出信息解算出行人在地理坐标系下的初始主航向角；

设置计算步数n，所述n为正整数；

在行人行走第k步后，执行：

基于陀螺仪的输出信息获取地理坐标系下第k步陀螺仪航向角变化值，基于腰部三轴加速度计和磁力计的输出信息获取地理坐标系下第k步磁力计航向角变化值；

当k＜n时，若第k步陀螺仪航向角变化值和第k步磁力计航向角变化值均小于航向角变化阈值，按照方式一更新主航向角，否则按照方式二更新主航向角；

当k≥n时，若第k步到第k-n步的陀螺仪航向角变化值的平均值和第k步到第k-n步的磁力计航向角变化值的平均值均小于航向角变化阈值，按照方式一更新主航向角，否则按照方式二更新主航向角；

方式一：将第k步的主航向角更新为第k－1步的主航向角；

方式二：融合第k步磁力计航向角变化值和第k步陀螺仪航向角变化值获得第k步航向角变化值，将第k步的主航向角更新为第k－1步的主航向角与所述第k步航向角变化值之和。

2.如权利要求1所述的行人主航向更新方法，其特征在于，初始主航向角Yaw₀不考虑陀螺仪解算的陀螺初始航向角Yaw_gry0，仅由磁初始主航向角Yaw_mag0决定，所述初始主航向角Yaw₀根据如下公式获得：

所述M_x表示行走起点在地理坐标系的x轴方向的磁场强度，M_x＝m_xcosγ+m_zsinγ，所述M_y表示行走起点在地理坐标系的y轴的磁场强度，M_y＝m_xsinγsinθ+m_ycosθ-m_zcosγsinθ，γ和θ分别表示在行走起点腰部三轴加速度计输出的辅助横滚角和辅助俯仰角，m_x、m_y、m_z分别表示行走起点的磁场在载体坐标系的x轴、y轴、z轴的投影分量，m_x、m_y、m_z从磁力计在行走起点的读数[m_x,m_y,m_z]^T中获取。

3.如权利要求1所述的行人主航向更新方法，其特征在于，基于陀螺仪的输出信息获取地理坐标系下第k步陀螺仪航向角变化值，包括：在行人行走第k步后，利用旋转矢量方法和三角函数关系，分别基于第k步起始时刻和结束时刻的陀螺仪输出信息解算出行人在第k步起始时刻和结束时刻的陀螺仪航向角，将第k步的结束时刻和起始时刻的陀螺仪航向角的差值作为第k步陀螺仪航向角变化值；

基于腰部三轴加速度计和磁力计的输出信息获取地理坐标系下第k步磁力计航向角变化值，包括：利用四元数法分别基于第k步的起始时刻和结束时刻腰部三轴加速度计和磁力计的输出信息解算出行人在第k步的起始时刻和结束时刻的磁力计航向角，将第k步的结束时刻和起始时刻的磁力计航向角的差值作为第k步磁力计航向角变化值。

4.如权利要求1所述的行人主航向更新方法，其特征在于，所述第k步航向角变化值为第k步磁力计航向角变化值和第k步陀螺仪航向角变化值的加权平均值。

5.一种行人航迹获取方法，其特征在于，应用于在行人腰部设置陀螺仪、腰部三轴加速度计和磁力计，以及在行人足部设有足部三轴加速度计的场景，包括：

实时采集陀螺仪、腰部三轴加速度计、磁力计和足部三轴加速度计的输出信息；

从行走起点开始，以足部三轴加速度计的合速度过零一次作为一步进行计步；

执行权利要求1－4之一所述行人主航向更新方法中的步骤进行行人主航向角更新；

基于计步信息，组合所述腰部三轴加速度计和所述足部三轴加速度计的输出信息获取行人的步长；

基于行人行走中每步的主航向角和步长获得行人航迹。