本发明涉及机器人与计算机视觉技术，尤其是一种无人机-无人车协同系统中的实时相对位姿估计方法。
背景技术：
1、单目视觉惯性里程计(vio)技术是结合单目摄像机图像与惯性测量单元(imu)数据实现实时定位和姿态估计的综合技术。它可帮助无人机和机器人在缺乏gps信号的复杂环境中无需人工参与而自主移动完成既定任务。单目vio能够显著提升移动设备的自主化和智能化水平。单目vio系统的搭建依赖于高效的图像处理算法和精确的惯性数据融合，需要完成环境感知、数据同步和轨迹估计等功能，并尽可能实现跨平台的通用性和实时性。
2、同步定位与地图构建(slam)是一种技术，通过在未知环境中使用传感器(如激光雷达、相机、imu等)实时估计机器人或设备的位置，同时构建出该环境的地图。slam技术能够使机器人在不依赖预先构建地图的情况下自主导航，有效应对动态和复杂的环境变化，并广泛应用于无人驾驶、无人机、服务机器人等领域。
3、因子图优化是一种将多变量问题建模为图形结构并通过全局优化求解的方法。它通过将位姿和观测值表示为节点，将约束表示为边，形成一个因子图，从而有效地解决机器人定位与地图构建(slam)中的大规模优化问题。因子图优化能够处理复杂的多维度数据关系，提高估计的精度和鲁棒性，并广泛应用于视觉slam、激光slam等领域。
4、根据实现方式的不同，位姿估计方法可以分为基于滤波的和基于优化的。基于滤波的位姿估计方法通过实时处理传感器(如imu、gps、激光雷达)的数据，采用卡尔曼滤波或粒子滤波等技术，对位姿进行增量更新和误差校正；而基于优化的位姿估计方法则利用全局优化技术，如因子图优化，通过最小化整个轨迹或地图的重投影误差来获得更高精度的位姿估计。这两种方法各有优劣，基于滤波的方法适用于实时性要求较高的场景，而基于优化的方法则在高精度和全局一致性方面表现更佳。
5、基于学习的方法通过学习图像和点云之间共同的特征描述符来建立对应关系，该方法利用端到端的深度网络架构，分别从图像和点云中联合学习2d和3d关键点的描述符，并基于描述符之间的匹配关系实现相对位姿估计。首先，使用端到端的深度网络架构从图像和点云中分别提取2d和3d关键点的描述符，并生成匹配关系数据库；接着，通过投影和优化算法，将图像特征点与点云特征点进行匹配，得到相对位姿估计。本方法提高了图像与点云数据融合的性能，显著提升了运算效率和精度，在复杂环境下具有较好的适应性和稳定性。此类方法通常将图像到点云的问题分解为两个模块，即2d-3d关联模块和位姿估计模块。然而，2d-3d关联模块中的误差不会在随后的位姿估计模块中得到纠正，因为它们没有为一个唯一的目标联合优化。基于学习的方法在未知复杂场景中的泛化性有限，同时所需计算资源较大，很难实现实时的位姿估计。且2d-3d关联时的误匹配会使位姿估计结果发散。
6、基于匹配的相对位姿估计算法通过直接或间接的方式在2d图像和3d点云之间建立匹配关系，然后基于这种匹配关系实现相对位姿估计。首先，在直接方法中，将3d点云投影到二维图像上，使用相似度指标进行比较，生成匹配关系；在间接方法中，提取2d和3d特征线，并在线特征之间建立匹配关系。接着，通过卡尔曼滤波或因子图优化算法，估计相机的相对位姿；然后，利用点到面的残差项和雅可比矩阵，进一步优化位姿估计结果；最后，通过滤波和误差修正方法，提升匹配和估计的准确性与鲁棒性。本方法提高了图像与点云数据融合的性能，显著提升了运算效率和精度，在复杂环境下具有较好的适应性和稳定性。基于预提取特征匹配的方法需要对环境地图进行精细的预处理，且高度依赖场景中的结构化信息，在野外等非结构化场景中，容易出现特征不足和误匹配等问题。
7、针对灾后搜救、大区域地图覆盖性探索、复杂地形协同导航等场景，无人机和无人车的协同配合通常能够利用两者之间的互补特点，即无人机具有高速灵活的敏捷移动能力但载重能力较差，无人车具有较大的负载能力但视野和移速都十分有限。在复杂场景中，无人机和无人车的配合能够利用两者互补优势，大大提高执行任务的效率和效果。因此，发展智能化无人机-无人车空地协同平台，研究无人机-无人车相对定位算法对实际应用场景具有极大的价值。
8、在无人机-无人车空地协同场景中，在没有先验环境地图且没有先验初始相对位姿估计的情况下实时估计无人机和无人车的相对位姿是两者协同的基础。仅仅依赖视觉-惯性里程计无法估计无人机和无人车的相对位姿，且在运行时存在轨迹估计误差积累和漂移等问题，难以在大尺度环境或视觉特征退化场景下稳定运行。基于预提取特征匹配的方法需要对环境地图进行精细的预处理，且高度依赖场景中的结构化信息，在野外等非结构化场景中，容易出现特征不足和误匹配等问题。基于深度学习的方法通常缺乏泛化性，且难以在机器人上实时运行。
9、因此，鲁棒且实时的无人机-无人车相对定位需要：
10、1、快速稳定的求解算法。
11、2、不依赖先验的环境地图信息。
12、3、利用环境信息实时校正视觉-惯性里程计的位姿估计漂移误差。
13、4、有效的融合视觉和激光雷达的异构信息。
14、需要说明的是，在上述背景技术部分公开的信息仅用于对本技术的背景的理解，因此可以包括不构成对本领域普通技术人员已知的现有技术的信息。
技术实现思路
1、本发明的主要目的在于解决上述背景技术中存在的问题，提供一种无人机-无人车协同系统中的实时相对位姿估计方法。
2、为实现上述目的，本发明采用以下技术方案：
3、一种无人机-无人车协同系统中的实时相对位姿估计方法，包括以下步骤：
4、步骤一、视觉-惯性里程计vio：使用单目相机图像和惯性测量单元imu数据，通过视觉特征点提取加追踪与imu预积分作为前端，以及因子图优化作为后端，分别实现无人机和无人车的实时位姿估计，并构建各自的稀疏特征点云地图；
5、步骤二、激光雷达-惯性里程计lio：利用激光雷达实时获取的点云数据与imu数据相结合，采用fast-lio算法，通过计算点到面残差并利用迭代误差状态卡尔曼滤波ieskf，分别完成无人机和无人车位姿与各自环境的稠密三维点云地图的实时构建；
6、步骤三、实时相对位姿估计：在步骤一和步骤二的基础上，构建因子图优化问题，包括基于步骤一得到的稀疏特征点云和步骤二得到的稠密点云之间的点面匹配残差因子，以及视觉特征点重投影残差因子和imu预积分残差因子，通过最小化残差来实时求解无人机与无人车之间的相对位姿。
7、其中，步骤一为步骤二和步骤三提供了初始位姿估计和稀疏特征点云，步骤二为步骤三提供了稠密点云数据，步骤三综合步骤一和步骤二的数据，通过点云配准和优化算法实现精确的相对位姿估计。
8、一种计算机可读存储介质，存储有计算机程序，所述计算机程序由处理器执行时实现所述的无人机-无人车协同系统中的实时相对位姿估计方法。
9、一种计算机程序产品，包括计算机程序，所述计算机程序由处理器执行时实现所述的无人机-无人车协同系统中的实时相对位姿估计方法。
10、本发明具有如下有益效果：
11、本发明的无人机-无人车协同系统中的实时相对位姿估计方法，创新性地融合了视觉-惯性里程计(vio)与激光雷达-惯性里程计(lio)技术，以及因子图优化策略，以实现在复杂环境中的高效协同作业。该方法通过单目相机和imu数据的紧密结合，利用高效的图像处理算法和精确的惯性数据融合，不仅提升了系统的自主化和智能化水平，还保障了跨平台的通用性和实时性。同时，通过引入激光雷达点云数据，结合fast-lio算法，本发明能够生成精确的三维环境地图，并通过点云配准技术，显著提高位姿估计的准确性和鲁棒性。
12、本发明的位姿估计方法实现了在非结构化环境下无人机-无人车无先验实时位姿估计功能，不依赖于先验的环境地图信息；实现了融合激光雷达点云信息的视觉里程计因子图优化问题构建，解决了视觉-惯性里程计固有的漂移问题，能够实时校正视觉-惯性里程计中的位姿估计漂移误差，有效解决了在大尺度环境或视觉特征退化场景下稳定运行的难题。其深度融合视觉和激光雷达的异构信息，增强了系统在野外等非结构化场景中的适应性，减少了对场景结构化信息的依赖，降低了因特征不足或误匹配导致的问题。最终，该方法通过快速稳定的求解算法，为无人机和无人车在灾后搜救、大区域地图覆盖性探索、复杂地形协同导航等场景中的协同作业提供了强有力的技术支持，显著提升了任务执行的效率和效果。
13、本发明实施例中的其他有益效果将在下文中进一步述及。
1.一种无人机-无人车协同系统中的实时相对位姿估计方法，其特征在于，包括以下步骤：
2.如权利要求1所述的无人机-无人车协同系统中的实时相对位姿估计方法，其特征在于，
3.如权利要求1或2所述的无人机-无人车协同系统中的实时相对位姿估计方法，其特征在于，步骤一具体包括：
4.如权利要求1至3任一项所述的无人机-无人车协同系统中的实时相对位姿估计方法，其特征在于，步骤二具体包括：
5.如权利要求1至4任一项所述的无人机-无人车协同系统中的实时相对位姿估计方法，其特征在于，步骤三具体包括：
6.如权利要求5所述的无人机-无人车协同系统中的实时相对位姿估计方法，其特征在于，步骤三中，所述基于点面匹配的点云残差因子的构建和使用包括：
7.如权利要求5所述的无人机-无人车协同系统中的实时相对位姿估计方法，其特征在于，步骤三中，所述视觉特征点重投影残差因子的处理步骤，具体包括：
8.如权利要求5所述的无人机-无人车协同系统中的实时相对位姿估计方法，其特征在于，步骤三中，imu预积分残差因子的处理步骤，具体包括：
9.一种计算机可读存储介质，存储有计算机程序，其特征在于，所述计算机程序由处理器执行时实现如权利要求1-8任一项所述的无人机-无人车协同系统中的实时相对位姿估计方法。
10.一种计算机程序产品，包括计算机程序，其特征在于，所述计算机程序由处理器执行时实现如权利要求1-8任一项所述的无人机-无人车协同系统中的实时相对位姿估计方法。
