基于地面移动机器人的视觉惯性同时定位与建图导航方法

1.一种基于地面移动机器人的视觉惯性同时定位与建图导航方法，其特征在于，包括：对地面移动机器人的相机参数和IMU参数进行联合标定，获得联合优化位姿，通过IMU偏置标定和固定尺度因子进行IMU初始化，得到关键帧；采用基于共视图的智能关键帧选择策略，通过自适应阈值调整机制进行共视帧筛选，创建点云地图；对点云地图进行栅格化，得到二进制地图；通过多级坐标系转换体系发布全局位姿，结合二进制地图完成导航地图发布；其中，IMU偏置标定包括：利用ORB-SLAM3中的惯性优化框架，对IMU传感器的系统误差进行建模和估计；获取IMU在静止或准静止状态下的数据，建立IMU陀螺仪偏置 和加速度计偏置 的数学模型；对数学模型进行优化，通过最小化重投影误差和IMU预积分误差的联合代价函数，获得IMU陀螺仪偏置估计值和加速度计偏置估计值。 2.根据权利要求1所述的基于地面移动机器人的视觉惯性同时定位与建图导航方法，其特征在于，对地面移动机器人的相机参数和IMU参数进行联合标定，获得联合优化位姿，包括：将相机与IMU固定至地面移动机器人平台上，执行相机IMU联合标定，获得相机与IMU之间的位置关系以及相机与地面移动机器人之间的位置关系。 3.根据权利要求1所述的基于地面移动机器人的视觉惯性同时定位与建图导航方法，其特征在于，IMU初始化包括：将已完成标定的IMU陀螺仪偏置 和加速度计偏置 进行固定，确定固定尺度因子；利用连续关键帧之间累积的IMU预积分观测数据，执行对重力加速度矢量方向的估计与对齐。 4.根据权利要求3所述的基于地面移动机器人的视觉惯性同时定位与建图导航方法，其特征在于，利用连续关键帧之间累积的IMU预积分观测数据，执行对重力加速度矢量方向的估计与对齐，包括：利用连续N个关键帧之间的IMU预积分信息，计算表征重力加速度方向的向量 ，具体公式为：其中 表示在第i-1个关键帧时刻，从IMU本体坐标系到ORB坐标系的旋转矩阵，旋转矩阵由视觉惯性里程计VIO前端跟踪或IMU积分提供， 表示关键帧数量， 表示IMU预积分计算得到的关键帧时间段内的速度增量；对表征重力加速度方向的向量 进行归一化操作，获得单位长度的重力方向向量 ：其中， 为范数操作；定义世界坐标系的参考重力方向为 ，分别计算旋转轴 以及旋转角度 ，构建旋转矩阵 ：其中， 是3x3单位矩阵， 是由旋转轴 构成的斜对称矩阵。 5.根据权利要求1所述的基于地面移动机器人的视觉惯性同时定位与建图导航方法，其特征在于，采用基于共视图的智能关键帧选择策略，通过自适应阈值调整机制进行共视帧筛选，包括：对于当前关键帧 ，从共视图中提取与当前关键帧 具有共同观测的连接关键帧 ，记录与当前关键帧 的共视点数量作为共视权重 ，根据共视权重 进行排序；设定最小阈值 ，若共视权重 满足 ，则确定连接关键帧 与当前关键帧 具有显著的观测重叠，跳过当前关键帧 的RGB-D数据传输，由每个满足条件的关键帧连接关键帧 将初始的最小阈值 递减预设值，进行自适应阈值调整；否则，将当前关键帧 标记为点云创建帧。 6.根据权利要求5所述的基于地面移动机器人的视觉惯性同时定位与建图导航方法，其特征在于，创建点云地图包括：提取点云创建帧的RGBD信息，所述RGBD信息包括彩色图像和深色图像；根据在RGB-D相机中深度图像的像素值代表相机坐标系下z轴方向的距离，设 为相机坐标系下的空间点，与像素坐标系下点P的坐标关系为：其中 为像素坐标，K为相机内参矩阵， 为相机内参， 为深度图像中的深度值；设 为深度图像中的深度值，则实际空间中坐标为：对于每个关键帧，相对于相机坐标系的空间点 的旋转为 ，平移为 ，相机坐标系相对于世界坐标系的旋转为 ，平移为 ，则点 在世界坐标系下的坐标为：依次将多个关键帧的点云数据融合成全局点云，并采用体素滤波进行点云过滤，根据x、y、z坐标进行体素划分，保留体素中最多的标签数据，形成点云地图。 7.根据权利要求1所述的基于地面移动机器人的视觉惯性同时定位与建图导航方法，其特征在于，通过多级坐标系转换体系发布全局位姿，结合二进制地图完成导航地图发布，包括：将相机在ORB坐标系下的位姿转换至世界坐标系，进行坐标轴重新映射，世界坐标系相对于ORB坐标系的第一变换矩阵为 ：利用第一变换矩阵 对相机旋转矩阵 和相机平移向量 进行转换：其中， 和 为世界坐标系到相机坐标系的变换 中的旋转矩阵和平移向量；进行相机位姿逆变换计算，将从世界坐标系到相机坐标系的变换 转换为从相机坐标系到世界坐标系的变换 ：其中， 和 为相机坐标系到世界坐标系的变换 中的旋转矩阵和平移向量；将世界坐标系下的相机位姿转换为世界坐标系下的机器人位姿，进行相机坐标系到机器人坐标系的转换，相机坐标系相对于机器人基坐标系base_link的第二变换矩阵为 ：最终的机器人基坐标系相对于世界坐标系的转换为：其中， 和 为机器人基坐标系相对于世界坐标系的变换 中的旋转矩阵和平移向量；将地图坐标系map固定为世界坐标系world，并从ROS中实时订阅机器人编码器计算的 ，则基于坐标变换的链式法则，构建odom至map的转换关系：其中，map为地图坐标系，odom为里程计坐标系，base_link为机器人基坐标系，camera_link为相机坐标系， 和 为里程计坐标系相对于地图坐标系的变换 中的旋转矩阵和平移向量；通过TF广播机制，持续发布map到odom的坐标变换，建立故障检测与恢复机制，当坐标系转换异常时，触发异常处理机制，采用里程计数据维持导航功能并重新初始化SLAM系统。 8.一种电子设备，包括存储器、处理器及存储在所述存储器上并可在所述处理器上运行的计算机程序，其特征在于，所述处理器执行所述程序时实现如权利要求1至6任一项所述基于地面移动机器人的视觉惯性同时定位与建图导航方法。 9.一种非暂态计算机可读存储介质，其上存储有计算机程序，其特征在于，所述计算机程序被处理器执行时实现如权利要求1至6任一项所述基于地面移动机器人的视觉惯性同时定位与建图导航方法。