CN117268408A - 一种激光slam定位方法及系统 - Google Patents

一种激光slam定位方法及系统 Download PDF

Info

Publication number
CN117268408A
CN117268408A CN202310961887.XA CN202310961887A CN117268408A CN 117268408 A CN117268408 A CN 117268408A CN 202310961887 A CN202310961887 A CN 202310961887A CN 117268408 A CN117268408 A CN 117268408A
Authority
CN
China
Prior art keywords
vehicle
factor
laser
gnss
road side
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
CN202310961887.XA
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.)
Huaneng Yimin Coal and Electricity Co Ltd
Original Assignee
Huaneng Yimin Coal and Electricity Co Ltd
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 Huaneng Yimin Coal and Electricity Co Ltd filed Critical Huaneng Yimin Coal and Electricity Co Ltd
Priority to CN202310961887.XA priority Critical patent/CN117268408A/zh
Publication of CN117268408A publication Critical patent/CN117268408A/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/26—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 specially adapted for navigation in a road network
    • G01C21/28—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 specially adapted for navigation in a road network with correlation of data from several navigational 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/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/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
    • 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/183—Compensation of inertial measurements, e.g. for temperature effects
    • G01C21/188—Compensation of inertial measurements, e.g. for temperature effects for accumulated errors, e.g. by coupling inertial systems with absolute positioning systems
    • 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/26—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 specially adapted for navigation in a road network
    • G01C21/34—Route searching; Route guidance
    • G01C21/3446—Details of route searching algorithms, e.g. Dijkstra, A*, arc-flags, using precalculated routes
    • 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
    • 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
    • G01S17/00—Systems using the reflection or reradiation of electromagnetic waves other than radio waves, e.g. lidar systems
    • G01S17/02—Systems using the reflection of electromagnetic waves other than radio waves
    • G01S17/06—Systems determining position data of a target
    • 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
    • G01S17/00—Systems using the reflection or reradiation of electromagnetic waves other than radio waves, e.g. lidar systems
    • G01S17/86—Combinations of lidar systems with systems other than lidar, radar or sonar, e.g. with direction finders
    • 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/48—Determining position by combining or switching between position solutions derived from the satellite radio beacon positioning system and position solutions derived from a further system
    • 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/48—Determining position by combining or switching between position solutions derived from the satellite radio beacon positioning system and position solutions derived from a further system
    • G01S19/49—Determining position by combining or switching between position solutions derived from the satellite radio beacon positioning system and position solutions derived from a further system whereby the further system is an inertial position system, e.g. loosely-coupled
    • 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)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Automation & Control Theory (AREA)
  • Computer Networks & Wireless Communication (AREA)
  • Electromagnetism (AREA)
  • Optical Radar Systems And Details Thereof (AREA)

Abstract

本发明涉及矿区自动驾驶领域，公开了一种激光slam定位方法包括，通过预积分因子获取从初始状态到当前状态的运动信息，利用激光雷达里程计算法计算设备的位置和姿态，并通过GNSS先验因子提升激光slam定位精度，通过车辆位置先验因子和车辆动力学校验，得出需要优化的车辆数据，通过因子图算法进行系统优化。本方法避免了在特征点稀疏的矿区场景提取点线特征，增强了激光slam系统的鲁棒性，在矿区桥遂场景中，路侧设备可以有效约束激光里程计漂移。

Description

一种激光slam定位方法及系统
技术领域
本发明涉及矿区自动驾驶领域，具体涉及一种激光slam定位方法。
背景技术
高精度定位是无人驾驶的关键技术之一，是无人驾驶车辆控制与环境感知的重要基础。
当前，露天矿场景多采用GNSS+IMU+RTK的组合导航技术方案来实现无人驾驶车辆的高精度定位，该方案具有实施简单、覆盖范围广等特点。露天矿场景中还存在隧道、天桥、室内破碎站等工作场景，GNSS+IMU+RTK的组合导航方案难以在这类场景中工作，GNSS在该类场景中会出现卫星丢失无法使用，IMU无法胜任长时间的位姿推算，易产生漂移问题。为解决这个问题，一些方案采取了GNSS+IMU+RTK+Lidar多传感器组合定位的方式，构建矿区的点云地图、语义地图或高精地图来实现车辆的高精度定位。但是与城市等其它场景不同的是，矿区场景由于环境特征点过于稀疏，常规的激光slam方案在矿区场景中经常发生特征退化而导致车辆位姿的漂移，导致定位系统无法使用。
为解决上述问题，本发明提出一种基于激光slam定位系统及方法，在slam系统中利用了直接法与车辆自身轮速构建更加精确的激光slam里程计，并利用车辆动力学约束来及时发现激光slam里程计的异常。同时，为了消除激光slam长时间运行带来的累计误差，本系统不仅融合了GNSS定位来修正累计误差，还利用路侧设备来修正激光slam里程计的累计漂移。
发明内容
本部分的目的在于概述本发明的实施例的一些方面以及简要介绍一些较佳实施例。在本部分以及本申请的说明书摘要和发明名称中可能会做些简化或省略以避免使本部分、说明书摘要和发明名称的目的模糊，而这种简化或省略不能用于限制本发明的范围。
鉴于上述现有存在的问题，提出了本发明。
因此，本发明提供了一种激光slam定位方法，能够具有较强的抗干扰能力，能够在复杂的环境中工作。
为解决上述技术问题，本发明提供如下技术方案，一种激光slam定位方法，包括：通过预积分因子获取从初始状态到当前状态的运动信息；利用激光雷达里程计算法计算设备的位置和姿态，并通过GNSS先验因子提升激光slam定位精度；通过车辆位置先验因子和车辆动力学校验，得出需要优化的车辆数据；通过因子图算法进行系统优化。
作为本发明所述的一种激光slam定位方法的一种优选方案，其中：所述预积分因子包括预积分是一种将惯性传感器的测量值进行积分的技术，用于获取从初始状态到当前状态的运动信息，通过对IMU测量进行积分，得到位置、速度和姿态等关键运动参数的估计值，IMU预积分的基本原理是将IMU测量的加速度和角速度进行离散时间的积分，加速度积分得到速度估计，角速度积分得到姿态估计，同时，利用加速度计的数据来估计地球的重力方向，从而使用姿态信息将加速度估计从IMU坐标系转换为世界坐标系，通过预积分，减少姿态估计过程中的误差累积，在预积分速度与z轴方向变量时，引入车辆自身轮速对速度变量进行约束，IMU的预积分结果将作为直接法激光雷达里程计帧间配准的先验位姿。
作为本发明所述的一种激光slam定位方法的一种优选方案，其中：所述激光雷达里程计算法包括直接法激光雷达里程计因子，包括以下步骤：
点云配准：首先通过最小化点云之间的差异，将连续帧的密集点云进行配准，估计两帧之间的相对运动，在配准时将IMU与轮速的预积分增量作为配准初始值；
运动估计：通过最小二乘法配准得到的相对运动，估计设备在每一帧之间的位移和旋转；
时序一致性：为了减少运动估计的漂移，引入时序一致性约束。
作为本发明所述的一种激光slam定位方法的一种优选方案，其中：所述GNSS先验因子包括根据GNSS测量值提供额外的约束条件，提升激光slam定位精度，GNSS测量车辆当前在全局坐标系下的位置与速度信息；
从激光雷达获取t时刻点云，设置时间阈值τ(τ>0)，通过最近邻查找的方法，找到t-τ到t+τ时间段内最近的两个时间戳tl、tr(tr>tl)，对应的GNSS组合惯导定位数据分别为对/>进行线性插值，计算t时刻车体后轴中心全局定位信息：
其中，a＝(tr-t)/(tr-tl)，b＝(t-tl)/(tr-tl)；
计算激光雷达全局定位信息：
其中，表示车体后轴坐标，将/>的位移部分作为GPS先验因子添加到因子图中。
作为本发明所述的一种激光slam定位方法的一种优选方案，其中：所述车辆位置先验因子包括车辆位置先验因子由路侧设备提供，当车辆进入到路侧设备感知视域时，通过感知算法率先识别车辆的包络，根据车辆包络，计算车辆在路侧设备坐标系下的车辆中心点坐标，根据下列公式，将路侧设备下的中心点坐标转换至世界系下，获得世界系下的车辆中心点坐标，将该坐标作为车辆的位置先验因子输入因子图中作为位置约束的条件之一：
其中，为路侧设备在世界坐标系下的位姿，/>为路侧设备坐标系下车辆中心坐标，/>为路侧设备感知到的车辆中心在世界坐标系下的位姿；
与GNSS先验因子相同的是，在计算车辆位置先验因子时，需要根据当前时间戳对车辆位置先验因子进行插值，差值获取激光雷达点云帧对应时刻的时间戳的位置作为车辆位置先验因子。
作为本发明所述的一种激光slam定位方法的一种优选方案，其中：所述车辆动力学校验包括从运动读取当前车辆编号，并加载当前车辆的动力学参数，获取当前车辆的最大加速度accmax与最大角速度gyromax值，订阅雷达里程计，获取从雷达里程计估计的当前车辆加速度accl与gyrol，由于车辆的实际运动不超过车辆的动力学约束，当accl大于accmax或gyrol大于gyromax时，则雷达里程计发生异常，在因子图优化中舍弃本次数据。
作为本发明所述的一种激光slam定位方法的一种优选方案，其中：所述因子图算法进行系统优化包括因子图由变量节点和因子节点组成，通过边连接在一起，在因子图优化中，变量节点表示问题中的变量，因子图算法的步骤为：
将车辆位姿估计问题转化为因子图表示，在因子图中添加各因子节点，每个因子节点与相关的变量节点连接，为变量节点设置初始估计值，通过迭代更新变量节点的估计值来优化因子图，当检测到GNSS先验因子或路侧设备感知获得的车辆先验位置因子被添加到因子图模型后，将因子图模型进行迭代优化，其余时刻只维护因子图当中的因子变量，不执行优化环节，点云帧间配准的距离误差作为因子图的收敛的判定条件，如果未达到收敛，则返回继续迭代，通过计算对偶残差的变化量DRS来判断算法的收敛，当DRS残差变化量小于阈值DRS_th时，算法收敛，输出最终收敛的车辆位姿估计，作为问题的最优解。
本发明的另一个目的是提供一种激光slam定位方法，其能可以在未知环境中实现自主探索和地图构建，并且可以利用构建的地图进行定位和导航。
一种激光slam定位方法，其特征在于：激光雷达传感器、IMU传感器、车辆轮速传感器、GNSS传感器、路侧设备传感器；
激光雷达传感器，通过发射激光束，并测量返回激光束的时间和强度，以获取周围环境的三维点云数据，点云数据可用于构建环境地图，并且可以通过与先前的点云数据匹配来进行定位，激光雷达提供了高精度的距离和方向信息，对于实时的定位和建图非常关键。
IMU传感器，IMU与轮速进行预计分获得100hz高频位姿增量信息，将预积分的增量信息作为激光雷达帧间配准的初始值，获得精度更高的帧间配准结果。
车辆轮速传感器，车辆轮速是从车辆的车轮运动中获得的信息，通过监测车轮的转速和里程计信息，估计车辆在空间中的运动，车辆轮速信息通常与激光雷达数据进行融合，用于改善定位的精度和鲁棒性，根据车辆模型和运动学原理，结合车辆轮速估计车辆的姿态变化和位置移动。
GNSS传感器，GNSS接收器接收卫星发射的信号，并通过测量信号传播时间和多普勒效应来确定接收器的位置，在激光SLAM中，GNSS用于提供车辆在全球地理坐标系下的初始位置估计，这对于快速初始化SLAM系统并进行全局定位非常有用。
路侧设备传感器，实时获得自身精确的厘米级位姿坐标，同时路侧设备上面安装了相机与雷达，在无人驾驶车辆走进路侧设备感知视野时，实时的识别无人驾驶车辆，并计算车辆中心坐标。
一种计算机设备，包括存储器和处理器，所述存储器存储有计算机程序，其特征在于，所述处理器执行所述计算机程序时实现一种激光slam定位方法的步骤。
一种计算机可读存储介质，其上存储有计算机程序，其特征在于，所述计算机程序被处理器执行时实现一种激光slam定位方法的步骤。
本发明的有益效果：在slam系统中利用了直接法与车辆自身轮速构建更加精确的激光slam里程计，避免了在特征点稀疏的矿区场景提取点线特征，增强了激光slam系统的鲁棒性，在低特征区域车辆位姿不会漂移。利用车辆动力学约束来及时发现激光slam里程计的异常。利用路侧设备来感知车辆中心点坐标，并将其作为先验因子来约束激光slam的累计漂移，减小激光里程计累计误差。尤其是在矿区桥遂场景中，GNSS无法有效工作，不能将GNSS作为先验因子的时候，路侧设备可以有效约束激光里程计漂移。
附图说明
为了更清楚地说明本发明实施例的技术方案，下面将对实施例描述中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图仅仅是本发明的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动性的前提下，还可以根据这些附图获得其它的附图。其中：
图1为本发明一个实施例提供的一种激光slam定位方法流程示意图；
图2为本发明一个实施例提供的一种激光slam定位方法的系统框架图；
图3为本发明一个实施例提供的一种激光slam定位方法的路侧设备车辆中心估计原理图；
图4为本发明一个实施例提供的一种激光slam定位方法的车辆动力学校验示意图；
图5为本发明一个实施例提供的一种激光slam定位方法的因子图优化原理图；
图6为本发明一个实施例提供的一种激光slam定位方法的的系统工作流程示意图.
具体实施方式
为使本发明的上述目的、特征和优点能够更加明显易懂，下面结合说明书附图对本发明的具体实施方式做详细的说明，显然所描述的实施例是本发明的一部分实施例，而不是全部实施例。基于本发明中的实施例，本领域普通人员在没有做出创造性劳动前提下所获得的所有其他实施例，都应当属于本发明的保护的范围。
在下面的描述中阐述了很多具体细节以便于充分理解本发明，但是本发明还可以采用其他不同于在此描述的其它方式来实施，本领域技术人员可以在不违背本发明内涵的情况下做类似推广，因此本发明不受下面公开的具体实施例的限制。
其次，此处所称的“一个实施例”或“实施例”是指可包含于本发明至少一个实现方式中的特定特征、结构或特性。在本说明书中不同地方出现的“在一个实施例中”并非均指同一个实施例，也不是单独的或选择性的与其他实施例互相排斥的实施例。
本发明结合示意图进行详细描述，在详述本发明实施例时，为便于说明，表示器件结构的剖面图会不依一般比例作局部放大，而且所述示意图只是示例，其在此不应限制本发明保护的范围。此外，在实际制作中应包含长度、宽度及深度的三维空间尺寸。
同时在本发明的描述中，需要说明的是，术语中的“上、下、内和外”等指示的方位或位置关系为基于附图所示的方位或位置关系，仅是为了便于描述本发明和简化描述，而不是指示或暗示所指的装置或元件必须具有特定的方位、以特定的方位构造和操作，因此不能理解为对本发明的限制。此外，术语“第一、第二或第三”仅用于描述目的，而不能理解为指示或暗示相对重要性。
本发明中除非另有明确的规定和限定，术语“安装、相连、连接”应做广义理解，例如：可以是固定连接、可拆卸连接或一体式连接；同样可以是机械连接、电连接或直接连接，也可以通过中间媒介间接相连，也可以是两个元件内部的连通。对于本领域的普通技术人员而言，可以具体情况理解上述术语在本发明中的具体含义。
实施例1
参照图1-图5，为本发明的第一个实施例，该实施例提供了一种激光slam定位方法，包括：
S1：通过预积分因子获取从初始状态到当前状态的运动信息。
更进一步的，所述预积分因子包括预积分是一种将惯性传感器的测量值进行积分的技术，用于获取从初始状态到当前状态的运动信息，通过对IMU测量进行积分，得到位置、速度和姿态等关键运动参数的估计值，IMU预积分的基本原理是将IMU测量的加速度和角速度进行离散时间的积分，加速度积分得到速度估计，角速度积分得到姿态估计，同时，利用加速度计的数据来估计地球的重力方向，从而使用姿态信息将加速度估计从IMU坐标系转换为世界坐标系，通过预积分，减少姿态估计过程中的误差累积，在预积分速度与z轴方向变量时，引入车辆自身轮速对速度变量进行约束，IMU的预积分结果将作为直接法激光雷达里程计帧间配准的先验位姿。
S2：利用激光雷达里程计算法计算设备的位置和姿态，并通过GNSS先验因子提升激光slam定位精度。
更进一步的，所述激光雷达里程计算法包括直接法激光雷达里程计因子，包括以下步骤：
点云配准：首先通过最小化点云之间的差异，将连续帧的密集点云进行配准，估计两帧之间的相对运动，在配准时将IMU与轮速的预积分增量作为配准初始值；
运动估计：通过最小二乘法配准得到的相对运动，估计设备在每一帧之间的位移和旋转；
时序一致性：为了减少运动估计的漂移，引入时序一致性约束。
应说明的是，直接法激光雷达里程计是一种基于密集点云的激光雷达里程计算法，用于在复杂环境中快速、准确地估计设备的位置和姿态。与传统的激光雷达里程计方法相比，这种方法通过使用密集的点云数据，能够对细节和结构更敏感，从而提高了定位的准确性。由于直接使用密集点云的特点，该算法非常适合特征稀疏的矿区场景。
更进一步的，所述GNSS先验因子包括根据GNSS测量值提供额外的约束条件，提升激光slam定位精度，GNSS测量车辆当前在全局坐标系下的位置与速度信息；
从激光雷达获取t时刻点云，设置时间阈值τ(τ>0)，通过最近邻查找的方法，找到t-τ到t+τ时间段内最近的两个时间戳tl、tr(tr>tl)，对应的GNSS组合惯导定位数据分别为对/>进行线性插值，计算t时刻车体后轴中心全局定位信息：
其中，a＝(tr-t)/(tr-tl)，b＝(t-tl)/(tr-tl)；
计算激光雷达全局定位信息：
其中，表示车体后轴坐标，将/>的位移部分作为GPS先验因子添加到因子图中。
S3：通过车辆位置先验因子和车辆动力学校验，得出需要优化的车辆数据。
更进一步的，所述车辆位置先验因子包括车辆位置先验因子由路侧设备提供，当车辆进入到路侧设备感知视域时，通过感知算法率先识别车辆的包络，根据车辆包络，计算车辆在路侧设备坐标系下的车辆中心点坐标，根据下列公式，将路侧设备下的中心点坐标转换至世界系下，获得世界系下的车辆中心点坐标，将该坐标作为车辆的位置先验因子输入因子图中作为位置约束的条件之一：
其中，为路侧设备在世界坐标系下的位姿，/>为路侧设备坐标系下车辆中心坐标，/>为路侧设备感知到的车辆中心在世界坐标系下的位姿；
应说明的是，与GNSS先验因子相同的是，在计算车辆位置先验因子时，需要根据当前时间戳对车辆位置先验因子进行插值，差值获取激光雷达点云帧对应时刻的时间戳的位置作为车辆位置先验因子。
S4：通过因子图算法进行系统优化。
更进一步的，所述车辆动力学校验包括从运动读取当前车辆编号，并加载当前车辆的动力学参数，获取当前车辆的最大加速度accmax与最大角速度gyromax值，订阅雷达里程计，获取从雷达里程计估计的当前车辆加速度accl与gyrol，由于车辆的实际运动不超过车辆的动力学约束，当accl大于accmax或gyrol大于gyromax时，则雷达里程计发生异常，在因子图优化中舍弃本次数据。
更进一步的，所述因子图算法进行系统优化包括因子图由变量节点和因子节点组成，通过边连接在一起，在因子图优化中，变量节点表示问题中的变量，因子图算法的步骤为：
将车辆位姿估计问题转化为因子图表示，在因子图中添加各因子节点，每个因子节点与相关的变量节点连接，为变量节点设置初始估计值，通过迭代更新变量节点的估计值来优化因子图，当检测到GNSS先验因子或路侧设备感知获得的车辆先验位置因子被添加到因子图模型后，将因子图模型进行迭代优化，其余时刻只维护因子图当中的因子变量，不执行优化环节，点云帧间配准的距离误差作为因子图的收敛的判定条件，如果未达到收敛，则返回继续迭代，通过计算对偶残差的变化量DRS来判断算法的收敛，当DRS残差变化量小于阈值DRS_th时，算法收敛，输出最终收敛的车辆位姿估计，作为问题的最优解。
实施例2
本发明的一个实施例，提供了一种激光slam定位方法，为了验证本发明的有益效果，通过实验进行科学论证。
激光雷达：30Hz
IMU:100Hz
车辆轮速：100Hz
GNSS:10Hz
路侧设备：实时厘米级位姿坐标、相机和雷达
实验过程中，首先使用IMU和轮速进行预积分，获得100Hz的高频位姿增量信息。然后将预积分的增量信息作为激光雷达帧间配准的初始值，以获得精度更高的帧间配准结果。由于矿区特征点稀疏，使用直接法进行激光雷达帧间配准。激光雷达帧间配准结果将作为激光雷达里程计因子输入因子图进行优化。
在因子图中，根据车辆自身的限制计算车辆最大加速度与角速度信息。当激光里程计的加速度与角速度超过车辆自身的运动学约束时，则判定激光里程计发生异常，在因子图优化过程中舍弃异常的激光雷达里程计因子。
IMU与轮速预积分的高频位姿增量信息与GNSS的10Hz低频信息进行ESKF融合后，作为GNSS因子输入到因子图中。路侧设备可实时获得自身精确的厘米级位姿坐标，同时路侧设备上面安装了相机与雷达，可在无人驾驶车辆走进路侧设备感知视野时，实时的识别无人驾驶车辆，并计算车辆中心坐标，将该坐标作为车辆位置先验因子输出至因子图中。最终，在因子图中融合所有信息后输出车辆当前的高频厘米级位姿。
应说明的是，以上实施例仅用于说明本发明的技术方案而非限制，尽管参照较佳实施例对本发明进行了详细说明，本领域的普通技术人员应当理解，可以对本发明的技术方案进行修改或者等同替换，而不脱离本发明技术方案的精神和范围，其均应涵盖在本发明的权利要求范围当中。
实施例3
本发明第三个实施例，其不同于前两个实施例的是：
所述功能如果以软件功能单元的形式实现并作为独立的产品销售或使用时，可以存储在一个计算机可读取存储介质中。基于这样的理解，本发明的技术方案本质上或者说对现有技术做出贡献的部分或者该技术方案的部分可以以软件产品的形式体现出来，该计算机软件产品存储在一个存储介质中，包括若干指令用以使得一台计算机设备(可以是个人计算机，服务器，或者网络设备等)执行本发明各个实施例所述方法的全部或部分步骤。而前述的存储介质包括：U盘、移动硬盘、只读存储器(ROM，Read-OnlyMemory)、随机存取存储器(RAM，Random Access Memory)、磁碟或者光盘等各种可以存储程序代码的介质。
在流程图中表示或在此以其他方式描述的逻辑和/或步骤，例如，可以被认为是用于实现逻辑功能的可执行指令的定序列表，可以具体实现在任何计算机可读介质中，以供指令执行系统、装置或设备(如基于计算机的系统、包括处理器的系统或其他可以从指令执行系统、装置或设备取指令并执行指令的系统)使用，或结合这些指令执行系统、装置或设备而使用。就本说明书而言，“计算机可读介质”可以是任何可以包含、存储、通信、传播或传输程序以供指令执行系统、装置或设备或结合这些指令执行系统、装置或设备而使用的装置。
计算机可读介质的更具体的示例(非穷尽性列表)包括以下：具有一个或多个布线的电连接部(电子装置)、便携式计算机盘盒(磁装置)、随机存取存储器(RAM)、只读存储器(ROM)、可擦除可编辑只读存储器(EPROM或闪速存储器)、光纤装置以及便携式光盘只读存储器(CDROM)。另外，计算机可读介质甚至可以是可在其上打印所述程序的纸或其他合适的介质，因为可以例如通过对纸或其他介质进行光学扫描，接着进行编辑、解译或必要时以其他合适方式进行处理来以电子方式获得所述程序，然后将其存储在计算机存储器中。
应当理解，本发明的各部分可以用硬件、软件、固件或它们的组合来实现。在上述实施方式中，多个步骤或方法可以用存储在存储器中且由合适的指令执行系统执行的软件或固件来实现。例如，如果用硬件来实现，和在另一实施方式中一样，可用本领域公知的下列技术中的任一项或他们的组合来实现：具有用于对数据信号实现逻辑功能的逻辑门电路的离散逻辑电路，具有合适的组合逻辑门电路的专用集成电路，可编程门阵列(PGA)，现场可编程门阵列(FPGA)等。
实施例4
参照图6，为本发明的一个实施例，提供了一种激光slam定位方法的系统，其特征在于：激光雷达传感器、IMU传感器、车辆轮速传感器、GNSS传感器、路侧设备传感器；
激光雷达传感器，通过发射激光束，并测量返回激光束的时间和强度，以获取周围环境的三维点云数据，点云数据可用于构建环境地图，并且可以通过与先前的点云数据匹配来进行定位，激光雷达提供了高精度的距离和方向信息，对于实时的定位和建图非常关键。
IMU传感器，IMU与轮速进行预计分获得100hz高频位姿增量信息，将预积分的增量信息作为激光雷达帧间配准的初始值，以获得精度更高的帧间配准结果。
车辆轮速传感器，车辆轮速是从车辆的车轮运动中获得的信息，通过监测车轮的转速和里程计信息，可以估计车辆在空间中的运动，车辆轮速信息通常与激光雷达数据进行融合，用于改善定位的精度和鲁棒性，根据车辆模型和运动学原理，结合车辆轮速可以估计车辆的姿态变化和位置移动。
GNSS传感器，GNSS接收器可以接收卫星发射的信号，并通过测量信号传播时间和多普勒效应来确定接收器的位置，在激光SLAM中，GNSS用于提供车辆在全球地理坐标系下的初始位置估计，这对于快速初始化SLAM系统并进行全局定位非常有用。
路侧设备传感器，可实时获得自身精确的厘米级位姿坐标，同时路侧设备上面安装了相机与雷达，可在无人驾驶车辆走进路侧设备感知视野时，实时的识别无人驾驶车辆，并计算车辆中心坐标。
应说明的是，以上实施例仅用于说明本发明的技术方案而非限制，尽管参照较佳实施例对本发明进行了详细说明，本领域的普通技术人员应当理解，可以对本发明的技术方案进行修改或者等同替换，而不脱离本发明技术方案的精神和范围，其均应涵盖在本发明的权利要求范围当中。

Claims (10)

1.一种激光slam定位方法，其特征在于：包括，
通过预积分因子获取从初始状态到当前状态的运动信息；
利用激光雷达里程计算法计算设备的位置和姿态，并通过GNSS先验因子提升激光slam定位精度；
通过车辆位置先验因子和车辆动力学校验，得出需要优化的车辆数据；
通过因子图算法进行系统优化。
2.如权利要求1所述的一种激光slam定位方法，其特征在于：所述预积分因子包括预积分是一种将惯性传感器的测量值进行积分的技术，用于获取从初始状态到当前状态的运动信息，通过对IMU测量进行积分，得到位置、速度和姿态等关键运动参数的估计值，IMU预积分的基本原理是将IMU测量的加速度和角速度进行离散时间的积分，加速度积分得到速度估计，角速度积分得到姿态估计，同时，利用加速度计的数据来估计地球的重力方向，从而使用姿态信息将加速度估计从IMU坐标系转换为世界坐标系，通过预积分，减少姿态估计过程中的误差累积，在预积分速度与z轴方向变量时，引入车辆自身轮速对速度变量进行约束，IMU的预积分结果将作为直接法激光雷达里程计帧间配准的先验位姿。
3.如权利要求2所述的一种激光slam定位方法，其特征在于：所述激光雷达里程计算法包括直接法激光雷达里程计因子，包括以下步骤：
点云配准：首先通过最小化点云之间的差异，将连续帧的密集点云进行配准，估计两帧之间的相对运动，在配准时将IMU与轮速的预积分增量作为配准初始值；
运动估计：通过最小二乘法配准得到的相对运动，估计设备在每一帧之间的位移和旋转；
时序一致性：为了减少运动估计的漂移，引入时序一致性约束。
4.如权利要求3所述的一种激光slam定位方法，其特征在于：所述GNSS先验因子包括根据GNSS测量值提供额外的约束条件，提升激光slam定位精度，GNSS测量车辆当前在全局坐标系下的位置与速度信息；
从激光雷达获取t时刻点云，设置时间阈值τ(τ>0)，通过最近邻查找的方法，找到t-τ到t+τ时间段内最近的两个时间戳tl、tr(tr>tl)，对应的GNSS组合惯导定位数据分别为对/>进行线性插值，计算t时刻车体后轴中心全局定位信息：
其中，a＝(tr-t)/(tr-tl)，b＝(t-tl)/(tr-tl)；
计算激光雷达全局定位信息：
其中，表示车体后轴坐标，将/>的位移部分作为GPS先验因子添加到因子图中。
5.如权利要求4所述的一种激光slam定位方法，其特征在于：所述车辆位置先验因子包括车辆位置先验因子由路侧设备提供，当车辆进入到路侧设备感知视域时，通过感知算法率先识别车辆的包络，根据车辆包络，计算车辆在路侧设备坐标系下的车辆中心点坐标，根据下列公式，将路侧设备下的中心点坐标转换至世界系下，获得世界系下的车辆中心点坐标，将坐标作为车辆的位置先验因子输入因子图中作为位置约束的条件之一：
其中，为路侧设备在世界坐标系下的位姿，/>为路侧设备坐标系下车辆中心坐标，/>为路侧设备感知到的车辆中心在世界坐标系下的位姿；
与GNSS先验因子相同的是，在计算车辆位置先验因子时，需要根据当前时间戳对车辆位置先验因子进行插值，差值获取激光雷达点云帧对应时刻的时间戳的位置作为车辆位置先验因子。
6.如权利要求5所述的一种激光slam定位方法，其特征在于：所述车辆动力学校验包括从运动读取当前车辆编号，并加载当前车辆的动力学参数，获取当前车辆的最大加速度accmax与最大角速度gyromax值，订阅雷达里程计，获取从雷达里程计估计的当前车辆加速度accl与gyrol，由于车辆的实际运动不超过车辆的动力学约束，当accl大于accmax或gyrol大于gyromax时，则雷达里程计发生异常，在因子图优化中舍弃本次数据。
7.如权利要求6所述的一种激光slam定位方法，其特征在于：所述因子图算法进行系统优化包括因子图由变量节点和因子节点组成，通过边连接在一起，在因子图优化中，变量节点表示问题中的变量，因子图算法的步骤为：
将车辆位姿估计问题转化为因子图表示，在因子图中添加各因子节点，每个因子节点与相关的变量节点连接，为变量节点设置初始估计值，通过迭代更新变量节点的估计值来优化因子图，当检测到GNSS先验因子或路侧设备感知获得的车辆先验位置因子被添加到因子图模型后，将因子图模型进行迭代优化，其余时刻只维护因子图当中的因子变量，不执行优化环节，点云帧间配准的距离误差作为因子图的收敛的判定条件，如果未达到收敛，则返回继续迭代，通过计算对偶残差的变化量DRS来判断算法的收敛，当DRS残差变化量小于阈值DRS_th时，算法收敛，输出最终收敛的车辆位姿估计，作为问题的最优解。
8.一种采用如权利要求1～7任一所述的一种激光slam定位方法的系统，其能够可以在未知环境中实现自主探索和地图构建，并且利用构建的地图进行定位和导航，其特征在于：激光雷达传感器、IMU传感器、车辆轮速传感器、GNSS传感器、路侧设备传感器；
所述激光雷达传感器，通过发射激光束，并测量返回激光束的时间和强度，以获取周围环境的三维点云数据，点云数据用于构建环境地图，并且通过与先前的点云数据匹配来进行定位，激光雷达提供了高精度的距离和方向信息，对于实时的定位和建图非常关键；
所述IMU传感器，IMU与轮速进行预计分获得100hz高频位姿增量信息，将预积分的增量信息作为激光雷达帧间配准的初始值，获得精度更高的帧间配准结果；
所述车辆轮速传感器，车辆轮速是从车辆的车轮运动中获得的信息，通过监测车轮的转速和里程计信息，估计车辆在空间中的运动，车辆轮速信息通常与激光雷达数据进行融合，用于改善定位的精度和鲁棒性，根据车辆模型和运动学原理，结合车辆轮速估计车辆的姿态变化和位置移动；
所述GNSS传感器，GNSS接收器接收卫星发射的信号，并通过测量信号传播时间和多普勒效应来确定接收器的位置，在激光SLAM中，GNSS用于提供车辆在全球地理坐标系下的初始位置估计，这对于快速初始化SLAM系统并进行全局定位非常有用；
所述路侧设备传感器，实时获得自身精确的厘米级位姿坐标，同时路侧设备上面安装了相机与雷达，在无人驾驶车辆走进路侧设备感知视野时，实时的识别无人驾驶车辆，并计算车辆中心坐标。
9.一种计算机设备，包括存储器和处理器，所述存储器存储有计算机程序，其特征在于，所述处理器执行所述计算机程序时实现权利要求1至7中任一项所述的方法的步骤。
10.一种计算机可读存储介质，其上存储有计算机程序，其特征在于，所述计算机程序被处理器执行时实现权利要求1至7中任一项所述的方法的步骤。
CN202310961887.XA 2023-08-02 2023-08-02 一种激光slam定位方法及系统 Pending CN117268408A (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202310961887.XA CN117268408A (zh) 2023-08-02 2023-08-02 一种激光slam定位方法及系统

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202310961887.XA CN117268408A (zh) 2023-08-02 2023-08-02 一种激光slam定位方法及系统

Publications (1)

Publication Number Publication Date
CN117268408A true CN117268408A (zh) 2023-12-22

Family

ID=89213220

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202310961887.XA Pending CN117268408A (zh) 2023-08-02 2023-08-02 一种激光slam定位方法及系统

Country Status (1)

Country Link
CN (1) CN117268408A (zh)

Cited By (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN117808882A (zh) * 2024-02-29 2024-04-02 上海几何伙伴智能驾驶有限公司 退化场景下基于多传感器融合的slam漂移检测与补偿方法

Cited By (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN117808882A (zh) * 2024-02-29 2024-04-02 上海几何伙伴智能驾驶有限公司 退化场景下基于多传感器融合的slam漂移检测与补偿方法
CN117808882B (zh) * 2024-02-29 2024-05-17 上海几何伙伴智能驾驶有限公司 退化场景下基于多传感器融合的slam漂移检测与补偿方法

Similar Documents

Publication Publication Date Title
CN110906923B (zh) 车载多传感器紧耦合融合定位方法、系统、存储介质及车辆
US9360323B2 (en) Systems and methods for estimating movements of a vehicle using a mobile device
CN105509738B (zh) 基于惯导/多普勒雷达组合的车载定位定向方法
EP1530024A1 (en) Motion estimation method and system for mobile body
KR20210111180A (ko) 위치 추적 방법, 장치, 컴퓨팅 기기 및 컴퓨터 판독 가능한 저장 매체
CN107145578A (zh) 地图构建方法、装置、设备和系统
JP7245084B2 (ja) 自動運転システム
CN112050817A (zh) 车道级地图匹配
KR101909953B1 (ko) 라이다 센서를 이용한 차량의 자세 추정 방법
US9002513B2 (en) Estimating apparatus, estimating method, and computer product
KR101239864B1 (ko) 다중 센서를 이용한 항법시스템 및 그 제공방법
CN114545472B (zh) 一种gnss/ins组合系统的导航方法和装置
CN117268408A (zh) 一种激光slam定位方法及系统
CN115060257B (zh) 一种基于民用级惯性测量单元的车辆变道检测方法
US20220205804A1 (en) Vehicle localisation
CN117387604A (zh) 基于4d毫米波雷达和imu融合的定位与建图方法及系统
CN112923934A (zh) 一种适用于非结构化场景中结合惯导的激光slam技术
CN114088104B (zh) 一种自动驾驶场景下的地图生成方法
CN114323034A (zh) 一种卫星遮蔽环境中的基于置信度传播的多车协同定位方法
Noureldin et al. a Framework for Multi-Sensor Positioning and Mapping for Autonomous Vehicles
US20240053491A1 (en) Error characterization for gnss-based position estimates on constrained routes
KR20210009688A (ko) Gps 음영지역 극복을 위한 ins/gps/초음파 속도계 결합 항법 시스템
CN111712855A (zh) 地面信息处理方法、装置和无人驾驶车辆
CN111486837B (zh) 基于图结构的交通工具位置估计方法和使用其的交通工具
CN117346768B (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