一种UWB_INS组合定位系统自主完好性检测方法与流程

一种uwb_ins组合定位系统自主完好性检测方法
技术领域
1.本发明属于组合导航定位技术领域，具体涉及一种uwb_ins组合定位系统自主完好性检测方法。


背景技术：

2.近年来，随着室内定位服务需求的不断增加，相关技术的应用也愈加成熟和广泛。超宽带（ultra wide band，uwb）定位技术采用了极低的功率谱密度、非正弦波极窄脉冲作为其通信信号，具有高时间分辨率、穿透力强、抗多径效应好等特点，在室内定位领域广泛应用。uwb定位方法根据测量参数可分为基于信号接收强度(received signal strength indication， rssi)法、基于信号到达角度(angle of arrival， aoa)法、基于信号到达时间 (time of arrival， toa)法和基于信号到达时间差(time difference of arrival， tdoa)法等。在非视距(non-line-of-sight， nlos)环境下，由于障碍物的遮挡信号不能直达，定位精度会急剧下降，故在复杂的室内环境中仅使用uwb测量信息定位时，很难实现高精度的定位，且无法解算姿态信息。惯性测量单元(inertial measurement unit， imu)一般由三轴加速度计和三轴陀螺仪组成，可以测出目标在载体坐标系下的加速度和角速度，根据初始时刻的位置和姿态角，基于运动学和经典力学求得载体位置和姿态信息，实现自主定位，不受nlos环境影响，短时间内定位精度较高，但加速度计、陀螺仪存在偏差和漂移，误差会随时间迅速积累。uwb和imu 组合定位导航可利用imu 短期解算准确的特点缓解uwb定位时nlos误差的影响，uwb测量信息又可抑制imu 产生累计误差。组合方式主要有紧组合和松组合，松组合只用imu 和uwb测量信息解算的定位值进行组合滤波，imu 和uwb信息并不互相校正，uwb算法定位精度对组合导航定位精度影响较大，而紧组合利用imu 和uwb测量信息相互校正，用组合滤波算法的预测值判别和缓解nlos 误差，显著提高组合定位导航精度。
3.对于uwb_ins组合定位系统而言，紧组合方式下，uwb不进行标签位置的解算，而是直接输出各基站的测距信息，ins（综合导航系统）将根据加速度计和陀螺仪测量信息阶段的位置信息转换成距离信息。滤波器对uwb测距信息与ins得到的距离信息进行融合，输出ins位置误差和速度误差的最优或次优估计值来矫正ins定位结果。组合定位系统中，由于uwb与ins误差机理不同，故障发生概率相互独立，为保证融合系统的稳定性，需要对融合系统的完好性进行检测。


技术实现要素：

4.为解决上述技术问题，本发明提供了一种uwb_ins组合定位系统自主完好性检测方法，本方法在uwb_ins组合定位系统的基础上，根据观测值冗余度，对观测值的故障情况进行检测及分析，在观测值上剔除掉异常测距值后，再进行组合导航滤波器的解算流程，以此提高系统稳定性和精度。
5.本发明实现其目的所采取的技术方案是：
一种uwb_ins组合定位系统自主完好性检测方法，包括如下步骤：步骤（1）建立uwb_ins组合定位系统紧组合离散状态模型及观测模型，观测模型中观测矩阵通过雅可比矩阵近似计算；步骤（2）基于组合定位系统方程，利用最小二乘法求解位置参数；步骤（3）根据位置参数求解方程计算位置的计算残差及残差平方和；步骤（4）根据分布概率密度函数和假设检验理论，以及给定误判概率p，计算得出卡方检测临界值，并以此设置检验门限，判断组合定位系统是否存在故障；步骤（5）判断系统存在故障后，通过统计量计算各基站检测统计量，并与极端检验阈值进行比较，判断基站有无异常；步骤（6）判断出异常基站后，剔除掉存在异常的测距数据，剩余数据重新计算观测数组，进行观测模型的重建。
6.进一步地，所述步骤（1）包括：建立uwb/ins组合定位系统紧组合离散状态模型如下：，，其中，状态量，状态量选择为北向位置误差、东向位置误差，北向速度误差、东向速度误差；为状态噪声阵，为观测噪声阵；其中，量测值，，表示ins解算位置与第i个基站之间的距离，表示第i个uwb基站的测量距离，表示紧组合系统的非线性测量函数，具体表达式如下：，式中，、为惯导计算北向位置及东向位置，、为第i个基站的位置坐标；将的雅可比矩阵作为近似量测矩阵h，作为组合定位扩展卡尔曼滤波的计算量测矩阵：
，其中，。
7.进一步地，所述步骤（2）包括：利用最小二乘法求解位置参数为：，其中，为权重矩阵，，即近似量测矩阵h的前两列。
8.进一步地，所述步骤（3）中所述位置的计算残差为：，残差平方和为：，观测噪声服从正态分布，残差平方和s服从自由度为n的分布。
9.进一步地，所述步骤（4）包括：根据分布概率密度函数和假设检验理论，给定误判概率，计算得到卡方检测临界值为：，为卡方分布概率密度函数，通过计算检验门限，为检测统计量，若，则不存在故障，反之存在故障。
10.进一步地，所述步骤（5）包括：当判断系统存在故障时，检测基站是否异常，包括：首先定义检验统计量，为矩阵的第i行第i
列，其中；对检验统计量进行二元假设，在一定的误判概率下通过正态分布概率密度函数极端检验阈值；，其中，n(0，1)为标准正态分布；通过检验统计量与正态分布概率密度函数极端检验阈值进行比较，如果，则基站无异常，反之异常。
11.与传统差分定位技术相比，本发明的优点在于：在uwb_ins紧组合定位系统整体状态及观测方程基础上，根据观测值冗余度，对观测值的故障情况进行检测及分析，在完好性分析基础上剔除异常数据，提高系统稳定性。
附图说明
12.图1为本发明的一种uwb_ins组合定位系统自主完好性检测方法流程图。
具体实施方式
13.为了使本发明的目的、技术方案及优点更加清楚明白，以下结合附图及实施例，对本发明进行进一步详细说明。应当理解，此处所描述的具体实施例仅仅用以解释本发明，并不用于限定本发明。此外，下面所描述的本发明各个实施方式中所涉及到的技术特征只要彼此之间未构成冲突就可以相互组合。
14.本发明基于uwb_ins紧组合定位系统，根据观测值冗余度，对观测值的故障情况进行检测及分析，在完好性分析基础上剔除异常数据，提高系统稳定性。
15.本发明的一种uwb_ins组合定位系统自主完好性检测方法具体包括如下步骤：步骤（1）建立uwb_ins组合定位系统紧组合离散状态模型及观测模型，观测模型中观测矩阵通过雅可比矩阵近似计算。
16.步骤（2）基于组合定位系统方程，利用最小二乘法求解位置各参数。
17.步骤（3）根据位置参数求解方程计算位置计算残差及残差平方和。
18.步骤（4）根据分布概率密度函数和假设检验理论，以及给定误判概率p，计算得出卡方检测临界值，并以此设置检验门限，判断是否存在故障。
19.步骤（5）判断系统存在故障后，通过统计量计算各基站检验统计量，并与极端检验阈值进行比较，判断基站有无异常。
20.步骤（6）判断出异常基站后，剔除掉存在异常测距数据，剩余数据重新计算观测数组，进行观测模型的重建。
21.具体地，建立uwb_ins组合定位系统紧组合离散状态模型如下：，
，其中，状态量，状态量选择为北向位置误差、东向位置误差，北向速度误差、东向速度误差；为状态噪声阵，为观测噪声阵。
22.其中，量测值，，表示ins解算位置与第i个基站之间的距离，表示第i个uwb基站的测量距离，表示紧组合系统的非线性测量函数，具体表达式如下：，式中，、为惯导计算北向位置及东向位置，、为第i个基站的位置坐标。
23.将的雅可比矩阵作为近似量测矩阵h，作为组合定位扩展卡尔曼滤波（ekf）的计算量测矩阵：，其中，。
24.基于上述组合定位系统紧组合离散状态模型，利用位置求解方程采用最小二乘法求解位置各参数为：，其中，为权重矩阵，为状态噪声方差阵，，即近似量测矩阵h的前两列。
25.因此，基于位置求解方程，位置的计算残差e为：，
残差平方和s为：，观测噪声服从正态分布，残差平方和s服从自由度为n的分布。根据分布概率密度函数和假设检验理论，给定误判概率，可以计算得到卡方检测临界值为：，其中，为卡方分布概率密度函数，通过计算检验门限，为检测统计量，因此实际应用中需要进行判断，若，则不存在故障，反之存在故障。
26.当判断系统有故障时，需要检测出存在问题的基站，首先定义检验统计量，为矩阵的第i行第i列，其中。对进行二元假设，在一定的误判概率下通过正态分布概率密度函数极端检验阈值。
27.，其中，n(0，1)为标准正态分布。
28.通过检验统计量与正态分布概率密度函数极端检验阈值进行比较，如果，则该基站无异常，反之异常。检测到基站数据异常后，剔除掉存在异常测距数据，剩余数据重新计算观测数组，进行观测模型的重建。通过以上过程即可完成uwb_ins组合导航系统的自主完好性检测。
29.本领域的技术人员容易理解，以上所述仅为本发明的较佳实施例而已，并不用以限制本发明，凡在本发明的精神和原则之内所作的任何修改、等同替换和改进等，均应包含在本发明的保护范围之内。

技术特征：
1.一种uwb_ins组合定位系统自主完好性检测方法，其特征在于，包括如下步骤：步骤（1）建立uwb_ins组合定位系统紧组合离散状态模型及观测模型，观测模型中观测矩阵通过雅可比矩阵近似计算；步骤（2）基于组合定位系统方程，利用最小二乘法求解位置参数；步骤（3）根据位置参数求解方程计算位置的计算残差及残差平方和；步骤（4）根据分布概率密度函数和假设检验理论，以及给定误判概率p，计算得出卡方检测临界值，并以此设置检验门限，判断组合定位系统是否存在故障；步骤（5）判断系统存在故障后，通过统计量计算各基站检测统计量，并与极端检验阈值进行比较，判断基站有无异常；步骤（6）判断出异常基站后，剔除掉存在异常的测距数据，剩余数据重新计算观测数组，进行观测模型的重建。2.根据权利要求1所述的一种uwb_ins组合定位系统自主完好性检测方法，其特征在于，所述步骤（1）包括：建立uwb/ins组合定位系统紧组合离散状态模型如下：，，其中，状态量，状态量选择为北向位置误差、东向位置误差，北向速度误差、东向速度误差；为状态噪声阵，为观测噪声阵；其中，量测值，，表示ins解算位置与第i个基站之间的距离，表示第i个uwb基站的测量距离，表示紧组合系统的非线性测量函数，具体表达式如下：，式中，、为惯导计算北向位置及东向位置，、为第i个基站的位置坐标；将的雅可比矩阵作为近似量测矩阵h，作为组合定位扩展卡尔曼滤波的计算量测矩阵：
，其中，。3.根据权利要求2所述的一种uwb_ins组合定位系统自主完好性检测方法，其特征在于，所述步骤（2）包括：利用最小二乘法求解位置参数为：，其中，为权重矩阵，，即近似量测矩阵h的前两列。4.根据权利要求3所述的一种uwb_ins组合定位系统自主完好性检测方法，其特征在于，所述步骤（3）中所述位置的计算残差为：，残差平方和为：，观测噪声服从正态分布，残差平方和s服从自由度为n的分布。5.根据权利要求4所述的一种uwb_ins组合定位系统自主完好性检测方法，其特征在于，所述步骤（4）包括：根据分布概率密度函数和假设检验理论，给定误判概率，计算得到卡方检测临界值为：，为卡方分布概率密度函数，通过计算检验门限，为检测统计量，若，则不存在故障，反之存在故障。6.根据权利要求5所述的一种uwb_ins组合定位系统自主完好性检测方法，其特征在
于，所述步骤（5）包括：当判断系统存在故障时，检测基站是否异常，包括：首先定义检验统计量，为矩阵的第i行第i列，其中；对检验统计量进行二元假设，在一定的误判概率下通过正态分布概率密度函数极端检验阈值；，其中，n(0，1)为标准正态分布；通过检验统计量与正态分布概率密度函数极端检验阈值进行比较，如果，则基站无异常，反之异常。

技术总结
本发明公开了一种UWB_INS组合定位系统自主完好性检测方法，该方法在UWB_INS组合定位系统的基础上，根据观测值冗余度，对观测值的故障情况进行检测及分析，在完好性分析基础上剔除异常数据，提高系统稳定性。提高系统稳定性。提高系统稳定性。


技术研发人员：汪亮 楚焕鑫 王宁波 李子申 王亮亮 韦永僧 谢吉顺 杨阔
受保护的技术使用者：齐鲁空天信息研究院
技术研发日：2023.07.06
技术公布日：2023/8/16