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

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

Info

Publication number
CN112489176A
CN112489176A CN202011351405.1A CN202011351405A CN112489176A CN 112489176 A CN112489176 A CN 112489176A CN 202011351405 A CN202011351405 A CN 202011351405A CN 112489176 A CN112489176 A CN 112489176A
Authority
CN
China
Prior art keywords
point cloud
matching
frame
pose
dimensional point
Prior art date
Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
Granted
Application number
CN202011351405.1A
Other languages
English (en)
Other versions
CN112489176B (zh
Inventor
史文中
包胜
范文诤
聂明炎
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Shenzhen Research Institute HKPU
Original Assignee
Shenzhen Research Institute HKPU
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by Shenzhen Research Institute HKPU filed Critical Shenzhen Research Institute HKPU
Priority to CN202011351405.1A priority Critical patent/CN112489176B/zh
Publication of CN112489176A publication Critical patent/CN112489176A/zh
Application granted granted Critical
Publication of CN112489176B publication Critical patent/CN112489176B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

Classifications

    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T17/00—Three dimensional [3D] modelling, e.g. data description of 3D objects
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T15/00—3D [Three Dimensional] image rendering

Abstract

本发明公开了一种融合ESKF，g2o和点云匹配的紧耦合建图方法，包括步骤：获取三维点云数据和惯性测量器件的输出数据；根据所述惯性测量器件的输出数据和状态误差卡尔曼滤波方程，确定匹配初始值；基于所述匹配初始值，对所述三维点云数据进行匹配，得到位姿关系；基于图优化通用框架，对所述位姿关系进行优化，得到帧位姿；基于所述帧位姿，对所述三维点云数据进行拼接，得到三维点云地图。由于使用状态误差卡尔曼滤波方程提供点云匹配的初始值进行匹配，并融合图优化通用框架进行优化，速度快，精度高，能有效提高点云匹配的精度和速度。

Description

一种融合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 δβa δβg]T
上式中，δp是位置误差，δθ是与四元数相关的三维角度误差，δv是速度误差，δβa是加速度计的零偏误差，δβg是陀螺仪的零偏误差。
则，ESKF状态方程为：
Figure BDA0002801414300000051
上式中，
Figure BDA0002801414300000052
是δp的导数，
Figure BDA0002801414300000053
是δv的导数，
Figure BDA0002801414300000054
是δθ的导数，
Figure BDA0002801414300000055
是δβa的导数，
Figure BDA0002801414300000056
是δβg的导数。am是三轴加计的输出，Rot是旋转矩阵，ωm是三轴陀螺仪的输出。an、ωn分别是加速度计的白噪声、陀螺仪的白噪声，na、ng分别是加速度计的高斯随机游走噪声、陀螺仪的高斯随机游走噪声。加速度g被认为是固定值而省略。运算符[·]×代表的运算如下所示：
Figure BDA0002801414300000057
其中，v是值为(a，b，c)的任意向量，也就是说，v为三维向量，运算符[·]×根据向量v中的各元素形成对称矩阵。
所述ESKF量测方程的量测量选取为：
z＝[δpM δθM δvM]T
其中，T表示矩阵的转置，下标M表示是相应状态量的量测值，例如δpM为位置误差δp的量测值。根据下式计算：
Figure BDA0002801414300000061
式中，p+，q+和v+分别是状态方程的预测位置，预测四元数和预测速度，pLiDAR，qLiDAR和vLiDAR分别是图优化通用框架(General Framework for Graph Optimisation，g2o)优化后得到的位置，四元数和速度。
Figure BDA0002801414300000062
是四元数的乘法，上标*表示共轭。
则，ESKF量测方程为：
Figure BDA0002801414300000063
随后将其离散化，得到离散化的ESKF状态方程和离散化的ESKF量测方程：
Figure BDA0002801414300000064
Figure BDA0002801414300000065
上式中I是三维的单位矩阵，03×3是三维的零矩阵，Δt是时间间隔，上标+表示为预测状态量，下标k表示在k时刻的状态，Fk表示在k时刻的状态转移矩阵，Hk+1表示在k+1时刻的量测矩阵，
Figure BDA0002801414300000066
表示k+1时刻状态量xk+1的真值。
具体地，所述匹配初始值包括：帧间匹配初始值和帧到图匹配初始值，匹配初始值为下一帧点云的位姿矩阵，包括三维位置和旋转矩阵；以当前时刻k为例，ESKF提供滤波后的状态量为
Figure BDA0002801414300000071
包括
Figure BDA0002801414300000072
依次为滤波后的位置，四元数，速度，加速度计的零偏和陀螺仪的零偏，选取滤波后的位置和四元数构成位姿矩阵，该位姿矩阵即为初始值。再如，以当前时刻k＝0为例，此时ESKF进行初始化，ESKF提供滤波后的状态量为
Figure BDA0002801414300000073
包括
Figure BDA0002801414300000074
滤波后的状态量
Figure BDA0002801414300000075
除四元数
Figure BDA0002801414300000076
外均初始化为0，
Figure BDA0002801414300000077
用IMU中姿态航向参考系统(Attitude and Heading Reference System,AHRS)得到的姿态进行初始化。所述位姿关系包括：帧间位姿关系和帧到图位姿关系。帧间位姿关系是基于帧间匹配初始值得到的，帧到图位姿关系是基于帧到图匹配初始值得到的。
步骤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、一步预测：
Figure BDA0002801414300000091
1.2、求解一步预测均方差：
Figure BDA0002801414300000101
上式中，xk是k时刻状态量；上标T表示矩阵的转置；上标+表示为预测状态量，例如，
Figure BDA0002801414300000102
是k+1时刻预测状态量；Fk表示在k时刻的状态转移矩阵，如步骤200进行计算；Qk表示在k时刻均值为0的系统噪声矩阵；Pk表示在k时刻的均方差误差矩阵。需要说明的是，这里的均值是指算术平均值，算术平均值＝系统噪声矩阵中所有元素之和/系统噪声矩阵中所有元素的数量，当然在系统噪声矩阵中所有元素之和为0时，算术平均值为0。
2、更新：
2.1、求解滤波增益：
Figure BDA0002801414300000103
2.2、更新状态误差：
Figure BDA0002801414300000104
2.3、更新均方差误差：
Figure BDA0002801414300000105
2.4、更新状态量：
Figure BDA0002801414300000106
2.5、状态误差置零：
xk+1＝0
上式中，(·)-1表示对矩阵求逆；Rk+1表示在k+1时刻均值为0的量测噪声矩阵；Hk+1表示在k+1时刻的量测矩阵，如步骤200进行计算；Kk+1表示在k+1时刻的滤波增益矩阵；I15×15是十五维的单位矩阵；
Figure BDA0002801414300000107
代表加法操作和四元数的乘法操作；上标～表示滤波后的状态量，例如，
Figure BDA0002801414300000108
是在k+1时刻滤波后的状态量。
Figure BDA0002801414300000109
是理想状态量，
Figure BDA00028014143000001010
Figure BDA00028014143000001011
是理想的位置，
Figure BDA00028014143000001012
是理想的与四元数，
Figure BDA00028014143000001013
是理想的速度，
Figure BDA00028014143000001014
是理想的加速度计的零偏，
Figure BDA00028014143000001015
是理想的陀螺仪的零偏，理想状态量
Figure BDA00028014143000001016
可由下式计算：
Figure BDA0002801414300000111
其中，上标·表示求导，例如，
Figure BDA0002801414300000112
是
Figure BDA0002801414300000113
的导数，
Figure BDA0002801414300000114
是
Figure BDA0002801414300000115
的导数，
Figure BDA0002801414300000116
是
Figure BDA0002801414300000117
的导数，
Figure BDA0002801414300000118
是
Figure BDA0002801414300000119
的导数，
Figure BDA00028014143000001110
是
Figure BDA00028014143000001111
的导数，g是重力加速度，am是加速度计的输出，例如可以是三轴加速度计的输出，Rot是旋转矩阵，ωm是三轴陀螺仪的输出。
步骤S513包括：
步骤S5131、根据所述状态误差卡尔曼滤波方程，确定下一时刻的预测均方差。
步骤S5132、根据所述预测均方差，确定下一时刻的滤波增益。
具体地，如1.2、求解一步预测均方差所示得到预测均方差，并如2.1、求解滤波增益所示，得到滤波增益。
步骤S520、将所述更新的状态误差卡尔曼滤波方程作为状态误差卡尔曼滤波方程，继续执行根据所述惯性测量器件的输出数据和状态误差卡尔曼滤波方程，确定匹配初始值的步骤，直至满足预设条件，得到更新的帧位姿。
具体地，在得到更新的状态误差卡尔曼滤波方程后，并将更新的状态误差卡尔曼滤波方程作为状态误差卡尔曼滤波方程，继续惯性测量器件的输出数据和状态误差卡尔曼滤波方程，确定匹配初始值，也就是说，返回到步骤S200，直至满足预设条件，得到更新的帧位姿。
具体地，所述预设条件包括：循环次数达到预设次数，预设次数即为点云的帧数。此时所有时刻的三维点云数据匹配完成，也就是说，最后一帧激光雷达输出的三维点云数据完成匹配且g2o优化后得到更新的帧位姿。
步骤S530、根据所述更新的帧位姿，对所述三维点云数据进行拼接，得到三维点云地图。
具体地，根据更新的帧位姿，对三维点云数据进行拼接，得到三维点云地图。
与现有的建图技术相比，本发明提出了一种融合ESKF，g2o和点云匹配的紧耦合的建图方法具有如下技术效果：
1)使用ESKF提供点云匹配的初始值，该方法速度快，精度高，能有效提高点云匹配的精度和速度。
2)通过g2o对多边图进行优化且对ESKF进行更新，能有效减小匹配误差，提高建图质量。
需要说明的是，传统g2o优化方法是在图中所有边建好后才进行优化，如果在建多边图的过程中存在点云匹配的结果发散，会出现无法优化的情况。本发明方法综合考虑点云匹配和后端优化的过程，三维点云建图效果较好。
应当理解的是，本发明的应用不限于上述的举例，对本领域普通技术人员来说，可以根据上述说明加以改进或变换，所有这些改进和变换都应属于本发明所附权利要求的保护范围。

Claims (10)

1.一种融合ESKF，g2o和点云匹配的紧耦合建图方法，其特征在于，包括步骤：
获取三维点云数据和惯性测量器件的输出数据；
根据所述惯性测量器件的输出数据和状态误差卡尔曼滤波方程，确定匹配初始值；
基于所述匹配初始值，对所述三维点云数据进行匹配，得到位姿关系；
基于图优化通用框架，对所述位姿关系进行优化，得到帧位姿；
基于所述帧位姿，对所述三维点云数据进行拼接，得到三维点云地图。
2.根据权利要求1所述的融合ESKF，g2o和点云匹配的紧耦合建图方法，其特征在于，所述匹配初始值包括：帧间匹配初始值和帧到图匹配初始值；所述位姿关系包括：帧间位姿关系和帧到图位姿关系；
所述基于所述匹配初始值，对所述三维点云数据进行匹配，得到位姿关系，包括：
基于所述帧间匹配初始值，对所述三维点云数据进行匹配，得到帧间位姿关系；
基于所述帧到图匹配初始值，对所述三维点云数据进行匹配，得到帧到图位姿关系。
3.根据权利要求2所述的融合ESKF，g2o和点云匹配的紧耦合建图方法，其特征在于，所述基于图优化通用框架，对所述位姿关系进行优化，得到帧位姿，包括：
将所述帧间位姿关系和所述帧到图位姿关系作为多边图的边，输入图优化通用框架进行图优化，得到帧位姿。
4.根据权利要求1所述的融合ESKF，g2o和点云匹配的紧耦合建图方法，其特征在于，所述基于所述帧位姿，对所述三维点云数据进行拼接，得到三维点云地图，包括：
基于所述帧位姿，对所述状态误差卡尔曼滤波方程进行更新，得到更新的状态误差卡尔曼滤波方程；
将所述更新的状态误差卡尔曼滤波方程作为状态误差卡尔曼滤波方程，继续执行根据所述惯性测量器件的输出数据和状态误差卡尔曼滤波方程，确定匹配初始值的步骤，直至满足预设条件，得到更新的帧位姿；
根据所述更新的帧位姿，对所述三维点云数据进行拼接，得到三维点云地图。
5.根据权利要求4所述的融合ESKF，g2o和点云匹配的紧耦合建图方法，其特征在于，所述三维点云数据包括至少两个时刻的三维点云数据；
所述基于所述帧位姿，对所述状态误差卡尔曼滤波方程进行更新，得到更新的状态误差卡尔曼滤波方程，包括：
根据所述帧位姿，确定状态量和理想状态量；
根据所述状态量和所述状态误差卡尔曼滤波方程确定下一时刻的预测状态量；
根据所述状态误差卡尔曼滤波方程，确定下一时刻的滤波增益；
根据所述预测状态量和所述滤波增益，确定下一时刻的状态误差；
根据所述理想状态量和所述状态误差，确定下一时刻的更新的状态量；
根据所述更新的状态量和所述状态量，确定更新的状态误差卡尔曼滤波方程。
6.根据权利要求5所述的融合ESKF，g2o和点云匹配的紧耦合建图方法，其特征在于，所述根据所述状态误差卡尔曼滤波方程，确定下一时刻的滤波增益，包括：
根据所述状态误差卡尔曼滤波方程，确定下一时刻的预测均方差；
根据所述预测均方差，确定下一时刻的滤波增益。
7.根据权利要求4所述的融合ESKF，g2o和点云匹配的紧耦合建图方法，其特征在于，所述预设条件包括：所有时刻的三维点云数据匹配完成。
8.根据权利要求4所述的融合ESKF，g2o和点云匹配的紧耦合建图方法，其特征在于，所述状态量包括：位值误差、与四元数相关的三维角度误差、速度误差、加速度计的零偏误差、陀螺仪的零偏误差中的一个或多个。
9.根据权利要求4所述的融合ESKF，g2o和点云匹配的紧耦合建图方法，其特征在于，所述状态误差卡尔曼滤波方程包括：状态误差卡尔曼滤波状态方程和状态误差卡尔曼滤波量测方程。
10.根据权利要求1-9任意一项所述的融合ESKF，g2o和点云匹配的紧耦合建图方法，其特征在于，所述三维点云数据为激光雷达输出的三维点云数据。
CN202011351405.1A 2020-11-26 2020-11-26 一种融合ESKF，g2o和点云匹配的紧耦合建图方法 Active CN112489176B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202011351405.1A CN112489176B (zh) 2020-11-26 2020-11-26 一种融合ESKF，g2o和点云匹配的紧耦合建图方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202011351405.1A CN112489176B (zh) 2020-11-26 2020-11-26 一种融合ESKF，g2o和点云匹配的紧耦合建图方法

Publications (2)

Publication Number Publication Date
CN112489176A true CN112489176A (zh) 2021-03-12
CN112489176B CN112489176B (zh) 2021-09-21

Family

ID=74935597

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202011351405.1A Active CN112489176B (zh) 2020-11-26 2020-11-26 一种融合ESKF，g2o和点云匹配的紧耦合建图方法

Country Status (1)

Country Link
CN (1) CN112489176B (zh)

Cited By (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN113267178A (zh) * 2021-03-25 2021-08-17 浙江大学 一种基于多传感器融合的模型位姿测量系统与方法

Citations (6)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN103644903A (zh) * 2013-09-17 2014-03-19 北京工业大学 基于分布式边缘无味粒子滤波的同步定位与地图构建方法
CN105953796A (zh) * 2016-05-23 2016-09-21 北京暴风魔镜科技有限公司 智能手机单目和imu融合的稳定运动跟踪方法和装置
US20170067739A1 (en) * 2014-05-05 2017-03-09 Hexagon Technology Center Gmbh Surveying system
CN106780699A (zh) * 2017-01-09 2017-05-31 东南大学 一种基于sins/gps和里程计辅助的视觉slam方法
CN108225327A (zh) * 2017-12-31 2018-06-29 芜湖哈特机器人产业技术研究院有限公司 一种顶标地图的构建与定位方法
CN110879400A (zh) * 2019-11-27 2020-03-13 炬星科技（深圳）有限公司 激光雷达与imu融合定位的方法、设备及存储介质

Patent Citations (6)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN103644903A (zh) * 2013-09-17 2014-03-19 北京工业大学 基于分布式边缘无味粒子滤波的同步定位与地图构建方法
US20170067739A1 (en) * 2014-05-05 2017-03-09 Hexagon Technology Center Gmbh Surveying system
CN105953796A (zh) * 2016-05-23 2016-09-21 北京暴风魔镜科技有限公司 智能手机单目和imu融合的稳定运动跟踪方法和装置
CN106780699A (zh) * 2017-01-09 2017-05-31 东南大学 一种基于sins/gps和里程计辅助的视觉slam方法
CN108225327A (zh) * 2017-12-31 2018-06-29 芜湖哈特机器人产业技术研究院有限公司 一种顶标地图的构建与定位方法
CN110879400A (zh) * 2019-11-27 2020-03-13 炬星科技（深圳）有限公司 激光雷达与imu融合定位的方法、设备及存储介质

Non-Patent Citations (2)

* Cited by examiner, † Cited by third party
Title
J SOLA: "Quaternion kinematics for the error-state KF", 《TECHNICAL REPORT》 *
ORB-SLAM2源码学习（八）——图优化G2O: "ORB-SLAM2源码学习（八）——图优化g2o", 《HTTPS://WWW.IT610.COM/ARTICLE/1287856078595338240.HTM》 *

Cited By (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN113267178A (zh) * 2021-03-25 2021-08-17 浙江大学 一种基于多传感器融合的模型位姿测量系统与方法

Also Published As

Publication number Publication date
CN112489176B (zh) 2021-09-21

Similar Documents

Publication Publication Date Title
Qin et al. Lins: A lidar-inertial state estimator for robust and efficient navigation
Ye et al. Tightly coupled 3d lidar inertial odometry and mapping
Heo et al. EKF-based visual inertial navigation using sliding window nonlinear optimization
Lupton et al. Visual-inertial-aided navigation for high-dynamic motion in built environments without initial conditions
WO2020087846A1 (zh) 基于迭代扩展卡尔曼滤波融合惯性与单目视觉的导航方法
Xiong et al. G-VIDO: A vehicle dynamics and intermittent GNSS-aided visual-inertial state estimator for autonomous driving
CN110726406A (zh) 一种改进的非线性优化单目惯导slam的方法
Kang et al. Vins-vehicle: A tightly-coupled vehicle dynamics extension to visual-inertial state estimator
CN110929402A (zh) 一种基于不确定分析的概率地形估计方法
CN114485643B (zh) 一种煤矿井下移动机器人环境感知与高精度定位方法
Chen et al. Stereo visual inertial pose estimation based on feedforward-feedback loops
CN115046545A (zh) 一种深度网络与滤波结合的定位方法
CN112489176B (zh) 一种融合ESKF，g2o和点云匹配的紧耦合建图方法
Zheng et al. SE (2)-constrained visual inertial fusion for ground vehicles
CN113091754B (zh) 一种非合作航天器位姿一体化估计和惯性参数确定方法
Mo et al. Continuous-time spline visual-inertial odometry
Barrau et al. Invariant filtering for pose ekf-slam aided by an imu
CN116429116A (zh) 一种机器人定位方法及设备
CN114993338B (zh) 基于多段独立地图序列的一致性高效视觉惯性里程计算法
CN115984463A (zh) 一种适用于狭窄巷道的三维重建方法和系统
CN115797490A (zh) 基于激光视觉融合的建图方法及系统
Liu et al. Integrating point and line features for visual-inertial initialization
Indelman Navigation performance enhancement using online mosaicking
CN115344033A (zh) 一种基于单目相机/imu/dvl紧耦合的无人船导航与定位方法
Thalagala Comparison of state marginalization techniques in visual inertial navigation filters

Legal Events

Date Code Title Description
PB01 Publication
PB01 Publication
SE01 Entry into force of request for substantive examination
SE01 Entry into force of request for substantive examination
GR01 Patent grant
GR01 Patent grant