[发明专利]一种动态场景下的激光单目视觉融合定位建图方法

权利要求书

1.一种动态场景下的激光单目视觉融合定位建图方法，其特征在于，包括：

步骤1，根据当前帧点云数据与上一帧视觉里程计预测到的位姿先验信息，以检测动态障碍物信息，并输出；

步骤2，根据步骤1输出的动态障碍物信息在相互标定外参的单目视觉上形成的图像掩膜，在图像掩膜上提取ORB特征点，并将提取的所述ORB特征点与上一帧ORB特征点匹配，再估计ORB特征点的深度，通过位姿解算获取相对位姿，并输出满足要求的关键帧信息；

步骤3，将步骤2输出的关键帧插入到共视图中，根据地图点共视程度更新该关键帧与其它关键帧的连接关系、以及生长树和词袋模型，生成新地图点，并根据当前关键帧的本质图找到相邻关键帧，构建非线性优化问题，优化关键帧的位姿和地图点；步骤3具体包括：

步骤31，将步骤2输出的关键帧信息插入到共视图中，并根据地图点共视程度更新此关键帧与其它关键帧的连接关系，同时更新生长树和词袋模型；

步骤32，判断当前关键帧与上一关键帧产生的平移是否满足三角化条件，若是，则进入步骤33；否则，则进入步骤34；

步骤33，将能在至少3帧中跟踪到的ORB特征点进行融合，并根据三角化生成新地图点；

步骤34，局部BA优化地图：根据当前关键帧的本质图，找到其它相邻关键帧及地图点，构建非线性优化问题(10)，获得优化位姿ξ^*和地图点

其中，为像素坐标的齐次坐标形式，即K为单目视觉内参矩阵，为地图点坐标的齐次坐标形式，即z_w为地图点深度；ξ为单目视觉位姿的李代数；ξ^*为单目视觉位姿的李代数的最优估计值，为地图点坐标的齐次坐标形式的最优估计值，ξ^{^}为ξ的反对称矩阵，n表示成功匹配且生成地图点的ORB特征点总数目，下标w表示第w个ORB特征点；

步骤35，判断步骤34优化后的关键帧超过设定数量的ORB特征点是否能被至少其它三个关键帧观测到，若是，则判定当前关键帧为冗余关键帧，从共视图中删除冗余关键帧，同时更新生长树和词袋模型，并删除属于该关键帧的地图点，随后在局部地图中删除步骤31的冗余关键帧；

步骤4，判断每一关键帧的图像数据与当前关键帧的图像数据的相似度是否达到阈值，如果是，则判定为出现回环，替换或填补当前关键帧与回环关键帧存在冲突的地图点，再在本质图上将当前关键帧与回环关键帧连接，并更新本质图，最后进行全局BA，获得优化后的关键帧位姿、特征点地图和点云地图。

2.如权利要求1所述的动态场景下的激光单目视觉融合定位建图方法，其特征在于，步骤1具体包括：

步骤11，将激光雷达输出的点云数据进行栅格化处理，再根据地面点云的分布特征进行地面分割操作，获取非地面点云；

步骤12，将步骤11输出的所有非地面点云聚类为多个点云簇，一个点云簇对应一个障碍物，以区分出各自独立的障碍物点云簇；

步骤13，估计步骤12获得的每个障碍物的位置、形状和尺寸信息；

步骤14，根据多帧步骤13估计得到的每个障碍物的差异程度计算差异度函数，建立关联矩阵，将多帧点云数据中障碍物关联起来；

步骤15，根据步骤14关联的障碍物情况，以及上一帧视觉里程计计算得到的两帧点云数据相对运动，判断障碍物运动状态，区分出动态障碍物和静态障碍物。