CN115655269A - 一种三轴超声波辅助下的行人自主定位导航方法 - Google Patents

一种三轴超声波辅助下的行人自主定位导航方法 Download PDF

Info

Publication number
CN115655269A
CN115655269A CN202211386612.XA CN202211386612A CN115655269A CN 115655269 A CN115655269 A CN 115655269A CN 202211386612 A CN202211386612 A CN 202211386612A CN 115655269 A CN115655269 A CN 115655269A
Authority
CN
China
Prior art keywords
foot
axis
navigation
representing
zero
Prior art date
Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
Pending
Application number
CN202211386612.XA
Other languages
English (en)
Inventor
路永乐
惠嘉威
修蔚然
杨杰
孙旗
邹新海
陈燕苹
刘宇
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Chongqing University of Post and Telecommunications
Original Assignee
Chongqing University of Post and Telecommunications
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by Chongqing University of Post and Telecommunications filed Critical Chongqing University of Post and Telecommunications
Priority to CN202211386612.XA priority Critical patent/CN115655269A/zh
Publication of CN115655269A publication Critical patent/CN115655269A/zh
Pending legal-status Critical Current

Links

Images

Landscapes

  • Navigation (AREA)

Abstract

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

Description

一种三轴超声波辅助下的行人自主定位导航方法
技术领域
本发明属于惯性导航定位技术领域，涉及一种三轴超声波辅助下的行人自主定位导航方法。
背景技术
在人员定位导航中的关键问题是获取精确地人员位置信息和姿态信息，以便准确的下达导航指令，实现高精度的人员定位导航功能。目前的人员定位导航分为卫星定位导航和自主导航，前者覆盖面积广，但对于环境封闭的环境无法使用，后者则是完全自主的无源导航，不用担心使用环境是否有遮挡，但精度会随着时间增加变小。这是因为惯性系统陀螺仪器件的累计误差会随着时间增加变大，导致航向角产生偏移。
为了解决上述问题，零速区间校准方法被提出，利用人员行走时刻，足部短暂的零速区间，对惯性器件的累计误差进行校准，可一定程度上抑制航向的偏移，但航向偏移依然会随着时间增加而变大。
进一步的，双足零速区间的约束方法被提出(CN112362057A)，利用人员行走过程中双足之间的理论约束条件，对导航姿态、速度和位置进行约束校准，但是校准结果受模型精度影响，约束模型精度越符合当前人员的双足约束关系，校准结果的精度就越高，反之则越低，且随着时间增加，校准结果依然会发生偏移。是一种可抑制航向偏移，但定位导航精度依旧受陀螺仪偏移和模型精度影响的方法。
以上方法中，除了惯性器件偏移导致人员定位导航产生误差以外，还存在零速区间的判断准确度问题。当零速区间判断失误时，会将非零速区间的特征数据带入校准，反而会给人员定位导航时引入误差。传统的零速区间判断方法，利用加速度计和陀螺仪的静止时最大模值和方差是否小于静止时最小阈值的方法综合判断零速区间，但随着运行时间的增加陀螺仪会产生零偏，原本设定的静止时最大模值的最小阈值可能将小于偏移后陀螺仪静止时的最大模值，导致零速区间判断出错。但单一使用加速度计判断零速区间也存在错误判断的可能，所以需要一种不受陀螺仪偏移影响，稳定且准确的零速区间判断方法。
发明内容
本发明旨在解决以上传统技术的问题。本文提出了一种三轴超声波辅助下的行人自主定位导航方法。该方法一方面使用三轴超声波测距辅助六轴姿态仪中的加速度计判断零速区间，解决传统方法可能出现零速区间误判的问题。另一方面，通过零速区间使用超声波测距传感器测量的足间距作为观测量校准纯惯导解算导致姿态、速度和位置累计误差变大的问题，从而使得人员定位导航得以实现。本发明的技术方案如下：
一种三轴超声波辅助下的行人自主定位导航方法，其包括以下步骤：
步骤1：采集行人足部前脚掌的三轴超声波和六轴姿态仪数据，并利用三轴超声波辅助六轴姿态仪判断行人前脚掌的零速区间；
步骤2：在零速区间利用三轴加速度计和三轴陀螺仪数据分别更新得到两组导航姿态四元数，并利用自适应互补滤波融合得到融合导航姿态四元数；
步骤3：在零速区间利用三轴超声波数据对导航位置和融合导航姿态四元数进行修正，从而实现行人的精确定位和导航。
进一步的，所述步骤1采集行人足部前脚掌的三轴超声波和六轴姿态仪数据，并利用三轴超声波辅助六轴姿态仪判断行人前脚掌的零速区间，具体步骤为：
步骤1.1：对零速区间状态标志符S和长度为L的滑动窗口中三轴超声波模块的测距信息进行计算并判断：当S＝0，且
Figure BDA0003930135110000021
Figure BDA0003930135110000022
小于等于阈值εultr时，令S＝1，反之则S＝0；
其中，
Figure BDA0003930135110000023
为由右脚(右脚)超声波
Figure BDA0003930135110000024
轴发出，k时刻左脚(右脚)超声波
Figure BDA0003930135110000031
轴接收的测距距离，
Figure BDA0003930135110000032
为k时刻左脚超声波
Figure BDA0003930135110000033
轴测的离地面的距离；
步骤1.2：计算k时刻长度为L的滑动窗口中，i(i＝x,y,z)轴加速度
Figure BDA0003930135110000034
(j＝k-L+1,k-L+2,…,k)与初始时刻k0加速度
Figure BDA0003930135110000035
之间斜率的平均值
Figure BDA0003930135110000036
计算公式为：
Figure BDA0003930135110000037
其中，N为相邻两个零速率区间间隔的数据长度；
步骤1.4：对
Figure BDA0003930135110000038
进行判断：当S＝1，且各轴的
Figure BDA0003930135110000039
的绝对值都分别小于各轴的阈值
Figure BDA00039301351100000310
时，令S＝2，反之则S＝0。
进一步的，步骤1利用三轴超声波辅助六轴姿态仪判断行人前脚掌的零速区间中各坐标系具体轴向为：
左、右脚三轴陀螺仪的轴向，xgyro轴向右，ygyro轴向前，zgyro轴向上，三轴正交，成右手坐标系xgyroygyrozgyro；
左、右脚三轴加速度计的轴向，xacc轴向右，yacc轴向前，zacc轴向上，三轴正交，成右手坐标系xaccyacczacc，与xgyroygyrozgyro坐标系重合；
左、右脚三轴超声波模块轴向，
Figure BDA00039301351100000311
轴向右(左)前方，
Figure BDA00039301351100000312
轴向右(左)后方，
Figure BDA00039301351100000313
轴向下，三轴正交，成右手坐标系
Figure BDA00039301351100000314
进一步的，所述步骤2在零速区间利用三轴加速度计和三轴陀螺仪数据分别更新得到两组导航姿态四元数，并利用自适应互补滤波融合得到融合导航姿态四元数，具体步骤为：
步骤2.1：利用四阶毕卡法对导航姿态四元数进行更新，得到k时刻左脚(右脚)的先验导航姿态四元数
Figure BDA00039301351100000315
步骤2.2：利用
Figure BDA00039301351100000316
解算由n系(东北天坐标系)到b系(右前上坐标系)的坐标转换矩阵
Figure BDA00039301351100000317
然后将零速区间n系加速度理论值a0通过坐标转换矩阵
Figure BDA0003930135110000041
进行坐标转换得到b系的加速度理论值
Figure BDA0003930135110000042
最后将k时刻加速度计的b系测量值
Figure BDA0003930135110000043
和
Figure BDA0003930135110000044
代入扩展卡尔曼滤波测量更新
Figure BDA0003930135110000045
得到左脚(右脚)第一组后验导航姿态四元数
Figure BDA0003930135110000046
步骤2.3：利用零速区间b系角速率理论值ω0更新四元数得到左脚零速区间理论导航姿态四元数
Figure BDA0003930135110000047
并将其作为测量值代入一般卡尔曼滤波的测量更新
Figure BDA0003930135110000048
得到左脚(右脚)第二组后验导航姿态四元数
Figure BDA0003930135110000049
步骤2.4：根据左脚(右脚)加速度计实际测量值
Figure BDA00039301351100000410
和理论值
Figure BDA00039301351100000411
之间的关系计算
Figure BDA00039301351100000412
权重w1，公式为：
Figure BDA00039301351100000413
其中，μL(R)1为权重wL(R)1的调整系数；
步骤2.5：
Figure BDA00039301351100000414
权重wL(R)2的计算公式为：
wL(R)2＝1-wL(R)1
步骤2.6：根据权重计算融合后的左脚(右脚)融合后验导航姿态四元数
Figure BDA00039301351100000415
公式为：
Figure BDA00039301351100000416
进一步的，所述步骤2.2的扩展卡尔曼滤波测量更新
Figure BDA00039301351100000417
的方程具体为：
Figure BDA00039301351100000418
Figure BDA00039301351100000419
Figure BDA00039301351100000420
Figure BDA00039301351100000421
Figure BDA0003930135110000051
其中，
Figure BDA0003930135110000052
表示左脚(右脚)第一组测量更新矩阵，
Figure BDA0003930135110000053
表示左脚(右脚)第一组卡尔曼增益系数，
Figure BDA0003930135110000054
表示左脚(右脚)第一组先验估计协方差，
Figure BDA0003930135110000055
表示左脚(右脚)第一组测量误差矩阵，
Figure BDA0003930135110000056
表示左脚(右脚)b系理论加速度测量值，
Figure BDA0003930135110000057
表示左脚(右脚)b系实际加速度测量值，
Figure BDA0003930135110000058
表示左脚(右脚)第一组后验估计协方差矩阵。
进一步的，所述步骤2.3的卡尔曼滤波测量更新
Figure BDA0003930135110000059
的方程具体为：
Figure BDA00039301351100000510
Figure BDA00039301351100000511
Figure BDA00039301351100000512
Figure BDA00039301351100000513
其中，
Figure BDA00039301351100000514
表示左脚(右脚)第二组测量更新矩阵，
Figure BDA00039301351100000515
表示左脚(右脚)第二组卡尔曼增益系数，
Figure BDA00039301351100000516
表示左脚(右脚)第二组先验估计协方差，
Figure BDA00039301351100000517
表示左脚(右脚)第二组测量误差矩阵，
Figure BDA00039301351100000518
表示左脚(右脚)理论导航姿态四元数，
Figure BDA00039301351100000519
表示左脚(右脚)第二组后验估计协方差矩阵。
进一步的，所述步骤3在零速区间利用三轴超声波数据对导航位置和融合导航姿态四元数进行修正，从而实现行人的精确定位和导航，具体步骤为：
步骤3.1：以左脚起始点
Figure BDA00039301351100000520
为原点建立轴向与n系坐标系重合的方位坐标系，确定初始时刻右脚相对于方位坐标系原点的坐标点
Figure BDA00039301351100000521
在两个相邻零速率区间之间的运动过程中只进行状态更新，不进行测量更新，因此
Figure BDA00039301351100000522
由
Figure BDA00039301351100000523
得从b系坐标系到n系坐标系的坐标转换矩阵
Figure BDA00039301351100000524
然后将k-1时刻左脚(右脚)加速度计的b系测量值
Figure BDA00039301351100000525
进行坐标转换，得到将k-1时刻左脚(右脚)加速度计的n系各轴的分量
Figure BDA00039301351100000526
在减去重力加速度g后，对x轴、y轴和z轴的加速度一重积分得到k时刻左脚(右脚)x轴向、y轴向和z轴向的移动速度
Figure BDA0003930135110000061
和
Figure BDA0003930135110000062
双重积分得到k时刻左脚(右脚)x轴向、y轴向和z轴向的移动距离
Figure BDA0003930135110000063
和
Figure BDA0003930135110000064
得到移动后左脚的坐标
Figure BDA0003930135110000065
右脚的坐标
Figure BDA0003930135110000066
利用左、右脚的坐标点就可以求得足间距离
Figure BDA0003930135110000067
具体为：
Figure BDA0003930135110000068
Figure BDA0003930135110000069
Figure BDA00039301351100000610
Figure BDA00039301351100000611
Figure BDA00039301351100000612
Figure BDA00039301351100000613
Figure BDA00039301351100000614
Figure BDA00039301351100000615
Figure BDA00039301351100000616
Figure BDA00039301351100000617
Figure BDA00039301351100000618
Figure BDA00039301351100000619
Figure BDA00039301351100000620
Figure BDA00039301351100000621
其中，
Figure BDA00039301351100000622
表示k-1时刻左脚(右脚)在n系坐标系的加速度，
Figure BDA00039301351100000623
Figure BDA00039301351100000624
分别表示k-1时刻左脚(右脚)在n系坐标系下x、y、z轴向的位移，
Figure BDA00039301351100000625
Figure BDA00039301351100000626
分别表示k时刻左脚(右脚)在n系坐标系下x、y、z轴向的坐标,
Figure BDA00039301351100000627
表示k时刻由惯导计算的左脚(右脚)与右脚(左脚)之间的足间距离。
步骤3.2：根据步骤2得到的左脚(右脚)融合后验导航姿态四元数
Figure BDA0003930135110000071
和步骤3.1中的左脚和右脚的速度和位置坐标构建状态空间向量Xk，具体为：
Figure BDA0003930135110000072
步骤3.3：对状态空间向量Xk进行状态更新，具体为：
Figure BDA0003930135110000073
Figure BDA0003930135110000074
Figure BDA0003930135110000075
Fk＝I20×20
Figure BDA00039301351100000712
Figure BDA0003930135110000076
其中，Uk-1表示变量向量，Bk-1和Gk-1表示变量向量与状态向量的关系矩阵，Fk表示k时刻的状态转换矩阵，Om×n表示m×n的0矩阵，
Figure BDA0003930135110000077
表示k时刻的先验估计协方差，
Figure BDA0003930135110000078
表示k-1时刻的后验估计协方差矩阵，Qk表示k时刻的状态更新误差矩阵。
步骤3.4：对状态空间向量Xk进行量测更新，具体为：
Figure BDA0003930135110000079
Figure BDA00039301351100000710
Figure BDA00039301351100000711
Figure BDA0003930135110000081
其中,Hk表示k时刻的测量更新矩阵，Kk表示k时刻的卡尔曼增益系数，Rk表示k时刻的测量误差矩阵，
Figure BDA0003930135110000082
表示由超声波传感器
Figure BDA0003930135110000083
轴发送超声波到
Figure BDA0003930135110000084
轴接收计算的足间距离，
Figure BDA0003930135110000085
表示k时刻的后验估计协方差矩阵。
步骤3.4：获得校准后左脚和右脚的导航姿态四元数和位置坐标：
Figure BDA0003930135110000086
Figure BDA0003930135110000087
Figure BDA0003930135110000088
Figure BDA0003930135110000089
Figure BDA00039301351100000810
Figure BDA00039301351100000811
步骤3.5：重复步骤1、步骤2.1和步骤3.3的状态更新，直至S＝2，姿态仪处于零速区间。此时重复步骤1、步骤2、步骤3.1、步骤3.2、步骤3.3和步骤3.4，直至S≠2，姿态仪处于非零速区间。
本发明的优点及有益效果如下：
(1)本发明在步骤1中提出了一种三轴超声波辅助姿态仪滑动均值斜坡法来判断零速区间，利用三轴超声波测距传感器和三轴加速度计无时间累计误差的优点，综合判断零速区间的数据是否满足零速区间特征，对于满足特征要求的数据进行下一步计算。与传统方法利用三轴陀螺仪和三轴加速度计综合判断的方法相比，本方法的优点在于，预设的阈值条件不会随传感器运行时间增加而出现失效的问题，进而实现更精准更稳定的锁定零速区间。
(2)在步骤2中，当判断处于零速区间时，利用自适应互补滤波方法，将陀螺仪更新的姿态四元数和加速度计更新的姿态四元数相融合，可以利用陀螺仪的高频信息补偿加速度计的高频误差，利用加速度计的低频信息补偿陀螺仪的低频误差，从而获取更高精度的姿态四元数。
(3)在步骤3中利用扩展卡尔曼滤波的方法，把惯性计算的足间距离作为理论测量值，把超声波测距传感器测量的足间距离作为观测量，进行测量更新，校准状态空间向量的导航姿态、速度和位置。与传统利用约束模型提供观测值的方法相比，可以避免模型误差给校准结果引入误差的可能，且超声波测距数据没有时间累计误差，是一个上时间可靠的信息源，使得本方法的人员定位导航拥有更高的精度和更高的稳定性。
附图说明
图1是本发明提供优选实施例本发明的方案流程图；
图2是本发明实施例中的三轴超声波辅助下的行人自主定位导航示意图。
具体实施方式
下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、详细地描述。所描述的实施例仅仅是本发明的一部分实施例。
本发明解决上述技术问题的技术方案是：
如图1所示，为本发明的一种三轴超声波辅助下的行人自主定位导航算法的流程示意图。一种三轴超声波辅助下的行人自主定位导航算法，具体包括以下步骤：
步骤1：采集行人足部前脚掌的三轴超声波和六轴姿态仪数据，并利用三轴超声波辅助六轴姿态仪判断行人前脚掌的零速区间；
步骤2：在零速区间利用三轴加速度计和三轴陀螺仪数据分别更新得到两组导航姿态四元数，并利用自适应互补滤波融合得到融合导航姿态四元数；
步骤3：在零速区间利用三轴超声波数据对导航位置和融合导航姿态四元数进行修正，从而实现行人的精确定位和导航。
进一步的，所述步骤1采集行人足部前脚掌的三轴超声波和六轴姿态仪数据，并利用三轴超声波辅助六轴姿态仪判断行人前脚掌的零速区间，具体步骤为：
步骤1.1：对零速区间状态标志符S和长度为L的滑动窗口中三轴超声波模块的测距信息进行计算并判断：当S＝0，且
Figure BDA0003930135110000101
Figure BDA0003930135110000102
小于等于阈值εultr时，令S＝1，反之则S＝0；
其中，
Figure BDA0003930135110000103
为由右脚(右脚)超声波
Figure BDA0003930135110000104
轴发出，k时刻左脚(右脚)超声波
Figure BDA0003930135110000105
轴接收的测距距离，
Figure BDA0003930135110000106
为k时刻左脚超声波
Figure BDA0003930135110000107
轴测的离地面的距离；
步骤1.2：计算k时刻长度为L的滑动窗口中，i(i＝x,y,z)轴加速度
Figure BDA0003930135110000108
(j＝k-L+1,k-L+2,…,k)与初始时刻k0加速度
Figure BDA0003930135110000109
之间斜率的平均值
Figure BDA00039301351100001010
计算公式为：
Figure BDA00039301351100001011
其中，N为相邻两个零速率区间间隔的数据长度；
步骤1.4：对
Figure BDA00039301351100001012
进行判断：当S＝1，且各轴的
Figure BDA00039301351100001013
的绝对值都分别小于各轴的阈值
Figure BDA00039301351100001014
时，令S＝2，反之则S＝0。
进一步的，步骤1利用三轴超声波辅助六轴姿态仪判断行人前脚掌的零速区间中各坐标系具体轴向为：
左、右脚三轴陀螺仪的轴向，xgyro轴向右，ygyro轴向前，zgyro轴向上，三轴正交，成右手坐标系xgyroygyrozgyro；
左、右脚三轴加速度计的轴向，xacc轴向右，yacc轴向前，zacc轴向上，三轴正交，成右手坐标系xaccyacczacc，与xgyroygyrozgyro坐标系重合；
左、右脚三轴超声波模块轴向，
Figure BDA00039301351100001015
轴向右(左)前方，
Figure BDA00039301351100001016
轴向右(左)后方，
Figure BDA00039301351100001017
轴向下，三轴正交，成右手坐标系
Figure BDA00039301351100001018
进一步的，所述步骤2在零速区间利用三轴加速度计和三轴陀螺仪数据分别更新得到两组导航姿态四元数，并利用自适应互补滤波融合得到融合导航姿态四元数，具体步骤为：
步骤2.1：利用四阶毕卡法对导航姿态四元数进行更新，得到k时刻左脚(右脚)的先验导航姿态四元数
Figure BDA0003930135110000111
步骤2.2：利用
Figure BDA0003930135110000112
解算由n系(东北天坐标系)到b系(右前上坐标系)的坐标转换矩阵
Figure BDA0003930135110000113
然后将零速区间n系加速度理论值a0通过坐标转换矩阵
Figure BDA0003930135110000114
进行坐标转换得到b系的加速度理论值
Figure BDA0003930135110000115
最后将k时刻加速度计的b系测量值
Figure BDA0003930135110000116
和
Figure BDA0003930135110000117
代入扩展卡尔曼滤波测量更新
Figure BDA0003930135110000118
得到左脚(右脚)第一组后验导航姿态四元数
Figure BDA0003930135110000119
步骤2.3：利用零速区间b系角速率理论值ω0更新四元数得到左脚零速区间理论导航姿态四元数
Figure BDA00039301351100001110
并将其作为测量值代入一般卡尔曼滤波的测量更新
Figure BDA00039301351100001111
得到左脚(右脚)第二组后验导航姿态四元数
Figure BDA00039301351100001112
步骤2.4：根据左脚(右脚)加速度计实际测量值
Figure BDA00039301351100001113
和理论值
Figure BDA00039301351100001114
之间的关系计算
Figure BDA00039301351100001115
权重w1，公式为：
Figure BDA00039301351100001116
其中，μL(R)1为权重wL(R)1的调整系数；
步骤2.5：
Figure BDA00039301351100001117
权重wL(R)2的计算公式为：
wL(R)2＝1-wL(R)1
步骤2.6：根据权重计算融合后的左脚(右脚)融合后验导航姿态四元数
Figure BDA00039301351100001118
公式为：
Figure BDA00039301351100001119
进一步的，所述步骤2.2的扩展卡尔曼滤波测量更新
Figure BDA00039301351100001120
的方程具体为：
Figure BDA0003930135110000121
Figure BDA0003930135110000122
Figure BDA0003930135110000123
Figure BDA0003930135110000124
Figure BDA0003930135110000125
其中，
Figure BDA0003930135110000126
表示左脚(右脚)第一组测量更新矩阵，
Figure BDA0003930135110000127
表示左脚(右脚)第一组卡尔曼增益系数，
Figure BDA0003930135110000128
表示左脚(右脚)第一组先验估计协方差，
Figure BDA0003930135110000129
表示左脚(右脚)第一组测量误差矩阵，
Figure BDA00039301351100001210
表示左脚(右脚)b系理论加速度测量值，
Figure BDA00039301351100001211
表示左脚(右脚)b系实际加速度测量值，
Figure BDA00039301351100001212
表示左脚(右脚)第一组后验估计协方差矩阵。
进一步的，所述步骤2.3的卡尔曼滤波测量更新
Figure BDA00039301351100001213
的方程具体为：
Figure BDA00039301351100001214
Figure BDA00039301351100001215
Figure BDA00039301351100001216
Figure BDA00039301351100001217
其中，
Figure BDA00039301351100001218
表示左脚(右脚)第二组测量更新矩阵，
Figure BDA00039301351100001219
表示左脚(右脚)第二组卡尔曼增益系数，
Figure BDA00039301351100001220
表示左脚(右脚)第二组先验估计协方差，
Figure BDA00039301351100001221
表示左脚(右脚)第二组测量误差矩阵，
Figure BDA00039301351100001222
表示左脚(右脚)理论导航姿态四元数，
Figure BDA00039301351100001223
表示左脚(右脚)第二组后验估计协方差矩阵。
进一步的，所述步骤3在零速区间利用三轴超声波数据对导航位置和融合导航姿态四元数进行修正，从而实现行人的精确定位和导航，具体步骤为：
步骤3.1：以左脚起始点
Figure BDA0003930135110000131
为原点建立轴向与n系坐标系重合的方位坐标系，确定初始时刻右脚相对于方位坐标系原点的坐标点
Figure BDA0003930135110000132
在两个相邻零速率区间之间的运动过程中只进行状态更新，不进行测量更新，因此
Figure BDA0003930135110000133
由
Figure BDA0003930135110000134
得从b系坐标系到n系坐标系的坐标转换矩阵
Figure BDA0003930135110000135
然后将k-1时刻左脚(右脚)加速度计的b系测量值
Figure BDA0003930135110000136
进行坐标转换，得到将k-1时刻左脚(右脚)加速度计的n系各轴的分量
Figure BDA0003930135110000137
在减去重力加速度g后，对x轴、y轴和z轴的加速度一重积分得到k时刻左脚(右脚)x轴向、y轴向和z轴向的移动速度
Figure BDA0003930135110000138
和
Figure BDA0003930135110000139
双重积分得到k时刻左脚(右脚)x轴向、y轴向和z轴向的移动距离
Figure BDA00039301351100001310
和
Figure BDA00039301351100001311
得到移动后左脚的坐标
Figure BDA00039301351100001312
右脚的坐标
Figure BDA00039301351100001313
利用左、右脚的坐标点就可以求得足间距离
Figure BDA00039301351100001314
具体为：
Figure BDA00039301351100001315
Figure BDA00039301351100001316
Figure BDA00039301351100001317
Figure BDA00039301351100001318
Figure BDA00039301351100001319
Figure BDA00039301351100001320
Figure BDA00039301351100001321
Figure BDA00039301351100001322
Figure BDA00039301351100001323
Figure BDA00039301351100001324
Figure BDA00039301351100001325
Figure BDA00039301351100001326
Figure BDA0003930135110000141
Figure BDA0003930135110000142
其中，
Figure BDA0003930135110000143
表示k-1时刻左脚(右脚)在n系坐标系的加速度，
Figure BDA0003930135110000144
Figure BDA0003930135110000145
分别表示k-1时刻左脚(右脚)在n系坐标系下x、y、z轴向的位移，
Figure BDA0003930135110000146
Figure BDA0003930135110000147
分别表示k时刻左脚(右脚)在n系坐标系下x、y、z轴向的坐标,
Figure BDA0003930135110000148
表示k时刻由惯导计算的左脚(右脚)与右脚(左脚)之间的足间距离。
步骤3.2：根据步骤2得到的左脚(右脚)融合后验导航姿态四元数
Figure BDA0003930135110000149
和步骤3.1中的左脚和右脚的速度和位置坐标构建状态空间向量Xk，具体为：
Figure BDA00039301351100001410
步骤3.3：对状态空间向量Xk进行状态更新，具体为：
Figure BDA00039301351100001411
Figure BDA00039301351100001412
Figure BDA00039301351100001413
Fk＝I20×20
Figure BDA00039301351100001417
Figure BDA00039301351100001414
其中，Uk-1表示变量向量，Bk-1和Gk-1表示变量向量与状态向量的关系矩阵，Fk表示k时刻的状态转换矩阵，Om×n表示m×n的0矩阵，
Figure BDA00039301351100001415
表示k时刻的先验估计协方差，
Figure BDA00039301351100001416
表示k-1时刻的后验估计协方差矩阵，Qk表示k时刻的状态更新误差矩阵。
步骤3.4：对状态空间向量Xk进行量测更新，具体为：
Figure BDA0003930135110000151
Figure BDA0003930135110000152
Figure BDA0003930135110000153
Figure BDA0003930135110000154
其中,Hk表示k时刻的测量更新矩阵，Kk表示k时刻的卡尔曼增益系数，Rk表示k时刻的测量误差矩阵，
Figure BDA0003930135110000155
表示由超声波传感器
Figure BDA0003930135110000156
轴发送超声波到
Figure BDA0003930135110000157
轴接收计算的足间距离，
Figure BDA0003930135110000158
表示k时刻的后验估计协方差矩阵。
步骤3.4：获得校准后左脚和右脚的导航姿态四元数和位置坐标：
Figure BDA0003930135110000159
Figure BDA00039301351100001510
Figure BDA00039301351100001511
Figure BDA00039301351100001512
Figure BDA00039301351100001513
Figure BDA00039301351100001514
步骤3.5：重复步骤1、步骤2.1和步骤3.3的状态更新，直至S＝2，姿态仪处于零速区间。此时重复步骤1、步骤2、步骤3.1、步骤3.2、步骤3.3和步骤3.4，直至S≠2，姿态仪处于非零速区间。
如图2所示，为本发明实施例中三轴超声波辅助下的行人自主定位导航示意图。本发明以左脚起始点作为原点建立轴向与地理坐标系重合的方位坐标系，原点为
Figure BDA00039301351100001515
右脚的初始坐标为
Figure BDA00039301351100001516
在迈步的过程中利用四阶毕卡法进行四元数的状态更新得到先验导航姿态四元数
Figure BDA00039301351100001517
并且此时不执行本方法的后续步骤，所以只有
Figure BDA0003930135110000161
当在超声波辅助下判断出零速率区间时，有零速区间状态标志符S＝2，则利用理论加速度测量值
Figure BDA0003930135110000162
和实际加速度测量值
Figure BDA0003930135110000163
进行扩展卡尔曼滤波测量更新得到左脚(右脚)第一组导航姿态四元数
Figure BDA0003930135110000164
利用理论导航姿态四元数
Figure BDA0003930135110000165
作为测量值进行卡尔曼滤波测量更新，修正
Figure BDA0003930135110000166
得到左脚(右脚)第二组导航姿态四元数
Figure BDA0003930135110000167
然后，使用自适应互补滤波得到融合导航姿态四元数
Figure BDA0003930135110000168
将惯导计算的足间距离
Figure BDA0003930135110000169
作为理论测量值，超声波测距传感器得到的足间距离
Figure BDA00039301351100001610
作为实际测量值进行扩展卡尔曼滤波校准人员的导航姿态、速度和位置，得到最优的导航姿态四元数，速度和位置估计。
上述实施例阐明的系统、装置、模块或单元，具体可以由计算机芯片或实体实现，或者由具有某种功能的产品来实现。一种典型的实现设备为计算机。具体的，计算机例如可以为个人计算机、膝上型计算机、蜂窝电话、相机电话、智能电话、个人数字助理、媒体播放器、导航设备、电子邮件设备、游戏控制台、平板计算机、可穿戴设备或者这些设备中的任何设备的组合。
还需要说明的是，术语“包括”、“包含”或者其任何其他变体意在涵盖非排他性的包含，从而使得包括一系列要素的过程、方法、商品或者设备不仅包括那些要素，而且还包括没有明确列出的其他要素，或者是还包括为这种过程、方法、商品或者设备所固有的要素。在没有更多限制的情况下，由语句“包括一个……”限定的要素，并不排除在包括所述要素的过程、方法、商品或者设备中还存在另外的相同要素。
以上这些实施例应理解为仅用于说明本发明而不用于限制本发明的保护范围。在阅读了本发明的记载的内容之后，技术人员可以对本发明作各种改动或修改，这些等效变化和修饰同样落入本发明权利要求所限定的范围。

Claims (7)

1.一种三轴超声波辅助下的行人自主定位导航方法，其特征在于，包括以下步骤：
步骤1：采集行人足部前脚掌的三轴超声波和六轴姿态仪数据，并利用三轴超声波辅助六轴姿态仪判断行人前脚掌的零速区间；
步骤2：在零速区间利用三轴加速度计和三轴陀螺仪数据分别更新得到两组导航姿态四元数，并利用自适应互补滤波融合得到融合导航姿态四元数；
步骤3：在零速区间利用三轴超声波数据对导航位置和融合导航姿态四元数进行修正，从而实现行人的精确定位和导航。
2.根据权利要求1所述的一种三轴超声波辅助下的行人自主定位导航方法，其特征在于，所述步骤1采集行人足部前脚掌的三轴超声波和六轴姿态仪数据，并利用三轴超声波辅助六轴姿态仪判断行人前脚掌的零速区间，具体步骤为：
步骤1.1：对零速区间状态标志符S和长度为L的滑动窗口中三轴超声波模块的测距信息进行计算并判断：当S＝0，且
Figure FDA0003930135100000011
k)小于等于阈值εultr时，令S＝1，反之则S＝0；
其中，
Figure FDA0003930135100000012
为由右脚(右脚)超声波
Figure FDA0003930135100000013
轴发出，k时刻左脚(右脚)超声波
Figure FDA0003930135100000014
轴接收的测距距离，
Figure FDA0003930135100000015
为k时刻左脚超声波
Figure FDA0003930135100000016
轴测的离地面的距离；
步骤1.2：计算k时刻长度为L的滑动窗口中，i(i＝x,y,z)轴加速度
Figure FDA0003930135100000017
(j＝k-L+1,k-L+2,…,k)与初始时刻k0加速度
Figure FDA0003930135100000018
之间斜率的平均值
Figure FDA0003930135100000019
计算公式为：
Figure FDA00039301351000000110
其中，N为相邻两个零速率区间间隔的数据长度；
步骤1.4：对
Figure FDA00039301351000000111
进行判断：当S＝1，且各轴的
Figure FDA00039301351000000112
的绝对值都分别小于各轴的阈值
Figure FDA00039301351000000113
时，令S＝2，反之则S＝0。
3.根据权利要求2所述的一种三轴超声波辅助下的行人自主定位导航方法，其特征在于，步骤1利用三轴超声波辅助六轴姿态仪判断行人前脚掌的零速区间中各坐标系具体轴向为：
左、右脚三轴陀螺仪的轴向，xgyro轴向右，ygyro轴向前，zgyro轴向上，三轴正交，成右手坐标系xgyroygyrozgyro；
左、右脚三轴加速度计的轴向，xacc轴向右，yacc轴向前，zacc轴向上，三轴正交，成右手坐标系xaccyacczacc，与xgyroygyrozgyro坐标系重合；
左、右脚三轴超声波模块轴向，
Figure FDA0003930135100000021
轴向右(左)前方，
Figure FDA0003930135100000022
轴向右(左)后方，
Figure FDA0003930135100000023
轴向下，三轴正交，成右手坐标系
Figure FDA0003930135100000024
4.根据权利要求1所述的一种三轴超声波辅助下的行人自主定位导航方法，其特征在于，所述步骤2在零速区间利用三轴加速度计和三轴陀螺仪数据分别更新得到两组导航姿态四元数，并利用自适应互补滤波融合得到融合导航姿态四元数，具体步骤为：
步骤2.1：利用四阶毕卡法对导航姿态四元数进行更新，得到k时刻左脚(右脚)的先验导航姿态四元数
Figure FDA0003930135100000025
步骤2.2：利用
Figure FDA0003930135100000026
解算由n系(东北天坐标系)到b系(右前上坐标系)的坐标转换矩阵
Figure FDA0003930135100000027
然后将零速区间n系加速度理论值a0通过坐标转换矩阵
Figure FDA0003930135100000028
进行坐标转换得到b系的加速度理论值
Figure FDA0003930135100000029
最后将k时刻加速度计的b系测量值
Figure FDA00039301351000000210
和
Figure FDA00039301351000000211
代入扩展卡尔曼滤波测量更新
Figure FDA00039301351000000212
得到左脚(右脚)第一组后验导航姿态四元数
Figure FDA00039301351000000213
步骤2.3：利用零速区间b系角速率理论值ω0更新四元数得到左脚零速区间理论导航姿态四元数
Figure FDA00039301351000000214
并将其作为测量值代入一般卡尔曼滤波的测量更新
Figure FDA00039301351000000215
得到左脚(右脚)第二组后验导航姿态四元数
Figure FDA00039301351000000216
步骤2.4：根据左脚(右脚)加速度计实际测量值
Figure FDA00039301351000000217
和理论值
Figure FDA00039301351000000218
之间的关系计算
Figure FDA0003930135100000031
权重w1，公式为：
Figure FDA0003930135100000032
其中，μL(R)1为权重wL(R)1的调整系数；
步骤2.5：
Figure FDA0003930135100000033
权重wL(R)2的计算公式为：
wL(R)2＝1-wL(R)1
步骤2.6：根据权重计算融合后的左脚(右脚)融合后验导航姿态四元数
Figure FDA0003930135100000034
公式为：
Figure FDA0003930135100000035
5.根据权利要求4所述的一种三轴超声波辅助下的行人自主定位导航方法，其特征在于，所述步骤2.2的扩展卡尔曼滤波测量更新
Figure FDA0003930135100000036
的方程具体为：
Figure FDA0003930135100000037
Figure FDA0003930135100000038
Figure FDA0003930135100000039
Figure FDA00039301351000000310
Figure FDA00039301351000000311
其中，
Figure FDA00039301351000000312
表示左脚(右脚)第一组测量更新矩阵，
Figure FDA00039301351000000313
表示左脚(右脚)第一组卡尔曼增益系数，
Figure FDA00039301351000000314
表示左脚(右脚)第一组先验估计协方差，
Figure FDA00039301351000000315
表示左脚(右脚)第一组测量误差矩阵，
Figure FDA00039301351000000316
表示左脚(右脚)b系理论加速度测量值，
Figure FDA00039301351000000317
表示左脚(右脚)b系实际加速度测量值，
Figure FDA00039301351000000318
表示左脚(右脚)第一组后验估计协方差矩阵。
6.根据权利要求4所述的一种三轴超声波辅助下的行人自主定位导航方法，其特征在于，所述步骤2.3的卡尔曼滤波测量更新
Figure FDA0003930135100000041
的方程具体为：
Figure FDA0003930135100000042
Figure FDA0003930135100000043
Figure FDA0003930135100000044
Figure FDA0003930135100000045
其中，
Figure FDA0003930135100000046
表示左脚(右脚)第二组测量更新矩阵，
Figure FDA0003930135100000047
表示左脚(右脚)第二组卡尔曼增益系数，
Figure FDA0003930135100000048
表示左脚(右脚)第二组先验估计协方差，
Figure FDA0003930135100000049
表示左脚(右脚)第二组测量误差矩阵，
Figure FDA00039301351000000410
表示左脚(右脚)理论导航姿态四元数，
Figure FDA00039301351000000411
表示左脚(右脚)第二组后验估计协方差矩阵。
7.根据权利要求1所述的一种三轴超声波辅助下的行人自主定位导航方法，其特征在于，所述步骤3在零速区间利用三轴超声波数据对导航位置和融合导航姿态四元数进行修正，从而实现行人的精确定位和导航，具体步骤为：
步骤3.1：以左脚起始点
Figure FDA00039301351000000412
为原点建立轴向与n系坐标系重合的方位坐标系，确定初始时刻右脚相对于方位坐标系原点的坐标点
Figure FDA00039301351000000413
在两个相邻零速率区间之间的运动过程中只进行状态更新，不进行测量更新，因此
Figure FDA00039301351000000414
由
Figure FDA00039301351000000415
得从b系坐标系到n系坐标系的坐标转换矩阵
Figure FDA00039301351000000416
然后将k-1时刻左脚(右脚)加速度计的b系测量值
Figure FDA00039301351000000417
进行坐标转换，得到将k-1时刻左脚(右脚)加速度计的n系各轴的分量
Figure FDA00039301351000000418
在减去重力加速度g后，对x轴、y轴和z轴的加速度一重积分得到k时刻左脚(右脚)x轴向、y轴向和z轴向的移动速度
Figure FDA00039301351000000419
和
Figure FDA00039301351000000420
双重积分得到k时刻左脚(右脚)x轴向、y轴向和z轴向的移动距离
Figure FDA00039301351000000421
和
Figure FDA00039301351000000422
得到移动后左脚的坐标
Figure FDA00039301351000000423
右脚的坐标
Figure FDA00039301351000000424
利用左、右脚的坐标点就可以求得足间距离
Figure FDA0003930135100000051
具体为：
Figure FDA0003930135100000052
Figure FDA0003930135100000053
Figure FDA0003930135100000054
Figure FDA0003930135100000055
Figure FDA0003930135100000056
Figure FDA0003930135100000057
Figure FDA0003930135100000058
Figure FDA0003930135100000059
Figure FDA00039301351000000510
Figure FDA00039301351000000511
Figure FDA00039301351000000512
Figure FDA00039301351000000513
Figure FDA00039301351000000514
Figure FDA00039301351000000515
其中，
Figure FDA00039301351000000516
表示k-1时刻左脚(右脚)在n系坐标系的加速度，
Figure FDA00039301351000000517
Figure FDA00039301351000000518
分别表示k-1时刻左脚(右脚)在n系坐标系下x、y、z轴向的位移，
Figure FDA00039301351000000519
Figure FDA00039301351000000520
分别表示k时刻左脚(右脚)在n系坐标系下x、y、z轴向的坐标,
Figure FDA00039301351000000521
表示k时刻由惯导计算的左脚(右脚)与右脚(左脚)之间的足间距离；
步骤3.2：根据步骤2得到的左脚(右脚)融合后验导航姿态四元数
Figure FDA00039301351000000522
和步骤3.1中的左脚和右脚的速度和位置坐标构建状态空间向量Xk，具体为：
Figure FDA00039301351000000523
步骤3.3：对状态空间向量Xk进行状态更新，具体为：
Figure FDA0003930135100000061
Figure FDA0003930135100000062
Figure FDA0003930135100000063
Fk＝I20×20
Figure FDA0003930135100000064
Figure FDA0003930135100000065
其中，Uk-1表示变量向量，Bk-1和Gk-1表示变量向量与状态向量的关系矩阵，Fk表示k时刻的状态转换矩阵，Om×n表示m×n的0矩阵，
Figure FDA00039301351000000615
表示k时刻的先验估计协方差，
Figure FDA0003930135100000066
表示k-1时刻的后验估计协方差矩阵，Qk表示k时刻的状态更新误差矩阵；
步骤3.4：对状态空间向量Xk进行量测更新，具体为：
Figure FDA0003930135100000067
Figure FDA0003930135100000068
Figure FDA0003930135100000069
Figure FDA00039301351000000610
其中,Hk表示k时刻的测量更新矩阵，Kk表示k时刻的卡尔曼增益系数，Rk表示k时刻的测量误差矩阵，
Figure FDA00039301351000000611
表示由超声波传感器
Figure FDA00039301351000000612
轴发送超声波到
Figure FDA00039301351000000613
轴接收计算的足间距离，
Figure FDA00039301351000000614
表示k时刻的后验估计协方差矩阵；
步骤3.4：获得校准后左脚和右脚的导航姿态四元数和位置坐标：
Figure FDA0003930135100000071
Figure FDA0003930135100000072
Figure FDA0003930135100000073
Figure FDA0003930135100000074
Figure FDA0003930135100000075
Figure FDA0003930135100000076
步骤3.5：重复步骤1、步骤2.1和步骤3.3的状态更新，直至S＝2，姿态仪处于零速区间；此时重复步骤1、步骤2、步骤3.1、步骤3.2、步骤3.3和步骤3.4，直至S≠2，姿态仪处于非零速区间。
CN202211386612.XA 2022-11-07 2022-11-07 一种三轴超声波辅助下的行人自主定位导航方法 Pending CN115655269A (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202211386612.XA CN115655269A (zh) 2022-11-07 2022-11-07 一种三轴超声波辅助下的行人自主定位导航方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202211386612.XA CN115655269A (zh) 2022-11-07 2022-11-07 一种三轴超声波辅助下的行人自主定位导航方法

Publications (1)

Publication Number Publication Date
CN115655269A true CN115655269A (zh) 2023-01-31

Family

ID=85016836

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202211386612.XA Pending CN115655269A (zh) 2022-11-07 2022-11-07 一种三轴超声波辅助下的行人自主定位导航方法

Country Status (1)

Country Link
CN (1) CN115655269A (zh)

Cited By (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN116380056A (zh) * 2023-06-02 2023-07-04 中国船舶集团有限公司第七〇七研究所 惯性定位方法、装置、电子设备及存储介质

Citations (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
WO2002035183A1 (en) * 2000-07-28 2002-05-02 Litton Systems, Inc. Attitude alignment of a slave inertial measurement system
IL180833A0 (en) * 2007-01-21 2007-12-03 Israel Aerospace Ind Ltd Pedestrian navigation system and method
CN109855621A (zh) * 2018-12-27 2019-06-07 国网江苏省电力有限公司检修分公司 一种基于uwb与sins的组合室内行人导航系统及方法
CN111024126A (zh) * 2019-12-26 2020-04-17 北京航天控制仪器研究所 一种行人导航定位中的自适应零速修正方法
CN118149823A (zh) * 2024-03-25 2024-06-07 重庆邮电大学 一种测距信息辅助的足绑式行人三维室内定位方法

Patent Citations (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
WO2002035183A1 (en) * 2000-07-28 2002-05-02 Litton Systems, Inc. Attitude alignment of a slave inertial measurement system
IL180833A0 (en) * 2007-01-21 2007-12-03 Israel Aerospace Ind Ltd Pedestrian navigation system and method
CN109855621A (zh) * 2018-12-27 2019-06-07 国网江苏省电力有限公司检修分公司 一种基于uwb与sins的组合室内行人导航系统及方法
CN111024126A (zh) * 2019-12-26 2020-04-17 北京航天控制仪器研究所 一种行人导航定位中的自适应零速修正方法
CN118149823A (zh) * 2024-03-25 2024-06-07 重庆邮电大学 一种测距信息辅助的足绑式行人三维室内定位方法

Non-Patent Citations (1)

* Cited by examiner, † Cited by third party
Title
路永乐, 苏胜, 杨杰, 等: "基于足间距信息辅助的行人三维惯性定位算法", 导航定位与授时, vol. 11, no. 1, 31 January 2024 (2024-01-31), pages 06 - 114 *

Cited By (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN116380056A (zh) * 2023-06-02 2023-07-04 中国船舶集团有限公司第七〇七研究所 惯性定位方法、装置、电子设备及存储介质
CN116380056B (zh) * 2023-06-02 2023-08-15 中国船舶集团有限公司第七〇七研究所 惯性定位方法、装置、电子设备及存储介质

Similar Documents

Publication Publication Date Title
CN111207774B (zh) 一种用于激光-imu外参标定的方法及系统
US10352959B2 (en) Method and system for estimating a path of a mobile element or body
CN109269471B (zh) 一种新型gnss接收机倾斜测量系统及方法
CN107014376B (zh) 一种适用于农业机械精准作业的姿态倾角估计方法
CN111896007B (zh) 一种补偿足地冲击的四足机器人姿态解算方法
JP4989035B2 (ja) 慣性ナビゲーションシステムの誤差補正
CN107607113B (zh) 一种两轴姿态倾角测量方法
CN110887481B (zh) 基于mems惯性传感器的载体动态姿态估计方法
CN111551174A (zh) 基于多传感器惯性导航系统的高动态车辆姿态计算方法及系统
CN109186597B (zh) 一种基于双mems-imu的室内轮式机器人的定位方法
CN110715659A (zh) 零速检测方法、行人惯性导航方法、装置及存储介质
CN114076610B (zh) Gnss/mems车载组合导航系统的误差标定、导航方法及其装置
CN111721288A (zh) 一种mems器件零偏修正方法、装置及存储介质
JP2016033473A (ja) 位置算出方法及び位置算出装置
CN113291493B (zh) 一种卫星多敏感器融合姿态确定方法和系统
CN111024126A (zh) 一种行人导航定位中的自适应零速修正方法
CN114383612B (zh) 一种视觉辅助惯性差分位姿测量系统
CN115540860A (zh) 一种多传感器融合位姿估计算法
CN114526731A (zh) 一种基于助力车的惯性组合导航方向定位方法
CN115655269A (zh) 一种三轴超声波辅助下的行人自主定位导航方法
CN111141285B (zh) 一种航空重力测量装置
CN117029810A (zh) 一种基于迭代EKF的Mahony姿态测量方法
CN114719858A (zh) 一种基于imu和楼层高度目标补偿的3维定位方法
CN113447960B (zh) 基于gnss/mems组合车载导航系统的误差标定方法及系统
US6085149A (en) Integrated inertial/VMS navigation solution

Legal Events

Date Code Title Description
PB01 Publication
PB01 Publication
SE01 Entry into force of request for substantive examination
SE01 Entry into force of request for substantive examination