一种融合ESKF，g2o和点云匹配的紧耦合建图方法

技术领域

本发明涉及移动建图技术领域，尤其涉及的是一种融合ESKF，g2o和点云匹配的紧耦合建图方法。

背景技术

移动建图相较于传统的建图方法来说，操作简便，作业效率高，是近年来比较热门的领域，并已广泛运用于自动驾驶，机器人SLAM(Simultaneous Localization andMapping，同步定位与地图构建)和BIM(Building Information Modelling，建筑信息建模)领域，展现出广阔的前景。

移动建图的误差主要存在两个方面，一个是前端点云匹配，另一个是后端对点云位姿的优化。现有的建图方法存在以下不足：较少提供初始值或提供的初始值不够准确，导致点云匹配收敛速度慢且最终匹配效果不好。

因此，现有技术还有待于改进和发展。

发明内容

本发明要解决的技术问题在于，针对现有技术的上述缺陷，提供一种融合ESKF，g2o和点云匹配的紧耦合建图方法，旨在解决现有技术中点云匹配收敛速度慢导致最终匹配效果不好的问题。

本发明解决技术问题所采用的技术方案如下：

一种融合ESKF，g2o和点云匹配的紧耦合建图方法，其中，包括步骤：

获取三维点云数据和惯性测量器件的输出数据；

根据所述惯性测量器件的输出数据和状态误差卡尔曼滤波方程，确定匹配初始值；

基于所述匹配初始值，对所述三维点云数据进行匹配，得到位姿关系；

基于图优化通用框架，对所述位姿关系进行优化，得到帧位姿；

基于所述帧位姿，对所述三维点云数据进行拼接，得到三维点云地图。

所述的融合ESKF，g2o和点云匹配的紧耦合建图方法，其中，所述匹配初始值包括：帧间匹配初始值和帧到图匹配初始值；所述位姿关系包括：帧间位姿关系和帧到图位姿关系；

所述基于所述匹配初始值，对所述三维点云数据进行匹配，得到位姿关系，包括：

基于所述帧间匹配初始值，对所述三维点云数据进行匹配，得到帧间位姿关系；

基于所述帧到图匹配初始值，对所述三维点云数据进行匹配，得到帧到图位姿关系。

所述的融合ESKF，g2o和点云匹配的紧耦合建图方法，其中，所述基于图优化通用框架，对所述位姿关系进行优化，得到帧位姿，包括：

将所述帧间位姿关系和所述帧到图位姿关系作为多边图的边，输入图优化通用框架进行图优化，得到帧位姿。

所述的融合ESKF，g2o和点云匹配的紧耦合建图方法，其中，所述基于所述帧位姿，对所述三维点云数据进行拼接，得到三维点云地图，包括：

基于所述帧位姿，对所述状态误差卡尔曼滤波方程进行更新，得到更新的状态误差卡尔曼滤波方程；

将所述更新的状态误差卡尔曼滤波方程作为状态误差卡尔曼滤波方程，继续执行根据所述惯性测量器件的输出数据和状态误差卡尔曼滤波方程，确定匹配初始值的步骤，直至满足预设条件，得到更新的帧位姿；

根据所述更新的帧位姿，对所述三维点云数据进行拼接，得到三维点云地图。

所述的融合ESKF，g2o和点云匹配的紧耦合建图方法，其中，所述三维点云数据包括至少两个时刻的三维点云数据；

所述基于所述帧位姿，对所述状态误差卡尔曼滤波方程进行更新，得到更新的状态误差卡尔曼滤波方程，包括：

根据所述帧位姿，确定状态量和理想状态量；

根据所述状态量和所述状态误差卡尔曼滤波方程确定下一时刻的预测状态量；

根据所述状态误差卡尔曼滤波方程，确定下一时刻的滤波增益；

根据所述预测状态量和所述滤波增益，确定下一时刻的状态误差；

根据所述理想状态量和所述状态误差，确定下一时刻的更新的状态量；

根据所述更新的状态量和所述状态量，确定更新的状态误差卡尔曼滤波方程。

所述的融合ESKF，g2o和点云匹配的紧耦合建图方法，其中，所述根据所述状态误差卡尔曼滤波方程，确定下一时刻的滤波增益，包括：

根据所述状态误差卡尔曼滤波方程，确定下一时刻的预测均方差；

根据所述预测均方差，确定下一时刻的滤波增益。

所述的融合ESKF，g2o和点云匹配的紧耦合建图方法，其中，所述预设条件包括：所有时刻的三维点云数据匹配完成。

所述的融合ESKF，g2o和点云匹配的紧耦合建图方法，其中，所述状态量包括：位值误差、与四元数相关的三维角度误差、速度误差、加速度计的零偏误差、陀螺仪的零偏误差中的一个或多个。

所述的融合ESKF，g2o和点云匹配的紧耦合建图方法，其中，所述状态误差卡尔曼滤波方程包括：状态误差卡尔曼滤波状态方程和状态误差卡尔曼滤波量测方程。

所述的融合ESKF，g2o和点云匹配的紧耦合建图方法，其中，所述三维点云数据为激光雷达输出的三维点云数据。

有益效果：由于使用状态误差卡尔曼滤波方程提供点云匹配的初始值进行匹配，并融合图优化通用框架进行优化，速度快，精度高，能有效提高点云匹配的精度和速度。

附图说明

图1是本发明中融合ESKF，g2o和点云匹配的紧耦合建图方法的第一流程图。

图2是本发明中融合ESKF，g2o和点云匹配的紧耦合建图方法的第二流程图。

具体实施方式

为使本发明的目的、技术方案及优点更加清楚、明确，以下参照附图并举实施例对本发明进一步详细说明。应当理解，此处所描述的具体实施例仅仅用以解释本发明，并不用于限定本发明。

请同时参阅图1-图2，本发明提供了一种融合ESKF，g2o和点云匹配的紧耦合建图方法的一些实施例。

如图1-图2所示，本发明的一种融合ESKF，g2o和点云匹配的紧耦合建图方法，包括以下步骤：

步骤S100、获取三维点云数据和惯性测量器件的输出数据。

具体地，三维点云数据和惯性测量器件的输出数据可以通过传感器采集，例如，所述三维点云数据为激光雷达输出的三维点云数据，也就是说，通过激光雷达采集三维点云数据。惯性测量器件(InertialMeasurementUnit，IMU)包括加速度计和陀螺仪，惯性测量器件的输出数据包括加速度计输出的数据和陀螺仪输出的数据。

在采集数据时，可以是采集一段时间的数据，那么可以将数据打上时刻，那么所述三维点云数据包括至少两个时刻的三维点云数据，惯性测量器件的输出数据包括至少两个时刻的惯性测量器件的输出数据。

步骤S200、根据所述惯性测量器件的输出数据和状态误差卡尔曼滤波方程，确定匹配初始值。

具体地，状态误差卡尔曼滤波(Error-StateKalmanFilter，ESKF)方程包括：状态误差卡尔曼滤波状态方程和状态误差卡尔曼滤波量测方程。状态误差卡尔曼滤波状态方程根据牛顿第二定律构建得到，其中，ESKF状态方程的状态量选取为：

x＝[δp δθ δv δβ

上式中，δp是位置误差，δθ是与四元数相关的三维角度误差，δv是速度误差，δβ

则，ESKF状态方程为：

上式中，

其中，v是值为(a，b，c)的任意向量，也就是说，v为三维向量，运算符[·]

所述ESKF量测方程的量测量选取为：

z＝[δp

其中，T表示矩阵的转置，下标M表示是相应状态量的量测值，例如δp

式中，p

则，ESKF量测方程为：

随后将其离散化，得到离散化的ESKF状态方程和离散化的ESKF量测方程：

上式中I是三维的单位矩阵，0

具体地，所述匹配初始值包括：帧间匹配初始值和帧到图匹配初始值，匹配初始值为下一帧点云的位姿矩阵，包括三维位置和旋转矩阵；以当前时刻k为例，ESKF提供滤波后的状态量为

步骤S300、基于所述匹配初始值，对所述三维点云数据进行匹配，得到位姿关系。

具体地，基于匹配初始值，对三维点云数据进行匹配，得到位姿关系。基于匹配初始值，采用点云匹配方法对三维点云数据进行匹配，得到位姿关系，点云匹配方法可采用流行的广义最近邻接点匹配算法(Generalized Iterative Closest Point,GICP)或正太分布变换匹配算法(Normal Distributions Transform,NDT)或自研匹配算法，本专利方法均可适用。

需要说明的是，在建图方法开始的时候，ESKF方程是初始化的，因此，此时，由ESKF方程提供的匹配初始值的准确性较低，因此，得到的位姿关系的准确性也较低。

具体地，步骤S300包括：

步骤S310、基于所述帧间匹配初始值，对所述三维点云数据进行匹配，得到帧间位姿关系。

步骤S320、基于所述帧到图匹配初始值，对所述三维点云数据进行匹配，得到帧到图位姿关系。

具体地，在匹配时，可以基于帧间匹配初始值和/或帧到图匹配初始值，对三维点云数据进行匹配，得到位姿关系。也就是说，可以分别基于帧间匹配初始值进行帧间匹配得到下一帧(时刻)与当前帧的位姿关系，帧到图匹配初始值进行帧到图的匹配得到下一帧(时刻)与当前地图的位姿关系，这种冗余的关系也是下一步进行图优化的前提条件。

步骤S400、基于图优化通用框架，对所述位姿关系进行优化，得到帧位姿。

具体地，图优化通用框架(General Framework for Graph Optimisation，g2o)将非线性优化与图论相结合。在使用g2o时，由于图是由节点和边组成的，首先以类的形式，定义节点和边。然后，根据节点和边构建图，并对其优化，得到帧位姿。

步骤S400包括：

步骤S410、将所述帧间位姿关系和所述帧到图位姿关系作为多边图的边，输入图优化通用框架进行图优化，得到帧位姿。

具体地，在使用g2o图优化得到帧位姿时，将帧间位姿关系和帧到图位姿关系作为多边图的边，输入图优化通用框架进行图优化，得到优化后的各帧位姿。

g2o优化具体可分为4个步骤，第一步为创建求解器，包括创建线性求解器，块求解器和总求解器，其中的算法可选择高斯牛顿法(Gauss-Newton)，莱文贝格马夸特法(Levenberg-Marquardt)或狗腿法(Dog-Leg)；第二步为创建稀疏优化器；第三步为定义图的顶点和边，并添加到第二步创建的稀疏优化器中，具体到本申请中图的顶点为各帧的位姿，边为匹配得到的帧间位姿关系和帧到图位姿关系；最后一步为设置优化参数，开始执行优化。

步骤S500、基于所述帧位姿，对所述三维点云数据进行拼接，得到三维点云地图。

具体地，基于帧位姿对三维点云数据进行拼接，得到三维点云地图。

步骤S500包括：

步骤S510、基于所述帧位姿，对所述状态误差卡尔曼滤波方程进行更新，得到更新的状态误差卡尔曼滤波方程。

具体地，通过g2o图优化得到帧位姿后，对状态误差卡尔曼滤波方程进行更新，得到更新的状态误差卡尔曼滤波方程，由于三维点云数据包括至少两个时刻的三维点云数据，惯性测量器件的输出数据包括至少两个时刻的惯性测量器件的输出数据，因此，可将更新的状态误差卡尔曼滤波方程应用于下一时刻。

步骤S510包括：

步骤S511、根据所述帧位姿，确定状态量和理想状态量。

所述状态量包括：位值误差、与四元数相关的三维角度误差、速度误差、加速度计的零偏误差、陀螺仪的零偏误差中的一个或多个。

步骤S512、根据所述状态量和所述状态误差卡尔曼滤波方程确定下一时刻的预测状态量。

步骤S513、根据所述状态误差卡尔曼滤波方程，确定下一时刻的滤波增益。

步骤S514、根据所述预测状态量和所述滤波增益，确定下一时刻的状态误差。

步骤S515、根据所述理想状态量和所述状态误差，确定下一时刻的更新的状态量。

步骤S516、根据所述更新的状态量和所述状态量，确定更新的状态误差卡尔曼滤波方程。

具体地，状态误差卡尔曼滤波方程的更新可以分为预测和更新两个部分：

1、预测：

1.1、一步预测：

1.2、求解一步预测均方差：

上式中，x

2、更新：

2.1、求解滤波增益：

2.2、更新状态误差：

2.3、更新均方差误差：

2.4、更新状态量：

2.5、状态误差置零：

x

上式中，(·)

其中，上标·表示求导，例如，

步骤S513包括：

步骤S5131、根据所述状态误差卡尔曼滤波方程，确定下一时刻的预测均方差。

步骤S5132、根据所述预测均方差，确定下一时刻的滤波增益。

具体地，如1.2、求解一步预测均方差所示得到预测均方差，并如2.1、求解滤波增益所示，得到滤波增益。

步骤S520、将所述更新的状态误差卡尔曼滤波方程作为状态误差卡尔曼滤波方程，继续执行根据所述惯性测量器件的输出数据和状态误差卡尔曼滤波方程，确定匹配初始值的步骤，直至满足预设条件，得到更新的帧位姿。

具体地，在得到更新的状态误差卡尔曼滤波方程后，并将更新的状态误差卡尔曼滤波方程作为状态误差卡尔曼滤波方程，继续惯性测量器件的输出数据和状态误差卡尔曼滤波方程，确定匹配初始值，也就是说，返回到步骤S200，直至满足预设条件，得到更新的帧位姿。

具体地，所述预设条件包括：循环次数达到预设次数，预设次数即为点云的帧数。此时所有时刻的三维点云数据匹配完成，也就是说，最后一帧激光雷达输出的三维点云数据完成匹配且g2o优化后得到更新的帧位姿。

步骤S530、根据所述更新的帧位姿，对所述三维点云数据进行拼接，得到三维点云地图。

具体地，根据更新的帧位姿，对三维点云数据进行拼接，得到三维点云地图。

与现有的建图技术相比，本发明提出了一种融合ESKF，g2o和点云匹配的紧耦合的建图方法具有如下技术效果：

1)使用ESKF提供点云匹配的初始值，该方法速度快，精度高，能有效提高点云匹配的精度和速度。

2)通过g2o对多边图进行优化且对ESKF进行更新，能有效减小匹配误差，提高建图质量。

需要说明的是，传统g2o优化方法是在图中所有边建好后才进行优化，如果在建多边图的过程中存在点云匹配的结果发散，会出现无法优化的情况。本发明方法综合考虑点云匹配和后端优化的过程，三维点云建图效果较好。

应当理解的是，本发明的应用不限于上述的举例，对本领域普通技术人员来说，可以根据上述说明加以改进或变换，所有这些改进和变换都应属于本发明所附权利要求的保护范围。