CN109855621B - 一种基于uwb与sins的组合室内行人导航***及方法 - Google Patents
一种基于uwb与sins的组合室内行人导航***及方法 Download PDFInfo
- Publication number
- CN109855621B CN109855621B CN201811607679.5A CN201811607679A CN109855621B CN 109855621 B CN109855621 B CN 109855621B CN 201811607679 A CN201811607679 A CN 201811607679A CN 109855621 B CN109855621 B CN 109855621B
- Authority
- CN
- China
- Prior art keywords
- filter
- speed
- sub
- zero
- output
- 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
- 238000000034 method Methods 0.000 title claims abstract description 50
- 238000005259 measurement Methods 0.000 claims abstract description 80
- 230000033001 locomotion Effects 0.000 claims abstract description 76
- 230000001133 acceleration Effects 0.000 claims abstract description 56
- 238000001514 detection method Methods 0.000 claims abstract description 40
- 230000004927 fusion Effects 0.000 claims abstract description 19
- 238000001914 filtration Methods 0.000 claims abstract description 11
- 238000012937 correction Methods 0.000 claims abstract description 8
- 239000011159 matrix material Substances 0.000 claims description 69
- 238000004364 calculation method Methods 0.000 claims description 20
- 230000008569 process Effects 0.000 claims description 19
- 230000005540 biological transmission Effects 0.000 claims description 9
- 238000005516 engineering process Methods 0.000 abstract description 11
- 238000003657 Likelihood-ratio test Methods 0.000 description 4
- 238000004891 communication Methods 0.000 description 4
- 230000007704 transition Effects 0.000 description 4
- 238000012360 testing method Methods 0.000 description 3
- 238000010586 diagram Methods 0.000 description 2
- 230000000694 effects Effects 0.000 description 2
- 238000009434 installation Methods 0.000 description 2
- 238000000638 solvent extraction Methods 0.000 description 2
- 238000013179 statistical model Methods 0.000 description 2
- 230000026676 system process Effects 0.000 description 2
- 239000004165 Methyl ester of fatty acids Substances 0.000 description 1
- 238000004458 analytical method Methods 0.000 description 1
- 230000009286 beneficial effect Effects 0.000 description 1
- 238000011161 development Methods 0.000 description 1
- 238000007499 fusion processing Methods 0.000 description 1
- 230000005021 gait Effects 0.000 description 1
- 238000012986 modification Methods 0.000 description 1
- 230000004048 modification Effects 0.000 description 1
- 230000000630 rising effect Effects 0.000 description 1
- 239000013589 supplement Substances 0.000 description 1
Images
Classifications
-
- Y—GENERAL TAGGING OF NEW TECHNOLOGICAL DEVELOPMENTS; GENERAL TAGGING OF CROSS-SECTIONAL TECHNOLOGIES SPANNING OVER SEVERAL SECTIONS OF THE IPC; TECHNICAL SUBJECTS COVERED BY FORMER USPC CROSS-REFERENCE ART COLLECTIONS [XRACs] AND DIGESTS
- Y02—TECHNOLOGIES OR APPLICATIONS FOR MITIGATION OR ADAPTATION AGAINST CLIMATE CHANGE
- Y02D—CLIMATE CHANGE MITIGATION TECHNOLOGIES IN INFORMATION AND COMMUNICATION TECHNOLOGIES [ICT], I.E. INFORMATION AND COMMUNICATION TECHNOLOGIES AIMING AT THE REDUCTION OF THEIR OWN ENERGY USE
- Y02D30/00—Reducing energy consumption in communication networks
- Y02D30/70—Reducing energy consumption in communication networks in wireless communication networks
Landscapes
- Navigation (AREA)
Abstract
本发明公开了一种基于UWB与SINS的组合室内行人导航***及方法,包括惯性测量单元、导航解算模块、零速检测单元、UWB无线定位单元、磁力计和联邦kalman滤波单元,其中,所述联邦kalman滤波单元对导航解算模块、零速检测单元、UWB无线定位单元和磁力计的输出信号进行数据融合,获得人体运动加速度、航向角和位置信息的误差信号发送至惯性测量单元中用作进行修正。本发明在传统的捷联算法上引入了零速修正技术用于检测零速时刻,在进入超宽带信号区域,利用超宽带提供位置航向角,在非超宽带区域利用磁力计提供航向角,通过联邦kalman滤波融合零速、位置、航向角信息,对导航***速度、位置、航向进行校正,实现室内人员定位。
Description
技术领域
本发明属于领域,具体涉及一种基于UWB与SINS的组合室内行人导航***及方法。
背景技术
行人导航始于20世纪八、九十年代,发展较晚且速度缓慢,直至1996年美国联邦通信委员会公布E-911的定位需求后,行人导航才逐渐发展起来。行人导航***主要用于实时跟踪个人的位置信息并提供位置服务,能够为士兵、救援抢险人员提供服务,为高危险工作人员提供了更高级别的安全保障。
在室外GPS/LBS定位技术已经十分成熟,已经在车载、手机等设备上成功运用。然而在室内或者信号严重遮挡的位置,全球定位***(GPS)或者空间定位服务(LBS)无法有效定位,对于严重依赖于这两种导航技术的导航设备则无法继续使用。随着计算机与通信技术的发展,无线射频识别、WIFI、超宽带(Ultra Wideband,UWB)技术等在室内定位领域得到了广泛应用。其中超宽带技术利用宽度仅为纳秒级的脉冲作为通信信号,具有非常高的时间分辨率,能获得高达厘米级的定位精度,还具有一定的抗多径能力和穿透性,适用于高精度的室内导航定位服务。但是UWB定位是通过测距或测向完成,在非视距环境下,由于障碍物的遮挡,脉冲信号发生反射,会降低UWB的测距精度,大幅影响其定位精度,分析如下:
超宽带技术是一种新型的无线通信技术。它通过对具有很陡上升和下降时间的冲激脉冲进行直接调制,使信号具有GHz量级的带宽。其定位使用的是时间飞行法,通过测量脉冲飞行时间差计算距离,当出现遮挡物,会影响脉冲传播速度;设脉冲飞行距离为S
S=c·Δt
其中c是光速,Δt是时间;由于时间Δt出现误差,c极大,则会严重影响S。因此在测距过程中经常出现野值,造成测距错误。所以对于惯性导航与超宽带定位都不能独自作为定位***。
基于微机电***(Micro-Electro-Mechanical System,MEMS)捷联式惯性导航***(strap-down inertial navigation System)能够为UWB定位提供有效的补充,具有短时精度高,抗干扰性能强且能够实现短时自主定位的功能,基于积分工作方式的导航***,定位误差会随着时间的推移迅速的累积,定位误差过大甚至不能使用,不能成为一种真正意义上的导航***,具体分析如下:
由惯性导航基本公式:
其中,vn为载体的速度,为速度的微分,/>为载体坐标系(b系)至导航坐标系(n系)转换矩阵,fb为加速度在b系投影、/>为哥氏加速度、/>为向心加速度、gn为重力加速度。考虑到行人导航实际情况,哥氏加速度与向心加速度影响很小,近似为下式:
发明内容
针对上述问题,本发明提出一种基于UWB与SINS的组合室内行人导航***及方法,在传统的捷联算法上引入了零速修正技术用于检测零速时刻,在进入超宽带信号区域,利用超宽带提供位置航向角,通过联邦kalman滤波融合零速、位置、航向角,对导航***速度、位置、航向进行校正,实现室内人员定位功能。
实现上述技术目的,达到上述技术效果,本发明通过以下技术方案实现:
第一方面,本发明提供了一种基于UWB与SINS的组合室内行人导航***,包括:
SINS单元,所述SINS单元包括相连的惯性测量单元和导航解算模块,所述惯性测量单元用于采集人体运动加速度和转动角速度;所述导航解算模块接收并基于所述人体运动加速度和转动角速度计算出航向角和位置信息;
零速检测单元,所述零速检测单元的输入端与所述惯性测量单元的输出端相连,用于接收惯性测量单元输出的人体运动加速度和转动角速度,并基于所述人体运动加速度和转动角速度进行零速判断,获得零速矢量;
UWB无线定位单元,所述UWB无线定位单元用于输出航向角和位置信息;
磁力计,所述磁力计用于输出航向角;
联邦kalman滤波单元,所述联邦kalman滤波单元的输入端分别与所述导航解算模块、零速检测单元、UWB无线定位单元和磁力计的输出端相连,并对所述导航解算模块、零速检测单元、UWB无线定位单元和磁力计的输出信号进行数据融合,获得人体运动速度、航向角和位置信息的误差信号,并反馈至导航解算模块进行速度、航向、位置矫正。
优选地,所述零速检测单元包括运动模式判断模块、广义似然比检验模块和零速判断模块;
所述运动模式判断模块用于根据惯性测量单元输出的人体运动加速度进行运动模式判断;
所述广义似然比检验模块用于基于惯性测量单元输出的人体运动加速度和转动角速度计算广义似然比;
所述零速判断模块用于根据广义似然比检验模块输出的广义似然比以及各运动模式下的经验阈值,进行零速判断,获得零速矢量。
优选地,所述广义似然比的计算公式具体为:
其中,表示惯性测量单元中的加速度计输出的人体运动加速度,/>表示惯性测量单元中的陀螺仪输出的转动角速度,||·||表示矢量的模值,Ωn为惯性测量单元测量时间序列的窗口周期,/>分别是惯性测量单元中的加速度计与陀螺仪的观测噪声方差,/>表示在窗口周期Ωn内加速度计数据的平均值,N表示窗口长度,g表示当地重力加速度矢量。
优选地,所述根据广义似然比检验模块输出的广义似然比以及各运动模式下的经验阈值,进行零速判断采用的判断公式为:
Thi为运动模式i下的经验阈值,zero_flag表示是零速标志,其值为1表示速度为零速,其值为0时表示非零速。
优选地,所述联邦kalman滤波器包括速度子滤波器、航向子滤波器、姿态子滤波器和主滤波器,所述速度子滤波器、航向子滤波器、姿态子滤波器的数据传输端均与所述主滤波器的数据传输端相连,所述速度子滤波器的输入端分别与所述导航解算模块和零速检测单元的输出端相连,所述航向子滤波器的输入端分别与所述导航解算模块和UWB无线定位单元的输出端相连,所述姿态子滤波器的输入端分别与所述导航解算模块和磁力计的输出端相连。
第二方面,本发明提供了一种基于UWB与SINS的组合室内行人导航方法,包括以下步骤:
(1)利用惯性测量单元采集人体运动加速度和转动角速度,并利用导航解算模块基于所述人体运动加速度和转动角速度计算出航向角和位置信息;
(2)利用零速检测单元基于惯性测量单元输出的人体运动加速度和转动角速度,并基于所述人体运动加速度和转动角速度进行零速判断,获得零速矢量;
(3)利用UWB无线定位单元输出人***置信息和航向角;
(4)利用联邦Kalman滤波器对所述导航解算模块、零速检测单元、UWB无线定位单元和磁力计的输出信号进行数据融合,获得人体运动速度、航向角和位置信息的误差信号,并反馈至导航解算模块进行速度、航向、位置矫正。
优选地,所述利用零速检测单元基于惯性测量单元输出的人体运动加速度和转动角速度,并基于所述人体运动加速度和转动角速度进行零速判断,获得零速矢量,具体包括以下步骤:
根据惯性测量单元输出的人体运动加速度进行运动模式判断;
基于惯性测量单元输出的人体运动加速度和转动角速度计算广义似然比;
根据广义似然比以及各运动模式下的经验阈值,进行零速判断,获得零速矢量。
优选地,所述广义似然比的计算公式具体为:
其中,表示惯性测量单元中的加速度计输出的人体运动加速度,/>表示惯性测量单元中的陀螺仪输出的转动角速度,||·||表示矢量的模值,Ωn为惯性测量单元测量时间序列的窗口周期,/>分别是惯性测量单元中的加速度计与陀螺仪的观测噪声方差,/>表示在窗口周期Ωn内加速度计数据的平均值,N表示窗口长度,g表示当地重力加速度矢量;
所述零速判断采用的判断公式为:
Thi为运动模式i下的经验阈值,zero_flag表示是零速标志,其值为1表示速度为零速,其值为0时表示非零速。
优选地,所述步骤(4)包括以下子步骤:
(4.1)利用速度子滤波器将零速检测单元获得的零速信号与导航解算模块解算速度进行速度融合:
(4.2)利用航向子滤波器将导航解算模块与UWB无线定位单元中输出的位置、航向测量进行位置和航向融合;
(4.3)利用姿态子滤波器将导航解算模块与磁力计输出的航向角进行航向融合;
(4.4)利用上界方差技术,将速度子滤波器、航向子滤波器、姿态子滤波器与主滤波器的初始估计协方差阵Pi,0与过程噪声协方差阵Qi,0设置为联邦kalman滤波器初始估计协方差阵Pg,0和过程噪声协方差阵Qg,0的倍,保证各个子滤波器互不相关,βi满足信息守恒原则;
(4.5)将速度子滤波器、航向子滤波器、姿态子滤波器分别独立进行时间更新;
(4.6)将速度子滤波器、航向子滤波器、姿态子滤波器分别进行量测更新;
(4.7)将主滤波器进行时间更新;
(4.8)将各子滤波器与主滤波器的估计信息融合成为新的全局状态估计信息;
(4.9)将获得的新的全局状态估计信息、误差协方差矩阵Pg以及公共***噪声矩阵Qg按照设定规则重置各子滤波器与主滤波器,同时获得的新的全局状态估计信息发送至惯性测量单元中去用于进行速度、航向角和位置的修正。
优选地,所述设定的规则为:
式中,k表示k时刻,i表示第i个滤波器,表示k时刻公共***的状态估计值,/>表示第i个滤波器的状态估计值,Pg,k、Qg,k表示k时刻公共***的过程协方差矩阵与噪声协方差矩阵,Pi,k、Qi,k表示k时刻第i个滤波器过程协方差矩阵与噪声协方差矩阵。
与现有技术相比,本发明的有益效果:
本发明针对不依赖GPS或者LBS的室内行人导航定位需求,阐述了一种基于惯性测量单元(IMU)与超宽带(UWB)相结合的室内行人导航装置及方法。由MEMS惯性测量单元与超宽带定位***组成,MEMS固连在行人鞋面,UWB固定于肩膀位置;室内行人导航算法在传统的捷联算法上引入了零速修正技术用于检测零速时刻,在进入超宽带信号区域,利用超宽带提供位置航向角,在非超宽带区域利用磁力计提供航向角,通过联邦kalman滤波融合零速、位置、航向角,对***速度、位置、航向进行校正,实现室内人员定位功能。
附图说明
图1为本发明一种实施例的原理示意图;
图2为本发明一种实施例的结构示意图。
具体实施方式
为了使本发明的目的、技术方案及优点更加清楚明白,以下结合实施例,对本发明进行进一步详细说明。应当理解,此处所描述的具体实施例仅仅用以解释本发明,并不用于限定本发明。
下面结合附图对本发明的应用原理作详细的描述。
实施例1
本发明实施例中提供了一种基于UWB与SINS的组合室内行人导航装置,如图1-2所示,具体包括:
SINS单元,所述SINS单元包括相连的惯性测量单元和导航解算模块,所述惯性测量单元用于采集人体运动加速度和转动角速度;所述导航解算模块接收并基于所述人体运动加速度和转动角速度计算出航向角和位置信息;
零速检测单元,所述零速检测单元的输入端与所述惯性测量单元的输出端相连,用于接收惯性测量单元输出的人体运动加速度和转动角速度,并基于所述人体运动加速度和转动角速度进行零速判断;在本发明的一种具体实施例中,所述零速检测单元包括运动模式判断模块、广义似然比检验模块和零速判断模块,其中:
所述运动模式判断模块用于根据惯性测量单元输出的人体运动加速度进行运动模式判断;所述运动模式判断模块的工作过程为:
设本发明实施例中的惯性测量单元的x轴为人体行走方向的前向轴,通过计算x轴基于时间窗口的平均加速度来判断人体运动的快慢,设k时刻x轴的平均加速度为:
根据经验值设置两个阈值Thv1,Thv2划分为三个运动速度模式mod1~mod3,分别代表慢速、中速和快速;具体划分方法如下所示:
modi表示运动模式i。
所述广义似然比检验模块的工作过程为:
建立广义似然比检验模型
定义k时刻惯性测量单元的输出为:
设惯性测量单元输出量从n时刻开始,连续采集输出N次(窗口周期),得到一组连续的量测值{yk|k∈[n,n-N+1]},k为正整数。基于广义似然比统计的零速检测方法通过对长度为N的惯性测量单元输出数据构建广义似然比统计值,定义广义似然比统计模型如下:
其中,表示加速度计输出值,/>表示陀螺输出值,||·||表示矢量的模值,Ωn为惯性测量单元测量时间序列的窗口周期,/>分别是加速度计与陀螺仪的观测噪声方差,/>表示在窗口周期Ωn内加速度计数据的平均值,N表示窗口长度,g表示当地重力加速度矢量。
Thi为运动模式i下的经验阈值,zero_flag表示是零速标志,其值为1表示速度为零速,其值为0时表示非零速,根据不同的运动速度设置不同的阈值可以更加准确的判断零速。
UWB无线定位单元,所述UWB无线定位单元用于输出人体航向信息和位置信息;所述UWB无线定位单元包括基站和标签,为了安装的便捷性,只考虑平面的情况;本发明实施例中的UWB无线定位单元包括三个基站和一个标签;当所述UWB无线定位单元工作时,所述标签能够输出自身到各个基站的距离,在已知各个基站的位置时,可以利用三点定位的方式完成定位工作;设三个基站A、B、C的位置分别为(xa,ya),(xb,yb),(xc,yc),标签输出到三个基站的位置为da,db,dc,则标签的坐标可以表示为,标签的坐标就是人***置:
在超宽带信号内,UWB无线定位单元提供的位置相对准确,其输出的航向角相对磁力计更加的稳定,利用两点确定直线的方式确定航向角,航向角的计算方程如式(7)所示:
Yn表示时刻n时的纵轴坐标,Xn表示时刻n时的横轴坐标;
磁力计,所述磁力计用于输出航向信息;
联邦kalman滤波单元,联邦kalman滤波单元,所述联邦kalman滤波单元的输入端分别与所述导航解算模块、零速检测单元、UWB无线定位单元和磁力计的输出端相连,并对所述导航解算模块、零速检测单元、UWB无线定位单元和磁力计的输出信号进行数据融合,获得人体运动速度、航向角和位置信息的误差信号,并反馈至导航解算模块进行速度、航向、位置矫正;具体地,所述联邦kalman滤波器包括速度子滤波器、航向子滤波器、姿态子滤波器和主滤波器,所述速度子滤波器、航向子滤波器、姿态子滤波器的数据传输端均与所述主滤波器的数据传输端相连,所述速度子滤波器的输入端分别与所述导航解算模块和零速检测单元的输出端相连,所述航向子滤波器的输入端分别与所述导航解算模块和UWB无线定位单元的输出端相连,所述姿态子滤波器的输入端分别与所述导航解算模块和磁力计的输出端相连。
所述联邦kalman滤波单元的工作过程具体为:
利用速度子滤波器将零速检测单元获得的零速信号与惯性测量单元解算速度进行速度融合,具体为:
其中,表示k-1时刻的状态估计量,/>表示/>的一步预测值,Wk-1和Vk均视为彼此不相关的零均值白噪声序列;k时刻到k-1时刻的状态转移矩阵Fk|k-1与噪声输入矩阵Gk|k-1分根据下面公式确定,。
利用速度子滤波器进行速度融合,将零速检测单元获得的零速信号与导航解算模块解算速度之差作为观测值,通过观测值对导航解算模块中获得的加速度信号进行修正,观测向量及观测矩阵分别为:
其中Z=03×1-vn中03×1表示零速检测获得的零速矢量,vn表示导航解算模块解算获得的速度矢量。
利用航向子滤波器将导航解算模块与UWB无线定位单元中输出的位置、航向测量进行位置和航向融合,具体为:
对于航向子滤波器,其状态方程与速度子滤波器相同,即航向子滤波器的状态方程也是公式(8),但是观测方程不同;航向子滤波器用于进行位置和航向融合,将导航解算模块与UWB无线定位单元中输出的位置、航向测量之差作为观测值,通过观测值对导航解算模块中的位置信息和航向角进行修正,观测向量及观测矩阵分别为:
式中,Hs是导航解算模块解算出来的航向角,Hu是UWB无线定位单元解算出来的航向角,Ps是导航解算模块解算出的位置,Pu是UWB无线定位单元解算得到的位置。
利用姿态子滤波器将导航解算模块与磁力计输出的航向角进行航向融合,具体为:
对于姿态子滤波器,其状态方程与速度子滤波器相同,即姿态子滤波器的状态方程也是公式(8),姿态子滤波器用于进行航向融合,将导航解算模块与磁力计输出的航向角之差作为观测值。通过观测值对导航解算模块中的航向角进行修正,观测向量及观测矩阵分别为:
式中,Hs是导航解算模块解算出来的航向角,Hc是磁力计解算出来的航向角。
利用上界方差技术,将速度子滤波器、航向子滤波器、姿态子滤波器与主滤波器的初始估计协方差阵与过程噪声协方差阵设置为联邦kalman滤波器的倍,保证各个子滤波器互不相关,βi满足信息守恒原则,具体见下式;
将速度子滤波器、航向子滤波器、姿态子滤波器分别独立进行时间更新,更新公式为:
式中,表示第i个子滤波器k-1时刻状态估计量,/>表示/>的当前预测向量,Fi,k|k-1、Gi,k|k-1表示第i个子滤波器k-1到k时刻的状态转移矩阵与噪声输入矩阵,Pi,k-1、Pi,k|k-1表示/>的协方差矩阵,Qi,k-1表示k-1时刻***过程协方差矩阵。
将速度子滤波器、航向子滤波器、姿态子滤波器分别进行量测更新(所述的量测即为观测量),更新公式为:
式中,Ki,k表示k时刻第i个滤波器滤波增益矩阵,Hi,k表示k时刻观测矩阵,Ri,k表示k时刻噪声的方差矩阵,I表示单位矩阵。
将主滤波器进行时间更新,所述主滤波器的状态方程也是公式(8),更新公式为:
将各子滤波器与主滤波器的估计信息融合成为新的全局状态估计信息,具体为:
将获得的全局状态估计误差协方差矩阵Pg以及公共***噪声矩阵Qg按照下式重置各子滤波器与主滤波器,同时将由步骤(4.8)获得的/>发送至导航解算模块中去用于进行速度、航向角和位置的修正,所述公式(19)具体为:
实施例2
本发明实施例提供了一种基于UWB与SINS的组合室内行人导航方法,包括以下步骤:
步骤(1)利用惯性测量单元采集人体运动加速度和转动角速度,并利用导航解算模块基于所述人体运动加速度和转动角速度计算出航向角和位置信息;其中,所述航向角是通过对转动角速度信号进行积分获得姿态信号,姿态信号中包含了俯仰角、横滚角和航向角,所述的航向角即为航向角;所述位置信息的计算过程采用的也是现有技术,因此,在本发明实施例中不做过多的赘述;
步骤(2)利用零速检测单元基于惯性测量单元输出的人体运动加速度和转动角速度,并基于所述人体运动加速度和转动角速度进行零速判断;
零速检测是零速修正的前提,现有的零速检测算法大都是基于加速度计与陀螺仪输出信号进行检测,主要包括广义似然比检测(Generalized Likelihood Ratio Test,GLRT)、加速度计量测方差检测(Accelerometer Measurements Variance Test,MV)、加速度计量幅值检测(Accelerometer Measurements Magnitude Test,MAG)、角速度量测能量检测(Angular Rate Measurement Energy Test,ARE),其中广义似然比检测效果显著优于其他方法检测,在本发明实施例的一种具体实施方式中,所述零速检测采用的是广义似然比检测,并通过判断运动模式设置不同的阈值来适应不同速度的步态;
(2.1)判断运动模式
设本发明实施例中的惯性测量单元的x轴为人体行走方向的前向轴,通过计算x轴基于时间窗口的平均加速度来判断人体运动的快慢,设k时刻x轴的平均加速度为:
根据经验值设置两个阈值Thv1,Thv2划分为三个运动速度模式mod1~mod3,分别代表慢速、中速和快速;具体划分方法如下所示:
modi表示运动模式i。
(2.2)建立广义似然比检验模型
定义k时刻惯性测量单元的输出为:
设惯性测量单元输出量从n时刻开始,连续采集输出N次(窗口周期),得到一组连续的量测值{yk|k∈[n,n-N+1]},k为正整数。基于广义似然比统计的零速检测方法通过对长度为N的惯性测量单元输出数据构建广义似然比统计值,定义广义似然比统计模型如下:
其中,表示加速度计输出值,/>表示陀螺输出值,||·||表示矢量的模值,Ωn为惯性测量单元测量时间序列的窗口周期,/>分别是加速度计与陀螺仪的观测噪声方差,/>表示在窗口周期Ωn内加速度计数据的平均值,N表示窗口长度,g表示当地重力加速度矢量。/>
Thi为运动模式i下的经验阈值,zero_flag表示是零速标志,其值为1表示速度为零速,其值为0时表示非零速,根据不同的运动速度设置不同的阈值可以更加准确的判断零速。
步骤(3)利用UWB无线定位单元输出人***置信息和航向角;
所述UWB无线定位单元包括基站和标签,为了安装的便捷性,只考虑平面的情况;本发明实施例中的UWB无线定位单元包括三个基站和一个标签;当所述UWB无线定位单元工作时,所述标签能够输出自身到各个基站的距离,在已知各个基站的位置时,可以利用三点定位的方式完成定位工作;设三个基站A、B、C的位置分别为(xa,ya),(xb,yb),(xc,yc),标签输出到三个基站的位置为da,db,dc,则标签的坐标可以表示为,标签的坐标就是人***置:
在超宽带信号内,UWB无线定位单元提供的位置相对准确,其输出的航向角相对磁力计更加的稳定,利用两点确定直线的方式确定航向角,航向角的计算方程如式(7)所示:
Yn表示时刻n时的纵轴坐标,Xn表示时刻n时的横轴坐标。
步骤(4)利用联邦Kalman滤波器对所述导航解算模块、零速检测单元、UWB无线定位单元输出和磁力计的信号进行数据融合处理;
如图2所示,所述联邦kalman滤波器包括速度子滤波器、航向子滤波器、姿态子滤波器和主滤波器;
所述速度子滤波器、航向子滤波器、姿态子滤波器的数据传输端均与所述主滤波器的数据传输端相连,所述速度子滤波器的输入端分别与所述导航解算模块和零速检测单元的输出端相连,所述航向子滤波器的输入端分别与所述导航解算模块和UWB无线定位单元的输出端相连,所述姿态子滤波器的输入端分别与所述导航解算模块和磁力计的输出端相连,其中,图2中的Xi、Pi分别表示各自滤波器的局部状态估计值与协方差矩阵,Xm、Pm分别表示主滤波器的状态估计值与协方差矩阵,Xg、Pg分别表示公共***的状态估计值与协方差矩阵;βi表示系数。
在本发明实施例的一种具体实施方式中,所述步骤(4)包括以下子步骤:
步骤(4.1):利用速度子滤波器将零速检测单元获得的零速信号与导航解算模块解算速度进行速度融合,具体为:
其中,表示k-1时刻的状态估计量,/>表示/>的一步预测值,Wk-1和Vk均视为彼此不相关的零均值白噪声序列;k时刻到k-1时刻的状态转移矩阵Fk|k-1与噪声输入矩阵Gk|k-1分根据下面公式确定,H表示观测矩阵,Zk表示k时刻观测向量。
利用速度子滤波器进行速度融合,将零速检测单元获得的零速信号与导航解算模块解算速度之差作为观测值,通过观测值对导航解算模块中获得的加速度信号进行修正,观测向量及观测矩阵分别为:
其中Z=03×1-vn中03×1表示零速检测获得的零速矢量,vn表示惯性导航解算获得的速度矢量。
步骤(4.2):利用航向子滤波器将导航解算模块与UWB无线定位单元中输出的位置、航向测量进行位置和航向融合,具体为:
对于航向子滤波器,其状态方程与速度子滤波器相同,即航向子滤波器的状态方程也是公式(8),但是观测方程不同;航向子滤波器用于进行位置和航向融合,将导航解算模块与UWB无线定位单元中输出的位置、航向测量之差作为观测值,通过观测值对导航解算模块中的位置信息和航向角进行修正,观测向量及观测矩阵分别为:
式中,Hs是导航解算模块解算出来的航向角,Hu是UWB无线定位单元解算出来的航向角,Ps是导航解算模块解算出的位置,Pu是UWB无线定位单元解算得到的位置。
步骤(4.3):利用姿态子滤波器将导航解算模块与磁力计输出的航向角进行航向融合,具体为:
对于姿态子滤波器,其状态方程与速度子滤波器相同,即姿态子滤波器的状态方程也是公式(8),姿态子滤波器用于进行航向融合,将导航解算模块与磁力计输出的航向角之差作为观测值。通过观测值对导航解算模块中的航向角进行修正,观测向量及观测矩阵分别为:
式中,Hs是导航解算模块解算出来的航向角,Hc是磁力计解算出来的航向角。
步骤(4.4)利用上界方差技术,将速度子滤波器、航向子滤波器、姿态子滤波器与主滤波器的初始估计协方差阵与过程噪声协方差阵设置为联邦kalman滤波器的倍,保证各个子滤波器互不相关,βi满足信息守恒原则,具体见下式;
步骤(4.5)将速度子滤波器、航向子滤波器、姿态子滤波器分别独立进行时间更新,更新公式为:
式中,表示第i个子滤波器k-1时刻状态估计量,/>表示/>的当前预测向量,Fi,k|k-1、Gi,k|k-1表示第i个子滤波器k-1到k时刻的状态转移矩阵与噪声输入矩阵,Pi,k-1、Pi,k|k-1表示/>的协方差矩阵,Qi,k-1表示k-1时刻***过程协方差矩阵。
步骤(4.6)将速度子滤波器、航向子滤波器、姿态子滤波器分别进行量测更新(所述的量测即为观测量),更新公式为:
式中,Ki,k表示k时刻第i个滤波器滤波增益矩阵,Hi,k表示k时刻观测矩阵,Ri,k表示k时刻噪声的方差矩阵,I表示单位矩阵。
步骤(4.7)将主滤波器进行时间更新,所述主滤波器的状态方程也是公式(8),更新公式为:
步骤(4.8)将各子滤波器与主滤波器的估计信息融合成为新的全局状态估计信息,具体为:
步骤(4.9)将获得的全局状态估计误差协方差矩阵Pg以及公共***噪声矩阵Qg按照下式重置各子滤波器与主滤波器,同时将由步骤(4.8)获得的/>发送至导航解算模块中去用于进行速度、航向角和位置的修正,所述公式(19)具体为:
以上显示和描述了本发明的基本原理和主要特征和本发明的优点。本行业的技术人员应该了解,本发明不受上述实施例的限制,上述实施例和说明书中描述的只是说明本发明的原理,在不脱离本发明精神和范围的前提下,本发明还会有各种变化和改进,这些变化和改进都落入要求保护的本发明范围内。本发明要求保护范围由所附的权利要求书及其等效物界定。
Claims (8)
1.一种基于UWB与SINS的组合室内行人导航***,其特征在于,包括:
SINS单元,所述SINS单元包括相连的惯性测量单元和导航解算模块,所述惯性测量单元用于采集人体运动加速度和转动角速度;所述导航解算模块接收并基于所述人体运动加速度和转动角速度计算出航向角和位置信息;
零速检测单元,所述零速检测单元的输入端与所述惯性测量单元的输出端相连,用于接收惯性测量单元输出的人体运动加速度和转动角速度,并基于所述人体运动加速度和转动角速度进行零速判断,获得零速矢量;
UWB无线定位单元,所述UWB无线定位单元用于输出航向角和位置信息;
磁力计,所述磁力计用于输出航向角;
联邦kalman滤波单元,所述联邦kalman滤波单元的输入端分别与所述导航解算模块、零速检测单元、UWB无线定位单元和磁力计的输出端相连,并对所述导航解算模块、零速检测单元、UWB无线定位单元和磁力计的输出信号进行数据融合,获得人体运动速度、航向角和位置信息的误差信号,并反馈至导航解算模块进行速度、航向、位置矫正;
在进入超宽带信号区域,利用超宽带提供位置航向角,在非超宽带区域利用磁力计提供航向角;
所述联邦kalman滤波器包括速度子滤波器、航向子滤波器、姿态子滤波器和主滤波器,所述速度子滤波器、航向子滤波器、姿态子滤波器的数据传输端均与所述主滤波器的数据传输端相连,所述速度子滤波器的输入端分别与所述导航解算模块和零速检测单元的输出端相连,所述航向子滤波器的输入端分别与所述导航解算模块和UWB无线定位单元的输出端相连,所述姿态子滤波器的输入端分别与所述导航解算模块和磁力计的输出端相连;
速度子滤波器将零速检测单元获得的零速信号与导航解算模块解算速度进行速度融合;
航向子滤波器将导航解算模块与UWB无线定位单元中输出的位置、航向测量进行位置和航向融合;
姿态子滤波器将导航解算模块与磁力计输出的航向角进行航向融合;
利用上界方差技术,将速度子滤波器、航向子滤波器、姿态子滤波器与主滤波器的初始估计协方差阵Pi,0与过程噪声协方差阵Qi,0设置为联邦kalman滤波器初始估计协方差阵Pg,0和过程噪声协方差阵Qg,0的βi -1倍,保证各个子滤波器互不相关,βi满足信息守恒原则;
将速度子滤波器、航向子滤波器、姿态子滤波器分别独立进行时间更新;
将速度子滤波器、航向子滤波器、姿态子滤波器分别进行量测更新;
将主滤波器进行时间更新;
将各子滤波器与主滤波器的估计信息融合成为新的全局状态估计信息;
将获得的新的全局状态估计信息、误差协方差矩阵Pg以及公共***噪声矩阵Qg按照设定规则重置各子滤波器与主滤波器,同时获得的新的全局状态估计信息发送至惯性测量单元中去用于进行速度、航向角和位置的修正。
2.根据权利要求1所述的一种基于UWB与SINS的组合室内行人导航***,其特征在于:所述零速检测单元包括运动模式判断模块、广义似然比检验模块和零速判断模块;
所述运动模式判断模块用于根据惯性测量单元输出的人体运动加速度进行运动模式判断;
所述广义似然比检验模块用于基于惯性测量单元输出的人体运动加速度和转动角速度计算广义似然比;
所述零速判断模块用于根据广义似然比检验模块输出的广义似然比以及各运动模式下的经验阈值,进行零速判断,获得零速矢量。
5.一种基于UWB与SINS的组合室内行人导航方法,其特征在于,包括以下步骤:
(1)利用惯性测量单元采集人体运动加速度和转动角速度,并利用导航解算模块基于所述人体运动加速度和转动角速度计算出航向角和位置信息;
(2)利用零速检测单元基于惯性测量单元输出的人体运动加速度和转动角速度,并基于所述人体运动加速度和转动角速度进行零速判断,获得零速矢量;
(3)利用UWB无线定位单元输出人***置信息和航向角;
(4)利用联邦Kalman滤波器对所述导航解算模块、零速检测单元、UWB无线定位单元和磁力计的输出信号进行数据融合,获得人体运动速度、航向角和位置信息的误差信号,并反馈至导航解算模块进行速度、航向、位置矫正;所述磁力计用于输出航向角;
在进入超宽带信号区域,利用超宽带提供位置航向角,在非超宽带区域利用磁力计提供航向角;
所述联邦kalman滤波器包括速度子滤波器、航向子滤波器、姿态子滤波器和主滤波器,所述速度子滤波器、航向子滤波器、姿态子滤波器的数据传输端均与所述主滤波器的数据传输端相连,所述速度子滤波器的输入端分别与所述导航解算模块和零速检测单元的输出端相连,所述航向子滤波器的输入端分别与所述导航解算模块和UWB无线定位单元的输出端相连,所述姿态子滤波器的输入端分别与所述导航解算模块和磁力计的输出端相连;
所述步骤(4)包括以下子步骤:
(4.1)利用速度子滤波器将零速检测单元获得的零速信号与导航解算模块解算速度进行速度融合;
(4.2)利用航向子滤波器将导航解算模块与UWB无线定位单元中输出的位置、航向测量进行位置和航向融合;
(4.3)利用姿态子滤波器将导航解算模块与磁力计输出的航向角进行航向融合;
(4.4)利用上界方差技术,将速度子滤波器、航向子滤波器、姿态子滤波器与主滤波器的初始估计协方差阵Pi,0与过程噪声协方差阵Qi,0设置为联邦kalman滤波器初始估计协方差阵Pg,0和过程噪声协方差阵Qg,0的βi -1倍,保证各个子滤波器互不相关,βi满足信息守恒原则;
(4.5)将速度子滤波器、航向子滤波器、姿态子滤波器分别独立进行时间更新;
(4.6)将速度子滤波器、航向子滤波器、姿态子滤波器分别进行量测更新;
(4.7)将主滤波器进行时间更新;
(4.8)将各子滤波器与主滤波器的估计信息融合成为新的全局状态估计信息;
(4.9)将获得的新的全局状态估计信息、误差协方差矩阵Pg以及公共***噪声矩阵Qg按照设定规则重置各子滤波器与主滤波器,同时获得的新的全局状态估计信息发送至惯性测量单元中去用于进行速度、航向角和位置的修正。
6.根据权利要求5所述的一种基于UWB与SINS的组合室内行人导航方法,其特征在于:所述利用零速检测单元基于惯性测量单元输出的人体运动加速度和转动角速度,并基于所述人体运动加速度和转动角速度进行零速判断,获得零速矢量,具体包括以下步骤:
根据惯性测量单元输出的人体运动加速度进行运动模式判断;
基于惯性测量单元输出的人体运动加速度和转动角速度计算广义似然比;
根据广义似然比以及各运动模式下的经验阈值,进行零速判断,获得零速矢量。
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN201811607679.5A CN109855621B (zh) | 2018-12-27 | 2018-12-27 | 一种基于uwb与sins的组合室内行人导航***及方法 |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN201811607679.5A CN109855621B (zh) | 2018-12-27 | 2018-12-27 | 一种基于uwb与sins的组合室内行人导航***及方法 |
Publications (2)
Publication Number | Publication Date |
---|---|
CN109855621A CN109855621A (zh) | 2019-06-07 |
CN109855621B true CN109855621B (zh) | 2023-06-02 |
Family
ID=66892588
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN201811607679.5A Active CN109855621B (zh) | 2018-12-27 | 2018-12-27 | 一种基于uwb与sins的组合室内行人导航***及方法 |
Country Status (1)
Country | Link |
---|---|
CN (1) | CN109855621B (zh) |
Families Citing this family (10)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN110579212B (zh) * | 2019-08-13 | 2022-11-29 | 湘潭大学 | 室内定位方法及装置 |
CN110763228B (zh) * | 2019-10-08 | 2022-12-13 | 哈尔滨工程大学 | 基于海底油气管节点位置的组合导航***误差修正方法 |
CN111007455B (zh) * | 2019-10-16 | 2024-04-30 | 张苏 | 定位***及方法、数据库、神经网络模型训练方法 |
CN110940334B (zh) * | 2019-10-23 | 2023-04-18 | 南京华科广发通信科技有限公司 | 一种人体行走测速徽章及测速方法 |
CN110764506B (zh) * | 2019-11-05 | 2022-10-11 | 广东博智林机器人有限公司 | 移动机器人的航向角融合方法、装置和移动机器人 |
CN110986952B (zh) * | 2019-12-11 | 2023-06-02 | 东北大学 | 一种针对行人的高精度抗干扰室内定位方法 |
CN111024070A (zh) * | 2019-12-23 | 2020-04-17 | 哈尔滨工程大学 | 一种基于航向自观测的惯性足绑式行人定位方法 |
CN113438615A (zh) * | 2020-03-04 | 2021-09-24 | 北京京东乾石科技有限公司 | 定位方法和装置 |
CN112711055B (zh) * | 2020-12-08 | 2024-03-19 | 重庆邮电大学 | 一种基于边缘计算的室内外无缝定位***及方法 |
CN114440876B (zh) * | 2022-01-21 | 2024-04-02 | 北京自动化控制设备研究所 | 一种井下掘进机定位导向方法及井下掘进机定位导向*** |
Family Cites Families (10)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN105173940B (zh) * | 2015-07-16 | 2017-06-09 | 中南大学 | 一种基于组合导航技术的超深矿井罐笼位姿测量***及方法 |
CN105783923B (zh) * | 2016-01-05 | 2018-05-08 | 山东科技大学 | 基于rfid和mems惯性技术的人员定位方法 |
CN106093858B (zh) * | 2016-06-22 | 2019-02-05 | 山东大学 | 一种基于uwb、rfid、ins多源联合定位技术的定位***及定位方法 |
CN106908759A (zh) * | 2017-01-23 | 2017-06-30 | 南京航空航天大学 | 一种基于uwb技术的室内行人导航方法 |
CN106871894B (zh) * | 2017-03-23 | 2020-03-13 | 北京工业大学 | 一种基于条件随机场的地图匹配方法 |
CN107270896A (zh) * | 2017-06-20 | 2017-10-20 | 华中科技大学 | 一种行人定位与轨迹跟踪方法和*** |
CN107655476B (zh) * | 2017-08-21 | 2021-04-20 | 南京航空航天大学 | 基于多信息融合补偿的行人高精度足部导航方法 |
CN108426574A (zh) * | 2018-02-02 | 2018-08-21 | 哈尔滨工程大学 | 一种基于zihr的航向角修正算法的mems行人导航方法 |
CN108490388B (zh) * | 2018-03-13 | 2021-06-29 | 同济大学 | 一种基于uwb与vlc技术的多源联合室内定位方法 |
CN108680184B (zh) * | 2018-04-19 | 2021-09-07 | 东南大学 | 一种基于广义似然比统计曲线几何变换的零速检测方法 |
-
2018
- 2018-12-27 CN CN201811607679.5A patent/CN109855621B/zh active Active
Also Published As
Publication number | Publication date |
---|---|
CN109855621A (zh) | 2019-06-07 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
CN109855621B (zh) | 一种基于uwb与sins的组合室内行人导航***及方法 | |
EP3430419B1 (en) | Estimating locations of mobile devices in a wireless tracking system | |
CN105607104B (zh) | 一种基于gnss与ins的自适应导航定位***及方法 | |
CN109682375B (zh) | 一种基于容错决策树的uwb辅助惯性定位方法 | |
CN104121905B (zh) | 一种基于惯性传感器的航向角获取方法 | |
US10557711B2 (en) | Apparatus for inferring pedestrian position based on pedestrian movement detection, and method therefor | |
CN111024075B (zh) | 一种结合蓝牙信标和地图的行人导航误差修正滤波方法 | |
US11035915B2 (en) | Method and system for magnetic fingerprinting | |
CN105357754B (zh) | 一种基于无线网络的移动节点组合定位方法 | |
CN105865450A (zh) | 一种基于步态的零速更新方法及*** | |
KR102172145B1 (ko) | 추측 항법 시스템에서의 밀결합 측위 방법 및 그 장치 | |
CN108761512A (zh) | 一种弹载bds/sins深组合自适应ckf滤波方法 | |
US11743687B2 (en) | Method and system for determining and tracking an indoor position of an object | |
CN110986952B (zh) | 一种针对行人的高精度抗干扰室内定位方法 | |
Hartmann et al. | Hybrid indoor pedestrian navigation combining an INS and a spatial non-uniform UWB-network | |
Long et al. | Single UWB anchor aided PDR heading and step length correcting indoor localization system | |
Tarrío et al. | Fusion of RSS and inertial measurements for calibration-free indoor pedestrian tracking | |
Tao et al. | Precise displacement estimation from time-differenced carrier phase to improve PDR performance | |
Qian et al. | RPNOS: Reliable pedestrian navigation on a smartphone | |
CN113325455B (zh) | 用于跟踪和确定物体的室内位置的方法和*** | |
CN110595465A (zh) | 一种基于gnss和imu的定位定姿*** | |
CN111708008A (zh) | 一种基于imu和tof的水下机器人单信标导航方法 | |
Lategahn et al. | Robust pedestrian localization in indoor environments with an IMU aided TDoA system | |
Tiemann et al. | Improving the robustness of control-grade ultra-wideband localization | |
GB2567889A (en) | Method and system for determining a direction of movement of an object |
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 |