带未知量测偏置的MEMS与UWB紧组合定位方法及系统

本发明涉及室内定位，具体而言，涉及带未知量测偏置的mems/uwb室内定位方法。

背景技术：

1、目前，室内定位技术在当前日益普及和精确化的定位应用中具有非常重要的研究意义和应用价值。随着室内定位技术的不断创新和改进，它能够在导航、安全监控、智能家居以及物流管理等多个领域带来更广阔的应用前景。其中mems/uwb紧组合定位系统，能够实现优势互补，提供丰富的传感器数据和精确的距离测量，为室内定位系统提供准确、实时和稳定的定位结果。由于室内环境通常存在信号衰减、多路径传播、信号阻塞等问题，因此提高室内定位技术的定位精确度和稳定性至关重要。卡尔曼滤波算法是目前基于mems/uwb紧组合室内定位技术中最常用的算法之一，它通过融合系统的动态模型和量测数据来提高状态的估计精度。但uwb输出的测距信息往往带未知量测偏置，这导致现有的室内定位方法精度降低。

2、目前研究室内定位系统都假设量测噪声是服从零均值的高斯分布，但带未知量测偏置的uwb室内定位系统中的量测噪声往往诱导量测噪声呈现非零均值高斯分布。因此，亟需一种室内定位方法来提高量测噪声的建模精度，能够能准确的估计量测偏置进而提高室内定位精度。

3、文献号为cn109764865a现有技术公开了一种基于mems和uwb的室内定位方法，其公开的方法通过获取连续的uwb数据从而得到移动终端的移动轨迹，然后利用移动终端上的mems测量模块测量得到的加速度值的振荡规律确定移动终端的运动状态。然后基于确定得到的运动状态对移动终端的移动轨迹进行修正得到移动终端当前的真实位置，从而可以获得移动终端室内环境中的正确移动轨迹。文献号为cn114216457a的现有技术公开了一种基于超宽带信号的多源数据融合定位方法及系统，其涉及非暴露空间环境下复杂建筑结构、多种服务设施及交通路线位置定位技术领域，特别涉及一种基于超宽带信号的多源融合定位方法及系统。其方法包括利用北斗定位系统确定室外定位的方法和利用imu/uwb/pdr技术完成室内定位的方法，基于城市轨道交通环境中部署的超宽带基站，融合北斗卫星导航定位信号和mems传感器信息，结合dbs+uwb+pdr(航迹跟踪)+imu多种算法，通过参数优化，实现非暴露空间的多源融合导航定位。

4、现有的室内定位方法针对如何提高量测噪声的建模精度较低，以能够准确地估计量测偏置进而提高室内定位精度，一直没有人提出来。

技术实现思路

1、本发明要解决的技术问题是：

2、本发明为了解决现有室内定位方法存在量测噪声的建模精度低，无法准确地估计量测偏置影响室内定位精度的问题，进而提供了一种带未知量测偏置的mems和uwb紧组合定位方法及系统(基于mems/uwb紧组合室内定位方法)。

3、本发明为解决上述技术问题所采用的技术方案：

4、本发明提供了带未知量测偏置的mems/uwb紧组合定位方法，包括以下步骤：

5、s1、mems的位置误差、速度误差、姿态角误差、陀螺偏置误差、加速度计偏置误差作为室内定位系统的导航误差状态向量并基于此构建状态方程和量测方程；

6、s2、带未知量测偏置的量测噪声被假设服从非零均值高斯分布；

7、s3、正态类别分布是多个高斯分布乘积形式，对其边缘化可得到多个高斯分布加权和形式，本方法利用正态类别分布边缘化对s2中所提出的非零均值高斯量测噪声进行建模；

8、s4、在贝叶斯框架下分别计算权重导航误差状态向量系统增益矩阵以及相应的估计误差协方差矩阵

9、s5、利用s4中得到的权重来更新正态类别分布中的形状参数

10、s6、利用s4中得到的导航误差状态向量来修正mems的导航状态向量，并实现定位结果输出。

11、进一步地，s1具体为：选取mems的位置误差、速度误差、姿态角误差、陀螺偏置误差、加速度计偏置误差作为室内定位系统的导航误差状态向量，即：

12、

13、其中，δψk表示姿态角误差，分别表示在k时刻的真实速度和位置误差，分别表示陀螺偏置误差和加速度计偏置误差；

14、构建室内定位的系统方程为：

15、δxk＝ak-1δxk-1+bk-1ωk-1                                          (2)

16、其中，δxk为导航误差状态向量，ak状态转移矩阵，bk控制输入矩阵，ωk为系统噪声；

17、状态转移矩阵ak和控制输入矩阵bk的表达式为：

18、

19、估计的导航误差向量反馈给mems，故可得：

20、ferr(015×1,xk,uk)＝015×1                                         (3)

21、根据公式(3)，本室内定位方法估计的导航误差向量反馈到mems，因此每个时间更新中的一步预测状态向量被设置为零。一步预测估计误差协方差更新如下：

22、

23、其中fk和gk分别为进行离散化的ak和bk。

24、s2具体为：当uwb输出的测距值可用的时候，室内定位方法进入量测更新阶段。量测方程如下：

25、

26、其中，hk是量测矩阵，vk是量测噪声。由于uwb输出的测距信息带未知量测偏置，故本室内定位方法假设量测噪声vk是服从非零均值高斯分布。

27、量测矩阵hk进行线性化后表示如下：

28、

29、根据误差协方差，卡尔曼滤波增益kk计算如下：

30、

31、其中rk为量测噪声。

32、进一步地，s3具体为：将量测噪声的概率密度函数建模成正态-类别分布：

33、

34、其中均值和相应的方差表示随机向量且服从类别分布；表示混合概率向量且服从狄利克雷分布，具体表达式如下：

35、

36、其中，狄利克雷分布的先验形状参数通过遗忘因子ρ∈(0,1]传播上一时刻的信息来获得的，即是上一时刻获得的后验形状参数。

37、将对概率密度函数边缘化，量测噪声的概率密度可以建模成5个高斯和形式：

38、

39、进一步地,将一步预测概率密度函数p(δxk|δz1:k-1)可以表示为:

40、p(δxk|δz1:k-1)＝n(δxk；015×1,pk|k-1)             (11)

41、其中，因为导航误差状态在上一时刻就反馈给了mems。所以一步预测概率密度函数p(δxk|δz1:k-1)的均值向量设为015×1。误差协方差矩阵pk|k-1可以通过经典卡尔曼滤波算法中的时间更新过程可得。

42、进一步地，s4中计算相关参数：

43、采用变分贝叶斯方法计算后验概率密度可以利用nc分布的性质和贝叶斯准则进行计算，具体表达式如下：

44、

45、其中表示权重，表示状态估计向量，为误差估计协方差矩阵。具体计算如下：

46、

47、进一步地，s5具体为：

48、根据vb方法将混合概率向量的后验概率密度函数更新为狄利克雷分布，并对狄利克雷分布中的形状参数更新计算，表达式如下：

49、

50、由于在进行状态估计时每一时刻的概率值都是相互独立的，仅仅依赖于残差和统计信息进行更新。

51、进一步地，s6具体为：

52、根据权利要求6所述的带未知量测偏置的mems/uwb紧组合定位方法，其特征在于，将导航误差反馈到mems，并对状态估计值即提供协方差矩阵进行状态更新，即：

53、

54、pk＝pk|k                          (16)

55、带未知量测偏置的mems/uwb紧组合定位方法，该紧组合定位系统具有与上述技术方案中任一项的步骤对应的程序模块，运行时执行上述的带未知量测偏置的mems/uwb紧组合定位方法中的步骤。

56、一种计算机可读存储介质，其特征在于：所述计算机可读存储介质存储有计算机程序，所述计算机程序配置为由处理器调用时实现上述的带未知量测偏置的mems和uwb紧组合定位方法的步骤。

57、相较于现有技术，本发明的有益效果是：

58、经研究发现，uwb输出带未知量测偏置的测距信息诱导基于mems/uwb紧组合系统中的量测噪声呈现非零均值高斯分布，这导致了现有的基于零均值假设条件的室内定位方法精度下降。因此，本发明提出通过提高量测噪声的建模精度来准确地估计量测偏置，从而实现室内定位精度的大幅度提高。

59、本发明基于带未知量测偏置的mems/uwb紧组合定位方法，在紧组合模式下由mems输出的状态信息辅助uwb系统完成定位，利用mems短期定位精度高且抗干扰性强的优点，使得uwb系统在测距信息不足的情况下也能够实现定位。其中将带量测偏置的uwb诱导的非零均值量测噪声进一步分析，并将非零均值量测噪声按正态-类别分布建模，来提高对非零均值量测噪声建模精度，降低估计误差，进而提高系统的定位精度。

60、本发明根据正态-类别分布来估计量测噪声与经典的卡尔曼滤波器相比，在量测噪声具有未知量测偏置情况下，通过对量测噪声的非零均值实时估计来更新状态估计，能够有效降低带未知量测偏置的量测噪声对定位精度产生的影响。

61、本发明提出的基于mems/uwb紧组合室内定位方法，有效解决了现有的室内定位系统由于uwb输出的测距信息带未知量测偏置而导致室内定位精度低的问题。包括如下过程：选取mems的位置误差、速度误差、姿态角误差、陀螺偏置误差、加速度计偏置误差作为室内定位系统的导航误差状态向量，并基于此构建状态方程和量测方程；因uwb输出的测距信息带未知量测偏置，故本室内定位方法的量测噪声被假设服从非零均值高斯分布；提出正态类别分布并对其边缘化来对上述的非零均值高斯量测噪声进行建模；在贝叶斯框架下分别计算权重，导航误差状态向量，系统增益矩阵以及相应的估计误差协方差矩阵；更新形状参数；更新导航误差量的状态，实现定位结果输出。本发明所述带未知量测偏置的mems与uwb紧组合定位方法可用于有遮挡非视距条件下的室内定位。