一种结合多线激光雷达的双向深度视觉惯性位姿估计方法

测量装置的制造及其应用技术1.本发明属于移动机器人技术与多传感器融合定位领域，更具体地，涉及一种结合多线激光雷达的双向深度视觉惯性位姿估计方法。背景技术：2.传统的室外定位方法主要是基于全局位置信号的定位方法。在城市环境中，gps的定位精度可以达到5-10m的量级，满足粗略定位的要求，但对于无人汽车自动驾驶等需要高精度位姿的移动机器人来说，是远远不够的。3.gps在室内、隧道等遮挡严重的环境中，容易出现信号丢失问题，无法稳定提供定位信息，鲁棒性不高。在缺少gps、北斗等全局位置信号或信号较弱的情况下，机器人的位姿估计需要借助激光雷达、相机和imu等传感器实现。4.视觉slam在光照条件差、无纹理的场景中，由于检测到的有效特征较少，容易导致跟踪失败。目前，视觉与imu融合是一种提高算法鲁棒性的常见方式，imu可以在视觉失效时提供短时间的位姿估计，但若机器人长期处于匀速运动或纯旋转，机器人的定位精度和鲁棒性会下降。激光slam技术成熟，地图构建清晰直观，定位精度高，目前在定位导航领域应用广泛，但在类似隧道、长廊和空地等无结构环境中，容易失败。不管是激光slam还是视觉slam都有其优势与缺陷，定位精度和鲁棒性有待提高，以适应不同的场景需求。5.由此可见，现有技术存在定位精度较低，鲁棒性较差和难以适应不同场景应用需求的技术问题。本发明基于各种传感器的优势和弊端，融合多传感器进行slam是可行的研究方向。技术实现要素：6.针对现有技术的以上缺陷或改进需求，本发明提供了一种结合多线激光雷达的双向深度视觉惯性位姿估计方法，由此解决现有技术存在定位精度较低，鲁棒性较差和难以适应不同场景应用需求的技术问题。7.为实现上述目的，按照本发明的一个方面，提供了一种结合多线激光雷达的双向深度视觉惯性位姿估计方法，包括：8.使用激光雷达点云恢复双向深度视觉图像中特征点的深度信息后，将双向深度视觉图像分别与imu数据进行紧耦合，得到两个位姿估计，对两个位姿估计加权融合得到视觉里程计；9.从激光雷达点云中提取特征点，构造激光点线残差，将视觉里程计或imu估计结果作为先验位姿估计，将先验位姿估计结合激光点线残差，构建高斯牛顿方程，对高斯牛顿方程进行迭代优化，得到激光里程计；10.分别对双向深度视觉图像和激光雷达点云进行闭环检测，用激光雷达点云配准对所有闭环检测结果进行校验，若校验闭环有效，则将当前时刻和闭环时刻对应的激光关键帧之间的相对位姿变换关系作为闭环约束；11.将相邻视觉深度图像关键帧对应的视觉里程计之间的位姿变换关系作为视觉里程计约束，将相邻激光关键帧之间的激光里程计对应的位姿变换关系作为激光里程计约束，将激光里程计约束、视觉里程计约束和闭环约束作为约束项，进行因子图优化，得到最优位姿估计。12.进一步地，所述闭环检测结果的校验包括：13.对双向深度视觉图像进行闭环检测，获取当前时刻和闭环时刻两个时间戳，在历史激光关键帧队列中搜索两个时间戳对应的两个激光关键帧，分别为两个激光关键帧构建局部特征点云，对两个局部特征点云进行点云配准，配准成功则视觉闭环有效；14.分别为激光雷达点云进行闭环检测中的当前时刻对应的激光关键帧和闭环时刻对应的激光关键帧构建局部特征点云，对两个局部特征点云进行点云配准，配准成功则激光闭环有效。15.进一步地，所述双向深度视觉图像闭环检测到的闭环视觉关键帧满足时间一致性检验和空间一致性检验的要求，所述时间一致性检验是指当前闭环时刻与上一次闭环时刻的间隔大于预设时间阈值，空间一致性检验是指在闭环发生时，当前深度视觉图像关键帧与闭环视觉关键帧之间的距离小于预设距离。16.进一步地，所述闭环检测包括：17.对于双向深度视觉图像中的各向深度视觉图像，计算深度视觉图像与词袋的相似度，将相似度最大的深度视觉图像作为闭环视觉关键帧；18.获取历史所有激光关键帧位置，构建3d的kd-tree，并每间隔一定时间根据当前激光关键帧位姿在kd-tree中进行搜索，找到距离当前激光关键帧最近的近邻帧；如果当前激光关键帧与近邻帧的位置距离小于设定阈值，则将该近邻帧作为闭环时刻对应的激光关键帧。19.进一步地，所述双向深度视觉图像中特征点的深度信息通过如下方式恢复：20.通过去畸变后的激光雷达点云构建局部点云集合，双向深度视觉图像中特征点在局部点云集合中找到匹配的深度信息进行恢复，若缺少匹配点，则使用采集双向深度视觉图像的深度相机的深度信息进行恢复。21.进一步地，所述局部点云集合通过如下方式构建：22.将去畸变后的激光雷达点云堆叠起来，保留邻近时间范围内的激光雷达点云，剔除旧的激光雷达点云，得到局部点云集合，所述邻近时间范围根据平移增量和旋转增量动态更新：23.timeset＝tmax*(1-(μ*deltaa/amax+η*deltap/pmax))24.其中，timeset为局部点云集合维护的时间长度即邻近时间范围，tmax为timeset的最大值，μ和η分别为旋转增量deltaa和平移增量deitap的比例系数，amax和pmax分别为角度增量和旋转增量的最大值。25.进一步地，结合激光点线残差进行迭代优化的方法为：26.激光点的曲率计算是通过前后五个点与当前点之间距离偏差之和的平方来得到的，并根据激光点的曲率大小，提取激光特征点，包括角点和平面点；27.采用基于滑动窗口的激光关键帧维护的角点特征图和平面特征图；28.对当前激光帧对应的角点，用k近邻搜索方法在角点特征图中查找与该点相邻的5个角点；若这5个点与基准点的距离均小于阈值并且近似共线，则认为该角点与特征地图匹配；29.对当前激光帧对应的平面点，在平面特征图中查找与其最近的5个点；若这5个点与基准点的距离均小于阈值并且近似共面，则认为该平面点与平面特征图匹配；30.对匹配上的两类特征点，以特征点到目标直线特征和目标平面特征的距离作为观测值，建立高斯牛顿方程，并以相对高频的视觉里程计或imu里程计为先验估计，通过最小化检测到的线面特征到特征图的距离，得到激光里程计。31.进一步地，所述视觉里程计的计算包括：32.使用激光雷达点云恢复双向深度视觉图像中特征点的深度信息后，双向深度视觉图像均各自通过联合优化视觉重投影误差和imu测量误差，基于滑动窗口优化的方式得到两个位姿估计，并采用加权融合的方式得到视觉里程计。33.进一步地，所述因子图优化还包括：34.基于当前激光关键帧与上一个激光关键帧之间的imu数据构建imu约束，所述imu约束包括imu预积分因子和imu偏置因子，其中，imu预积分因子由imu的加速度、角速度数据构建，imu偏置因子由imu偏置量构建，将激光里程计约束、视觉里程计约束、闭环约束和imu约束作为约束项，进行因子图优化，得到当前激光关键帧的最优位姿估计以及偏置量。35.进一步地，所述方法还包括：36.以因子图优化后的偏置量对当前激光关键帧时刻之后的imu数据重新进行imu预积分，进而得到当前激光关键帧时刻之后的每一时刻的imu估计结果，将imu估计结果作为视觉里程计的位姿估计初值。37.所述imu估计结果为imu频率的机器人位姿估计结果。38.按照本发明的另一方面，提供了一种结合多线激光雷达的双向深度视觉惯性位姿估计系统，包括：双深度相机、多线激光雷达、imu传感器和处理器；39.所述双深度相机，用于采集双向深度视觉图像；40.所述多线激光雷达，用于采集激光雷达点云；41.所述imu传感器，用于采集imu数据；42.所述处理器与双深度相机、多线激光雷达、imu传感器通信，包括：43.视觉里程计估计器，用于使用激光雷达点云恢复双向深度视觉图像中特征点的深度信息后，双向深度视觉图像均各自通过联合优化视觉重投影误差和imu测量误差，基于滑动窗口优化的方式得到两个位姿估计，并采用加权融合的方式得到视觉里程计；44.激光里程计估计器，用于从激光雷达点云中提取特征点，构造激光点线残差，将视觉里程计或imu估计结果作为先验位姿估计，将先验位姿估计结合激光点线残差，构建高斯牛顿方程，对高斯牛顿方程进行迭代优化，得到激光里程计；45.闭环检测模块，用于分别对双向深度视觉图像和激光雷达点云进行闭环检测，用激光雷达点云配准对所有闭环检测结果进行校验，若校验闭环有效，则将当前时刻和闭环时刻对应的激光关键帧之间的相对位姿变换关系作为闭环约束；46.因子图优化模块，用于将相邻视觉深度图像关键帧对应的视觉里程计之间的位姿变换关系作为视觉里程计约束，将相邻激光关键帧之间的激光里程计对应的位姿变换关系作为激光里程计约束，将激光里程计约束、视觉里程计约束和闭环约束作为约束项，进行因子图优化，得到最优位姿估计。47.总体而言，通过本发明所构思的以上技术方案与现有技术相比，能够取得下列有益效果：48.(1)本发明使用双向深度视觉图像进行位姿估计，避免视觉因视角受限而导致闭环漏检，以及单视角特征缺失的情况。使用激光雷达点云恢复双向深度视觉图像中特征点的深度信息，激光雷达和视觉在数据层面是紧耦合的，利用激光雷达点云的深度信息，来辅助恢复视觉特征点的深度，另外，将视觉里程计或imu估计结果作为先验位姿估计计算激光里程计，这是松耦合的。松耦合方法具有结构简单、计算成本低和扩展性强的优势，先验位姿估计有两个选择可分担失效风险。本发明将紧耦合和松耦合相结合，既保证定位精度有具有避险策略，即使在缺少gps、gnss、北斗等全局位置信号的条件，也能精确定位。本发明结合视觉、激光和闭环数据进行因子图优化，采用多数据融合，在光照不良和纹理缺失等场景中仍然具有高精度且鲁棒的位姿估计，实现精确定位，可以应用于无人汽车自动驾驶。49.(2)本发明联合双向深度视觉和多线激光雷达进行闭环检测，视觉的闭环检测成功后，通过激光雷达点云配准进行二次确认，以剔除视觉存在的闭环重复检测和误检情况，提高闭环检测的准确率和召回率。50.(3)为提高算法整体运行效率，视觉闭环帧的选取需要进行时间一致性检验和空间一致性检验，避免出现前后双相机同时检测到闭环或短时间内连续检测到闭环的情况。当前向或后向视觉特征缺失不便于进行闭环检测时，另一视角的图像可以起到弥补的作用，从而借助闭环，优化关键帧位姿。51.(4)激光雷达也会根据里程计近邻检测进行激光闭环检测。本发明对视觉和激光进行闭环，并保证闭环关键帧有效，校验闭环有效，由此能够精确检测到历史闭环，进行闭环优化，为后续在已知地图下的定位方法提供全局一致且平滑的点云地图。52.(5)本发明在数据层面将双向深度视觉与多线激光雷达进行融合，通过动态更新局部点云集合，并将视觉特征点关联到激光点云进行深度恢复，提高了视觉特征点深度恢复的精度和计算效率。本发明根据平移增量和旋转增量动态更新邻近时间范围，由此改变点云集合的稠密程度。53.(6)随着系统运行时间增长，若同时考虑所有关键帧的位姿和特征，会使得计算效率逐渐下降甚至无法实时稳定运行。为了省去大量特征点的优化计算，本发明根据关键帧之间的位姿变换关系，构建里程计约束。通过因子图联合优化，得到高精度且鲁棒的位姿估计。54.(7)双向深度视觉图像均各自通过联合优化视觉重投影误差和imu测量误差，基于滑动窗口优化的方式得到两个位姿估计，并采用加权融合的方式得到视觉里程计，以避免单一视觉特征缺失或跟踪失败，而导致系统定位精度和可靠性的降低。55.(8)本发明通过在因子图优化过程中引入imu约束，联合激光里程计约束、视觉里程计约束、闭环约束和imu约束优化更新位姿和imu偏置。优化imu偏置可以抑制预积分存在的漂移问题。56.(9)本发明以因子图优化后的偏置量对当前激光关键帧时刻之后的imu数据重新进行imu预积分，imu预积分精度的提高可以为后续激光里程计提供更加精确的初值估计。后续将imu估计结果作为视觉里程计的位姿估计初值，然后新的视觉里程计和imu估计结果用于计算激光里程计，而视觉和激光为imu限制了偏差。在偏置受限的条件下，imu的里程计非常准确，进一步拓展了本发明的适用场景。附图说明57.图1是本发明实施例提供的结合多线激光雷达的双向深度视觉惯性位姿估计方法的流程图；58.图2是本发明实施例提供的结合多线激光雷达的双向深度视觉惯性位姿估计系统的核心架构图；59.图3中(a)是本发明实施例提供的kitti数据集形成的三维点云稠密地图；60.图3中(b)是本发明实施例提供的m2dgr数据集形成的三维点云稠密地图；61.图4中(a)是本发明实施例提供的闭环矫正前状态图；62.图4中(b)是本发明实施例提供的闭环矫正后状态图；63.图5是本发明实施例提供的机器人数据流图；64.图6是本发明实施例提供的ros系统节点图。具体实施方式65.为了使本发明的目的、技术方案及优点更加清楚明白，以下结合附图及实施例，对本发明进行进一步详细说明。应当理解，此处所描述的具体实施例仅仅用以解释本发明，并不用于限定本发明。此外，下面所描述的本发明各个实施方式中所涉及到的技术特征只要彼此之间未构成冲突就可以相互组合。66.本发明提供了一种结合多线激光雷达的双向深度视觉惯性位姿估计方法，可应用于城市街道、走廊楼道和室内等环境中，实现高精度位姿估计，并在光照条件不良的环境中，仍具有较高的鲁棒性。本发明的目的是在缺少gps、北斗等全局位置信号下，提高机器人位姿估计的精度和鲁棒性。67.本发明在缺少全局位置信号的场景下，基于多线激光雷达、双向深度视觉和imu融合的位姿估计方法，该方法整体流程如图1所示，包括：68.使用激光雷达点云恢复双向深度视觉图像中特征点的深度信息后，将双向深度视觉图像分别与imu数据进行紧耦合，得到两个位姿估计，对两个位姿估计加权融合得到视觉里程计；69.从激光雷达点云中提取特征点，构造激光点线残差，将视觉里程计或imu估计结果作为先验位姿估计，将先验位姿估计结合激光点线残差，构建高斯牛顿方程，对高斯牛顿方程进行迭代优化，得到激光里程计；70.分别对双向深度视觉图像和激光雷达点云进行闭环检测，用激光雷达点云配准对所有闭环检测结果进行校验，若校验闭环有效，则将当前时刻和闭环时刻对应的激光关键帧之间的相对位姿变换关系作为闭环约束；71.将相邻视觉深度图像关键帧对应的视觉里程计之间的位姿变换关系作为视觉里程计约束，将相邻激光关键帧之间的激光里程计对应的位姿变换关系作为激光里程计约束，将激光里程计约束、视觉里程计约束和闭环约束作为约束项，进行因子图优化，得到最优位姿估计。72.如图2所示，一种结合多线激光雷达的双向深度视觉惯性位姿估计系统，包括：双深度相机、多线激光雷达、imu传感器和处理器；73.所述双深度相机，用于采集双向深度视觉图像；74.所述多线激光雷达，用于采集激光雷达点云；75.所述imu传感器，用于采集imu数据；76.所述处理器与双深度相机、多线激光雷达、imu传感器通信，包括：77.视觉里程计估计器，用于使用激光雷达点云恢复双向深度视觉图像中特征点的深度信息后，将双向深度视觉图像分别与imu数据进行紧耦合，得到两个位姿估计，对两个位姿估计加权融合得到视觉里程计；78.激光里程计估计器，用于从激光雷达点云中提取特征点，构造激光点线残差，将视觉里程计或imu估计结果作为先验位姿估计，将先验位姿估计结合激光点线残差，构建高斯牛顿方程，对高斯牛顿方程进行迭代优化，得到激光里程计；79.闭环检测模块，用于分别对双向深度视觉图像和激光雷达点云进行闭环检测，用激光雷达点云配准对所有闭环检测结果进行校验，若校验闭环有效，则将当前时刻和闭环时刻对应的激光关键帧之间的相对位姿变换关系作为闭环约束；80.因子图优化模块，用于将相邻视觉深度图像关键帧对应的视觉里程计之间的位姿变换关系作为视觉里程计约束，将相邻激光关键帧之间的激光里程计对应的位姿变换关系作为激光里程计约束，将激光里程计约束、视觉里程计约束和闭环约束作为约束项，进行因子图优化，得到最优位姿估计。81.具体地，视觉里程计通过如下方式计算：82.通过去畸变后的激光雷达点云构建局部点云集合，双向深度视觉图像中特征点在局部点云集合中找到匹配的深度信息进行恢复，若缺少匹配点，则使用采集双向深度视觉图像的深度相机的深度信息进行恢复。83.激光雷达点云结合imu进行去畸变，获得去畸变后的激光雷达点云。84.所述局部点云集合通过如下方式构建：85.将去畸变后的激光雷达点云堆叠起来，保留邻近时间范围内的激光雷达点云，剔除旧的激光雷达点云，得到局部点云集合，所述邻近时间范围根据平移增量和旋转增量动态更新，改变点云集合的稠密程度；邻近时间范围的计算公式为：86.timeset＝tmax*(1-(μ*deltaa/amax+η*deltap/pmax))87.其中，timeset为局部点云集合维护的时间长度即邻近时间范围，tmax为timeset的最大值，μ和η分别为旋转增量deltaa和平移增量deltap的比例系数，amax和pmax分别为角度增量和旋转增量的最大值。88.双向深度视觉前端均对输入图像进行自适应均值直方图处理，以提高图像的对比度和清晰度，再进行harris角点特征提取。采用klt光流跟踪算法对前一帧的特征点进行跟踪，在该帧上寻找匹配点，并记录特征点的状态，从而剔除跟踪失败的特征点，并更新特征点被跟踪到的次数。依据特征点被跟踪的次数，尽可能保留被跟踪次数多的特征点，同时对整幅图像上的特征点进行均匀化处理。通过设置邻近特征点之间的最小距离，防止图像中纹理丰富区域的特征点过于密集。由于特征点的剔除和均匀化处理，以及运动过程中当前帧特征点数目的减少，需要根据特征点的数量进行补充提取，以提高特征点的个数来满足后续算法的基本要求。同时，依据相邻两帧图像跟踪的特征点计算基础矩阵，通过ransac算法进行异常点剔除。89.双向深度视觉图像均各自通过联合优化视觉重投影误差和imu测量误差，基于滑动窗口优化的方式得到两个位姿估计，并采用加权融合的方式得到视觉里程计，以避免单一视觉特征缺失或跟踪失败，而导致系统定位精度和可靠性的降低；视觉里程计可作为激光配准初值，以提高配准精度。90.双向深度视觉图像包括前向深度视觉图像和后向深度视觉图像，前向深度视觉图像和后向深度视觉图像中特征点的深度信息均通过以下步骤恢复，以前向深度视觉图像和后向深度视觉图像为例：91.(1)根据移动平台的旋转增量和平移增量，动态维护局部点云集合，改变点云集合的稠密程度；92.(2)将局部点云集合转换到相机坐标系下，将前向深度视觉图像中特征点和激光雷达点云投影到一个以前向深度相机为中心的单位球体上；93.(3)对每一个前向深度视觉图像中特征点，以前向深度视觉图像中特征点的极坐标为基准，搜索一个2d的k-d树，获取球体上相距该特征点最近的3个激光雷达点云；94.(4)若3个激光雷达点云之间的最大距离大于1.5m，则认为局部点云集合中没有与该前向深度视觉图像中特征点匹配的信息，需要采用单目三角化或双目立体匹配方法恢复深度信息，否则跳转步骤(5)；95.(5)联合3个激光雷达点云，通过插值计算得到该特征点的深度值。96.由于系统运行过程中源源不断的采集传感器数据，若考虑所有时刻的传感器测量，数据预处理和优化所需的计算量过大。为了减少计算量，通过基于滑动窗口优化的方式处理图像关键帧数据，优化过程中仅考虑窗口内的关键帧。本质上，视觉惯性紧耦合非线性优化问题是为了求出视觉和imu预积分的雅克比矩阵以及协方差矩阵。如公式(1)所示，该问题涉及到的待优化状态向量包括滑动窗口内imu的状态，包括pvq(位置、速度、旋转)、bias，以及相机到imu的外参和路标点的逆深度。[0097][0098]上式中，xi表示第i帧图像对应的imu状态量，和分别表示位置、方向和速度，ba和bg分别表示加速度计和陀螺仪的偏置量，λm表示第m个3d特征点的逆深度，n表示滑动窗口中imu状态量的起始索引，n表示的是窗口中包含的关键帧数量，m表示窗口中点特征状态变量的起始索引，m表示窗口中的特征点数量，表示相机到imu的外参。[0099]通过最小化多项误差之和来求解非线性优化问题，系统优化需要构建的代价函数如公式(2)所示：[0100][0101]其中，表示的是imu残差项，表示的是视觉的观测残差项，ρ是鲁棒核函数。{rp，hp}为边缘化的先验约束项，可以保留帧间约束对于当前状态的影响。[0102]激光里程计的计算包括：[0103]激光特征点的提取以激光点的曲率为基准，对去畸变后的激光点云，计算每一个激光点的曲率。如公式(3)所示，激光点的曲率计算是通过前后五个点与当前点之间距离偏差之和的平方来得到的。若激光点附近的点近似在同一个球面上，则曲率大小的计算结果应接近于0。[0104][0105]类似于视觉特征点的提取，对激光特征点也需要做均匀化的处理，将激光点云分成若干段，每一子段激光点云最多取30个角点。角点的选择优先考虑曲率值大的点，而对于非角点的点则认为是平面点。将角点与平面点分别加入到角点和平面点云集合，并对平面点云集合做降采样处理。最后在ros平台发布当前激光帧经过特征提取之后的点云信息，其中包含激光点云、初始位姿、特征点云等数据。该点云信息在算法后端部分会被订阅和使用。[0106]激光里程计只使用激光雷达关键帧，其余帧忽略不计。激光帧图匹配的地图采用基于滑动窗口的激光关键帧维护的特征图，以减少计算复杂度。经过点云去畸变和点云特征提取后，以相对高频的视觉里程计或imu估计为先验估计，通过最小化检测到的线面特征到特征图的距离，得到激光里程计。其中，先验估计以视觉里程计为更高优先级。视觉惯性子系统在运动快速、光照条件差和纹理缺失等环境下，由于跟踪的特征点数量不足容易失效。当单个视觉传感器失效时，另一个模块仍能够正常工作，从而正确估计imu的bias，避免bias估计错误而影响整个系统的位姿估计。判定视觉传感器失效的条件是跟踪的特征点过少、bias超过阈值、旋转增量或是车速过快，当满足该失效条件时，重新进行视觉初始化。当前后双向深度视觉系统均失效时，激光里程计进行估计时不会将视觉里程计作为先验。[0107]第i个激光关键帧表现形式为其中为角点特征，为平面特征。局部特征图mi由前n个关键帧的位姿{ti-n，...，ti}和前n+1个关键帧的特征{ki-n，...，ki}构建而成，其中mi包括角点特征地图和平面特征地图当最新一帧雷达关键帧ki+1到来时，匹配该帧与局部特征图mi，构建点线和点面的约束项。通过最小化总约束，得到最新一帧的位姿ti+1。[0108]在优化之前，先将激光雷达关键帧对应的特征点转换到特征地图的坐标系下，定义为第k+1帧激光关键帧中第i个特征点在特征地图坐标系下的坐标。优化方法采用高斯牛顿法，其迭代方法如公式(4)所示，基于点线和点面残差构建残差方程进行优化。[0109][0110]其中，f为残差方程，j为残差项相对于待优化状态变量的jacobian矩阵，δp为待优化变量的增量，包括平移和旋转部分。[0111]在使用高斯牛顿法求解前，需构建残差方程。分别提取角点特征和角点特征地图匹配的特征，以及平面特征和平面特征地图匹配的特征，构建残差约束项。[0112]对当前激光帧对应的角点特征而言，用pcl库的k近邻搜索方法在地图中查找与该点相邻的5个角点。若这5个点与基准点的距离均小于阈值并且近似共线，则认为该角点特征与地图匹配。角点的残差计算，本质上是求该点到特征地图上对应的特征直线的距离，如公式(5)所示：[0113][0114]其中，和为角点特征与特征地图对应的特征直线上的两个点。[0115]对当前激光帧对应的平面特征而言，在地图中查找最近的5个点。若这5个点与基准点的距离均小于阈值并且近似共面，则认为该平面特征与地图匹配。平面特征点的残差计算，本质上是求该点到特征地图上目标平面特征的距离，如公式(6)所示：[0116][0117]对匹配上的两类特征点，计算雅克比矩阵，以特征点到目标直线特征和目标平面特征的距离作为观测值，建立高斯牛顿方程，按公式(4)迭代优化当前位姿，从而得到激光校准后的激光里程计。[0118]因子图优化包括：[0119]随着系统运行时间增长，若同时考虑所有关键帧的位姿和特征，会使得计算效率逐渐下降甚至无法实时稳定运行。为了省去大量特征点的优化计算，根据关键帧之间的位姿变换关系，构建里程计约束。对于相邻的激光关键帧，以它们之间的位姿变换关系构建激光里程计因子(即激光里程计约束)；对于相邻的视觉关键帧，以它们之间的位姿变换关系构建视觉里程计因子(即视觉里程计约束)；对于闭环关键帧，通过icp配准得到当前时刻激光关键帧与闭环时刻激光关键帧之间的位姿变换关系，并基于此构建闭环因子(闭环约束)。[0120]进一步地，里程计因子可以被统一定义为：[0121][0122]上式中，和表示第i、j两个关键帧之间的相对位姿关系，rij表示里程计因子。[0123]整个优化问题的目标函数如下：[0124][0125]上式中，t表示历史关键帧位姿集合，o、v和l分别表示激光里程计因子集合、视觉里程计因子集合和闭环因子集合，h表示限制误差过大的huber函数，用于避免闭环误检导致对位姿优化产生过大影响。[0126]进一步地，基于isam2优化器对上述问题进行求解，得到优化后的激光关键帧位姿。根据当前激光关键帧与上一个激光关键帧之间的imu数据，计算并添加imu预积分因子和imu偏置因子，其中，imu预积分因子由imu的加速度、角速度数据进行构建，imu偏置因子由imu偏置量构建，进一步优化当前激光关键帧的状态，包括当前帧位姿、速度和imu偏置。具体是由开源的非线性优化库gtsam中的isam2优化器对因子图进行优化，更新机器人位姿和偏置量。优化imu偏置可以抑制预积分存在的漂移问题。[0127]进一步地，对优化后的激光里程计，以最新的imu偏置，对当前帧时刻之后的imu数据重新进行imu预积分。imu预积分精度的提高可以为激光里程计提供更加精确的初值估计。对于优化过的激光里程计对应时刻之后的imu数据，以该时刻对应的状态和偏置为基础，根据imu预积分量，推算得到每一时刻imu的状态，得到每一时刻的imu里程计(即imu估计结果)，从而实现视觉里程计、激光里程计、imu里程计的并行估计。imu里程计作为视觉里程计的位姿估计初值，视觉里程计和imu里程计作为激光里程计的匹配初值，而视觉和激光为imu限制了偏差。在偏置受限的条件下，imu的里程计非常准确。[0128]在获得高精度位姿的基础上，将激光关键帧的特征点云注册到环境地图中，构建激光数据三维场景地图。以kitti的05序列和m2dgr的street 04序列数据集为例，建图效果如图3中(a)和图3中(b)所示，本发明可以建立全局一致且平滑的点云地图，真实反映实际场景的三维结构。[0129]闭环检测包括：[0130]视觉闭环检测采用dbow2方法，当新的图像关键帧被添加后，通过brief特征描述子来描述该图像的所有特征点。由于视觉前端提取的harris角点数量较少，需要提取500个fast特征点，并计算brief描述子用于闭环检测。计算当前图像关键帧与词袋的相似度分数，并根据相似度大小返回闭环视觉关键帧。通过brief描述子对当前帧与候选闭环关键帧进行匹配，并利用ransac方法剔除异常值。当匹配点对个数超过阈值时，则认为闭环候选帧有效。[0131]视觉部分采用双相机同时检测闭环。为提高算法整体运行效率，视觉闭环帧的选取需要进行时间一致性检验和空间一致性检验，避免出现前后双相机同时检测到闭环或短时间内连续检测到闭环的情况。时间一致性检验是指当前视觉闭环时间与上一次闭环时间的间隔必须大于设定的时间阈值，空间一致性检验是指在闭环发生时，当前图像关键帧与闭环帧之间的距离必须小于设定距离。[0132]在实际场景中，当前向或后向视觉特征缺失不便于进行闭环检测时，另一视角的相机可以起到弥补的作用，从而借助闭环，优化历史关键帧位姿。当视觉成功检测到闭环时，将包含当前图像帧和闭环帧时间戳的信息以ros的topic形式发布。由激光惯性子系统根据视觉发布的闭环时间戳信息，进行激光闭环二次确认。[0133]对双向深度视觉图像进行闭环检测，获取当前时刻和闭环时刻两个时间戳，在历史激光关键帧队列中搜索两个时间戳对应的两个激光关键帧，分别为两个激光关键帧构建局部特征点云，对两个局部特征点云进行点云配准，配准成功则视觉闭环有效；[0134]分别为激光雷达点云进行闭环检测中的当前时刻对应的激光关键帧和闭环时刻对应的激光关键帧构建局部特征点云，对两个局部特征点云进行点云配准，配准成功则激光闭环有效。[0135]为了提高点云配准的效率，将这两个局部点云进行icp点云配准，当icp迭代收敛并且匹配得分满足阈值要求，则认为该闭环有效。图4中(a)是本发明实施例提供的闭环矫正前状态图，图4中(b)是本发明实施例提供的闭环矫正后状态图，说明本发明闭环矫正效果较好。[0136]在本发明实施例中，在公开的基准数据集kitti和m2dgr序列的数据集上进行实验，验证激光雷达对视觉闭环二次校验的效果。基于激光点云配准来校验双向深度视觉的闭环检测，结果如表1所示，记录的数据仅包含视觉闭环检测成功时的数据。表中数据元素的格式为：(id1-id2)，其中id1和id2分别表示视觉闭环信息对应的激光关键帧和闭环激光关键帧的id号。[0137]表1.激光视觉联合的闭环确认[0138][0139]表1中，经过激光点云配准进行二次确认后，序号1和序号3对应的闭环失效原因为，在当前位置附近视觉闭环与激光闭环均检测成功，无需重复计算闭环因子。表中序号2对应的失效原因为点云配准的得分大于设定的闭环阈值，因此需要舍去该类闭环信息，视为闭环误检。基于校验结果可以得出，视觉在部分场景中存在闭环误检的情况，而在激光雷达闭环检验中可以通过点云配准，有效剔除这种误检情况，提高闭环检测的准确率和召回率。[0140]如图5所示，一种安装于移动机器人上的测量设备，包括多线激光雷达、双深度相机和imu传感器；传感器安装在双层支撑架上，支撑架底部固定在机器人移动平台上，平台侧边固定安装24v转220v逆变器；机器人底盘的电机驱动器采用交流伺服马达驱动器，上层采用高性能笔记本电脑作为计算平台，通过网口连接激光雷达，usb口连接相机，下层采用嵌入式控制板作为主控板，通过串口与上层平台通讯。[0141]具体地，一种安装于移动机器人上的测量设备，包括：镭神32线激光雷达、前后两个zed2相机(内置imu)、高性能计算平台和主控板。传感器安装在双层支撑架上，支撑架底部固定在移动平台上，平台侧边固定安装24v转220v逆变器。机器人底盘的电机驱动器采用松下的交流伺服马达驱动器，上层采用高性能笔记本电脑作为计算平台，通过网口连接激光雷达，usb口连接zed2相机，下层采用嵌入式控制板作为主控板，通过串口与上层平台通讯。[0142]zed2双目深度相机是美国的stereo labs公司生产的，图像高清并且高帧率。该相机内置了高精度imu、磁力计和气压计等传感器，6轴姿态位置精度1mm，方向精度0.1°，广泛应用于目标检测、场景重建、视觉定位和人机交互等应用场景。激光雷达选用镭神32线激光雷达c32-151a，共32路激光通道，测距范围100～200m，测距精度达到3cm，单回波数据速率达到65万点每秒。激光雷达垂直视场角为-16°～15°，垂直角度分辨率为1°，水平视场角为360°，水平角度分辨率为0.18°。本实施例中，设定的相机图像输出频率为20hz，imu输出频率为400hz，激光雷达输出频率为10hz，其中，zed2相机和激光雷达均采用ros驱动获得其图像和点云数据。高性能笔记本电脑的配置为英特尔i7-10870h处理器，16g内存和rtx 2060显卡。嵌入式控制板基于stm32f103设计和开发，具备pwm输出、io采集、定时器和串口通信等功能。[0143]整体软件架构基于机器人操作系统ros进行开发。高性能计算平台安装ubuntu 18.04系统和ros的melodic版本，两者版本彼此兼容，同时以c++作为主要编程语言。ros系统为机器人操作系统，如图6所示，软件系统ros节点图，包含视觉特征提取与跟踪节点、视觉里程计节点、视觉闭环检测节点、激光点云去畸变节点、激光点云特征提取节点、imu预积分节点和后端优化节点，分别用于视觉特征提取与跟踪、视觉里程计估计、视觉闭环检测、激光点云去畸变、激光点云特征提取、imu预积分和因子图优化。[0144]本领域的技术人员容易理解，以上所述仅为本发明的较佳实施例而已，并不用以限制本发明，凡在本发明的精神和原则之内所作的任何修改、等同替换和改进等，均应包含在本发明的保护范围之内。









图片声明：本站部分配图来自人工智能系统AI生成,觅知网授权图片,PxHere摄影无版权图库。本站只作为美观性配图使用,无任何非法侵犯第三方意图,一切解释权归图片著作权方,本站不承担任何责任。如有恶意碰瓷者,必当奉陪到底严惩不贷!




内容声明：本文中引用的各种信息及资料（包括但不限于文字、数据、图表及超链接等）均来源于该信息及资料的相关主体（包括但不限于公司、媒体、协会等机构）的官方网站或公开发表的信息。部分内容参考包括:(百度百科,百度知道,头条百科,中国民法典,刑法,牛津词典,新华词典,汉语词典,国家院校,科普平台)等数据,内容仅供参考使用,不准确地方联系删除处理！本站为非盈利性质站点,发布内容不收取任何费用也不接任何广告!




免责声明：我们致力于保护作者版权，注重分享，被刊用文章因无法核实真实出处，未能及时与作者取得联系，或有版权异议的，请联系管理员，我们会立即处理，本文部分文字与图片资源来自于网络，部分文章是来自自研大数据AI进行生成,内容摘自(百度百科,百度知道,头条百科,中国民法典,刑法,牛津词典,新华词典,汉语词典,国家院校,科普平台)等数据,内容仅供学习参考,不准确地方联系删除处理!的,若有来源标注错误或侵犯了您的合法权益，请立即通知我们，情况属实，我们会第一时间予以删除，并同时向您表示歉意,谢谢!