CN117387604A - 基于4d毫米波雷达和imu融合的定位与建图方法及系统 - Google Patents

基于4d毫米波雷达和imu融合的定位与建图方法及系统 Download PDF

Info

Publication number
CN117387604A
CN117387604A CN202311085217.2A CN202311085217A CN117387604A CN 117387604 A CN117387604 A CN 117387604A CN 202311085217 A CN202311085217 A CN 202311085217A CN 117387604 A CN117387604 A CN 117387604A
Authority
CN
China
Prior art keywords
point cloud
millimeter wave
wave radar
imu
cloud data
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
CN202311085217.2A
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.)
Northwestern Polytechnical University
Original Assignee
Northwestern Polytechnical University
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by Northwestern Polytechnical University filed Critical Northwestern Polytechnical University
Priority to CN202311085217.2A priority Critical patent/CN117387604A/zh
Publication of CN117387604A publication Critical patent/CN117387604A/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/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
    • 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
    • G01S13/00—Systems using the reflection or reradiation of radio waves, e.g. radar systems; Analogous systems using reflection or reradiation of waves whose nature or wavelength is irrelevant or unspecified
    • G01S13/86—Combinations of radar systems with non-radar systems, e.g. sonar, direction finder
    • 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
    • G01S7/00—Details of systems according to groups G01S13/00, G01S15/00, G01S17/00
    • G01S7/02—Details of systems according to groups G01S13/00, G01S15/00, G01S17/00 of systems according to group G01S13/00
    • G01S7/40—Means for monitoring or calibrating
    • 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
    • G06T7/75—Determining position or orientation of objects or cameras using feature-based methods involving models
    • 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
    • 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/10032—Satellite or aerial image; Remote sensing
    • G06T2207/10044—Radar image
    • 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)
  • Computer Vision & Pattern Recognition (AREA)
  • Theoretical Computer Science (AREA)
  • Radar Systems Or Details Thereof (AREA)

Abstract

本发明涉及一种基于4D毫米波雷达和IMU融合的定位与建图方法及系统，该方法先将4D毫米波雷达获得的4D点云数据通过点云预处理算法进行去噪获得稳定的4D点云数据，然后根据稳定的4D点云数据解算出自我平台的三维体速度，再融合稳定的4D点云数据以及惯导器件的位姿数据，优化解算出自我平台的里程计信息，最后融合自我平台的里程计信息和三维体速度，优化解算出最优里程计信息，同时根据稳定的4D点云数据和最优里程计信息绘制出环境点云地图。本发明的基于4D毫米波雷达和IMU融合的定位与建图方法及系统具有环境感知力广泛、成本低且实时性能好的特点。

Description

基于4D毫米波雷达和IMU融合的定位与建图方法及系统
技术领域
本发明涉及无人系统自主导航和环境感知技术领域，特别是涉及一种基于4D毫米波雷达和IMU融合的定位与建图方法及系统。
背景技术
SLAM代表同时定位与地图构建(Simultaneous Localization and Mapping)，是一种机器人或无人驾驶车辆能够在未知环境中实现自主定位和建立环境地图的技术。IMU(惯性测量单元)是一种集成了加速度计和陀螺仪的传感器装置。它可以测量物体的加速度、角速度和姿态等动态信息。IMU可以提供高频率的姿态更新，对于快速运动的机器人或车辆的定位至关重要。
传统的SLAM系统主要依赖于激光雷达进行环境感知和距离测量。激光雷达可以提供高精度的距离数据，配合IMU的姿态和运动信息，可以实现较为准确的SLAM定位和地图构建。
但是，激光雷达不仅价格昂贵，且对于一些特定环境，如透明或反射表面的物体无法提供准确的数据，在雨雪等遮挡环境下数据量会骤降，这会导致使用SLAM获得的地图的不完整或不准确。另外，现有的SLAM方法在处理大规模环境时会存在计算和处理延迟，实时性能差。
发明内容
基于此，有必要针对上述技术问题，提供一种环境感知力广泛、成本低且实时性能好的基于4D毫米波雷达和IMU融合的定位与建图方法及系统。
第一方面，本发明提供了一种基于4D毫米波雷达和IMU融合的定位与建图方法，包括以下步骤：
将4D毫米波雷达获得的4D点云数据通过点云预处理算法进行去噪获得稳定的4D点云数据；
根据稳定的4D点云数据解算出自我平台的三维体速度；
融合稳定的4D点云数据以及惯导器件的位姿数据，优化解算出自我平台的里程计信息；
融合自我平台的里程计信息和三维体速度，优化解算出最优里程计信息，同时根据稳定的4D点云数据和最优里程计信息绘制出环境点云地图。
在其中一个实施例中，根据稳定的4D点云数据解算出自我平台的三维体速度，包括：.
通过稳定的4D点云数据的多维信息构建最小二乘求解模型；
利用线性最小二乘求解方法对最小二乘求解模型进行求解获得自我平台的三维体速度。
在其中一个实施例中，融合稳定的4D点云数据以及惯导器件的位姿数据以及融合自我平台的里程计信息和三维体速度均采用图优化模型以滑动窗口方式进行融合。
在其中一个实施例中，最小二乘求解模型为
式中，是测得的多普勒速度，N为测量值总个数，r代表雷达坐标系，vr表示雷达速度。
在其中一个实施例中，图优化模型的因子建模结果包括IMU预积分因子、里程计因子和速度先验因子。
在其中一个实施例中，IMU预积分因子为
式中，运算符vec(·)用于提取四元数的矢量部分，m代表第m帧，代表旋转矩阵，/>分别代表系统的位置、速度、姿态状态，ba,m、bg,m代表加速度计和陀螺仪的偏置，/>代表预积分量，gw代表重力参数，Δτm为高斯白噪声参数。
在其中一个实施例中，里程计因子为
式中，表示四元数乘法运算符，vec(·)用于提取四元数的矢量部分，/> 分别表示m帧的位姿的逆和预测位姿，Δpm和/>分别表示位置变化量和预测位置变化量。
在其中一个实施例中，速度先验因子为
式中，vradar是一个先验速度估计值，是估计速度。
第二方面，本发明还提供了一种基于4D毫米波雷达和IMU融合的定位与建图装置。装置包括：
点云预处理模块，用于将4D毫米波雷达获得的4D点云数据通过点云预处理算法进行去噪获得稳定的4D点云数据；
雷达自运动估计模块，用于根据稳定的4D点云数据解算出自我平台的三维体速度；
位姿解算模块，用于融合稳定的4D点云数据以及惯导器件的位姿数据，优化解算出自我平台的里程计信息；
基于图优化的多传感器融合SLAM模块，用于融合自我平台的里程计信息和三维体速度，优化解算出最优里程计信息，同时根据稳定的4D点云数据和最优里程计信息绘制出环境点云地图。
本发明的有益效果是：
(1)本发明通过将4D毫米波雷达与IMU相结合，利用4D毫米波雷达的高精度距离信息和IMU的高频率姿态信息，实现精确的定位和地图构建。4D毫米波雷达能够提供准确的环境感知和障碍物检测，而IMU则可以提供连续的姿态更新。通过将两者进行融合，可以克服彼此的局限性，提高SLAM系统的鲁棒性和精度，增加了SLAM系统实时性能。
(2)4D毫米波雷达价格低，能够降低整个SLAM系统的成本，并且对透明和反射表面具有较好的测量能力，在雨雪等遮挡环境下数据量也比较充沛，保证SLAM系统最终建图的完整性和准确性。
附图说明
图1为本发明实施例中提供的一种基于4D毫米波雷达和IMU融合的定位与建图方法的流程示意图；
图2为本发明实施例中提供的另一种基于4D毫米波雷达和IMU融合的定位与建图方法的流程示意图；
图3为本发明实施例中提供的基于图优化的多传感器融合SLAM模块结构示意图。
具体实施方式
为了使本发明的目的、技术方案及优点更加清楚明白，以下结合附图及实施例，对本发明进行进一步详细说明。应当理解，此处描述的具体实施例仅仅用以解释本发明，并不用于限定本发明。
在一个实施例中，如图1所示，图1是本发明实施例提供的基于4D毫米波雷达和IMU融合的定位与建图方法的流程示意图之一，本实施例的基于4D毫米波雷达和IMU融合的定位与建图方法包括以下步骤：
S101、将4D毫米波雷达获得的4D点云数据通过点云预处理算法进行去噪获得稳定的4D点云数据。
具体的，4D毫米波雷达获得的4D点云数据含有噪点点云，通过点云预处理算法可以将噪点点云进行去除。需要说明的是，点云预处理算法为本领域人员熟知，在此不再赘述。
S102、根据稳定的4D点云数据解算出自我平台的三维体速度。自我平台的三维体速度基于单次雷达扫描，每次扫描由雷达帧中的3D点和从多普勒频移获得的相应速度组成。其中，自我平台可以是但不限于无人机和无人车。
自我平台的三维体速度的估计可以基于最小平方解进行
其中，给定目标位置pr，通过归一化得到方向rr，测得的多普勒速度是方向rr与雷达速度vr的标量积：
S103、融合稳定的4D点云数据以及惯导器件的位姿数据，优化解算出自我平台的里程计信息。
S104、融合自我平台的里程计信息和三维体速度，优化解算出最优里程计信息，同时根据稳定的4D点云数据和最优里程计信息绘制出环境点云地图。
本实施例的基于4D毫米波雷达和IMU融合的定位与建图方法通过将4D毫米波雷达与IMU相结合，利用4D毫米波雷达的高精度距离信息和IMU的高频率姿态信息，实现精确的定位和地图构建。4D毫米波雷达能够提供准确的环境感知和障碍物检测，而IMU则可以提供连续的姿态更新。通过将两者进行融合，可以克服彼此的局限性，提高SLAM系统的鲁棒性和精度，增加了SLAM方法实时性能。另外，4D毫米波雷达价格低，能够降低整个SLAM系统的成本，并且对透明和反射表面具有较好的测量能力，在雨雪等遮挡环境下数据量也比较充沛，保证SLAM系统最终建图的完整性和准确性。
在其中一个实施例中，如图2所示，图2为本发明实施例中提供的另一种基于4D毫米波雷达和IMU融合的定位与建图方法的流程示意图，本实施例涉及的是如何根据稳定的4D点云数据解算出自我平台的三维体速度，在上述实施例的基础上，步骤S102包括：
S201、通过稳定的4D点云数据的多维信息构建最小二乘求解模型。
具体的，最小二乘求解模型为
式中，是测得的多普勒速度，N为测量值总个数，r代表雷达坐标系，vr表示雷达速度。
S202、利用线性最小二乘求解方法对最小二乘求解模型进行求解获得自我平台的三维体速度。
利用线性最小二乘(LSQ)解可得到如下解：
式中，为求得的平台三维速度，H矩阵为最小二乘求解模型中含rx,N、ry,N、rz,N的矩阵，yr为测得的多普勒速度/>
用所有的测量方法求解此最小二乘问题都容易出错。环境不能假设是静态的，所以噪声、反射或重影而产生的异常值必须被删除，前述的点云预处理是为了解决此问题。
在一个实施例中，融合稳定的4D点云数据以及惯导器件的位姿数据以及融合自我平台的里程计信息和三维体速度均采用图优化模型以滑动窗口方式进行融合。
具体的，如图3所示，图3为本发明实施例中提供的基于图优化的多传感器融合SLAM模块结构示意图，可以将稳定的4D点云数据、惯导器件的位姿数据以及三维体速度置于一个多传感器融合SLAM框架中以滑动窗口方式进行融合。
在融合过程中，时间t时滑动窗口中包含的状态定义为其中在时间t内滑动窗口中活动IMU状态。Tt表示t处的IMU量测集合。
IMU状态为
式中，是一个单位四元数，表示从世界帧{w}到IMU帧{I}的旋转。pwT和vwT分别是IMU位置和速度，bg、ba分别是陀螺仪和加速器的随机行走偏差。
利用状态的定义，目标是最小化上式中不同量测产生残差的代价函数
其中，第一项是基于IMU的残差，rI,m定义帧m和m+1之间的测量残差。第二项是里程计量测残差，最后一项是速度残差。Ot和V是时间t时滑动窗口内的一组测量值Σi和Σj分别是两个测量值的协方差。
融合后的优化解算通常通过线性近似使用迭代最小二乘解算器来解决。在本实施例中，具体但不限于基于GTSAM进行解算。
在一个可选的实施例中，图优化模型的因子建模结果包括IMU预积分因子、里程计因子和速度先验因子。
在其中一个实施例中，IMU预积分因子为
式中，运算符vec(·)用于提取四元数的矢量部分，m代表第m帧，代表旋转矩阵，/>分别代表系统的位置、速度、姿态状态，ba,m、bg,m代表加速度计和陀螺仪的偏置，/>代表预积分量，gw代表重力参数，Δτm为高斯白噪声参数。
具体的，IMU预积分因子rI,m为包含雷达关键帧之间的相对运动约束的误差项。通过IMU预积分的方式，在已知上一时刻的IMU状态量的基础上，利用IMU测量得到的线加速度和角速度，做积分运算得到当前时刻状态量，最终能实现在点云匹配得到雷达位姿的基础上，通过IMU数据获得IMU频率下的里程计信息。
在其中一个实施例中，里程计因子为
式中，表示四元数乘法运算符，vec(·)用于提取四元数的矢量部分，/> 分别表示m帧的位姿的逆和预测位姿，Δpm和/>分别表示位置变化量和预测位置变化量。
在其中一个实施例中，速度先验因子为
式中，vradar是一个先验速度估计值，是估计速度。
具体的，速度先验因子限制了机器人速度的估计，以提高SLAM的鲁棒性。
需要说明的是，本发明的传感器基于4D毫米波雷达和惯性测量元件，可安装在车载/无人机平台，为平台提供精准定位信息的同时提供点云地图。
基于同样的发明构思，本发明还提供了一种基于4D毫米波雷达和IMU融合的定位与建图装置。装置包括：
点云预处理模块，用于将4D毫米波雷达获得的4D点云数据通过点云预处理算法进行去噪获得稳定的4D点云数据。点云预处理模块输入是ros点云数据类型，来自雷达点云驱动，输出是ros点云数据类型，发送给NDT点云匹配模块和雷达自运动估计模块。
具体的，点云预处理模块的数据流如表1所示。
表1点云预处理模块的数据流表
雷达自运动估计模块，用于根据稳定的4D点云数据解算出自我平台的三维体速度。雷达自运动估计模块是通过4D毫米波雷达点云中的多普勒速度信息，计算雷达自身运动速度。输入是经过点云预处理模块处理后发送的ros点云数据，输出是雷达当前的瞬时速度。其中速度结果发送给gtsam。
具体的，雷达自运动估计模块的数据流如表2所示。
表2雷达自运动估计模块数据流表
位姿解算模块，用于融合稳定的4D点云数据以及惯导器件的位姿数据，优化解算出自我平台的里程计信息。输入是经过点云预处理模块处理后发送的ros点云数据，输出是雷达当前六自由度位姿和投影到世界坐标系下的全局点云地图。其中位姿结果发送给gtsam。
具体的，位姿解算模块的数据流如表3所示。
表3位姿解算模块
基于图优化的多传感器融合SLAM模块，用于融合自我平台的里程计信息和三维体速度，优化解算出最优里程计信息，同时根据稳定的4D点云数据和最优里程计信息绘制出环境点云地图。
具体的，基于图优化的多传感器融合SLAM模块的数据流如表4所示。
表格4基于图优化的毫米波雷达与IMU融合SLAM算法数据流表
本实施例的基于4D毫米波雷达和IMU融合的定位与建图系统通过将4D毫米波雷达与IMU两者进行融合，可以克服彼此的局限性，提高SLAM系统的鲁棒性和精度，增加了SLAM系统实时性能。另外，4D毫米波雷达价格低，能够降低整个SLAM系统的成本。
以上所述实施例仅表达了本发明的几种实施方式，其描述较为具体和详细，但并不能因此而理解为对本发明专利范围的限制。应当指出的是，对于本领域的普通技术人员来说，在不脱离本发明构思的前提下，还可以做出若干变形和改进，这些都属于本发明的保护范围。因此，本发明的保护范围应以所附权利要求为准。

Claims (9)

1.一种基于4D毫米波雷达和IMU融合的定位与建图方法，其特征在于，包括以下步骤：
将4D毫米波雷达获得的4D点云数据通过点云预处理算法进行去噪获得稳定的4D点云数据；
根据所述稳定的4D点云数据解算出自我平台的三维体速度；
融合所述稳定的4D点云数据以及惯导器件的位姿数据，优化解算出自我平台的里程计信息；
融合所述自我平台的里程计信息和三维体速度，优化解算出最优里程计信息，同时根据稳定的4D点云数据和最优里程计信息绘制出环境点云地图。
2.根据权利要求1所述的基于4D毫米波雷达和IMU融合的定位与建图方法，其特征在于，根据所述稳定的4D点云数据解算出自我平台的三维体速度，包括：
通过稳定的4D点云数据的多维信息构建最小二乘求解模型；
利用线性最小二乘求解方法对所述最小二乘求解模型进行求解获得自我平台的三维体速度。
3.根据权利要求2所述的基于4D毫米波雷达和IMU融合的定位与建图方法，其特征在于，融合所述稳定的4D点云数据以及惯导器件的位姿数据以及融合所述自我平台的里程计信息和三维体速度均采用图优化模型以滑动窗口方式进行融合。
4.根据权利要求3所述的基于4D毫米波雷达和IMU融合的定位与建图方法，其特征在于，所述最小二乘求解模型为
式中，是测得的多普勒速度，N为测量值总个数，r表示雷达坐标系，vr表示雷达速度。
5.根据权利要求3所述的基于4D毫米波雷达和IMU融合的定位与建图方法，其特征在于，所述图优化模型的因子建模结果包括IMU预积分因子、里程计因子和速度先验因子。
6.根据权利要求5所述的基于4D毫米波雷达和IMU融合的定位与建图方法，其特征在于，所述IMU预积分因子为
式中，运算符vec(·)用于提取四元数的矢量部分，m代表第m帧，代表旋转矩阵，分别代表系统的位置、速度、姿态状态，ba,m、bg,m代表加速度计和陀螺仪的偏置，/>代表预积分量，gw代表重力参数，Δτm为高斯白噪声参数。
7.根据权利要求5所述的基于4D毫米波雷达和IMU融合的定位与建图方法，其特征在于，所述里程计因子为
式中，表示四元数乘法运算符，vec(·)用于提取四元数的矢量部分，/> 分别表示m帧的位姿的逆和预测位姿，Δpm和/>分别表示位置变化量和预测位置变化量。
8.根据权利要求5所述的基于4D毫米波雷达和IMU融合的定位与建图方法，其特征在于，所述速度先验因子为
式中，vradar是一个先验速度估计值，是估计速度。
9.一种基于4D毫米波雷达和IMU融合的定位与建图系统，其特征在于，所述系统包括：
点云预处理模块，用于将4D毫米波雷达获得的4D点云数据通过点云预处理算法进行去噪获得稳定的4D点云数据；
雷达自运动估计模块，用于根据所述稳定的4D点云数据解算出自我平台的三维体速度；
位姿解算模块，用于融合所述稳定的4D点云数据以及惯导器件的位姿数据，优化解算出自我平台的里程计信息；
基于图优化的多传感器融合SLAM模块，用于融合所述自我平台的里程计信息和三维体速度，优化解算出最优里程计信息，同时根据稳定的4D点云数据和最优里程计信息绘制出环境点云地图。
CN202311085217.2A 2023-08-25 2023-08-25 基于4d毫米波雷达和imu融合的定位与建图方法及系统 Pending CN117387604A (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202311085217.2A CN117387604A (zh) 2023-08-25 2023-08-25 基于4d毫米波雷达和imu融合的定位与建图方法及系统

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202311085217.2A CN117387604A (zh) 2023-08-25 2023-08-25 基于4d毫米波雷达和imu融合的定位与建图方法及系统

Publications (1)

Publication Number Publication Date
CN117387604A true CN117387604A (zh) 2024-01-12

Family

ID=89463748

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202311085217.2A Pending CN117387604A (zh) 2023-08-25 2023-08-25 基于4d毫米波雷达和imu融合的定位与建图方法及系统

Country Status (1)

Country Link
CN (1) CN117387604A (zh)

Cited By (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN117593650A (zh) * 2024-01-18 2024-02-23 上海几何伙伴智能驾驶有限公司 基于4d毫米波雷达与sam图像分割的动点滤除视觉slam方法

Cited By (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN117593650A (zh) * 2024-01-18 2024-02-23 上海几何伙伴智能驾驶有限公司 基于4d毫米波雷达与sam图像分割的动点滤除视觉slam方法
CN117593650B (zh) * 2024-01-18 2024-04-26 上海几何伙伴智能驾驶有限公司 基于4d毫米波雷达与sam图像分割的动点滤除视觉slam方法

Similar Documents

Publication Publication Date Title
CN112347840B (zh) 视觉传感器激光雷达融合无人机定位与建图装置和方法
CN109709801B (zh) 一种基于激光雷达的室内无人机定位系统及方法
CN109211251B (zh) 一种基于激光和二维码融合的即时定位与地图构建方法
CN112639502B (zh) 机器人位姿估计
JP2022019642A (ja) マルチセンサ融合に基づく測位方法及び装置
CN114526745B (zh) 一种紧耦合激光雷达和惯性里程计的建图方法及系统
KR20210111180A (ko) 위치 추적 방법, 장치, 컴퓨팅 기기 및 컴퓨터 판독 가능한 저장 매체
Barjenbruch et al. Joint spatial-and Doppler-based ego-motion estimation for automotive radars
CN114323033B (zh) 基于车道线和特征点的定位方法、设备及自动驾驶车辆
CN113933818A (zh) 激光雷达外参的标定的方法、设备、存储介质及程序产品
CN112967392A (zh) 一种基于多传感器触合的大规模园区建图定位方法
Wu et al. Robust LiDAR-based localization scheme for unmanned ground vehicle via multisensor fusion
CN117387604A (zh) 基于4d毫米波雷达和imu融合的定位与建图方法及系统
CN117490683A (zh) 一种井下隧道多传感器融合算法的定位建图方法
CN116202509A (zh) 一种面向室内多层建筑的可通行地图生成方法
CN118310531A (zh) 一种由粗到细点云配准的机器人跨场景定位方法及系统
CN114608568A (zh) 一种基于多传感器信息即时融合定位方法
CN115900697B (zh) 对象运动轨迹信息处理方法、电子设备以及自动驾驶车辆
CN112923934A (zh) 一种适用于非结构化场景中结合惯导的激光slam技术
CN113495281B (zh) 可移动平台的实时定位方法及装置
CN116222541A (zh) 利用因子图的智能多源组合导航方法及装置
CN115562076A (zh) 用于无人驾驶矿车的仿真系统、方法以及存储介质
Kwon et al. Radar velocity measurements aided navigation system for UAVs
Zhou et al. Localization for unmanned vehicle
Conway et al. Vision-based Velocimetry over Unknown Terrain with a Low-Noise IMU

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