【技术实现步骤摘要】
本专利技术属于多源传感器融合导航，具体涉及一种视觉惯性超宽带融合导航中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小于设定的阈值，则认为估本文档来自技高网...
【技术保护点】
1.一种视觉惯性超宽带融合导航中UWB锚点位置估计方法，其特征在于，包含：
2.根据权利要求1所述的视觉惯性超宽带融合导航中UWB锚点位置估计方法，其特征在于，视觉/惯性融合导航系统初始化包括传感器数据时间对齐处理：VINS-MONO中实现了视觉传感器数据和惯性传感器数据的时间同步，采用线性插值的方式实现视觉传感器数据和UWB传感器数据的时间同步。
3.根据权利要求1所述的视觉惯性超宽带融合导航中UWB锚点位置估计方法，其特征在于，所述先通过最小二乘原理估计得到锚点位置的初值包含：
4.根据权利要求3所述的视觉惯性超宽带融合导航中UWB锚点位置估计方法，其特征在于，所述通过抗差岭最小二乘迭代计算UWB锚点位置包含：
5.根据权利要求3所述的视觉惯性超宽带融合导航中UWB锚点位置估计方法，其特征在于，对于公式(6)，GDOP值的计算公式为：
6.根据权利要求5所述的视觉惯性超宽带融合导航中UWB锚点位置估计方法，其特征在于，如果GDOP小于设定的阈值，则认为估计的锚点位置精度较高，UWB锚点位置估计完成并将其作为已知值用于
7.根据权利要求6所述的视觉惯性超宽带融合导航中UWB锚点位置估计方法，其特征在于，如果观测值数量小于最大阈值，则继续增加观测值数量，然后通过抗差岭最小二乘迭代重新估计UWB锚点位置，并重新计算GDOP并判断GDOP值是否大于阈值；如果观测值数量大于最大阈值，结束UWB锚点位置估计，并将最新估计的UWB锚点位置作为已知值用于后续的视觉/惯性/UWB融合导航算法。
8.一种视觉惯性超宽带融合导航中UWB锚点位置估计系统，其特征在于，用于实现如权利要求1-7任一项所述的视觉惯性超宽带融合导航中UWB锚点位置估计方法，包含：
...【技术特征摘要】
1.一种视觉惯性超宽带融合导航中uwb锚点位置估计方法，其特征在于，包含：
2.根据权利要求1所述的视觉惯性超宽带融合导航中uwb锚点位置估计方法，其特征在于，视觉/惯性融合导航系统初始化包括传感器数据时间对齐处理：vins-mono中实现了视觉传感器数据和惯性传感器数据的时间同步，采用线性插值的方式实现视觉传感器数据和uwb传感器数据的时间同步。
3.根据权利要求1所述的视觉惯性超宽带融合导航中uwb锚点位置估计方法，其特征在于，所述先通过最小二乘原理估计得到锚点位置的初值包含：
4.根据权利要求3所述的视觉惯性超宽带融合导航中uwb锚点位置估计方法，其特征在于，所述通过抗差岭最小二乘迭代计算uwb锚点位置包含：
5.根据权利要求3所述的视觉惯性超宽带融合导航中uwb锚点位置估计方法，其特征在于，对于公式(6)，gdop值的计算公式为：
6.根据权利要求5所述的视觉惯性...
【专利技术属性】
技术研发人员：罗豪龙，李广云，李建胜，王安成，王力，李雪强，杨子迪，郭雨岩，魏亚旭，
申请(专利权)人：中国人民解放军战略支援部队信息工程大学，
类型：发明
国别省市：
还没有人留言评论。发表了对其他浏览者有用的留言会获得科技券。