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

权利要求书

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

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

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

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

2.根据权利要求1所述的一种三轴超声波辅助下的行人自主定位导航方法，其特征在于，所述步骤1采集行人足部前脚掌的三轴超声波和六轴姿态仪数据，并利用三轴超声波辅助六轴姿态仪判断行人前脚掌的零速区间，具体步骤为：

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

其中，为由右脚(右脚)超声波轴发出，k时刻左脚(右脚)超声波轴接收的测距距离，为k时刻左脚超声波轴测的离地面的距离；

步骤1.2：计算k时刻长度为L的滑动窗口中，i(i＝x,y,z)轴加速度(j＝k-L+1,k-L+2,…,k)与初始时刻k₀加速度之间斜率的平均值计算公式为：

其中，N为相邻两个零速率区间间隔的数据长度；

步骤1.4：对进行判断：当S＝1，且各轴的的绝对值都分别小于各轴的阈值时，令S＝2，反之则S＝0。

3.根据权利要求2所述的一种三轴超声波辅助下的行人自主定位导航方法，其特征在于，步骤1利用三轴超声波辅助六轴姿态仪判断行人前脚掌的零速区间中各坐标系具体轴向为：

左、右脚三轴陀螺仪的轴向，x_gyro轴向右，y_gyro轴向前，z_gyro轴向上，三轴正交，成右手坐标系x_gyroy_gyroz_gyro；

左、右脚三轴加速度计的轴向，x_acc轴向右，y_acc轴向前，z_acc轴向上，三轴正交，成右手坐标系x_accy_accz_acc，与x_gyroy_gyroz_gyro坐标系重合；

左、右脚三轴超声波模块轴向，轴向右(左)前方，轴向右(左)后方，轴向下，三轴正交，成右手坐标系

4.根据权利要求1所述的一种三轴超声波辅助下的行人自主定位导航方法，其特征在于，所述步骤2在零速区间利用三轴加速度计和三轴陀螺仪数据分别更新得到两组导航姿态四元数，并利用自适应互补滤波融合得到融合导航姿态四元数，具体步骤为：

步骤2.1：利用四阶毕卡法对导航姿态四元数进行更新，得到k时刻左脚(右脚)的先验导航姿态四元数

步骤2.2：利用解算由n系(东北天坐标系)到b系(右前上坐标系)的坐标转换矩阵然后将零速区间n系加速度理论值a₀通过坐标转换矩阵进行坐标转换得到b系的加速度理论值最后将k时刻加速度计的b系测量值和代入扩展卡尔曼滤波测量更新得到左脚(右脚)第一组后验导航姿态四元数

步骤2.3：利用零速区间b系角速率理论值ω₀更新四元数得到左脚零速区间理论导航姿态四元数并将其作为测量值代入一般卡尔曼滤波的测量更新得到左脚(右脚)第二组后验导航姿态四元数

步骤2.4：根据左脚(右脚)加速度计实际测量值和理论值之间的关系计算权重w₁，公式为：

其中，μ_L(R)1为权重w_L(R)1的调整系数；

步骤2.5：权重w_L(R)2的计算公式为：

w_L(R)2＝1-w_L(R)1

步骤2.6：根据权重计算融合后的左脚(右脚)融合后验导航姿态四元数公式为：