[go: up one dir, main page]
More Web Proxy on the site http://driver.im/

CN113763549B - 融合激光雷达和imu的同时定位建图方法、装置和存储介质 - Google Patents

融合激光雷达和imu的同时定位建图方法、装置和存储介质 Download PDF

Info

Publication number
CN113763549B
CN113763549B CN202110953344.4A CN202110953344A CN113763549B CN 113763549 B CN113763549 B CN 113763549B CN 202110953344 A CN202110953344 A CN 202110953344A CN 113763549 B CN113763549 B CN 113763549B
Authority
CN
China
Prior art keywords
imu
map
sub
initial
point cloud
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.)
Active
Application number
CN202110953344.4A
Other languages
English (en)
Other versions
CN113763549A (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.)
Tongji University
Original Assignee
Tongji University
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 Tongji University filed Critical Tongji University
Priority to CN202110953344.4A priority Critical patent/CN113763549B/zh
Publication of CN113763549A publication Critical patent/CN113763549A/zh
Application granted granted Critical
Publication of CN113763549B publication Critical patent/CN113763549B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

Classifications

    • GPHYSICS
    • G06COMPUTING; CALCULATING OR COUNTING
    • G06TIMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T17/00Three dimensional [3D] modelling, e.g. data description of 3D objects
    • G06T17/05Geographic models
    • GPHYSICS
    • G06COMPUTING; CALCULATING OR COUNTING
    • G06TIMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T3/00Geometric image transformations in the plane of the image
    • G06T3/06Topological mapping of higher dimensional structures onto lower dimensional surfaces
    • GPHYSICS
    • G06COMPUTING; CALCULATING OR COUNTING
    • G06TIMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T7/00Image analysis
    • G06T7/30Determination of transform parameters for the alignment of images, i.e. image registration
    • G06T7/33Determination of transform parameters for the alignment of images, i.e. image registration using feature-based methods
    • YGENERAL 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
    • Y02TECHNOLOGIES OR APPLICATIONS FOR MITIGATION OR ADAPTATION AGAINST CLIMATE CHANGE
    • Y02ATECHNOLOGIES FOR ADAPTATION TO CLIMATE CHANGE
    • Y02A90/00Technologies having an indirect contribution to adaptation to climate change
    • Y02A90/10Information and communication technologies [ICT] supporting adaptation to climate change, e.g. for weather forecasting or climate simulation

Landscapes

  • Engineering & Computer Science (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Theoretical Computer Science (AREA)
  • Geometry (AREA)
  • Software Systems (AREA)
  • Remote Sensing (AREA)
  • Computer Graphics (AREA)
  • Computer Vision & Pattern Recognition (AREA)
  • Navigation (AREA)

Abstract

本发明涉及一种融合激光雷达和IMU的同时定位建图方法、装置和存储介质。对激光雷达点云数据和IMU数据预处理后,初始化系统,并对预处理后的点云和现有地图采用直接法配准,并计算相关参数从而构建因子图,得到初始3D子地图,将3D子地图映射到2D子地图后提取特征点,计算2D刚体变换参数,从而计算出3D坐标初始值,迭代优化不断更新3D坐标的值,最终输出达到精确度要求的3D坐标以及3D地图。与现有技术相比,本发明采用直接法将点云与已有地图配准,并后续不断迭代得到最终输出,具有可适应多雷达系统、检测效率高等优点。

Description

融合激光雷达和IMU的同时定位建图方法、装置和存储介质
技术领域
本发明涉及同时定位建图领域,尤其是涉及一种融合激光雷达和IMU的同时定位建图方法、装置和存储介质。
背景技术
目前,在自主测绘、机器人等自主化相关领域,同时定位与建图(SLAM)是其中的一项非常重要的基础性技术。SLAM在为载体提供实时位置估计的同时进行环境地图的构建,使得更上层的应用如自主导航、路径规划等得以实现。然而,复杂环境下,如高楼、树木等遮挡常使得GPS等不可用,这种情况下,人们主要依靠主动式传感器如激光雷达(LiDAR)和IMU来进行SLAM系统的构建。但现有的激光雷达和IMU的融合方式直接使用IMU的测量值计算,缺乏实时性,精度较低,且对点云信息的配准方法上,基于特征点的方法与激光雷达传感器的点采集方式高度耦合,不易扩展到多雷达系统。因此,同时定位建图方法目前仍然存在精确度和适应性的问题。
发明内容
本发明的目的就是为了克服上述现有技术存在的缺陷而提供一种融合激光雷达和IMU的同时定位建图方法、装置和存储介质,从而解决目前定位建图方法精确度低且适应性差的问题。
本发明的目的可以通过以下技术方案来实现:
一种融合激光雷达和IMU的同时定位建图方法,包括以下步骤:
S1、获取点云数据和IMU数据,对点云数据和IMU数据进行预处理。
S2、使用预处理后的点云数据和IMU数据初始化定位建图系统,并根据初始化后的定位建图系统,将预处理后的点云数据与现有地图配准,得到初始配准值,并计算当前重力向量、初始姿态和IMU积分值。
S3、根据重力向量、配准值和IMU预积分,构建因子图模型。
S4、将预处理后的点云数据输入因子图模型,获得实时3D载体坐标,根据所述3D载体坐标和预处理后的点云数据生成初始3D子地图。
S5、将初始3D子地图根据初始姿态在水平方向上投影得到测试2D子地图。
S6、提取测试2D子地图的特征点,并通过定位建图系统,将测试2D子地图的特征点与现有2D子地图的特征点进行匹配,当匹配的特征点数量超过数量阈值时,则根据匹配的特征点计算2D刚体变换参数。
S7、根据预处理后的点云数据、初始3D子地图和2D刚体变换参数,计算得到3D坐标初始值,以3D坐标初始值为起点,在Z方向上进行滑动配准,得到进阶配准值,根据进阶配准值,计算3D坐标转换参数;根据3D坐标转换参数建立位姿图,并得到进阶姿态。;
S8、判断相邻两次的3D坐标转换参数的差小于参数阈值,若是,则执行步骤S10;若否,则执行步骤S9。
S9、保持初始3D子地图不变,根据进阶姿态,重新映射得到新的2D子地图,执行步骤S6。
S10、将当前3D坐标转换参数对应的3D坐标作为最终的3D坐标,并导入初始3D子地图,生成最终3D子地图。
进一步地,步骤S2中初始化包括静态初始化或动态初始化。
进一步地,静态初始化包括:计算IMU加速度测量值的平均值和世界坐标系的重力向量之间的旋转矩阵,确定初始姿态,将IMU角速度测量值的平均值设为IMU偏差初始值,并将IMU速度及坐标均设为0。
进一步地,动态初始化包括:建立起始帧坐标系,计算点云相对旋转值和预积分相对旋转值,得到IMU偏差初始值;计算点云相对位移值和预积分相对位移值,得到IMU速度、坐标、初始姿态以及初始重力向量,并根据初始重力向量将所有值转换至世界坐标系中。
进一步地,步骤S6中测试2D子地图的特征点为SURF特征点。
进一步地,步骤S1中预处理步骤包括:将时间戳小于主雷达时间戳的IMU进行预积分,以主雷达时间戳为参考,将辅助雷达中采样时间戳落在该时间戳内的所有激光点,按照该时间戳重新排序;将每一个激光点转换至IMU坐标系中,计算相对运动并进行运动畸变校正,得到预处理后的点云数据。
进一步地,步骤S6中的数量阈值范围为4~6。
进一步地,步骤S1中点云数据和IMU数据是通过多个十六线激光雷达和IMU获取的。
一种融合激光雷达和IMU的同时定位建图装置,包括存储器和处理器;存储器,用于存储计算机程序;处理器,用于当执行计算机程序时,实现如上文的融合激光雷达和IMU的同时定位建图方法。
一种计算机可读存储介质,计算机可读存储介质上存储有计算机程序,计算机程序被处理器执行时实现如上文的一种融合激光雷达和IMU的同时定位建图方法的步骤。
与现有技术相比,本发明具有以下优点:
1、对预处理后的点云和现有地图采用了直接配准的方法,使得系统适应性更强,可实时反映参数变化,易扩展到多雷达输入系统,并将3D子地图映射到2D子地图后提取特征点,计算2D刚体变换参数,提高了检测效率并可从而得到初始坐标,最后循环迭代优化坐标,使得结果更为精确。
2、在系统初始化时支持静态初始化或动态初始化,应用范围广;且在初始化中均引入了世界坐标系,可与同使用该坐标系的应用所配合。
附图说明
图1为本发明的流程示意图。
图2为本发明中2D子地图的特征点处理示意图。
图3为本发明中2D子地图到3D子地图的空间关系转换示意图。
具体实施方式
下面结合附图和具体实施例对本发明进行详细说明。本实施例以本发明技术方案为前提进行实施,给出了详细的实施方式和具体的操作过程,但本发明的保护范围不限于下述的实施例。
本实施例提供了一种融合激光雷达和IMU的同时定位建图方法,流程如图1所示,具体展开如下:
步骤S1、获取多雷达系统的点云数据和IMU数据,并进行预处理:将时间戳小于主雷达时间戳的IMU读数进行预积分,获取预积分测量值和相对运动的初始估计值。按照主雷达的时间戳为参考,取所有辅雷达中激光点的采样时间戳落在此区间的所有点,并将它们按照时间戳重新排序,得到按时间顺序增长的点云。对每一个雷达点进行坐标准换到IMU坐标系下。对每一个雷达点依据其时间戳从IMU的积分值中计算相对运动,对其作运动畸变修正经上述步骤后,得到在主雷达时间戳下的经畸变纠正后的点云数据。
步骤S2、使用预处理后的点云数据和IMU数据初始化定位建图系统,初始化的过程可分为静态初始化和动态初始化。静态初始化具体包括将IMU的加速度测量值的平均值与世界坐标系的重力向量对齐,计算两个向量之间的旋转矩阵,确定初始时刻的IMU姿态,并将IMU角速度测量值的平均值作为IMU陀螺仪Bias的初始值。IMU的位置、速度则初始化为0。动态初始化具体包括利用“Normal Distribution Transform”算法配准窗口内的多帧点云,计算点云间的相对运动,并将之归算到窗口内的起始帧坐标系下,计算相邻帧之间的IMU预积分值。将点云相对旋转值与预积分的相对旋转值联立求解IMU陀螺仪偏置,将点云的相对位移与预积分的相对位移联立求解IMU在初始时间窗口内的位置、速度、姿态及重力向量。根据所解得的重力向量和状态将窗口内所有帧的状态值准换到世界坐标系下。并根据初始化后的定位建图系统,将预处理后的点云数据与现有地图配准,得到初始配准值,并利用过去至少3帧的数据计算当前重力向量、初始姿态和IMU积分值。
步骤S3、根据重力向量、配准值和IMU预积分,构建因子图模型,更新IMU偏差等参数。
步骤S4、将预处理后的点云数据输入因子图模型,获得实时3D载体坐标,根据所述3D载体坐标和预处理后的点云数据生成初始3D子地图。
步骤S5、将初始3D子地图根据初始姿态在水平方向上投影得到测试2D子地图。
步骤S6、提取测试2D子地图的SURF特征点,并计算特征描述子,并将测试2D子地图的特征点与现有2D子地图的特征点进行匹配,如图2所示,当匹配的特征点数量超过5个时,则认定两张子地图存在回环,根据存在回环的子地图的特征点计算2D刚体变换参数。
步骤S7、如图3所示,根据原始帧与初始3D子地图的空间关系和2D刚体变换参数,计算得到3D坐标初始值,以3D坐标初始值为起点,在Z方向上进行滑动配准,得到进阶配准值,根据进阶配准值,局部迭代优化,计算3D坐标转换参数;根据3D坐标转换参数建立位姿图,并联合优化,得到进阶姿态。
步骤S8、判断相邻两次的3D坐标转换参数的差小于参数阈值,若是,则执行步骤S10;若否,则执行步骤S9。
步骤S9、保持初始3D子地图不变,根据进阶姿态,重新映射得到新的2D子地图,执行步骤S6(更新3D转换坐标和进阶姿态的值)。
步骤S10、将当前3D坐标转换参数对应的3D坐标作为最终的3D坐标,并导入初始3D子地图,生成最终3D子地图。
本实施例还提供了一种融合激光雷达和IMU的同时定位建图装置,包括存储器和处理器;存储器,用于存储计算机程序;处理器,用于当执行计算机程序时,实现如上文所述的的融合激光雷达和IMU的同时定位建图方法。
本实施例还提供了一种计算机可读存储介质,计算机可读存储介质上存储有计算机程序,计算机程序被处理器执行时实现如上文所述的一种融合激光雷达和IMU的同时定位建图方法的步骤。
以上详细描述了本发明的较佳具体实施例。应当理解,本领域的普通技术人员无需创造性劳动就可以根据本发明的构思作出诸多修改和变化。因此,凡本技术领域中技术人员依本发明的构思在现有技术的基础上通过逻辑分析、推理或者有限的实验可以得到的技术方案,皆应在由权利要求书所确定的保护范围内。

Claims (10)

1.一种融合激光雷达和IMU的同时定位建图方法,其特征在于,包括以下步骤:
S1、获取点云数据和IMU数据,对点云数据和IMU数据进行预处理;
S2、使用预处理后的点云数据和IMU数据初始化定位建图系统,并根据初始化后的定位建图系统,将预处理后的点云数据与现有地图配准,得到初始配准值,并计算当前重力向量、初始姿态和IMU积分值;
S3、根据所述重力向量、所述配准值和IMU预积分,构建因子图模型;
S4、将预处理后的点云数据输入所述因子图模型,获得实时3D载体坐标,根据所述3D载体坐标和预处理后的点云数据生成初始3D子地图;
S5、将初始3D子地图根据所述初始姿态在水平方向上投影得到测试2D子地图;
S6、提取测试2D子地图的特征点,并通过定位建图系统,将测试2D子地图的特征点与现有2D子地图的特征点进行匹配,当匹配的特征点数量超过数量阈值时,则根据匹配的特征点计算2D刚体变换参数;
S7、根据预处理后的点云数据、初始3D子地图和2D刚体变换参数,计算得到3D坐标初始值,以所述3D坐标初始值为起点,在Z方向上进行滑动配准,得到进阶配准值,根据所述进阶配准值,计算3D坐标转换参数;根据所述3D坐标转换参数建立位姿图,并得到进阶姿态;
S8、判断相邻两次的3D坐标转换参数的差小于参数阈值,若是,则执行步骤S10;若否,则执行步骤S9;
S9、保持初始3D子地图不变,根据所述进阶姿态,重新映射得到新的2D子地图,执行步骤S6;
S10、将当前3D坐标转换参数对应的3D坐标作为最终的3D坐标,并导入初始3D子地图,生成最终3D子地图。
2.根据权利要求1所述的一种融合激光雷达和IMU的同时定位建图方法,其特征在于,步骤S2中所述初始化包括静态初始化或动态初始化。
3.根据权利要求2所述的一种融合激光雷达和IMU的同时定位建图方法,其特征在于,所述静态初始化包括:计算IMU加速度测量值的平均值和世界坐标系的重力向量之间的旋转矩阵,确定初始姿态,将IMU角速度测量值的平均值设为IMU偏差初始值,并将IMU速度及坐标均设为0。
4.根据权利要求2所述的一种融合激光雷达和IMU的同时定位建图方法,其特征在于,所述动态初始化包括:建立起始帧坐标系,计算点云相对旋转值和预积分相对旋转值,得到IMU偏差初始值;计算点云相对位移值和预积分相对位移值,得到IMU速度、坐标、初始姿态以及初始重力向量,并根据初始重力向量将所有值转换至世界坐标系中。
5.根据权利要求1所述的一种融合激光雷达和IMU的同时定位建图方法,其特征在于,步骤S6中测试2D子地图的特征点为SURF特征点。
6.根据权利要求1所述的一种融合激光雷达和IMU的同时定位建图方法,其特征在于,步骤S1中所述预处理步骤包括:将时间戳小于主雷达时间戳的IMU进行预积分,以主雷达时间戳为参考,将辅助雷达中采样时间戳落在该时间戳内的所有激光点,按照该时间戳重新排序;将每一个激光点转换至IMU坐标系中,计算相对运动并进行运动畸变校正,得到预处理后的点云数据。
7.根据权利要求1所述的一种融合激光雷达和IMU的同时定位建图方法,其特征在于,步骤S6中的数量阈值范围为4~6。
8.根据权利要求1所述的一种融合激光雷达和IMU的同时定位建图方法,其特征在于,步骤S1中所述点云数据和IMU数据是通过多个十六线激光雷达和IMU获取的。
9.一种融合激光雷达和IMU的同时定位建图装置,其特征在于,包括存储器和处理器;所述存储器,用于存储计算机程序;所述处理器,用于当执行所述计算机程序时,实现如权利要求1~8任一项所述的融合激光雷达和IMU的同时定位建图方法。
10.一种计算机可读存储介质,其特征在于,所述计算机可读存储介质上存储有计算机程序,所述计算机程序被处理器执行时实现如权利要求1至8任一项所述的一种融合激光雷达和IMU的同时定位建图方法的步骤。
CN202110953344.4A 2021-08-19 2021-08-19 融合激光雷达和imu的同时定位建图方法、装置和存储介质 Active CN113763549B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202110953344.4A CN113763549B (zh) 2021-08-19 2021-08-19 融合激光雷达和imu的同时定位建图方法、装置和存储介质

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202110953344.4A CN113763549B (zh) 2021-08-19 2021-08-19 融合激光雷达和imu的同时定位建图方法、装置和存储介质

Publications (2)

Publication Number Publication Date
CN113763549A CN113763549A (zh) 2021-12-07
CN113763549B true CN113763549B (zh) 2023-07-07

Family

ID=78790411

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202110953344.4A Active CN113763549B (zh) 2021-08-19 2021-08-19 融合激光雷达和imu的同时定位建图方法、装置和存储介质

Country Status (1)

Country Link
CN (1) CN113763549B (zh)

Families Citing this family (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN114046792B (zh) * 2022-01-07 2022-04-26 陕西欧卡电子智能科技有限公司 一种无人船水面定位与建图方法、装置及相关组件
CN114593724B (zh) * 2022-01-21 2024-06-14 北京邮电大学 集群融合定位方法和装置
CN116878488B (zh) * 2023-09-07 2023-11-28 湘江实验室 一种建图方法、装置、存储介质及电子装置

Citations (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN111207774A (zh) * 2020-01-17 2020-05-29 山东大学 一种用于激光-imu外参标定的方法及系统
WO2020155616A1 (zh) * 2019-01-29 2020-08-06 浙江省北大信息技术高等研究院 一种基于数字视网膜的拍摄装置的定位方法
CN113066105A (zh) * 2021-04-02 2021-07-02 北京理工大学 激光雷达和惯性测量单元融合的定位与建图方法及系统

Family Cites Families (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US11567201B2 (en) * 2016-03-11 2023-01-31 Kaarta, Inc. Laser scanner with real-time, online ego-motion estimation

Patent Citations (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
WO2020155616A1 (zh) * 2019-01-29 2020-08-06 浙江省北大信息技术高等研究院 一种基于数字视网膜的拍摄装置的定位方法
CN111207774A (zh) * 2020-01-17 2020-05-29 山东大学 一种用于激光-imu外参标定的方法及系统
CN113066105A (zh) * 2021-04-02 2021-07-02 北京理工大学 激光雷达和惯性测量单元融合的定位与建图方法及系统

Non-Patent Citations (2)

* Cited by examiner, † Cited by third party
Title
一种改进ICP算法的移动机器人激光与视觉建图方法研究;张杰;周军;;机电工程(12);全文 *
基于混合地图的护理机器人室内导航方法;张立志;陈殿生;刘维惠;;北京航空航天大学学报(05);全文 *

Also Published As

Publication number Publication date
CN113763549A (zh) 2021-12-07

Similar Documents

Publication Publication Date Title
CN112347840B (zh) 视觉传感器激光雷达融合无人机定位与建图装置和方法
CN113763549B (zh) 融合激光雷达和imu的同时定位建图方法、装置和存储介质
CN110070615B (zh) 一种基于多相机协同的全景视觉slam方法
CN109522832B (zh) 基于点云片段匹配约束和轨迹漂移优化的回环检测方法
Wu et al. Hand-eye calibration: 4-D procrustes analysis approach
CN113066105A (zh) 激光雷达和惯性测量单元融合的定位与建图方法及系统
CN112987065B (zh) 一种融合多传感器的手持式slam装置及其控制方法
CN110570449B (zh) 一种基于毫米波雷达与视觉slam的定位与建图方法
CN110285806A (zh) 基于多次位姿校正的移动机器人快速精确定位算法
JP5627325B2 (ja) 位置姿勢計測装置、位置姿勢計測方法、およびプログラム
Pan et al. Voxfield: Non-projective signed distance fields for online planning and 3d reconstruction
CN114013449B (zh) 针对自动驾驶车辆的数据处理方法、装置和自动驾驶车辆
WO2020063878A1 (zh) 一种处理数据的方法和装置
CN111366153B (zh) 一种激光雷达与imu紧耦合的定位方法
CN113933818A (zh) 激光雷达外参的标定的方法、设备、存储介质及程序产品
CN109087394A (zh) 一种基于低成本rgb-d传感器的实时室内三维重建方法
CN115272596A (zh) 一种面向单调无纹理大场景的多传感器融合slam方法
CN111998862A (zh) 一种基于bnn的稠密双目slam方法
CN116878488B (zh) 一种建图方法、装置、存储介质及电子装置
Hu et al. Efficient Visual-Inertial navigation with point-plane map
CN117169942A (zh) 一种基于LiDAR/GPS/IMU融合的无人车重定位方法
Kupervasser et al. Robust positioning of drones for land use monitoring in strong terrain relief using vision-based navigation
CN113495281B (zh) 可移动平台的实时定位方法及装置
Pan et al. LiDAR-IMU Tightly-Coupled SLAM Method Based on IEKF and Loop Closure Detection
Zhang et al. The Robot Fusion Mapping Method Applied in Indoor Poultry Farming Environment

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