本发明涉及计算机视觉与导航技术，特别是涉及一种基于粒子群优化的视觉惯性里程计校正方法。
背景技术：
1、视觉惯性里程计(vio)系统通常依赖于相机和惯性测量单元(imu)来估计设备的运动轨迹。然而，在某些情况下，例如相机剧烈抖动或imu信噪比低时，vio系统可能会产生显著的漂移，从而影响定位的准确性。为了校正这种漂移，近年来研究人员开始结合光探测与测距(lidar)点云信息来改进vio的精度。
2、lidar能够生成高精度的三维点云数据，提供比单纯图像特征点更为准确的空间几何信息。因此，lidar点云数据可以用来校正vio在某些极端情况下的漂移，提升整体系统的鲁棒性和精度。
3、现有的一些方法采用先验点云地图来辅助vio系统，例如通过将相机特征点与lidar点云特征点进行匹配，以提高定位精度。这些方法通常需要预先构建高精度的三维点云地图，且依赖于稳定的初始位姿来实现特征点的有效匹配。其缺点有：1、依赖先验点云地图，增加了部署的复杂性和成本。2、对初始位姿要求高，如果初始位姿不准确，匹配过程容易失败。3、难以适应动态环境中的变化，实时性和鲁棒性不足。
4、另一种方法是利用深度学习技术进行2d-3d特征匹配，通过训练好的模型在图像和点云之间建立匹配关系，从而实现相对位姿估计。其缺点有：1、需要大量的训练数据和计算资源，训练过程复杂且耗时。2、在未知环境中，模型可能无法有效泛化，导致匹配精度下降。3、实时性不足，难以满足高频率的位姿估计需求。
5、需要说明的是，在上述背景技术部分公开的信息仅用于对本技术的背景的理解，因此可以包括不构成对本领域普通技术人员已知的现有技术的信息。
技术实现思路
1、本发明的主要目的在于解决上述背景技术中存在的问题，提供一种基于粒子群优化的视觉惯性里程计校正方法。
2、为实现上述目的，本发明采用以下技术方案：
3、在本发明第一方面，一种基于粒子群优化的视觉惯性里程计校正方法，包括以下步骤：
4、步骤一、采用特定函数初始化采样空间，将给定的初始采样区域划分成多个均匀的小立方体，形成搜索空间；
5、步骤二、在每个小立方体内随机生成一组粒子，代表可能的解决方案，并赋予初始速度；
6、步骤三、将每个粒子的当前位置设为其个人最优解，并从所有粒子中选择适应度最低的作为全局最优解；
7、步骤四、进入迭代过程，对每个粒子进行评估和更新，通过关联函数匹配粒子位置与点云数据，计算关联特征点，并根据适应度结果更新个人和全局最优解；
8、步骤五、更新粒子的速度和位置，模拟鸟群觅食行为，逐步逼近最优解，其中速度和位置更新考虑个人最优解和全局最优解，以及引入的随机探索性；
9、步骤六、迭代达到预定次数后，输出全局最优解作为相机和lidar的相对位姿估计结果，完成优化校正过程。
10、进一步地，步骤一中，通过函数以初始采样空间b为基础，以为中心，将采样空间均匀划分为sc个立方块。
11、进一步地，步骤二中，通过函数在每个采样空间bi中随机选择m个点作为初始粒子pi，并初始化初始粒子的速度vi。
12、进一步地，步骤三中，将每个粒子的初始位置设为其个人最佳位置pbesti，并将全局最佳位置gbesti设置为当前粒子中适应度最小的位置；
13、步骤四中，在每次迭代中，针对每个粒子，进行以下操作：
14、通过函数将当前粒子位置pi与lidar点云l和视觉特征点云f进行关联，计算关联特征点；
15、通过函数评估当前粒子位置的适应度ρ(pi)；
16、如果当前粒子位置的适应度优于其个人最佳位置，则更新个人最佳位置pbesti；
17、如果当前粒子位置的适应度优于全局最佳位置，则更新全局最佳位置gbesti。
18、进一步地，步骤五中，根据以下公式更新每个粒子的速度和位置：
19、vi←w·vi+c1·r1·(pbesti-pi)+c2·r2·(gbest-pi)
20、pi←pi+vi
21、其中，w是惯性权重，c1是个人学习系数，c2是社会学习系数，r1和r2是随机数。
22、进一步地，步骤五中，速度和位置更新包括：
23、选择惯性权重，以平衡算法的全局搜索能力和收敛速度；
24、调整社会学习系数，以促进算法快速收敛，同时避免陷入局部最优解；
25、设置个人学习系数，以增强粒子向个人最佳位置的收敛性，减少适应度方差；
26、根据算法的收敛情况和适应度方差，动态调整以上参数，实现算法性能的优化。
27、在本发明第二方面，一种基于粒子群优化的视觉惯性里程计校正方法，包括：
28、检测触发条件：当协同视觉惯性里程计模块检测到相机剧烈抖动或imu信噪比低时，触发事件采样模块；
29、判断特征点与点云数据关联程度：基于有效特征-平面关联数(vfpn)，评估当前环境中的特征点和点云数据的关联程度；
30、触发粒子群优化算法：当vfpn达到预设阈值时，表明vio系统漂移显著，系统自动启动粒子群优化(pso)算法进行相机-lidar相对位姿的粗略估计；
31、坐标系变换与位姿估计：在相邻的两个相机帧之间，估计相对位姿并进行坐标系变换，使用vio推导当前帧相对于lidar坐标系的旋转和平移；
32、参数标定：预先准确标定从相机坐标系到imu坐标系的旋转矩阵，作为外参参数；
33、粒子群优化算法迭代：初始化一组粒子，每个粒子代表一个候选解，通过迭代更新粒子的速度和位置，逼近全局最优解；
34、适应度评估与更新：对每个粒子进行适应度评估，根据评估结果更新个人最佳位置和全局最佳位置；
35、速度和位置更新：根据惯性权重、个人学习系数和社会学习系数，更新粒子的速度和位置；
36、输出最优解：当迭代达到预定次数后，输出全局最佳位置作为相机和lidar的最优相对位姿。
37、在本发明第三方面，一种计算机可读存储介质，存储有计算机程序，所述计算机程序由处理器执行时实现所述的基于粒子群优化的视觉惯性里程计校正方法。
38、在本发明第四方面，一种计算机程序产品，包括计算机程序，所述计算机程序由处理器执行时实现所述的基于粒子群优化的视觉惯性里程计校正方法。
39、本发明具有如下有益效果：
40、本发明有效解决了当前视觉惯性里程计(vio)系统在以下方面的不足：
41、1、校正vio系统的漂移问题
42、现有vio系统在相机剧烈抖动或imu信噪比低的情况下，容易产生显著的位姿漂移，影响定位精度。本发明结合lidar点云信息，通过捆绑调整优化框架，校正vio的漂移问题，提升整体定位精度。
43、2、提高系统的实时性和鲁棒性
44、相较于现有方法在动态环境中难以有效应对环境变化，导致定位精度下降。本发明通过事件触发机制和粒子群优化算法，动态调整相机和lidar的相对位姿估计，增强系统在动态环境中的适应性和稳定性。
45、与传统方法相比，本发明的技术优势主要有：
46、1、提高定位精度。本发明通过引入基于事件触发的粒子群优化(pso)算法，有效校正了视觉惯性里程计(vio)的漂移问题。在多种极端情况下(如相机剧烈抖动或imu信噪比低)，pso能够快速逼近最优解，从而显著提高系统的定位精度。
47、2、降低计算和时间成本
48、实验结果表明，pso算法在单次采样执行时间和采样次数上表现优异。尽管相对于grs略慢，但明显优于aus，且在误差和收敛速度上具有显著优势。这意味着在实际应用中，pso算法能够以较低的计算和时间成本，提供高精度的定位服务,
49、3、处理异常值和噪声。pso算法能够有效处理采样空间中的异常值和噪声问题。通过在全局范围内进行优化，避免了特征点云与lidar地图之间的错误配准，提升了系统的稳健性和可靠性。
50、本发明实施例中的其他有益效果将在下文中进一步述及。
1.一种基于粒子群优化的视觉惯性里程计校正方法，其特征在于，包括以下步骤：
2.如权利要求1所述的基于粒子群优化的视觉惯性里程计校正方法，其特征在于，步骤一中，通过函数以初始采样空间b为基础，以为中心，将采样空间均匀划分为sc个立方块。
3.如权利要求1或2所述的基于粒子群优化的视觉惯性里程计校正方法，其特征在于，步骤二中，通过函数在每个采样空间bi中随机选择m个点作为初始粒子pi，并初始化初始粒子的速度υi。
4.如权利要求1至3任一项所述的基于粒子群优化的视觉惯性里程计校正方法，其特征在于，步骤三中，将每个粒子的初始位置设为其个人最佳位置pbesti，并将全局最佳位置gbesti设置为当前粒子中适应度最小的位置；
5.如权利要求4所述的基于粒子群优化的视觉惯性里程计校正方法，其特征在于，步骤五中，根据以下公式更新每个粒子的速度和位置：
6.如权利要求1至5任一项所述的基于粒子群优化的视觉惯性里程计校正方法，其特征在于，步骤五中，速度和位置更新包括：
7.一种基于粒子群优化的视觉惯性里程计校正方法，其特征在于，包括：
8.一种计算机可读存储介质，存储有计算机程序，其特征在于，所述计算机程序由处理器执行时实现如权利要求1-7任一项所述的基于粒子群优化的视觉惯性里程计校正方法。
9.一种计算机程序产品，包括计算机程序，其特征在于，所述计算机程序由处理器执行时实现如权利要求1-7任一项所述的基于粒子群优化的视觉惯性里程计校正方法。
