[发明专利]基于激光雷达与双目相机的定位与导航方法及装置

权利要求书

1.基于激光雷达与双目相机的定位与导航方法，其特征在于，包括步骤：

通过双目立体相机获取相机图像，处理来自双目立体相机的图像信息获取位姿；

通过激光雷达获取雷达图像，处理来自激光雷达的图像信息获取位姿；

对来自双目立体相机图像的位姿和来自激光雷达的位姿进行优化融合，获取关于环境的稠密点云模型用于导航；获取关于环境的稀疏点云模型用于精确定位；

双目立体相机的图像位姿获取步骤包括：

一、对左右目相机获取的采集图像分别提取特征点，并完成左右目特征点的匹配，通过左右目外参完成深度计算；

二、利用匀速假设模型和跟踪参考帧模型为视觉特征点在指定帧中寻找匹配点，并用光束平差法计算相机的位姿，得到初步位姿；

三、局部地图跟踪；利用当前帧的共视关系构建共视地图优化初步位姿，给出视觉里程计的位姿估计；

四、在当前帧的局部地图上进行局部光束平差法优化，以调整相机位姿，给出视觉端的位姿估计结果。

2.根据权利要求1所述的基于激光雷达与双目相机的定位与导航方法，其特征在于，激光雷达的图像位姿获取步骤包括：

一、将激光雷达采集到的数据转化为点云，并计算本帧雷达数据中每个点的曲率，完成边缘点和平面点的提取；

二、在上一帧点云数据和当前帧点云数据中寻找对应的边缘点匹配对和平面点匹配对，并计算激光雷达的位姿，给出激光里程计的位姿估计；

三、对稠密地图进行可观测性判断，并构建小规模匹配候选点云；

四、在匹配候选点云中寻找与当前帧点云数据构成边缘点匹配对和平面点匹配对的点，并计算激光雷达的位姿，给出激光雷达端的位姿优化结果。

3.根据权利要求2所述的基于激光雷达与双目相机的定位与导航方法，其特征在于，双目立体相机图像的位姿和来自激光雷达的位姿的优化融合步骤包括：

一、如激光雷达端给出的位姿结果或视觉端给出的位姿结果小于阈值，则直接输出激光雷达端的位姿；

二、如激光雷达端给出的位姿结果或者视觉端给出的位姿结果大于阈值，则将视觉端的位姿结果作为初值在稠密地图中进行可观测性判断，并构建小规模的匹配候选点云；在匹配候选点云中寻找与当前帧点云数据构成边缘点匹配对和平面点匹配对的点，并以视觉端给出的位姿为初值优化新的位姿。

4.根据权利要求1所述的基于激光雷达与双目相机的定位与导航方法，其特征在于：

左右目所提取的特征点为ORB特征；左右目特征点匹配的过程包括：

一、建立从左目到右目的特征点搜索范围对应表；

二、匹配约束包括：邻近尺度约束，匹配范围约束以及特征相似度约束；

三、得到初步的匹配结果之后，在匹配点附近用二次多项式来进行拟合，最终得到亚像素的匹配结果。

5.根据权利要求1所述的基于激光雷达与双目相机的定位与导航方法，其特征在于：

跟踪参考帧模型是使用上一个关键帧作为参考帧，使用BoW描述向量的反向查找加速和参考帧中特征的匹配速度；

判断使用匀速假设模型和跟踪参考帧模型的依据为：默认使用匀速假设模型，当匀速假设模型跟踪失败或者没有初速度的时候使用跟踪参考帧模型；对于BA优化初值设定，匀速假设模型使用上一帧的位姿经过匀速运动得到，跟踪参考帧模型直接使用上一帧的位姿；

共视地图的构建主要包括两个部分：局部共视特征点、局部共视关键帧；对于局部共视关键帧的选择依据为：与当前帧上任一特征点存在共视关系的关键帧，对于局部共视特征点的选择依据为：所有局部共视关键帧上的所有特征点；

局部共视关键帧包括两种类型：直接相连关键帧，关联关键帧；直接相连关键帧表示与当前帧直接关联的关键帧和当前帧，关联关键帧表示与当前帧通过特征点间接关联的关键帧；在进行局部BA优化时，观测信息使用局部共视特征点，固定关联关键帧的位姿，优化直接相连关键帧的位姿。

6.根据权利要求2所述的基于激光雷达与双目相机的定位与导航方法，其特征在于：

边缘点和平面点的选择步骤如下：

a、对激光雷达的每一条扫描线进行分段；

b、从每个分段中找到曲率很大的点做为非常尖锐的边缘点，曲率较大的点曲率的点做为尖锐的边缘点；其中曲率很大的点指的是此分段曲率最大的两个点，曲率较大的点曲率的点指的是此分段曲率最大的二十个点；

c、从每个分段中找到曲率很小的点做为非常平坦的平面点，将其它未分类的点全部作为平坦的平面点；其中曲率很小的点指的是此分段曲率最小的四个点；

d、为了防止特征点聚集将非常尖锐的边缘点和尖锐的边缘点以及非常平坦的平面点的附近点都进行剔除。