本发明涉及空间探测图像数据处理领域，具体涉及月面环境下相机-雷达-惯性耦合建图方法及系统。
背景技术：
1、月球作为地球唯一的天然卫星，是人类进行深空探测和太空资源开发利用的首选目标。在月面和月球轨道上建设大型的科学实验设施，即月球科研站，有望能够实现月球资源利用的全方位发展、开展长期、较大规模的科研工作。
2、月面探测器是月面作业的执行体和重要组成部分，其正常活动的前提是利用多种传感器对周围环境进行感知，实现自身位姿估计和周围环境建图，为路径规划、自主避障、导航控制等功能打下基础。
3、目前月面探测器的感知多依赖于双目立体视觉系统，利用双目相机模仿人眼感知外部世界中景物位置的模型，来估计景物深度；以及使用双目相机构建帧间特征匹配来估计探测器相对位姿。然而，双目相机的结果对光照条件依赖较大，并且深度估计有理论最大值，在月面光照条件不稳定和大范围探测的情况下表现欠佳；单独使用激光雷达探测，精度又略逊一筹。
4、在生成可行性地图时，目前月面重建算法多采用二维栅格地图，构建简单，数据量小，计算速度快。但同时二维栅格地图存储的信息量也低，无法对探测器周围环境有更丰富的描述，因此它并不适用于在非结构化且复杂的月面环境中进行感知。三维栅格地图虽然信息丰富，但对存储空间的开销较大且计算复杂度较高，月面探测器计算资源有限，并不适合实时路径搜索与规划。
5、常用的月面三维重建算法只考虑对静态地形的分析，但是为了实现月球资源的原位开发与利用，以及后续月球科研基地的建设，月面探测器不再是孤立行动，而是趋向于多智能体协同作业。在此情景下，单单考虑对静态环境建图已经不能满足需求。
6、目前科研人员也提出了许多在地面上应用的耦合相机-雷达-惯性进行建图的方案。例如公布号为cn116182844a的现有发明专利申请文献《基于立体视觉、惯性、激光雷达的同步定位与建图系统及方法》，在该现有系统中，设置视觉前段模块、vio后端模块、回环检测模块和lidar建图四个模块；由该现有方案的具体实施内容可知，该文献采用了vio模块先进行位姿获取，随后lidar进行校正，再用lidar数据建图的传感器数据耦合。然而，该现有技术未对相机图像进行深度配准、同时该现有技术中的因子图中的约束也较单一。
7、以及公布号为cn114608561a的现有发明专利申请文献《一种基于多传感器融合的定位与建图方法及系统》，该现有方法包括：获取目标区域的激光雷达数据、惯性测量数据和视频图像数据，并基于上述数据构建激光惯性里程计和视觉惯性融合里程计，并将所述激光惯性里程计和视觉惯性融合里程计进行融合，完成局部位姿优化和全局位姿优化，并基于所述全局位姿优化的结果对目标区域进行定位与建图。由该现有技术的具体实施方案可知，该方案构建激光惯性里程计和视觉惯性融合并融合，完成位姿优化、定位与建图。在前述现有方案中，尽管采用了多模态的数据融合，但没有考虑某一传感器失效时系统的运行状况，欠缺鲁棒性；并且该现有方案数据的耦合方式固定，缺乏不同环境下因传感器的数据质量差别而动态耦合的技术。
8、以及公布号为cn110261870a的现有发明专利申请文献《一种用于视觉-惯性-激光融合的同步定位与建图方法》，在该现有方案中，将多个传感器耦合使用，并且在某个传感器失效时其他模块仍能正常工作。该现有技术更侧重于其中一个模块失效时，系统对剩余模块的自动调整。同时存在数据耦合不全面的问题。
9、以及公布号为cn116182837a的现有发明专利申请文献《基于视觉激光雷达惯性紧耦合的定位建图方法》，该现有方案针对典型激光雷达退化场景，提出一种基于视觉激光雷达惯性紧耦合的定位建图方法，以满足建图需求和鲁棒性。相对于本专利，该专利的数据耦合体现在构建视觉重投影、imu预积分和雷达几何约束，联合三种约束建立非线性优化问题。然而，该现有技术采用的优化因子单一，同时选择新雷达关键帧时未添加新机器人状态至因子图中，在建图过程中难以处理动态物。
10、综上，现有技术存在回环检测效率低、对月面探测器计算资源消耗较高、在多智能体协作情形下存在动态干扰以及鲁棒性较差的技术问题。
技术实现思路
1、本发明所要解决的技术问题在于：如何解决现有技术中回环检测效率低、对月面探测器计算资源消耗较高、在多智能体协作情形下存在动态干扰以及鲁棒性较差的技术问题。
2、本发明是采用以下技术方案解决上述技术问题的：月面环境下相机-雷达-惯性耦合建图方法包括：
3、s1、将双目相机、激光雷达设置于探测器上，利用双目相机采集获取相机图像，利用激光雷达采集获取深度信息，利用imu模块采集探测器的加速度数据、角速度数据；
4、s2、建立并利用雷达-惯性子系统，获得的系统状态、imu参数，以初始化相机-惯性子系统；建立并利用相机-惯性子系统，提取并追踪相机图像中的点线特征，据以处理得到探测器的初步位姿估计数据、周围环境三维点云；
5、s3、利用深度信息，对相机图像进行深度配准；利用相机-惯性子系统，采用fbow方法进行回环检测，以得到回环关键帧，提取brief描述子并聚类，通过对比词袋向量，比较图像之间相似性；将检测到的回环关键帧发送给至雷达-惯性子系统供判定操作；
6、s4、在雷达-惯性子系统、相机-惯性子系统中，存在失效子系统时，利用未失效子系统进行建图操作；利用未失效子系统初始化失效子系统，据以处理得到因子图约束；
7、s5、将因子图约束添加至因子图；
8、s6、获取并添加新机器人状态至因子图，以确定新雷达关键帧；
9、s7、根据因子图优化初步位姿估计数据、周围环境三维点云，以得到点云输入数据、位姿估计输入数据，根据新雷达关键帧进行雷达坐标系和相机坐标系的标定，获取传感器坐标系；
10、s8、根据传感器坐标系，处理点云输入数据、位姿估计输入数据，以得到预置维数高程图，据以获取局部高程图；
11、s9、采用基于光线追踪方法，检测并去除局部高程图中的动态环境地图构建错误并重新观测，以处理高程图上的移动物。
12、本发明通过雷达-惯性子系统对视觉惯性里程计进行深度配准和初始化，优化了全局因子图。本发明将点云数据转换为高程图，并实现局部高程图的快速更新。增加建图过程中对动态物的处理，可规避多智能体协作时的动态干扰。新的机器人状态以节点的形式添加到因子图中，不仅实现了内存消耗与地图密度之间的平衡，也有利于相对稀疏的因子图用于实时优化。
13、本发明针对月面探测器趋向于多智能体协同作业的应用情景，不仅进行静态环境建图，还考虑其他运动的智能体对建图的影响，本发明增加了建图过程中对动态物处理的方法，防止高程图上的移动物造成动态干扰。本发明采用的相机-惯性系统和雷达-惯性系统既能相互补充，获得更精确的位姿和地形数据，在一个系统失效时，另一个系统也能正常运作，提高了整体系统的鲁棒性。
14、在更具体的技术方案中，步骤s2包括：
15、s21、插值处理系统状态、imu参数，并与图像的关键帧关联，作为视觉-惯性系统的初始化初值；
16、s22、利用贝叶斯树增量平滑方法isam2优化因子图；
17、s23、根据因子图估计imu模块的bias、位姿、速度，据以获取获取关键帧至当前时刻的imu模块的位姿积分结果，以作为匹配预测值；
18、s24、利用相机-惯性子系统，提取并跟踪点线特征，基于滑动窗口模型，联合处理点线特征中的先验信息、点线残差、imu残差、闭环状态，以进行位姿求解，通过相机-惯性子系统、雷达-惯性子系统进行紧耦合处理，得到初步位姿估计数据、周围环境三维点云。
19、本发明采用激光雷达辅助测量，用相机-雷达-惯性测量单元紧耦合来估计位姿和获得点云数据。本发明利用雷达-惯性子系统的数据初始化相机-惯性子系统。本发明相对于现有技术中更侧重于其中一个模块失效时，系统对剩余模块的自动调整，本发明在里程计初始化、深度配准、全局优化等方面均有数据耦合。
20、在更具体的技术方案中，步骤s3包括：
21、s31、在特征追踪数量减少情形下，通过激光雷达测量提取视觉特征深度信息；
22、s32、在imu模块的偏置估计值超过特定阈值时，判定相机-惯性子系统失效，利用雷达-惯性子系统的状态量、imu模块的偏置重新初始化；
23、s33、基于fbow方法进行回环检测，以得到闭环因子，将闭环因子添加至预置因子图，检测出候选时间戳，发送至雷达-惯性系统供判定操作。
24、本发明采用雷达数据对相机图像进行了深度配准，全局优化的因子图中有imu预积分因子、视觉里程计因子、雷达里程计因子以及闭环因子四种约束。本发明改进了传感器数据耦合方式，针对月面作业背景，采用了回环检测方法，提高了检测速度。
25、在更具体的技术方案中，步骤s33包括：
26、s31、快速聚类生成词袋树；
27、s32、建立单词集合；
28、s33、设置单词权重；
29、s34、根据词袋树提取特征和描述子；
30、s35、聚成k类；
31、s36、判断是否达到预设层数，若是，则结束fbow流程；若否，则循环执行前述步骤s34至s35。
32、本发明采用的fbow方法与其他基于词袋模型的检测算法相比，在词袋创建和加载上做了改进，最大的优点是计算迅速，实时性更好。
33、在更具体的技术方案中，因子图的约束包括：imu预积分因子、视觉里程计因子、雷达里程计因子以及闭环因子。
34、在更具体的技术方案中，步骤s5中，将这imu预积分因子、雷达里程计因子、视觉里程因子以及闭环因子，添加至因子图；通过贝叶斯树增量平滑方法isam2优化处理因子图，以消除积累误差，优化全局位姿。
35、本发明优化时选择了imu预积分因子、视觉里程计因子、雷达里程计因子以及闭环因子四种约束，并且在如何选择新的雷达关键帧上，新的机器人状态x会以节点的形式添加到因子图中，通过贝叶斯树增量平滑方法(isam2)优化因子图，消除传感器累积误差，优化全局位姿，提高了建图精度。
36、在更具体的技术方案中，步骤s6包括：
37、s61、在机器人位姿超过特定阈值时，选去新激光雷达关键帧，丢弃位于成对关键帧之间的间歇性帧；
38、s62、将新机器人状态x，以节点形式添加到因子图中，以选取新雷达关键帧。
39、在更具体的技术方案中，步骤s8包括：
40、s81、对高度进行不确定性建模，得到子图坐标系下的高度值；
41、s82、利用预置向量，将三维向量转化为一维值，据以提取高度值，据以求解高度方差信息
42、s83、利用下述逻辑，求偏导得到点云观测值的雅克比矩阵js和雷达旋转矩阵jq：
43、
44、
45、式中，c(φ)是一种用来描述旋转矩阵的映射函数，而×表示斜对称矩阵，φsm是传感器坐标系转到子图坐标系的旋转矩阵，p为由传感器的观测值推算得到的子图坐标系下的高度值，其中，p＝[0,0,1]。
46、s84、利用下述逻辑，处理得到高度方差信息的误差：
47、
48、式中，σs是雷达误差模型的协方差矩阵，σp,q是雷达旋转估计的协方差矩阵；
49、s85、利用下述逻辑，假设目前已存在的高程图高度估计为均值为方差为的高斯分布，即其中，右上标“-”表示更新前，“+”表示更新后，对当前的雷达观测值与已存在的高程图高度估计进行卡尔曼滤波融合操作，得到更新后高度估计：
50、
51、
52、s86、根据探测器的位姿估计数据，更新高程图。
53、本发明选择将三维点云数据转换为2.5维高程栅格地图并进行局部图的实时更新，在该地图中，每个单元存储表示环境中一个小区域的映射点的高度，可以高效的表征环境中的障碍物特征信息，同时其环境内存储的数据量相对较小，具有更小的维护成本与更好的实时性。本发明建图并非为三维点云图，而转换为了实时高程图，减小了月面探测器的计算资源。
54、在更具体的技术方案中，步骤s81包括：
55、s811、设高度的观测模型遵从均值为p，方差为的高斯分布：
56、
57、s812、设在传感器坐标系下，单个观测值记为进行位姿变换，得到子图坐标系下的高度值：
58、
59、式中，p＝[0,0,1]，是传感器坐标系转到子图坐标系的旋转矩阵的逆矩阵，是子图坐标系下的传感器地图坐标系之间的距离信息，w表示世界坐标系，子图坐标系表示为m，s为传感器坐标系。
60、在更具体的技术方案中，月面环境下相机-雷达-惯性耦合建图系统包括：
61、图像姿态数据采集模块，用以将双目相机、激光雷达设置于探测器上，利用双目相机采集获取相机图像，利用激光雷达采集获取深度信息，利用imu模块采集探测器的加速度数据、角速度数据；
62、初步位姿点云获取模块，用以建立并利用雷达-惯性子系统，获得的系统状态、imu参数，以初始化相机-惯性子系统；建立并利用相机-惯性子系统，提取并追踪相机图像中的点线特征，据以处理得到探测器的初步位姿估计数据、周围环境三维点云，初步位姿点云获取模块与图像姿态数据采集模块连接；
63、图像配准及回环检测模块，用以利用深度信息，对相机图像进行深度配准；利用相机-惯性子系统，采用fbow方法进行回环检测，以得到回环关键帧，提取brief描述子并聚类，通过对比词袋向量，比较图像之间相似性；将检测到的回环关键帧发送给至雷达-惯性子系统供判定操作，图像配准及回环检测模块与图像姿态数据采集模连接；
64、因子约束处理模块，用以在雷达-惯性子系统、相机-惯性子系统中，存在失效子系统时，利用未失效子系统进行建图操作；利用未失效子系统初始化失效子系统，据以处理得到因子图约束，因子约束处理模块与图像配准及回环检测模块连接；
65、因子添加模块，用以将因子图约束添加至因子图，因子添加模块与因子约束处理模块连接；
66、机器人状态处理模块，用以获取并添加新机器人状态至因子图，以确定新雷达关键帧，机器人状态处理模块与因子添加模块连接；
67、点云位姿优化及坐标标定模块，用以根据因子图优化初步位姿估计数据、周围环境三维点云，以得到点云输入数据、位姿估计输入数据，根据新雷达关键帧进行雷达坐标系和相机坐标系的标定，获取传感器坐标系，点云位姿优化及坐标标定模块与因子添加模块及初步位姿点云获取模块连接；
68、局部高程图获取模块，用以根据传感器坐标系，处理点云输入数据、位姿估计输入数据，以得到预置维数高程图，据以获取局部高程图，局部高程图获取模块与点云位姿优化及坐标标定模块连接；
69、移动物处理模块，用以采用基于光线追踪方法，检测并去除局部高程图中的动态环境地图构建错误并重新观测，以处理高程图上的移动物，移动物处理模块与局部高程图获取模块连接。
70、本发明主要在多媒态数据的耦合上做出改进。首先针对月面非结构化的环境，传感器效果容易退化的情况，提出了视觉里程计-雷达里程计分层动态耦合的方案，当某个传感器的数据可靠性降低，则降低它在耦合中的权值。当某一传感器彻底失效时，其他传感器仍能正常工作，并且可利用其他传感器数据初始化失效传感器，保证了系统的鲁棒性。同时针对月面有限计算资源，本发明选择了计算量更低的回环检测和局部高程图的构建方法。增加了处理月面多智能体对动态建图的影响。
71、本发明相比现有技术具有以下优点：
72、本发明通过雷达-惯性子系统对视觉惯性里程计进行深度配准和初始化，优化了全局因子图。本发明将点云数据转换为高程图，并实现局部高程图的快速更新。增加建图过程中对动态物的处理，可规避多智能体协作时的动态干扰。新的机器人状态以节点的形式添加到因子图中，不仅实现了内存消耗与地图密度之间的平衡，也有利于相对稀疏的因子图用于实时优化。
73、本发明针对月面探测器趋向于多智能体协同作业的应用情景，不仅进行静态环境建图，还考虑其他运动的智能体对建图的影响，本发明增加了建图过程中对动态物处理的方法，防止高程图上的移动物造成动态干扰。本发明采用的相机-惯性系统和雷达-惯性系统既能相互补充，获得更精确的位姿和地形数据，在一个系统失效时，另一个系统也能正常运作，提高了整体系统的鲁棒性。
74、本发明采用激光雷达辅助测量，用相机-雷达-惯性测量单元紧耦合来估计位姿和获得点云数据。本发明利用雷达-惯性子系统的数据初始化相机-惯性子系统。本发明相对于现有技术中更侧重于其中一个模块失效时，系统对剩余模块的自动调整，本发明在里程计初始化、深度配准、全局优化等方面均有数据耦合。
75、本发明采用雷达数据对相机图像进行了深度配准，全局优化的因子图中有imu预积分因子、视觉里程计因子、雷达里程计因子以及闭环因子四种约束。本发明改进了传感器数据耦合方式，针对月面作业背景，采用了回环检测方法，提高了检测速度。
76、本发明采用的fbow方法与其他基于词袋模型的检测算法相比，在词袋创建和加载上做了改进，最大的优点是计算迅速，实时性更好。
77、本发明优化时选择了imu预积分因子、视觉里程计因子、雷达里程计因子以及闭环因子四种约束，并且在如何选择新的雷达关键帧上，新的机器人状态x会以节点的形式添加到因子图中，通过贝叶斯树增量平滑方法(isam2)优化因子图，消除传感器累积误差，优化全局位姿，提高了建图精度。
78、本发明选择将三维点云数据转换为2.5维高程栅格地图并进行局部图的实时更新，在该地图中，每个单元存储表示环境中一个小区域的映射点的高度，可以高效的表征环境中的障碍物特征信息，同时其环境内存储的数据量相对较小，具有更小的维护成本与更好的实时性。本发明建图并非为三维点云图，而转换为了实时高程图，减小了月面探测器的计算资源。
79、本发明解决了现有技术中存在的回环检测效率低、对月面探测器计算资源消耗较高、在多智能体协作情形下存在动态干扰以及鲁棒性较差的技术问题。
1.月面环境下相机-雷达-惯性耦合建图方法，其特征在于，所述方法包括：
2.根据权利要求1所述的月面环境下相机-雷达-惯性耦合建图方法，其特征在于，所述步骤s2包括：
3.根据权利要求1所述的月面环境下相机-雷达-惯性耦合建图方法，其特征在于，所述步骤s3包括：
4.根据权利要求3所述的月面环境下相机-雷达-惯性耦合建图方法，其特征在于，所述步骤s33包括：
5.根据权利要求3所述的月面环境下相机-雷达-惯性耦合建图方法，其特征在于，所述因子图的约束包括：imu预积分因子、视觉里程计因子、雷达里程计因子以及闭环因子。
6.根据权利要求1所述的月面环境下相机-雷达-惯性耦合建图方法，其特征在于，所述步骤s5中，将这所述imu预积分因子、所述雷达里程计因子、所述视觉里程因子以及所述闭环因子，添加至所述因子图；通过贝叶斯树增量平滑方法isam2优化处理所述因子图，以消除积累误差，优化全局位姿。
7.根据权利要求1所述的月面环境下相机-雷达-惯性耦合建图方法，其特征在于，所述步骤s6包括：
8.根据权利要求1所述的月面环境下相机-雷达-惯性耦合建图方法，其特征在于，所述步骤s8包括：
9.根据权利要求8所述的月面环境下相机-雷达-惯性耦合建图方法，其特征在于，所述步骤s81包括：
10.月面环境下相机-雷达-惯性耦合建图系统，其特征在于，所述系统包括：
