基于激光雷达与双目相机的定位与导航方法及装置制造方法及图纸

本发明专利技术涉及基于激光雷达与双目相机的定位与导航方法，通过双目立体相机获取相机图像，处理来自双目立体相机的图像信息获取位姿；通过激光雷达获取雷达图像，处理来自激光雷达的图像信息获取位姿；对两种位姿进行优化融合，获取关于环境的稠密点云模型和稀疏点云模型分别用于导航和精确定位；基于激光雷达与双目相机的定位与导航装置，包括图像模块、激光雷达模块、位姿优化模块、双目立体相机、激光雷达。本申请采用用于对来自双目立体相机的位姿和来自激光雷达的位姿进行优化融合的位姿优化模块，将双目立体相机和多线激光雷达进行融合获得了高精度的位姿估计，以及高精度的稠密点云地图，其可直接用于无人车的导航和定位。

【技术实现步骤摘要】
基于激光雷达与双目相机的定位与导航方法及装置
本专利技术属于无人小车的定位方法
，具体涉及基于激光雷达与双目相机的定位与导航方法及装置。
技术介绍
随着传感器种类的多元化和计算能力的增强，各型专用机器人的适用范围变得更加广泛。为了适应广泛多变的环境，单一的传感器在探测距离和探测精度已经成为了专用机器人更大的瓶颈。多传感器的融合能够能够提升机器人在复杂场景下的探测精度。为了解决以上问题我方研发出了一种基于激光雷达与双目相机的定位与导航方法及装置。
技术实现思路
本专利技术的目的就在于为了解决上述问题而提供基于激光雷达与双目相机的定位与导航方法及装置。本专利技术通过以下技术方案来实现上述目的：基于激光雷达与双目相机的定位与导航方法，包括步骤：通过双目立体相机获取相机图像，处理来自双目立体相机的图像信息获取位姿；通过激光雷达获取雷达图像，处理来自激光雷达的图像信息获取位姿；对来自双目立体相机图像的位姿和来自激光雷达的位姿进行优化融合，获取关于环境的稠密点云模型和稀疏点云模型分别用于导航和精确定位。基于激光雷达与双目相机的定位与导航装置，包括：用于处理双目立体相机采集的图像的图像模块；用于处理激光雷达采集的数据的激光雷达模块；用于对来自双目立体相机的位姿和来自激光雷达的位姿进行优化融合的位姿优化模块；用于获取相机数据的双目立体相机；用于获取雷达数据的激光雷达。本专利技术的有益效果在于：本专利技术的一种基于激光雷达与双目相机的定位与导航方法及装置，1、本申请采用用于对来自双目立体相机的位姿和来自激光雷达的位姿进行优化融合的位姿优化模块，将双目立体相机和多线激光雷达进行融合获得了高精度的位姿估计，以及高精度的稠密点云地图，其可直接用于无人车的导航和定位；2、使用保留了视觉稀疏点云，其保留了特征描述子，配合BoW可实现与地图的快速匹配；3、本申请在位姿融合过程中综合考虑了双目立体相机和多线激光雷达的优缺点，使得精度更高。附图说明图1为本专利技术基于激光雷达与双目相机的定位与导航方法的框图；图2为本专利技术基于激光雷达与双目相机的定位与导航方法及装置的结构示意图。图中：1、无人小车；2、双目相机；3、激光雷达。具体实施方式下面结合附图对本专利技术作进一步说明：如图1所示：基于激光雷达3与双目相机2的定位与导航方法，包括步骤：通过双目立体相机获取相机图像，处理来自双目立体相机的图像信息获取位姿；通过激光雷达3获取雷达图像，处理来自激光雷达3的图像信息获取位姿；对来自双目立体相机图像的位姿和来自激光雷达3的位姿进行优化融合，获取关于环境的稠密点云模型和稀疏点云模型分别用于导航和精确定位。双目立体相机的图像位姿获取步骤包括：一、对左右目相机获取的采集图像分别提取特征点，并完成左右目特征点的匹配，通过左右目外参完成深度计算；其中，左右目所提取的特征为ORB特征。左右目特征点匹配的过程包括：1)：建立从左目到右目的特征点搜索范围对应表。2)：匹配约束包括：邻近尺度约束，匹配范围约束以及特征相似度约束。3)：得到初步的匹配结果之后，在匹配点附近用二次多项式来进行拟合，最终得到亚像素的匹配结果。这里使用的多项式如下：y＝ax2+bx+c二、利用匀速假设模型和跟踪参考帧模型为视觉特征点在指定帧中寻找匹配点，并用光束平差法计算相机的位姿，得到初步位姿；其中，跟踪参考帧模型是使用上一个关键帧作为参考帧，使用BoW描述向量的反向查找加速和参考帧中特征的匹配速度。其中匀速假设模型是指假设允许运动，使用上一帧的位姿通过匀速运动得到当前帧的位姿，将当前帧的点投影至上一帧，再在投影点附近匹配以加速匹配过程。判断使用匀速假设模型和跟踪参考帧模型的依据为：默认使用匀速模型，当匀速模型跟踪失败或者没有初速度的时候使用跟踪参考帧模型。对于BA优化初值设定，匀速假设模型使用上一帧的位姿经过匀速运动得到，跟踪参考帧模型直接使用上一帧的位姿；三、局部地图跟踪；利用当前帧的共视关系构建共视地图优化初步位姿，给出视觉里程计的位姿估计；其中，共视地图的构建主要包括两个部分：局部特征点、局部关键帧。对于局部共视关键帧的选择依据与当前帧上任一特征点存在共视关系的关键帧，对于局部共视特征点的选择依据为：所有局部共视关键帧上的所有特征点。四、在当前帧的局部地图上中进行局部光束平差法优化，以调整相机位姿，给出视觉端的位姿估计结果。其中，局部关键帧包括两种类型：直接相连关键帧，关联关键帧。直接相连关键帧表示与当前帧直接关联的关键帧和当前帧，关联关键帧表示与当前帧通过特征点间接关联的关键帧。局部特征点为所有直接相连关键帧上的所有特征的合集。其中在进行局部BA优化时，观测信息使用局部特征点，固定关联关键帧的位姿，优化直接相连关键帧的位姿。激光雷达3的图像位姿获取步骤包括：一、将激光雷达3采集到的数据转化为点云，并计算本帧雷达数据中每个点的曲率，完成边缘点和平面点的提取；其中，曲率的表示为当前点与前后五个点的方差diff＝point[cur-5]+point[cur-4]+point[cur-3]+point[cur-2]+point[cur-1]-10point[cur]+point[cur+1]+point[cur+2]+point[cur+3]+point[cur+4]+point[cur+5]curvature＝diff.x*diff.x+diff.y*diff.y+diff.z*diff.z其中，边缘点和平面点的选择步骤如下：1)：对激光雷达3的每一条扫描线进行分段；2)：从每个分段中找到曲率很大(此分段曲率最大的两个)的点做为非常尖锐点的边缘点，曲率较大的点曲率(此分段曲率最大的二十个)的点做为尖锐点的边缘点；3)：从每个分段中找到曲率很小(此分段曲率最小的四个)的点做为非常平坦的平面点，将其它为分类的点全部做为平坦的平面点；4)：为了防止特征点聚集将非常尖锐的边缘点和尖锐的边缘点以及非常平坦的平面点的附近点都进行剔除。二、在上一帧点云数据和当前帧点云数据中寻找对应的边缘点匹配对和平面点匹配对，并计算激光雷达3的位姿，给出激光里程计的位姿估计；其中，匹配都是位于当前帧点云的起始坐标系下完成，将上一帧的点云转换到当前帧的起始坐标系，并构建KDtree加速查找。其中，对于边缘点的匹配形式为在上一帧点云的两个点，其中一个点为与待匹配点距离最近的点，另一个为在最近点邻近扫描线(+-1)上与待匹配点的距离最近点。其中，对于平面点的匹配形式为在上一帧点云中的三个点，其中一个为与带匹配点距离最近的点，另一个为与最近点同一扫描线的第二近点，第三个为，与最近点不同线上的最近点。其中，计算位姿估计所使用的损失函数包括两个部分，平面点匹配对损失，以及边缘点匹配对损失。其中平面点的匹配对包括四个点分别为O(待匹配点)A(匹配点1)B(匹配点2)C(匹配点3),平面点匹配对损失如下：其中边缘点的匹配对包括三个点，分别为O(带匹配点)A(匹配点1)B(匹配点2),边缘点匹配对损失如下：三、对稠密地图进行可观测性判断，并构建小规模匹配候选点云；其中，在稠密地图中进行可观测性判断的方式为，将地图分割为小的cube，然后判断cube的定点是否位于观测角内，观测角为与水平呈60°范围内，具体公式(余弦定理)如下：check本文档来自技高网...

【技术保护点】
1.基于激光雷达与双目相机的定位与导航方法，其特征在于，包括步骤：通过双目立体相机获取相机图像，处理来自双目立体相机的图像信息获取位姿；通过激光雷达获取雷达图像，处理来自激光雷达的图像信息获取位姿；对来自双目立体相机图像的位姿和来自激光雷达的位姿进行优化融合，获取关于环境的稠密点云模型和稀疏点云模型分别用于导航和精确定位。

【技术特征摘要】
1.基于激光雷达与双目相机的定位与导航方法，其特征在于，包括步骤：通过双目立体相机获取相机图像，处理来自双目立体相机的图像信息获取位姿；通过激光雷达获取雷达图像，处理来自激光雷达的图像信息获取位姿；对来自双目立体相机图像的位姿和来自激光雷达的位姿进行优化融合，获取关于环境的稠密点云模型和稀疏点云模型分别用于导航和精确定位。2.根据权利要求1所述的基于激光雷达与双目相机的定位与导航方法，其特征在于，双目立体相机的图像位姿获取步骤包括：一、对左右目相机获取的采集图像分别提取特征点，并完成左右目特征点的匹配，通过左右目外参完成深度计算；二、利用匀速假设模型和跟踪参考帧模型为视觉特征点在指定帧中寻找匹配点，并用光束平差法计算相机的位姿，得到初步位姿；三、局部地图跟踪；利用当前帧的共视关系构建共视地图优化初步位姿，给出视觉里程计的位姿估计；四、在当前帧的局部地图上中进行局部光束平差法优化，以调整相机位姿，给出视觉端的位姿估计结果。3.根据权利要求2所述的基于激光雷达与双目相机的定位与导航方法，其特征在于，激光雷达的图像位姿获取步骤包括：一、将激光雷达采集到的数据转化为点云，并计算本帧雷达数据中每个点的曲率，完成边缘点和平面点的提取；二、在上一帧点云数据和当前帧点云数据中寻找对应的边缘点匹配对和平面点匹配对，并计算激光雷达的位姿，给出激光里程计的位姿估计；三、对稠密地图进行可观测性判断，并构建小规模匹配候选点云；四、在匹配候选点云中寻找与当前帧点云数据构成边缘点匹配对和平面点匹配对的点，并计算激光雷达的位姿，给出激光雷达端的位姿优化结果。4.根据权利要求3所述的基于激光雷达与双目相机的定位与导航方法，其特征在于，双目立体相机图像的位姿和来自激光雷达的位姿的优化融合步骤包括：一、如激光雷达端给出的位姿结果或视觉短给出的位姿结果小于阈值，则直接输出激光雷达端的位姿；二、如果激光端给出的位姿结果或者视觉端给出的位姿结果大于阈值，则将视觉端的位姿结果作为初值在稠密地图中进行可观测性判断，并构建小规模的匹配候选点云；三、在匹配候选点云中寻找与当前帧点云数据构成边缘点匹配对和平面点匹配对的点，并以视觉端给出的位姿为初值优化新的位姿。5.根据权利要求2所述的基于激光雷达与双目相机的定位...

【专利技术属性】
技术研发人员：张轶，张钊，
申请(专利权)人：四川大学，
类型：发明
国别省市：四川,51