CN115407357A - 基于大场景的低线束激光雷达-imu-rtk定位建图算法 - Google Patents
基于大场景的低线束激光雷达-imu-rtk定位建图算法 Download PDFInfo
- Publication number
- CN115407357A CN115407357A CN202210784265.XA CN202210784265A CN115407357A CN 115407357 A CN115407357 A CN 115407357A CN 202210784265 A CN202210784265 A CN 202210784265A CN 115407357 A CN115407357 A CN 115407357A
- Authority
- CN
- China
- Prior art keywords
- point
- imu
- point cloud
- points
- rtk
- 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
Links
- 238000013507 mapping Methods 0.000 title claims abstract description 28
- 238000005457 optimization Methods 0.000 claims abstract description 22
- 230000010354 integration Effects 0.000 claims abstract description 12
- 238000004364 calculation method Methods 0.000 claims description 18
- 239000011159 matrix material Substances 0.000 claims description 15
- 238000000034 method Methods 0.000 claims description 14
- 230000004927 fusion Effects 0.000 claims description 5
- 230000007613 environmental effect Effects 0.000 claims description 4
- 238000005259 measurement Methods 0.000 claims description 4
- 150000001875 compounds Chemical class 0.000 claims description 3
- 239000000284 extract Substances 0.000 claims description 3
- 238000001914 filtration Methods 0.000 claims description 3
- 230000005484 gravity Effects 0.000 claims description 3
- 230000009466 transformation Effects 0.000 claims description 3
- 238000012937 correction Methods 0.000 claims description 2
- 238000005516 engineering process Methods 0.000 description 4
- 238000012360 testing method Methods 0.000 description 3
- 238000005034 decoration Methods 0.000 description 2
- 238000010586 diagram Methods 0.000 description 2
- 238000000605 extraction Methods 0.000 description 2
- 239000000203 mixture Substances 0.000 description 2
- 238000012986 modification Methods 0.000 description 2
- 230000004048 modification Effects 0.000 description 2
- 235000014594 pastries Nutrition 0.000 description 2
- 238000012545 processing Methods 0.000 description 2
- 230000009286 beneficial effect Effects 0.000 description 1
- 230000000295 complement effect Effects 0.000 description 1
- 230000007547 defect Effects 0.000 description 1
- 238000005286 illumination Methods 0.000 description 1
- 230000010365 information processing Effects 0.000 description 1
- 230000002401 inhibitory effect Effects 0.000 description 1
- 238000009434 installation Methods 0.000 description 1
- 230000004807 localization Effects 0.000 description 1
- 230000008447 perception Effects 0.000 description 1
- 230000001360 synchronised effect Effects 0.000 description 1
Images
Classifications
-
- 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/88—Lidar systems specially adapted for specific applications
- G01S17/89—Lidar systems specially adapted for specific applications for mapping or imaging
-
- 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
- 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
- G01S19/485—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 optical system or imaging 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
-
- G—PHYSICS
- G06—COMPUTING; CALCULATING OR COUNTING
- G06F—ELECTRIC DIGITAL DATA PROCESSING
- G06F16/00—Information retrieval; Database structures therefor; File system structures therefor
- G06F16/20—Information retrieval; Database structures therefor; File system structures therefor of structured data, e.g. relational data
- G06F16/29—Geographical information databases
-
- 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
- G06T7/73—Determining position or orientation of objects or cameras using feature-based methods
-
- 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)
- Remote Sensing (AREA)
- Radar, Positioning & Navigation (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Computer Networks & Wireless Communication (AREA)
- Theoretical Computer Science (AREA)
- Databases & Information Systems (AREA)
- Electromagnetism (AREA)
- Data Mining & Analysis (AREA)
- General Engineering & Computer Science (AREA)
- Automation & Control Theory (AREA)
- Computer Vision & Pattern Recognition (AREA)
- Navigation (AREA)
Abstract
本发明公开了一种基于大场景的低线束激光雷达‑IMU‑RTK定位建图算法,包括:初始化阶段使用RTK位置信息快速精准的完成IMU对准,并解算激光雷达‑IMU‑RTK***在全局坐标下的正确初始位姿;提取周围点云环境特征,根据曲率区分角点和面点;实时构建局部点云地图,利用当前帧点云信息与局部地图动态匹配,计算当前激光雷达位姿;图优化模型构建激光雷达里程计因子、IMU预积分因子、RTK绝对位置因子,优化求解定位结果并进行实时点云地图拼接。本发明通过图优化模型,能够解决低线束激光雷达在大场景特征稀疏环境下定位和建图问题,同时能够保障实时采集。
Description
技术领域
本发明属于激光雷达SLAM(Simultaneous Localization And Mapping)领域,涉及多传感器融合技术,具体涉及基于大场景的低线束激光雷达-IMU-RTK定位建图算法。
背景技术
获得精准、可靠的定位是移动机器人、自动驾驶的基本需求。近年来,视觉、激光同步定位与建图技术取得长足的发展。视觉定位依靠相机可实现六自由度状态估计,但视觉定位受限环境较多,容易受到***初始化、光照、低纹理特征等因素影响。而激光传感器可直接获得深度信息,并且具备较高分辨率,可在夜间工作,能实现更为准确的位姿估计。
点云匹配方案以往是迭代ICP算法,但容易陷入局部最小值,且配准耗时长。2014年发布的LOAM首次采用点线、点面特征匹配位姿计算,在建图过程利用帧与帧(scan-to-scan) 里程计结果初值和帧与图(scan-to-map)匹配共同更新维护点云地图。2020年发布的LIO-SAM 融合了激光雷达-IMU-GNSS三种传感器信息,采用IMU里程计高频解算、激光雷达里程计特征匹配的方式,GNSS和回环信息在建图过程用于抑制定位结果漂移,获得全局一致性较好的点云地图。
根据上述原理,激光雷达SLAM技术主要采用提取角点和面点特征的前端处理方式,通过IMU里程计或者scan-to-scan匹配提供预测值。优化过程采用当前帧和局部地图匹配获得更为精准的定位结果。在后端处理普遍利用因子图框架,进行多源信息处理融合。但是现有的算法主要针对小场景运动状态。而对于场景复杂,载体环境变化大,易出现累计误差,稳健性差。为此,需在保证定位和建图的条件下,精度探索基于大场景的城市环境下可靠的激光SLAM技术。
发明内容
为解决上述问题,本发明公开了基于大场景的低线束激光雷达-IMU-RTK定位建图算法,根据如今城市环境定位的复杂性,利用低线束激光雷达、IMU与RTK实现信息互补利用,实时采集并获得可靠的定位建图结果。
为达到上述目的,本发明的技术方案如下:
基于大场景的低线束激光雷达-IMU-RTK定位建图算法,包含以下步骤:
步骤1、初始化利用RTK位置信息快速精准的完成IMU对准,并解算激光雷达 -IMU-RTK***在全局坐标下的正确初始位姿;
步骤2、提取周围点云环境特征,根据扫描点曲率区分角点和面点,采用IMU数据进行点云去畸变校正;
步骤3、实时构建局部点云地图,对地图与当前帧进行体素滤波提升后续计算效率,并利用当前帧点云信息与局部地图动态匹配,迭代计算当前激光雷达位姿;
步骤4、图优化模型构建激光雷达里程计因子、IMU预积分因子、RTK绝对位置因子,优化求解定位结果并进行实时点云地图拼接,获得一致性较好的全局地图。
具体步骤为:
步骤1.IMU快速精准的初始对准
式中,表示导航坐标系到载体坐标系的转换矩阵;ab、bb、cb分别表示载体坐标系下的位置坐标;an、bn、cn表示对应点在导航坐标系下的坐标值。初始对准确定惯性器件在导航坐标系下的姿态,获得激光雷达-IMU-RTK融合定位在全局坐标下的姿态初值。
步骤2.提取周围点云环境特征,根据扫描点曲率区分角点和面点
提取环境点云特征,根据扫描点曲率区分角点和面点,曲率计算公式如下式:
|S|代表此扫描点的同线束周围点集合,ri表示此点的深度值。通过此式可依次计算每帧激光雷达扫描到的所有点的曲率。同时,利用每个点的深度信息剔除平行点和遮挡点。为了提升遍历效率,每帧点云坐标信息投影成二维图像,并分为六个子图,对子图分别提取角点和面点。根据阈值cth进行判定,c<cth为角点,c>cth为面点。由于载体运动,每帧扫描过程会出现点云畸变,采用高频IMU数据短时间积分可实现运动补偿。
步骤3.构建局部点云地图,点云配准优化位姿
采用滑窗的方式合并一定范围内的关键帧信息。为了提升点云匹配速率,局部地图的点云信息都存储在kd-tree的数据结构中,方便后续搜索。运动估计模块将当前帧与局部地图匹配求解运动状态。
对于角点,搜索其在局部地图最近的五个点,计算五点均值和协方差矩阵。当有满足条件的线特征,可计算当前角点到线的距离。距离计算公式如下:
其中·和×分别表示点积和叉乘运算,pn是单位向量,pε代表当前角点,代表特征值对应的特征向量。同理,面点ps在局部地图中搜索出五点构成平面。此时五点的协方差矩阵的最小特征值对应的特征向量为这个平面所对应法向量。点面距离计算如下:
min{∑f(pε)+f(ps)} (6)
步骤4.图优化模型求解定位结果并进行实时点云地图拼接
式中Rp(x)代表边缘化残差,边缘化的目的是减少优化方程规模,移出滑窗的关键帧信息保存为当前优化的先验信息。
式中i和j分别表示上一帧和当前帧,T是位姿的欧式矩阵表示,ΔT即为位姿变化量。
式中是旋转矩阵表示的姿态,W是世界坐标系。δα、δβ和δθ分别表示位置、速度和角度的变化量,δba和δbw分别表示加速度计和陀螺仪的偏置变化。gW表示世界坐标系下的重力值,pW、vW和qW是世界系下的位置、速度和四元数表示的姿态。[×]xyz指四元数虚部,分别代表IMU预积分计算的相邻两帧之间的位置、速度和角度。同时每帧解算估计的加速度计和陀螺仪的偏置为ba和bw。
Rg(x)表示RTK绝对位置残差约束,计算公式如下:
本步骤根据(7)式利用Ceres优化库求解滑窗内每帧的状态量,并实时更新采集到的点云姿态,将载体运动状态下的激光雷达扫描点转到世界坐标系下,依次拼接合成全局一致的点云地图。
本发明的有益效果包括:
本发明提出的基于大场景的低线束激光雷达-IMU-RTK定位建图算法,利用图优化模型,合理利用三种传感器的数据信息,克服低线束激光雷达在大场景易定位发散,RTK在城市环境信号遮挡,IMU长时间出现累积漂移的缺点,实现高精度定位和建图的SLAM***。所提方法在城市环境中车载测试,定位精度和鲁棒性可明显提升,对于三维实景测绘、自动驾驶等行业具有重要借鉴意义。
附图说明
图1是本发明所述的基于低线束激光雷达-IMU-RTK定位建图算法流程图;
图2是应用图优化模型在后端解算定位结果的示意图;
图3是激光雷达-IMU-RTK实验设备,用于采集数据并实时定位与建图;
图4是实验***在车载测试中大场景定位和建图的示意图,可实时输出高精度结果且稳定运行。
具体实施方式
下面结合附图和具体实施方式,进一步阐明本发明,应理解下述具体实施方式仅用于说明本发明而不用于限制本发明的范围。
如附图1所示,本实施例公开一种基于低线束激光雷达-IMU-RTK定位建图算法,具体步骤如下:
步骤1.IMU快速精准的初始对准
式中,表示导航坐标系到载体坐标系的转换矩阵;ab、bb、cb分别表示载体坐标系下的位置坐标;an、bn、cn表示对应点在导航坐标系下的坐标值。初始对准确定惯性器件在导航坐标系下的姿态,获得激光雷达-IMU-RTK融合定位在全局坐标下的姿态初值。
步骤2.提取周围点云环境特征,根据扫描点曲率区分角点和面点
提取环境点云特征,根据扫描点曲率区分角点和面点,曲率计算公式如下式:
|S|代表此扫描点的同线束周围点集合,ri表示此点的深度值。通过此式可依次计算每帧激光雷达扫描到的所有点的曲率。同时,利用每个点的深度信息剔除平行点和遮挡点。为了提升遍历效率,每帧点云坐标信息投影成二维图像,并分为六个子图,对子图分别提取角点和面点。根据阈值cth进行判定,c<cth为角点,c>cth为面点。由于载体运动,每帧扫描过程会出现点云畸变,采用高频IMU数据短时间积分可实现运动补偿。
步骤3.构建局部点云地图,点云配准优化位姿
采用滑窗的方式合并一定范围内的关键帧信息。为了提升点云匹配速率,局部地图的点云信息都存储在kd-tree的数据结构中,方便后续搜索。运动估计模块将当前帧与局部地图匹配求解运动状态。
对于角点,搜索其在局部地图最近的五个点,计算五点均值和协方差矩阵。当有满足条件的线特征,可计算当前角点到线的距离。距离计算公式如下:
其中·和×分别表示点积和叉乘运算,pn是单位向量,pε代表当前角点,代表特征值对应的特征向量。同理,面点ps在局部地图中搜索出五点构成平面。此时五点的协方差矩阵的最小特征值对应的特征向量为这个平面所对应法向量。点面距离计算如下:
min{∑f(pε)+f(ps)} (6)
步骤4.图优化模型求解定位结果并进行实时点云地图拼接
式中Rp(x)代表边缘化残差,边缘化的目的是减少优化方程规模,移出滑窗的关键帧信息保存为当前优化的先验信息。
式中i和j分别表示上一帧和当前帧,T是位姿的欧式矩阵表示,ΔT即为位姿变化量。
式中是旋转矩阵表示的姿态,W是世界坐标系。δα、δβ和δθ分别表示位置、速度和角度的变化量,δba和δbw分别表示加速度计和陀螺仪的偏置变化。gW表示世界坐标系下的重力值,pW、vW和qW是世界系下的位置、速度和四元数表示的姿态。[×]xyz指四元数虚部,分别代表IMU预积分计算的相邻两帧之间的位置、速度和角度。同时每帧解算估计的加速度计和陀螺仪的偏置为ba和bw。
Rg(x)表示RTK绝对位置残差约束,计算公式如下:
本步骤根据(7)式利用Ceres优化库求解滑窗内每帧的状态量,并实时更新采集到的点云姿态,将载体运动状态下的激光雷达扫描点转到世界坐标系下,依次拼接合成全局一致的点云地图。
附图3是在城市环境下定位与建图的实验设备,采用Velodyne-VLP16激光雷达、ADIS16488惯导、天宝接收机。传感器之间安装参数已知,硬件方式已实现时间同步。附图 4是车载平台在大场景下的实验定位与建图结果,在城市环境23.6公里车载测试(包含RTK遮挡的高架、林荫场景,点云稀疏退化的大场景环境等)全程定位精度(均方根误差RMSE)指标为0.24m。同时本***可利用点云实时构建周围场景,实现自动驾驶建图与感知。
需要说明的是,以上内容仅仅说明了本发明的技术思想,不能以此限定本发明的保护范围,对于本技术领域的普通技术人员来说,在不脱离本发明原理的前提下,还可以做出若干改进和润饰,这些改进和润饰均落入本发明权利要求书的保护范围之内。
Claims (5)
1.基于大场景的低线束激光雷达-IMU-RTK定位建图算法,其特征在于,具体方法如下:
步骤1、初始化利用RTK位置信息完成IMU对准,并解算激光雷达-IMU-RTK***在全局坐标下的正确初始位姿;
步骤2、提取周围点云环境特征,根据扫描点曲率区分角点和面点,采用IMU数据进行点云去畸变校正;
步骤3、实时构建局部点云地图,对地图与当前帧进行体素滤波提升后续计算效率,并利用当前帧点云信息与局部地图动态匹配,迭代计算当前激光雷达位姿;
步骤4、图优化模型构建激光雷达里程计因子、IMU预积分因子、RTK绝对位置因子,优化求解定位结果并进行实时点云地图拼接,获得一致性较好的全局地图。
3.如权利要求1所述的基于大场景的低线束激光雷达-IMU-RTK定位建图算法,其特征在于,步骤2所述提取环境点云特征,根据扫描点曲率区分角点和面点,曲率c计算公式如下式:
|S|代表此扫描点的同线束周围点集合,ri表示此点的深度值,rj表示此点附近同线束点深度值,通过此式依次计算每帧激光雷达扫描到的所有点的曲率,同时,利用每个点的深度信息剔除平行点和遮挡点,为了提升遍历效率,每帧点云坐标信息投影成二维图像,并分为六个子图,对子图分别提取角点和面点,根据阈值cth进行判定,c<cth为角点,c>cth为面点,由于载体运动,每帧扫描过程会出现点云畸变,采用高频IMU数据短时间积分运算实现运动补偿。
4.如权利要求1所述的基于大场景的低线束激光雷达-IMU-RTK定位建图算法,其特征在于,步骤3所述的实时构建局部点云地图,对地图与当前帧进行体素滤波提升后续计算效率,利用当前帧点云信息与局部地图动态匹配,迭代计算当前激光雷达位姿,采用滑窗的方式合并一定范围内的关键帧信息,为了提升点云匹配速率,局部地图的点云信息都存储在kd-tree的数据结构中,方便后续搜索,
运动估计模块将当前帧与局部地图匹配求解运动状态,对于角点,搜索其在局部地图最近的五个点,计算五点均值和协方差矩阵,当有满足条件的线特征,计算当前角点到线的距离,距离计算公式如下:
其中·和×分别表示点积和叉乘运算,pn是单位向量,pε代表当前角点,代表特征值对应的特征向量,同理,面点ps在局部地图中搜索出五点构成平面,此时五点的协方差矩阵的最小特征值对应的特征向量为这个平面所对应法向量,点面距离计算如式(5):
min{∑f(pε)+f(ps)} (6)。
5.如权利要求1所述的基于大场景的低线束激光雷达-IMU-RTK定位建图算法,其特征在于,步骤4所述的图优化模型构建激光雷达里程计因子、IMU预积分因子、RTK绝对位置因子,优化求解定位结果并进行实时点云地图拼接,对于一定窗口内的关键帧,数量为n,优化状态量最小化求解方程如下式:
式中Rp(x)代表边缘化残差,边缘化的目的是减少优化方程规模,移出滑窗的关键帧信息保存为当前优化的先验信息,
式中i和j分别表示上一帧和当前帧,T是位姿的欧式矩阵表示,ΔT即为位姿变化量,
式中是旋转矩阵表示的姿态,W是世界坐标系,bi和bj指第i帧与第j帧所在载体系,δα、δβ和δθ分别表示位置、速度和角度的变化量,δba和δbw分别表示加速度计和陀螺仪的偏置变化,Δt指两帧之间的时间间隔,gW表示世界坐标系下的重力值,pW、vW和qW是世界系下的位置、速度和四元数表示的姿态,[×]xyz指四元数虚部,分别代表IMU预积分计算的相邻两帧之间的位置、速度和角度,同时每帧解算估计的加速度计和陀螺仪的偏置为ba和bw,
Rg(x)表示RTK绝对位置残差约束,计算公式如下:
本步骤根据(7)式利用Ceres优化库求解滑窗内每帧的状态量,并实时更新采集到的点云姿态,将载体运动状态下的激光雷达扫描点转到世界坐标系下,依次拼接合成全局一致的点云地图。
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202210784265.XA CN115407357B (zh) | 2022-07-05 | 2022-07-05 | 基于大场景的低线束激光雷达-imu-rtk定位建图算法 |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202210784265.XA CN115407357B (zh) | 2022-07-05 | 2022-07-05 | 基于大场景的低线束激光雷达-imu-rtk定位建图算法 |
Publications (2)
Publication Number | Publication Date |
---|---|
CN115407357A true CN115407357A (zh) | 2022-11-29 |
CN115407357B CN115407357B (zh) | 2024-05-31 |
Family
ID=84157404
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN202210784265.XA Active CN115407357B (zh) | 2022-07-05 | 2022-07-05 | 基于大场景的低线束激光雷达-imu-rtk定位建图算法 |
Country Status (1)
Country | Link |
---|---|
CN (1) | CN115407357B (zh) |
Cited By (7)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN113551666A (zh) * | 2021-07-06 | 2021-10-26 | 杭州鸿泉物联网技术股份有限公司 | 自动驾驶多传感器融合定位方法和装置、设备及介质 |
CN115561731A (zh) * | 2022-12-05 | 2023-01-03 | 安徽蔚来智驾科技有限公司 | 位姿优化方法、点云地图建立方法、计算机设备及介质 |
CN115797425A (zh) * | 2023-01-19 | 2023-03-14 | 中国科学技术大学 | 一种基于点云鸟瞰图和由粗到精策略的激光全局定位方法 |
CN116242372A (zh) * | 2023-01-03 | 2023-06-09 | 东南大学 | 一种gnss拒止环境下的uwb-激光雷达-惯导融合定位方法 |
CN116592888A (zh) * | 2023-05-08 | 2023-08-15 | 五八智能科技(杭州)有限公司 | 一种巡逻机器人全局定位的方法、***、装置和介质 |
CN117250623A (zh) * | 2023-11-20 | 2023-12-19 | 山东新坐标智能装备有限公司 | 激光雷达与互补定位融合的定位方法、***及移动机器人 |
CN117433511A (zh) * | 2023-12-20 | 2024-01-23 | 绘见科技(深圳)有限公司 | 一种多传感器融合定位方法 |
Citations (6)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN108303710A (zh) * | 2018-06-12 | 2018-07-20 | 江苏中科院智能科学技术应用研究院 | 基于三维激光雷达的无人机多场景定位建图方法 |
CN110009739A (zh) * | 2019-01-29 | 2019-07-12 | 浙江省北大信息技术高等研究院 | 移动摄像机的数字视网膜的运动特征的提取与编码方法 |
CN113781582A (zh) * | 2021-09-18 | 2021-12-10 | 四川大学 | 基于激光雷达和惯导联合标定的同步定位与地图创建方法 |
US20220018962A1 (en) * | 2020-07-16 | 2022-01-20 | Beijing Tusen Weilai Technology Co., Ltd. | Positioning method and device based on multi-sensor fusion |
CN114136311A (zh) * | 2021-11-08 | 2022-03-04 | 上海应用技术大学 | 一种基于imu预积分的激光slam定位方法 |
CN114659514A (zh) * | 2022-03-21 | 2022-06-24 | 东南大学 | 一种基于体素化精配准的LiDAR-IMU-GNSS融合定位方法 |
-
2022
- 2022-07-05 CN CN202210784265.XA patent/CN115407357B/zh active Active
Patent Citations (6)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN108303710A (zh) * | 2018-06-12 | 2018-07-20 | 江苏中科院智能科学技术应用研究院 | 基于三维激光雷达的无人机多场景定位建图方法 |
CN110009739A (zh) * | 2019-01-29 | 2019-07-12 | 浙江省北大信息技术高等研究院 | 移动摄像机的数字视网膜的运动特征的提取与编码方法 |
US20220018962A1 (en) * | 2020-07-16 | 2022-01-20 | Beijing Tusen Weilai Technology Co., Ltd. | Positioning method and device based on multi-sensor fusion |
CN113781582A (zh) * | 2021-09-18 | 2021-12-10 | 四川大学 | 基于激光雷达和惯导联合标定的同步定位与地图创建方法 |
CN114136311A (zh) * | 2021-11-08 | 2022-03-04 | 上海应用技术大学 | 一种基于imu预积分的激光slam定位方法 |
CN114659514A (zh) * | 2022-03-21 | 2022-06-24 | 东南大学 | 一种基于体素化精配准的LiDAR-IMU-GNSS融合定位方法 |
Non-Patent Citations (1)
Title |
---|
彭雅慧 等: "基于直线检测和数字地图匹配的车辆航向角估计", 《电子测量与仪器学报》, vol. 36, no. 3, 31 March 2022 (2022-03-31), pages 194 - 199 * |
Cited By (10)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN113551666A (zh) * | 2021-07-06 | 2021-10-26 | 杭州鸿泉物联网技术股份有限公司 | 自动驾驶多传感器融合定位方法和装置、设备及介质 |
CN115561731A (zh) * | 2022-12-05 | 2023-01-03 | 安徽蔚来智驾科技有限公司 | 位姿优化方法、点云地图建立方法、计算机设备及介质 |
CN115561731B (zh) * | 2022-12-05 | 2023-03-10 | 安徽蔚来智驾科技有限公司 | 位姿优化方法、点云地图建立方法、计算机设备及介质 |
CN116242372A (zh) * | 2023-01-03 | 2023-06-09 | 东南大学 | 一种gnss拒止环境下的uwb-激光雷达-惯导融合定位方法 |
CN115797425A (zh) * | 2023-01-19 | 2023-03-14 | 中国科学技术大学 | 一种基于点云鸟瞰图和由粗到精策略的激光全局定位方法 |
CN116592888A (zh) * | 2023-05-08 | 2023-08-15 | 五八智能科技(杭州)有限公司 | 一种巡逻机器人全局定位的方法、***、装置和介质 |
CN117250623A (zh) * | 2023-11-20 | 2023-12-19 | 山东新坐标智能装备有限公司 | 激光雷达与互补定位融合的定位方法、***及移动机器人 |
CN117250623B (zh) * | 2023-11-20 | 2024-02-27 | 苏州新坐标智能装备有限公司 | 激光雷达与互补定位融合的定位方法、***及移动机器人 |
CN117433511A (zh) * | 2023-12-20 | 2024-01-23 | 绘见科技(深圳)有限公司 | 一种多传感器融合定位方法 |
CN117433511B (zh) * | 2023-12-20 | 2024-03-12 | 绘见科技(深圳)有限公司 | 一种多传感器融合定位方法 |
Also Published As
Publication number | Publication date |
---|---|
CN115407357B (zh) | 2024-05-31 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
CN109709801B (zh) | 一种基于激光雷达的室内无人机定位***及方法 | |
CN115407357A (zh) | 基于大场景的低线束激光雷达-imu-rtk定位建图算法 | |
CN111862672B (zh) | 基于顶视图的停车场车辆自定位及地图构建方法 | |
CN110261870B (zh) | 一种用于视觉-惯性-激光融合的同步定位与建图方法 | |
CN109993113B (zh) | 一种基于rgb-d和imu信息融合的位姿估计方法 | |
CN112347840B (zh) | 视觉传感器激光雷达融合无人机定位与建图装置和方法 | |
CN113781582B (zh) | 基于激光雷达和惯导联合标定的同步定位与地图创建方法 | |
CN110411462B (zh) | 一种gnss/惯导/车道线约束/里程计多源融合方法 | |
CN114526745B (zh) | 一种紧耦合激光雷达和惯性里程计的建图方法及*** | |
CN110044354A (zh) | 一种双目视觉室内定位与建图方法及装置 | |
CN107167826B (zh) | 一种自动驾驶中基于可变网格的图像特征检测的车辆纵向定位***及方法 | |
CN114199259B (zh) | 一种基于运动状态与环境感知的多源融合导航定位方法 | |
CN111426320B (zh) | 一种基于图像匹配/惯导/里程计的车辆自主导航方法 | |
CN112987065B (zh) | 一种融合多传感器的手持式slam装置及其控制方法 | |
CN112556719B (zh) | 一种基于cnn-ekf的视觉惯性里程计实现方法 | |
CN208323361U (zh) | 一种基于深度视觉的定位装置及机器人 | |
CN114019552A (zh) | 一种基于贝叶斯多传感器误差约束的定位置信度优化方法 | |
CN115272596A (zh) | 一种面向单调无纹理大场景的多传感器融合slam方法 | |
CN112781582A (zh) | 卫星弱观测条件下的多传感器融合高精度位姿估计算法 | |
CN116448100A (zh) | 一种多传感器融合的近岸无人船slam方法 | |
CN115574816A (zh) | 仿生视觉多源信息智能感知无人平台 | |
CN116242374A (zh) | 一种基于直接法的多传感器融合的slam定位方法 | |
CN115930977A (zh) | 特征退化场景的定位方法、***、电子设备和可读存介质 | |
CN117367427A (zh) | 一种适用于室内环境中的视觉辅助激光融合IMU的多模态slam方法 | |
CN115218889A (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 |