[发明专利]一种三轴超声波辅助下的行人自主定位导航方法

说明书

本发明请求保护一种三轴超声波辅助下的行人自主定位导航方法：该方法首先利用三轴超声波模块来辅助六轴姿态仪(三轴陀螺仪和三轴加速度计)的滑动均值斜坡法判定行人步行前脚掌的零速率区间，然后利用加速度计和陀螺仪的数据分别进行扩展卡尔曼滤波和卡尔曼滤波解算出两组姿态角，并使用自适应互补滤波融合。最后利用超声波测距传感器测量的足间距离作为观测值进行扩展卡尔曼滤波，校准导航姿态、速度和位置，进而实现精准的人员的定位导航。本发明能够在行人步行的动态过程中，准确判断前脚掌的零速率区间，并能自适应调节导航姿态的融合权重，校准和更新行人足部的导航姿态、速度和位置信息。

技术领域

本发明属于惯性导航定位技术领域，涉及一种三轴超声波辅助下的行人自主定位导航方法。

背景技术

在人员定位导航中的关键问题是获取精确地人员位置信息和姿态信息，以便准确的下达导航指令，实现高精度的人员定位导航功能。目前的人员定位导航分为卫星定位导航和自主导航，前者覆盖面积广，但对于环境封闭的环境无法使用，后者则是完全自主的无源导航，不用担心使用环境是否有遮挡，但精度会随着时间增加变小。这是因为惯性系统陀螺仪器件的累计误差会随着时间增加变大，导致航向角产生偏移。

为了解决上述问题，零速区间校准方法被提出，利用人员行走时刻，足部短暂的零速区间，对惯性器件的累计误差进行校准，可一定程度上抑制航向的偏移，但航向偏移依然会随着时间增加而变大。

进一步的，双足零速区间的约束方法被提出(CN112362057A)，利用人员行走过程中双足之间的理论约束条件，对导航姿态、速度和位置进行约束校准，但是校准结果受模型精度影响，约束模型精度越符合当前人员的双足约束关系，校准结果的精度就越高，反之则越低，且随着时间增加，校准结果依然会发生偏移。是一种可抑制航向偏移，但定位导航精度依旧受陀螺仪偏移和模型精度影响的方法。

以上方法中，除了惯性器件偏移导致人员定位导航产生误差以外，还存在零速区间的判断准确度问题。当零速区间判断失误时，会将非零速区间的特征数据带入校准，反而会给人员定位导航时引入误差。传统的零速区间判断方法，利用加速度计和陀螺仪的静止时最大模值和方差是否小于静止时最小阈值的方法综合判断零速区间，但随着运行时间的增加陀螺仪会产生零偏，原本设定的静止时最大模值的最小阈值可能将小于偏移后陀螺仪静止时的最大模值，导致零速区间判断出错。但单一使用加速度计判断零速区间也存在错误判断的可能，所以需要一种不受陀螺仪偏移影响，稳定且准确的零速区间判断方法。

发明内容

本发明旨在解决以上传统技术的问题。本文提出了一种三轴超声波辅助下的行人自主定位导航方法。该方法一方面使用三轴超声波测距辅助六轴姿态仪中的加速度计判断零速区间，解决传统方法可能出现零速区间误判的问题。另一方面，通过零速区间使用超声波测距传感器测量的足间距作为观测量校准纯惯导解算导致姿态、速度和位置累计误差变大的问题，从而使得人员定位导航得以实现。本发明的技术方案如下：

一种三轴超声波辅助下的行人自主定位导航方法，其包括以下步骤：

步骤1：采集行人足部前脚掌的三轴超声波和六轴姿态仪数据，并利用三轴超声波辅助六轴姿态仪判断行人前脚掌的零速区间；

步骤2：在零速区间利用三轴加速度计和三轴陀螺仪数据分别更新得到两组导航姿态四元数，并利用自适应互补滤波融合得到融合导航姿态四元数；

步骤3：在零速区间利用三轴超声波数据对导航位置和融合导航姿态四元数进行修正，从而实现行人的精确定位和导航。

进一步的，所述步骤1采集行人足部前脚掌的三轴超声波和六轴姿态仪数据，并利用三轴超声波辅助六轴姿态仪判断行人前脚掌的零速区间，具体步骤为：

步骤1.1：对零速区间状态标志符S和长度为L的滑动窗口中三轴超声波模块的测距信息进行计算并判断：当S＝0，且小于等于阈值ε_ultr时，令S＝1，反之则S＝0；