CN114777775A - 一种多传感器融合的定位方法及系统 - Google Patents

一种多传感器融合的定位方法及系统 Download PDF

Info

Publication number
CN114777775A
CN114777775A CN202210484848.0A CN202210484848A CN114777775A CN 114777775 A CN114777775 A CN 114777775A CN 202210484848 A CN202210484848 A CN 202210484848A CN 114777775 A CN114777775 A CN 114777775A
Authority
CN
China
Prior art keywords
factor
data
pose
map
positioning
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
CN202210484848.0A
Other languages
English (en)
Other versions
CN114777775B (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.)
Zhejiang Normal University CJNU
Original Assignee
Zhejiang Normal University CJNU
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 Normal University CJNU filed Critical Zhejiang Normal University CJNU
Priority to CN202210484848.0A priority Critical patent/CN114777775B/zh
Publication of CN114777775A publication Critical patent/CN114777775A/zh
Application granted granted Critical
Publication of CN114777775B publication Critical patent/CN114777775B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

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/10—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration
    • G01C21/12—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning
    • G01C21/16—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation
    • G01C21/165—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation combined with non-inertial navigation instruments
    • 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/005—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 with correlation of navigation data from several sources, e.g. map or contour matching
    • 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/10—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration
    • G01C21/12—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning
    • G01C21/16—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation
    • G01C21/165—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation combined with non-inertial navigation instruments
    • G01C21/1652—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation combined with non-inertial navigation instruments with ranging devices, e.g. LIDAR or RADAR
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
    • G01S19/00—Satellite radio beacon positioning systems; Determining position, velocity or attitude using signals transmitted by such systems
    • G01S19/38—Determining a navigation solution using signals transmitted by a satellite radio beacon positioning system
    • G01S19/39—Determining a navigation solution using signals transmitted by a satellite radio beacon positioning system the satellite radio beacon positioning system transmitting time-stamped messages, e.g. GPS [Global Positioning System], GLONASS [Global Orbiting Navigation Satellite System] or GALILEO
    • G01S19/42—Determining position
    • G01S19/45—Determining position by combining measurements of signals from the satellite radio beacon positioning system with a supplementary measurement
    • G01S19/47—Determining position by combining measurements of signals from the satellite radio beacon positioning system with a supplementary measurement the supplementary measurement being an inertial measurement, e.g. tightly coupled inertial
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06F—ELECTRIC DIGITAL DATA PROCESSING
    • G06F18/00—Pattern recognition
    • G06F18/20—Analysing
    • G06F18/25—Fusion techniques

Landscapes

  • Engineering & Computer Science (AREA)
  • Remote Sensing (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Automation & Control Theory (AREA)
  • Data Mining & Analysis (AREA)
  • Theoretical Computer Science (AREA)
  • Bioinformatics & Cheminformatics (AREA)
  • Evolutionary Computation (AREA)
  • General Engineering & Computer Science (AREA)
  • Evolutionary Biology (AREA)
  • Computer Vision & Pattern Recognition (AREA)
  • Bioinformatics & Computational Biology (AREA)
  • Artificial Intelligence (AREA)
  • Life Sciences & Earth Sciences (AREA)
  • Computer Networks & Wireless Communication (AREA)
  • Optical Radar Systems And Details Thereof (AREA)
  • Navigation (AREA)

Abstract

本发明公开了一种多传感器融合的定位方法及系统，涉及移动机器人三维地图构建与定位技术领域。所述定位方法包括：获取目标区域的先验地图和多传感器数据；根据先验地图和多传感器数据建立位姿因子；根据先验地图、惯性测量数据和激光雷达数据建立转换因子；根据GPS因子和转换因子，采用因子图优化方法确定优化位姿；判断优化位姿的协方差是否小于设定协方差阈值；若是，则根据优化位姿对目标区域进行建图；若否，则采用地图定位因子对优化位姿进行更新，得到优化更新位姿，并根据优化更新位姿对目标区域进行定位。本发明通过自适应选用地图定位因子进行因子图优化，显著提升多传感器融合的定位能力。

Description

一种多传感器融合的定位方法及系统
技术领域
本发明涉及移动机器人三维地图构建与定位技术领域，特别是涉及一种多传感器融合的定位方法及系统。
背景技术
同时定位与建图(Simultaneous Localization and Mapping，SLAM)是当前机器人导航定位领域具有研究前景的技术领域。其技术意义在于，机器人在未知环境中能够依赖于自身传感器确定自身在环境地图中的位置，同时可以根据传感器信息建立环境点云地图。激光雷达由于精度高，鲁棒性强，已经被广泛应用于自动驾驶、智慧物流、智能农业等多个领域中。但是单一的激光雷达SLAM技术仍存在一些问题，因此以激光雷达、全球定位系统(Global Positioning System，GPS)、惯性测量传感器(Inertial Measurement Unit，IMU)为主要传感器的多传感器融合SLAM成为了专家学者们的主要选择。
针对目前SLAM领域存在的问题，已有的解决方案有如下几种：
以正态分布转换(Normal Distributions Transform，NDT)算法作为前端匹配算法，使用原始点云基于概率匹配的方式与先验地图进行点云配准，虽然得到了比较好的定位结果，但是会由于原始点云数量巨大造成计算资源浪费与运算效率降低的问题。
激光雷达里程计(Lidar Odometry and Mapping in Real-time，LOAM)算法，其核心主要在于两个部分：特征提取和里程计解算，其提取了边缘点和平面点作为特征匹配的依据；然后通过两个高频率的里程计实现粗定位和低频率的里程计实现精定位。不过，LOAM算法没有后端优化，并且不能处理大规模的旋转变换，并且LOAM中使用IMU为点云配准提供初始值，相对于原始版本并没有取得很好的结果。
基于线、面特征匹配的轻量化激光雷达测距与地图构建(Lightweight andGround-Optimized Lidar Odometry and Mapping on Variable Terrain，LEGO_LOAM)算法，该算法相对于LOAM算法的提升主要在于轻量级和地面优化，在点云处理部分加入了分割模块去除地面点的干扰，然后利用剩余的特征点进行点云配准，提高了匹配效率，同时加入了后端优化的部分使得回环之后点云地图会更加精确，但是其并没有完全解决大尺度环境下激光SLAM算法高度漂移的问题。
通过增量平滑和映射实现紧耦合的激光雷达惯性里程计(Tightly-coupledLidar Inertial Odometry via Smoothing and Mapping，LIO-SAM)算法，紧耦合了多种不同传感器，通过优化包含激光雷达里程计因子，IMU预积分因子，GPS因子和回环因子来得到机器人的全局一致的位姿，但是在该算法的全局特征匹配方法中容易丢失部分点云信息，从而导致定位不精确。
发明内容
本发明的目的是提供一种多传感器融合的定位方法及系统，能够显著提升同时定位与建图能力。
为实现上述目的，本发明提供了如下方案：
一种多传感器融合的定位方法，包括：
获取目标区域的先验地图和多传感器数据；所述多传感器数据包括惯性测量数据、激光雷达数据和GPS数据；
根据所述先验地图和所述多传感器数据建立位姿因子；所述位姿因子包括GPS因子和地图定位因子；
根据所述先验地图、所述惯性测量数据和所述激光雷达数据建立转换因子；所述转换因子包括激光里程计因子、惯性测量传感器预积分因子和闭环因子；
根据所述GPS因子和所述转换因子，采用因子图优化方法确定所述目标区域的优化位姿；
判断所述优化位姿的协方差是否小于设定协方差阈值；
若是，则根据所述优化位姿对所述目标区域进行定位；
若否，则采用所述地图定位因子对所述优化位姿进行更新，得到优化更新位姿，并根据所述优化更新位姿对所述目标区域进行定位。
可选地，所述根据所述先验地图和所述多传感器数据建立位姿因子，具体包括：
对所述GPS数据进行滤波，将滤波后的GPS数据作为GPS因子；
根据所述先验地图、所述激光雷达数据、所述惯性测量数据和所述GPS因子进行正态分布转换点云配准定位，得到地图定位因子。
可选地，所述对所述GPS数据进行滤波，将滤波后的GPS数据作为GPS因子，具体包括：
将所述GPS数据进行格式转换，得到局部笛卡尔坐标系下的GPS数据；
对所述局部笛卡尔坐标系下的GPS数据进行扩展卡尔曼滤波，得到GPS因子。
可选地，所述根据所述先验地图、所述激光雷达数据、所述惯性测量数据和所述GPS因子进行正态分布转换点云配准定位，得到地图定位因子，具体包括：
对所述激光雷达数据进行降采样，得到降采样后的激光雷达数据；
根据所述降采样后的激光雷达数据、所述惯性测量数据和所述GPS因子构建局部地图；
将所述先验地图与所述局部地图进行正态分布转换点云配准定位，得到地图定位因子。
可选地，所述根据所述先验地图、所述惯性测量数据和所述激光雷达数据建立转换因子，具体包括：
根据所述激光雷达数据和所述先验地图进行特征点匹配运算，得到激光里程计因子；
根据所述惯性测量数据和所述激光雷达数据进行预积分运算，得到惯性测量传感器预积分因子；
根据所述激光里程计因子和所述惯性测量数据进行回环检测，并根据回环检测结果构建闭环因子。
可选地，所述根据所述激光雷达数据和所述先验地图进行特征点匹配运算，得到激光里程计因子，具体包括：
对所述激光雷达数据进行特征提取，得到当前帧特征点；
对所述先验地图进行特征提取，得到地图特征点；
将所述地图特征点和所述当前帧特征点进行特征点匹配运算，得到激光里程计因子。
可选地，所述根据所述惯性测量数据和所述激光雷达数据进行预积分运算，得到惯性测量传感器预积分因子，具体包括：
对所述激光雷达数据进行关键帧提取，得到激光雷达关键帧信息；
根据所述惯性测量数据和所述激光雷达关键帧信息进行预积分运算，得到惯性测量传感器预积分因子。
可选地，所述根据所述激光里程计因子和所述惯性测量数据进行回环检测，并根据回环检测结果构建闭环因子，具体包括：
对所述激光里程计因子进行位姿优化，得到当前帧优化位姿；
将所述当前帧优化位姿和所述惯性测量数据进行数据融合，得到融合后的当前帧位姿；
对所述融合后的当前帧位姿进行回环检测，当所述融合后的当前帧位姿满足所述回环检测的判别条件时，构建闭环因子。
可选地，所述根据所述GPS因子和所述转换因子，采用因子图优化方法确定所述目标区域的优化位姿，具体包括：
根据所述GPS因子、所述激光里程计因子、所述惯性测量传感器预积分因子和所述闭环因子构建第一因子图模型；
对所述第一因子图模型进行优化，得到所述目标区域的优化位姿。
本发明还提供了一种多传感器融合的定位系统，包括：
数据获取单元，用于获取目标区域的先验地图和多传感器数据；所述多传感器数据包括惯性测量数据、激光雷达数据和GPS数据；
位姿因子构建单元，用于根据所述先验地图和所述多传感器数据建立位姿因子；所述位姿因子包括GPS因子和地图定位因子；
转换因子构建单元，用于根据所述先验地图、所述惯性测量数据和所述激光雷达数据建立转换因子；所述转换因子包括激光里程计因子、惯性测量传感器预积分因子和闭环因子；
优化位姿确定单元，用于根据所述GPS因子和所述转换因子，采用因子图优化方法确定所述目标区域的优化位姿；
比较单元，用于判断所述优化位姿的协方差是否小于设定协方差阈值；
第一定位单元，用于当所述优化位姿的协方差小于设定协方差阈值时，根据所述优化位姿对所述目标区域进行定位；
第二定位单元，用于当所述优化位姿的协方差不小于设定协方差阈值时，采用所述地图定位因子对所述优化位姿进行更新，得到优化更新位姿，并根据所述优化更新位姿对所述目标区域进行定位。
根据本发明提供的具体实施例，本发明公开了以下技术效果：
本发明公开了一种多传感器融合的定位方法及系统，在获取先验地图和多传感器数据后，经过运算获得位姿因子和转换因子，其中位姿因子包括GPS因子和地图定位因子；转换因子包括激光里程计因子、惯性测量传感器预积分因子和闭环因子。然后先通过GPS因子和转换因子确定目标区域的优化位姿，再将优化位姿的协方差与设定协方差阈值进行比较，根据比较结果判断是否需要加入地图定位因子对优化位姿进行更新，进而完成定位。本发明不仅通过地图定位因子更新优化位姿，解决了现有技术中前端关键帧特征提取点云损失带来的定位精度损失问题，而且通过多传感器数据融合，解决了现有技术中采用纯NDT定位带来的计算资源耗费效率低问题、纯激光雷达里程计的姿态角估计偏差问题和高度误差累积问题，还通过因子图优化，解决了大尺度环境下激光SLAM算法高度漂移的问题，显著提升了定位能力。
附图说明
为了更清楚地说明本发明实施例或现有技术中的技术方案，下面将对实施例中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图仅仅是本发明的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动性的前提下，还可以根据这些附图获得其他的附图。
图1为本发明多传感器融合的定位方法的方法流程图；
图2为本发明多传感器融合的定位方法的逻辑示意图；
图3为本发明多传感器融合的定位方法的畸变校正示意图；
图4为本发明多传感器融合的定位方法的因子图优化示意图；
图5为本发明多传感器融合的定位系统的系统结构图。
具体实施方式
下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。
本发明的目的是提供一种多传感器融合的定位方法及系统，通过自适选择因子图优化方法，显著提升基于激光雷达、惯性测量传感器和全球定位系统的多传感器融合的同时定位与建图能力。
为使本发明的上述目的、特征和优点能够更加明显易懂，下面结合附图和具体实施方式对本发明作进一步详细的说明。
如图1-图2所示，本发明实施例提供的一种多传感器融合的定位方法，包括：
步骤100：获取目标区域的先验地图和多传感器数据；所述多传感器数据包括惯性测量数据、激光雷达数据和GPS数据。
步骤200：根据所述先验地图和所述多传感器数据建立位姿因子；所述位姿因子包括GPS因子和地图定位因子。
步骤300：根据所述先验地图、所述惯性测量数据和所述激光雷达数据建立转换因子；所述转换因子包括激光里程计因子、惯性测量传感器预积分因子和闭环因子。
步骤400：根据所述GPS因子和所述转换因子，采用因子图优化方法确定所述目标区域的优化位姿。
步骤500：判断所述优化位姿的协方差是否小于设定协方差阈值。若是则执行步骤600，若否则执行步骤700。
步骤600：根据所述优化位姿对所述目标区域进行定位。
步骤700：采用所述地图定位因子对所述优化位姿进行更新，得到优化更新位姿，并根据所述优化更新位姿对所述目标区域进行定位。
其中在步骤100中，所述获取目标区域的多传感器数据具体包括：通过手眼标定方程对激光雷达和惯性测量传感器进行联合标定，并通过联合标定后的激光雷达获得激光雷达数据，通过联合标定后的惯性测量传感器获得惯性测量数据，通过全球定位系统获得GPS数据。所述激光雷达数据为通过激光雷达测得的原始点云数据。所述先验地图是基于NDT算法所建立的环境地图。
其中所述步骤200具体包括：
对所述GPS数据进行滤波，将滤波后的GPS数据作为GPS因子。这一步具体包括：将所述GPS数据进行格式转换，得到局部笛卡尔坐标系下的GPS数据；对所述局部笛卡尔坐标系下的GPS数据进行扩展卡尔曼滤波，得到GPS因子。
根据所述先验地图、所述激光雷达数据、所述惯性测量数据和所述GPS因子进行正态分布转换点云配准定位，得到地图定位因子。这一步具体包括：对所述激光雷达数据进行降采样，得到降采样后的激光雷达数据；根据所述降采样后的激光雷达数据、所述惯性测量数据和所述GPS因子构建局部地图；将所述先验地图与所述局部地图进行正态分布转换点云配准定位，得到地图定位因子。所述地图定位因子为(x,y,z)格式的初始化位姿。
其中所述步骤300具体包括：
根据所述激光雷达数据和所述先验地图进行特征点匹配运算，得到激光里程计因子。这一步具体包括：对所述激光雷达数据进行特征提取，得到当前帧特征点；对所述先验地图进行特征提取，得到地图特征点；将所述地图特征点和所述当前帧特征点进行特征点匹配运算，得到激光里程计因子。
根据所述惯性测量数据和所述激光雷达数据进行预积分运算，得到惯性测量传感器预积分因子。这一步具体包括：对所述激光雷达数据进行关键帧提取，得到激光雷达关键帧信息；根据所述惯性测量数据和所述激光雷达关键帧信息进行预积分运算，得到惯性测量传感器预积分因子。
根据所述激光里程计因子和所述惯性测量数据进行回环检测，并根据回环检测结果构建闭环因子。这一步具体包括：对所述激光里程计因子进行位姿优化，得到当前帧优化位姿；将所述当前帧优化位姿和所述惯性测量数据进行数据融合，得到融合后的当前帧位姿；对所述融合后的当前帧位姿进行回环检测，当所述融合后的当前帧位姿满足所述回环检测的判别条件时，构建闭环因子。
其中，所述对所述激光雷达数据进行特征提取，得到当前帧特征点，具体包括：
对所述激光雷达数据进行运动畸变校正，得到当前帧激光点云数据。将所述当前帧激光点云数据均分为若干部分，根据每部分所述当前帧激光点云数据中的曲率确定当前帧激光点云数据的边缘点和平面点，以及根据边缘点和平面点确定当前帧特征点。
其中，所述对所述激光雷达数据进行运动畸变校正，得到当前帧激光点云数据，具体包括：
以已知的第k-2帧、第k-1帧、第k帧的激光点为例，其中第k-2帧中的激光点使用e来表示，激光点est表示第k-2的第1个激光点、激光点eend表示第k-2的最后1个激光点，同理第k-1帧中的激光点使用i来表示，第k帧中的激光点使用j来表示；第k-1帧中第ist个激光点和第i个激光点的激光雷达位姿分别使用
Figure BDA0003628812700000091
(或
Figure BDA0003628812700000092
)和
Figure BDA0003628812700000093
来表示，第k帧中第jst个激光点和第j个激光点的激光雷达位姿分别使用
Figure BDA0003628812700000094
和
Figure BDA0003628812700000095
(或
Figure BDA0003628812700000096
)来表示；第k-1帧到第k帧的激光雷达变换矩阵平移分量为
Figure BDA0003628812700000097
则第k帧的j激光点到jend激光点的平移分量由k-2帧的时刻到k-1帧的时刻的平移分量和k-1帧的时刻到k帧的时刻的平移分量的线性插值表示，为
Figure BDA0003628812700000098
第k帧的j激光点到jend激光点的旋转分量由IMU测量差值表示，为
Figure BDA0003628812700000099
将
Figure BDA00036288127000000910
和
Figure BDA00036288127000000911
表示为激光雷达运动关系矩阵
Figure BDA00036288127000000912
最后，得到除去运动畸变的第k帧的激光点云数据。如图3所示。
其中，所述将所述当前帧激光点云数据均分为若干部分，根据每部分所述当前帧激光点云数据中的曲率确定当前帧激光点云数据的边缘点和平面点，以及根据边缘点和平面点确定当前帧特征点，具体包括：
激光雷达数据和先验地图的特征点包括边缘点和平面点，首先将去除运动畸变的激光点云数据中的一帧扫描内的一条扫描线均分为六部分，对于每一部分点云，求解其中每条线束中每个点的曲率，曲率求解公式为：
Figure BDA00036288127000000913
式中，k表示激光雷达的扫描帧数，k∈Z+；
Figure BDA00036288127000000914
表示第k-1帧的激光点在雷达坐标系下的数据；
Figure BDA00036288127000000915
表示第k帧的激光点在雷达坐标系下的数据；i∈Pk；j∈Pk；Pk表示代表了第k个扫描周期内获取到的所有点云数据；S表示激光雷达在同一扫描中返回的连续点的集合。
将c>m的点作为边缘点，提取到边缘点集，c<m的点作为平面点，提取到平面点集，由此得到当前帧特征点。m为设定曲率阈值。
对前后两次的激光雷达的扫描帧对应的特征点进行配准，得到激光雷达的帧间变换矩阵
Figure BDA00036288127000000916
即为激光雷达里程计因子。
其中，所述对所述融合后的当前帧位姿进行回环检测，当所述融合后的当前帧位姿满足所述回环检测的判别条件时，构建闭环因子，具体包括：
对所述融合后的当前帧位姿和历史关键帧在设定回环距离中进行回环检测，当所述融合后的当前帧位姿满足所述回环检测的判别条件时，构建所述闭环因子。所述历史关键帧为所述当前帧之前每一帧的位姿集合。
其中所述步骤400具体包括：
根据所述GPS因子、所述激光里程计因子、所述惯性测量传感器预积分因子和所述闭环因子构建第一因子图模型。所述第一因子图模型表示为B＝(F，X，J，P，G，H)，其中F表示因子图模型关键帧，F＝1,2,3…K；变量节点X＝{X1,X2,X3,…,XK}，为第1～第K个关键帧时刻激光雷达在世界坐标系下的位姿；因子节点J＝{J1,J2,J3,...,JK}，为第1～K帧的时刻下的激光里程计因子；因子节点P＝{P1,P2,P3,...,PK}，为第1～K帧的时刻下的惯性测量传感器预积分因子；因子节点G＝{G1,G2,G3,...,GK}，为第1～K帧的时刻下的GPS因子；因子节点H＝{H1,H2,H3,...,HK}，为第1～K帧的时刻下的闭环因子。如图4所示。
对所述第一因子图模型进行优化，得到所述目标区域的优化位姿。这一步具体包括：对所述第一因子图模型进行增量式平滑与映射运算，得到所述目标区域的优化位姿。所述目标区域的优化位姿是激光雷达在世界坐标系下的位姿。
其中在所述步骤700中，所述采用所述地图定位因子对所述优化位姿进行更新，得到优化更新位姿，并根据所述优化更新位姿对所述目标区域进行定位，具体包括：
根据所述地图定位因子和所述第一因子图模型构建第二因子图模型。所述第二因子图模型表示为B＝(F，X，J，P，G，D，H)，其中因子节点D＝{D1,D2,D3,...,DK}，为第1～K帧的时刻下的地图定位因子。
根据所述第二因子图模型对所述优化位姿进行更新，得到优化更新位姿，并根据所述优化更新位姿对所述目标区域进行定位。
如图5所示，本发明实施例提供的一种多传感器融合的定位系统，包括：数据获取单元1、位姿因子构建单元2、转换因子构建单元3、优化位姿确定单元4、比较单元5、第一定位单元6和第二定位单元7。
所述数据获取单元1用于获取目标区域的先验地图和多传感器数据；所述多传感器数据包括惯性测量数据、激光雷达数据和GPS数据。
所述位姿因子构建单元2用于根据所述先验地图和所述多传感器数据建立位姿因子；所述位姿因子包括GPS因子和地图定位因子。
所述转换因子构建单元3用于根据所述先验地图、所述惯性测量数据和所述激光雷达数据建立转换因子；所述转换因子包括激光里程计因子、惯性测量传感器预积分因子和闭环因子。
所述优化位姿确定单元4用于根据所述GPS因子和所述转换因子，采用因子图优化方法确定所述目标区域的优化位姿。
所述比较单元5用于判断所述优化位姿的协方差是否小于设定协方差阈值。
所述第一定位单元6用于若当所述优化位姿的协方差小于设定协方差阈值时，根据所述优化位姿对所述目标区域进行定位。
所述第二定位单元7用于若当所述优化位姿的协方差不小于设定协方差阈值时，采用所述地图定位因子对所述优化位姿进行更新，得到优化更新位姿，并根据所述优化更新位姿对所述目标区域进行定位。
本说明书中各个实施例采用递进的方式描述，每个实施例重点说明的都是与其他实施例的不同之处，各个实施例之间相同相似部分互相参见即可。对于实施例公开的系统而言，由于其与实施例公开的方法相对应，所以描述的比较简单，相关之处参见方法部分说明即可。
本文中应用了具体个例对本发明的原理及实施方式进行了阐述，以上实施例的说明只是用于帮助理解本发明的方法及其核心思想；同时，对于本领域的一般技术人员，依据本发明的思想，在具体实施方式及应用范围上均会有改变之处。综上所述，本说明书内容不应理解为对本发明的限制。

Claims (10)

1.一种多传感器融合的定位方法，其特征在于，包括：
获取目标区域的先验地图和多传感器数据；所述多传感器数据包括惯性测量数据、激光雷达数据和GPS数据；
根据所述先验地图和所述多传感器数据建立位姿因子；所述位姿因子包括GPS因子和地图定位因子；
根据所述先验地图、所述惯性测量数据和所述激光雷达数据建立转换因子；所述转换因子包括激光里程计因子、惯性测量传感器预积分因子和闭环因子；
根据所述GPS因子和所述转换因子，采用因子图优化方法确定所述目标区域的优化位姿；
判断所述优化位姿的协方差是否小于设定协方差阈值；
若是，则根据所述优化位姿对所述目标区域进行定位；
若否，则采用所述地图定位因子对所述优化位姿进行更新，得到优化更新位姿，并根据所述优化更新位姿对所述目标区域进行定位。
2.根据权利要求1所述的多传感器融合的定位方法，其特征在于，所述根据所述先验地图和所述多传感器数据建立位姿因子，具体包括：
对所述GPS数据进行滤波，将滤波后的GPS数据作为GPS因子；
根据所述先验地图、所述激光雷达数据、所述惯性测量数据和所述GPS因子进行正态分布转换点云配准定位，得到地图定位因子。
3.根据权利要求2所述的多传感器融合的定位方法，其特征在于，所述对所述GPS数据进行滤波，将滤波后的GPS数据作为GPS因子，具体包括：
将所述GPS数据进行格式转换，得到局部笛卡尔坐标系下的GPS数据；
对所述局部笛卡尔坐标系下的GPS数据进行扩展卡尔曼滤波，得到GPS因子。
4.根据权利要求2所述的多传感器融合的定位方法，其特征在于，所述根据所述先验地图、所述激光雷达数据、所述惯性测量数据和所述GPS因子进行正态分布转换点云配准定位，得到地图定位因子，具体包括：
对所述激光雷达数据进行降采样，得到降采样后的激光雷达数据；
根据所述降采样后的激光雷达数据、所述惯性测量数据和所述GPS因子构建局部地图；
将所述先验地图与所述局部地图进行正态分布转换点云配准定位，得到地图定位因子。
5.根据权利要求1所述的多传感器融合的定位方法，其特征在于，所述根据所述先验地图、所述惯性测量数据和所述激光雷达数据建立转换因子，具体包括：
根据所述激光雷达数据和所述先验地图进行特征点匹配运算，得到激光里程计因子；
根据所述惯性测量数据和所述激光雷达数据进行预积分运算，得到惯性测量传感器预积分因子；
根据所述激光里程计因子和所述惯性测量数据进行回环检测，并根据回环检测结果构建闭环因子。
6.根据权利要求5所述的多传感器融合的定位方法，其特征在于，所述根据所述激光雷达数据和所述先验地图进行特征点匹配运算，得到激光里程计因子，具体包括：
对所述激光雷达数据进行特征提取，得到当前帧特征点；
对所述先验地图进行特征提取，得到地图特征点；
将所述地图特征点和所述当前帧特征点进行特征点匹配运算，得到激光里程计因子。
7.根据权利要求5所述的多传感器融合的定位方法，其特征在于，所述根据所述惯性测量数据和所述激光雷达数据进行预积分运算，得到惯性测量传感器预积分因子，具体包括：
对所述激光雷达数据进行关键帧提取，得到激光雷达关键帧信息；
根据所述惯性测量数据和所述激光雷达关键帧信息进行预积分运算，得到惯性测量传感器预积分因子。
8.根据权利要求5所述的多传感器融合的定位方法，其特征在于，所述根据所述激光里程计因子和所述惯性测量数据进行回环检测，并根据回环检测结果构建闭环因子，具体包括：
对所述激光里程计因子进行位姿优化，得到当前帧优化位姿；
将所述当前帧优化位姿和所述惯性测量数据进行数据融合，得到融合后的当前帧位姿；
对所述融合后的当前帧位姿进行回环检测，当所述融合后的当前帧位姿满足所述回环检测的判别条件时，构建闭环因子。
9.根据权利要求1所述的多传感器融合的定位方法，其特征在于，所述根据所述GPS因子和所述转换因子，采用因子图优化方法确定所述目标区域的优化位姿，具体包括：
根据所述GPS因子、所述激光里程计因子、所述惯性测量传感器预积分因子和所述闭环因子构建第一因子图模型；
对所述第一因子图模型进行优化，得到所述目标区域的优化位姿。
10.一种多传感器融合的定位系统，其特征在于，包括：
数据获取单元，用于获取目标区域的先验地图和多传感器数据；所述多传感器数据包括惯性测量数据、激光雷达数据和GPS数据；
位姿因子构建单元，用于根据所述先验地图和所述多传感器数据建立位姿因子；所述位姿因子包括GPS因子和地图定位因子；
转换因子构建单元，用于根据所述先验地图、所述惯性测量数据和所述激光雷达数据建立转换因子；所述转换因子包括激光里程计因子、惯性测量传感器预积分因子和闭环因子；
优化位姿确定单元，用于根据所述GPS因子和所述转换因子，采用因子图优化方法确定所述目标区域的优化位姿；
比较单元，用于判断所述优化位姿的协方差是否小于设定协方差阈值；
第一定位单元，用于当所述优化位姿的协方差小于设定协方差阈值时，根据所述优化位姿对所述目标区域进行定位；
第二定位单元，用于当所述优化位姿的协方差不小于设定协方差阈值时，采用所述地图定位因子对所述优化位姿进行更新，得到优化更新位姿，并根据所述优化更新位姿对所述目标区域进行定位。
CN202210484848.0A 2022-05-06 2022-05-06 一种多传感器融合的定位方法及系统 Active CN114777775B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202210484848.0A CN114777775B (zh) 2022-05-06 2022-05-06 一种多传感器融合的定位方法及系统

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202210484848.0A CN114777775B (zh) 2022-05-06 2022-05-06 一种多传感器融合的定位方法及系统

Publications (2)

Publication Number Publication Date
CN114777775A true CN114777775A (zh) 2022-07-22
CN114777775B CN114777775B (zh) 2024-06-14

Family

ID=82434483

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202210484848.0A Active CN114777775B (zh) 2022-05-06 2022-05-06 一种多传感器融合的定位方法及系统

Country Status (1)

Country Link
CN (1) CN114777775B (zh)

Cited By (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN115218892A (zh) * 2022-09-20 2022-10-21 成都睿芯行科技有限公司 一种基于多传感器融合的轻量级定位与建图方法

Citations (8)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
WO2020155616A1 (zh) * 2019-01-29 2020-08-06 浙江省北大信息技术高等研究院 一种基于数字视网膜的拍摄装置的定位方法
CN112268559A (zh) * 2020-10-22 2021-01-26 中国人民解放军战略支援部队信息工程大学 复杂环境下融合slam技术的移动测量方法
CN112304307A (zh) * 2020-09-15 2021-02-02 浙江大华技术股份有限公司 一种基于多传感器融合的定位方法、装置和存储介质
CN113066105A (zh) * 2021-04-02 2021-07-02 北京理工大学 激光雷达和惯性测量单元融合的定位与建图方法及系统
WO2021147546A1 (zh) * 2020-01-20 2021-07-29 深圳市普渡科技有限公司 多传感器融合slam系统、多传感器融合方法及机器人、介质
CN113340295A (zh) * 2021-06-16 2021-09-03 广东工业大学 一种多个测距传感器的无人艇近岸实时定位与建图方法
WO2021232470A1 (zh) * 2020-05-19 2021-11-25 北京数字绿土科技有限公司 基于多传感器融合的slam制图方法、系统
CN113781582A (zh) * 2021-09-18 2021-12-10 四川大学 基于激光雷达和惯导联合标定的同步定位与地图创建方法

Patent Citations (8)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
WO2020155616A1 (zh) * 2019-01-29 2020-08-06 浙江省北大信息技术高等研究院 一种基于数字视网膜的拍摄装置的定位方法
WO2021147546A1 (zh) * 2020-01-20 2021-07-29 深圳市普渡科技有限公司 多传感器融合slam系统、多传感器融合方法及机器人、介质
WO2021232470A1 (zh) * 2020-05-19 2021-11-25 北京数字绿土科技有限公司 基于多传感器融合的slam制图方法、系统
CN112304307A (zh) * 2020-09-15 2021-02-02 浙江大华技术股份有限公司 一种基于多传感器融合的定位方法、装置和存储介质
CN112268559A (zh) * 2020-10-22 2021-01-26 中国人民解放军战略支援部队信息工程大学 复杂环境下融合slam技术的移动测量方法
CN113066105A (zh) * 2021-04-02 2021-07-02 北京理工大学 激光雷达和惯性测量单元融合的定位与建图方法及系统
CN113340295A (zh) * 2021-06-16 2021-09-03 广东工业大学 一种多个测距传感器的无人艇近岸实时定位与建图方法
CN113781582A (zh) * 2021-09-18 2021-12-10 四川大学 基于激光雷达和惯导联合标定的同步定位与地图创建方法

Non-Patent Citations (3)

* Cited by examiner, † Cited by third party
Title
倪志康;厉茂海;林睿;孙立宁;刘仕琦;: "基于三维激光雷达与RTK融合的SLAM研究", 制造业自动化, no. 07, 25 July 2020 (2020-07-25), pages 57 - 60 *
柯艳国;王雄奇;魏新;吴贤斌;朱仲贤;董翔宇;黄杰;董二宝;: "一种基于图优化的实时3D激光SLAM算法", 机电一体化, no. 1, 15 April 2020 (2020-04-15), pages 32 - 38 *
王巍;浦云明;李旺;: "一种基于CI因子图的多机器人2D地图融合算法", 机器人, no. 06, 15 November 2017 (2017-11-15), pages 106 - 112 *

Cited By (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN115218892A (zh) * 2022-09-20 2022-10-21 成都睿芯行科技有限公司 一种基于多传感器融合的轻量级定位与建图方法

Also Published As

Publication number Publication date
CN114777775B (zh) 2024-06-14

Similar Documents

Publication Publication Date Title
CN113781582B (zh) 基于激光雷达和惯导联合标定的同步定位与地图创建方法
CN106127739B (zh) 一种结合单目视觉的rgb-d slam方法
Huang Review on LiDAR-based SLAM techniques
CN102313547B (zh) 基于手绘轮廓语义地图的移动机器人视觉导航方法
CN110361027A (zh) 基于单线激光雷达与双目相机数据融合的机器人路径规划方法
CN112634451A (zh) 一种融合多传感器的室外大场景三维建图方法
CN113706626B (zh) 一种基于多传感器融合及二维码校正的定位与建图方法
CN115407357B (zh) 基于大场景的低线束激光雷达-imu-rtk定位建图算法
CN115372989A (zh) 基于激光雷达的越野自动小车长距离实时定位系统及方法
CN108549376A (zh) 一种基于信标的导航定位方法及系统
CN114526745A (zh) 一种紧耦合激光雷达和惯性里程计的建图方法及系统
CN112965063A (zh) 一种机器人建图定位方法
CN114004869A (zh) 一种基于3d点云配准的定位方法
CN113763549B (zh) 融合激光雷达和imu的同时定位建图方法、装置和存储介质
CN115272596A (zh) 一种面向单调无纹理大场景的多传感器融合slam方法
CN114689035A (zh) 基于多传感器融合的长航程农田地图构建方法及系统
CN116429116A (zh) 一种机器人定位方法及设备
CN114777775A (zh) 一种多传感器融合的定位方法及系统
CN114993298A (zh) 基于ekf的模板匹配vo与轮式里程计融合定位方法
WO2024120269A1 (zh) 一种融合点云地图、运动模型和局部特征的位置识别方法
CN114565726A (zh) 一种非结构化动态环境下的同时定位与建图方法
CN117367427A (zh) 一种适用于室内环境中的视觉辅助激光融合IMU的多模态slam方法
CN117029870A (zh) 一种基于路面点云的激光里程计
CN116878542A (zh) 一种抑制里程计高度漂移的激光slam方法
CN117053779A (zh) 一种基于冗余关键帧去除的紧耦合激光slam方法及装置

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