CN113074739B - 基于动态鲁棒容积卡尔曼的uwb/ins融合定位方法 - Google Patents

Abstract

本发明涉及复杂室内定位技术领域，具体涉及一种基于动态鲁棒容积卡尔曼的UWB/INS融合定位方法，包括：建立UWB‑INS融合定位的运动模型；采用超宽带UWB定位技术确定待定位点的第一位置和第一速度；采用惯性导航系统INS获取待定位点的第二位置数据和第二速度；计算以上两种定位方式的实际测量误差；采用动态鲁棒容积卡尔曼滤波算法对实际测量误差进行修正处理，得到最终位置误差；通过最终位置误差对第二位置数据进行修正，得到准确位置s。本发明方法可以缓解外界噪声对系统估计的影响，又可以追踪系统的运动误差模型状态，降低运动不确定性对目标轨迹预测的影响，从而实现精确、稳定的室内导航。

Description

基于动态鲁棒容积卡尔曼的UWB/INS融合定位方法

技术领域

本发明涉及复杂室内定位技术领域，具体涉及一种基于动态鲁棒容积卡尔曼的UWB/INS融合定位方法。

背景技术

近年来，随着无线技术的快速发展，基于室内场景的定位服务(如超市购物导航、火灾紧急定位、住院病人位置追踪等)，已成为众多物联网应用的基本需求。然而在大型机场、工厂车间、矿洞隧道等复杂环境下，无线电信号由于受到障碍物遮挡、电磁干扰、信号反射等因素影响无法满足室内的高精度定位需求。

单一的室内定位技术可以分为无线定位技术和传感定位技术，其中无线定位技术中以UWB的定位精度表现最优。然而在某些特殊的复杂场景下(如紧急救援和高铁站)，UWB定位技术也会因为信号受到墙壁、人或其他障碍物的阻挡而无法实现精确的定位。而在传感定位技术中，最具代表性的INS虽然能通过ZUPT(零速检测算法)或PDR(行人航迹推算算法)来克服多径效应和非视距影响，获得短时间内的精确行人位置。但INS固有的传感器漂移偏差以及拐角处的角速度偏差累积仍会阻碍其在长期部署期间的定位精度。因此，单一定位系统仍然无法满足某些高精度定位场景。

为了解决单一定位系统存在的定位精度问题，结合两种以及以上单一定位系统的融合定位方法应运而生。基于UWB/INS的室内定位技术既可以提高系统的平均定位精度又可以实现对目标长时间的连续定位跟踪。专利号202010034636.3中公开了一种基于粒子滤波算法的UWB/INS组合室内定位方法，该方法主要通过UWB系统获取待定位人员到各参考基站的距离，通过UWB位置解算单元计算出人员的东方向位置和北方向位置。通过INS(惯性导航系统)获取人员在行走过程中的三轴加速度、三轴角速度、以及三轴磁场强度，通过INS解算单元计算出人员行进时的步长、东方向步速、北方向步速、迈步时间以及姿态角。通过粒子滤波算法，将UWB系统计算结果与INS系统计算结果进行数据融合。最终达到降低非视距复杂环境的影响，提高定位精度的目的。

融合定位方法主要存在两方面的问题。一方面主要利用传统的贝叶斯滤波算法来减轻外界噪声。然而最接近贝叶斯估计的容积卡尔曼滤波算法很少应用于室内组合定位中。另一方面在实际复杂的定位环境中，外界噪声的不确定性会造成定位系统的测量值异常。而且实际物体的无规律运动或状态突变，也会给定位系统的建模造成很大的影响。所以一种既能够滤除测量噪声，又能适应无规律运动而造成的模型误差的滤波算法，成为精确定位的关键。但是现有UWB/INS融合定位系统只考虑了外界噪声对定位的影响，并未考虑运动模型的不确定性给融合定位系统带来的影响。

发明内容

为了解决上述问题，本发明提供一种适用于复杂室内环境、精确高、鲁棒性好的基于动态鲁棒容积卡尔曼滤波的UWB/INS融合定位方法，能够实现室内行人的精确跟踪。

一种基于动态鲁棒容积卡尔曼的UWB/INS融合定位方法，包括以下步骤：

S1、建立UWB-INS融合定位的运动模型，运动模型中包括室内行人的状态方程和测量方程；

S2、采用超宽带UWB定位技术确定待定位点的第一位置和第一速度；

S3、超宽带UWB定位的同时，采用惯性导航系统INS获取待定位点的第二位置数据和第二速度；

S4、根据第一位置、第一速度、第二位置数据和第二速度计算以上两种定位方式的实际测量误差；

S5、采用动态鲁棒容积卡尔曼滤波DRCKF算法对实际测量误差进行融合修正处理，得到最终位置误差；

S6、通过最终位置误差对惯性导航系统INS获得的第二位置数据进行修正，得到准确位置s。

进一步的，UWB-INS融合定位的运动模型中包括室内行人的状态方程和测量方程，其中，状态方程和测量方程分别如下式所示：

x_k＝f(x_k-1,u_k-1)+w_k-1

z_k＝h(x_k)+v_k

其中，x_k是与传输函数f(·)的相关的状态向量，传输函数f(·)是贝叶斯估计的一般表达式，这个函数反映位置、速度、加速度等状态的变化规律；z_k是通过测量函数h(·)得到的测量向量，测量函数h(·)反映的实际的测量规律，u是控制输入向量，w和v分别表示过程噪声和测量噪声，k表示时刻。

进一步的，采用超宽带UWB定位技术确定待定位点的第一位置和第一速度，分别表示如下：

其中，s_m,k表示k时刻第m个UWB标签的位置坐标，v_m,k表示k时刻第m个UWB标签的速度，G是一个线性矩阵，且

s_k表示在k时刻的标签的位置坐标、t_k表示k时刻。

进一步的，步骤S4中，计算两种定位方式的实际测量误差包括：将采用UWB定位技术获得的第一位置数据与采用惯性导航系统INS获取的第二位置数据求差值，得到位置误差δs_k；将UWB定位技术获得的速度数据与采用惯性导航系统INS获取的速度数据求差值，得到速度误差δv_k，最终得到实际测量误差如下：

其中，z_k表示实际测量误差，s_m,k表示第m个标签k时刻的第一位置坐标；v_m,k表示待定位点的第一速度；s_INS和v_INS分别为k时刻的惯性导航系统INS的速度和位置，H表示误差状态测量矩阵，δx_k表示状态误差向量，v_k表示测量噪声向量。

进一步的，步骤S5中，采用动态鲁棒容积卡尔曼滤波DRCKF算法对实际测量误差进行融合修正处理包括：利用强跟踪算法来修正标准容积卡尔曼的时间预测过程中过程噪声，然后利用Huber M估计来改进标准容积卡尔曼的测量更新过程中的测量噪声，从而最终达到缓解外界噪声、模型误差对精确定位的影响。

有益效果：

本发明与现有技术相比，其显著优点为：1)利用UWB与INS定位系统的互补特性，克服了单一定位系统在复杂环境下的局限性；(2)将标准的容积卡曼滤波算法与Huber-M估计、强跟踪算法结合在一起，既可以缓解外界噪声对系统估计的影响，又可以追踪系统的运动误差模型状态，降低运动不确定性对目标轨迹预测的影响；(3)利用惯导的短时精度和抗干扰性，来克服UWB的非视距误差，同时利用的UWB的高精度特性来缓解累积误差对长期定位的影响，从而实现精确、稳定的室内导航。

附图说明

下面结合附图和具体实施方式对本发明做进一步详细的说明。

图1是本发明基于动态鲁棒容积卡尔曼的室内行人跟踪法总体框架图；

图2是发明中UWB定位原理图；

图3是发明中动态鲁棒容积卡尔曼滤波算法流程图；

图4是本发明实施例中单一定位系统与组合定位系统在NLOS环境下的位置均方误差对比图；

图5是本发明实施例中经过EKF、CKF、DRCKF算法优化后的行人定位轨迹对比图。

具体实施方式

下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。

本实施例提供一种基于动态鲁棒容积卡尔曼的UWB/INS融合定位方法，在NLOS/LOS环境下，通过动态容积卡尔曼滤波算法，来滤除外界噪声对系统估计的影响，同时通过融入的强跟踪算法来克服模型误差，从而达到精确鲁棒的轨迹估计。如图1所示，在一种优选实施方式中，该方法包括以下步骤：

S1、建立UWB-INS融合定位的运动模型。运动模型中包括室内行人的状态方程和测量方程。

根据运动学规律构建室内行人的状态方程和测量方程分别如下式所示：

x_k＝f(x_k-1,u_k-1)+w_k-1 (1)

z_k＝h(x_k)+v_k (2)

其中，x_k是与传输函数f(·)的相关的状态向量，x是一个多维向量，包含了目标载体的位置坐标s,速度v,姿态q，加速度偏差α_b，角速度偏差ω_b等分量；传输函数f(·)是贝叶斯估计的一般表达式，这个函数反映位置、速度、加速度等状态的变化规律；z_k是通过测量函数h(·)得到的测量向量，测量函数h(·)反映的实际的测量规律，u是控制输入向量，w和v分别表示过程噪声和测量噪声，k表示时刻。

在导航坐标系中{N}中UWB-INS融合定位系统的联合状态向量表示为

其中s,v,q,α_b,ω_b分别代表目标载体的位置坐标、速度、姿态、加速度偏差，角速度偏差。它由名义状态

和误差状态δx组成并且满足以下等式：

其中，误差状态

表示对四元数状态进行四元数相乘，对其他状态进行求和，θ表示转动角度，T表示转置。

根据(4)式，相应的名义状态和误差状态的转换如下式所示：

其中，g表示{N}坐标系中的重力加速度，a_m和ω_m分别为加速度测量值和角速度测量值。a_n和ω_n分别为加速度计和陀螺仪的测量噪声，a_ω和ω_ω分别为加速计和陀螺仪的内部器件偏差，

表示状态向量的导数，

表示转换矩阵。

S2、建立UWB-INS融合定位的测量模型。UWB-INS融合定位的测量模型中包括状态方程和测量方程，利用这两个方程来实现UWB与INS的数据融合和位置预测。

采用超宽带UWB定位技术确定UWB标签(待定位点)的第一位置和第一速度：首先通过UWB系统获取获取待定节点(UWB标签)到各UWB基站的距离，再通过三边测距算法(TOA)得到UWB标签的位置。

超宽带UWB定位技术的原理如图2所示，UWB系统中包括N个基站，M个UWB标签。在该UWB系统中，采用超宽带UWB定位技术实现UWB标签定位的具体过程包括：

首先利用4个基站所得到的TOA测量值来估算k时刻的待定位节点(UWB标签，后简称为“标签”)的位置坐标s_m,k＝[x_m,k,y_m,k,z_m,k]^T，其中，s_m,k表示第m个标签k时刻的位置坐标；m代表标签序号；x、y、z分别表示X、Y、Z三个方向位置坐标，即x_m,k表示标签m在k时刻的的横坐标，y_m,k表示标签m在k时刻的的纵坐标，z_m,k表示标签m在k时刻的的z坐标。

具体地，通过UWB系统获取获取标签的真实位置坐标s_m,k，计算标签到各UWB基站的真实距离，UWB标签到n个UWB基站的真实距离可表示为：

其中，l_n,k表示UWB标签在k时刻到基站n的真实距离，s_n表示基站n的位置坐标，基站n的位置向量为s_n＝[x_n,y_n,z_n]^T,n∈{1,2,3,…,N}表示基站的序号，N是基站总数；s_m,k表示第m个标签k时刻的真实位置坐标。

例子：假设已知一个待定位节点的真实位置s_t,k，那么它到基站的真实距离表示为：

通过UWB位置解算单元(采用最小二乘法解算)解算估计出标签的坐标，得到标签的估计位置坐标

具体过程包括：为了得到UWB标签的估计位置

并使测量距离与真实距离的误差最小，将确定UWB标签位置的问题表示为：

其中，N，l_n,k，

分别表示基站的数量、UWB标签在k时刻到基站n的真实距离、UWB标签在k时刻到基站n的测量距离

从等式(1)可以得到：

其中，l_1,k、l_n,k分别表示UWB标签在k时刻到基站1的真实距离、UWB标签在k时刻到基站n的真实距离，x_n表示第n个基站的x轴坐标，y_n表示第n个基站的y轴坐标，z_n表示第n个基站的z轴坐标，z_m,k y_m,k表示标签m在k时刻的的纵坐标，z_m,k表示标签m在k时刻的的z坐标。

让(4)式的第2行到第n行分别减去第一行并进行线性化得到以下等式:

2Gs_m,k＝b_k (5)

其中

其中

利用最小二乘法对(5)式求解，得到k时刻第m个UWB标签的估计位置坐标s_m,k(标签的第一位置数据s_m,k)，表达式如下：

通过下式，计算UWB标签的速度为：

其中，s_k表示在k时刻的标签的位置坐标，v_m,k表示k时刻第m个UWB标签的速度，t_k表示k时刻。

S3、使用惯导的内置传感器获取加速度和角速度数据，通过惯导解算算法求得标签的实时位置(标签的第二位置数据)和速度，假设在k时刻的惯性导航系统INS的速度和位置分别为s_INS和v_INS。

S4、计算以上两种定位方式的实际测量误差：将采用UWB定位技术获得的第一位置数据与采用惯性导航系统INS获取的第二位置数据求差值，得到位置误差δs_k；将UWB定位技术获得的速度数据与采用惯性导航系统INS获取的速度数据求差值，得到速度误差δv_k，具体如图1所示。

于是融合导航系统的实际测量误差方程为：

其中，H表示状态误差的测量矩阵，

I₃表示3*3单位矩阵，δx_k表示状态误差，v_k表示k时刻的测量噪声。

S5、采用动态鲁棒容积卡尔曼滤波DRCKF算法对实际测量误差进行融合修正处理，得到最终位置误差。

本实施例提供一种非线性行人跟踪中动态鲁棒容积卡尔曼滤波DRCKF算法，该算法是基于标准容积卡尔曼滤波算法CKF的改进。利用将Huber-M估计算法、强跟踪算法、不确定性检测算法来改进标准的容积卡尔曼滤波算法的时间更新过程和测量更新过程，利用前两种算法的优势，构成既可以滤除外界噪声又可实时检测和矫正不确定型模型误差的滤波算法。

标准的容积卡尔曼滤波算法具体包括以下步骤：

(1)初始条件

其中，

表示初始时刻状态变量的估计初值，x₀表示状态变量的初值，P_0|0表示初始时刻状态变量的估计误差协方差，S_0|0表示初始时刻的Cholesky因子。

(2)时间预测阶段：传播容积点，预测状态预测值、相应的协方差矩阵。

根据k时刻状态变量的估计误差协方差为P_k|k，得到平方根矩阵为S_k|k，从而获得一系列同等权重的状态容积点(χ_i,k|k)，计算表达式如下：

其中，P_k|k表示k时刻状态变量的估计误差协方差，ξ_i表示容积点集，S_k|k表示k时刻的平方根矩阵，χ_i,k|k表示k时刻的状态容积点，

表示k时刻状态变量的估计值。

通过状态方程获得k时刻状态容积点的预测状态值

从而可得状态变量的预测值，最终得到状态变量的预测误差协方差矩阵P_k+1|k和过程噪声协方差矩阵Q_k，计算表达式如下：

其中，

表示对状态变量的容积点χ_i,k|k的预测值，u_k是控制输入向量，

表示k时刻状态容积点的预测状态值，n表示系统状态变量的维度。

(3)测量更新阶段：这一阶段主要是为了得到预测误差协方差、状态变量的误差协方差矩阵、下一时刻的估计误差协方差，更新增益矩阵。

预测误差协方差为P_k+1|k得到平方根矩阵为S_k+1|k，从而根据k时刻的容积点来估计k+1时刻同等权重的状态容积点(X_i,k+1|k)：

其中，X_i,k+1|k表示k+1时刻的容积点，P_k+1|k表示k+1时刻的平方根矩阵的误差协方差，S_k+1|k表示k+1时刻的平方根矩阵，

表示k时刻状态容积点的预测状态值。

通过测量方程获得状态容积点X_i,k+1|k的预测测量值Z_i,k+1|k，即状态变量的预测值，最终得到状态变量的误差协方差矩阵(P_zz,k+1|k)。

Z_i,k+1|k＝h(X_i,k+1|k,u_k) (19)

其中，Z_i,k+1|k表示测量值的容积点，

表示测量值预测值。

得到容积点的预测测量值Z_i,k+1|k后，计算测量误差协方差P_zz,k+1|k、互协方差P_xz,k+1|k、滤波增益W_k+1以及状态变量的估计值

再更新下一时刻的估计误差协方差P_k+1|k+1，计算表达式分别如下：

其中，测量误差协方差P_zz,k+1|k表示测量值的容积点与预测量值的预测值的协方差，互协方差P_xz,k+1|k表示状态变量的容积点与状态变量的预测值的协方差，R_k+1表示k+1时刻的测量噪声矩阵，z_k+1表示k+1时刻的实际测量误差，

表示k+1时刻状态容积点的预测状态值。

如图3所示是动态鲁棒容积卡尔曼滤波算法的流程图，在一种优选实施方式中，包括以下实施过程：

将预测误差和估计误差定义如下：

其中，η_k+1表示预测误差，σ_k+1表示估计误差，x_k+1表示k+1时刻的状态向量，

表示k时刻状态容积点的预测状态值。

根据泰勒级数展开式以及容积卡尔曼转换理论，上述预测误差方程可以重新改写为：

η_k+1＝F_kσ_k+Δ(σ_k)+w_k

其中

表示在线性化的过程中二阶以及高阶矩阵产生的不可避免的误差。

为了构建标准的估计等式，更好地描述一阶线性估计误差，引入一种未知的时变对角矩阵如下所示：

β_k＝diag(β_1,k,...,β_n,k)

将上式中的时变对角矩阵代入到预测误差方程可得：

η_k+1＝β_kF_kσ_k+Δ(σ_k)+w_k

其中，

类似的，可以得到一种新的更新值如下：

其中，y_k+1表示k+1时刻的未经过线性化的测量误差向量，H表示状态误差的测量矩阵，v_k+1表示测量噪声，α_k+1表示时变对角矩阵。

根据上述预测误差和估计误差，结合标准容积卡尔曼算法计算新的状态变量预测值

预测测量值

测量误差更新协方差

然后计算协方差和交叉协方差如下：

因此，可以得到：

Y_k+1＝α_k+1H(β_kF_kσ_k+w_k)+v_k+1

其中，Y_k+1表示经过线性化的k+1时刻的测量误差向量。

本实施例的一种动态鲁棒容积卡尔曼滤波算法相对于标准的容积卡尔曼滤波算法CKF，主要改进了时间预测环节与测量更新环节，具体改进包括以下几点：

1)利用强跟踪算法改进时间预测环节的误差协方差矩阵来消除运动模型误差；

在标准的容积卡尔曼滤波算法中，为了实现状态估计可靠收敛，可以牺牲一定的精度换取滤波的稳定性，选取合适的增强因子乘以状态变量误差协方差或者观测变量误差协方差，来包含更多未建模的误差，在降低一定的初值和噪声统计特性的敏感性的情况下，提高算法实时应对状态突变的能力。在室内定位系统中，为了使滤波器具有应对系统模型不确定的鲁棒性，本实施例在状态变量误差协方差预测值中引入增强因子λ_k，对不同的状态估计进行调整，来保证系统具有较好的跟踪能力。

引入增强因子λ_k，增强因子λ_k可通过以下等式推导求得：

其中，E[·]表示期望，x_k表示状态变量，

表示状态变量的估计值，z_k表示实际测量误差，

表示测量值预测值，公式中第一行是滤波器的性能指数，第二行表示新旧的状态变量差值的正交关系，这能够确保有效信息被提取出来，并且确定滤波器性能是否最优。

得到增强因子λ_k后，根据强跟踪算法和(16)式，构建一个新的协方差矩阵

为：

2)利用Huber-M估计算法改进测量更新环节的测量噪声协方差矩阵，来缓解外界噪声造成的测量误差。

在复杂环境下，由于传感器噪声和外界噪声的影响下，UWB或INS的测量值中通常存在异常值，而标准容积卡尔曼滤波算法的观测噪声具有很大的不确定性，为了缓解外界噪声对系统轨迹预测的影响，所以利用Huber-M估计算法来重新构建准确的测量噪声协方差矩阵。

UWB或INS的测量值中出现异常值时，(21)式中测量噪声协方差R_k+1将与实际误差不一致，导致(21)式中协方差矩阵P_zz,k+1|k无法反映预测值的偏差。为了对测量值的异常值进行检测和矫正，采用Huber的M估计方法对以下目标函数进行最小化，可以得到去除异常值的状态估计结果，目标函数表达式为：

其中，J(x_k)表示代价函数，ρ(·)代表凸性Huber函数，rs_i＝r_i/(ω_ri)代表残差的第i个分量，

代表残差，ω_ri＝(P_zz,k+1|k)_i,i代表r_i的标准差，参数c通常设为1.5来获得高统计效率。

为了使目标函数(29)取得最小值，令(28)式关于x_k的一阶偏导等于0，于是可以求得最优解的充分必要条件：

这里

x_k,i表示状态向量的第i个分量。

定义g_i＝Ψ(rs_i)/rs_i，则有：

基于上式，同等权重矩阵可以表示为：

其中，

和

是矩阵

的对角元素和非对角元素；ω_i,i和ω_i,j分别是测量噪声矩阵R_k+1中的对角元素和非对角元素。因为测量噪声协方差矩阵是一个对角矩阵，ω_i,j为0。r_i为测量值的残差，rs_i为测量值的标准差，ω_ri是关于r_i的均方差。由(32)和(33)式可得同等权重

代入到式(21)中的测量噪声协方差中，更新测量更新环节的测量噪声协方差矩阵为：

其中，

表示权重矩阵。

3)采用不确定性检测算法，在运动模型出现误差时入强跟踪算法实时调整状态变量的误差协方差矩阵。

不确定性检测策略：当测量模型的存在不确定性时，引入增强因子，改进(21)式中的误差协方差矩阵，可以确保更准确的估计结果。因此，我们提出了一个不确定性检测算法计算不确定性误差D_k+1值，用来判断运动过程中是否出现了运动模型误差，在适当的时候调整状态变量的误差协方差矩阵。不确定性误差值的计算表达式如下：

其中Y_k+1为新的更新值。

定义以下两种测量场景：

情景1C₀:估计系统正常工作；情景2C₁:估计系统包含模型不确定性。所述估计系统指的是容积卡尔曼滤波算法，负责数据融合和轨迹预测。

检测指数D_k+1总是严格服从自由度为m的χ²分布，利用性能指数l∈(0,1)可以确定门限值

S6、采用最终位置误差对惯性导航系统INS获得的第二位置数据进行修正，得到准确位置s。

图4是本发明实施例中单一定位系统与组合定位系统在NLOS环境下的位置均方误差对比图。图4的仿真结果表明，当基站受到遮挡或用人体对基站进行遮挡时，单一的UWB定位效果比较差。UWB的最大均方误差可达到50cm以上，移动轨迹中的定位点跳动性比较大，与实际路线的吻合度变差。单一的INS虽然有一定的累积误差，但定位误差基本上在20cm以下。而利用INS与UWB的融合数据实现的组合定位，其实际定位效果更加接近实际行人路线，进而证明了组合定位的有效性。

图5是本发明实施例中经过EKF、CKF、DRCKF算法优化后的行人定位轨迹对比图。结合图5可以看出不同滤波算法下的优化后的行人轨迹。从图5中可知，CKF的估计效果较差，最大估计误差可达50cm,而经过EKF优化的路线轨迹虽然优于CKF，并且对定位过程中产生的异常值有一定的鲁棒性，与实际轨迹仍有较大偏差。而DRCKF对于异常值具有更好的鲁棒性，对于UWB系统与惯导系统之间的偏差估计更加准确，估计误差在15cm以下，最终优化后的轨迹与实际轨迹更加贴合。进一步证明了该算法能够适应模型误差和噪声较大的环境。

需要说明的是，本领域普通技术人员可以理解实现上述方法实施例中的全部或部分流程，是可以通过计算机程序来指令相关的硬件来完成，所述的程序可存储于一计算机可读取存储介质中，该程序在执行时，可包括如上述各方法实施例的流程。其中，所述存储介质可为磁碟、光盘、只读存储记忆体(Read-0nly Memory，ROM)或随机存储记忆体(RandomAccess Memory，RAM)等。

以上所述仅是本申请的具体实施方式，应当指出，对于本领域的普通技术人员而言，可以理解在不脱离本发明的原理和精神的情况下可以对这些实施例进行多种变化、修改、替换和变型，本发明的范围由所附权利要求及其等同物限定。

Claims (3)

1.一种基于动态鲁棒容积卡尔曼的UWB/INS融合定位方法，其特征在于，包括以下步骤：

S1、建立UWB-INS融合定位的运动模型，运动模型中包括室内行人的状态方程和测量方程；

S2、采用超宽带UWB定位技术确定待定位点的第一位置和第一速度；

S3、超宽带UWB定位的同时，采用惯性导航系统INS获取待定位点的第二位置数据和第二速度；

S4、根据第一位置、第一速度、第二位置数据和第二速度计算以上两种定位方式的实际测量误差；包括：

将采用UWB定位技术获得的第一位置数据与采用惯性导航系统INS获取的第二位置数据求差值，得到位置误差δs_k；将UWB定位技术获得的速度数据与采用惯性导航系统INS获取的速度数据求差值，得到速度误差δv_k，最终得到实际测量误差如下：

其中，z_k表示实际测量误差，s_m,k表示第m个标签k时刻的第一位置坐标；v_m,k表示第m个标签k时刻的第一速度；s_INS和v_INS分别为第m个标签k时刻采用惯性导航系统INS获取的第二位置和第二速度，H表示误差状态测量矩阵，δx_k表示状态误差向量，v_k表示测量噪声向量；

S5、采用动态鲁棒容积卡尔曼滤波DRCKF算法对实际测量误差进行融合修正处理，得到最终位置误差；包括：

1)利用强跟踪算法改进时间预测环节的误差协方差矩阵来消除运动模型误差；包括：

引入增强因子λ_k，增强因子λ_k通过以下等式推导求得：

其中，E[·]表示期望，x_k表示状态变量，

表示状态变量的估计值，z_k表示实际测量误差，

表示测量值预测值；

得到增强因子λ_k后，构建一个新的协方差矩阵

其中，Q_k表示过程噪声协方差矩阵，

表示k时刻状态容积点的预测状态值，X_i,k+1|k表示k+1时刻的容积点，n表示系统状态变量的维度；

2)利用Huber-M估计算法改进测量更新环节的测量噪声协方差矩阵，来缓解外界噪声造成的测量误差；包括更新测量更新环节的测量噪声协方差矩阵为：

其中，

表示同等权重矩阵；

3)采用不确定性检测算法，在运动模型出现误差时引入强跟踪算法实时调整状态变量的误差协方差矩阵；包括利用不确定性检测算法计算不确定性误差值D_k+1，用来判断运动过程中是否出现了运动模型误差，在适当的时候调整状态变量的误差协方差矩阵；

其中Y_k+1为新的更新值，

表示协方差，v_k+1表示测量噪声，

表示测量值预测值；

定义以下两种测量场景：

情景1 C₀:估计系统正常工作；情景2 C₁:估计系统包含模型不确定性；所述估计系统指的是容积卡尔曼滤波算法，负责数据融合和轨迹预测；

检测指数不确定性误差值D_k+1总是严格服从自由度为m的χ²分布，利用性能指数l∈(0,1)确定门限值

S6、通过最终位置误差对惯性导航系统INS获得的第二位置数据进行修正，得到准确位置s。

2.根据权利要求1所述的一种基于动态鲁棒容积卡尔曼的UWB/INS融合定位方法，其特征在于，UWB-INS融合定位的运动模型中包括室内行人的状态方程和测量方程，其中，状态方程和测量方程分别如下式所示：

x_k＝f(x_k-1,u_k-1)+w_k-1

z_k＝h(x_k)+v_k

其中，x_k是与传输函数f(·)的相关的状态向量，传输函数f(·)是贝叶斯估计的一般表达式，这个函数反映位置、速度、加速度状态的变化规律；z_k是通过测量函数h(·)得到的测量向量，测量函数h(·)反映实际的测量规律，u_k-1是控制输入向量，w_k-1表示过程噪声向量，k表示时刻。

3.根据权利要求1所述的一种基于动态鲁棒容积卡尔曼的UWB/INS融合定位方法，其特征在于，采用超宽带UWB定位技术确定待定位点的第一位置和第一速度，分别表示如下：

其中，G是一个线性矩阵，且

t_k表示k时刻；l_n,k表示UWB标签在k时刻到基站n的真实距离；x_n,y_n,z_n分别表示基站n的X、Y、Z三个方向位置坐标，n∈{1,2,3,…,N}表示基站的序号，N是基站总数。

Images