[发明专利]基于视觉惯导融合的移动机器人地图构建方法及相关设备

权利要求书

1.基于视觉惯导融合的移动机器人地图构建方法，其特征在于，所述方法包括：

步骤S1，根据视觉姿态和惯性姿态融合状态确定融合达成度，完成视觉初始化、视觉惯性校准对齐以及惯性初始化；

步骤S2，构建情形评估函数实时分析姿态趋势；

步骤S3，确定融合状态局部期望值并输出该状态：通过评估函数值与阈值比较判断此时机器人的运动状态观测数据是否具有高确定性，同时增加时间阈值设定双重约束评估状态以确定是否终止视觉惯性联合初始化并输出当前状态；

步骤S4，根据公共地图点稀疏程度将关键帧分为强共视关键帧和弱共视关键帧；

步骤S5，根据分级结果将强共视关键帧送入滑动窗口并利用帧间位姿密集连续性优化强共视关键帧，借助观测地图点的联系对自感知地图进行绘制和校正。

2.根据权利要求1所述的基于视觉惯导融合的移动机器人地图构建方法，其特征在于，所述步骤S1中，IMU测量值由真实值、随机游走偏差和白噪声三个部分组成，加速度计仍受到重力加速度的影响，将IMU运动状态变量利用中值法离散化以方便相机跟IMU进行状态融合，离散化的运动状态变量算式如下：

p_pre、v_pre和q_pre分别代表IMU位置、速度和姿态四元数的初始值；和分别代表第i和i+1个时刻下IMU在世界坐标系下的加速度；和分别代表第i和i+1个时刻下IMU在世界坐标系下的角速度；为内积的运算法则；和表示在第i和i+1个时刻的时间段里IMU加速度偏差和角速度偏差的变化量；是从IMU坐标系转移到世界坐标系下的旋转矩阵，Δt为相邻时刻的间隔时间；

上述运动状态变量算式能计算出在i+1个时刻下的计算结果为z_i+1，z₁为初始值，在第一次计算时直接利用相机采集的信息确定，之后的计算利用IMU上一时刻的信息计算得到当前时刻的观测数据。

3.根据权利要求2所述的基于视觉惯导融合的移动机器人地图构建方法，其特征在于，所述步骤S2中，用Z_F表示机器人的运动状态观测数据，则存在Z_F＝{z₁,z₂,...,z_n}，z_i为Z_F中具体的第i个时刻下的观测数据；若机器人的运动状态观测数据Z_F＝{z₁,z₂,...,z_n}服从概率分布f(Z_F；Y)，其中Y是需要求解的机器人状态变量目标函数，得到的状态估计函数如下式：

构建机器人状态变量目标函数Y来度量机器人观测状态数据并估计观测的位姿随时间变化的趋向的置信度，机器人运动状态置信度表达如下式：

其中Ω(Z_F；Y)为机器人运动状态观测数据Z_F的置信度，E(·)为计算机器人运动状态观测数据Z_F的期望，S(·)为计算机器人运动状态观测数据Z_F的标准差，Var(·)为计算机器人运动状态观测数据Z_F的方差，通过机器人运动状态置信度确定融合达成度。

4.根据权利要求3所述的基于视觉惯导融合的移动机器人地图构建方法，其特征在于，所述步骤S2中，定义视觉惯性观测样本情形评估函数如下式：

其中，f_λ(Z_F)为机器人运动状态观测数据情形评估函数，Ω(Z_F；Y)是从关于评估机器人运动状态观测数据Z_F的位置、速度、旋转四元数和偏差这些相关状态获得的信息矩阵置信度，为机器人运动状态中某个关键帧F观测数据Z_F对应的协方差矩阵，F为运行过程中某个图像帧，Δτ是与图像帧相关的图像信息矩阵。