CN111896008A - 一种改进的鲁棒无迹卡尔曼滤波组合导航方法 - Google Patents
一种改进的鲁棒无迹卡尔曼滤波组合导航方法 Download PDFInfo
- Publication number
- CN111896008A CN111896008A CN202010843690.2A CN202010843690A CN111896008A CN 111896008 A CN111896008 A CN 111896008A CN 202010843690 A CN202010843690 A CN 202010843690A CN 111896008 A CN111896008 A CN 111896008A
- Authority
- CN
- China
- Prior art keywords
- error
- measurement
- state
- covariance
- robust
- 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
- 238000001914 filtration Methods 0.000 title claims abstract description 36
- 238000000034 method Methods 0.000 title claims abstract description 26
- 238000005259 measurement Methods 0.000 claims abstract description 64
- 239000011159 matrix material Substances 0.000 claims abstract description 26
- 238000004364 calculation method Methods 0.000 claims abstract description 10
- 230000008569 process Effects 0.000 claims description 5
- 230000007704 transition Effects 0.000 claims description 4
- 238000001514 detection method Methods 0.000 claims description 3
- 230000009897 systematic effect Effects 0.000 claims description 3
- 238000004422 calculation algorithm Methods 0.000 abstract description 52
- 238000012545 processing Methods 0.000 abstract description 6
- 230000003044 adaptive effect Effects 0.000 abstract description 4
- 230000035772 mutation Effects 0.000 abstract description 3
- 230000002708 enhancing effect Effects 0.000 abstract description 2
- 230000033001 locomotion Effects 0.000 description 10
- 238000004891 communication Methods 0.000 description 8
- 238000005516 engineering process Methods 0.000 description 5
- 230000008859 change Effects 0.000 description 3
- 238000011161 development Methods 0.000 description 3
- 238000010586 diagram Methods 0.000 description 3
- 230000001133 acceleration Effects 0.000 description 2
- 230000000694 effects Effects 0.000 description 2
- 238000013178 mathematical model Methods 0.000 description 2
- 230000001105 regulatory effect Effects 0.000 description 2
- 238000004088 simulation Methods 0.000 description 2
- 230000009466 transformation Effects 0.000 description 2
- 238000004458 analytical method Methods 0.000 description 1
- 230000009286 beneficial effect Effects 0.000 description 1
- 239000000969 carrier Substances 0.000 description 1
- 238000006243 chemical reaction Methods 0.000 description 1
- 230000007547 defect Effects 0.000 description 1
- 238000009795 derivation Methods 0.000 description 1
- 238000013461 design Methods 0.000 description 1
- 230000007613 environmental effect Effects 0.000 description 1
- 238000005562 fading Methods 0.000 description 1
- 238000012986 modification Methods 0.000 description 1
- 230000004048 modification Effects 0.000 description 1
- 239000011541 reaction mixture Substances 0.000 description 1
- 238000011160 research Methods 0.000 description 1
- 235000002020 sage Nutrition 0.000 description 1
- 238000012360 testing method Methods 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
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和MEMS惯导定位原理，建立了UWB/MEMS紧组合数学模型，该模型比常规的UWB/MEMS松组合模型定位精度更高，对环境适应能力更强。本发明针对量测突变噪声和野值，在传统鲁棒算法的基础上引入测量误差判据，增强了对野值的处理能力。本发明针对传统鲁棒算法对部分观测受干扰调节能力差的问题，并结合UWB测距误差的实际情况，提出一种多重鲁棒自适应矩阵的计算方法，该算法针对各量测不同的误差情况，实时调节滤波增益矩阵，使滤波器具有更强的自适应性。
Description
技术领域
本发明涉及组合导航技术领域，具体为一种改进的鲁棒无迹卡尔曼滤波组合导航方法。
背景技术
近几年来，随着科技的发展和人们生活的改变，基于位置服务(Location-basedservices，LBS)在工业和市场上都有着广泛的应用背景。现如今，全球导航卫星***(Global Navigation Satellite System，GNSS)飞速发展，其在室外环境下可以为用户提供高精度的位置服务。然而，在室内环境下，GNSS信号极易受遮挡，使得用户无法通过GNSS实现正常的导航定位。同时，人类绝大部分的活动发生于室内环境下，高精度室内定位服务可以为用户提供路线导航、商品导购、资源引导等网络化服务，因此，高精度的室内定位技术具有广阔的应用前景。
作为LBS的核心内容之一，室内定位***的设计以及成为该领域的热点。现如今，室内定位技术普遍采用无线定位，无线定位***的缺陷是其定位效果易受到非视距(NLOS,non line of sight)等环境的影响。应用较广泛的室内定位技术主要有无线局域网(Wireless Local Area Networks，WLAN)、射频识别(Radio Frequence Identification，RFID)、超宽带(Ultra-wide Band，UWB)、蓝牙等，其中，UWB具有超大的带宽、良好的抗多径干扰能力等优点，更适合用于提供室内高精度定位服务。然而，UWB定位精度易受非视距环境的影响。因此，许多学者提出将两种或两种以上的传感器进行融合来弥补单一传感器导航***的缺点。随着微电子机械***(Micro Electro Mechanical System，MEMS)技术的兴起和发展，使得传统惯性导航设备在体积、成本等方面得到有效降低。基于UWB/MEMS的组合导航***能够充分利用各自的优势，提高***定位精度和稳定性，具有广阔的发展前景。
在室内组合导航领域，UWB/MEMS组合导航研究大致分为两类：一种是通过研究UWB/MEMS组合方式来提高室内定位精度和解决UWB失效时定位发散的问题。这些多传感器组合虽然都在一定程度上提高了组合定位精度，但引入其他传感器也使得其在工程上的实现变得更加困难。另一种是从滤波的角度来解决UWB的噪声和野值问题。虽然有学者从滤波的角度提出采用强跟踪Sage自适应滤波解决UWB定位噪声的问题，但强跟踪中的渐消因子若选取不当，会导致滤波精度降低，且其对野值的处理能力较差。
发明内容
本发明的目的是：针对现有技术中滤波精度低，进而导致定位精度差，且其对野值的处理能力较差的问题，提出一种改进的鲁棒无迹卡尔曼滤波组合导航方法。
本发明为了解决上述技术问题采取的技术方案是：
一种改进的鲁棒无迹卡尔曼滤波组合导航方法，包括以下几个步骤：
步骤一：收集UWB***和MEMS***输出的数据；
步骤二：选择***的状态量和观测量，建立UWB/MEMS组合导航***状态空间模型；
步骤三：对UWB/MEMS组合导航***进行滤波初始化；
步骤四：在k时刻对UWB/MEMS组合导航***进行时间更新和量测更新，得到***状态一步预测的误差协方差和一步预测的互协方差；
步骤五：引入量测误差判据，若量测存在误差，则计算多重因子的鲁棒矩阵，并对状态一步预测的误差协方差进行校正，然后进入步骤六，若量测不存在误差，则直接进入步骤六；
步骤六：若量测存在误差，则利用校正后的状态一步预测误差协方差，计算k时刻***的状态估计值和状态误差协方差，若量测不存在误差，则利用步骤四中的状态一步预测误差协方差，计算k时刻***的状态估计值和状态误差协方差；
步骤七：根据步骤六中得到的状态估计值获取MEMS惯导的陀螺仪、加速度计的常值漂移，并将其反馈到组合***中，输出姿态、速度和位置信息。
进一步的，所述UWB/MEMS组合导航***状态空间模型的***状态方程表示为：
Xk+1＝FkXk+wk
其中，Xk为15维的惯导解算信息误差，表示为
其中，φk＝[φk,x φk,y φk,z]T为k时刻惯导解算的姿态误差，ΔVk＝[ΔVk,x ΔVk,y ΔVk,z]T为k时刻惯导解算的地理坐标系下的速度误差，ΔPk＝[ΔLk Δλk Δhk]T为k时刻惯导解算的经纬高误差，εk＝[εk,x εk,y εk,z]T为k时刻陀螺常值漂移，
为k时刻加速度计零偏，wk为k时刻组合***过程噪声，wk噪声协方差矩阵为Qk，Fk为k时刻惯导***状态转移矩阵。
进一步的，所述UWB/MEMS组合导航***状态空间模型的***量测方程表示为：
Zk＝h(X'k)+Vk
其中，Zk为***量测输入，h(X'k)为***非线性量测函数，Vk为组合***量测噪声，Vk噪声协方差矩阵为Rk，h(X'k)和Vk分别表示为：
Vk＝-2ρUi,kε+ε2
其中，X'k包含惯导在地心地固坐标系下的位置误差Δxk、Δyk，其中Δxk、Δyk与状态量Xk中惯导在地理坐标系下的位置误差ΔLk、Δλk、Δhk的关系表示为：
Δxk＝-(Rn+hk)cosλk sinLkΔLk-(Rn+hk)cosLk sinλkΔλk+cosLk cosλkΔhk
Δyk＝-(Rn+hk)sinλk sinLkΔLk+(Rn+hk)cosLk cosλkΔλk+cosLksinλkΔhk
其中，e为地球离心率，Rn为地球子午圈半径。
进一步的，所述步骤五中量测误差判据具体表示为：
λk＞β时，量测存在误差；
λk＜β时，量测不存在误差；
其中，β为门限值，λk为检测函数，λk服从自由度为m的χ2分布，即λk～χ2(m)，λk具体表示为：
其中，vk为***k时刻的新息序列，
为新息序列vk的理论协方差。
进一步的，所述步骤五中多重因子的鲁棒矩阵表示为：
其中，Sk为多重因子的鲁棒调节矩阵，sk(i,i)表示第i个观测量的鲁棒调节因子，sk(i,i)计算方法如下：
其中，
为新息理论协方差对角线的值，Yk(i,i)为新息实际协方差对角线的值。
本发明的有益效果是：
(1)本发明根据UWB和MEMS惯导定位原理，建立了UWB/MEMS紧组合数学模型，该模型比常规的UWB/MEMS松组合模型定位精度更高，对环境适应能力更强。
(2)本发明针对量测突变噪声和野值，在传统鲁棒算法的基础上引入测量误差判据，增强了对野值的处理能力。
(3)本发明针对传统鲁棒算法对部分观测受干扰调节能力差的问题，并结合UWB测距误差的实际情况，提出一种多重鲁棒自适应矩阵的计算方法，该算法针对各量测不同的误差情况，实时调节滤波增益矩阵，使滤波器具有更强的自适应性。
附图说明
图1为本发明的原理图；
图2为本发明的方针运动轨迹图；
图3为UWB、MEMS、UWB/MEMS松组合和UWB/MEMS紧组合算法解算的载体轨迹对比图；
图4(a)为姿态角误差对比图1；
图4(b)为姿态角误差对比图2；
图4(c)为姿态角误差对比图3；
图5(a)为速度误差对比图1；
图5(b)为速度误差对比图2；
图6(a)为速度误差对比图1；
图6(b)为速度误差对比图2；
图7(a)为姿态角误差对比图1；
图7(b)为姿态角误差对比图2；
图7(c)为姿态角误差对比图3；
图8(a)为速度误差对比图1；
图8(b)为速度误差对比图2；
图9(a)为位置误差对比图1；
图9(b)为位置误差对比图2；
图10(a)为姿态角误差对比图1；
图10(b)为姿态角误差对比图2；
图10(c)为姿态角误差对比图3；
图11(a)为速度误差对比图1；
图11(b)为速度误差对比图2；
图12(a)为位置误差对比图1；
图12(b)为位置误差对比图2。
具体实施方式
为提高室内定位***的精度和鲁棒性，本发明根据UWB和MEMS定位模型，推导出了一种UWB/MEMS紧组合算法，同时在传统的鲁棒算法的基础上引入测量误差判据，并针对传统鲁棒算法对部分观测受误差干扰调节能力差的问题，提出一种多重鲁棒自适应滤波算法，避免了对正确的量测量的调节，从而提高了组合***的精度和鲁棒性。
具体实施方式一：参照图1具体说明本实施方式，本实施方式所述的一种改进的鲁棒无迹卡尔曼滤波组合导航方法，包括以下步骤：
(1)收集UWB***和MEMS***输出的数据；
(2)选择状态量和观测量，建立UWB/MEMS组合导航***状态空间模型；
(3)UWB/MEMS组合***滤波初始化；
(4)时间更新。由k-1时刻状态估计值及协方差计算k时刻***状态预测值及协方差；
(5)Sigma点选择。根据状态预测值及其协方差选择一组加权Sigma点；
(6)量测更新。利用经过非线性量测方程变换后的Sigma点进行量测更新，得到一步状态预测的误差协方差和一步预测的互协方差；
(7)引入量测误差判据，若量测存在误差，则计算鲁棒矩阵并对一步状态预测的误差协方差进行校正；
(8)利用校正后的一步状态误差协方差，计算k时刻状态估计和状态误差协方差；
(9)将滤波输出的误差信息反馈补偿到UWB***和MEMS***，输出高精度姿态、速度、位置信息。
本发明一种改进的鲁棒无迹卡尔曼滤波组合导航方法还包括：
建立UWB/MEMS紧组合***模型
(1)***状态方程：
Xk+1＝FkXk+wk
其中，Xk为15维的惯导解算信息误差，其可写成
其中，φk＝[φk,x φk,y φk,z]T为k时刻惯导解算的姿态误差，ΔVk＝[ΔVk,x ΔVk,y ΔVk,z]T为k时刻惯导解算的地理坐标系下的速度误差，ΔPk＝[ΔLk Δλk Δhk]T为k时刻惯导解算的经纬高误差，εk＝[εk,x εk,y εk,z]T为k时刻陀螺常值漂移，
为k时刻加速度计零偏。wI,k为k时刻惯导***过程噪声，且满足E[wI,k]＝0，FI,k为k时刻惯导***状态转移矩阵。
(2)***量测数学模型推导：
在地心地固坐标系中，假设k时刻载体的真实位置为(xk,yk)，惯导解算得到的载体的位置为(xI,k,yI,k)，第i个基站Si的真实位置为
因此由MEMS惯导解算所得到的载体到基站Si之间的伪距ρIi,k为：
载体到基站Si的真实距离ρi为
两式相减，得
由于惯导解算的位置(xI,k,yI,k)存在误差，其与真实值(xk,yk)之间的关系为：
xk＝xI,k-Δxk
yk＝yI,k-Δyk
其中，Δxk和Δyk为k时刻MEMS惯导解算的载***置误差。
整理可得：
假设k时刻UWB测量的载体到基站Si的距离为ρUi,k，其与载体到基站Si的真实距离ρi,k之间的关系可表示为：
ρUi,k＝ρi,k+ε
其中，ε为UWB测量噪声。整理可得***量测模型：
式中，
为量测输入，-2ρUi,kε+ε2为***观测噪声，其协方差矩阵为Rk。
(3)***量测方程：
Zk＝h(X'k)+Vk
其中，Zk为***量测输入，h(X'k)为***非线性量测函数，Vk为***量测噪声，其噪声协方差矩阵为Rk，h(X'k)和Vk分别可以表示为：
Vk＝-2ρUi,kε+ε2
其中，X'k包含惯导在地心地固坐标系下的位置误差Δxk、Δyk，其中Δxk、Δyk与状态量Xk中惯导在地理坐标系下的位置误差ΔLk、Δλk、Δhk的关系可以表示为：
Δxk＝-(Rn+hk)cosλk sinLkΔLk-(Rn+hk)cosLk sinλkΔλk+cosLk cosλkΔhk
Δyk＝-(Rn+hk)sinλk sinLkΔLk+(Rn+hk)cosLk cosλkΔλk+cosLksinλkΔhk
其中，e为地球离心率，Rn为地球子午圈半径。
2.导数无迹卡尔曼滤波(Derivative Unscented Kalman Filter，DUKF)算法
假设非线性离散时间***为：
其中，xk∈Rn和zk∈Rm分别为k时刻统的状态向量和量测向量，Φk/k-1为离散型状态转移矩阵，h(·)为描述量测模型的非线性函数，wk和vk为互不相关的零均值高斯白噪声，其方差为：
根据DUKF滤波算法，进行***状态更新和量测更新得到载体的导航信息，具体步骤包括：
(1)给定状态估计值
及其协方差
(2)时间更新。由于***状态方程为线性，状态预测值
及其协方差
的可表示为：
(3)Sigma点选择。根据状态预测值及其协方差选择一组加权Sigma点，选择的Sigma点为：
(4)量测更新。经过非线性量测方程h(·)变换后的Sigma点为：
γi,k/k-1＝h(ξi,k/k-1),i＝0,1,...,2n
计算量测预测值及其协方差：
计算状态预测值与量测预测值间的互协方差：
其中，ωi为权值，其可表述为
(5)确定卡尔曼滤波增益矩阵：
(6)更新状态估计值
及其协方差
从DUKF的算法步骤可以看出，在时间更新过程中，它与线性KF具有相同的形式，可以简洁有效地计算状态预测值及其协方差，避免了标准UKF因UT变换导致的额外计算；在量测更新过程中，DUKF选择一组加权Sigma点捕获状态预测值及其方差的信息，然后根据所选择的Sigma点和经过非线性量测方程变换后的Sigma点的统计信息，更新状态估计值及其协方差，继承了标准UKF处理非线性滤波问题的优良特性。
3.量测噪声和野值判据
卡尔曼滤波框架中，量测误差对滤波结果的影响是通过新息引入的，因此可以从新息出发对量测误差进行在线检测，***的新息序列可以表示为：
在无噪声和野值情况下，k时刻的新息vk是零均值高斯白噪声，其协方差为：
当量测存在误差时，新息vk均值不再为零，因此，通过新息序列的检测可以判断是否存在噪声和野值，对新息vk作二元假设：
H0:无误差E[vk]＝0，
H1:有误差E[vk]＝μk，E[(vk-μk)(vk-μk)T]＝Ak
通过上述对新息vk的假设检验可得检测函数：
其中，λk是服从自由度为m的χ2分布，即λk～χ2(m)。当量测存在误差时，λk将会发生变化，如果取λk大于某一门限值β的概率为α，即p{λk＞β}＝α，则组合***量测误差判断准则为：
λk＞β时，量测存在误差
λk＜β时，量测不存在误差
4.多重因子的鲁棒算法
传统的鲁棒算法是通过引入一个时变的鲁棒因子实时调整量测协方差
从而达到调节滤波增益的目的，使有误差的量测量被较小的用于状态量的估计中，实现***对量测误差的鲁棒自适应性。调整后的量测协方差可以表示为：
其中，sk是鲁棒因子。在量测存在误差时，新息的实际协方差大于理论的协方差，两者的关系可以表示为：
其中，Yk是新息的实际协方差，ρ为遗忘因子，通常取值为0.95。传统的鲁棒因子的获取方法是计算新息序列实际协方差和理论协方差的比值，具体表示如下:
其中，tr(·)表示对矩阵求迹。从式中可以看出，传统的鲁棒因子虽然计算简单，但对各个观测量的调节能力相同。对于UWB***，由于基站分布在四周，***在非视距环境下，通常是某个基站与标签的通信受障碍物遮挡而产生测距误差，故组合***各个观测量的误差大小不一样，因此，传统的标量鲁棒因子不适合对UWB/MEMS组合***观测误差的调节。本发明根据UWB测距误差特性，建立多重鲁棒因子，对UWB测距的每个通道给予不同的调节能力，多重鲁棒因子的计算方法如下：
其中，Sk为鲁棒调节矩阵，sk(i,i)表示第i个观测量的鲁棒调节因子，其计算方法如下：
本发明首先针对室内环境下UWB易受非视距环境(NLOS)影响而导致定位精度低的问题，提出UWB/MEMS紧组合策略；其次，在传统的鲁棒算法的基础上引入量测误差判据，并且针对传统鲁棒算法对部分观测受干扰调节能力差的问题，并结合UWB测距误差的实际情况，提出一种多重鲁棒自适应矩阵的计算方法，该算法针对各量测不同的误差情况，实时调节滤波增益矩阵，使滤波器具有更强的自适应性。所以本发明利用MATLAB仿真软件进行仿真实验，在各个基站通信均受误差干扰和部分基站通信受误差干扰的情况下，将本发明的MRUKF算法与现有的UKF算法和RUKF算法进行比较。
下面结合具体事例进行仿真分析：
传感器参数设置为：陀螺仪的常值漂移和随机噪声分别为10°/h和5°/h，加速度计的常值漂移和随机噪声分别为500ug和70ug，陀螺仪和加速度计的输出频率均为100Hz；UWB标签测距的随机漂移为0.3m，UWB***的输出频率为10Hz。UWB基站含误差时，设置各基站位置存在1m的偏差。载体运动设置为：假设物体M在一个二维平面上做圆周运动，水平方向(x轴)上做匀加速直线运动，垂直方向(y轴)上也做匀加速直线运动，运动速度为1m/s，运动两周，总的运动时长为280s。4个基站布置在运动轨迹四周，图2位载体运动轨迹和基站位置分布。
图3为UWB、MEMS、UWB/MEMS松组合、UWB/MEMS紧组合算法解算的载体轨迹对比图，从图中可以看出，MEMS解算轨迹较为平滑稳定，但是随着时间推移惯导误差逐渐累加导致定位结果偏离参考轨迹。与MEMS惯导对比，UWB定位精度较高，但是UWB测距存在噪声扰动，导致UWB定位轨迹有较大的波动。相较于MEMS和UWB，UWB/MEMS组合***继承了MEMS惯导和UWB的优点，因此解算结果与参考轨迹更为接近，稳定性更好。
图4(a)、图4(b)、图4(c)、图5(a)、图5(b)、图6(a)和图6(b)为各个基站通信均受误差干扰情况下本发明MRUKF算法与DUKF算法和传统RUKF算法滤波输出误差对比图，从误差图中可以看出，当标签和4个基站的通信都受噪声影响时，由于RUKF算法对量测增益进行调节，因此其滤波精度要高于传统的DUKF。虽然MRUKF是单独对各个量测进行调节，但由于各个量测所受噪声和野值大小相同，因此其解算的鲁棒矩阵和RUKF算出的鲁棒因子大小相同，故两个算法算法滤波精度相当。
表1为各个基站通信均受误差干扰情况下，本发明MRUKF算法与DUKF算法和RUKF算法滤波解算的姿态、速度、位置的均方根误差。从表中可以看到，当观测量均含误差时，RUKF和MRUKF对误差均具有很好的调节作用。
表1
图7(a)、图7(b)、图7(c)、图8(a)、图8(b)、图9(a)和图9(b)为部分基站通信受误差干扰情况下，当部分观测量含突变噪声时，本发明MRUKF算法与DUKF算法和传统RUKF算法滤波输出误差对比图。从图可以看出，本发明所提的MRUKF的滤波精度是明显高于RUKF的，其主要原因是RUKF算法对各个观测量的调节能力相同，当只有2个UWB测距受噪声影响时，通过RUKF算法求得的鲁棒因子，其会污染另外两个没有受噪声影响的观测量，迫使其原本合理的滤波增益发生改变，而本发明所提的MRUKF算法是根据其各个量测的误差情况，分别独立调整其滤波增益，因此，其滤波输出精度要明显高于RUKF算法。
表2为部分基站通信受误差干扰情况下，当部分观测量含突变噪声时，本发明MRUKF算法与DUKF算法和RUKF算法滤波解算的姿态、速度、位置的均方根误差。从表中也可以看出，本发明所提的MRUKF算法滤波精度明显高于RUKF算法和UKF算法。
表2
图10(a)、图10(b)、图10(c)、图11(a)、图11(b)、图12(a)和图12(b)为部分基站通信受误差干扰情况下，当部分观测量含野值时，本发明MRUKF算法与DUKF算法和传统RUKF算法滤波输出误差对比图，从上图可以看出，相较于UKF算法，RUKF算法对量测野值具有很好的调节能力，但由于其对各个量测的调节能力相同，会污染不受野值干扰的量测，因此，其滤波输出精度相对MRUKF较低，鲁棒性较差。
表3为部分基站通信受误差干扰情况下，当部分观测量含野值时，本发明MRUKF算法与DUKF算法和RUKF算法滤波解算的姿态、速度、位置的均方根误差。从表中也可以看出，本发明所提的MRUKF算法对部分观测量受误差干扰的调节能力明显高于RUKF算法和UKF算法。
表3
综上，本发明一种改进的鲁棒无迹卡尔曼滤波组合导航方法，定位精度高，鲁棒性强，对实际中部分UWB基站含误差具有很好的调节能力，工程应用价值高。
需要注意的是，具体实施方式仅仅是对本发明技术方案的解释和说明，不能以此限定权利保护范围。凡根据本发明权利要求书和说明书所做的仅仅是局部改变的，仍应落入本发明的保护范围内。
Claims (5)
1.一种改进的鲁棒无迹卡尔曼滤波组合导航方法，其特征在于包括以下几个步骤：
步骤一：收集UWB***和MEMS***输出的数据；
步骤二：选择***的状态量和观测量，建立UWB/MEMS组合导航***状态空间模型；
步骤三：对UWB/MEMS组合导航***进行滤波初始化；
步骤四：在k时刻对UWB/MEMS组合导航***进行时间更新和量测更新，得到***状态一步预测的误差协方差和一步预测的互协方差；
步骤五：引入量测误差判据，若量测存在误差，则计算多重因子的鲁棒矩阵，并对状态一步预测的误差协方差进行校正，然后进入步骤六，若量测不存在误差，则直接进入步骤六；
步骤六：若量测存在误差，则利用校正后的状态一步预测误差协方差，计算k时刻***的状态估计值和状态误差协方差，若量测不存在误差，则利用步骤四中的状态一步预测误差协方差，计算k时刻***的状态估计值和状态误差协方差；
步骤七：根据步骤六中得到的状态估计值获取MEMS惯导的陀螺仪、加速度计的常值漂移，并将其反馈到组合***中，输出姿态、速度和位置信息。
2.根据权利要求1所述的一种改进的鲁棒无迹卡尔曼滤波组合导航方法，其特征在于所述UWB/MEMS组合导航***状态空间模型的***状态方程表示为：
Xk+1＝FkXk+wk
其中，Xk为15维的惯导解算信息误差，表示为
其中，φk＝[φk,x φk,y φk,z]T为k时刻惯导解算的姿态误差，ΔVk＝[ΔVk,x ΔVk,y ΔVk,z]T为k时刻惯导解算的地理坐标系下的速度误差，ΔPk＝[ΔLk Δλk Δhk]T为k时刻惯导解算的经纬高误差，εk＝[εk,x εk,y εk,z]T为k时刻陀螺常值漂移，
为k时刻加速度计零偏，wk为k时刻组合***过程噪声，wk噪声协方差矩阵为Qk，Fk为k时刻惯导***状态转移矩阵。
3.根据权利要求2所述的一种改进的鲁棒无迹卡尔曼滤波组合导航方法，其特征在于所述UWB/MEMS组合导航***状态空间模型的***量测方程表示为：
Zk＝h(X′k)+Vk
其中，Zk为***量测输入，h(X′k)为***非线性量测函数，Vk为组合***量测噪声，Vk噪声协方差矩阵为Rk，h(X′k)和Vk分别表示为：
Vk＝-2ρUi,kε+ε2
其中，X′k包含惯导在地心地固坐标系下的位置误差Δxk、Δyk，其中Δxk、Δyk与状态量Xk中惯导在地理坐标系下的位置误差ΔLk、Δλk、Δhk的关系表示为：
Δxk＝-(Rn+hk)cosλksinLkΔLk-(Rn+hk)cosLk sinλkΔλk+cosLkcosλkΔhk
Δyk＝-(Rn+hk)sinλksinLkΔLk+(Rn+hk)cosLkcosλkΔλk+cosLk sinλkΔhk
其中，e为地球离心率，Rn为地球子午圈半径。
4.根据权利要求3所述的一种改进的鲁棒无迹卡尔曼滤波组合导航方法，其特征在于所述步骤五中量测误差判据具体表示为：
λk＞β时，量测存在误差；
λk＜β时，量测不存在误差；
其中，β为门限值，λk为检测函数，λk服从自由度为m的χ2分布，即λk～χ2(m)，λk具体表示为：
其中，vk为***k时刻的新息序列，
为新息序列vk的理论协方差。
5.根据权利要求4所述的一种改进的鲁棒无迹卡尔曼滤波组合导航方法，其特征在于所述步骤五中多重因子的鲁棒矩阵表示为：
其中，Sk为多重因子的鲁棒调节矩阵，sk(i,i)表示第i个观测量的鲁棒调节因子，sk(i,i)计算方法如下：
其中，
为新息理论协方差对角线的值，Yk(i,i)为新息实际协方差对角线的值。
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202010843690.2A CN111896008A (zh) | 2020-08-20 | 2020-08-20 | 一种改进的鲁棒无迹卡尔曼滤波组合导航方法 |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202010843690.2A CN111896008A (zh) | 2020-08-20 | 2020-08-20 | 一种改进的鲁棒无迹卡尔曼滤波组合导航方法 |
Publications (1)
| Publication Number | Publication Date |
|---|---|
| CN111896008A true CN111896008A (zh) | 2020-11-06 |
Family
ID=73229826
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202010843690.2A Pending CN111896008A (zh) | 2020-08-20 | 2020-08-20 | 一种改进的鲁棒无迹卡尔曼滤波组合导航方法 |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN111896008A (zh) |
Cited By (12)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN112432644A (zh) * | 2020-11-11 | 2021-03-02 | 杭州电子科技大学 | 基于鲁棒自适应无迹卡尔曼滤波的无人艇组合导航方法 |
| CN113063429A (zh) * | 2021-03-18 | 2021-07-02 | 苏州华米导航科技有限公司 | 一种自适应车载组合导航定位方法 |
| CN113916222A (zh) * | 2021-09-15 | 2022-01-11 | 北京自动化控制设备研究所 | 基于卡尔曼滤波估计方差约束的组合导航方法 |
| CN114088086A (zh) * | 2021-11-23 | 2022-02-25 | 江苏科技大学 | 一种抗测量野值干扰的多目标鲁棒定位方法 |
| CN114166221A (zh) * | 2021-12-08 | 2022-03-11 | 中国矿业大学 | 动态复杂矿井环境中辅助运输机器人定位方法及*** |
| CN114659526A (zh) * | 2022-02-11 | 2022-06-24 | 北京空间飞行器总体设计部 | 基于序列图像状态表达的航天器自主导航鲁棒滤波算法 |
| CN115096321A (zh) * | 2022-06-23 | 2022-09-23 | 中国人民解放军63921部队 | 一种车载捷联惯导***鲁棒无迹信息滤波对准方法及*** |
| CN116086466A (zh) * | 2022-12-28 | 2023-05-09 | 淮阴工学院 | 一种提高ins误差精度的方法 |
| CN116182854A (zh) * | 2023-04-28 | 2023-05-30 | 中国人民解放军海军工程大学 | 自适应鲁棒惯性/重力匹配组合导航方法、***及终端 |
| CN116448106A (zh) * | 2023-05-24 | 2023-07-18 | 中铁第四勘察设计院集团有限公司 | 一种基于uwb/sins组合***的狭长环境定位方法及装置 |
| CN117705108A (zh) * | 2023-12-12 | 2024-03-15 | 中铁第四勘察设计院集团有限公司 | 一种基于最大相关熵的滤波定位方法、***及滤波器 |
| CN117990112A (zh) * | 2024-04-03 | 2024-05-07 | 中国人民解放军海军工程大学 | 基于鲁棒无迹卡尔曼滤波的无人机光电平台目标定位方法 |
Citations (5)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| US20170212529A1 (en) * | 2013-11-27 | 2017-07-27 | The Trustees Of The University Of Pennsylvania | Multi-sensor fusion for robust autonomous flight in indoor and outdoor environments with a rotorcraft micro-aerial vehicle (mav) |
| CN107289933A (zh) * | 2017-06-28 | 2017-10-24 | 东南大学 | 基于mems传感器和vlc定位融合的双卡尔曼滤波导航装置和方法 |
| CN109324330A (zh) * | 2018-09-18 | 2019-02-12 | 东南大学 | 基于混合无导数扩展卡尔曼滤波的usbl/sins紧组合导航定位方法 |
| US20190129044A1 (en) * | 2016-07-19 | 2019-05-02 | Southeast University | Cubature Kalman Filtering Method Suitable for High-dimensional GNSS/INS Deep Coupling |
| CN109916410A (zh) * | 2019-03-25 | 2019-06-21 | 南京理工大学 | 一种基于改进平方根无迹卡尔曼滤波的室内定位方法 |
-
2020
- 2020-08-20 CN CN202010843690.2A patent/CN111896008A/zh active Pending
Patent Citations (5)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| US20170212529A1 (en) * | 2013-11-27 | 2017-07-27 | The Trustees Of The University Of Pennsylvania | Multi-sensor fusion for robust autonomous flight in indoor and outdoor environments with a rotorcraft micro-aerial vehicle (mav) |
| US20190129044A1 (en) * | 2016-07-19 | 2019-05-02 | Southeast University | Cubature Kalman Filtering Method Suitable for High-dimensional GNSS/INS Deep Coupling |
| CN107289933A (zh) * | 2017-06-28 | 2017-10-24 | 东南大学 | 基于mems传感器和vlc定位融合的双卡尔曼滤波导航装置和方法 |
| CN109324330A (zh) * | 2018-09-18 | 2019-02-12 | 东南大学 | 基于混合无导数扩展卡尔曼滤波的usbl/sins紧组合导航定位方法 |
| CN109916410A (zh) * | 2019-03-25 | 2019-06-21 | 南京理工大学 | 一种基于改进平方根无迹卡尔曼滤波的室内定位方法 |
Non-Patent Citations (1)
| Title |
|---|
| 周卫东等: "基于新息和残差的自适应UKF算法", 《宇航学报》 * |
Cited By (19)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN112432644A (zh) * | 2020-11-11 | 2021-03-02 | 杭州电子科技大学 | 基于鲁棒自适应无迹卡尔曼滤波的无人艇组合导航方法 |
| CN112432644B (zh) * | 2020-11-11 | 2022-03-25 | 杭州电子科技大学 | 基于鲁棒自适应无迹卡尔曼滤波的无人艇组合导航方法 |
| CN113063429A (zh) * | 2021-03-18 | 2021-07-02 | 苏州华米导航科技有限公司 | 一种自适应车载组合导航定位方法 |
| CN113063429B (zh) * | 2021-03-18 | 2023-10-24 | 苏州华米导航科技有限公司 | 一种自适应车载组合导航定位方法 |
| CN113916222B (zh) * | 2021-09-15 | 2023-10-13 | 北京自动化控制设备研究所 | 基于卡尔曼滤波估计方差约束的组合导航方法 |
| CN113916222A (zh) * | 2021-09-15 | 2022-01-11 | 北京自动化控制设备研究所 | 基于卡尔曼滤波估计方差约束的组合导航方法 |
| CN114088086A (zh) * | 2021-11-23 | 2022-02-25 | 江苏科技大学 | 一种抗测量野值干扰的多目标鲁棒定位方法 |
| CN114088086B (zh) * | 2021-11-23 | 2023-11-24 | 江苏科技大学 | 一种抗测量野值干扰的多目标鲁棒定位方法 |
| CN114166221A (zh) * | 2021-12-08 | 2022-03-11 | 中国矿业大学 | 动态复杂矿井环境中辅助运输机器人定位方法及*** |
| CN114659526A (zh) * | 2022-02-11 | 2022-06-24 | 北京空间飞行器总体设计部 | 基于序列图像状态表达的航天器自主导航鲁棒滤波算法 |
| CN115096321A (zh) * | 2022-06-23 | 2022-09-23 | 中国人民解放军63921部队 | 一种车载捷联惯导***鲁棒无迹信息滤波对准方法及*** |
| CN116086466A (zh) * | 2022-12-28 | 2023-05-09 | 淮阴工学院 | 一种提高ins误差精度的方法 |
| CN116086466B (zh) * | 2022-12-28 | 2024-03-26 | 淮阴工学院 | 一种提高ins误差精度的方法 |
| CN116182854A (zh) * | 2023-04-28 | 2023-05-30 | 中国人民解放军海军工程大学 | 自适应鲁棒惯性/重力匹配组合导航方法、***及终端 |
| CN116182854B (zh) * | 2023-04-28 | 2023-09-05 | 中国人民解放军海军工程大学 | 自适应鲁棒惯性/重力匹配组合导航方法、***及终端 |
| CN116448106A (zh) * | 2023-05-24 | 2023-07-18 | 中铁第四勘察设计院集团有限公司 | 一种基于uwb/sins组合***的狭长环境定位方法及装置 |
| CN116448106B (zh) * | 2023-05-24 | 2024-05-03 | 中铁第四勘察设计院集团有限公司 | 一种基于uwb/sins组合***的狭长环境定位方法及装置 |
| CN117705108A (zh) * | 2023-12-12 | 2024-03-15 | 中铁第四勘察设计院集团有限公司 | 一种基于最大相关熵的滤波定位方法、***及滤波器 |
| CN117990112A (zh) * | 2024-04-03 | 2024-05-07 | 中国人民解放军海军工程大学 | 基于鲁棒无迹卡尔曼滤波的无人机光电平台目标定位方法 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN111896008A (zh) | 一种改进的鲁棒无迹卡尔曼滤波组合导航方法 | |
| CN112073909B (zh) | 基于uwb/mems组合的uwb基站位置误差补偿方法 | |
| Liu et al. | Mercury: An infrastructure-free system for network localization and navigation | |
| CN109916407B (zh) | 基于自适应卡尔曼滤波器的室内移动机器人组合定位方法 | |
| EP2486418B1 (en) | Improvements in or relating to radio navigation | |
| Zhao et al. | Localization of indoor mobile robot using minimum variance unbiased FIR filter | |
| Zampella et al. | Robust indoor positioning fusing PDR and RF technologies: The RFID and UWB case | |
| EP1497711B1 (en) | Method for improving accuracy of a velocity model | |
| Ibrahim et al. | Inertial measurement unit based indoor localization for construction applications | |
| Symington et al. | Encounter based sensor tracking | |
| US11550025B2 (en) | Methods for optimization in geolocation using electronic distance measurement equipment | |
| CN104869637A (zh) | 用户站定位方法及装置 | |
| US20120059621A1 (en) | Method and device for localizing objects | |
| Shubair et al. | Enhanced WSN localization of moving nodes using a robust hybrid TDOA-PF approach | |
| EP3844522B1 (en) | Methods for geolocation using electronic distance measurement equipment | |
| CN117320148A (zh) | 多源数据融合定位方法、***、电子设备及存储介质 | |
| CN113891270B (zh) | 用于提升定位的平滑度与精准度的电子装置及方法 | |
| Riesebos et al. | Smartphone-based real-time indoor positioning using BLE beacons | |
| Völker et al. | Force-directed tracking in wireless networks using signal strength and step recognition | |
| Latif et al. | Online indoor localization using DOA of wireless signals | |
| Nickel et al. | Analysis and evaluation of a particle filter for Wi-Fi azimuth and position tracking | |
| Gong et al. | Application of improved robust adaptive algorithm in UWB/MEMS positioning system | |
| Mäkelä et al. | Cooperative heading estimation with von mises-fisher distribution and particle filtering | |
| CN113984073B (zh) | 一种基于方位的移动机器人协同校正算法 | |
| Wang et al. | A Dynamic Weighting Fusion Positioning Method Based on Opportunity Correction for INS and UWB |
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: 20201106 |
|
| WD01 | Invention patent application deemed withdrawn after publication |