视觉惯性超宽带融合导航中UWB锚点位置估计方法及系统

本发明属于多源传感器融合导航，具体涉及一种视觉惯性超宽带融合导航中uwb锚点位置估计方法及系统。

背景技术：

1、多源传感器融合导航技术能够充分发挥每种传感器的优势并弥补单一传感器的不足，其在自动驾驶，智慧城市，无人配送，航空航天等领域的应用愈加广泛。其中，基于视觉/惯性/超宽带的融合导航技术具有成本低，定位精度高，系统配置简单等优势，受到学者的广泛研究和关注。根据导航原理的不同，视觉/惯性/uwb耦合导航方法可以分为紧耦合导航和松耦合导航。相比于视觉/惯性/uwb松耦合导航方法，视觉/惯性/uwb紧耦合导航方法具有更高的鲁棒性，其定位精度也往往高于松耦合导航方法。然而，视觉/惯性/uwb紧耦合导航过程中，由于导航系统的坐标系在系统初始化时在线建立，因此无法提前获取uwb锚点在导航系下的位置。这就要求在利用uwb的测距信息辅助视觉/惯性导航前，首先获取uwb锚点在导航坐标系下的位置。如何准确快速地估计uwb锚点位置，对于修正定位累积误差和保证系统稳定工作具有重要作用。

2、视觉/惯性/超宽带(uwb：ultra wide band)的紧耦合导航系统初始化完成后，需要首先估计uwb锚点在系统下的位置。现有的uwb锚点位置估计方法主要分为两种：一种在系统初始化完成后，根据一定数量的观测值(包括视觉/惯性导航算法解算的系统位置和uwb的测距值信息)，通过非线性最小二乘原理如levenberg-marquardt(lm)方法估计锚点位置；另一种是通过fisher信息矩阵等方式来评估锚点位置的不确定性，然后规划系统的运动路径，从而实现锚点的准确位置估计。然而，现有方法存在两方面的问题：对于第一类锚点位置方法，锚点位置的估计精度严重依赖于观测值的数据质量，估计的锚点位置精度具有极大的不确定性；对于第二类锚点位置估计方法，其严格限制了载体的运动路径，与载体的自主导航初衷相矛盾且计算复杂。

3、一方面，快速的估计uwb锚点位置，有利于尽早利用uwb的测距值修正系统的累积误差，从而得到更准确的定位结果。另一方面，准确的估计uwb锚点位置，能够降低uwb测距值约束项的误差，从而提高系统的定位精度。然而，实际中获取的观测值越多，uwb的锚点估计往往越可靠，但uwb锚点估计所花费的时间越长。因此，兼顾uwb锚点的估计精度和估计时间，实现uwb锚点位置的准确快速估计，对于提高视觉/惯性/uwb组合导航的定位精度和鲁棒性具有重要作用。

技术实现思路

1、本发明的目的在于解决现有技术存在的问题，提出一种视觉惯性超宽带融合导航中uwb锚点位置估计方法及系统，在载体自主导航的基础上，不对运动路径进行限制，在保证uwb锚点位置估计精度的前提下实现uwb锚点位置的快速估计。

2、为了实现上述目的，本发明采用以下的技术方案：

3、一种视觉惯性超宽带融合导航中uwb锚点位置估计方法，包含：

4、首先通过开源vins-mono算法完成视觉/惯性融合导航系统初始化，并建立视觉/惯性融合导航系统坐标系；之后开始收集观测值数据，该观测值数据包括视觉/惯性融合导航系统的定位结果和uwb测距值；

5、当观测值数量达到设置的阈值，先通过最小二乘原理估计得到锚点位置的初值，之后通过抗差岭最小二乘迭代计算uwb锚点位置，然后计算gdop对估计的uwb锚点位置进行精度评估。

6、根据本发明视觉惯性超宽带融合导航中uwb锚点位置估计方法，进一步地，视觉/惯性融合导航系统初始化包括传感器数据时间对齐处理：vins-mono中实现了视觉传感器数据和惯性传感器数据的时间同步，采用线性插值的方式实现视觉传感器数据和uwb传感器数据的时间同步。

7、根据本发明视觉惯性超宽带融合导航中uwb锚点位置估计方法，进一步地，所述先通过最小二乘原理估计得到锚点位置的初值包含：

8、首先根据视觉/惯性融合导航系统实时获取惯性坐标系相对于世界坐标系的位置和姿态；根据视觉/惯性融合导航系统的定位结果和uwb测距值，构造残差函数：

9、

10、其中，||·||表示向量的2范数，dk表示k时刻uwb标签与uwb锚点的测距值，和分别表示k时刻惯性坐标系相对于世界坐标系的位置和姿态，表示uwb锚点在世界坐标系下的位置，表示惯性坐标系下uwb标签的位置，通过离线测量得到；均为已知量，k时刻世界坐标系下uwb标签的位置记作则有：

11、

12、观测方程改写为：

13、

14、将uwb锚点位置和uwb标签位置代入方程(3)得到：

15、

16、方程(4)经过变化得到：

17、

18、通过最小二乘原理，即得到锚点位置的初值

19、根据公式(5)得到的锚点位置初值将公式(4)在处进行一阶泰勒级数线性展开，并忽略二次幂及以上的项，经整理得到：

20、

21、其中：

22、

23、

24、

25、

26、根据最小二乘原理，得到：

27、

28、其中，表示估计的uwb锚点位置增量。

29、根据本发明视觉惯性超宽带融合导航中uwb锚点位置估计方法，进一步地，所述通过抗差岭最小二乘迭代计算uwb锚点位置包含：

30、利用等价权矩阵替代公式(7)中的单位权矩阵p，其中为对角矩阵，中的每个对角元素满足pi为每次观测的权因子系数，wi的形式为：

31、

32、其中，k0,k1为经验定值，v为残差量与标准化残差的比值；

33、采用岭估计原理通过有偏估计避免估计的uwb锚点位置结果不稳定，首先根据岭估计中的条件数法判断方程(6)是否病态，令：

34、f(n)＝||n||||n-1||

35、n的特征根记作λ1≥λ2≥…≥λt＞0，则有：

36、

37、即当f(n)＜100，认为法方程非病态，岭参数α的值取为0；当f(n)≥100，认为法方程病态，岭参数α的计算方式通过l曲线方法得到；

38、因此，根据抗差岭估计原理，uwb锚点位置增量的估计方程变换为：

39、

40、通过抗差岭最小二乘迭代方法即可得到uwb锚点位置增量；

41、最终，估计的uwb锚点位置为：

42、

43、其中，x0为估计的锚点位置初值

44、根据本发明视觉惯性超宽带融合导航中uwb锚点位置估计方法，进一步地，对于公式(6)，gdop值的计算公式为：

45、

46、其中，tr(·)表示矩阵的秩；如果gdop值过大，表明估计的uwb锚点位置精度低，如果gdop值小，表明估计的uwb锚点位置精度高。

47、根据本发明视觉惯性超宽带融合导航中uwb锚点位置估计方法，进一步地，如果gdop小于设定的阈值，则认为估计的锚点位置精度较高，uwb锚点位置估计完成并将其作为已知值用于后续的视觉/惯性/uwb融合导航算法；如果gdop大于设定的阈值，则认为估计的锚点位置精度较差，此时判断观测值数量是否大于设定的最大阈值。

48、根据本发明视觉惯性超宽带融合导航中uwb锚点位置估计方法，进一步地，如果观测值数量小于最大阈值，则继续增加观测值数量，然后通过抗差岭最小二乘迭代重新估计uwb锚点位置，并重新计算gdop并判断gdop值是否大于阈值；如果观测值数量大于最大阈值，结束uwb锚点位置估计，并将最新估计的uwb锚点位置作为已知值用于后续的视觉/惯性/uwb融合导航算法。

49、一种视觉惯性超宽带融合导航中uwb锚点位置估计系统，用于实现如上述的视觉惯性超宽带融合导航中uwb锚点位置估计方法，包含：

50、初始化及数据采集模块，用于首先通过开源vins-mono算法完成视觉/惯性融合导航系统初始化，并建立视觉/惯性融合导航系统坐标系；之后开始收集观测值数据，该观测值数据包括视觉/惯性融合导航系统的定位结果和uwb测距值；

51、位置估计及精度评估模块，用于当观测值数量达到设置的阈值，先通过最小二乘原理估计得到锚点位置的初值，之后通过抗差岭最小二乘迭代计算uwb锚点位置，然后计算gdop对估计的uwb锚点位置进行精度评估。

52、与现有技术相比，本发明具有以下优点：

53、1、对于视觉/惯性/uwb紧耦合导航过程中uwb锚点位置估计问题，为避免法方程病态导致解的结果不稳定，并根据观测值的数据质量赋予合理的权重，提出一种基于抗差岭最小二乘的uwb锚点位置估计方法，不仅对观测值的数据质量赋予合理的权重，而且避免了锚点位置估计结果的不稳定，提高了uwb锚点位置估计的精度和稳定性。

54、2、为了对估计的uwb锚点位置进行精度评估，本发明基于卫星导航中gdop的原理，采用逆向的思路，将视觉/惯性算法解算的运动轨迹看作是多个uwb锚点，将待估计的uwb锚点作为优化变量，计算uwb锚点位置的gdop以评估uwb锚点位置的估计精度。

55、3、本发明基于抗差岭最小二乘原理进行锚点的位置估计，并基于gdop的原理评估锚点的定位精度，从而实现了视觉/惯性/uwb融合导航中uwb锚点位置的准确快速估计，将估计的uwb锚点位置作为已知值用于后续的视觉/惯性/uwb融合导航算法，既能提高系统的定位精度，又能增强系统的鲁棒性。本发明为视觉/惯性/uwb融合导航中uwb锚点位置估计和精度评估提供了一种新思路和计算方法，有利于推动视觉/惯性/uwb融合导航技术的研究，对于多源传感器融合导航技术的发展具有重要意义。