[发明专利]一种非结构化环境四足机器人立体视觉定位建图方法

权利要求书

1.一种非结构化环境四足机器人立体视觉定位建图方法，其特征在于，包括

步骤S1：对相机和IMU内方位参数进行互标定，包括左右视对标、相机与惯导标定，充分融合IMU与双目相机之间的信息；

步骤S2：使用双目相机两帧间的IMU信息预测上一帧的像素点在当前帧的位置，利用最小化光度误差得到两帧之间的像素跟踪信息，提取成功跟踪的特征组合，筛选局部地图特征点，完成当前帧像素与局部地图点对应像素的匹配；

步骤S3：特征提取线程负责计算关键帧队列中当前关键帧左、右图像的ORB局部图像特征描述算子；

步骤S4：局部建图线程等待当前关键帧的特征提取完成后，进行双目立体像对匹配，融合通过最小化光度误差得到的像素与通过特征提取得到的像素位置，并筛选当前关键帧的冗余地图特征点，根据双目匹配的结果来创建新的地图特征点；

步骤S5：利用位姿图优化减小该周期内四足机器人运动漂移，进行回环检测。

2.如权利要求1所述的非结构化环境四足机器人立体视觉定位建图方法，其特征在于，所述步骤S1中标定过程如下：利用imu_utils库进行IMU内参标定，校准IMU的零漂白噪声与随机游走误差，对双目相机进行标定得到yaml，并且与IMU标定文件一起进行联合标定。

3.如权利要求1所述的非结构化环境四足机器人立体视觉定位建图方法，其特征在于，所述步骤S2中通过最小化重投影误差和IMU预积分的误差得到当前帧的位姿估计，并判断、插入关键帧。

4.如权利要求1所述的非结构化环境四足机器人立体视觉定位建图方法，其特征在于，所述步骤S3中，将观察到匹配地图点集合中地图点的关键帧设置为局部关键帧；局部关键帧观察到的地图点设置为局部地图点。

5.如权利要求1所述的非结构化环境四足机器人立体视觉定位建图方法，其特征在于，所述步骤S4中的当关键帧队列中所有的关键帧处理完毕后，进行局部地图的光束法平差优化，从而得到定位和建图结果，最后对候选关键帧进行筛选。

6.如权利要求1所述的非结构化环境四足机器人立体视觉定位建图方法，其特征在于，所述步骤S5中，通过最小化重投影误差和IMU项误差得到全局一致性更好的环境地图和四足机器人的运动轨迹，该项工作是由Ceres优化工具包完成。

7.如权利要求6所述的非结构化环境四足机器人立体视觉定位建图方法，其特征在于，前部帧的误差会不断累计，后程位姿估计的误差不断增大；通过回环检测方法修正前部帧累计的误差，修正的步骤具体包括：

步骤S51：组建特征点数据库中三个同名特征点的三角形；

步骤S52：构建每个特征点对应的仿射不变量；

步骤S53：建立仿射不变量距离判断矩阵和建立集合相似度。

8.一种计算机可读存储介质，其上存储有计算机程序，其特征在于，所述计算机程序被处理器执行时，实现如权利要求1-7中任一项所述的非结构化环境四足机器人立体视觉定位建图方法。

9.一种电子设备，其特征在于，包括处理器和存储器，所述存储器上存储有计算机程序，所述计算机程序被所述处理器执行时，实现如权利要求1-7中任一项所述的非结构化环境四足机器人立体视觉定位建图方法。