CN116452763A - 一种基于误差卡尔曼滤波和因子图的三维点云地图构建方法 - Google Patents

一种基于误差卡尔曼滤波和因子图的三维点云地图构建方法 Download PDF

Info

Publication number
CN116452763A
CN116452763A CN202310407457.3A CN202310407457A CN116452763A CN 116452763 A CN116452763 A CN 116452763A CN 202310407457 A CN202310407457 A CN 202310407457A CN 116452763 A CN116452763 A CN 116452763A
Authority
CN
China
Prior art keywords
pose
point cloud
frame
error
gnss
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.)
Pending
Application number
CN202310407457.3A
Other languages
English (en)
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.)
Zhejiang University ZJU
Original Assignee
Zhejiang University ZJU
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 Zhejiang University ZJU filed Critical Zhejiang University ZJU
Priority to CN202310407457.3A priority Critical patent/CN116452763A/zh
Publication of CN116452763A publication Critical patent/CN116452763A/zh
Pending legal-status Critical Current

Links

Classifications

    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/38—Electronic maps specially adapted for navigation; Updating thereof
    • G01C21/3804—Creation or updating of map data
    • G01C21/3833—Creation or updating of map data characterised by the source of data
    • G01C21/3841—Data obtained from two or more sources, e.g. probe vehicles
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06F—ELECTRIC DIGITAL DATA PROCESSING
    • G06F17/00—Digital computing or data processing equipment or methods, specially adapted for specific functions
    • G06F17/10—Complex mathematical operations
    • G06F17/16—Matrix or vector computation, e.g. matrix-matrix or matrix-vector multiplication, matrix factorization
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06N—COMPUTING ARRANGEMENTS BASED ON SPECIFIC COMPUTATIONAL MODELS
    • G06N7/00—Computing arrangements based on specific mathematical models
    • G06N7/01—Probabilistic graphical models, e.g. probabilistic networks
    • 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
    • G06T17/05—Geographic models
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T3/00—Geometric image transformations in the plane of the image
    • G06T3/40—Scaling of whole images or parts thereof, e.g. expanding or contracting
    • G06T3/4038—Image mosaicing, e.g. composing plane images from plane sub-images
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T7/00—Image analysis
    • G06T7/20—Analysis of motion
    • G06T7/277—Analysis of motion involving stochastic approaches, e.g. using Kalman filters
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T7/00—Image analysis
    • G06T7/70—Determining position or orientation of objects or cameras
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T2207/00—Indexing scheme for image analysis or image enhancement
    • G06T2207/10—Image acquisition modality
    • G06T2207/10028—Range image; Depth image; 3D point clouds
    • Y—GENERAL TAGGING OF NEW TECHNOLOGICAL DEVELOPMENTS; GENERAL TAGGING OF CROSS-SECTIONAL TECHNOLOGIES SPANNING OVER SEVERAL SECTIONS OF THE IPC; TECHNICAL SUBJECTS COVERED BY FORMER USPC CROSS-REFERENCE ART COLLECTIONS [XRACs] AND DIGESTS
    • Y02—TECHNOLOGIES OR APPLICATIONS FOR MITIGATION OR ADAPTATION AGAINST CLIMATE CHANGE
    • Y02T—CLIMATE CHANGE MITIGATION TECHNOLOGIES RELATED TO TRANSPORTATION
    • Y02T10/00—Road transport of goods or passengers
    • Y02T10/10—Internal combustion engine [ICE] based vehicles
    • Y02T10/40—Engine management systems

Landscapes

  • Engineering & Computer Science (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Theoretical Computer Science (AREA)
  • Remote Sensing (AREA)
  • Software Systems (AREA)
  • Mathematical Physics (AREA)
  • Data Mining & Analysis (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Pure & Applied Mathematics (AREA)
  • Mathematical Analysis (AREA)
  • Mathematical Optimization (AREA)
  • Computational Mathematics (AREA)
  • Geometry (AREA)
  • Computing Systems (AREA)
  • Computer Vision & Pattern Recognition (AREA)
  • Algebra (AREA)
  • General Engineering & Computer Science (AREA)
  • Multimedia (AREA)
  • Computer Graphics (AREA)
  • Automation & Control Theory (AREA)
  • Databases & Information Systems (AREA)
  • Probability & Statistics with Applications (AREA)
  • Artificial Intelligence (AREA)
  • Evolutionary Computation (AREA)
  • Navigation (AREA)

Abstract

本发明公开了一种基于误差卡尔曼滤波和因子图的三维点云地图构建方法，该方法首先利用IMU获得的加速度和角速度进行积分得到先验位姿，联合激光雷达帧和局部地图配准距离的观测进行误差卡尔曼滤波，经过多次迭代以后获得最优位姿，在后端优化过程中使用前端里程计获得的位姿构建因子图，同时加入回环检测因子和GNSS因子，实现了多传感器的融合和联合优化，最终通过优化后的位姿将点云帧拼接在一起获得三维点云地图。本发明构建的三维点云地图具有更高的精度和鲁棒性，并且本发明的点云地图在不损失实时性效率的情况下具有更大的密度，同时由于后端使用因子图优化，加入其他类型的传感器也是可行的，具有良好的可扩展性。

Description

一种基于误差卡尔曼滤波和因子图的三维点云地图构建方法
技术领域
本发明涉及自动驾驶技术领域，具体涉及一种基于误差卡尔曼滤波和因子图的三维点云地图构建方法。
背景技术
近年来，随着智能化概念的兴起以及新能源汽车等技术的发展，自动驾驶逐渐成为未来汽车行业发展的新趋势。自动驾驶技术的发展将引发交通出行方面的重大变革,对人们生活的各个方面都会产生重大影响。随着社会数字化的推进，数字化自动驾驶将逐渐代替传统单车自动驾驶，使自动驾驶可以真正走向应用领域，创造实用价值。同时定位与建图(simultaneous localization and mapping)技术是自动驾驶的关键技术之一。目前主流的slam分为两种：基于摄像头的视觉slam和基于激光雷达的激光slam。视觉slam由于摄像头的硬件的固有特性，对于光照和天气等条件十分敏感，在实际场景使用中遇到诸多难题，目前难以进行产业落地，而激光雷达可以从环境中获取精确的距离及强度信息，激光slam系统具有更高的鲁棒性和准确性，一直都是自动驾驶slam领域研究的重点。基于激光雷达的同步定位和建图及定位技术是无人驾驶的关键技术，被广泛应用于高精度地图构建及基于地图的匹配定位。
目前主流的激光雷达slam方案主要有两种，一种是基于滤波的方案，这种方案主要以卡尔曼滤波及其变种为基础，将激光雷达里程计和IMU的信息相结合，获得最终的融合里程计，但这种方案没有后端处理，在长距离的建图过程中很容易积累误差，尽管可以通过回环检测进行纠正，但仍会影响最终的建图质量，而且这种方案难以和其他传感器进行融合从而进一步提高精度；另一种是基于优化的方案，这种方案一般是将部分前端里程计作为关键帧储存起来，同时储存其他传感器的信息，最后将这些信息汇总一起进行优化，但是目前用于图优化的前端里程计的精度普遍不高，结果导致图优化后的地图精度也不高。
发明内容
为解决现有技术中的问题，本发明提出了一种基于误差卡尔曼滤波和因子图的三维点云地图构建方法，该方法利用激光雷达帧信息、IMU数据和GNSS信息，将迭代误差卡尔曼滤波和因子图优化相结合，构建高精度点云地图，减少累计误差。具体而言，在本发明中，分别对前端里程计，后端优化这两个slam架构中最基本的模块进行考虑，结合迭代误差卡尔曼滤波和因子图优化，提出了一种新的三维高精点云地图构建方案。
本发明的基于误差卡尔曼滤波和因子图的三维点云地图构建方法，包括如下步骤：
1)按照时间序列接收激光雷达帧信息、IMU数据、GNSS信息；
2)将一个雷达帧内的IMU信息中的加速度和角速度进行积分，获得这一帧时间内的位姿变化，并将其加到上一帧的估计位姿上，作为此时刻位姿的先验；
3)以先验位姿为基础，将新的激光雷达帧和先前构建的局部地图进行匹配，新的点云中的点和对应面的距离作为残差向量，用迭代误差卡尔曼滤波进行迭代优化后获得前端里程计的最优位姿以及此时刻最优位姿对应的协方差矩阵；
4)使用滤波得到的最优位姿和协方差矩阵构建因子图，同时判断每一帧是否会和先前帧构成回环，如果出现回环将回环因子加入因子图中；将GNSS信息同样加入因子图中，进行联合优化，获得更加准确的位姿；使用优化后的位姿将激光雷达点云帧拼接起来构成三维点云地图。
作为本发明的优选方案，所述的步骤2)具体为：
在接收到步骤1)的信息和数据后，将激光雷达帧和IMU数据的时间戳进行对齐，找出在一个激光雷达帧中的所有IMU信息，使用IMU观测的信息进行运动学模拟，包括位置p、姿态R、激光雷达与IMU之间的相对位姿、速度v、陀螺仪零偏bg、加速度计零偏ba和重力g，其中，激光雷达与IMU之间的相对位姿由相对位置tbl和相对姿态Rbl组成；将这些物理量写成统一的状态量x：
在i时刻的状态量为xi，通过IMU读取此时刻的角速度ω和加速度a，将二者合并成一个矩阵其中角速度对应的白噪声和零偏噪声分别为ng和/>加速度对应的白噪声和零偏噪声分别为na和/>这四种噪声均为高斯分布，将四种噪声合并成一个噪声矩阵/>依靠这几个量递推得到Δt时间后i+1时刻的状态量，
其中f(xi,ui,wi)代表状态量变化率函数；代表状态量的相加；
在实际过程中，噪声的大小是无法准确获得的，因此每一次递推得到的状态量都是有误差的。然而真实值是定义在流形空间的随机变量，其协方差难以定义，但真实值中的误差量是定义在流形切空间上的随机变量，且服从高斯分布，更容易处理，剩下的部分被称作名义量，是一个确定值。所以可以将真实值中的误差量剥离出来再进行卡尔曼滤波，也就是迭代误差卡尔曼滤波。
在迭代误差卡尔曼滤波的预测过程中，将加速度和角速度的噪声置零，即可得到i+1时刻的名义量名义量作为后续的观测的先验位姿：
误差量服从均值为0，方差为自身协方差矩阵的高斯分布；名义量更新后，误差量的协方差也随之更新；将误差量进行一阶泰勒展开得到：
其中和Fw分别是误差量/>对/>和wi的一阶偏导；i时刻的误差量/>的协方差矩阵为/>i+1时刻噪声的协方差矩阵为W，因此i+1时刻的协方差/>为：
根据本发明的优选方案所述的步骤3)中：在接收到步骤2)的信息后，在局部地图中找出距离新激光雷达点云中的点最近的五个点，并计算这个点与对应的五个点构成的平面的距离，构建成一个残差向量，随后在先验位姿的基础上迭代求解点云帧的点和对应平面的距离最小时的状态量，获得的结果便是前端里程计获得的最优位姿。
根据本发明的优选方案，所述的步骤3)具体为：
假设某时刻的状态量为x，此时刻点云帧中的点与对应平面的点面距离残差观测为z，观测z由状态量x和观测噪声v决定；点面距离函数根据此时刻的状态量和点云帧计算出残差向量，将函数/>进行一阶泰勒展开获得线性化的表示：
其中，H为函数对误差量/>的一阶导数。基于状态量计算出的观测量与实际的观测之间必然存在差异，这个差距来自两个方面，一方面是来自状态估计的误差/>另一方面是来自测量的误差v；在误差量为/>的情况下，基于状态的观测z和实际观测zobs相同的概率为：
其中，代表状态量的相加；根据贝叶斯公式，在观测z＝zobs的前提下，状态误差量为/>的后验概率为：
其中是先验概率；随后可以通过最大化后验概率(MAP)得到误差量的最优解并更新协方差矩阵为/>
其中将/>和/>相加得到更新后的状态量xo即为最优位姿，并且最优位姿服从协方差为/>的高斯分布；由此便完成了一次滤波优化，获得了世界坐标系下的最优位姿xo以及其对应的协方差矩阵/>通过这个最优位姿将去畸变后的点云帧投影到世界坐标系中，并合并到局部地图中，供下一次的点云配准使用，协方差矩阵将传递到下一帧辅助进行下一次的滤波过程。
根据本发明的优选方案，所述的步骤4)具体包括如下步骤：
4.1)接收每一帧的位姿，将每个帧结束时刻的位姿作为图中的顶点，将每两帧之间的运动变化作为图中的边；
4.2)同时使用GICP检测当前关键帧是否和之前的关键帧构成回环，当检测成功时将这两帧之间的位姿变换作为新的边加入因子图中；
4.3)接收到GNSS信息后，首先将其GNSS的信息转换到ENU坐标系下，再寻找时间上最接近的雷达里程计关键帧，使用线性插值其时间戳对齐；当再次收到GNSS信息时，计算出两次信息之间的相对位置，并将这种约束作为边加入因子图中；加入所有待优化的顶点和边后求解图对应的目标函数即获得一系列更为准确的位姿，从而可以拼接成更加准确的三维点云地图。
根据本发明的优选方案，所述的步骤4.1)具体为：经步骤1)-步骤3)获得了每一帧的位姿，将所有帧通过时间和距离的限制进行筛选，留下一部分既可以充分表示运动信息又不会需要过多计算资源的关键帧，将这些关键帧按时间顺序进行编号，以便和回环因子和GNSS因子进行时间戳对齐，之后以每一帧结束的位姿作为图的顶点，以相邻两帧之间的位姿变换作为边，构建出因子图进行优化。
根据本发明的优选方案，所述的步骤4.3)具体为：当接收GNSS测量值时，首先将其转换为本地笛卡尔坐标系。在因子图中添加新节点后，将新的GNSS因子与该节点相关联；如果GNSS信号没有与激光雷达帧的时间戳同步，会根据激光雷达帧的时间戳对GNSS测量值进行线性插值；在经过一个GNSS接收周期后，会再次获得一个GNSS信息，经过上面同样的步骤后，因子图中会有两个GNSS因子，和激光雷达里程计因子处于相同的时间戳上，这样就可以根据这两帧GNSS信息之间的相对距离，对整体进行约束。
与现有技术相比，本发明提供了一种基于迭代误差状态卡尔曼滤波和因子图优化的三维点云地图实时构建方法，该方法首先利用IMU获得的运动加速度和角速度进行积分得到位姿先验，联合激光雷达帧和局部地图配准距离的观测进行卡尔曼滤波，经过多次迭代以后获得最优解，在后端优化过程中使用前端里程计获得的位姿构建因子图，同时加入回环检测因子和GNSS因子，实现了多传感器的融合和联合优化。实验证明，本发明构建的三维点云地图比其他点云地图构建方法具有更高的精度和鲁棒性，在地图质量是有较大提高，并且本发明的点云地图在不损失实时性效率的情况下具有更大的密度，同时由于后端使用因子图优化，加入其他类型的传感器也是可行的，具有良好的可扩展性。
附图说明
图1为一种基于误差卡尔曼滤波和因子图的三维点云地图构建方法示意图；
图2为使用本发明建立的三维点云地图；
图3为本发明与其他两种三维点云地图构建方法的精度对比。
具体实施方式
下面结合具体实施方式对本发明做进一步阐述和说明。所述实施例仅是本公开内容的示范且不圈定限制范围。本发明中各个实施方式的技术特征在没有相互冲突的前提下，均可进行相应组合。
三维点云地图的构建是自动驾驶的关键技术，在具有高精度的三维点云地图后才能进行下游的定位循迹等模块，为了增加点云地图的精度和建图过程的鲁棒性，本发明首先选择精度更高全点匹配，不再进行特征选取，之后通过IMU积分获得的先验和激光雷达点云匹配的残差并通过多次迭代获得最优解。在后端优化部分将前端里程计和其他的传感器信息连同这些信息的协方差一起构建出因子图，并在所有帧都结束后一起优化所有信息，获得更加准确的路径，从而获得更加准确的高精点云地图。
本发明可以分为以下几个步骤：
1)按照时间序列接收激光雷达帧信息、IMU数据、GNSS信息；
2)将一个雷达帧内的IMU信息中的加速度和角速度进行积分，获得这一帧时间内的位姿变化，并将其加到上一帧的估计位姿上，作为此时刻位姿的先验；
3)以先验位姿为基础，将新的激光雷达帧和先前构建的局部地图进行匹配，新的点云中的点和对应面的距离作为残差向量，用迭代误差卡尔曼滤波进行迭代优化后获得前端里程计的最优位姿以及此时刻最优位姿对应的协方差矩阵；
4)使用滤波得到的最优位姿和协方差矩阵构建因子图，同时判断每一帧是否会和先前帧构成回环，如果出现回环将回环因子加入因子图中；将GNSS信息同样加入因子图中，进行联合优化，获得更加准确的位姿；使用优化后的位姿将激光雷达点云帧拼接起来构成三维点云地图。
步骤2)中，在接收到传感器的信息后，需要将激光雷达帧和IMU的时间戳进行对齐，找出在一个激光雷达帧中的所有IMU信息，使用IMU观测的信息进行运动模拟，包括位置p、姿态R、激光雷达与IMU之间的相对位姿(由相对位置tbl和相对姿态Rbl组成)、速度v、陀螺仪零偏bg、加速度计零偏ba和重力g；将这些物理量写成统一的状态量x：
在i时刻的状态量为xi，通过IMU读取此时刻的角速度ω和加速度a，将二者合并成一个矩阵其中角速度对应的白噪声和零偏噪声分别为nh和/>加速度对应的白噪声和零偏噪声分别为na和/>这四种噪声均为高斯分布，将四种噪声合并成一个噪声矩阵/>依靠这几个量递推得到Δt时间后i+1时刻的状态量，其中f(xi,ui,wi)代表状态量变化率函数。
由于状态量中姿态的变换不能直接使用加法而是需要旋转矩阵相乘，所以使用广义加法符号代表状态量的相加，广义减法符号/>同理。在实际过程中，噪声的大小是无法准确获得的，因此每一次递推得到的状态量都是有误差的。然而真实值是定义在流形空间的随机变量，其协方差难以定义，但真实值中的误差量是定义在流形切空间上的随机变量，且服从高斯分布，更容易处理，剩下的部分被称作名义量，是一个确定值。所以可以将真实值中的误差量剥离出来再进行卡尔曼滤波，也就是迭代误差卡尔曼滤波。
在迭代误差卡尔曼滤波的预测过程中，将加速度和角速度的噪声置零，即可得到i+1时刻的名义量名义量作为后续的观测的先验位姿：
误差量服从均值为0，方差为自身协方差矩阵的高斯分布。名义量更新后，误差量的协方差也随之更新。将误差量进行一阶泰勒展开可以得到：
其中和Fw分别是误差量/>对/>和wi的一阶偏导。i时刻的误差量/>的协方差矩阵为/>i+1时刻噪声的协方差矩阵为W，因此i+1时刻的协方差/>为：
在接收到步骤2)的信息后，在局部地图中找出距离新激光雷达点云中的点最近的五个点，并计算这个点与对应的五个点构成的平面的距离，构建成一个残差向量，随后在先验位姿的基础上迭代求解点云帧的点和对应平面的距离最小时的状态量，获得的结果便是前端里程计获得的最优位姿。
在此状态下，接收到了新的一帧雷达点云。由于激光雷达在采集信息的过程中每个点之间是有时间间隔的，所以不能直接将一帧点云视作同一个时刻的信息使用，这将导致误差的增加。所以需要将新接收到的雷达点云去除噪声点后依靠IMU的高频信息进行畸变修正，将这些点都投影到帧起始坐标系下。依靠IMU获得的先验位姿可以将去畸变后的点云帧投影到世界坐标系下，进而和局部地图进行匹配。但是由于此时的先验并不是真实值，所以投影到真实世界的点云和其真实位置仍具有一定误差。通过计算点云中的点和其局部地图中最近点组成的面的距离可以衡量这种误差，将这一误差逐渐缩小的过程就是趋近真实值的过程。
步骤3)中，假设某时刻的状态量为x，此时刻点云帧中的点与对应平面的点面距离残差观测为z，观测z由状态量x和观测噪声v决定。点面距离函数根据此时刻的状态量和点云帧计算出残差向量，将函数/>进行一阶泰勒展开获得线性化的表示：
其中，H为函数对误差量/>的一阶导数。基于状态量计算出的观测量与实际的观测之间必然存在差异，这个差距来自两个方面，一方面是来自状态估计的误差/>另一方面是来自测量的误差v；在误差量为/>的情况下，基于状态的观测z和实际观测zobs相同的概率为：
根据贝叶斯公式，在观测z＝zobs的前提下，状态误差量为的后验概率为：
其中是先验概率。随后可以通过最大化后验概率(MAP)得到误差量的最优解并更新协方差矩阵为/>
其中将/>和/>相加得到更新后的状态量xo即为最优位姿，并且最优位姿服从协方差为/>的高斯分布；由此便完成了一次滤波优化，获得了世界坐标系下的最优位姿xo以及其对应的协方差矩阵/>通过这个最优位姿将去畸变后的点云帧投影到世界坐标系中，并合并到局部地图中，供下一次的点云配准使用，协方差矩阵将传递到下一帧辅助进行下一次的滤波过程。
所述的步骤4)具体包括如下步骤：
4.1)接收每一帧的位姿，将每个帧结束时刻的位姿作为图中的顶点，将每两帧之间的运动变化作为图中的边；
具体的，经步骤1)-步骤3)获得了每一帧的位姿，将所有帧通过时间和距离的限制进行筛选，留下一部分既可以充分表示运动信息又不会需要过多计算资源的关键帧，将这些关键帧按时间顺序进行编号，以便和回环因子和GNSS因子进行时间戳对齐，之后以每一帧结束的位姿作为图的顶点，以相邻两帧之间的位姿变换作为边，构建出因子图进行优化。
4.2)同时使用GICP检测当前关键帧是否和之前的关键帧构成回环，当检测成功时将这两帧之间的位姿变换作为新的边加入因子图中；
4.3)接收到GNSS信息后，首先将其GNSS的信息转换到ENU坐标系下，再寻找时间上最接近的雷达里程计关键帧，使用线性插值其时间戳对齐；当再次收到GNSS信息时，计算出两次信息之间的相对位置，并将这种约束作为边加入因子图中；加入所有待优化的顶点和边后求解图对应的目标函数即获得一系列更为准确的位姿，从而可以拼接成更加准确的三维点云地图。
在本发明的一个具体实施例中，所述的步骤4.3)具体为：当接收GNSS测量值时，首先将其转换为本地笛卡尔坐标系。在因子图中添加新节点后，将新的GNSS因子与该节点相关联；如果GNSS信号没有与激光雷达帧的时间戳同步，会根据激光雷达帧的时间戳对GNSS测量值进行线性插值；在经过一个GNSS接收周期后，会再次获得一个GNSS信息，经过上面同样的步骤后，因子图中会有两个GNSS因子，和激光雷达里程计因子处于相同的时间戳上，这样就可以根据这两帧GNSS信息之间的相对距离，对整体进行约束。
本发明优化后的结果即为最终的关键帧位姿，根据这些关键帧的位姿可以将激光雷达点云拼接成三维点云地图。图2是使用本发明对一个工厂园区建图后获得的三维点云地图的俯视图。从俯视图可以看出此三维点云地图基本没有重影出现，说明点云地图质量较高。
图3显示了本发明和现有三维点云地图构建方法LIO-SAM以及FAST-LIO的对比。图3中以直线形式绘制的是本发明方法，以点形式绘制的是LIO-SAM方法，以三角形式绘制的是FAST-LIO方法。对比方式是使用统一数据集进行三维点云地图的构建，计算每个方法的最大位置误差。图中每一个点都是代表某时刻点云地图构建算法估计的位置和实际位置的距离。从图中可以看出，本方法的最大误差小于10cm，而另外两种方法的最大误差都大于10cm，所以本方法相比于其他方法可以构建更为精准，质量更好的三维点云地图。
以上所述实施例仅表达了本发明的几种实施方式，其描述较为具体和详细，但并不能因此而理解为对本发明专利范围的限制。对于本领域的普通技术人员来说，在不脱离本发明构思的前提下，还可以做出若干变形和改进，这些都属于本发明的保护范围。

Claims (7)

1.一种基于误差卡尔曼滤波和因子图的三维点云地图构建方法，其特征在于，包括如下步骤：
1)按照时间序列接收激光雷达帧信息、IMU数据、GNSS信息；
2)将一个雷达帧内的IMU信息中的加速度和角速度进行积分，获得这一帧时间内的位姿变化，并将其加到上一帧的估计位姿上，作为此时刻位姿的先验；
3)以先验位姿为基础，将新的激光雷达帧和先前构建的局部地图进行匹配，新的点云中的点和对应面的距离作为残差向量，用迭代误差卡尔曼滤波进行迭代优化后获得前端里程计的最优位姿以及此时刻最优位姿对应的协方差矩阵；
4)使用滤波得到的最优位姿和协方差矩阵构建因子图，同时判断每一帧是否会和先前帧构成回环，如果出现回环将回环因子加入因子图中；将GNSS信息同样加入因子图中，进行联合优化，获得更加准确的位姿；使用优化后的位姿将激光雷达点云帧拼接起来构成三维点云地图。
2.根据权利要求1所述的基于误差卡尔曼滤波和因子图的三维点云地图构建方法，其特征在于，所述的步骤2)具体为：
在接收到步骤1)的信息和数据后，将激光雷达帧和IMU数据的时间戳进行对齐，找出在一个激光雷达帧中的所有IMU信息，使用IMU观测的信息进行运动学模拟，包括位置p、姿态R、激光雷达与IMU之间的相对位姿、速度v、陀螺仪零偏bg、加速度计零偏ba和重力g，其中，激光雷达与IMU之间的相对位姿由相对位置tbl和相对姿态Rbl组成；将这些物理量写成统一的状态量x：
在i时刻的状态量为xi，通过IMU读取此时刻的角速度ω和加速度a，将二者合并成一个矩阵其中角速度对应的白噪声和零偏噪声分别为ng和/>加速度对应的白噪声和零偏噪声分别为na和/>这四种噪声均为高斯分布，将四种噪声合并成一个噪声矩阵/>依靠这几个量递推得到Δt时间后i+1时刻的状态量，
其中f(xi,ui,wi)代表状态量变化率函数；代表状态量的相加；
在迭代误差卡尔曼滤波的预测过程中，将加速度和角速度的噪声置零，即可得到i+1时刻的名义量名义量作为后续的观测的先验位姿：
误差量服从均值为0，方差为自身协方差矩阵的高斯分布；名义量更新后，误差量的协方差也随之更新；将误差量进行一阶泰勒展开得到：
其中和Fw分别是误差量/>对/>和wi的一阶偏导；i时刻的误差量/>的协方差矩阵为/>时刻噪声的协方差矩阵为W，因此i+1时刻的协方差/>为：
3.根据权利要求1所述的基于误差卡尔曼滤波和因子图的三维点云地图构建方法，其特征在于，所述的步骤3)中：
在接收到步骤2)的信息后，在局部地图中找出距离新激光雷达点云中的点最近的五个点，并计算这个点与对应的五个点构成的平面的距离，构建成一个残差向量，随后在先验位姿的基础上迭代求解点云帧的点和对应平面的距离最小时的状态量，获得的结果便是前端里程计获得的最优位姿。
4.根据权利要求1所述的基于误差卡尔曼滤波和因子图的三维点云地图构建方法，其特征在于，所述的步骤3)具体为：
假设某时刻的状态量为x，此时刻点云帧中的点与对应平面的点面距离残差观测为z，观测z由状态量x和观测噪声v决定；点面距离函数根据此时刻的状态量和点云帧计算出残差向量，将函数/>进行一阶泰勒展开获得线性化的表示：
其中，H为函数对误差量/>的一阶导数；基于状态量计算出的观测量与实际的观测之间必然存在差异，这个差距来自两个方面，一方面是来自状态估计的误差/>另一方面是来自测量的误差v；在误差量为/>的情况下，基于状态的观测z和实际观测zobs相同的概率为：
其中，代表状态量的相加；根据贝叶斯公式，在观测z＝zobs的前提下，状态误差量为/>的后验概率为：
其中是先验概率；随后可以通过最大化后验概率(MAP)得到误差量的最优解/>并更新协方差矩阵为/>
其中将/>和/>相加得到更新后的状态量xo即为最优位姿，并且最优位姿服从协方差为/>的高斯分布；由此便完成了一次滤波优化，获得了世界坐标系下的最优位姿xo以及其对应的协方差矩阵/>通过这个最优位姿将去畸变后的点云帧投影到世界坐标系中，并合并到局部地图中，供下一次的点云配准使用，协方差矩阵将传递到下一帧辅助进行下一次的滤波过程。
5.根据权利要求1所述的基于误差卡尔曼滤波和因子图的三维点云地图构建方法，其特征在于，所述的步骤4)具体包括如下步骤：
4.1)接收每一帧的位姿，将每个帧结束时刻的位姿作为图中的顶点，将每两帧之间的运动变化作为图中的边；
4.2)同时使用GICP检测当前关键帧是否和之前的关键帧构成回环，当检测成功时将这两帧之间的位姿变换作为新的边加入因子图中；
4.3)接收到GNSS信息后，首先将其GNSS的信息转换到ENU坐标系下，再寻找时间上最接近的雷达里程计关键帧，使用线性插值其时间戳对齐；当再次收到GNSS信息时，计算出两次信息之间的相对位置，并将这种约束作为边加入因子图中；加入所有待优化的顶点和边后求解图对应的目标函数即获得一系列更为准确的位姿，从而可以拼接成更加准确的三维点云地图。
6.根据权利要求5所述的基于误差卡尔曼滤波和因子图的三维点云地图构建方法，其特征在于，所述的步骤4.1)具体为：
经步骤1)-步骤3)获得了每一帧的位姿，将所有帧通过时间和距离的限制进行筛选，留下一部分既可以充分表示运动信息又不会需要过多计算资源的关键帧，将这些关键帧按时间顺序进行编号，以便和回环因子和GNSS因子进行时间戳对齐，之后以每一帧结束的位姿作为图的顶点，以相邻两帧之间的位姿变换作为边，构建出因子图进行优化。
7.根据权利要求5所述的基于误差卡尔曼滤波和因子图的三维点云地图构建方法，其特征在于，所述的步骤4.3)具体为：
当接收GNSS测量值时，首先将其转换为本地笛卡尔坐标系；在因子图中添加新节点后，将新的GNSS因子与该节点相关联；如果GNSS信号没有与激光雷达帧的时间戳同步，会根据激光雷达帧的时间戳对GNSS测量值进行线性插值；在经过一个GNSS接收周期后，会再次获得一个GNSS信息，经过上面同样的步骤后，因子图中会有两个GNSS因子，和激光雷达里程计因子处于相同的时间戳上，这样就可以根据这两帧GNSS信息之间的相对距离，对整体进行约束。
CN202310407457.3A 2023-04-17 2023-04-17 一种基于误差卡尔曼滤波和因子图的三维点云地图构建方法 Pending CN116452763A (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202310407457.3A CN116452763A (zh) 2023-04-17 2023-04-17 一种基于误差卡尔曼滤波和因子图的三维点云地图构建方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202310407457.3A CN116452763A (zh) 2023-04-17 2023-04-17 一种基于误差卡尔曼滤波和因子图的三维点云地图构建方法

Publications (1)

Publication Number Publication Date
CN116452763A true CN116452763A (zh) 2023-07-18

Family

ID=87133269

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202310407457.3A Pending CN116452763A (zh) 2023-04-17 2023-04-17 一种基于误差卡尔曼滤波和因子图的三维点云地图构建方法

Country Status (1)

Country Link
CN (1) CN116452763A (zh)

Cited By (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN116908810A (zh) * 2023-09-12 2023-10-20 天津大学四川创新研究院 一种无人机搭载激光雷达测量建筑土方的方法和系统
CN117387598A (zh) * 2023-10-08 2024-01-12 北京理工大学 一种紧耦合轻量级的实时定位与建图方法
CN117760417A (zh) * 2023-12-19 2024-03-26 苏州尚同墨方智能科技有限公司 一种基于4d毫米波雷达与imu的融合定位方法及系统

Cited By (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN116908810A (zh) * 2023-09-12 2023-10-20 天津大学四川创新研究院 一种无人机搭载激光雷达测量建筑土方的方法和系统
CN116908810B (zh) * 2023-09-12 2023-12-12 天津大学四川创新研究院 一种无人机搭载激光雷达测量建筑土方的方法和系统
CN117387598A (zh) * 2023-10-08 2024-01-12 北京理工大学 一种紧耦合轻量级的实时定位与建图方法
CN117760417A (zh) * 2023-12-19 2024-03-26 苏州尚同墨方智能科技有限公司 一种基于4d毫米波雷达与imu的融合定位方法及系统

Similar Documents

Publication Publication Date Title
CN116452763A (zh) 一种基于误差卡尔曼滤波和因子图的三维点云地图构建方法
CN107167826B (zh) 一种自动驾驶中基于可变网格的图像特征检测的车辆纵向定位系统及方法
CN114526745B (zh) 一种紧耦合激光雷达和惯性里程计的建图方法及系统
CN110375738A (zh) 一种融合惯性测量单元的单目同步定位与建图位姿解算方法
CN113358112B (zh) 一种地图构建方法及一种激光惯性里程计
CN114018248B (zh) 一种融合码盘和激光雷达的里程计方法与建图方法
CN114323033B (zh) 基于车道线和特征点的定位方法、设备及自动驾驶车辆
CN111366153B (zh) 一种激光雷达与imu紧耦合的定位方法
CN114019552A (zh) 一种基于贝叶斯多传感器误差约束的定位置信度优化方法
CN112781594A (zh) 基于imu耦合的激光雷达迭代最近点改进算法
WO2024001649A1 (zh) 机器人定位方法、装置和计算可读存储介质
CN114563000B (zh) 一种基于改进型激光雷达里程计的室内外slam方法
CN115728803A (zh) 一种城市驾驶车辆连续定位系统及方法
CN115930977A (zh) 特征退化场景的定位方法、系统、电子设备和可读存介质
CN113838129B (zh) 一种获得位姿信息的方法、装置以及系统
CN117824666A (zh) 融合定位用二维码对、二维码标定方法及融合定位方法
CN117824667A (zh) 一种基于二维码和激光的融合定位方法及介质
CN112798020B (zh) 一种用于评估智能汽车定位精度的系统及方法
CN117388830A (zh) 激光雷达与惯性导航的外参标定方法、装置、设备及介质
CN115326068B (zh) 激光雷达-惯性测量单元融合里程计设计方法及系统
CN116045965A (zh) 一种融合多传感器的环境地图构建方法
CN114323038B (zh) 融合双目视觉和2d激光雷达的室外定位方法
CN114723920A (zh) 一种基于点云地图的视觉定位方法
CN114459474A (zh) 一种基于因子图的惯性/偏振/雷达/光流紧组合导航的方法
CN114440892B (zh) 一种基于拓扑地图和里程计的自定位方法

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