本发明属于计算机视觉，具体涉及一种基于多相机观测的高精度高鲁棒性视觉惯性里程计定位方法。
背景技术：
1、视觉惯性里程计(visual-inertial odometry,vio)是计算机视觉领域的一个重要研究课题，其研究的问题是：如何有效地结合视觉信息与惯性测量数据，实现精准且鲁棒的位姿估计。特别是在机器人应用环境日益复杂的背景下，对定位系统精度和鲁棒性的要求愈发严格。为了提升视觉惯性里程计的精度，许多研究人员开始积极寻找有效的策略。其中一个策略是通过增加多个相机扩大观测视野，利用更丰富的观测数据来提升定位系统的精度。
2、然而，当前基于多相机观测的定位方法(如vins-fusion视觉惯性里程计定位方法)在面对具有挑战性的场景，如纹理稀疏或图像模糊等情况时，其定位精度和稳定性往往会下降。这主要是因为现有定位方法中的状态估计模块通常对不同传感器设置了固定的权重，未能充分考虑采集数据质量与场景变化之间的动态关系，从而限制了其场景灵活性。
3、基于此，本发明提出了一种基于多相机观测的高精度高鲁棒性视觉惯性里程计定位方法，可根据场景变化动态地调整不同传感器的权重，从而实现精准且鲁棒的状态估计。
技术实现思路
1、针对当前基于多相机观测的定位方法在面对具有挑战性的场景，其定位精度和稳定性会下降的不足，本发明提供了一种基于多相机观测的高精度高鲁棒性视觉惯性里程计定位方法，可根据场景变化动态地调整不同传感器的权重，从而实现精准且鲁棒的状态估计。
2、一种基于多相机观测的高精度高鲁棒性视觉惯性里程计定位方法，包括以下步骤：
3、步骤1：数据采集阶段
4、同步x个相机采集的图像，将同一时刻采集的x个图像打包为图像组，共计采集多个图像组帧，获取相邻图像组帧之间的imu(惯性测量单元)测量数据，并进行预积分处理，得到imu预积分量；其中，x为正整数；
5、假设有一个3d空间点p，能同时被不同图像组帧观测到；
6、步骤2：对图像组中的各图像进行多相机前端特征跟踪，具体为：
7、步骤2.1：将各图像均划分为m个图像网格块，m为正整数；
8、步骤2.2：利用fast特征检测算法，并行地对各图像的所有图像网格块提取fast特征，在各图像网格块中保留所得权重值最高的特征点，进而得到各图像的特征；
9、步骤2.3：使用klt光流跟踪上一图像组中各图像的特征，建立当前图像组与上一图像组中对应特征的关联，得到特征关联对，完成前向klt光流跟踪；
10、步骤2.4：基于前向klt光流跟踪所得特征关联对，使用klt光流跟踪当前图像组中各图像的特征，完成反向klt光流跟踪，得到对应于特征关联对的跟踪结果；
11、步骤2.5：计算所得跟踪结果与上一图像组中各图像的特征所在位置的距离差，根据预设的距离误差阈值，剔除距离差大于距离误差阈值的特征关联对，获得各图像正确的特征关联对；
12、步骤2.6：假设所有图像正确的特征关联对共计n个，将n个正确的特征关联对打包后，得到该图像组的特征关联结果；其中，第b,b＝1,2,...,n个正确的特征关联对表示为(ib,jb),b＝1,2,...,n，ib为上一图像组中的特征点，jb为当前图像组的特征点，代表上一图像组与当前图像组能同时观测到3d空间点p；其中，n为正整数；
13、步骤3：自适应多相机后端状态估计
14、步骤3.1：根据imu预积分量，构建imu误差模型，并固定imu误差的权重为1；
15、步骤3.2：根据所有图像组的特征关联结果，统计3d空间点p被观测到的帧数n，以及第l,l＝1,2,...,x个相机跟踪到的正确的特征关联对数量ol,l＝1,2,...,x，并构建视觉观测误差模型，包括第1次观测到3d空间点p时对应图像组帧，与此后每一次观测到3d空间点p时对应图像组帧之间的视觉观测误差；基于imu误差模型和视觉观测误差模型，构建代价函数；
16、步骤3.3：构建三层自适应置信度加权模型，包括依次的局部视觉特征层、全局视觉特征层和多传感器层；其中，
17、所述局部视觉特征层用于动态调整视觉观测误差模型的协方差矩阵因子，具体为：
18、计算各图像网格块的图像梯度评价指标qgradient：
19、qgradient＝kg·e(g)/s(g)
20、式中，g为各图像网格块中所有像素点的归一化梯度累加值；e(·)和s(·)分别代表计算均值和协方差；kg为取值0～1的权重；
21、计算各图像网格块的图像熵评价指标qentropy：
22、
23、式中，ke为取值0～1的权重；p(k)表示各图像网格块中像素值为k的概率；
24、综合考虑图像梯度评价指标qgradient和图像熵评价指标qentropy，计算各图像网格块的特征分数q：
25、q＝α×qgradient+(1-α)×qentropy
26、式中，α表示图像梯度的权重因子，取值为0～1；
27、计算特征关联结果中各特征关联对(ib,jb)的评分
28、
29、式中，β为特征点ib的质量权重因子，取值0～1；为特征点ib对应图像网格块的特征分数；为特征点jb对应图像网格块的特征分数；
30、进而，动态调整视觉观测误差模型的协方差矩阵因子
31、
32、所述全局视觉特征层用于计算视觉观测误差模型中第1次观测到3d空间点p时对应图像组帧，与此后第u,u＝2,3,...,n次观测到3d空间点p时对应图像组帧之间的视觉观测误差的权重ω1,u,u＝2,3,...,n：
33、
34、式中，σ为预设的权重上限，取值为2；
35、所述多传感器层用于计算第l个相机的权重ωl；
36、
37、式中，omax为所有相机中跟踪到的正确的特征关联对数量最大值；
38、假设第g个相机观测到3d空间点p，进而在各视觉观测误差的权重ω1,u上乘以第g个相机的权重ωg，得到自适应视觉观测误差模型；
39、步骤3.4：以z个图像组帧为一个滑动窗口，紧耦合优化代价函数，以最小化imu误差和视觉观测误差，进而得到定位信息；
40、步骤3.5：当出现新的图像组帧时，将滑动窗口中最初始的图像组帧的定位信息边缘化处理，作为下一次优化代价函数的先验信息。
41、进一步地，步骤2.2中所述fast特征检测算法具体为均匀化自适应特征检测算法，其阈值t通过计算各图像的平均梯度t确定，计算公式为：
42、
43、
44、式中，m为各图像的像素总个数；和分别为第a个像素所在位置沿x和y方向上的梯度。
45、与现有技术相比，本发明的有益效果如下：
46、1、本发明提出了一种基于多相机观测的高精度高鲁棒性视觉惯性里程计定位方法，通过构建可适应场景变化的三层自适应置信度加权模型，实现相机权重的动态调整，最大化融合并有效利用多相机数据；
47、2、本发明的三层自适应置信度加权模型从局部视觉特征层、全局视觉特征层以及多传感器层三个维度，全面评估特征质量、跟踪误差以及相机观测数据的可靠性，显著提升视觉惯性里程计的精度和鲁棒性，为定位系统带来更高的稳定性和优异性能。
1.一种基于多相机观测的高精度高鲁棒性视觉惯性里程计定位方法，其特征在于，包括以下步骤：
2.根据权利要求1所述基于多相机观测的高精度高鲁棒性视觉惯性里程计定位方法，其特征在于，步骤2.2中所述fast特征检测算法具体为均匀化自适应特征检测算法，其阈值t通过计算各图像的平均梯度t确定，计算公式为：
