一种二维超宽带与惯性导航融合定位方法及系统

本发明属于二维情况下的室内定位，具体涉及一种二维超宽带与惯性导航融合定位方法及系统。

背景技术：

1、伴随着第三次工业革命的到来，人们在电子科技、智能软硬件、数据处理能力等等方面的跃进，我们迎来了新的挑战。进入信息时代后人们对空间信息的需求越来越强烈：在物流方面，包裹实时数据便于对车辆交通协调及配送时间预估；在车辆出行方面，旅游业的兴起带动的经济的发展需要导航定位服务支撑；在养老院方面，需要照顾的老人日常起居出行需要定位来帮助医疗护理人员实时知道老年人的位置；在停车场方面，定位系统为车辆停放带来高效率。由此为了提升军事实力而产生的导航系统开始进入民用化阶段。

2、据统计人类一生有70％～90％的时间位于室内。从室外的停车导航到室内的图书馆导航、医院科室就医导航，这意味着室内定位技术有着很大的需求。目前应用广泛的定位技术包括有无线局域网技术(wireless fidelity，wifi)、移动蜂窝(cell)技术、超宽带(ultra wide band,uwb)技术、蓝牙(bluetooth low energy，ble)技术、超声波、红外线、地磁匹配、5g、行人航迹推算。其中uwb技术是不依靠载波且宽带高的一种信号，具有定位精度高、功耗低、有一定的信号穿透能力，是在短距离室内定位的研究热点之一。但在室内复杂的环境中uwb技术也有局限性，如需要接收到足够的信息量才能解算出点位；受到多路径效应和非视距的影响而产生的时间误差即信号延迟，这会导致测距误差的增大影响定位精度。成本低、使用方便的惯性测量单元(imu)器件含有加速度计、磁力计等传感器在不使用基站的条件下，可以不依赖外界环境条件下持续稳定地输出数据，同时能依靠行人航位推算技术独立实现行人的定位。但行人走动以及行人走动习惯特点不同的变化等因素会导致数据出现累计误差，而随着时间变化导致定位精度出现偏差甚至远离真实轨迹。

3、综上所述，把uwb技术与惯性导航技术进行组合得到一种新的组合式定位导航方式，融合两种定位方法的优点将两者之间的缺点互补减小累计误差和非视距等影响，提高定位精度及稳定性。

技术实现思路

1、本发明旨在解决现有技术的不足，室内环境下超宽带(uwb)基站与标签之间的距离会受到随机误差的影响，而惯性导航系统因其内部硬件敏感度易受测量随机噪声影响，本发明提出了一种二维超宽带与惯性导航融合定位方法及系统来削弱测量随机误差。本发明中超宽带部分可用于动态与静态场景中，计步器算法可用于在原地踏步与行进的运动过程中，并且能够达到较高的准确度。

2、为实现上述目的，本发明提供了如下方案：

3、一种二维超宽带与惯性导航融合定位方法，包括以下步骤：

4、s1：根据目标空间，构建矩形三维空间，并在所述矩形三维空间中部署基站、标签以及惯性测量单元；

5、s2：获得所述基站与所述标签之间的直线距离，并基于所述直线距离以及所述基站的二维坐标建立关系，获得所述标签的初始位置；

6、s3：将所述初始位置代入垂线段计算，获得垂线段长度，并以所述垂线段长度为观测量建立方程，对所述方程进行泰勒展开做线性化并解算，获得定位位置；

7、s4：基于所述惯性测量单元，获得目标空间内行走实时加速度数据；

8、s5：对所述加速度数据进行butterworth滤波，获得步数和行走时间；并通过weinberg模型，获得步长；基于所述步数、所述行走时间以及所述步长，反算获得速度；

9、s6：将所述定位位置、所述速度以及所述行走时间，组合构建卡尔曼滤波方程，完成二维超宽带与惯性导航融合定位。

10、优选的，步骤s2中，获得所述标签的初始位置的方法为：

11、设置n个基站保持同一高度随机分布在三维空间中，第i个基站的已知坐标为(xi,yi),i＝1,2,…,n，标签到基站的测量值为ri,i＝1,2,…,n，标签位置为(x,y)，则预设理想状态下标签与基站之间建立如下关系：

12、(x-xi)2+(y-yi)2＝ri2,i＝1,2,…,n                  (1)

13、将式(1)联立，获得多基站情况下的所述关系：

14、

15、将式(2)方程组内相互做差获得线性方程组：

16、ax＝b                          (3)

17、其中

18、基于式(3)采用最小二乘法：

19、x＝(ata)-1(atb)                            (4)

20、基于式(4)，获得所述标签的初始位置x＝(x0,y0)。

21、优选的，步骤s3中，获得所述定位位置的方法为：

22、基于所述初始位置与垂线段长度建立所述线性方程：

23、

24、其中，表示标签与基站之间的垂线段长度；

25、基于所述垂线段长度，获得垂足点坐标即(x,y)沿着(x,y)到圆心(xi,yi)的垂线段

26、

27、基于垂线段获得垂足点平均值；基于所述垂足点平均值，获得参考坐标

28、

29、以所述垂线段长度为观测量，由参考坐标与基站坐标(xi,yi),i＝1,2,…,n构建垂线段改进最小二乘模型并进行线性化：

30、

31、其中参数

32、整理参数，构建最小二乘矩阵计算模型：

33、

34、其中表示标签位置与参考坐标的差值；

35、基于所述差值，计算经改正后的标签位置，获得所述定位位置：

36、

37、优选的，步骤s5中，滤波处理的方法包括：

38、采用低通滤波，剔除所述加速度数据中的重力加速度；

39、采用十二极无阶butterworth滤波，对剔除所述重力加速度的所述加速度数据进行滤波以及平滑，去除加速度曲线的伪波峰波谷，完成滤波处理。

40、优选的，步骤s5中，获得速度的方法为：

41、对所述加速度数据进行butterworth滤波，获得过零时间与步数；

42、寻找滤波平滑后的所述加速度数据的波峰波谷值，获得波峰个数；

43、基于所述波峰个数，获得波峰波谷位置；

44、预设判断条件，基于所述波峰波谷位置，筛选所述过零时间，获得所述行走时间；

45、将所述行走时间相减，获得每一步的行走时间；

46、基于所述加速度数据与weinberg模型，计算每一步的步长；

47、基于所述每一步的步长、步数以及所述行走时间，获得每一步的速度。

48、优选的，步骤s6中，完成二维超宽带与惯性导航融合定位的方法为：

49、s61：基于坐标观测值以及所述每一步的速度，构建观测矩阵；

50、s62：基于所述行走时间，构建状态转移矩阵；

51、s63：基于所述观测矩阵以及所述状态转移矩阵，构建先验估计方程；

52、s64：基于过程噪声协方差矩阵、所述状态转移矩阵以及协方差，获得先验协方差；

53、s65：基于测量误差，构建观测噪声协方差矩阵，计算校正过程的卡尔曼增益；

54、s66：基于超宽带的观测方程参数以及二维坐标观测值，构建后验估计方程；

55、s67：基于单位阵、所述卡尔曼增益、所述观测方程参数以及所述协方差，更新所述先验协方差；

56、s68：基于步骤s61-s67，完成卡尔曼滤波矩阵初始化；基于所述卡尔曼滤波矩阵的计算，完成二维超宽带与惯性导航融合定位。

57、本发明还提供一种二维超宽带与惯性导航融合定位系统，所述系统应用所述的方法，包括：三维空间构建模块、标签位置获取模块、定位模块、惯性测量模块、速度获取模块以及融合定位模块；

58、所述三维空间构建模块，用于根据目标空间，构建矩形三维空间，并在所述矩形三维空间中部署基站、标签以及惯性测量单元；

59、所述标签获取模块，用于获得所述基站与所述标签之间的直线距离，并基于所述直线距离以及所述基站的二维坐标建立关系，获得所述标签的初始位置；

60、所述定位模块，用于将所述初始位置代入垂线段计算，获得垂线段长度，并以所述垂线段长度为观测量建立方程，对所述方程进行泰勒展开做线性化并解算，获得定位位置；

61、所述惯性测量模块，用于基于所述惯性测量单元，获得目标空间内行走实时加速度数据；

62、所述速度获取模块，用于对所述加速度数据进行butterworth滤波，获得步数和行走时间；并通过weinberg模型，获得步长；基于所述步数、所述行走时间以及所述步长，反算获得速度；

63、所述融合定位模块，用于将所述定位位置、所述速度以及所述行走时间，组合构建卡尔曼滤波方程，完成二维超宽带与惯性导航融合定位。

64、优选的，获得所述标签的初始位置的方法为：

65、设置n个基站保持同一高度随机分布在三维空间中，第i个基站的已知坐标为(xi,yi),i＝1,2,…,n，标签到基站的测量值为ri,i＝1,2,…,n，标签位置为(x,y)，则预设理想状态下标签与基站之间建立如下关系：

66、(x-xi)2+(y-yi)2＝ri2,i＝1,2,…,n                 (1)

67、将式(1)联立，获得多基站情况下的所述关系：

68、

69、将式(2)方程组内相互做差获得线性方程组：

70、ax＝b                          (3)

71、其中

72、基于式(3)采用最小二乘法：

73、x＝(ata)-1(atb)                           (4)

74、基于式(4)，获得所述标签的初始位置x＝(x0,y0)。

75、与现有技术相比，本发明的有益效果为：

76、构建矩形三维空间，并在矩形三维空间中部署基站、标签以及惯性测量单元；获得标签的初始位置；获得垂线段长度，并以垂线段长度为观测量建立线性方程，解算线性方程，获得定位位置获得行走实时加速度数据以及角速度数据，并将加速度数据进行滤波处理，获得行走时间与步数；通过weinberg模型计算每步步长并结合步数、行走时间反算获得速度；将定位位置、速度以及行走时间，组合构建卡尔曼滤波方程，完成二维超宽带与惯性导航融合定位。本发明中超宽带部分可用于动态与静态场景中，计步器算法可用于在原地踏步与行进的运动过程中，并且能够达到较高的准确度。使用butterworth滤波剔除测量随机误差，通过设定计步器参数解算步数确定行走时间，提高卡尔曼滤波融合定位精度。