CN115797490A - 基于激光视觉融合的建图方法及系统 - Google Patents

Abstract

本发明公开了一种基于激光视觉融合的建图方法及系统，包括：接收激光传感器采集到的激光观测数据和视觉传感器采集到的视觉观测数据；根据所述激光观测数据中的每一帧激光点云图像生成局部点云地图；基于卡尔曼滤波算法，根据所述激光观测数据和所述视觉观测数据对所述建图系统的每一时刻的当前状态进行更新；当满足预设的停止更新条件时，基于所述局部点云地图对更新后的所有时刻的所述当前状态进行全局优化，生成全局地图。采用本发明实施例能够实现对整体环境特点的精确描述，鲁棒性高。

Description

基于激光视觉融合的建图方法及系统

技术领域

本发明涉及地图创建技术领域，尤其涉及一种基于激光视觉融合的建图方法及系统。

背景技术

目前，建立周边环境地图主要采用同步定位与建图技术进行建图，针对使用不同传感器的SLAM方案，可以分为激光SLAM和视觉SLAM。

激光SLAM建图具有精度高，受环境光照影响小等优点，但是该方法只能获取空间的几何信息，但是当处于缺失几何信息的空间，例如隧道，激光SLAM建图将会非常不稳定；视觉SLAM建图过程是实时的对每一帧图片提取特征点，跟关键帧做特征匹配，以及局部的地图优化等，但是该方法无法处理纹理质量低的区域，例如空白的墙面。可见，由于环境复杂，单纯只靠一种传感器利用SLAM建立的地图存在各自的缺陷，且获取的地图维度具有视野的局限性，无法对整体的环境特点进行精确的描述。

发明内容

本发明实施例的目的是提供一种基于激光视觉融合的建图方法及系统，能够实现对整体环境特点的精确描述，鲁棒性强。

为实现上述目的，本发明实施例提供了一种基于激光视觉融合的建图方法，应用于建图系统，所述建图系统包括激光传感器和视觉传感器，所述基于激光视觉融合的建图方法包括：

接收所述激光传感器采集到的激光观测数据和所述视觉传感器采集到的视觉观测数据；

根据所述激光观测数据中的每一帧激光点云图像生成局部点云地图；

基于卡尔曼滤波算法，根据所述激光观测数据和所述视觉观测数据对所述建图系统的每一时刻的当前状态进行更新；

当满足预设的停止更新条件时，基于所述局部点云地图对更新后的所有时刻的所述当前状态进行全局优化，生成全局地图。

作为上述方案的改进，所述建图系统还包括GPS传感器，则所述基于激光视觉融合的建图方法还包括：

接收所述GPS传感器采集到的GPS观测数据；

基于卡尔曼滤波算法，利用所述GPS观测数据对所述当前状态进行更新。

作为上述方案的改进，所述建图系统还包括陀螺仪，则，通过以下步骤对所述当前状态进行预测：

接收所述陀螺仪采集到的陀螺仪观测数据；

基于卡尔曼滤波算法和高阶Runge Kutta插值法，利用所述陀螺仪观测数据对所述建图系统的当前状态进行预测。

作为上述方案的改进，所述基于卡尔曼滤波算法，根据所述激光观测数据和所述视觉观测数据对所述建图系统的每一时刻的当前状态进行更新，包括：

当当前时刻接收到所述激光观测数据时，基于卡尔曼滤波算法，根据所述激光观测数据对所述建图系统的所述当前时刻的当前状态进行更新；

当当前时刻接收到所述视觉观测数据时，基于卡尔曼滤波算法，根据所述视觉观测数据对所述建图系统的所述当前时刻的当前状态进行更新；

当当前时刻同时接收到所述激光观测数据和所述视觉观测数据时，基于卡尔曼滤波算法，按照预设的顺序，先后分别根据所述激光观测数据和所述视觉观测数据对所述建图系统的所述当前时刻的当前状态进行更新。

作为上述方案的改进，基于卡尔曼滤波算法，根据所述激光观测数据对所述建图系统的当前状态进行更新，包括：

对每一帧所述激光点云图像进行预处理，提取每一帧所述激光点云图像的第一面特征点和第一线特征点；

将每一帧所述激光点云图像的坐标数据投影到陀螺仪坐标系下，将所述第一面特征点与所述局部点云地图中的第二面特征点进行匹配，并将所述第一线特征点与所述局部点云地图中的第二线特征点进行匹配；

利用匹配到的面特征点和线特征点对所述建图系统的当前状态进行更新。

作为上述方案的改进，所述利用匹配到的面特征点和线特征点对所述建图系统的当前状态进行更新，包括：

根据匹配到的面特征点，计算局部面片的面中心和法向量；

根据所述面中心和所述法向量计算第一激光误差；

对所述第一激光误差求一阶偏导，并线性化处理得到第一激光H矩阵；

根据匹配到的线特征点，计算局部线的线中心和线方向；

根据所述线中心和所述线方向计算第二激光误差；

对所述第二激光误差求一阶偏导，并线性化处理得到第二激光H矩阵；

将所述第一激光H矩阵和所述第二激光H矩阵进行叠加处理，得到激光H矩阵；

根据所述激光H矩阵更新所述当前状态的协方差；

根据更新后的所述协方差对所述当前状态进行更新。

作为上述方案的改进，所述对每一帧所述激光点云图像进行预处理，提取每一帧所述激光点云图像的第一面特征点和第一线特征点，包括：

根据每一帧所述激光点云图像的每一激光扫描线的曲率，提取每一帧所述激光点云图像的初始面特征点和初始线特征点；

统计每一帧所述激光点云图像的点云和所述局部点云地图中的点云在2D栅格下的分布，比较每一个所述2D栅格内的点云分布，得到动态物体所处的2D栅格，删除所述2D栅格中的所述动态物体的点云，以从所述初始面特征点和初始线特征点中过滤动态物体的特征点，得到每一帧所述激光点云图像的第一面特征点和第一线特征点。

作为上述方案的改进，基于卡尔曼滤波算法，根据所述视觉观测数据对所述建图系统的当前状态进行更新，包括：

获取所述视觉观测数据中的每一关键帧图像；

计算上一关键帧图像的每一像素的光度梯度，并将所述局部点云地图的点云投影到所述上一关键帧图像，得到每一像素的深度；

将光度梯度的模值大于预设的模值阈值的像素点作为追踪点，通过所述深度，将所述追踪点投影到当前关键帧图像，得到若干光度误差；

将每一所述光度误差作为每一残差，对其求一阶偏微分，并线性化处理，得到每一第一视觉H矩阵；

将所有所述第一视觉H矩阵进行叠加处理，得到视觉H矩阵；

根据所述视觉H矩阵更新所述当前状态的协方差；

根据更新后的所述协方差对所述当前状态进行更新。

作为上述方案的改进，所述当满足预设的停止更新条件时，基于所述局部点云地图对更新后的所有时刻的所述当前状态进行全局优化，生成全局地图，包括：

当满足预设的停止更新条件时，对所述局部点云地图进行回环检测，得到每一关键帧激光点云图像；

计算得到每一所述关键帧激光点云图像之间的相对位姿和所述相对位姿的协方差矩阵；

根据所述相对位姿、所述相对位姿的协方差矩阵对更新后的所有时刻的所述当前状态进行全局优化，生成全局地图。

为实现上述目的，本发明实施例提供了一种基于激光视觉融合的建图系统，所述建图系统包括：激光传感器、视觉传感器和控制器，所述控制器被配置为：

接收所述激光传感器采集到的激光观测数据和所述视觉传感器采集到的视觉观测数据；

根据所述激光观测数据中的每一帧激光点云图像生成局部点云地图；

基于卡尔曼滤波算法，根据所述激光观测数据和所述视觉观测数据对所述建图系统的每一时刻的当前状态进行更新；

当满足预设的停止更新条件时，基于所述局部点云地图对更新后的所有时刻的所述当前状态进行全局优化，生成全局地图。

与现有技术相比，本发明实施例提供的一种基于激光视觉融合的建图方法及系统，通过采用激光传感器采集到的激光观测数据和视觉传感器采集到的视觉观测数据对建图系统的每一时刻的当前状态进行更新，利用更新后的所有当前状态，生成全局地图，能够解决激光SLAM建图无法处理几何缺失和视觉SLAM建图无法处理弱纹理的问题，而且本发明实施例适用于各种复杂的环境，建图精度高，对动态物体和光照变化都有不俗的抗性，鲁棒性强。

附图说明

图1是本发明实施例提供的一种基于激光视觉融合的建图方法的流程图。

具体实施方式

下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有作出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。

参见图1，图1是本发明实施例提供的一种基于激光视觉融合的建图方法的流程图，所述建图系统包括激光传感器和视觉传感器，所述基于激光视觉融合的建图方法包括：

S1、接收所述激光传感器采集到的激光观测数据和所述视觉传感器采集到的视觉观测数据；

S2、根据所述激光观测数据中的每一帧激光点云图像生成局部点云地图；

S3、基于卡尔曼滤波算法，根据所述激光观测数据和所述视觉观测数据对所述建图系统的每一时刻的当前状态进行更新；

S4、当满足预设的停止更新条件时，基于所述局部点云地图对更新后的所有时刻的所述当前状态进行全局优化，生成全局地图。

在一可选实施例中，所述建图系统还包括陀螺仪，则，通过以下步骤对所述当前状态进行预测：

接收所述陀螺仪采集到的陀螺仪观测数据；

基于卡尔曼滤波算法和高阶Runge Kutta插值法，利用所述陀螺仪观测数据对所述建图系统的当前状态进行预测。

示例性地，所述当前状态包括：所述建图系统的位置、旋转、速度、加速度计偏差，角度速度计偏差和重力的方向，可表示为

其中，传感器的旋转方向为

位置为p，速度为v，加速度计的速度为b_a，陀螺仪的偏差为b_g，重力为g，k代表离散化的时间；

对当前状态预测进行说明：

预测当前状态：

预测当前状态的协方差：

可以理解的是，在本发明实施例中，基于陀螺仪的运动模型预测当前状态，具体地，使用高阶Runge Kutta插值法对当前状态进行预测：：

b_a,k|k-1＝b_a,k-1|k-1

b_g,k|k-1＝b_g,k-1|k-1

g_k|k-1＝g_k-1|k-1

其中，更新位置p时有y＝p，更新速度v时，y＝v：

k₁＝f(t,y)

k₄＝f(t+dt,y+k₃*dt)

f(t,p)＝v_k-1|k-1

根据下式预测当前状态的协方差：

可以理解的是，在步骤S3完成后，得到240Hz的当前状态的更新，其中，200Hz的更新来自陀螺仪的预测，30Hz来自视觉观测数据的更新，10Hz来自激光观测数据的更新。

在一可选实施例中，所述建图系统还包括GPS传感器，则所述基于激光视觉融合的建图方法还包括：

接收所述GPS传感器采集到的GPS观测数据；

基于卡尔曼滤波算法，利用所述GPS观测数据对所述当前状态进行更新。

在本发明实施例中，通过卡尔曼滤波算法分别利用GPS观测数据、激光观测数据和视觉观测数据对建图系统的当前状态进行更新：

计算观测(z)的残差：

计算观测的协方差：

计算卡尔曼增益：

更新当前状态：

更新当前状态的协方差P_k|k＝P_k|k-1(I-K_kH_k)

可以理解的是，在本发明实施例中，GPS观测数据为位置观测，其观测的协方差R直接使用GPS观测数据，利用下式进行卡尔曼滤波算法的更新：

r_k＝z_k-p_k

在一可选实施例中，所述基于卡尔曼滤波算法，根据所述激光观测数据和所述视觉观测数据对所述建图系统的每一时刻的当前状态进行更新，包括：

当当前时刻接收到所述激光观测数据时，基于卡尔曼滤波算法，根据所述激光观测数据对所述建图系统的所述当前时刻的当前状态进行更新；

当当前时刻接收到所述视觉观测数据时，基于卡尔曼滤波算法，根据所述视觉观测数据对所述建图系统的所述当前时刻的当前状态进行更新；

当当前时刻同时接收到所述激光观测数据和所述视觉观测数据时，基于卡尔曼滤波算法，按照预设的顺序，先后分别根据所述激光观测数据和所述视觉观测数据对所述建图系统的所述当前时刻的当前状态进行更新。

可以理解的是，建图系统会持续接收激光传感器采集到的激光观测数据和视觉传感器采集到的视觉观测数据；当当前时刻接收到所述激光观测数据时，根据所述激光观测数据对所述建图系统的所述当前时刻的当前状态进行更新；当当前时刻接收到所述视觉观测数据时，根据所述视觉观测数据对所述建图系统的所述当前时刻的当前状态进行更新；当当前时刻同时接收到所述激光观测数据和所述视觉观测数据时，按照预设的顺序，先后分别根据所述激光观测数据和所述视觉观测数据对所述建图系统的所述当前时刻的当前状态进行更新；进一步地，当当前时刻接收到GPS观测数据时，根据所述GPS观测数据对所述建图系统的所述当前时刻的当前状态进行更新；当当前时刻同时接收到所述激光观测数据、所述视觉观测数据和所述GPS观测数据时，按照预设的顺序，先后分别根据所述激光观测数据、所述视觉观测数据和所述GPS观测数据对所述建图系统的所述当前时刻的当前状态进行更新。

在一可选实施例中，基于卡尔曼滤波算法，根据所述激光观测数据对所述建图系统的当前状态进行更新，包括：

对每一帧所述激光点云图像进行预处理，提取每一帧所述激光点云图像的第一面特征点和第一线特征点；

将每一帧所述激光点云图像的坐标数据投影到陀螺仪坐标系下，将所述第一面特征点与所述局部点云地图中的第二面特征点进行匹配，并将所述第一线特征点与所述局部点云地图中的第二线特征点进行匹配；

利用匹配到的面特征点和线特征点对所述建图系统的当前状态进行更新。

在一可选实施例中，所述利用匹配到的面特征点和线特征点对所述建图系统的当前状态进行更新，包括：

根据匹配到的面特征点，计算局部面片的面中心和法向量；

根据所述面中心和所述法向量计算第一激光误差；

对所述第一激光误差求一阶偏导，并线性化处理得到第一激光H矩阵；

根据匹配到的线特征点，计算局部线的线中心和线方向；

根据所述线中心和所述线方向计算第二激光误差；

对所述第二激光误差求一阶偏导，并线性化处理得到第二激光H矩阵；

将所述第一激光H矩阵和所述第二激光H矩阵进行叠加处理，得到激光H矩阵；

根据所述激光H矩阵更新所述当前状态的协方差；

根据更新后的所述协方差对所述当前状态进行更新。

可以理解的是，在本发明实施例中，使用两个残差函数r来计算激光传感器观测到的误差(第一激光误差和第二激光误差)：

示例性的，将所有的点都从激光传感器参考系，通过离线标定的外参，投影到陀螺仪坐标系下；

第一激光误差r_1(i,k)：根据所述第一面特征点q_1(i)和所述局部点云地图中的第二面特征点进行KNN匹配到的面特征点，计算局部面片的面中心q_1(ref)和法向量n，每一个匹配都会构成一个点到面的误差，最终有l个点，则会构成l×11维的第一激光H矩阵H_1(k)，具体的表达式为：

r_1(i,k)＝n(R_k*q_1(i)+p_k-q_1(ref))

H_1(i,k)＝[(q_i×(R_kn))^T n^T 0_1×11]

第二激光误差r_2(i,k)：根据所述第一线特征点q_2(i)和所述局部点云地图中的第二线特征点进行KNN匹配到的线特征点，计算局部线的线中心q_ref和线的方向d，每一个匹配都会构成一个点到线的误差，最终有l个点，则会构成3l×11维的第二激光H矩阵H_2(k)，具体的表达式为：

r_2(i,k)＝d×(R_k*q_2(i)+p_k-q_2(ref))

H_2(i,k)＝[(d)^ -(d)^R_k*(q_i)^ 0_3×11]

最后，将第一激光H矩阵H_1(k)和第二激光H矩阵H_2(k)叠加计算，得到激光H矩阵H_k：

利用激光H矩阵H_k进行当前状态的更新。

在一可选实施例中，所述对每一帧所述激光点云图像进行预处理，提取每一帧所述激光点云图像的第一面特征点和第一线特征点，包括：

根据每一帧所述激光点云图像的每一激光扫描线的曲率，提取每一帧所述激光点云图像的初始面特征点和初始线特征点；

统计每一帧所述激光点云图像的点云和所述局部点云地图中的点云在2D栅格下的分布，比较每一个所述2D栅格内的点云分布，得到动态物体所处的2D栅格，删除所述2D栅格中的所述动态物体的点云，以从所述初始面特征点和初始线特征点中过滤动态物体的特征点，得到每一帧所述激光点云图像的第一面特征点和第一线特征点。

示例性的，通过对激光点云图像的每一条激光扫描线做卷积处理得到曲率，根据曲率提取初始面特征点和初始线特征点；

通过删除所述2D栅格中的所述动态物体的点云，过滤初始面特征点和初始线特征点中的动态物体的特征点，得到第一面特征点和第一线特征点。

在一可选实施例中，基于卡尔曼滤波算法，根据所述视觉观测数据对所述建图系统的当前状态进行更新，包括：

获取所述视觉观测数据中的每一关键帧图像；

计算上一关键帧图像的每一像素的光度梯度，并将所述局部点云地图的点云投影到所述上一关键帧图像，得到每一像素的深度；

将光度梯度的模值大于预设的模值阈值的像素点作为追踪点，通过所述深度，将所述追踪点投影到当前关键帧图像，得到若干光度误差；

将每一所述光度误差作为每一残差，对其求一阶偏微分，并线性化处理，得到每一第一视觉H矩阵；

将所有所述第一视觉H矩阵进行叠加处理，得到视觉H矩阵；

根据所述视觉H矩阵更新所述当前状态的协方差；

根据更新后的所述协方差对所述当前状态进行更新。

示例性的，根据下式计算真实的光度梯度：

其中，t为关键帧图像的曝光时间，a为预设的非线性参数，b为光度的偏差值；

将上一关键帧图像((u₀,v₀)∈S₀)追踪点投影到当前关键帧图像((u_k,v_k)∈S_k)，计算每一第一视觉H矩阵

式中，

表示关键帧图像的光度梯度。

将所有第一视觉H矩阵

进行叠加，得到视觉H矩阵H_k：

利用视觉H矩阵H_k对当前状态进行更新。

在一可选实施例中，所述当满足预设的停止更新条件时，基于所述局部点云地图对更新后的所有时刻的所述当前状态进行全局优化，生成全局地图，包括：

当满足预设的停止更新条件时，对所述局部点云地图进行回环检测，得到每一关键帧激光点云图像；

计算得到每一所述关键帧激光点云图像之间的相对位姿和所述相对位姿的协方差矩阵；

根据所述相对位姿、所述相对位姿的协方差矩阵对更新后的所有时刻的所述当前状态进行全局优化，生成全局地图。

可以理解的是，当不再接收到新的观测数据时，停止更新，对所述局部点云地图进行回环检测，使用激光点云数据的矩阵描述子，对每一帧激光点云图像进行描述F({p_i})＝D，D∈R²，通过遍历地图关键帧计算min_j||D_i-D_j||得到与第i帧匹配的关键帧j。矩阵描述子D的计算如下：将每一帧激光点云图像的点云按照极坐标系分为2D的栅格，统计每一栅格内的激光点的强度的最大值，赋值描述子矩阵：D(θ，ρ)＝I_max。

在检测到回环候选之后，进行点到面的ICP匹配，并利用Hessian矩阵，计算得到每一所述关键帧激光点云图像之间的相对位姿和所述相对位姿的协方差矩阵；

根据所述相对位姿、所述相对位姿的协方差矩阵对更新后的所有时刻的所述当前状态进行全局优化，生成全局地图；可选地，所述全局地图包括kdtree地图和TSDF地图。

为实现上述目的，本发明实施例提供了一种基于激光视觉融合的建图系统，所述建图系统包括：激光传感器、视觉传感器和控制器，所述控制器被配置为：

接收所述激光传感器采集到的激光观测数据和所述视觉传感器采集到的视觉观测数据；

根据所述激光观测数据中的每一帧激光点云图像生成局部点云地图；

基于卡尔曼滤波算法，根据所述激光观测数据和所述视觉观测数据对所述建图系统的每一时刻的当前状态进行更新；

当满足预设的停止更新条件时，基于所述局部点云地图对更新后的所有时刻的所述当前状态进行全局优化，生成全局地图。

优选地，所述建图系统还包括GPS传感器，则所述控制器还被配置为：

接收所述GPS传感器采集到的GPS观测数据；

基于卡尔曼滤波算法，利用所述GPS观测数据对所述当前状态进行更新。

优选地，所述建图系统还包括陀螺仪，则所述控制器还被配置为：

接收所述陀螺仪采集到的陀螺仪观测数据；

基于卡尔曼滤波算法和高阶Runge Kutta插值法，利用所述陀螺仪观测数据对所述建图系统的当前状态进行预测。

优选地，所述基于卡尔曼滤波算法，根据所述激光观测数据和所述视觉观测数据对所述建图系统的每一时刻的当前状态进行更新，包括：

当当前时刻接收到所述激光观测数据时，基于卡尔曼滤波算法，根据所述激光观测数据对所述建图系统的所述当前时刻的当前状态进行更新；

当当前时刻接收到所述视觉观测数据时，基于卡尔曼滤波算法，根据所述视觉观测数据对所述建图系统的所述当前时刻的当前状态进行更新；

当当前时刻同时接收到所述激光观测数据和所述视觉观测数据时，基于卡尔曼滤波算法，按照预设的顺序，先后分别根据所述激光观测数据和所述视觉观测数据对所述建图系统的所述当前时刻的当前状态进行更新。

优选地，基于卡尔曼滤波算法，根据所述激光观测数据对所述建图系统的当前状态进行更新，包括：

对每一帧所述激光点云图像进行预处理，提取每一帧所述激光点云图像的第一面特征点和第一线特征点；

将每一帧所述激光点云图像的坐标数据投影到陀螺仪坐标系下，将所述第一面特征点与所述局部点云地图中的第二面特征点进行匹配，并将所述第一线特征点与所述局部点云地图中的第二线特征点进行匹配；

利用匹配到的面特征点和线特征点对所述建图系统的当前状态进行更新。

优选地，所述利用匹配到的面特征点和线特征点对所述建图系统的当前状态进行更新，包括：

根据匹配到的面特征点，计算局部面片的面中心和法向量；

根据所述面中心和所述法向量计算第一激光误差；

对所述第一激光误差求一阶偏导，并线性化处理得到第一激光H矩阵；

根据匹配到的线特征点，计算局部线的线中心和线方向；

根据所述线中心和所述线方向计算第二激光误差；

对所述第二激光误差求一阶偏导，并线性化处理得到第二激光H矩阵；

将所述第一激光H矩阵和所述第二激光H矩阵进行叠加处理，得到激光H矩阵；

根据所述激光H矩阵更新所述当前状态的协方差；

根据更新后的所述协方差对所述当前状态进行更新。

优选地，所述对每一帧所述激光点云图像进行预处理，提取每一帧所述激光点云图像的第一面特征点和第一线特征点，包括：

根据每一帧所述激光点云图像的每一激光扫描线的曲率，提取每一帧所述激光点云图像的初始面特征点和初始线特征点；

统计每一帧所述激光点云图像的点云和所述局部点云地图中的点云在2D栅格下的分布，比较每一个所述2D栅格内的点云分布，得到动态物体所处的2D栅格，删除所述2D栅格中的所述动态物体的点云，以从所述初始面特征点和初始线特征点中过滤动态物体的特征点，得到每一帧所述激光点云图像的第一面特征点和第一线特征点。

优选地，基于卡尔曼滤波算法，根据所述视觉观测数据对所述建图系统的当前状态进行更新，包括：

获取所述视觉观测数据中的每一关键帧图像；

计算上一关键帧图像的每一像素的光度梯度，并将所述局部点云地图的点云投影到所述上一关键帧图像，得到每一像素的深度；

将光度梯度的模值大于预设的模值阈值的像素点作为追踪点，通过所述深度，将所述追踪点投影到当前关键帧图像，得到若干光度误差；

将每一所述光度误差作为每一残差，对其求一阶偏微分，并线性化处理，得到每一第一视觉H矩阵；

将所有所述第一视觉H矩阵进行叠加处理，得到视觉H矩阵；

根据所述视觉H矩阵更新所述当前状态的协方差；

根据更新后的所述协方差对所述当前状态进行更新。

优选地，所述当满足预设的停止更新条件时，基于所述局部点云地图对更新后的所有时刻的所述当前状态进行全局优化，生成全局地图，包括：

当满足预设的停止更新条件时，对所述局部点云地图进行回环检测，得到每一关键帧激光点云图像；

计算得到每一所述关键帧激光点云图像之间的相对位姿和所述相对位姿的协方差矩阵；

根据所述相对位姿、所述相对位姿的协方差矩阵对更新后的所有时刻的所述当前状态进行全局优化，生成全局地图。

值得说明的是，本发明实施例所述的基于激光视觉融合的建图装置10中控制器的工作过程可参考上述实施例所述的基于激光视觉融合的建图方法的工作过程，在此不再赘述。

与现有技术相比，本发明实施例提供的一种基于激光和视觉融合的建图方法及系统，通过采用激光传感器采集到的激光观测数据和视觉传感器采集到的视觉观测数据对建图系统的每一时刻的当前状态进行更新，利用更新后的所有当前状态，生成全局地图，能够解决激光SLAM建图无法处理几何缺失和视觉SLAM建图无法处理弱纹理的问题，而且本发明实施例适用于各种复杂的环境，建图精度高，对动态物体和光照变化都有不俗的抗性，鲁棒性强。

以上所述是本发明的优选实施方式，应当指出，对于本技术领域的普通技术人员来说，在不脱离本发明原理的前提下，还可以做出若干改进和润饰，这些改进和润饰也视为本发明的保护范围。

Claims (10)

1.一种基于激光视觉融合的建图方法，其特征在于，应用于建图系统，所述建图系统包括激光传感器和视觉传感器，所述基于激光视觉融合的建图方法包括：

接收所述激光传感器采集到的激光观测数据和所述视觉传感器采集到的视觉观测数据；

根据所述激光观测数据中的每一帧激光点云图像生成局部点云地图；

基于卡尔曼滤波算法，根据所述激光观测数据和所述视觉观测数据对所述建图系统的每一时刻的当前状态进行更新；

当满足预设的停止更新条件时，基于所述局部点云地图对更新后的所有时刻的所述当前状态进行全局优化，生成全局地图。

2.如权利要求1所述的基于激光视觉融合的建图方法，其特征在于，所述建图系统还包括GPS传感器，则所述基于激光视觉融合的建图方法还包括：

接收所述GPS传感器采集到的GPS观测数据；

基于卡尔曼滤波算法，利用所述GPS观测数据对所述当前状态进行更新。

3.如权利要求1所述的基于激光视觉融合的建图方法，其特征在于，所述建图系统还包括陀螺仪，则，通过以下步骤对所述当前状态进行预测：

接收所述陀螺仪采集到的陀螺仪观测数据；

基于卡尔曼滤波算法和高阶Runge Kutta插值法，利用所述陀螺仪观测数据对所述建图系统的当前状态进行预测。

4.如权利要求1所述的基于激光视觉融合的建图方法，其特征在于，所述基于卡尔曼滤波算法，根据所述激光观测数据和所述视觉观测数据对所述建图系统的每一时刻的当前状态进行更新，包括：

当当前时刻接收到所述激光观测数据时，基于卡尔曼滤波算法，根据所述激光观测数据对所述建图系统的所述当前时刻的当前状态进行更新；

当当前时刻接收到所述视觉观测数据时，基于卡尔曼滤波算法，根据所述视觉观测数据对所述建图系统的所述当前时刻的当前状态进行更新；

当当前时刻同时接收到所述激光观测数据和所述视觉观测数据时，基于卡尔曼滤波算法，按照预设的顺序，先后分别根据所述激光观测数据和所述视觉观测数据对所述建图系统的所述当前时刻的当前状态进行更新。

5.如权利要求4所述的基于激光视觉融合的建图方法，其特征在于，基于卡尔曼滤波算法，根据所述激光观测数据对所述建图系统的当前状态进行更新，包括：

对每一帧所述激光点云图像进行预处理，提取每一帧所述激光点云图像的第一面特征点和第一线特征点；

将每一帧所述激光点云图像的坐标数据投影到陀螺仪坐标系下，将所述第一面特征点与所述局部点云地图中的第二面特征点进行匹配，并将所述第一线特征点与所述局部点云地图中的第二线特征点进行匹配；

利用匹配到的面特征点和线特征点对所述建图系统的当前状态进行更新。

6.如权利要求5所述的基于激光视觉融合的建图方法，其特征在于，所述利用匹配到的面特征点和线特征点对所述建图系统的当前状态进行更新，包括：

根据匹配到的面特征点，计算局部面片的面中心和法向量；

根据所述面中心和所述法向量计算第一激光误差；

对所述第一激光误差求一阶偏导，并线性化处理得到第一激光H矩阵；

根据匹配到的线特征点，计算局部线的线中心和线方向；

根据所述线中心和所述线方向计算第二激光误差；

对所述第二激光误差求一阶偏导，并线性化处理得到第二激光H矩阵；

将所述第一激光H矩阵和所述第二激光H矩阵进行叠加处理，得到激光H矩阵；

根据所述激光H矩阵更新所述当前状态的协方差；

根据更新后的所述协方差对所述当前状态进行更新。

7.如权利要求5所述的基于激光视觉融合的建图方法，其特征在于，所述对每一帧所述激光点云图像进行预处理，提取每一帧所述激光点云图像的第一面特征点和第一线特征点，包括：

根据每一帧所述激光点云图像的每一激光扫描线的曲率，提取每一帧所述激光点云图像的初始面特征点和初始线特征点；

统计每一帧所述激光点云图像的点云和所述局部点云地图中的点云在2D栅格下的分布，比较每一个所述2D栅格内的点云分布，得到动态物体所处的2D栅格，删除所述2D栅格中的所述动态物体的点云，以从所述初始面特征点和初始线特征点中过滤动态物体的特征点，得到每一帧所述激光点云图像的第一面特征点和第一线特征点。

8.如权利要求4所述的基于激光视觉融合的建图方法，其特征在于，基于卡尔曼滤波算法，根据所述视觉观测数据对所述建图系统的当前状态进行更新，包括：

获取所述视觉观测数据中的每一关键帧图像；

计算上一关键帧图像的每一像素的光度梯度，并将所述局部点云地图的点云投影到所述上一关键帧图像，得到每一像素的深度；

将光度梯度的模值大于预设的模值阈值的像素点作为追踪点，通过所述深度，将所述追踪点投影到当前关键帧图像，得到若干光度误差；

将每一所述光度误差作为每一残差，对其求一阶偏微分，并线性化处理，得到每一第一视觉H矩阵；

将所有所述第一视觉H矩阵进行叠加处理，得到视觉H矩阵；

根据所述视觉H矩阵更新所述当前状态的协方差；

根据更新后的所述协方差对所述当前状态进行更新。

9.如权利要求1所述的基于激光视觉融合的建图方法，其特征在于，所述当满足预设的停止更新条件时，基于所述局部点云地图对更新后的所有时刻的所述当前状态进行全局优化，生成全局地图，包括：

当满足预设的停止更新条件时，对所述局部点云地图进行回环检测，得到每一关键帧激光点云图像；

计算得到每一所述关键帧激光点云图像之间的相对位姿和所述相对位姿的协方差矩阵；

根据所述相对位姿、所述相对位姿的协方差矩阵对更新后的所有时刻的所述当前状态进行全局优化，生成全局地图。

10.一种基于激光视觉融合的建图系统，其特征在于，所述建图系统包括：激光传感器、视觉传感器和控制器，所述控制器被配置为：

接收所述激光传感器采集到的激光观测数据和所述视觉传感器采集到的视觉观测数据；

根据所述激光观测数据中的每一帧激光点云图像生成局部点云地图；

基于卡尔曼滤波算法，根据所述激光观测数据和所述视觉观测数据对所述建图系统的每一时刻的当前状态进行更新；

当满足预设的停止更新条件时，基于所述局部点云地图对更新后的所有时刻的所述当前状态进行全局优化，生成全局地图。

Images