CN113029148A - 一种基于航向角准确修正的惯性导航室内定位方法 - Google Patents
一种基于航向角准确修正的惯性导航室内定位方法 Download PDFInfo
- Publication number
- CN113029148A CN113029148A CN202110247534.4A CN202110247534A CN113029148A CN 113029148 A CN113029148 A CN 113029148A CN 202110247534 A CN202110247534 A CN 202110247534A CN 113029148 A CN113029148 A CN 113029148A
- Authority
- CN
- China
- Prior art keywords
- value
- angle
- peak
- straight
- course
- 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
Links
- 238000000034 method Methods 0.000 title claims abstract description 49
- 238000012937 correction Methods 0.000 title claims abstract description 39
- 238000001514 detection method Methods 0.000 claims abstract description 37
- 230000001133 acceleration Effects 0.000 claims abstract description 22
- 238000001914 filtration Methods 0.000 claims description 10
- 238000004364 calculation method Methods 0.000 claims description 5
- 230000005484 gravity Effects 0.000 claims description 5
- 238000007781 pre-processing Methods 0.000 claims description 5
- 238000012360 testing method Methods 0.000 claims description 5
- 238000005070 sampling Methods 0.000 claims description 4
- 239000011159 matrix material Substances 0.000 claims description 3
- 238000012545 processing Methods 0.000 claims description 3
- 238000012549 training Methods 0.000 claims description 3
- 238000000605 extraction Methods 0.000 claims description 2
- 238000013500 data storage Methods 0.000 claims 1
- 238000013507 mapping Methods 0.000 claims 1
- 238000007670 refining Methods 0.000 abstract 1
- 238000005516 engineering process Methods 0.000 description 7
- 238000010586 diagram Methods 0.000 description 6
- 230000008030 elimination Effects 0.000 description 3
- 238000003379 elimination reaction Methods 0.000 description 3
- 238000011160 research Methods 0.000 description 3
- 238000004458 analytical method Methods 0.000 description 2
- 238000005259 measurement Methods 0.000 description 2
- 235000018734 Sambucus australis Nutrition 0.000 description 1
- 244000180577 Sambucus australis Species 0.000 description 1
- 238000009825 accumulation Methods 0.000 description 1
- 238000004891 communication Methods 0.000 description 1
- 238000011161 development Methods 0.000 description 1
- 230000000694 effects Effects 0.000 description 1
- 238000002474 experimental method Methods 0.000 description 1
- 230000001788 irregular Effects 0.000 description 1
- 230000002427 irreversible effect Effects 0.000 description 1
Images
Classifications
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
- G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
- G01C21/20—Instruments for performing navigational calculations
- G01C21/206—Instruments for performing navigational calculations specially adapted for indoor navigation
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
- G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
- G01C21/10—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration
- G01C21/12—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning
- G01C21/16—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation
- G01C21/165—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation combined with non-inertial navigation instruments
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
- G01S19/00—Satellite radio beacon positioning systems; Determining position, velocity or attitude using signals transmitted by such systems
- G01S19/38—Determining a navigation solution using signals transmitted by a satellite radio beacon positioning system
- G01S19/39—Determining a navigation solution using signals transmitted by a satellite radio beacon positioning system the satellite radio beacon positioning system transmitting time-stamped messages, e.g. GPS [Global Positioning System], GLONASS [Global Orbiting Navigation Satellite System] or GALILEO
- G01S19/42—Determining position
- G01S19/45—Determining position by combining measurements of signals from the satellite radio beacon positioning system with a supplementary measurement
- G01S19/47—Determining position by combining measurements of signals from the satellite radio beacon positioning system with a supplementary measurement the supplementary measurement being an inertial measurement, e.g. tightly coupled inertial
Landscapes
- Engineering & Computer Science (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Automation & Control Theory (AREA)
- Computer Networks & Wireless Communication (AREA)
- Navigation (AREA)
Abstract
本发明公开了一种基于航向角准确修正的惯性导航室内定位方法,本发明利用IMU器件,在传统HDE及PDR算法基础上,基于精细化的主方向设定、是否是直行以及是否沿建筑物主方向直行的区分、反馈系数的不同修正、峰峰值探测法、动态递推算法等,实现复杂室内环境对行人的准确定位。精细化定义16个主方向,采用三个相邻步间的航向变化来检测直线行走和转弯对是否是直行,以及是否沿建筑物主方向直行进行区分,并基于判决结果对反馈系数做相应修正,实现航向角准确估计。提升了航向角的解算精度。通过提出的峰峰值探测法,能在上下楼梯等复杂室内环境中,克服和加速度波形关于X轴不对称问题,降低了步频检测误差,对和加速度数据进行分析处理,避免了对航向角过度修正,准确解算出定位数据,满足对行人进行实时定位跟踪的需求。
Description
技术领域
本发明涉及室内定位技术领域,尤其是基于惯性导航器件的行人室内导航定位方法。
背景技术
近年来,各种室内环境基于位置的服务受到了广泛关注,如何在室内环境实现高精度的行人定位导航已成为一个研究热点。当今城市各种建筑物内部结构错综复杂,GPS/北斗等全球卫星定位导航系统在室内环境一般缺乏有效信号覆盖,很难进行准确、有效的定位。基于WiFi、UWB、ZigBee、RFID等设备的室内定位技术一般都需要提前在建筑物内部署信号源等基础设施,难以在没有基础设施支撑的大量一般建筑物内实施实时精准定位。惯性导航定位技术是一种可以全天候、全时段、不停歇工作,且工作时不依赖事先部署基础设施,不易受到外界干扰的一种自主式定位导航技术。近年来随着微电机(MEMS)技术的发展,基于MEMS惯性测量单元(IMU)的体积越来越小,测量精度越来越高,性能愈来愈强,使得基于IMU的室内定位技术的研究及应用受到了更多的关注。基于IMU进行三维室内定位,需要实时采集行人行走过程中的三轴加速度、三轴角速度、高度数据,进而计算出步频、步长以及航向角,用于PDR定位。因此步频检测、步长估计和航向估计的精确度决定了IMU定位性能的优劣。
文献1:王鹏宇,孟之栋,邓志红.腰绑式行人导航系统基于缓存区的启发式航向补偿算法研究[J].导航定位与授时2020,7(03):150-156.。为避免航向角过度修正,动态改变反馈系数来调整航向补偿算法的修正强度。
文献2:A.R.Jiménez,F.Seco,F.Zampella,J.C.Prieto and J.Guevara,"Improved Heuristic Drift Elimination(iHDE)for pedestrian navigation incomplex buildings,"2011 International Conference on Indoor Positioning andIndoor Navigation,Guimaraes,2011,pp.1-8,doi:10.1109/IPIN.2011.6071923.基于脚踏式惯导模块和扩展卡尔曼滤波,改进的启发式漂移消除算法,用于复杂的建筑中行人导航。
目前步频检测采用的方法主要有峰值检测法,零点交叉法等,但这类方法在上下楼梯时会产生较大的步频检测误差。
文献3:申请号为CN202010819613.3,发明名称为一种基于改进式HDE的室内行人导航航向修正方法及系统。(1)基于启发式偏移消除算法为中心,设定8个主方向,主方向间的角度间隔Δ为45°。(2)使用航向角差值、角速率变化规律等进行运动类型分类,构建主方向航向偏移差值、角速率差值、位置差值作为状态量进行扩展卡尔曼滤波(EKF),得到航向的最优估计从而修正航向。
文献4:申请号为CN201810230263.X,发明名称为一种行人室内轨迹定位方法。(1)基于主方向进行所述航向角修正:走直线时,利用启发式随机漂移消除法进行航向角修正;走曲线(非直线)时,利用扩展卡尔曼滤波器进行偏移补偿,修正航向角。(2)在启发式随机漂移消除算法的基础上,设定“米”字形的8个主方向,采用三个相邻步间的航向变化来检测直线行走和转弯。
但在航向角的估计过程中由于陀螺仪自身不可避免会出现数据漂移,航向角会随着时间累计而产生不可逆的误差,且现代室内建筑主方向不仅仅是横平竖直,诸如环形、弧形等多种不规则转弯方向使得室内行人行走路径转向角度多样化,传统HDE算法的航向角估计精度已不能满足室内精确定位导航的需求。
发明内容
鉴于现有技术的以上情况,本发明的目的在于提出一种基于航向角准确修正的惯性导航室内定位方法,利用IMU器件,在传统HDE及PDR算法基础上,基于精细化的主方向设定、是否是直行以及是否沿建筑物主方向直行的区分、反馈系数的不同修正、峰峰值探测法、动态递推算法等,实现复杂室内环境对行人的准确定位。精细化定义主方向,采用三个相邻步间的航向变化来检测直线行走和转弯对是否是直行,以及是否沿建筑物主方向直行进行区分,并基于判决结果对反馈系数做相应修正,实现航向角准确估计,在上下楼梯等复杂室内环境中,克服和加速度波形关于X轴不对称问题,降低步频检测误差,对和加速度数据进行分析处理,避免对航向角过度修正,准确解算出定位数据,满足对行人进行实时定位跟踪的需求。
实现本发明目的的技术方案如下:
1、一种基于航向角准确修正的惯性导航室内定位方法,包括以下步骤:
A、初始位置与初始航向角的确定:在建筑物入口通过GPS进行初始位置确定,并分别进行两点位置坐标数据采集,获取初始位置以及初始航向角;
B、对采集的三轴和加速度进行去重力、低通滤波等数据预处理;
C、步频检测:采用峰峰值探测法,首先在三窗式滑动窗口中对和加速度进行波峰波谷检测,并分别存放在于队列中;然后分别依次获取一个波峰、波谷并用横向时间阈值和纵向峰峰值幅值进行有效步频检测;
D、步长估计:采用Weinberg等人提出的非线性的模型:
式中,k为跟行人身高、步频等参数有关的常数,通过训练来获得;Amax、Amin分别是波峰、波谷队列中存储的每一步的加速度的最大值和最小值;
E、航向角估计:使用的九轴惯性导航器件能够直接实时输出运载体坐标系下的三轴矢量角速度和四元数q并更新;首先将陀螺仪数据进行坐标系转换得到导航坐标系下的角速度然后在步骤A得到的初始方位角ψ0的基础上,进行角速度积分运算,得到当前航向在下一个周期中需要更新ψ0=ψ;为载体坐标系到导航坐标系下的姿态旋转矩阵;
F、航向角修正:室内航向标定为16个主方向,主方向间隔Δ=22.5°;计算前一时刻航向角ψi-1相对于当前主方向的偏移角以及偏移方向;接着计算偏移方向值Ei,通过判断Ei的正负来决定反馈变量Ii;通过增强算法得到实时修正后的航向角ψi=ψ'i+Ii;
G、航向角的进一步修正;在所得实时修正后的航向角的基础上,通过对行走的路线进行直行和非直行两种路径类型判别,在直线行走下再区分沿着建筑主方向行走和非建筑主方向行走路径,然后分别使用不同的反馈系数ic,以降低系统对修正航向角的反馈变量的Ii的敏感性,达到航向角准确修正目的;
H、位置推算:通过步频探测、步长估计得到每一步步长d,航向角估计值ψ'i以及修正得到航向角ψ后,推算得到当前三维位置坐标(Xk,Yk,Zk):
采用本发明的方法,在传统HDE及PDR算法基础上,基于精细化的主方向设定、是否是直行以及是否沿建筑物主方向直行的区分、反馈系数的不同修正、峰峰值探测法、动态递推算法等,实现复杂室内环境对行人的准确定位。精细化定义主方向,采用三个相邻步间的航向变化来检测直线行走和转弯对是否是直行,以及是否沿建筑物主方向直行进行区分,并基于判决结果对反馈系数做相应修正,实现了航向角准确估计,在上下楼梯等复杂室内环境中,克服了和加速度波形关于X轴不对称问题,降低步频检测误差,对和加速度数据进行分析处理,避免对航向角过度修正,准确解算出定位数据,满足对行人进行实时定位跟踪的需求。
附图说明
图1本发明方法主要步骤的工作框图。
图2为初始位置和初始航向角确定示意图。
图3为基于三窗式滑动窗口的峰峰值探测图
图4为航向角准确修正算法示意图。
图5为主方向的设定图。
图6为直行和转弯时的航向角示意图。
图7为主方向的偏移角度示意图。
图8为行人航迹推算示意图。
图9便携式设备穿戴示意图。
图10多楼层行的3d轨迹示意图。
具体实施方式
下面结合附图对本发明方法作进一步地详述。
本发明一种基于航向角准确修正的惯性导航室内定位方法,包括以下步骤:
一种基于航向角准确修正的室内惯性导航定位方法,包括以下步骤:
A、初始位置与初始航向角的确定:如图2所示,在建筑物入口外通过GPS进行初始位置确定,获取初始位置(x0,y0,z0);通过行两个位置坐标数据采集,由两点连线和正北方向的夹角作为初始航向角ψ0。
B、对采集的三轴和加速度进行去重力、低通滤波等数据预先处理,以便后续精准的步频探测。
C、步频检测:如图3所示,采用峰峰值探测法,首先在三窗式滑动窗口中进行波峰波谷检测并分别存放在于队列中,然后分别依次获取一个波峰、波谷并用横向时间阈值和纵向峰峰值幅值进行有效步频检测。
D、步长估计:采用Weinberg等人提出的非线性的模型:
式中,k为跟身高、步频等参数有关的常数,可通过训练来获得;Amax、Amin分别是波峰、波谷队列中存储的每一步的加速度的最大值和最小值。
E、航向角估计:本专利使用的惯性导航器件能够直接实时输出运载体坐标系下的三轴矢量角速度和四元数q并更新。故首先将陀螺仪数据进行坐标系转换得到导航坐标系下的角速度然后在步骤A得到的初始方位角ψ0的基础上,进行角速度积分运算,得到当前航向在下一个周期中需要更新ψ0=ψ。载体坐标系到导航坐标系下的姿态旋转矩阵为:
F、航向角的修正:通过如图4所示航向角准确修正算法,有效地减小定位模块在室内长时间工作所产生的累计误差。首先建立坐标系,将室内航向标定为16个主方向,主方向间隔Δ=22.5°;然后计算前一时刻航向角ψi-1相对于当前主方向的偏移角以及偏移方向;最后计算正负偏移方向值Ei,通过判断Ei的正负来决定反馈变量Ii。最后,得到实时修正后的航向角ψi=ψ'i+Ii。
G、航向角的进一步修正:为了达到航向角准确修正的目的,在此基础上,首先对行走的路线进行了直行和转弯两种路径类型判别;然后区分直行和转弯路径以及在直线行走下区分沿着建筑主方向行走和非建筑主方向行走路径;最后使用不同的反馈系数ic来降低系统对用来修正航向角的反馈变量的Ii的敏感性,从而达到准确修正的目的。图4中,ψ为航向角、Ii反馈变量、ic为反馈系数、MOD(ψi-1,Δ)是用来计算前一时刻航向角相对于当前主方向的偏移角,定义为:
MOD(n,m)=n-m×INT(n/m) (3)
其中,INT(k)为取整函数,结果为不大于k值的最大整数,所以MOD函数的结果在区间[0,m]内。
因此,可得反馈变量Ii=Ii-1+ic×sgn(Ei),最后得到实时修正后的航向角ψi=ψ'i+Ii。经大量测试确定的ic取值为:反馈系数初始值取0.01,在非直线行走时取0.001,沿着非建筑物主方向行走时取0.0001,航向修正效果最佳。
1)主方向的设定
如图5所示,为本文设定的16个主方向,主方向的角度间隔Δ=22.5°。将主导方向细分为16个方向是为了应对长时间沿着非主方向行走或者持续小角度转弯行走的情况,以减缓HDE算法对角度的过度修正,增强算法适应各种不同室内道路环境的能力。
2)直行的判决
惯性导航模块佩戴于腰部,航向角曲线在直行和转弯时有着明显的特征。图6是算法测试时行走的两段转弯和三段直行的航向角曲线图,其中星号点为直行每一步的航向角,圆圈点为转弯每一步的航向角。
对行走中的航向角进行时域特征提取并分类。假定连续三步的航向角数值分别为ψi-2、ψi-1、ψi,基于其差值可分为4类,对不同类别及其组合进行直行判决。
第一类为T1直行类,可采用三步航向数值分别进行差分相乘,得到判决条件C1=(ψi-ψi-1)×(ψi-1-ψi-2),当C1<0时,则判决当前在直行;
第二类为T2直行变转弯类,航向角ψi仍满足条件C1<0,但是此时已经处于转弯状态,显然发生了误判,此时可对相邻的航向角数值进行差分求绝对值,得到判决条件C2=|ψi-ψi-1|,当同时满足C1<0和C2<θ时,则可以防止误判发生;
第三类为T3转弯类,可用连续的三步航向数值两两差分取绝对值后,得判决条件C3=(|ψi-1-ψi-2|<βand|ψi-ψi-1|<β),当C3为true成立时,可判决当前在走直线,反之则在转弯;
第四类为T4转弯变直行类,当航向角ψi满足C1<0时,即可判断当前在直行。是否直行可按式(5)进行判断。
当S1为1的时判定为直线行走,可以进入下一项主方向直行判决;当S1为0时为转弯,需修正反馈系数ic。上述判决门限θ和β为相邻两航向角差值的门限阈值,一般取经验值15°和10°。
3)主方向直行的判决
对于直行前提下还需要判断是否是沿着建筑物间隔90°的4个主方向之一直行,先选择4个建筑物主方向,其角度间隔Δ=90°。在图5中设定的16个主方向中,一方面22.5°的倍数中虽包含了4个建筑物主方向,但还有12个非建筑物主方向,需要加以区分;另一方面正常行走过程中航向值波动在正负10°左右,在建筑主方向间隔Δ=22.5°上进行主方向直行判别,极容易发生误判。
可对连续的三步航向角数值求出其均值后模360°得到一个角度值μ,再把μ映射到第一象限中去,最后选择与象限轴夹角最小的角度σ=min(μ,90°-μ),如图7所示。筑物主方向直行判决公式如式所示。
当S2为1时判定为在四个间隔90°的建筑物主方向之一上直行,不用修正反馈系数ic;当S2为0时判定为在非建筑物主方向上直行,修正反馈系数ic,γ为判断主方向预设的最大角度阈值,经验值为10°。
H、位置推算:通过步频探测、步长估计得到每一步步长d,航向角估计值ψ'i以及修正得到航向角ψ后,可进行如图8所示的行人航迹推算,采用公式(7)由前一位置推算得到行人的当前位置坐标(Xk,Yk,Zk)。
其中Vk为第k-步和k-1步的高度差值,可以由两步间气压差Δρ通过下式得到。
其中,ρk表示k时刻行人所处位置的大气压力值,ρ0表示标准大气压。
还需要说明的是,为提高求算的精确度,本发明从步骤B至步骤F的数据解算过程是在一个队列式动态递推算法中进行,该算法能够实现数据近实时地被处理、分析。算法过程描述如下:
(1)给定一个固定长度为N的队列Q,分为三个窗口,用于存放和加速度数据;
(2)先填充N/3冗余数据进队首,为后续滤波做准备;
(3)队列中N个数据填满后做低通滤波、去重力等数据预处理;
(4)进行峰峰值探测,获取N/3~2N/3区间探测的步频、步长和航向角等数据;
(5)队首N/3个数据出队列,队尾进N/3个数据后,重复第3~4步操作;
在步骤G中,动态递推算法使用的队列长度N是3倍的惯导数据采集频率。
实施例
实验场地:西南交通大学9号教学楼的负一层至四层。
实验设备:基于本方法设计的定位设备,穿戴方式如图9所示。定位设备硬件中惯性导航器件的数据采样速率设置为200Hz,通信波特率为460800Hz,工作电压为3.3V。定位设备软件程序中,步频检测的横向时间阈值预设为10个采样点,纵向峰峰值阈值预设为2.3;步长估计模型参数k值采用离线测试获取,预设行走k值为0.4332,奔跑k值为0.4792;航向角准确修正算法中主方向间隔预设Δ为22.5°,相邻两航向角差值的门限阈值θ和β预设为15°和10°。判断主方向阈值γ预设为10°。
实验内容:定位设备穿戴好后,如图2所示,首先正面对着教学楼1楼入口站立,上电开启设备并进行初始方位和初始航向角的确定;然后进入1楼,下楼梯进入地下室,先在地下室绕墙柱环绕行走一圈,接着爬楼梯到地上四层每一层依次行走一圈,最后下楼梯返回出发点,全程行走大约1123米。在行走过程中穿戴设备实时按照图1所示的流程进行步频检测、步长估计、航向估计、航向角准确修正以及高度数据采集,在得到每一步的步长、航向角及高度数据后,利用PDR算法进行位置推算。基于位置推算的结果可画出整个行程的行走轨迹,并计算、统计定位误差。
实验目的:模拟复杂室内行走场景,在地下室绕柱行走、爬4层楼梯及途中转弯都是沿着非主方向行走,以检验本发明方法对复杂室内场景的适应性。
实验结果:由定位设备采集数据推算的行走过程三维坐标数据绘制的行走轨迹如图10所示,与教学楼楼道结构及地下室结构准确吻合。实验测得起始点和终点的首尾水平误差仅为0.77米,与总航程的相对误差为0.07%,三维相对定位误差为0.17%,与现有方法相比取得了更好的定位性能。
与现有室内定位技术相比,本发明具有明显的优点:
一、与基于WiFi、UWB、ZigBee、蓝牙的室内定位技术相比,本发明无需在待定位场所事先部署基站、锚节点等基础设施,适合在任意建筑物室内及隧道、矿井等环节中对人员的定位。
二、与现有室内惯性导航定位方法相比,本发明提出的设计峰峰值探测法,能在上下楼梯等复杂室内环境中,解决和加速度波形关于X轴不对称问题,降低了步频检测误差。
三、本发明在航向角解算过程中,将主方向精细化为16个,能适应建筑物内转角、转弯的各种情况,最大程度兼容室内建筑风格,保证了航向角解算的准确性和可靠性。
四、本发明在航向角解算过程中,对是否是直行,以及是否沿建筑物主方向直行进行了区分,设计了相应的判决条件,并基于判决结果对反馈系数做相应修正,提升了航向角的解算精度。
本发明在数据解算过程中,采用动态递推算法,对和加速度数据进行分析处理,准确解算出定位数据,满足对行人进行实时定位跟踪的需求。
尽管上面对本发明说明性的具体实施方式进行了描述,以便本技术领域的技术人员理解本发明,但应该清楚,本发明不限于具体实施方式的范围,只要各种变化在本发明所附权利要求限定和精神范围内,这些变化是显而易见的,一切利用本发明构思的发明创造均在保护之列。
Claims (8)
1.一种基于航向角准确修正的惯性导航室内定位方法,包括以下步骤:
A、初始位置与初始航向角的确定:在建筑物入口通过GPS进行初始位置确定,并分别进行两点位置坐标数据采集,获取初始位置以及初始航向角;
B、对采集的三轴和加速度进行去重力、低通滤波等数据预处理;
C、步频检测:在三窗式滑动窗口中采用峰峰值探测法进行检测;首先在三窗式滑动窗口中对和加速度进行波峰波谷检测,并分别存放在于队列中;然后分别依次获取一个波峰、波谷并用横向时间阈值和纵向峰峰值幅值进行有效步频检测;
D、步长估计:采用Weinberg等人提出的非线性的模型:
式中,k为跟行人身高、步频等参数有关的常数,通过训练来获得;Amax、Amin分别是波峰、波谷队列中存储的每一步的加速度的最大值和最小值;
E、航向角估计:使用的九轴惯性导航器件,能够直接实时输出运载体坐标系下的三轴矢量角速度和四元数q并更新;首先将陀螺仪数据进行坐标系转换得到导航坐标系下的角速度然后在步骤A得到的初始方位角ψ0的基础上,进行角速度积分运算,得到当前航向在下一个周期中需要更新ψ0=ψ;为载体坐标系到导航坐标系下的姿态旋转矩阵;
F、航向角修正:室内航向标定为16个主方向,主方向间隔Δ=22.5°;计算前一时刻航向角ψi-1相对于当前主方向的偏移角以及偏移方向;接着计算偏移方向值Ei,通过判断Ei的正负来决定反馈变量Ii;通过增强算法得到实时修正后的航向角ψi=ψ′i+Ii;
G、航向角的进一步修正:在所得实时修正后的航向角的基础上,通过对行走的路线进行直行和非直行两种路径类型判别,在直线行走下再区分沿着建筑主方向行走和非建筑主方向行走路径,然后分别使用不同的反馈系数ic,以降低系统对修正航向角的反馈变量的Ii的敏感性,达到航向角准确修正目的;
H、位置推算:通过步频探测、步长估计得到每一步步长d,航向角估计值ψ′i以及修正得到航向角ψ后,推算得到当前三维位置坐标(Xk,Yk,Zk):
2.根据权利要求1所述的基于航向角准确修正的惯性导航室内定位方法,其特征在于,步骤C中,三窗式滑动窗口为一存储和加速度等数据的队列,将其均等分成三段即三个窗口,选择中间一个窗口数据进行峰值特征提取,而左右两个窗口只参与滤波及数据保存;所述峰峰值探测法是利用横向时间阈值和纵向峰峰值幅值进行加速度波形的波峰值和波谷值的识别、提取;行人常规行走时,每步大概在50~60个采样点之间,横向时间阈值典型值为55;跑的情况下,每步大概在10~40个采样点之间,横向时间阈值典型值为25;纵向峰峰值幅值即波峰值和波谷的绝对值差值大于2.3。
5.根据权利要求1所述的基于航向角准确修正的惯性导航室内定位方法,其特征在于,步骤G中,为了达到对航向角进一步准确修正的目的,首先对行走的路径进行直行和非直行两种路径类型判别,在直线行走下再区分沿着建筑物主方向行走和非建筑物主方向行走,然后分别使用不同的反馈系数ic进行修正;
ic取值为:反馈系数初始值取0.01,在非直线行走时取0.001,沿着非建筑物主方向行走时取0.0001。
6.根据权利要求5所述的基于航向角准确修正的惯性导航室内定位方法,其特征在于,是否直行判决方法为:
首先对行走过程中的航向角进行时域特征提取并分类,假定连续三步的航向角数值分别为ψi-2、ψi-1、ψi,基于其差值可分为四类,对不同类别及其组合进行直行判决:
第一类为T1直行类,采用三步航向数值分别进行差分相乘,得到判决条件C1=(ψi-ψi-1)×(ψi-1-ψi-2),当C1<0时,则判决当前在直行;
第二类为T2直行变转弯类,航向角ψi仍满足条件C1<0,对相邻的航向角数值进行差分求绝对值,得到判决条件C2=|ψi-ψi-1|,当同时满足C1<0和C2<θ时,则可以防止误判发生;
第三类为T3转弯类,可、用连续的三步航向数值两两差分取绝对值后,得判决条件C3=(|ψi-1-ψi-2|<βand|ψi-ψi-1|<β),当C3为true成立时,判决当前在走直线,反之则在转弯;
第四类为T4转弯变直行类,当航向角ψi满足C1<0时,即可判断当前在直行,是否直行由下式进行判断:
当S1为1的时判定为直线行走,可以进入下一项主方向直行判决;当S1为0时为转弯,需修正反馈系数ic;上述判决门限θ和β为相邻两航向角差值的门限阈值,取经验值15°和10°。
8.根据权利要求1的基于航向角准确修正的惯性导航室内定位方法,其特征在于,
所述步骤B至步骤F的数据解算是基于下述动态递推算法,算法处理过程描述如下:
(1)给定一个固定长度为N的队列Q,分为三个窗口,用于存放和加速度数据;
(2)先填充N/3冗余数据进队首,为后续滤波做准备;
(3)队列中N个数据填满后做低通滤波、去重力等数据预处理;
(4)进行峰峰值探测,获取N/3~2N/3区间探测的步频、步长和航向角等数据;
(5)队首N/3个数据出队列,队尾进N/3个数据后,重复第3~4步操作;
所述步骤G中,动态递推算法使用的队列长度N是3倍的惯导数据采集频率。
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202110247534.4A CN113029148A (zh) | 2021-03-06 | 2021-03-06 | 一种基于航向角准确修正的惯性导航室内定位方法 |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202110247534.4A CN113029148A (zh) | 2021-03-06 | 2021-03-06 | 一种基于航向角准确修正的惯性导航室内定位方法 |
Publications (1)
Publication Number | Publication Date |
---|---|
CN113029148A true CN113029148A (zh) | 2021-06-25 |
Family
ID=76468506
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN202110247534.4A Pending CN113029148A (zh) | 2021-03-06 | 2021-03-06 | 一种基于航向角准确修正的惯性导航室内定位方法 |
Country Status (1)
Country | Link |
---|---|
CN (1) | CN113029148A (zh) |
Cited By (3)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN113654573A (zh) * | 2021-10-15 | 2021-11-16 | 成都云智北斗科技有限公司 | 地面机动载体组合导航系统粗对准方法及其设备 |
CN113867387A (zh) * | 2021-09-27 | 2021-12-31 | 中国航空无线电电子研究所 | 一种无人机自主着降航向识别方法 |
CN115235455A (zh) * | 2022-09-19 | 2022-10-25 | 中国人民解放军国防科技大学 | 一种基于智能手机pdr与视觉修正的行人定位方法 |
Citations (3)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN202975337U (zh) * | 2012-12-27 | 2013-06-05 | 山东大学 | 一种消防员室内外3d无缝定位及姿态检测系统 |
CN104713554A (zh) * | 2015-02-01 | 2015-06-17 | 北京工业大学 | 一种基于mems惯性器件与安卓智能手机融合的室内定位方法 |
CN111912406A (zh) * | 2020-08-14 | 2020-11-10 | 航天科工通信技术研究院有限责任公司 | 一种基于改进式hde的室内行人导航航向修正方法及系统 |
-
2021
- 2021-03-06 CN CN202110247534.4A patent/CN113029148A/zh active Pending
Patent Citations (3)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN202975337U (zh) * | 2012-12-27 | 2013-06-05 | 山东大学 | 一种消防员室内外3d无缝定位及姿态检测系统 |
CN104713554A (zh) * | 2015-02-01 | 2015-06-17 | 北京工业大学 | 一种基于mems惯性器件与安卓智能手机融合的室内定位方法 |
CN111912406A (zh) * | 2020-08-14 | 2020-11-10 | 航天科工通信技术研究院有限责任公司 | 一种基于改进式hde的室内行人导航航向修正方法及系统 |
Non-Patent Citations (1)
Title |
---|
邓平 等: ""一种基于航向角准确修正的惯性导航室内定位算法"", 《中国惯性技术学报》 * |
Cited By (6)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN113867387A (zh) * | 2021-09-27 | 2021-12-31 | 中国航空无线电电子研究所 | 一种无人机自主着降航向识别方法 |
CN113867387B (zh) * | 2021-09-27 | 2024-04-12 | 中国航空无线电电子研究所 | 一种无人机自主着降航向识别方法 |
CN113654573A (zh) * | 2021-10-15 | 2021-11-16 | 成都云智北斗科技有限公司 | 地面机动载体组合导航系统粗对准方法及其设备 |
CN113654573B (zh) * | 2021-10-15 | 2022-03-08 | 成都云智北斗科技有限公司 | 地面机动载体组合导航系统粗对准方法及其设备 |
CN115235455A (zh) * | 2022-09-19 | 2022-10-25 | 中国人民解放军国防科技大学 | 一种基于智能手机pdr与视觉修正的行人定位方法 |
CN115235455B (zh) * | 2022-09-19 | 2023-01-13 | 中国人民解放军国防科技大学 | 一种基于智能手机pdr与视觉修正的行人定位方法 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
CN113029148A (zh) | 一种基于航向角准确修正的惯性导航室内定位方法 | |
CN107289941B (zh) | 一种基于惯导的室内定位方法与装置 | |
CN106017463B (zh) | 一种基于定位传感装置的飞行器定位方法 | |
CA3029940C (en) | System and method for localizing a trackee at a location and mapping the location using transitions | |
EP2909582B1 (en) | Map-assisted sensor-based positioning of mobile devices | |
CN105509738B (zh) | 基于惯导/多普勒雷达组合的车载定位定向方法 | |
CN113945206A (zh) | 一种基于多传感器融合的定位方法及装置 | |
CN104061934A (zh) | 基于惯性传感器的行人室内位置跟踪方法 | |
CN104180805A (zh) | 基于智能手机的室内行人定位与跟踪方法 | |
CN109648558B (zh) | 机器人曲面运动定位方法及其运动定位系统 | |
CN107218938A (zh) | 基于人体运动模型辅助的穿戴式行人导航定位方法和设备 | |
KR20170060034A (ko) | 맵 정보 보조 향상된 휴대가능 내비게이션을 이용하기 위한 방법 및 장치 | |
CN111024075B (zh) | 一种结合蓝牙信标和地图的行人导航误差修正滤波方法 | |
CN105241454A (zh) | 一种基于多传感器的行人导航系统及导航方法 | |
CN107014375B (zh) | 超低部署的室内定位系统及方法 | |
CN108195376A (zh) | 小型无人机自主导航定位方法 | |
CN105865450A (zh) | 一种基于步态的零速更新方法及系统 | |
JP2013531781A (ja) | 対象物のゼロ速度状態の検出方法及びシステム | |
CN109612463B (zh) | 一种基于侧向速度约束优化的行人导航定位方法 | |
Zheng et al. | An optimization-based UWB-IMU fusion framework for UGV | |
Zhang et al. | Pedestrian motion based inertial sensor fusion by a modified complementary separate-bias Kalman filter | |
CN115574816B (zh) | 仿生视觉多源信息智能感知无人平台 | |
CN107677267A (zh) | 基于mems‑imu的室内行人导航航向反馈修正方法 | |
Gao et al. | An integrated land vehicle navigation system based on context awareness | |
CN112362057A (zh) | 基于零速修正与姿态自观测的惯性行人导航算法 |
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 | ||
WD01 | Invention patent application deemed withdrawn after publication |
Application publication date: 20210625 |
|
WD01 | Invention patent application deemed withdrawn after publication |