CN114777775B - 一种多传感器融合的定位方法及*** - Google Patents

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

Info

Publication number
CN114777775B
CN114777775B CN202210484848.0A CN202210484848A CN114777775B CN 114777775 B CN114777775 B CN 114777775B CN 202210484848 A CN202210484848 A CN 202210484848A CN 114777775 B CN114777775 B CN 114777775B
Authority
CN
China
Prior art keywords
factor
data
pose
positioning
map
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
CN202210484848.0A
Other languages
English (en)
Other versions
CN114777775A (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

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个激光点的激光雷达位姿分别使用(或/>)和/>来表示，第k帧中第jst个激光点和第j个激光点的激光雷达位姿分别使用/>和/>(或/>)来表示；第k-1帧到第k帧的激光雷达变换矩阵平移分量为/>则第k帧的j激光点到jend激光点的平移分量由k-2帧的时刻到k-1帧的时刻的平移分量和k-1帧的时刻到k帧的时刻的平移分量的线性插值表示，为/>第k帧的j激光点到jend激光点的旋转分量由IMU测量差值表示，为/>将/>和/>表示为激光雷达运动关系矩阵/>最后，得到除去运动畸变的第k帧的激光点云数据。如图3所示。
其中，所述将所述当前帧激光点云数据均分为若干部分，根据每部分所述当前帧激光点云数据中的曲率确定当前帧激光点云数据的边缘点和平面点，以及根据边缘点和平面点确定当前帧特征点，具体包括：
激光雷达数据和先验地图的特征点包括边缘点和平面点，首先将去除运动畸变的激光点云数据中的一帧扫描内的一条扫描线均分为六部分，对于每一部分点云，求解其中每条线束中每个点的曲率，曲率求解公式为：
式中，k表示激光雷达的扫描帧数，k∈Z+；表示第k-1帧的激光点在雷达坐标系下的数据；/>表示第k帧的激光点在雷达坐标系下的数据；i∈Pk；j∈Pk；Pk表示代表了第k个扫描周期内获取到的所有点云数据；S表示激光雷达在同一扫描中返回的连续点的集合。
将c>m的点作为边缘点，提取到边缘点集，c<m的点作为平面点，提取到平面点集，由此得到当前帧特征点。m为设定曲率阈值。
对前后两次的激光雷达的扫描帧对应的特征点进行配准，得到激光雷达的帧间变换矩阵即为激光雷达里程计因子。
其中，所述对所述融合后的当前帧位姿进行回环检测，当所述融合后的当前帧位姿满足所述回环检测的判别条件时，构建闭环因子，具体包括：
对所述融合后的当前帧位姿和历史关键帧在设定回环距离中进行回环检测，当所述融合后的当前帧位姿满足所述回环检测的判别条件时，构建所述闭环因子。所述历史关键帧为所述当前帧之前每一帧的位姿集合。
其中所述步骤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 CN114777775A (zh) 2022-07-22
CN114777775B true 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)

Families Citing this family (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN115218892A (zh) * 2022-09-20 2022-10-21 成都睿芯行科技有限公司 一种基于多传感器融合的轻量级定位与建图方法
CN117470230A (zh) * 2023-10-23 2024-01-30 广州创源机器人有限公司 基于深度学习的视觉激光传感器融合定位算法

Citations (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN112304307A (zh) * 2020-09-15 2021-02-02 浙江大华技术股份有限公司 一种基于多传感器融合的定位方法、装置和存储介质
CN113781582A (zh) * 2021-09-18 2021-12-10 四川大学 基于激光雷达和惯导联合标定的同步定位与地图创建方法

Family Cites Families (6)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN110009739B (zh) * 2019-01-29 2023-03-24 浙江省北大信息技术高等研究院 移动摄像机的数字视网膜的运动特征的提取与编码方法
CN111258313B (zh) * 2020-01-20 2022-06-07 深圳市普渡科技有限公司 多传感器融合slam***及机器人
CN111561923B (zh) * 2020-05-19 2022-04-15 北京数字绿土科技股份有限公司 基于多传感器融合的slam制图方法、***
CN112268559B (zh) * 2020-10-22 2023-03-28 中国人民解放军战略支援部队信息工程大学 复杂环境下融合slam技术的移动测量方法
CN113066105B (zh) * 2021-04-02 2022-10-21 北京理工大学 激光雷达和惯性测量单元融合的定位与建图方法及***
CN113340295B (zh) * 2021-06-16 2021-12-21 广东工业大学 一种多个测距传感器的无人艇近岸实时定位与建图方法

Patent Citations (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN112304307A (zh) * 2020-09-15 2021-02-02 浙江大华技术股份有限公司 一种基于多传感器融合的定位方法、装置和存储介质
CN113781582A (zh) * 2021-09-18 2021-12-10 四川大学 基于激光雷达和惯导联合标定的同步定位与地图创建方法

Also Published As

Publication number Publication date
CN114777775A (zh) 2022-07-22

Similar Documents

Publication Publication Date Title
CN108759833B (zh) 一种基于先验地图的智能车辆定位方法
CN114777775B (zh) 一种多传感器融合的定位方法及***
CN113781582B (zh) 基于激光雷达和惯导联合标定的同步定位与地图创建方法
CN112965063B (zh) 一种机器人建图定位方法
CN112268559B (zh) 复杂环境下融合slam技术的移动测量方法
CN111486845B (zh) 基于海底地形匹配的auv多策略导航方法
CN113066105A (zh) 激光雷达和惯性测量单元融合的定位与建图方法及***
CN115407357B (zh) 基于大场景的低线束激光雷达-imu-rtk定位建图算法
CN110187375A (zh) 一种基于slam定位结果提高定位精度的方法及装置
CN114526745A (zh) 一种紧耦合激光雷达和惯性里程计的建图方法及***
CN105973265A (zh) 一种基于激光扫描传感器的里程估计方法
CN113920198B (zh) 一种基于语义边缘对齐的由粗到精的多传感器融合定位方法
CN113763549A (zh) 融合激光雷达和imu的同时定位建图方法、装置和存储介质
CN116608873A (zh) 一种自动驾驶车辆的多传感器融合定位建图方法
CN115908539A (zh) 一种目标体积自动测量方法和装置、存储介质
CN115728803A (zh) 一种城市驾驶车辆连续定位***及方法
CN112612034B (zh) 基于激光帧和概率地图扫描的位姿匹配方法
CN114565726A (zh) 一种非结构化动态环境下的同时定位与建图方法
CN117392268A (zh) 一种基于自适应结合cpd和icp算法的激光扫描建图方法及***
CN117029870A (zh) 一种基于路面点云的激光里程计
CN116878542A (zh) 一种抑制里程计高度漂移的激光slam方法
CN115103299B (zh) 一种基于rfid的多传感器融合定位方法
CN115754977A (zh) 一种用于智能桥式起重机的重定位方法
CN113379915B (zh) 一种基于点云融合的行车场景构建方法
CN113554705B (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
GR01 Patent grant
GR01 Patent grant