【技术实现步骤摘要】
本专利技术涉及定位与建图，具体为一种视觉激光紧耦合定位与建图方法及系统。
技术介绍
1、现有工作的主要缺点是没有一种方法能充分利用所有传感器测量值。或仅利用激光雷达或相机的一小部分来增强特征，放弃了大多数传感器测量。或没有在后端中纳入两个传感器的测量。因此，尽管增强了系统鲁棒性，但总体slam精度的改进很小。现有的视觉激光雷达里程计/即时定位与建图(slam)工作主要集中在前端集成，例如将激光雷达深度分配给视觉特征和视觉惯性里程计的顺序组成以及激光雷达扫描数据与地图进行匹配，而视觉和激光雷达后端，特别是闭环检测和光束平差法并没有以紧密耦合的方式集成。现有的视觉激光雷达slam后端可分为视觉驱动和激光驱动两种。视觉驱动方法将激光雷达确定的深度值分配给视觉特征，然后遵循传统的视觉slam框架。而激光雷达驱动的方法是使用视觉/视觉惯性里程计来预测相机的运动，并在激光雷达退化的情况下使用特定维度的运动估计进行补偿，然后使用激光雷达slam来改进位姿。这可以提高在高机动和激光雷达退化场景下的鲁棒性。现有工作的主要缺点是没有一种方法能充分利用所有传感器测量值。或仅利用激光雷达或相机的一小部分来增强特征，放弃了大多数传感器测量。或没有在后端中纳入两个传感器的测量。因此，尽管增强了系统鲁棒性，但总体slam精度的改进很小。
技术实现思路
1、鉴于上述存在的问题，提出了本专利技术。
2、因此，本专利技术解决的技术问题是：现有的视觉slam和激光雷达独立定位方法存在数据融合不充分和位姿估计
3、为解决上述技术问题，本专利技术提供如下技术方案：一种视觉激光紧耦合定位与建图方法，包括：
4、对于每个输入图像和点云对，输入图像被输入到视觉slam前端中，所述前端采用开源视觉orb-slam2算法，提取orb特征并与参考关键帧匹配以计算初始位姿估计；
5、当视觉前端得到初始位姿估计后，将激光雷达点云与最新的激光雷达子地图匹配以找到一些点对线和点对平面的残差；
6、通过所有视觉和激光雷达残差的光束平差法来改进位姿，光束平差法采用原始的orb-slam2中的g20因子图实现，并添加了激光雷达残差和激光雷达地图原点姿势；
7、当利用光束平差完成位姿估计后，利用改进后的位姿将点云累积到最新的激光雷达子地图，若所述子地图上的行进距离超过阈值，创建新的激光雷达子地图，并存储旧的激光雷达子地图；
8、对于输入图像，若机器人移动距离超过预设阈值后，当前图像标记为关键帧，所有关键帧和激光雷达地图原点位姿在局部和全局光束平差法中进行优化，当添加新的关键帧时进行局部光束平差法；
9、激光雷达闭环检测模块与orb-slam2闭环检测模块一起运行，当检测到视觉或激光雷达闭环时进行全局光束平差法。
10、作为本专利技术所述的视觉激光紧耦合定位与建图方法的一种优选方案，其中：所述输入图像包括输入是激光雷达点云和相机图像，输出是相机位姿、三维视觉地图点，以及累积的激光雷达地图；
11、视觉激光雷达slam中，通过相机位姿维护不同类型的地图，由若干三维地图点和视觉描述符组成视觉地图，由许多子地图及其地图原点位姿表示，每个体素在相对于相应地图原点位姿的坐标中表示，组成激光雷达体素地图。
12、作为本专利技术所述的视觉激光紧耦合定位与建图方法的一种优选方案，其中：所述激光雷达残差是通过将采样和补偿的激光雷达点云与最近的激光雷达子地图相匹配而创建的，包括激光雷达位姿的点对线和点对平面约束，以及起始于的激光雷达地图，在每次激光雷达配准中，求解最佳以最小化成本函数。
13、作为本专利技术所述的视觉激光紧耦合定位与建图方法的一种优选方案，其中：所述最小化成本函数表示为，
14、
15、式中,w表示世界坐标系，m表示激光雷达子图坐标系，l表示雷达坐标系，表示激光雷达坐标系到激光雷达子图坐标系的位姿，表示激光雷达子图初始位姿，表示激光雷达位姿，rpl和rpp分别为点对线和点对面残差函数,表示从当前雷达位置pl中的激光雷达点到激光雷达子图中的线和平面的最小距离，下标k和l表示第k个和第l个，pm是对应直线和平面上的三维点，三维向量是归一化法线，pm和都表示在坐标系m下；
16、对于每个点，从两类残差中选择一个残差函数:
17、
18、
19、由相应地图体素的3d正态分布确定每个激光雷达点是否对应于一条线或一个平面残差，对于每个地图体素，通过计算协方差的特征值和特征向量来找到线/平面方程；
20、在三维协方差矩阵的三个特征值中，若其中一个特征值明显大于其他特征值，则将该体素分类为线，特征向量则对应最大的特征值，若一个特征值明显小于其他特征值，则将其分类为平面；在这种情况下是最小特征值的特征向量。
21、作为本专利技术所述的视觉激光紧耦合定位与建图方法的一种优选方案，其中：所述局部光束平差法包括当添加新的关键帧时进行局部光束平差法，当检测到视觉或激光雷达闭环时进行全局光束平差法，在光束平差法中，同时维护激光雷达体素地图和视觉地图点，通过在光束平差法中结合视觉和激光雷达残差确保两种地图相互一致，光束平差法的因子图公式估计一组状态变量，包括相机位姿、激光雷达位姿、视觉地图点和激光雷达子地图原点位姿，在紧耦合状态估计中，将因子图转化为以下非线性优化问题，共同估计所有状态变量。
22、作为本专利技术所述的视觉激光紧耦合定位与建图方法的一种优选方案，其中：所述将因子图转化为以下非线性优化问题表示为，
23、
24、式中，为视觉因子，表示三维地图的投影指向图像；为激光雷达因子，表示压缩扫描地图配准的信息和激光雷达闭环检测约束，如下所示的glf技术，激光雷达因子是扫描数据与地图进行匹配的转换函数；为位姿转换因子；为视觉/激光约束因子，表示视觉地图对激光雷达体素地图的约束，c表示相机坐标系，表示相机坐标系到世界坐标系的第j个位姿，表示第i个激光雷达子图初始位姿，表示激光雷达到相机的位姿，为第k个世界坐标系下的位置，α1、α2、α3表示对应约束是否启用，设置为0或1，以启用或禁用单个约束；局部光束平差法α1＝1、α2＝1、α3＝0；全局光束平差法α1＝1、α2＝1、α3＝1。
25、作为本专利技术所述的视觉激光紧耦合定位与建图方法的一种优选方案，其中：所述将因子图转化为以下非线性优化问题包括简化激光雷达残差，将多个激光雷达残差压缩成一个残差，每个激光雷达配准都被压缩成扫描数据与地图匹配的单个6d残差函数，压缩过程表示为，
26、
27、hδx+r＝0
28、
29、
30、
31、
32、式中，为线性化点；δx为线性化误差；
33、对h应用cholesky分解得到一个6×6本文档来自技高网...
【技术保护点】
1.一种视觉激光紧耦合定位与建图方法，其特征在于，包括：
2.如权利要求1所述的视觉激光紧耦合定位与建图方法，其特征在于：所述输入图像包括输入是激光雷达点云和相机图像，输出是相机位姿、三维视觉地图点，以及累积的激光雷达地图；
3.如权利要求2所述的视觉激光紧耦合定位与建图方法，其特征在于：所述激光雷达残差是通过将采样和补偿的激光雷达点云与最近的激光雷达子地图相匹配而创建的，包括激光雷达位姿的点对线和点对平面约束，以及起始于的激光雷达地图，在每次激光雷达配准中，求解最佳以最小化成本函数。
4.如权利要求3所述的视觉激光紧耦合定位与建图方法，其特征在于：所述最小化成本函数表示为，
5.如权利要求4所述的视觉激光紧耦合定位与建图方法，其特征在于：所述局部光束平差法包括当添加新的关键帧时进行局部光束平差法，当检测到视觉或激光雷达闭环时进行全局光束平差法，在光束平差法中，同时维护激光雷达体素地图和视觉地图点，通过在光束平差法中结合视觉和激光雷达残差确保两种地图相互一致，光束平差法的因子图公式估计一组状态变量，包括相机位姿、激光雷达位姿、视觉地
6.如权利要求5所述的视觉激光紧耦合定位与建图方法，其特征在于：所述将因子图转化为以下非线性优化问题表示为，
7.如权利要求6所述的视觉激光紧耦合定位与建图方法，其特征在于：所述将因子图转化为以下非线性优化问题包括简化激光雷达残差，将多个激光雷达残差压缩成一个残差，每个激光雷达配准都被压缩成扫描数据与地图匹配的单个6d残差函数，压缩过程表示为，
8.一种采用如权利要求1～7任一所述的视觉激光紧耦合定位与建图方法的系统，其特征在于，包括：视觉SLAM前端模块、激光雷达点云处理模块、位姿改进模块、点云累积模块、关键帧管理模块以及闭环检测模块；
9.一种计算机设备，包括存储器和处理器，所述存储器存储有计算机程序，其特征在于，所述处理器执行所述计算机程序时实现权利要求1至7中任一项所述的视觉激光紧耦合定位与建图方法的步骤。
10.一种计算机可读存储介质，其上存储有计算机程序，其特征在于，所述计算机程序被处理器执行时实现权利要求1至7中任一项所述的视觉激光紧耦合定位与建图方法的步骤。
...【技术特征摘要】
1.一种视觉激光紧耦合定位与建图方法，其特征在于，包括：
2.如权利要求1所述的视觉激光紧耦合定位与建图方法，其特征在于：所述输入图像包括输入是激光雷达点云和相机图像，输出是相机位姿、三维视觉地图点，以及累积的激光雷达地图；
3.如权利要求2所述的视觉激光紧耦合定位与建图方法，其特征在于：所述激光雷达残差是通过将采样和补偿的激光雷达点云与最近的激光雷达子地图相匹配而创建的，包括激光雷达位姿的点对线和点对平面约束，以及起始于的激光雷达地图，在每次激光雷达配准中，求解最佳以最小化成本函数。
4.如权利要求3所述的视觉激光紧耦合定位与建图方法，其特征在于：所述最小化成本函数表示为，
5.如权利要求4所述的视觉激光紧耦合定位与建图方法，其特征在于：所述局部光束平差法包括当添加新的关键帧时进行局部光束平差法，当检测到视觉或激光雷达闭环时进行全局光束平差法，在光束平差法中，同时维护激光雷达体素地图和视觉地图点，通过在光束平差法中结合视觉和激光雷达残差确保两种地图相互一致，光束平差法的因子图公式估计一组状态变量，包括相机位姿、激光雷达位姿、视觉地图点和激光雷达子地图原点位姿，...
【专利技术属性】
技术研发人员：班国邦，邓松，欧阳广泽，钟晶亮，吴建蓉，何雨旻，高吉普，欧家祥，陈泽瑞，黄亮程，李翱鹏，杨凤生，马晓红，杨平安，张晓春，
申请(专利权)人：贵州电网有限责任公司，
类型：发明
国别省市：
还没有人留言评论。发表了对其他浏览者有用的留言会获得科技券。