CN113188542A - 一种行人主航向更新方法、行人航迹获取方法及系统 - Google Patents

Abstract

本发明公开了一种行人主航向更新方法、行人航迹获取方法及系统。该主航向更新方法包括：在行人行走第k步后执行：获取地理坐标系下第k步陀螺仪航向角变化值，获取地理坐标系下第k步磁力计航向角变化值；当k≥n时，若第k步到第k‑n步的陀螺仪航向角变化值的平均值和第k步到第k‑n步的磁力计航向角变化值的平均值均小于航向角变化阈值，将第k步的主航向角更新为第k‑1步的主航向角，否则将第k步的主航向角更新为第k‑1步的主航向角与第k步航向角变化值之和。通过多个传感器在同一个部位的测量信息作为主航向更新的数据来源，以此解算出每一步的航向角变化值，有效降低身体随机小幅摆动等行为特征对航向信息带来的影响，有效提高了航向精度。

Description

一种行人主航向更新方法、行人航迹获取方法及系统

技术领域

本发明涉及惯性导航领域，特别是涉及一种行人主航向更新方法、行人航迹获取方法及系统。

背景技术

行人运动主要的三个角度变化为俯仰角、横滚角与航向角，分别记为Pitch，Roll，Yaw。其中，航向角是行人水平行走转动方向的角度变化，是决定人员定位的最主要的评价标准之一。传统的螺仪解算航向角方法中，由于陀螺仪智能获得角速度的变化量，需要对获取的角速度相对时间进行积分，才可以得到航向角的变化值，会出现累计误差问题，并且适应性较差。在行人航迹推算中需要获得行人每步的航向角和步长，现有的步长获取方法通常通过设置于足部的加速度传感器获得足部步长，以足部步长作为所求步长，这样获取的步长误差较大。

发明内容

本发明旨在至少解决现有技术中存在的技术问题，特别创新地提出了一种基于主航向更新的行人航迹获取方法及系统。

为了实现本发明的上述目的，根据本发明的第一个方面，本发明提供了一种行人主航向更新方法，应用于在行人腰部同时设置陀螺仪、腰部三轴加速度计和磁力计的场景，包括：基于行走起点腰部三轴加速度计和磁力计的输出信息解算出行人在地理坐标系下的初始主航向角；设置计算步数n，所述n为正整数；在行人行走第k步后，执行：基于陀螺仪的输出信息获取地理坐标系下第k步陀螺仪航向角变化值，基于腰部三轴加速度计和磁力计的输出信息获取地理坐标系下第k步磁力计航向角变化值；当k＜n时，若第k步陀螺仪航向角变化值和第k步磁力计航向角变化值均小于航向角变化阈值，按照方式一更新主航向角，否则按照方式二更新主航向角；当k≥n时，若第k步到第k-n步的陀螺仪航向角变化值的平均值和第k步到第k-n步的磁力计航向角变化值的平均值均小于航向角变化阈值，按照方式一更新主航向角，否则按照方式二更新主航向角；方式一：将第k步的主航向角更新为第k－1步的主航向角；方式二：将第k步的主航向角更新为第k－1步的主航向角与第k步航向角变化值之和，融合第k步磁力计航向角变化值和第k步陀螺仪航向角变化值获得所述第k步航向角变化值。

上述技术方案：根据人员行走的航向特征，选择航向稳定的腰部作为航向信息数据采集点，在腰部放置陀螺仪、磁力计与加速度计，通过多个传感器在同一个部位的测量信息作为主航向更新的数据来源，以此解算出每一步的航向角变化值，有效降低身体随机小幅摆动等行为特征对航向信息带来的影响，提高组合导航的测量精度和稳定性，有效提高了航向精度，进而为后续的行人航迹推算过程提供精准、可靠的航向信息。

在本发明的一种优选实施方式中，初始主航向角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中获取。

上述技术方案：能够快速准确的获得初始主航向角。

在本发明的一种优选实施方式中，所述第k步航向角变化值为第k步磁力计航向角变化值和第k步陀螺仪航向角变化值的加权平均值。

上述技术方案：便于快速融合出第k步航向角变化值。

为了实现本发明的上述目的，根据本发明的第二个方面，本发明提供了一种行人航迹获取方法，应用于在行人腰部设置陀螺仪、腰部三轴加速度计和磁力计，以及在行人足部设有足部三轴加速度计的场景，包括：实时采集陀螺仪、腰部三轴加速度计、磁力计和足部三轴加速度计的输出信息；从行走起点开始，以足部三轴加速度计的合速度过零一次作为一步进行计步；执行本发明所述行人主航向更新方法中的步骤进行行人主航向角更新；基于计步信息，组合所述腰部三轴加速度计和所述足部三轴加速度计的输出信息获取行人的步长；基于行人行走中每步的主航向角和步长获得行人航迹。

上述技术方案：该方法除了具备本发明所述的行人主航向更新方法的有益效果外，还具备选择步态特征稳定的腰部和足部作为步长信息数据采集点，通过组合腰部三轴加速度计和足部三轴加速度计的输出信息获取行人的步长，提高组合导航的步长计算精度，可被应用于人员导航领域。本方法通过确保航向精度和步长计算精度，能够准确获得行人航迹。

在本发明的一种优选实施方式中，基于计步信息，组合所述腰部三轴加速度计和所述足部三轴加速度计的输出信息获取行人的步长，具体包括：获取第k步对应的腰部三轴加速度计输出数据区间和足部三轴加速度计输出数据区间；基于所述腰部三轴加速度计输出数据区间获取腰部步长；基于所述足部三轴加速度计输出数据区间获取足部步长；融合腰部步长和足部步长得到第k步的步长。

上述技术方案：通过融合腰部步长和足部步长得到第k步的步长，能够减小误差，提高步长计算精度。

在本发明的一种优选实施方式中，基于所述腰部三轴加速度计输出数据区间获取腰部步长，包括：从所述腰部三轴加速度计输出数据区间中提取出相邻两个腰部xy合加速度波峰所在的子区间作为腰部步长解算区间；通过如下公式求出第k步的腰部步长L_b,k：

其中，M表示预设的第一经验系数，R表示预设的第二经验系数；A_b,k,max表示腰部步长解算区间中腰部xy合加速度的最大值；A_b,k,min表示腰部步长解算区间中腰部xy合加速度的最小值，a_b,z,k,max表示腰部加速度计在第k步中最大的z轴加速度，a_b,z,k,min表示腰部加速度计在第k步中最小的z轴加速度。和/或基于所述足部三轴加速度计输出数据区间获取足部步长，包括：通过如下公式获得第k步的足部步长L_f,k：

其中，W表示预设的第三经验系数，D表示预设的第四经验系数；L_f,x,k表示第k步足部三轴加速度计输出的x轴加速度a_f,x,k在x轴方向上的积分位移；L_f,y,k表示第k步足部三轴加速度计输出的y轴加速度a_f,y,k在y轴方向上的积分位移；L_f,z,k表示第k步足部三轴加速度计输出的z轴加速度a_f,z,k在z轴方向上的积分位移。

上述技术方案：利用腰部xy合加速度的波峰与步态的对应关系，准确获取腰部步长。利用足部在x轴和y轴的加速度运算能够快速准确求得足部步长。

在本发明的一种优选实施方式中，融合腰部步长和足部步长得到第k步的步长L_k为：L_k＝q·L_b,k+p·L_f,k，其中，L_b,k表示腰部步长；L_f,k表示足部步长；q表示预设的第五经验系数，q∈[0,1]；p表示预设的第六经验系数，p∈[0,1]。

上述技术方案：融合方法便于计算。

为了实现本发明的上述目的，根据本发明的第三个方面，本发明提供了一种行人导航系统，包括设置于行人腰部的陀螺仪、腰部三轴加速度计和磁力计，以及处理器，所述处理器接收陀螺仪、腰部三轴加速度计和磁力计的输出信息并执行本发明所述行人主航向更新方法中的步骤更新行人行走中的主航向角。

上述技术方案：该系统根据人员行走的航向特征，选择航向稳定的腰部作为航向信息数据采集点，在腰部放置陀螺仪、磁力计与加速度计，通过多个传感器在同一个部位的测量信息作为主航向更新的数据来源，以此解算出每一步的航向角变化值，有效降低身体随机小幅摆动等行为特征对航向信息带来的影响，提高组合导航的测量精度和稳定性，有效提高了航向精度，进而为后续的行人航迹推算过程提供精准、可靠的航向信息。

在本发明的一种优选实施方式中，还包括设置于行人足部的足部三轴加速度计，所述处理器接收所述足部三轴加速度计输出信息并执行本发明所述的行人航迹获取方法获得行人航迹。

上述技术方案：该系统选择步态特征稳定的腰部和足部作为步长信息数据采集点，通过组合腰部三轴加速度计和足部三轴加速度计的输出信息获取行人的步长，提高组合导航的步长计算精度，可被应用于人员导航领域，通过确保航向精度和步长计算精度，能够准确获得行人航迹。

附图说明

图1是本发明一具体实施方式中行人主航向更新方法流程示意图；

图2是本发明提供的行人主航向更新方法在一种应用场景中的流程图；

图3是本发明一具体实施方式中获取行人步长的流程示意图。

具体实施方式

下面详细描述本发明的实施例，所述实施例的示例在附图中示出，其中自始至终相同或类似的标号表示相同或类似的元件或具有相同或类似功能的元件。下面通过参考附图描述的实施例是示例性的，仅用于解释本发明，而不能理解为对本发明的限制。

在本发明的描述中，需要理解的是，术语“纵向”、“横向”、“上”、“下”、“前”、“后”、“左”、“右”、“竖直”、“水平”、“顶”、“底”“内”、“外”等指示的方位或位置关系为基于附图所示的方位或位置关系，仅是为了便于描述本发明和简化描述，而不是指示或暗示所指的装置或元件必须具有特定的方位、以特定的方位构造和操作，因此不能理解为对本发明的限制。

在本发明的描述中，除非另有规定和限定，需要说明的是，术语“安装”、“相连”、“连接”应做广义理解，例如，可以是机械连接或电连接，也可以是两个元件内部的连通，可以是直接相连，也可以通过中间媒介间接相连，对于本领域的普通技术人员而言，可以根据具体情况理解上述术语的具体含义。

本发明为有效降低身体随机小幅摆动等行为特征对航向信息带来的影响，有效提高航向精度，提供了一种行人主航向更新方法，应用于在行人腰部同时设置陀螺仪、腰部三轴加速度计和磁力计的场景，在一种优选实施方式中，如图1所示，航向更新方法包括：

步骤S101，基于行走起点腰部三轴加速度计和磁力计的输出信息解算出行人在地理坐标系下的初始主航向角Yaw₀。设置计算步数阈值n，n为正整数，n优选但不限于为2，当步数k小于n时即为初始航向角计算区间。

初始主航向角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中获取。

在本实施方式中，本发明默认行人行走时是近似与地面平行，且xyz三轴方向与地理坐标系重合，那么最后的航向，就近似于是地理坐标系下的航向。

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

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

在步骤S103中，基于陀螺仪的输出信息获取地理坐标系下第k步陀螺仪航向角变化值，包括：在行人行走第k步后，利用旋转矢量方法和三角函数关系，分别基于第k步起始时刻和结束时刻的陀螺仪输出信息解算出行人在第k步起始时刻和结束时刻的陀螺仪航向角，将第k步的结束时刻和起始时刻的陀螺仪航向角的差值作为第k步陀螺仪航向角变化值。

优选的，利用旋转矢量方法和三角函数关系，分别基于第k步起始时刻和结束时刻的陀螺仪输出信息解算出行人在第k步起始时刻和结束时刻的陀螺仪航向角，具体过程如下：

处理陀螺仪原始数据，采用旋转矢量方法得出变换矩阵，结合三角函数关系式，解算出陀螺仪的俯仰角、横滚角、航向角数据。

载体的姿态角可以精准表示出载体坐标系(b)系和地理坐标系(n)系之间的方位关系，常用于描述载体的方位取向信息。针对不同的系统，姿态角有着不同的定义，一般用航向角(yaw)、横滚角(roll)、俯仰角(pitch)来进行描述，从原点向各轴的正向看去，顺时针方向为正。其中，yaw表示载体Z_b轴和地理北向之间的夹角；pitch表示载体坐标系Y_b轴和水平面之间的夹角，向上为正；roll表示载体坐标系X_b轴和水平面之间的夹角。

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

优选的，利用四元数法分别基于第k步的起始时刻和结束时刻腰部三轴加速度计和磁力计的输出信息解算出行人在第k步的起始时刻和结束时刻的磁力计航向角，具体包括：

对磁力计与加速度计原始数据进行解算，利用重力加速度与磁强度得出磁力计磁偏角与磁北向夹角，进而估算出初始航向角与磁力计航向角。

在初始时刻利用磁力计与加速度的数据得到初始航向角，利用磁力计采集行走前开机静止几秒钟的磁航向角度信息，来确定相对于地理坐标系的初始航向角Yaw₀。然后沿直线平走，利用磁力计与加速度的数据得到航向角Yaw_mag，利用陀螺仪的角度解算方法确定相对于载体坐标系的航向角Yaw_gry，求解公式如下所示：

其中e₁，e₂，e₃，e₄都表示四元数法求解人员导航姿态角的拟合参数，p₁、p₂、p₃表示姿态解算的线性系数。把Yaw_mag与Yaw_gry两个航向角结合起来，就可以得到行人运动的完整航向角Yaw信息。

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

在本实施方式中，方式一为：将第k步的主航向角更新为第k－1步的主航向角。方式二为：将第k步的主航向角更新为第k－1步的主航向角与第k步航向角变化值之和，融合第k步磁力计航向角变化值和第k步陀螺仪航向角变化值获得第k步航向角变化值。当k等于1时，第k－1步的主航向角为初始主航向角Yaw₀。

在本实施方式中，优选的，第k步航向角变化值为第k步磁力计航向角变化值和第k步陀螺仪航向角变化值的加权平均值，进一步优选的，第k步航向角变化值为第k步磁力计航向角变化值和第k步陀螺仪航向角变化值的平均值。

在本实施方式中，当n等于2并且k≥n时：

第k步到第k-n步的陀螺仪航向角变化值的平均值

为：

其中，ψ_gry,k表示第k步陀螺仪航向角值，ψ_gry,k-1表示第k－1步陀螺仪航向角值，ψ_gry,k-2表示第k－2步陀螺仪航向角值，Δψ_gry,k,k-1表示第k步到第k－1陀螺仪航向角变化值，Δψ_gry,k,k-2表示第k步到第k－2步陀螺仪航向角变化值。

第k步到第k-n步的磁力计航向角变化值的平均值

为：

其中，ψ_mag,k表示第k步磁力计航向角值，ψ_mag,k-1表示第k－1步磁力计航向角值，ψ_mag,k-2表示第k－2步磁力计航向角值，Δψ_mag,k,k-1表示第k步到第k－1陀螺仪航向角变化值，Δψ_mag,k,k-2表示第k步到第k－2步陀螺仪航向角变化值。

在本发明提供的行人主航向更新方法的一种应用场景中，具体流程图如图2所示，包括：步骤a，把陀螺仪、磁力计、加速度计等多种传感器放置在人员腰部，采集人员在不同运动状态下的原始数据。步骤b，处理陀螺仪原始数据，采用旋转矢量方法得出变换矩阵，结合三角函数关系式，解算出陀螺仪的俯仰角、横滚角、航向角数据。步骤c，对磁力计与加速度计原始数据进行解算，利用重力加速度与磁强度得出磁力计磁偏角与磁北向夹角，进而估算出初始航向角与磁力计航向角。步骤d，利用主航向更新方法对磁力计航向角与陀螺仪航向角进行偏差阈值判定，进而对航向误差进行修正，得到精准的航向角信息。步骤e，实验验证，分析航向角偏差，若验证通过，则航向解算结束；若验证不通过，则返回步骤a继续分析修改主航向更新方法。在步骤a中，分析陀螺仪、磁力计与加速度计误差来源，把陀螺仪、磁力计、加速度计等多种传感器放置在人员腰部，采集人员在不同运动状态下的原始数据。从器件制造的材料、各组成单元和系统之间，把握器件单元的质量标准，寻求整体系统的优化设计，整体方法流程图如图2所示。

在另一种应用场景中，设定10个主航向(0°方向、22.5°方向、45°方向、67.5°方向、90°方向、180°方向、270°方向、292.5°方向、315°方向、337.5°方向)与航向角变化阈值(r1、r2、r3、r4、r5、r6、r7、r8、r9、r10)对行人航向进行最优估计的主航向更新算法，优选的，为每个主航向设置一个航向角变化阈值。本应用场景提出的10个主航向中，每个主航向角设置一个航向角变化阈值，主航向间隔的角度Δ₁＝22.5°或者Δ₂＝90°。对行走过程中初始航向定义为0°方向，而其余9个方向则分别定义为22.5°方向、45°方向、67.5°方向、90°方向、180°方向、270°方向、292.5°方向、315°方向、337.5°方向。主航向范围设置：根据行人的设定直行晃动角度范围r1、r2、r3、r4、r5、r6、r7、r8、r9、r10和主航向航向，当前航向角与初始时刻航向角(即0°)的差值为：－r1到r1、22.5－r2到22.5+r2、45－r3到45+r3、67.5－r4到67.5+r4、90－r5到90+r5、180－r6到180+r6、270－r7到270+r7、292.5－r8到292.5+r8、315.5－r9到315.5+r9、337.5－r10到337.5+r10的角度范围内时，即认为处于主航向范围内。

本发明还公开了一种行人步长获取方法，应用于在行人腰部设置三轴加速度计，以及在行人足部设有足部三轴加速度计的场景，包括：从行走起点开始，以足部三轴加速度计的合速度过零一次作为一步进行计步；基于计步信息，组合腰部三轴加速度计和足部三轴加速度计的输出信息获取行人的步长。

本发明还公开了一种行人航迹获取方法，应用于在行人腰部设置陀螺仪、腰部三轴加速度计和磁力计，以及在行人足部设有足部三轴加速度计的场景，包括：实时采集陀螺仪、腰部三轴加速度计、磁力计和足部三轴加速度计的输出信息；从行走起点开始，以足部三轴加速度计的合速度过零一次作为一步进行计步；执行上述行人主航向更新方法中的步骤进行行人主航向角更新；如图3所示，基于计步信息，组合腰部三轴加速度计和足部三轴加速度计的输出信息获取行人的步长；基于行人行走中每步的主航向角和步长获得行人航迹。

在一种优选实施方式中，如图3所示，基于计步信息，组合腰部三轴加速度计和足部三轴加速度计的输出信息获取行人的步长，具体包括：获取第k步对应的腰部三轴加速度计输出数据区间和足部三轴加速度计输出数据区间；基于腰部三轴加速度计输出数据区间获取腰部步长；基于足部三轴加速度计输出数据区间获取足部步长；融合腰部步长和足部步长得到第k步的步长。

在一种优选实施方式中，基于腰部三轴加速度计输出数据区间获取腰部步长，包括：

从腰部三轴加速度计输出数据区间中提取出相邻两个腰部xy合加速度波峰所在的子区间作为腰部步长解算区间；

通过如下公式求出第k步的腰部步长L_b,k：

其中，M表示预设的第一经验系数，M的取值范围在0.3到0.9之间，R表示预设的第二经验系数，R的取值范围在0.2到0.3之间，两者均可预先设定，M和R为有量纲系数，量纲均为

m是米的简写，s是秒的简写；A_b,k,max表示腰部步长解算区间中腰部xy合加速度的最大值；A_b,k,min表示腰部步长解算区间中腰部xy合加速度的最小值。第k步对应的腰部步长解算区间中，第i个采集点对应的腰部xy合加速度A_b,k,i为：

a_b,x,k,i、a_b,y,k,i、a_b,z,k,i分别表示腰部三轴加速度计在第k步的第i个采集点输出的x轴、y轴、z轴加速度，a_b,z,k,max表示腰部加速度计在第k步中最大的z轴加速度，a_b,z,k,min表示腰部加速度计在第k步中最小的z轴加速度。

在一种优选实施方式中，基于足部三轴加速度计输出数据区间获取足部步长，包括：

通过如下公式获得第k步的足部步长L_f,k：

其中，W表示预设的第三经验系数，W的取值范围在0.2到1.0之间，D表示预设的第四经验系数，D的取值范围在0.1到0.2之间，W和D均为无量纲常数，具体数值可预先设定；L_f,x,k表示第k步足部三轴加速度计输出的x轴加速度a_f,x,k在x轴方向上的积分位移；L_f,y,k表示第k步足部三轴加速度计输出的y轴加速度a_f,y,k在y轴方向上的积分位移；L_f,z,k表示第k步足部三轴加速度计输出的z轴加速度a_f,z,k在z轴方向上的积分位移。L_f,x,k、L_f,y,k和L_f,z,k通过如下公式计算能获得：

ΔT表示表示一个步态区间的持续时间。

在一种优选实施方式中，融合腰部步长和足部步长得到第k步的步长L_k为：

L_k＝q·L_b,k+p·L_f,k，其中，L_b,k表示腰部步长；L_f,k表示足部步长；q表示预设的第五经验系数，q∈[0,1]；p表示预设的第六经验系数，p∈[0,1]，q和p均无量纲。q和p的数值可预先设定，也可以分别设置多个数值对应不同场景，选择足部步长算法与腰部步长算法来组合解算行人步长，增加步长的冗余信息，提高步长算法对具体测试场景的容错性，实现精准持久的人员定位。

本发明还公开了一种行人导航系统，包括设置于行人腰部的陀螺仪、腰部三轴加速度计和磁力计，以及处理器，处理器接收陀螺仪、腰部三轴加速度计和磁力计的输出信息并执行上述行人主航向更新方法中的步骤更新行人行走中的主航向角。

在一种优选实施方式中，还包括设置于行人足部的足部三轴加速度计，所述处理器接收所述足部三轴加速度计输出信息并执行上述行人航迹获取方法获得行人航迹。

在本说明书的描述中，参考术语“一个实施例”、“一些实施例”、“示例”、“具体示例”、或“一些示例”等的描述意指结合该实施例或示例描述的具体特征、结构、材料或者特点包含于本发明的至少一个实施例或示例中。在本说明书中，对上述术语的示意性表述不一定指的是相同的实施例或示例。而且，描述的具体特征、结构、材料或者特点可以在任何的一个或多个实施例或示例中以合适的方式结合。

尽管已经示出和描述了本发明的实施例，本领域的普通技术人员可以理解：在不脱离本发明的原理和宗旨的情况下可以对这些实施例进行多种变化、修改、替换和变型，本发明的范围由权利要求及其等同物限定。

Claims (10)

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之一所述行人主航向更新方法中的步骤进行行人主航向角更新；

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

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

6.如权利要求5所述的行人航迹获取方法，其特征在于，基于计步信息，组合所述腰部三轴加速度计和所述足部三轴加速度计的输出信息获取行人的步长，具体包括：

获取第k步对应的腰部三轴加速度计输出数据区间和足部三轴加速度计输出数据区间；

基于所述腰部三轴加速度计输出数据区间获取腰部步长；

基于所述足部三轴加速度计输出数据区间获取足部步长；

融合腰部步长和足部步长得到第k步的步长。

7.如权利要求6所述的行人航迹获取方法，其特征在于，基于所述腰部三轴加速度计输出数据区间获取腰部步长，包括：

从所述腰部三轴加速度计输出数据区间中提取出相邻两个腰部xy合加速度波峰所在的子区间作为腰部步长解算区间；

通过如下公式求出第k步的腰部步长L_b,k：

其中，M表示预设的第一经验系数，R表示预设的第二经验系数；A_b,k,max表示腰部步长解算区间中腰部xy合加速度的最大值；A_b,k,min表示腰部步长解算区间中腰部xy合加速度的最小值，a_b,z,k,max表示腰部加速度计在第k步中最大的z轴加速度，a_b,z,k,min表示腰部加速度计在第k步中最小的z轴加速度；

和/或基于所述足部三轴加速度计输出数据区间获取足部步长，包括：

通过如下公式获得第k步的足部步长L_f,k：

其中，W表示预设的第三经验系数，D表示预设的第四经验系数；L_f,x,k表示第k步足部三轴加速度计输出的x轴加速度a_f,x,k在x轴方向上的积分位移；L_f,y,k表示第k步足部三轴加速度计输出的y轴加速度a_f,y,k在y轴方向上的积分位移；L_f,z,k表示第k步足部三轴加速度计输出的z轴加速度a_f,z,k在z轴方向上的积分位移。

8.如权利要求6所述的行人航迹获取方法，其特征在于，融合腰部步长和足部步长得到第k步的步长L_k为：

L_k＝q·L_b,k+p·L_f,k，其中，L_b,k表示腰部步长；L_f,k表示足部步长；q表示预设的第五经验系数，q∈[0,1]；p表示预设的第六经验系数，p∈[0,1]。

9.一种行人导航系统，其特征在于，包括设置于行人腰部的陀螺仪、腰部三轴加速度计和磁力计，以及处理器，所述处理器接收陀螺仪、腰部三轴加速度计和磁力计的输出信息并执行权利要求1－4之一所述行人主航向更新方法中的步骤更新行人行走中的主航向角。

10.如权利要求9所述的行人导航系统，其特征在于，还包括设置于行人足部的足部三轴加速度计，所述处理器接收所述足部三轴加速度计输出信息并执行权利要求5－8之一所述的行人航迹获取方法获得行人航迹。

Images