[发明专利]基于动态鲁棒容积卡尔曼的UWB/INS融合定位方法

权利要求书

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。