CN110514226B - 一种行人惯性导航的航向校正方法 - Google Patents
一种行人惯性导航的航向校正方法 Download PDFInfo
- Publication number
- CN110514226B CN110514226B CN201910832314.0A CN201910832314A CN110514226B CN 110514226 B CN110514226 B CN 110514226B CN 201910832314 A CN201910832314 A CN 201910832314A CN 110514226 B CN110514226 B CN 110514226B
- Authority
- CN
- China
- Prior art keywords
- heading
- information
- course
- pedestrian
- current
- 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.)
- Active
Links
Images
Classifications
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
- G01C25/00—Manufacturing, calibrating, cleaning, or repairing instruments or devices referred to in the other groups of this subclass
- G01C25/005—Manufacturing, calibrating, cleaning, or repairing instruments or devices referred to in the other groups of this subclass initial alignment, calibration or starting-up of inertial devices
Abstract
本发明提供了一种行人惯性导航的航向校正方法,相较于利用多传感器融合的航向校正方法,本方法只用惯导传感器实现了较为准确的航向校正,节省了硬件成本,简化了算法复杂度。相较于现有的基于纯惯性导航系统的航向校正方法,本方法深入挖掘惯导元器件的特性,充分利用了行进过程中已有的信息,而不是只局限在某一个特定时间段内的局部信息,并且利用时间优先的原则对筛选出的匹配信号进行再一次选取,这使得本方法有更好的鲁棒性、更精确的航向校正效果。
Description
技术领域
本发明属于惯性导航技术领域,尤其涉及一种行人惯性导航的航向校正方法。
背景技术
随着社会的快速发展,导航技术无论在军用还是民用方面都得到了广泛的应用,在近些年也取得了长足的发展。在室外定位中,全球导航定位系统有着优秀的表现,然而,由于卫星信号穿透性较差,在高楼、地下建筑等室内场景中,卫星定位系统的表现不尽如人意。惯性导航技术作为一种不易受外界环境干扰,无需预先部署,无依赖,低成本的导航技术,在无GPS情况下的定位中有着独特的优势。
惯性导航系统,是一种根据惯性传感器模块测量行人的运动参数并通过导航解算,解算出行人运动的位置信息的导航方法。惯性传感器模块主要由三轴加速度计、三轴陀螺仪组成,有些惯性传感器模块还会集成温度计、磁力计以及气压计。由于惯性导航系统单纯依靠惯性传感器的原始数据进行导航解算(无其他额外信息辅助)的特点,惯性传感器的质量往往成为左右惯性导航系统精度的主要因素。传统的惯性导航系统主要是对惯性传感器测得的加速度以及角加速度进行二次积分得到导航轨迹。然而,以积分的形式去推算导航轨迹,会不可避免地对噪声以及误差进行累积,这就会导致惯性导航系统在长距离、长时间的定位中出现严重的轨迹发散现象,难以符合长时间定位的要求。而基于行人航向推算系统(PDR)的惯性导航系统虽然可以有效减少积分所带来的航向累积误差,但在长距离的定位上仍然会存在航向的发散问题。
现有的解决航向发散问题的方法主要依赖于外界的信号辅助,比如磁场信号、毫米波雷达信号、激光信号、相机图像等等。然而,如毫米波雷达信号、激光信号及相机图像,对外界的设备要求比较高,造价昂贵且不易携带。磁场信号虽然容易获取,却非常容易受到外界信号的干扰,而室内磁场干扰源复杂多变,难以进行定量分析,所以现有的磁场融合算法都不能很好地证明算法的通用性与准确性。而不依赖于外界信号的航向纠正算法主要是利用行人的运动特性,典型的就是启发式漂移减少算法(HDR)。HDR算法利用行人行进过程中趋向于直线行走的特性,通过动态滑动窗口的方式,比较当前脚步的方向与过去几步中的脚步方向的差值,得出一个直线行走的判断。而后将当前脚步方向设置为过去几步脚步行进方向的均值,将此作为实际航向代入到惯导系统中求取接下来的航向。这种利用行人短期内运动特性的航向校正方法在一定程度上减小了航向误差,但由于这种方法只能利用短期内的惯导信息,未能充分利用过去时刻的惯导信息,因此在长时间上,它只能对航向信息微弱地进行补偿,不能完全满足惯性导航系统的要求。
发明内容
针对现有技术中的上述不足,本发明提供的一种行人惯性导航的航向校正方法解决了现有的航向校正方法依赖外界信号、不能充分利用已有的惯导器件的信息、依赖短期行人运动信息而忽视长时间的运动信息的问题。
为了达到以上目的,本发明采用的技术方案为:
本方案提供一种行人惯性导航的航向校正方法,包括如下步骤:
S1、将行人惯性导航数据输入至航向推算系统中,得到推算航向;
S2、将单位加速度在载体坐标系的值作为观测量,将利用旋转矩阵得到的单位加速度的分量作为预测量;
S3、利用卡尔曼滤波算法对所述观测量和预测量进行融合计算,得到行人当前的航向信息,并存储至预设的航向信息存储池中;
S4、根据行人脚步落地的时刻,利用航向匹配算法从预设的航向信息存储池中寻找与所述行人当前的航向信息匹配的已有航向信息;
S5、判断是否寻找到匹配的航向信息,若是,则将所述匹配的航向信息作为输出结果,从而完成对行人惯性导航的航向校正,否则进入步骤S6;
S6、将航向推算系统中的推算航向作为最终的输出结果,并将该输出的航向信息输入至航向信息存储池中进行存储。
进一步地,所述步骤S2中预测量的表达式如下:
accpre=[2×q(2)×q(4)-q(1)×q(3),2×q(3)×q(4)+q(1)×q(2),q(1)2-q(2)2-q(3)2+q(4)2]
其中,accpre表示利用旋转矩阵得到的单位加速度,q(1),q(2),q(3)和q(4)均表示以四元数表示的旋转矩阵的四个分量。
再进一步地,所述步骤S4包括如下步骤:
S401、根据行人脚步落地的时刻,将所述行人的初步航向作为行人当前脚步周期内的航向信息;
S402、根据所述航向信息存储池中的已有航向信息,利用阈值筛选法筛选出与当前脚步周期的航向信息匹配的已有航向信息;
S403、查询筛选出的已有航向信息中最早时刻的航向信息点,将该时刻的航向信息作为参考的航向信息,并与当前航向信息进行融合处理,得到融合后的航向信息;
S404、将融合后的航向信息作为实际航向信息的观测量,将当前时刻的航向信息作为预测量,并利用卡尔曼算法对当前时间的航向信息进行补偿,得到当前时刻航向信息;
S405、将所述当前时刻的航向信息输入至航向信息存储池中进行存储,从而得到与所述行人当前航向匹配的已有航向信息。
再进一步地,所述步骤S402中的筛选的条件为:
|heading(t)-heading(i)|<180±3
|heading(t)-heading(i)|<±3
其中,heading(t)表示当前时刻的航向信息,heading(i)表示航向信息存储池中的已有航向。
再进一步地,所述步骤S403中融合后的航向信息heading_act的表达式如下:
headingob=a×heading(t)+(1-a)×heading(j)
a=init×e-t/τ
其中,heading(j)表示筛选出的前期航向信息,heading(t)表示当前时刻的航向信息,headingob表示融合后的航向信息,a表示前期的权重值,init表示初始权重系数,τ表示控制衰减程度的系数,e-t/τ表示权重的衰减系数。
本发明的有益效果:
本发明提供了一种行人惯性导航的航向校正方法。相较于利用多传感器融合的航向校正方法,本方法只使用惯导信息(加速度信号与陀螺仪信号)实现了较为准确的航向校正,节省了硬件成本,简化了算法复杂度。相较于现有的基于纯惯性导航系统的航向校正方法,本方法深入挖掘惯导元器件的特性,充分利用了行进过程中已有的信息,而不是只局限在某一个特定时间段内的局部信息,并且利用时间优先的原则对筛选出的匹配信号进行再一次选取,使得本方法有更好的鲁棒性、更精确的航向校正效果。
附图说明
图1为本发明的方法流程图。
图2为基于惯性导航系统的航向校正方法的总体框图。
图3为本实施例中的航向推算系统的具体框图。
图4为本实施例中的航向匹配算法的具体实现框图。
图5为本实施例中的卡尔曼滤波器的具体框图。
图6为本方法与传统基于纯惯导系统的航向校正方法的比较示意图。
具体实施方式
下面对本发明的具体实施方式进行描述,以便于本技术领域的技术人员理解本发明,但应该清楚,本发明不限于具体实施方式的范围,对本技术领域的普通技术人员来讲,只要各种变化在所附的权利要求限定和确定的本发明的精神和范围内,这些变化是显而易见的,一切利用本发明构思的发明创造均在保护之列。
实施例
如图1-图2所示,本发明公开了一种行人惯性导航的航向校正方法,其实现方法如下:
S1、将行人惯性导航数据输入至航向推算系统中,得到推算航向;
S2、将单位加速度在载体坐标系的值作为观测量,将利用旋转矩阵得到的单位加速度的分量作为预测量,所述预测量的表达式如下:
accpre=[2×q(2)×q(4)-q(1)×q(3),2×q(3)×q(4)+q(1)×q(2),q(1)2-q(2)2-q(3)2+q(4)2]
其中,accpre表示利用旋转矩阵得到的单位加速度,q(1),q(2),q(3)和q(4)均表示以四元数表示的旋转矩阵的四个分量;
本实施例中,如图3所示,将惯导数据送入航向推算系统中,在航向推算系统中把加速度作为观测量,以单位加速度在载体坐标系中的值作为观测量,以旋转矩阵解算出的单位加速度的分量作为预测值,利用卡尔曼滤波进行融合,从而推算出行人当前初步的航向信息。
S4、如图4所示,根据行人脚步落地的时刻,利用航向匹配算法从预设的航向信息存储池中寻找与所述行人初步航向匹配的已有航向信息,其实现方法如下:
S401、根据行人脚步落地的时刻,将所述行人的初步航向作为行人当前脚步周期内的航向信息;
S402、根据所述航向信息存储池中的已有航向信息,利用阈值筛选法筛选出与当前脚步周期的航向信息匹配的已有航向信息;
本实施例中,将当前脚步周期的航向与航向信息存储池中的储存的过去航向信息相比较,利用阈值筛选的方法,筛选出符合要求的过去航向信息。如果当前时刻方向与前期某个时刻方向差小于某个阈值,就可以利用前期的方向去修正当前时刻的方向。在算法过程中,取相同方向和相反方向两种方向差(当前方向与选定方向差值为0或180度),航向筛选的条件为:
|heading(t)-heading(i)|<180±3
|heading(t)-heading(i)|<±3
其中,heading(t)表示当前时刻的航向信息,heading(i)表示航向信息存储池中的已有航向;
S403、查询筛选出的已有航向信息中最早时刻的航向信息点,将该时刻的航向信息作为参考的航向信息,并与当前航向信息进行融合处理,得到融合后的航向信息;
本实施例中,对筛选出的过去航向信息进行查询,找出最早时刻的航向信息点,将该时刻的航向信息作为参考的航向与当前航向进行融合处理。选择最早时刻的航向,是因为根据陀螺仪信号的短期精准的特点,由于航向漂移,越是时刻靠前,航向信息就越准确地趋于实际航向值。融合处理后的航向公式为:
headingob=a×heading(t)+(1-a)×heading(j)
a=init×e-t/τ
其中,heading(j)表示筛选出的前期航向信息,heading(t)表示当前时刻的航向信息,headingob表示融合后的航向信息,即进行权重取舍之后得到的行人的方向,用于卡尔曼滤波中作观测量,a表示前期的权重值,init表示初始权重系数,τ表示控制衰减程度的系数,e-t/τ表示权重的衰减系数。
本实施例中,如图5所示,利用卡尔曼滤波与航向匹配结果校正航向信息,将取得的航向信息当作实际的航向信息作为观测量,将当前时刻的航向作为预测量,利用卡尔曼滤波的方法对当前时刻的航向进行补偿,得到一个相对准确的当前时刻的航向信息输出;
S405、将所述当前时刻的航向信息输入至航向信息存储池中进行存储,从而得到与所述行人当前航向匹配的已有航向信息
S5、判断是否寻找到匹配的航向信息,若是,则将所述匹配的航向信息作为输出结果,从而完成对行人惯性导航系统的航向校正,否则进入步骤S6;
S6、将航向推算系统中的推算航向作为最终的输出结果,并将该输出的航向信息输入至航向信息存储池中进行存储。
如图6所示,实验中走了一个闭环轨迹,传统的零速更新方法的轨迹会随着时间的推移而逐渐发散,而本文所提出的方法轨迹更接近于一个完整的闭环,相较而言,在航向的校正方面有着更为优秀的表现。总体而言,本发明提供了一种行人惯性导航的航向校正方法。相较于利用多传感器融合的航向校正方法,本方法只使用惯导信息(加速度信号与陀螺仪信号)实现了较为准确的航向校正,节省了硬件成本,简化了算法复杂度。相较于现有的基于纯惯性导航系统的航向校正方法,本方法深入挖掘惯导元器件的特性,充分利用了行进过程中已有的信息,而不是只局限在某一个特定时间段内的局部信息,并且利用时间优先的原则对筛选出的匹配信号进行再一次选取,这使得本方法有更好的鲁棒性、更精确的航向校正效果。
Claims (7)
1.一种行人惯性导航的航向校正方法,其特征在于,包括如下步骤:
S1、将行人惯性导航数据输入至航向推算系统中;
S2、将单位加速度在载体坐标系的值作为观测量,将利用旋转矩阵得到的单位加速度的分量作为预测量;
S3、利用卡尔曼滤波算法对所述观测量和预测量进行融合计算,得到行人当前的航向信息,并存储至预设的航向信息存储池中;
S4、根据行人脚步落地的时刻,利用航向匹配算法从预设的航向信息存储池中寻找与所述行人当前的航向信息匹配的已有航向信息;
S5、判断是否寻找到匹配的航向信息,若是,则将所述匹配的航向信息作为输出结果,从而完成对行人惯性导航的航向校正,否则进入步骤S6;
S6、将航向推算系统中的推算航向作为最终的输出结果,并将该输出的航向信息输入至预设的航向信息存储池中进行存储。
2.根据权利要求1所述的行人惯性导航的航向校正方法,其特征在于,所述步骤S2中预测量的表达式如下:
accpre=[2×q(2)×q(4)-q(1)×q(3),2×q(3)×q(4)+q(1)×q(2),q(1)2-q(2)2-q(3)2+q(4)2]
其中,accpre表示利用旋转矩阵得到的单位加速度,q(1),q(2),q(3)和q(4)均表示以四元数表示的旋转矩阵的四个分量。
4.根据权利要求1所述的行人惯性导航的航向校正方法,其特征在于,所述步骤S4包括如下步骤:
S401、根据行人脚步落地的时刻,将所述行人的初步航向作为行人当前脚步周期内的航向信息;
S402、根据所述航向信息存储池中的已有航向信息,利用阈值筛选法筛选出与当前脚步周期的航向信息匹配的已有航向信息;
S403、查询筛选出的已有航向信息中最早时刻的航向信息点,将该时刻的航向信息作为参考的航向信息,并与当前航向信息进行融合处理,得到融合后的航向信息;
S404、将融合后的航向信息作为实际航向信息的观测量,将当前时刻的航向信息作为预测量,并利用卡尔曼算法对当前时刻的航向信息进行补偿,得到当前时刻航向信息;
S405、将所述当前时刻的航向信息输入至航向信息存储池中进行存储,从而得到与所述行人当前航向匹配的已有航向信息。
5.根据权利要求4所述的行人惯性导航的航向校正方法,其特征在于,所述步骤S402中的筛选的条件为:
|heading(t)-heading(i)|<180±3或
|heading(t)-heading(i)|<3
其中,heading(t)表示当前时刻的航向信息,heading(i)表示航向信息存储池中的已有航向。
6.根据权利要求4所述的行人惯性导航的航向校正方法,其特征在于,所述步骤S403中融合后的航向信息的表达式如下:
headingob=a×heading(t)+(1-a)×heading(j)
a=init×e-t/τ
其中,heading(j)表示筛选出的前期航向信息,heading(t)表示当前时刻的航向信息,headingob表示融合后的航向信息,a表示前期的权重值,init表示初始权重系数,τ表示控制衰减程度的系数,e-t/τ表示权重的衰减系数。
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN201910832314.0A CN110514226B (zh) | 2019-09-04 | 2019-09-04 | 一种行人惯性导航的航向校正方法 |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN201910832314.0A CN110514226B (zh) | 2019-09-04 | 2019-09-04 | 一种行人惯性导航的航向校正方法 |
Publications (2)
Publication Number | Publication Date |
---|---|
CN110514226A CN110514226A (zh) | 2019-11-29 |
CN110514226B true CN110514226B (zh) | 2021-04-27 |
Family
ID=68629849
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN201910832314.0A Active CN110514226B (zh) | 2019-09-04 | 2019-09-04 | 一种行人惯性导航的航向校正方法 |
Country Status (1)
Country | Link |
---|---|
CN (1) | CN110514226B (zh) |
Families Citing this family (1)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN113203416B (zh) * | 2021-03-19 | 2022-07-12 | 电子科技大学 | 一种针对摆臂行人的行人航位推算方法 |
Citations (6)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
EP2657647A1 (en) * | 2012-04-23 | 2013-10-30 | Deutsches Zentrum für Luft- und Raumfahrt e. V. | Method for estimating the position and orientation using an inertial measurement unit fixed to a moving pedestrian |
CN104197938A (zh) * | 2014-09-03 | 2014-12-10 | 厦门大学 | 基于脚部惯性传感器的室内定位方法 |
KR101642286B1 (ko) * | 2015-02-12 | 2016-07-25 | 한국항공우주연구원 | 보행특성을 이용한 실내에서의 보행자 이동방향 추정 방법 |
CN109297484A (zh) * | 2017-07-25 | 2019-02-01 | 北京信息科技大学 | 一种有步态约束的行人自主定位误差修正方法 |
US10215587B2 (en) * | 2012-05-18 | 2019-02-26 | Trx Systems, Inc. | Method for step detection and gait direction estimation |
CN109855620A (zh) * | 2018-12-26 | 2019-06-07 | 北京壹氢科技有限公司 | 一种基于自回溯算法的室内行人导航方法 |
Family Cites Families (3)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
US8694251B2 (en) * | 2010-11-25 | 2014-04-08 | Texas Instruments Incorporated | Attitude estimation for pedestrian navigation using low cost mems accelerometer in mobile applications, and processing methods, apparatus and systems |
CN103412291B (zh) * | 2013-08-16 | 2015-02-04 | 四川九洲空管科技有限责任公司 | 一种基于二次雷达目标多路径效应抑制技术的实现方法 |
CN106851578B (zh) * | 2017-02-23 | 2020-08-21 | 烟台中飞海装科技有限公司 | 复杂未知室内环境中人员定位系统和方法 |
-
2019
- 2019-09-04 CN CN201910832314.0A patent/CN110514226B/zh active Active
Patent Citations (6)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
EP2657647A1 (en) * | 2012-04-23 | 2013-10-30 | Deutsches Zentrum für Luft- und Raumfahrt e. V. | Method for estimating the position and orientation using an inertial measurement unit fixed to a moving pedestrian |
US10215587B2 (en) * | 2012-05-18 | 2019-02-26 | Trx Systems, Inc. | Method for step detection and gait direction estimation |
CN104197938A (zh) * | 2014-09-03 | 2014-12-10 | 厦门大学 | 基于脚部惯性传感器的室内定位方法 |
KR101642286B1 (ko) * | 2015-02-12 | 2016-07-25 | 한국항공우주연구원 | 보행특성을 이용한 실내에서의 보행자 이동방향 추정 방법 |
CN109297484A (zh) * | 2017-07-25 | 2019-02-01 | 北京信息科技大学 | 一种有步态约束的行人自主定位误差修正方法 |
CN109855620A (zh) * | 2018-12-26 | 2019-06-07 | 北京壹氢科技有限公司 | 一种基于自回溯算法的室内行人导航方法 |
Non-Patent Citations (2)
Title |
---|
Advanced Heuristic Drift Elimination for indoor pedestrian navigation;Ho Jin Ju,Min Su Lee,Chan Gook Park,Soyeon Lee,Sangjoon;《International Conference on Indoor Positioning and Indoor Navigation》;20141030;729-732 * |
基于主方向的行人自主定位航向修正算法;赵辉,李擎,李超;《计算机技术与应用》;20161130;第42卷(第11期);108-111 * |
Also Published As
Publication number | Publication date |
---|---|
CN110514226A (zh) | 2019-11-29 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
CN110243358B (zh) | 多源融合的无人车室内外定位方法及系统 | |
CN108535755B (zh) | 基于mems的gnss/imu车载实时组合导航方法 | |
CN109931926B (zh) | 一种基于站心坐标系的小型无人机无缝自主式导航方法 | |
CN111024064B (zh) | 一种改进Sage-Husa自适应滤波的SINS/DVL组合导航方法 | |
CN110706279A (zh) | 基于全局地图与多传感器信息融合的全程位姿估计方法 | |
CN113124856B (zh) | 基于uwb在线锚点的视觉惯性紧耦合里程计及计量方法 | |
CN108896044B (zh) | 一种基于惯性导航和卫星导航的定位方法及装置 | |
CN112505737B (zh) | 一种gnss/ins组合导航方法 | |
CN114018242A (zh) | 一种基于偏振/太阳/惯性信息智能匹配的自主定姿方法 | |
CN113029139A (zh) | 基于运动检测的机场飞行区车辆差分北斗/sins组合导航方法 | |
CN114166221A (zh) | 动态复杂矿井环境中辅助运输机器人定位方法及系统 | |
CN110514226B (zh) | 一种行人惯性导航的航向校正方法 | |
CN114216459B (zh) | Elm辅助的gnss/ins组合导航无人靶车定位方法 | |
CN113110556B (zh) | 一种基于视觉传感器的无人机位置估计系统及估计方法 | |
CN114485623B (zh) | 一种聚焦距离的相机-imu-uwb融合精准定位方法 | |
CN114088104B (zh) | 一种自动驾驶场景下的地图生成方法 | |
CN114915913A (zh) | 一种基于滑窗因子图的uwb-imu组合室内定位方法 | |
CN114894180A (zh) | 基于相对导航信息的多源融合导航方法及系统 | |
CN114646993A (zh) | 一种基于gnss、视觉以及imu进行精确定位的数据融合算法 | |
Noureldin et al. | a Framework for Multi-Sensor Positioning and Mapping for Autonomous Vehicles | |
CN113483752A (zh) | 一种基于加速度匹配的航向快速初始化方法、电子设备、存储介质 | |
CN115420288B (zh) | 一种轻量地图信息鲁棒约束pdr的室内定位方法 | |
Luo et al. | An imu/visual odometry integrated navigation method based on measurement model optimization | |
CN113899368B (zh) | 一种基于室内指纹点拓扑关系的行人航向校正方法 | |
An et al. | A robust visual-aided inertial navigation algorithm for pedestrians |
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 | ||
GR01 | Patent grant | ||
GR01 | Patent grant |