一种适用于月球车的SLAM方法、电子设备及存储介质技术

本发明专利技术公开了一种适用于月球车的SLAM方法、电子设备及存储介质，所述方法包括：步骤S1、对观测到的特征点建立局部子地图；步骤S2、当观测到初始的特征点时，将初始的所述特征点存储在所述局部子地图中；步骤S3、当观测到新的所述特征点时，将新的特征点在局部子地图中进行存储与更新；步骤S4、当所述局部子地图中的特征点数目达到预设阈值时，将所述局部子地图与全局地图进行融合。本发明专利技术能够有效降低SLAM算法的计算复杂度，提高计算速度和估计一致性。

【技术实现步骤摘要】
一种适用于月球车的SLAM方法、电子设备及存储介质
本专利技术涉及计算机视觉
，特别涉及一种适用于月球车的SLAM方法、电子设备及存储介质。
技术介绍
视觉导航SLAM技术一直是近些年移动机器人导航定位中非常重要的研究内容。在月球车巡视探测过程中，导航系统是月球车系统中重要的组成部分。月球执行任务的区域是一个复杂的环境，月球车要进行自主定位，明确自身的位姿信息以及周围环境的地形和障碍信息，才能实现自主导航、自主避障、路径规划，确保月球车在未知区域中有效且安全地执行这些任务。SLAM是在没有先验信息的陌生环境中，目标通过携带传感器进行自身定位的同时对周围环境进行重建的过程，非常适合月球车自主导航任务。考虑月球车的机载计算能力与工程实际，如何降低月球车SLAM算法计算复杂度也是一个重要问题。近些年对SLAM算法计算复杂度问题的研究度不高，现有技术中没有能够快速降低计算复杂度的方法，并且降低计算复杂度的同时提高估计的一致性。
技术实现思路
本专利技术的目的是提供一种适用于月球车的SLAM方法、电子设备及存储介质，实现有效降低SLAM算法的计算复杂度，提高计算速度和估计一致性的目的。为了实现以上目的，本专利技术通过以下技术方案实现：一种适用于月球车的SLAM方法，包括：步骤S1、对观测到的特征点建立局部子地图；步骤S2、当观测到初始的特征点时，将初始的所述特征点存储在所述局部子地图中；步骤S3、当观测到新的所述特征点时，将新的特征点在局部子地图中进行存储与更新；步骤S4、当所述局部子地图中的特征点数目达到预设阈值时，将所述局部子地图与全局地图进行融合。优选地，所述局部子地图为某一时刻以月球车当前的位置为参考中心初始化的一个相对独立于全局地图的局部子地图。优选地，所述步骤S3还包括:将所述新的特征点的更新建立在月球车的本体坐标系下，并将月球车的位姿改变量由惯性测量元件测量后作为一个单独的特征点加入到状态变量中，与所述新的特征点一并在所述月球车的本体坐标系下进行更新。优选地，所述步骤S3还包括：系统状态预测值如下：式中，表示k-1时刻特征点在月球车的本体坐标系下的估计值；表示从k-1时刻到k时刻月球车位移量在月球车的本体坐标系下的估计值；协方差预测值RP(k|k-1)如下：式中，表示k-1时刻月球车的本体坐标系下状态变量协方差矩阵；QIMU表示IMU噪声矩阵；观测量的估计值如下：式中，h表示观测估计值方程；表示第i个特征点在月球车的本体坐标系下的位置估计值；表示从k-1时刻到k时刻月球车位姿改变量在月球车的本体坐标系下的估计值；观测量的残差r(k)如下：式中，z(k)表示k时刻实际观测值；观测量的残差的协方差矩阵S(k)如下：式中，Qν(k)是k时刻观测过程的噪声协方差矩阵，是k时刻观测模型雅克比矩阵；P(k|k-1)表示从k-1时刻到k时刻状态变量协方差矩阵；当观测到新的特征点时，状态量增广后的协方差矩阵RP(k)为：式中，RP(k|k-1)表示从k-1时刻到k时刻状态变量在月球车的本体坐标系下的协方差矩阵；所述特征点和全局坐标系在月球车的本体坐标系下的位置通过所述月球车的位姿改变量进行更新：式中，表示k时刻绝对坐标系在月球车的本体坐标系下的状态量；表示k-1时刻绝对坐标系在月球车的本体坐标系下的状态量；表示k时刻特征点在月球车的本体坐标系下的状态量；表示k-1时刻特征点在月球车的本体坐标系下的状态量；所述协方差矩阵的更新过程如下：式中，RPBB(k)表示在k时刻绝对坐标系在月球车的本体坐标系下的非观测状态量协方差，R表示月球车的本体坐标系，BB表示绝对坐标系；RPii(k)表示在k时刻月球车的本体坐标系下特征点协方差，ii表示特征点；表示月球车的本体坐标系下从k-1时刻到k时刻状态变量协方差矩阵，表示月球车位姿变化量。优选地，所述步骤S4包括：建立所述局部子地图所述与全局地图之间的约束关系，进行全局一致性估计；局部子地图与全局地图的转换表示如下：式中，表示k时刻全局坐标系下状态变量；表示k时刻局部地图与全局地图的转换矩阵；表示k时刻局部坐标系下状态变量；GPlS(k)表示k时刻全局坐标系下状态变量协方差，G表示全局坐标系，lS表示局部子地图系，用以单独突出局部子地图算法的局部子地图；表示k时刻转换矩阵对状态变量的Jacobian矩阵，S表示局部坐标系；PlS(k)表示k时刻局部坐标系下状态变量协方差；不同坐标系之间状态变量利用交叉投影关系进行约束，建立局部坐标系中特征点与全局坐标系中特征点的联系；相同状态变量的投影约束可以表示为：式中，GXi(k)表示k时刻该特征点在全局坐标系下的坐标，G表示全局坐标系，SXi(k)表示k时刻该特征点在局部坐标系下的坐标，GXS(k)表示k时刻局部坐标系在全局坐标系下的位置和姿态矢量；相同状态变量的投影约束展开后表示如下：式中，I表示单位矩阵；SXR(k)表示k时刻月球车在局部子地图中位姿。另一方面，本专利技术还提供一种电子设备，包括处理器和存储器，所述存储器上存储有计算机程序，所述计算机程序被所述处理器执行时，实现如上文所述的方法。再一方面，本专利技术还提供一种可读存储介质，所述可读存储介质内存储有计算机程序，所述计算机程序被处理器执行时，实现如上文所述的方法。本专利技术至少具有以下优点之一：本专利技术考虑月球表面的复杂环境、月球车的机载计算能力与工程实际，提出了一种基于局部子地图的具有低计算复杂度与高估计一致性的适用于月球车的SLAM方法。随着月球车巡视探测进程的增加，观测到的特征点数目逐渐增多，状态变量维度急剧上升，基于EKF框架的SLAM过程计算复杂度不断上升。针对此问题，本专利技术在对特征点观测过程中建立局部子地图，将观测的特征点在局部子地图中进行存储与更新，并阶段性地通过一致性约束与全局地图融合，保持局部子地图与全局地图的相对独立性。此外，将特征点的更新建立在月球车本体坐标系下，观测方程线性化过程中不会将车体位姿不确定性和特征点位置不确定性引入观测方程，通过降低观测过程不确定性，减少误差源的引入与传播，并将月球车位姿改变量作为一个单独的状态变量加入更新过程，最后对全局坐标系到月球车的本体坐标系的位置进行更新。最终，能够有效减少月球车SLAM算法中单次滤波过程中系统状态变量包含的特征点个数和最终特征点总数，减少误差源引入，降低算法的计算复杂度，提高计算速度与估计一致性。附图说明图1为月球车运动坐标系示意图；图2a和图2b分别为双目相机安装模型的俯视图和侧视图；图3为本专利技术一实施例提供的适用于月球车的SLAM方法的流程图；图4为标准E本文档来自技高网...

【技术保护点】
1.一种适用于月球车的SLAM方法，其特征在于，包括：/n步骤S1、对观测到的特征点建立局部子地图；/n步骤S2、当观测到初始的特征点时，将初始的所述特征点存储在所述局部子地图中；/n步骤S3、当观测到新的所述特征点时，将新的特征点在局部子地图中进行存储与更新；/n步骤S4、当所述局部子地图中的特征点数目达到预设阈值时，将所述局部子地图与全局地图进行融合。/n

【技术特征摘要】
1.一种适用于月球车的SLAM方法，其特征在于，包括：
步骤S1、对观测到的特征点建立局部子地图；
步骤S2、当观测到初始的特征点时，将初始的所述特征点存储在所述局部子地图中；
步骤S3、当观测到新的所述特征点时，将新的特征点在局部子地图中进行存储与更新；
步骤S4、当所述局部子地图中的特征点数目达到预设阈值时，将所述局部子地图与全局地图进行融合。


2.如权利要求1所述的适用于月球车的SLAM方法，其特征在于，所述局部子地图为某一时刻以月球车当前的位置为参考中心初始化的一个相对独立于全局地图的局部子地图。


3.如权利要求2所述的适用于月球车的SLAM方法，其特征在于，所述步骤S3还包括:
将所述新的特征点的更新建立在月球车的本体坐标系下，并将月球车的位姿改变量由惯性测量元件测量后作为一个单独的特征点加入到状态变量中，与所述新的特征点一并在所述月球车的本体坐标系下进行更新。


4.如权利要求3适用于月球车的SLAM方法，其特征在于，所述步骤S3还包括：
系统状态预测值如下：



式中，表示k-1时刻特征点在月球车的本体坐标系下的估计值；表示从k-1时刻到k时刻月球车位移量在月球车的本体坐标系下的估计值；
协方差预测值RP(k|k-1)如下：



式中，表示k-1时刻月球车的本体坐标系下状态变量协方差矩阵；QIMU表示IMU噪声矩阵；
观测量的估计值如下：



式中，h表示观测估计值方程；表示第i个特征点在月球车的本体坐标系下的位置估计值；表示从k-1时刻到k时刻月球车位姿改变量在月球车的本体坐标系下的估计值；
观测量的残差r(k)如下：



式中，z(k)表示k时刻实际观测值；
观测量的残差的协方差矩阵S(k)如下：



式中，Qν(k)是k时刻观测过程的噪声协方差矩阵，是k时刻观测模型雅克比矩阵；P(k|k-1)表示从k-1时刻到k时刻状态变量协方差矩阵；
当观测到新的特征点时，状态量增广后的协方差矩阵RP(k)为：



式中，RP(k|k-1)表示从k-1时刻到k时刻状态变量在月球车的本体坐标系下的协方差矩阵；
所述特征点和全局坐标系在月球车的本体坐标系下的位置通过所述月球车的位姿改变量进行更新：

【专利技术属性】
技术研发人员：贺亮，郑博，曹涛，韩宇，李木子，胡涛，
申请(专利权)人：上海航天控制技术研究所，
类型：发明
国别省市：上海;31