CN109682375A - 一种基于容错决策树的uwb辅助惯性定位方法 - Google Patents
一种基于容错决策树的uwb辅助惯性定位方法 Download PDFInfo
- Publication number
- CN109682375A CN109682375A CN201910053745.7A CN201910053745A CN109682375A CN 109682375 A CN109682375 A CN 109682375A CN 201910053745 A CN201910053745 A CN 201910053745A CN 109682375 A CN109682375 A CN 109682375A
- Authority
- CN
- China
- Prior art keywords
- uwb
- fault
- tolerant
- inertial
- decision tree
- 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.)
- Granted
Links
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/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
- 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
-
- 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
- Engineering & Computer Science (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Automation & Control Theory (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Navigation (AREA)
Abstract
本发明公开了一种基于容错决策树的UWB辅助惯性定位方法,所述方法针对惯性导航系统误差随时间累积和UWB定位受到非视距问题、多径效应和人体影响出现粗大误差的问题,提出并采用陀螺仪高精度分段拟合误差补偿模型,抑制惯性导航误差漂移;同时在UWB辅助人员室内惯性定位的基础上,构建惯性导航与UWB单点定位数据共同作用的容错决策树判定模型,剔除UWB定位的粗大误差因子,进而对惯性导航和UWB参数差值应用扩展卡尔曼滤波,实现UWB辅助增强惯性导航定位,能够长时间的连续可靠定位,具有很强的工程应用价值。
Description
技术领域
本发明属于人员室内定位领域,尤其涉及多源信息辅助增强惯性定位。
背景技术
基于位置的服务是现代生活里许多应用场景所需的关键特征,人们越来越依赖所带来的便利性。在室外,GPS、北斗等得到了很好的探索和标准化,但由于其信号容易受到室内复杂环境的阻隔,导致无法在室内复杂环境实现高精度的定位。因此,室内高精度定位研究具有很高的商用价值。
惯性导航系统利用载体自身的角速率和加速度进行实时测量解算姿态信息和相对位置信息,具有成本低、体积小、无源等优点,在短期内对室内复杂环境能有效实现较高精度定位,但惯性导航系统随着时间增长,陀螺和加速度计等惯性元器件本身产生的测量误差等使其相对定位点出现较大误差,单一模式不适合长时间定位。
为解决惯性导航存在的自身缺陷,在基于RF的定位技术中,超宽带系统可以通过使用窄脉冲来区分原始信号和反射信号由于其具有高精度、低成本和低功耗,基于UWB的室内定位已经变得非常流行。相比较于超声波,红外,蓝牙,ZigBee,射频识别技术和WIFI等有显著优势,虽然这些技术在一定程度上满足了部分室内活动的需求,但仍存在成本高、定位精度差等缺点。目前,国内外学者提出了采用UWB定位技术的多源信息对惯性导航进行辅助修正。Lukasz Zwirello等人提出了一个基于仿真的紧密UWB和惯性数据集成的可行性研究;Qigao Fan等人提出了利用双态自适应卡尔曼滤波器的INS/UWB定位系统;曾庆化等人提出了基于零速修正的UWB优化配置室内行人导航。但大多数学者没有进行组合定位的相互容错判定,在建筑结构复杂的室内环境中,UWB定位易受到非视距问题、多径效应和人体的影响出现粗大误差,可能导致惯性导航/UWB定位精度下降,甚至错误地辅助修正惯性导航。
发明内容
本发明旨在解决以上现有技术的问题。提出了一种基于容错决策树的UWB辅助惯性定位方法。本发明针对惯性导航系统误差随时间累积和UWB定位受到非视距问题、多径效应和人体影响出现粗大误差的问题,提出并采用陀螺仪高精度分段拟合误差补偿模型,抑制惯性导航误差漂移;同时在UWB辅助人员室内惯性定位的基础上,构建惯性导航与UWB单点定位数据共同作用的容错决策树判定模型,剔除UWB定位的粗大误差因子,进而对惯性导航和UWB参数差值应用扩展卡尔曼滤波,实现UWB辅助增强惯性导航定位,能够长时间的连续可靠定位,具有很强的工程应用价值。
本发明的具体技术方案是:一种基于容错决策树的UWB辅助惯性定位方法,包括以下步骤:
步骤一,使用加速度和磁力计解算得到初始横滚角、俯仰角和航向角;
步骤二,采用陀螺仪分段拟合误差补偿模型区分补偿高低动态速率段,更进一步消除角速率输出误差,并根据步骤二中的初始横滚角、俯仰角和航向角,采用四元数算法获得姿态角信息数据;
步骤三,根据加速度特征数据,发现行人走路的波峰、零点和波谷数据有似正弦波的周期性规律,获得有效步数,并利用加速度与人员行走时刻步长的波峰波谷差值之间的关联性,进行自适应步长计算,得到惯性定位数据信息;
步骤四,采用UWB双向飞行时间测量UWB模块间距离,随后根据三边测距定位方法计算行人当前时刻的位置;
步骤五,将步骤四获得的行人当前时刻的位置作为容错决策树判定模型的输入量,步骤三获得的惯性定位数据作为容错决策树判定模型的参考量,进行容错判定,得到当前时刻UWB单点定位数据;
步骤六,将步骤三获得的惯性定位数据和步骤四获得的UWB单点定位数据进行扩展卡尔曼滤波定位信息融合。
步骤二所述陀螺仪分段拟合误差补偿模型,分析传感器自身的安装标定误差和非线性误差,利用公式(1)角速率输出误差模型,从而得到陀螺仪的补偿矩阵:
ωx、ωy、ωz分别代表x、y、z轴的陀螺仪真实角速率,Sx、Sz、Sy代表x、y、z轴的陀螺仪标定因数,Kyx、Kzx、Kxy、Kzy、Kxz、Kyz代表x、y、z轴的陀螺仪安装误差系数,Bx、By、Bz分别代表x、y、z轴的陀螺仪零偏,wx、wy、wz分别代表x、y、z轴的陀螺仪实际角速率测量值。
所述分段拟合误差补偿中,选取三轴转台动态速率为-100°/s~100°/s,以10°/s作为间隔,共20个角速率,每组采集保留500kb数据,按照速率数据正负对称和动态速率高低,将20个角速率分为8组,每段包含的角速率密度为m,在每个分段组中对其数据进行二次拟合,得到该分段组更为接近真实角速率的标定因数。
所述容错判定包括,第一层容错判定,获知UWB标签是否接收到定位基站的脉冲信号,判定是否存在UWB单点定位数据;第二层容错判定,根据行人运动规律特性,利用当前k时刻惯导解算对应的航向角ψk和初始航向角ψzero进行判断行人是否发生转弯模式变化;第三层容错判定,采用同一采样点的姿态角信息与UWB定位信息之差Δp,并结合行人运动步数与距离之间的关系,进行测量数据的活动阈值判定。
所述第一层容错判定具体包括利用Tuwb标志位判定是否存在UWB单点定位数据;
当标志位为1时,exist代表存在满足条件的当前k时刻的UWB单点定位数据,反之亦然;
通过Tuwb标志位判定后,利用UWB单点定位数据获得其标签与定位基站之间的飞行时间后,使用TW-TOF和三边测距定位方法计算行人当前k时刻的移动位置最优解Puwb,k(xn,yn)。
所述第三层容错判定的Δp规定如下:
式中,Pins,k代表惯性导航在k时刻的位置数据,Puwb,k代表UWB在k时刻单点定位距离数据,σ代表惯性导航与UWB位置数据之差的活动阈值。
所述活动阈值σ能同时满足下列行走步数范围的关系式:
式中,step表示加速度解算的有效步数,σ1、σ2、σ3表示分段步数对应的阈值。
步骤六所述扩展卡尔曼滤波的状态方程与观测方程如下:
式中,k为离散时间所对应的时刻,系统在k时刻的状态Xk∈Rn;Zk∈Rm为对应状态的观测信号,Φk为状态转移矩阵,Gk为状态噪声驱动矩阵,Wk∈Rn为输入信号的白噪声,φk为非随机的外作用项,Hk为观测矩阵,Vk∈Rm为观测信号的白噪声。
令X=[δp,δv,δφ,δa]T为所述扩展卡尔曼滤波的状态向量,包含12个状态量,其中行人的位置误差δp、惯性导航东北天坐标的速度误差δv、当前时刻惯性导航所解算的姿态角误差δφ以及三轴加速度误差δa作为状态向量。
所述扩展卡尔曼滤波,针对姿态角误差补偿,当初始时刻检测到开机站立时,默认此时惯性导航解算的姿态角为初始姿态角,将惯性导航解算的实际姿态角与初始姿态角之差Δψ作为当前时刻姿态角的观测量;针对位置误差补偿,使用惯性导航与UWB解算的位置信息之差作为当前时刻位置的观测量Δp,则观测向量为Z=[Δp,Δψ]。
本发明的优点及有益效果如下:
目前大多数没有进行组合定位的相互容错判定,在建筑结构复杂的室内环境中,UWB定位易受到非视距问题、多径效应和人体的影响出现粗大误差,可能导致惯性导航/UWB定位精度下降,甚至错误地辅助修正惯性导航。
本发明提出了一种基于容错决策树的UWB辅助惯性定位方法,针对惯性导航系统误差随时间累积和UWB定位受到非视距问题、多径效应和人体影响出现粗大误差的问题,利用惯性导航系统对载体自身的角速率和加速度进行实时测量,解算得到姿态信息和相对位置导航信息后,结合UWB定位辅助增强室内定位精度,实现多传感器多源定位信息下的连续性可靠定位。创新在于提出并采用陀螺仪高精度分段拟合误差补偿模型,抑制惯性导航误差漂移;同时在UWB辅助人员室内惯性定位的基础上,构建惯性导航与UWB单点定位数据共同作用的容错决策树判定模型,剔除UWB定位的粗大误差因子,进而对惯性导航和UWB参数差值应用扩展卡尔曼滤波,实现UWB辅助增强惯性导航定位。UWB设备的布设将针对不同的室内环境而定,在复杂狭窄巷道环境时,两端布设UWB基站进行辅助;常规环境下,在同一楼层拐点或分叉口布设UWB基站进行辅助,合理减少室内环境中UWB基站数量。UWB所获得的定位数据用于辅助修正惯性导航解算的定位信息,降低系统随时间累积误差,能够长时间的连续可靠定位,具有很强的工程应用价值。
附图说明
图1是本发明的基于容错决策树的UWB辅助惯性定位系统图;
图2是本发明的UWB单点定位数据的容错决策树判定流程。
具体实施方式
下面对本发明实施例中的技术方案进行清楚、详细地描述。所描述的实施例仅仅是本发明的一部分实施例。
本发明解决上述技术问题的技术方案是:
本发明提供了一种基于容错决策树的UWB辅助惯性定位方法,针对惯性导航系统误差随时间累积和UWB定位受到非视距问题、多径效应和人体影响出现粗大误差的问题,提出并采用陀螺仪高精度分段拟合误差补偿模型,抑制惯性导航误差漂移;同时在UWB辅助人员室内惯性定位的基础上,构建惯性导航与UWB单点定位数据共同作用的容错决策树判定模型,剔除UWB定位的粗大误差因子,进而对惯性导航和UWB参数差值应用扩展卡尔曼滤波,实现UWB辅助增强惯性导航定位,能够长时间的连续可靠定位,具有很强的工程应用价值。
进一步地,所述室内惯性导航定位系统采用地理坐标系即东北天坐标系(n系),使用加速度解算的初始俯仰角、横滚角,再加上磁力计进行共同解算,得到姿态角信息数据。设θ,γ,ψ分别为俯仰角、横滚角和航向角。故得:
ax、ay、az分别代表x、y、z轴的加速度数据,分别代表x、y方向磁力计数据。
进一步地,所述陀螺仪高精度分段拟合误差补偿模型,为了进一步减小误差,分析了传感器自身的安装标定误差、非线性误差等,利用公式(2)角速率输出误差模型,从而得到陀螺仪的补偿矩阵:
ωx、ωy、ωz分别代表x、y、z轴的陀螺仪真实角速率,Sx、Sz、Sy代表x、y、z轴的陀螺仪标定因数,Kyx、Kzx、Kxy、Kzy、Kxz、Kyz分别代表x、y、z轴的陀螺仪安装误差系数,Bx、By、Bz分别代表x、y、z轴的陀螺仪零偏,wx、wy、wz分别代表x、y、z轴的陀螺仪实际角速率测量值。
在高精度分段拟合误差补偿的实验中,选取三轴转台动态速率为-100°/s~100°/s,以10°/s作为间隔,共20个角速率,每组采集保留500kb数据。按照速率数据正负对称和动态速率高低,将20个角速率分为8组,每段包含的角速率密度为m,在每个分段组中对其数据进行二次拟合,得到该分段组更为接近真实角速率的标定因数。以x轴为例:
Sx代表x轴的标定因数,S-100……S-100分别代表-100°/s~100°/s的20个角速率所对应的标定因数,ω代表陀螺仪角速率。
经过高精度分段拟合误差补偿后,可以区分补偿高低动态速率段,更进一步消除了角速率输出误差,减少了1~2个数量级。
进一步地,所述初始加速度计/磁力计组合姿态解算采用四元数算法对传感器测量值进行姿态解算,利用惯性导航姿态解算的四元数参考值,实时计算出坐标矩阵,结合姿态转换算法,即可以解算出i时刻三轴俯仰角、横滚角、航向角:
q0代表四元数的标量,q1、q2、q3代表四元数的矢量。
进一步地,所述根据加速度特征数据仿真,发现行人走路的波峰、零点和波谷数据有似正弦波的周期性规律,利用汉明窗提取出特征数据。但由于伪波峰会被误判出现在行走身体晃动时,进一步消除其造成的步数误判采用阈值判别法:
式中,ax为加速度计X轴测量值,ay为加速度计Y轴测量值,az为加速度计Z轴测量值。
利用加速度与人员行走时刻步长的波峰波谷差值之间的关联性,采用以下公式进行自适应步长计算:
式中,j表示行人的第j步,Lj表示行人第j步的实时步长,K表示行人个体的步长标定参考值,amax,j表示在第j步行人自身目标加速度的最大波峰值,amin,j表示在第j步行人自身目标加速度的最小波谷值。
进一步地,所述UWB定位系统解算,本发明使用P440定位测距通信模块,它使用双向飞行时间(TW-TOF)测量来测量两个或更多UWB模块之间的距离,其可以提供具有短脉冲的精确时间测量,同时保持对多径信号传播的鲁棒性。
首先分别得到模块A、B的脉冲发送和接收时刻之间的传递时间Ta、Tb:
再利用无线电信号传播的速度即光速C,可以计算两个实体的距离P:
选用三边测距定位方法计算行人当前k时刻的移动位置最优解Puwb,k(xn,yn):
式中,xn、yn分别表示由n个UWB定位基站共同解算的标签x、y轴移动位置最优解,Pn表示标签到第n个定位基站的距离。
进一步地,所述UWB单点定位数据的容错决策树判定,针对UWB定位在室内环境中,存在易受到遮挡物引起的非视距(NLOS)误差以及信号不稳定等多种影响定位精度情况,先行对UWB是否接收到多边基站信号和出现粗大误差进行有效判定,并将在容错判定开始前将当前k时刻的UWB单点定位数据作为输入量。惯性导航解算的定位数据作为决策树容错判定的参考量共同构建模型流程,如图2所示。
进一步地,所述UWB单点定位数据的容错决策树判定,首先进去到第一层容错判定,需要获知UWB标签是否接收到定位基站的脉冲信号,则利用Tuwb标志位判定是否存在UWB单点定位数据。
当标志位为1时,exist代表存在满足条件的当前k时刻的UWB单点定位数据,反之亦然。通过Tuwb标志位判定后,利用UWB单点定位数据获得其标签与定位基站之间的飞行时间后,使用TW-TOF和三边测距定位方法计算行人当前k时刻的移动位置最优解Puwb,k(xn,yn)。
通过第一层容错判定,若不存在UWB单点定位数据,单边基站所得到定位数据不具可信度,则不使用UWB辅助惯性导航解算。如果存在UWB单点定位数据,由于UWB数据存在粗大误差在第一层容错判定中无法完全消除,需结合惯性导航定位数据作为参考量进一步提高可信度。
进一步地,所述UWB单点定位数据的容错决策树判定,接下来进入第二层容错判定,根据行人运动规律特性,安放在人员腰间的定位模块在出现航向角度大幅度变化时,UWB数据存在粗大误差的几率大大降低。故利用当前k时刻惯导解算对应的航向角ψk和初始航向角ψzero进行判断行人是否发生转弯模式变化。
式中,Δψ表示k时刻航向角与初始航向角的绝对差值,ε代表判断行人转弯模式的活动阈值。
从第二层容错判定中可以看出,行人转弯模式是对UWB粗大误差的剔除,在确定转弯判定能接收到基本精确的UWB单点定位数据后,需结合惯性导航解算的位置数据和行人步数进行判断位置条件,进入第三层容错判定。
进一步地,所述UWB单点定位数据的容错决策树判定,根据在行走定位过程中的运动特性,佩戴人员在常规情况下,其前后采样点的位置不会突变,应该满足UWB位置和惯性导航位置之间的距离差条件。并且随着运动时间累积,其距离差值将会保持在合理可接受的范围内。因此采用同一采样点的惯性导航与UWB定位信息之差Δp进行测量数据的活动阈值判定。
式中,Pins,k代表惯性导航在k时刻的位置数据,Puwb,k代表UWB在k时刻单点定位距离数据,σ代表惯性导航与UWB位置数据之差的活动阈值。
考虑到随时间累积后定位误差的线性变化,若UWB和惯性导航解算位置差保持在合理的范围内,允许其活动阈值σ存在波动。由于在时域中无法将每一时刻与距离误差相一一对应,故使用行人运动步数与距离之间的关系,其活动阈值σ能同时满足下列行走步数范围的关系式:
式中,step表示加速度解算的有效步数,σ1、σ2、σ3表示分段步数对应的阈值。
经过容错决策树的三层判定,最终实现了利用惯性导航解算的定位信息与UWB单点定位数据作为参考量共同构建容错决策树模型流程,大大提高了定位精度。
进一步地,所述基于EKF滤波的惯性导航/UWB定位信息融合,通过UWB单点稀疏定位数据决策树容错判定后得到了当前时刻稳定的位置信息,将其作为观测信息与惯性导航解算数据进行扩展卡尔曼滤波,其状态方程与观测方程如下:
式中,k为离散时间所对应的时刻,系统在k时刻的状态Xk∈Rn;Zk∈Rm为对应状态的观测信号,Φk为状态转移矩阵,Gk为状态噪声驱动矩阵,Wk∈Rn为输入信号的白噪声,φk为非随机的外作用项,H为观测矩阵,Vk∈Rm为观测信号的白噪声。
设计X=[δp,δv,δφ,δa]T为的该EKF滤波状态向量,包含12个状态量,其中行人的位置误差δp(δpx、δpy、δpz)、惯性导航东北天坐标的速度误差δv(δvE、δvN、δvU)、当前时刻惯性导航所解算的姿态角误差δφ(δφγ、δφθ、δφψ)以及三轴加速度误差δa(δax、δay、δaz)作为状态向量,E、N、U分别为东北天坐标系中的东向、北向和天向位置。
针对姿态角误差补偿,当初始时刻检测到开机站立时,默认此时惯性导航解算的姿态角为初始姿态角。将惯性导航解算的实际姿态角与初始姿态角之差Δψ作为当前时刻姿态角的观测量。针对位置误差补偿,使用惯性导航与UWB解算的位置信息之差作为当前时刻位置的观测量Δp。观测向量为Z=[Δp,Δψ]。其中:
wΔp、wΔψ分别为对应观测量的观测模型噪声。在这个过程中,wΔp、wΔψ假设相互独立且满足高斯白噪声。首先对状态方程进行求导可得Φk为12×12的状态转移矩阵。
Hk观测量的转换矩阵:
本发明中使用的惯性定位模块采样频率为50Hz,UWB模块采样频率为10Hz。因此仅使用经过容错决策树后的UWB测量数据与惯性导航解算的数据进行EKF滤波定位信息融合。
本发明提供的一种基于容错决策树的UWB辅助惯性定位方法,针对惯性导航系统误差随时间累积和UWB定位受到非视距问题、多径效应和人体影响出现粗大误差的问题,提出并采用陀螺仪高精度分段拟合误差补偿模型,抑制惯性导航误差漂移;同时在UWB辅助人员室内惯性定位的基础上,构建惯性导航与UWB单点定位数据共同作用的容错决策树判定模型,剔除UWB定位的粗大误差因子,进而对惯性导航和UWB参数差值应用扩展卡尔曼滤波,实现UWB辅助增强惯性导航定位,能够长时间的连续可靠定位,具有很强的工程应用价值。以上这些实施例应理解为仅用于说明本发明而不用于限制本发明的保护范围。在阅读了本发明的记载的内容之后,技术人员可以对本发明作各种改动或修改,这些等效变化和修饰同样落入本发明权利要求所限定的范围。
Claims (10)
1.一种基于容错决策树的UWB辅助惯性定位方法,其特征在于,包括以下步骤:
步骤一,使用加速度和磁力计解算得到初始横滚角、俯仰角和航向角;
步骤二,采用陀螺仪分段拟合误差补偿模型区分补偿高低动态速率段,更进一步消除角速率输出误差,并根据步骤二中的初始横滚角、俯仰角和航向角,采用四元数算法获得姿态角信息数据;
步骤三,根据加速度特征数据,发现行人走路的波峰、零点和波谷数据有似正弦波的周期性规律,获得有效步数,并利用加速度与人员行走时刻步长的波峰波谷差值之间的关联性,进行自适应步长计算,得到惯性定位数据信息;
步骤四,采用UWB双向飞行时间测量UWB模块间距离,随后根据三边测距定位方法计算行人当前时刻的位置;
步骤五,将步骤四获得的UWB行人当前时刻的位置作为容错决策树判定模型的输入量,步骤三获得的惯性定位数据作为容错决策树判定模型的参考量,进行容错判定,得到当前时刻UWB单点定位数据;
步骤六,将步骤三获得的惯性定位数据和步骤四获得的UWB单点定位数据进行扩展卡尔曼滤波定位信息融合。
2.根据权利要求1所述一种基于容错决策树的UWB辅助惯性定位方法,其特征在于:步骤二所述陀螺仪分段拟合误差补偿模型,分析传感器自身的安装标定误差和非线性误差,利用公式(1)角速率输出误差模型,从而得到陀螺仪的补偿矩阵:
ωx、ωy、ωz分别代表x、y、z轴的陀螺仪真实角速率,Sx、Sz、Sy代表x、y、z轴的陀螺仪标定因数,Kyx、Kzx、Kxy、Kzy、Kxz、Kyz代表x、y、z轴的陀螺仪安装误差系数,Bx、By、Bz分别代表x、y、z轴的陀螺仪零偏,wx、wy、wz分别代表x、y、z轴的陀螺仪实际角速率测量值。
3.根据权利要求2所述一种基于容错决策树的UWB辅助惯性定位方法,其特征在于:所述分段拟合误差补偿中,选取三轴转台动态速率为-100°/s~100°/s,以10°/s作为间隔,共20个角速率,每组采集保留500kb数据,按照速率数据正负对称和动态速率高低,将20个角速率分为8组,每段包含的角速率密度为m,在每个分段组中对其数据进行二次拟合,得到该分段组更为接近真实角速率的标定因数。
4.根据权利要求1或2或3所述一种基于容错决策树的UWB辅助惯性定位方法,其特征在于:所述容错判定包括,第一层容错判定,获知UWB标签是否接收到定位基站的脉冲信号,判定是否存在UWB单点定位数据;第二层容错判定,根据行人运动规律特性,利用当前k时刻惯导解算对应的航向角ψk和初始航向角ψzero进行判断行人是否发生转弯模式变化;第三层容错判定,采用同一采样点的姿态角信息与UWB定位信息之差Δp,并结合行人运动步数与距离之间的关系,进行测量数据的活动阈值判定。
5.根据权利要求4所述一种基于容错决策树的UWB辅助惯性定位方法,其特征在于:所述第一层容错判定具体包括利用Tuwb标志位判定是否存在UWB单点定位数据;
当标志位为1时,exist代表存在满足条件的当前k时刻的UWB单点定位数据,反之亦然;
通过Tuwb标志位判定后,利用UWB单点定位数据获得其标签与定位基站之间的飞行时间后,使用TW-TOF和三边测距定位方法计算行人当前k时刻的移动位置最优解Puwb,k(xn,yn)。
6.根据权利要求5所述一种基于容错决策树的UWB辅助惯性定位方法,其特征在于:所述第三层容错判定的Δp规定如下:
式中,Pins,k代表惯性导航在k时刻的位置数据,Puwb,k代表UWB在k时刻单点定位距离数据,σ代表惯性导航与UWB位置数据之差的活动阈值。
7.根据权利要求6所述一种基于容错决策树的UWB辅助惯性定位方法,其特征在于:所述活动阈值σ能同时满足下列行走步数范围的关系式:
式中,step表示加速度解算的有效步数,σ1、σ2、σ3表示分段步数对应的阈值。
8.根据权利要求1或2或3或5或6或7所述一种基于容错决策树的UWB辅助惯性定位方法,其特征在于:步骤六所述扩展卡尔曼滤波的状态方程与观测方程如下:
式中,k为离散时间所对应的时刻,系统在k时刻的状态Xk∈Rn;Zk∈Rm为对应状态的观测信号,Φk为状态转移矩阵,Gk为状态噪声驱动矩阵,Wk∈Rn为输入信号的白噪声,φk为非随机的外作用项,Hk为观测矩阵,Vk∈Rm为观测信号的白噪声。
9.根据权利要求8所述一种基于容错决策树的UWB辅助惯性定位方法,其特征在于:令X=[δp,δv,δφ,δa]T为所述扩展卡尔曼滤波的状态向量,包含12个状态量,其中行人的位置误差δp、惯性导航东北天坐标的速度误差δv、当前时刻惯性导航所解算的姿态角误差δφ以及三轴加速度误差δa作为状态向量。
10.根据权利要求8所述一种基于容错决策树的UWB辅助惯性定位方法,其特征在于:所述扩展卡尔曼滤波,针对姿态角误差补偿,当初始时刻检测到开机站立时,默认此时惯性导航解算的姿态角为初始姿态角,将惯性导航解算的实际姿态角与初始姿态角之差Δψ作为当前时刻姿态角的观测量;针对位置误差补偿,使用惯性导航与UWB解算的位置信息之差作为当前时刻位置的观测量Δp,则观测向量为Z=[Δp,Δψ]。
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN201910053745.7A CN109682375B (zh) | 2019-01-21 | 2019-01-21 | 一种基于容错决策树的uwb辅助惯性定位方法 |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN201910053745.7A CN109682375B (zh) | 2019-01-21 | 2019-01-21 | 一种基于容错决策树的uwb辅助惯性定位方法 |
Publications (2)
Publication Number | Publication Date |
---|---|
CN109682375A true CN109682375A (zh) | 2019-04-26 |
CN109682375B CN109682375B (zh) | 2022-08-23 |
Family
ID=66193488
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN201910053745.7A Active CN109682375B (zh) | 2019-01-21 | 2019-01-21 | 一种基于容错决策树的uwb辅助惯性定位方法 |
Country Status (1)
Country | Link |
---|---|
CN (1) | CN109682375B (zh) |
Cited By (12)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN110243363A (zh) * | 2019-07-03 | 2019-09-17 | 西南交通大学 | 一种基于低成本imu与rfid技术结合的agv实时定位方法 |
CN110567461A (zh) * | 2019-08-01 | 2019-12-13 | 北京航空航天大学 | 一种考虑无陀螺仪的非合作航天器姿态和参数估计方法 |
CN110940334A (zh) * | 2019-10-23 | 2020-03-31 | 山东笛卡尔智能科技有限公司 | 一种人体行走测速徽章及测速方法 |
CN110986936A (zh) * | 2019-12-17 | 2020-04-10 | 武汉理工大学 | 一种基于边缘计算的客轮人员定位导航方法 |
CN111197983A (zh) * | 2020-01-15 | 2020-05-26 | 重庆邮电大学 | 基于人体分布惯性节点矢量测距的三维位姿测量方法 |
CN112527010A (zh) * | 2020-11-09 | 2021-03-19 | 福州大学 | 基于人工势场与粒子优化的室内变电站无人机多机协同巡检方法 |
CN112747747A (zh) * | 2021-01-20 | 2021-05-04 | 重庆邮电大学 | 一种改进的uwb/imu融合室内行人定位方法 |
CN113074739A (zh) * | 2021-04-09 | 2021-07-06 | 重庆邮电大学 | 基于动态鲁棒容积卡尔曼的uwb/ins融合定位方法 |
CN113155128A (zh) * | 2021-03-31 | 2021-07-23 | 西安电子科技大学 | 基于合作型博弈的uwb和惯性导航的室内行人定位方法 |
CN114353825A (zh) * | 2021-12-06 | 2022-04-15 | 重庆邮电大学 | 基于无迹卡尔曼滤波的磁力计在线校准算法、介质及系统 |
CN114526727A (zh) * | 2022-01-11 | 2022-05-24 | 重庆邮电大学 | 一种基于决策树的步数判别方法 |
CN114765564A (zh) * | 2020-12-30 | 2022-07-19 | 华为技术有限公司 | 一种家居设备的控制方法及电子设备 |
Citations (7)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN105890600A (zh) * | 2016-04-14 | 2016-08-24 | 南京大学 | 基于手机传感器的地铁乘客位置推测方法 |
CN106772231A (zh) * | 2016-12-23 | 2017-05-31 | 深圳市金溢科技股份有限公司 | 基于uwb的电子标签、基站、定位系统及方法 |
CN107014375A (zh) * | 2017-02-22 | 2017-08-04 | 上海谦尊升网络科技有限公司 | 超低部署的室内定位系统及方法 |
WO2017149526A2 (en) * | 2016-03-04 | 2017-09-08 | May Patents Ltd. | A method and apparatus for cooperative usage of multiple distance meters |
CN108369643A (zh) * | 2016-07-20 | 2018-08-03 | 优森公司 | 用于3d手部骨架跟踪的方法和系统 |
CN108562301A (zh) * | 2018-05-21 | 2018-09-21 | 北京石油化工学院 | 一种行驶路径的规划方法及装置 |
US20180375940A1 (en) * | 2012-01-09 | 2018-12-27 | May Patents Ltd. | System and method for server based control |
-
2019
- 2019-01-21 CN CN201910053745.7A patent/CN109682375B/zh active Active
Patent Citations (7)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
US20180375940A1 (en) * | 2012-01-09 | 2018-12-27 | May Patents Ltd. | System and method for server based control |
WO2017149526A2 (en) * | 2016-03-04 | 2017-09-08 | May Patents Ltd. | A method and apparatus for cooperative usage of multiple distance meters |
CN105890600A (zh) * | 2016-04-14 | 2016-08-24 | 南京大学 | 基于手机传感器的地铁乘客位置推测方法 |
CN108369643A (zh) * | 2016-07-20 | 2018-08-03 | 优森公司 | 用于3d手部骨架跟踪的方法和系统 |
CN106772231A (zh) * | 2016-12-23 | 2017-05-31 | 深圳市金溢科技股份有限公司 | 基于uwb的电子标签、基站、定位系统及方法 |
CN107014375A (zh) * | 2017-02-22 | 2017-08-04 | 上海谦尊升网络科技有限公司 | 超低部署的室内定位系统及方法 |
CN108562301A (zh) * | 2018-05-21 | 2018-09-21 | 北京石油化工学院 | 一种行驶路径的规划方法及装置 |
Non-Patent Citations (6)
Title |
---|
ALI KHALAJMEHRABADI: "Modern WLAN Fingerprinting Indoor Positioning Methods and Deployment Challenges", 《IEEE COMMUNICATIONS SURVEYS & TUTORIALS》 * |
ZAHER MERHI: "A Lightweight Collaborative Fault Tolerant Target Localization System for Wireless Sensor Networks", 《IEEE TRANSACTIONS ON MOBILE COMPUTING》 * |
徐龙阳: "一种基于智能手机的行人航位推算室内定位方法", 《电脑知识与技术》 * |
杨海: "SINS/WSN组合定位下采煤机精确位姿感知理论及技术研究", 《中国优秀博硕士学位论文全文数据库(博士)工程科技Ⅰ辑》 * |
杨海: "复杂坏境下基于SINS/UWB的容错组合定位技术研究", 《仪器仪表学报》 * |
范建英: "高精度数字陀螺仪安装误差标定与补偿方法", 《传感技术学报》 * |
Cited By (17)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN110243363A (zh) * | 2019-07-03 | 2019-09-17 | 西南交通大学 | 一种基于低成本imu与rfid技术结合的agv实时定位方法 |
CN110243363B (zh) * | 2019-07-03 | 2020-07-17 | 西南交通大学 | 一种基于低成本imu与rfid技术结合的agv实时定位方法 |
CN110567461A (zh) * | 2019-08-01 | 2019-12-13 | 北京航空航天大学 | 一种考虑无陀螺仪的非合作航天器姿态和参数估计方法 |
CN110940334A (zh) * | 2019-10-23 | 2020-03-31 | 山东笛卡尔智能科技有限公司 | 一种人体行走测速徽章及测速方法 |
CN110986936A (zh) * | 2019-12-17 | 2020-04-10 | 武汉理工大学 | 一种基于边缘计算的客轮人员定位导航方法 |
CN111197983A (zh) * | 2020-01-15 | 2020-05-26 | 重庆邮电大学 | 基于人体分布惯性节点矢量测距的三维位姿测量方法 |
CN111197983B (zh) * | 2020-01-15 | 2022-12-27 | 重庆邮电大学 | 基于人体分布惯性节点矢量测距的三维位姿测量方法 |
CN112527010A (zh) * | 2020-11-09 | 2021-03-19 | 福州大学 | 基于人工势场与粒子优化的室内变电站无人机多机协同巡检方法 |
CN114765564A (zh) * | 2020-12-30 | 2022-07-19 | 华为技术有限公司 | 一种家居设备的控制方法及电子设备 |
CN112747747B (zh) * | 2021-01-20 | 2022-10-11 | 重庆邮电大学 | 一种改进的uwb/imu融合室内行人定位方法 |
CN112747747A (zh) * | 2021-01-20 | 2021-05-04 | 重庆邮电大学 | 一种改进的uwb/imu融合室内行人定位方法 |
CN113155128A (zh) * | 2021-03-31 | 2021-07-23 | 西安电子科技大学 | 基于合作型博弈的uwb和惯性导航的室内行人定位方法 |
CN113074739A (zh) * | 2021-04-09 | 2021-07-06 | 重庆邮电大学 | 基于动态鲁棒容积卡尔曼的uwb/ins融合定位方法 |
CN114353825A (zh) * | 2021-12-06 | 2022-04-15 | 重庆邮电大学 | 基于无迹卡尔曼滤波的磁力计在线校准算法、介质及系统 |
CN114353825B (zh) * | 2021-12-06 | 2023-11-03 | 重庆邮电大学 | 基于无迹卡尔曼滤波的磁力计在线校准算法、介质及系统 |
CN114526727A (zh) * | 2022-01-11 | 2022-05-24 | 重庆邮电大学 | 一种基于决策树的步数判别方法 |
CN114526727B (zh) * | 2022-01-11 | 2023-11-21 | 重庆邮电大学 | 一种基于决策树的步数判别方法 |
Also Published As
Publication number | Publication date |
---|---|
CN109682375B (zh) | 2022-08-23 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
CN109682375A (zh) | 一种基于容错决策树的uwb辅助惯性定位方法 | |
CN105628024B (zh) | 基于多传感器融合的单人定位导航器和定位导航方法 | |
CN102519450B (zh) | 一种用于水下滑翔器的组合导航装置及方法 | |
CN104655137B (zh) | 行人航迹推测辅助的Wi‑Fi信号指纹定位算法 | |
Sun et al. | Indoor positioning tightly coupled Wi-Fi FTM ranging and PDR based on the extended Kalman filter for smartphones | |
CN109807911B (zh) | 基于gnss、uwb、imu、激光雷达、码盘的室外巡逻机器人多环境联合定位方法 | |
CN110057354B (zh) | 一种基于磁偏角修正的地磁匹配导航方法 | |
CN109827577A (zh) | 基于运动状态检测的高精度惯性导航定位算法 | |
CN108426574A (zh) | 一种基于zihr的航向角修正算法的mems行人导航方法 | |
CN106908060A (zh) | 一种基于mems惯性传感器的高精度室内定位方法 | |
Ladetto et al. | In step with INS navigation for the blind, tracking emergency crews | |
CN110686671B (zh) | 基于多传感器信息融合的室内3d实时定位方法及装置 | |
CN108844533A (zh) | 一种基于多传感器融合和姿态解算的自由姿态pdr定位方法 | |
CN107907900A (zh) | 一种gnss双天线辅助的多传感器组合导航系统及方法 | |
CN103487050A (zh) | 一种室内移动机器人定位方法 | |
CN102128625A (zh) | 重力辅助惯性导航系统中重力图匹配的初始匹配方法 | |
CN108759815A (zh) | 一种用于全局视觉定位方法中的信息融合组合导航方法 | |
Luo et al. | Indoor multi-floor 3D target tracking based on the multi-sensor fusion | |
CN111025366A (zh) | 基于ins及gnss的网格slam的导航系统及方法 | |
CN112923919A (zh) | 基于图优化的行人定位方法及系统 | |
CN113325455B (zh) | 用于跟踪和确定物体的室内位置的方法和系统 | |
Su et al. | Sensor-aided personal navigation systems for handheld devices | |
CN109781138A (zh) | 一种基于浮空平台的风速风向测量方法及装置 | |
CN113029173A (zh) | 车辆导航方法及装置 | |
CN108332749B (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 | ||
GR01 | Patent grant | ||
GR01 | Patent grant |