CN117330052A - 基于红外视觉、毫米波雷达和imu融合的定位与建图方法及系统 - Google Patents
基于红外视觉、毫米波雷达和imu融合的定位与建图方法及系统 Download PDFInfo
- Publication number
- CN117330052A CN117330052A CN202311083869.2A CN202311083869A CN117330052A CN 117330052 A CN117330052 A CN 117330052A CN 202311083869 A CN202311083869 A CN 202311083869A CN 117330052 A CN117330052 A CN 117330052A
- Authority
- CN
- China
- Prior art keywords
- point cloud
- imu
- information
- infrared
- millimeter wave
- 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
Links
- 238000000034 method Methods 0.000 title claims abstract description 68
- 230000004927 fusion Effects 0.000 title claims abstract description 34
- 238000013507 mapping Methods 0.000 title claims abstract description 28
- 238000012545 processing Methods 0.000 claims abstract description 39
- 125000004122 cyclic group Chemical group 0.000 claims abstract description 30
- 238000007781 pre-processing Methods 0.000 claims abstract description 17
- 238000004422 calculation algorithm Methods 0.000 claims abstract description 12
- 238000005457 optimization Methods 0.000 claims description 37
- 238000005259 measurement Methods 0.000 claims description 29
- 230000008569 process Effects 0.000 claims description 21
- 239000011159 matrix material Substances 0.000 claims description 16
- 238000013138 pruning Methods 0.000 claims description 13
- 230000009466 transformation Effects 0.000 claims description 11
- 230000004807 localization Effects 0.000 claims description 4
- 238000007670 refining Methods 0.000 claims description 3
- 230000002123 temporal effect Effects 0.000 claims description 3
- 230000008447 perception Effects 0.000 abstract description 6
- 238000010586 diagram Methods 0.000 description 6
- 230000010354 integration Effects 0.000 description 6
- 230000006870 function Effects 0.000 description 5
- 230000005855 radiation Effects 0.000 description 5
- 238000012937 correction Methods 0.000 description 4
- 238000005286 illumination Methods 0.000 description 4
- 230000001133 acceleration Effects 0.000 description 2
- 230000008859 change Effects 0.000 description 2
- 238000006243 chemical reaction Methods 0.000 description 2
- 230000008030 elimination Effects 0.000 description 2
- 238000003379 elimination reaction Methods 0.000 description 2
- 238000005516 engineering process Methods 0.000 description 2
- 230000007613 environmental effect Effects 0.000 description 2
- 230000004044 response Effects 0.000 description 2
- 230000003068 static effect Effects 0.000 description 2
- 238000013519 translation Methods 0.000 description 2
- 230000002159 abnormal effect Effects 0.000 description 1
- 230000009286 beneficial effect Effects 0.000 description 1
- 238000004364 calculation method Methods 0.000 description 1
- 238000010276 construction Methods 0.000 description 1
- 238000001514 detection method Methods 0.000 description 1
- 239000000428 dust Substances 0.000 description 1
- 230000000694 effects Effects 0.000 description 1
- 230000002708 enhancing effect Effects 0.000 description 1
- 239000003897 fog Substances 0.000 description 1
- 238000007499 fusion processing Methods 0.000 description 1
- 230000005484 gravity Effects 0.000 description 1
- 238000010438 heat treatment Methods 0.000 description 1
- 239000000463 material Substances 0.000 description 1
- 238000000691 measurement method Methods 0.000 description 1
- 239000000203 mixture Substances 0.000 description 1
- 238000012986 modification Methods 0.000 description 1
- 230000004048 modification Effects 0.000 description 1
- 238000010606 normalization Methods 0.000 description 1
- 230000000149 penetrating effect Effects 0.000 description 1
- 238000005295 random walk Methods 0.000 description 1
- 238000002310 reflectometry Methods 0.000 description 1
- 239000000779 smoke Substances 0.000 description 1
- 238000001931 thermography Methods 0.000 description 1
- 230000000007 visual effect Effects 0.000 description 1
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/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
- 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/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/1656—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 passive imaging devices, e.g. cameras
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01J—MEASUREMENT OF INTENSITY, VELOCITY, SPECTRAL CONTENT, POLARISATION, PHASE OR PULSE CHARACTERISTICS OF INFRARED, VISIBLE OR ULTRAVIOLET LIGHT; COLORIMETRY; RADIATION PYROMETRY
- G01J5/00—Radiation pyrometry, e.g. infrared or optical thermometry
- G01J5/48—Thermography; Techniques using wholly visual means
-
- 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
- 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
- G01S13/867—Combination of radar systems with cameras
-
- 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)
- Spectroscopy & Molecular Physics (AREA)
- Aiming, Guidance, Guns With A Light Source, Armor, Camouflage, And Targets (AREA)
Abstract
本发明涉及一种基于红外视觉、毫米波雷达和IMU融合的定位与建图方法及系统，先将红外图像进行循环处理，同时将循环处理输出的红外相机的位姿数据与惯导器件的位姿数据融合，优化解算出自我平台的第一里程计信息，然后将4D毫米波雷达获得的4D点云数据通过点云预处理算法进行去噪获得稳定的4D点云数据，再根据稳定的4D点云数据解算出自我平台的三维体速度，再融合稳定的4D点云数据以及惯导器件的位姿数据，优化解算出自我平台的第二里程计信息，最后融合第一里程计信息和第二里程计信息和三维体速度，优化解算出最优里程计信息，同时根据稳定的4D点云数据和最优里程计信息绘制出环境点云地图。本发明具有环境感知更全面且动态物体处理能力好的特点。
Description
技术领域
本发明涉及无人系统自主导航和环境感知技术领域，特别是涉及一种基于红外视觉、毫米波雷达和IMU融合的定位与建图方法及系统。
背景技术
SLAM代表同时定位与地图构建(Simultaneous Localization and Mapping)，是一种机器人或无人驾驶车辆能够在未知环境中实现自主定位和建立环境地图的技术。SLAM技术旨在通过使用移动机器人上的传感器来同时实现定位自身位置和构建周围环境的地图，而不需要预先获取地图信息。这对于自主导航、环境感知和路径规划等应用非常重要。
在传统的SLAM系统中，常用的传感器包括激光雷达、摄像头和IMU。激光雷达可以提供高精度的距离测量，但对于低反射率的表面或者特殊材质的物体存在限制。摄像头可以获取丰富的视觉信息，但在光照不足或者纹理较弱的情况下表现不佳。IMU可以提供高频率的姿态和运动状态测量，但存在漂移问题。
发明内容
基于此，有必要针对上述技术问题，提供一种环境感知更全面且动态物体处理能力好的基于红外视觉、毫米波雷达和IMU融合的定位与建图方法及系统。
第一方面，本发明提供了一种基于红外视觉、毫米波雷达和IMU融合的定位与建图方法，包括以下步骤：
将红外图像进行循环处理，同时将循环处理输出的红外相机的位姿数据与惯导器件的位姿数据融合，优化解算出自我平台的第一里程计信息，循环处理包括图像对齐、点初始化、点位优化、地标初始化和剪枝处理；
将4D毫米波雷达获得的4D点云数据通过点云预处理算法进行去噪获得稳定的4D点云数据；
根据稳定的4D点云数据解算出自我平台的三维体速度；
融合稳定的4D点云数据以及惯导器件的位姿数据，优化解算出自我平台的第二里程计信息；
融合自我平台的第一里程计信息和第二里程计信息和三维体速度，优化解算出最优里程计信息，同时根据稳定的4D点云数据和最优里程计信息绘制出环境点云地图。
在其中一个实施例中，图像对齐处理为通过构建图像金字塔模型，利用IMU的姿态数据作为信息先验，通过直接法执行红外图像的配准，根据配准结果计算出自我平台的位姿信息。
在其中一个实施例中，点初始化处理为将红外图像划分为32×32个像素块，在像素块中通过设定阈值筛选出图像关键帧。
在其中一个实施例中，点位优化处理为通过细化像素块位置计算残差和比率消除不受约束的点。
在其中一个实施例中，地标初始化和剪枝处理为根据红外图像中设定的对应关系点确定初始化3D地标，同时根据红外图像位置和质量优化剪枝算法对深度不一致的地标进行剪枝。
在其中一个实施例中，将循环处理输出的红外相机的位姿数据与惯导器件的位姿数据融合，优化解算出自我平台的第一里程计信息包括：
根据惯导器件的测量值和循环处理输出的红外相机的位姿数据建立重投影误差模型；
利用重投影误差模型构建非线性优化问题；
根据非线性优化问题求解出自我平台的里程计信息。
在其中一个实施例中，融合稳定的4D点云数据以及惯导器件的位姿数据以及融合第一里程计信息和第二里程计信息和三维体速度均采用图优化模型以滑动窗口方式进行融合。
在其中一个实施例中，重投影误差模型为
式中，是相机坐标系中的点坐标，/>是世界坐标系中对应的3D地标位置，/>是从IMU帧中IMU到相机坐标系的变换，/>是IMU在世界坐标系中的变换，K是固有相机矩阵。
在其中一个实施例中，非线性优化问题的成本函数为
式中，k表示在包含时间帧和关键帧的滑动窗口中正在处理的帧，L(k)表示在帧k中可观察到的一组地标，表示在帧k中可见的每个地标l的重投影误差的叠加向量，表示帧k中地标测量的协方差矩阵，/>表示IMU测量的协方差矩阵。
第二方面，本发明还提供了一种基于红外视觉、毫米波雷达和IMU融合的定位与建图系统，系统包括：
循环解算模块，用于将红外图像进行循环处理，同时将循环处理输出的红外相机的位姿数据与惯导器件的位姿数据融合，优化解算出自我平台的第一里程计信息，循环处理包括图像对齐、点初始化、点位优化、地标初始化和剪枝处理；
点云预处理模块，用于将4D毫米波雷达获得的4D点云数据通过点云预处理算法进行去噪获得稳定的4D点云数据；
雷达自运动估计模块，用于根据稳定的4D点云数据解算出自我平台的三维体速度；
位姿解算模块，用于融合稳定的4D点云数据以及惯导器件的位姿数据，优化解算出自我平台的第二里程计信息；
基于图优化的多传感器融合SLAM模块，用于融合第一里程计信息和第二里程计信息和三维体速度，优化解算出最优里程计信息，同时根据稳定的4D点云数据和最优里程计信息绘制出环境点云地图。
本发明的有益效果是：
(1)本发明通过融合红外视觉、毫米波雷达和IMU的数据，可以获得更全面、多样化的环境感知能力。红外视觉提供独特的热特征感知，毫米波雷达提供高精度的距离和速度测量，IMU提供快速的姿态和运动状态估计，从而增强系统对周围环境的感知能力。
(2)本发明中，红外视觉能够在光照较差的情况下提供可靠的环境感知，毫米波雷达具备强大的穿透能力，IMU提供快速响应和姿态校正能力，共同提高系统的适应性和稳定性，能够在光照不足、恶劣天气和动态环境等挑战性条件下表现出良好的鲁棒性。
(3)本发明中，红外视觉和毫米波雷达的数据融合能够补充传统视觉SLAM系统的局限性，IMU的姿态校正和状态估计提高了定位的精度和稳定性，能够提供更准确和稳定的定位结果。
(4)本发明中，通过红外视觉和毫米波雷达的信息融合，可以区分动态物体和静态环境，动态物体处理能力好，从而能够避免姿态漂移或地图构建错误。
附图说明
图1是本发明实施例提供的基于红外视觉、毫米波雷达和IMU融合的定位与建图方法的流程示意图之一；
图2为本发明实施例中提供的解算出自我平台的三维体速度方法的流程示意图；
图3是本发明实施例提供的图像金字塔模型结构示意图；
图4是本发明实施例提供的红外-惯性里程计优化方法框架示意图之一；
图5是本发明实施例提供的基于红外视觉、毫米波雷达和IMU融合的定位与建图方法的流程示意图之一；
图6为本发明实施例中提供的雷达、IMU融合里程计结构示意图。
具体实施方式
为了使本发明的目的、技术方案及优点更加清楚明白，以下结合附图及实施例，对本发明进行进一步详细说明。应当理解，此处描述的具体实施例仅仅用以解释本发明，并不用于限定本发明。
在一个实施例中，如图1所示，图1是本发明实施例提供的基于红外视觉、毫米波雷达和IMU融合的定位与建图方法的流程示意图之一，本实施例中基于红外视觉、毫米波雷达和IMU融合的定位与建图方法包括以下步骤：
S101、将红外图像进行循环处理，同时将循环处理输出的红外相机的位姿数据与惯导器件的位姿数据融合，优化解算出自我平台的第一里程计信息，循环处理包括图像对齐、点初始化、点位优化、地标初始化和剪枝处理。
红外视觉提供独特的热特征感知，能够在光照较差的情况下提供可靠的环境感知。惯导器件(IMU)提供快速的姿态和运动状态估计以及快速响应和姿态校正能力。
其中，自我平台可以是但不限于无人机和无人车。
S102、将4D毫米波雷达获得的4D点云数据通过点云预处理算法进行去噪获得稳定的4D点云数据。
具体的，4D毫米波雷达获得的4D点云数据含有噪点点云，通过点云预处理算法可以将噪点点云进行去除。需要说明的是，点云预处理算法为本领域人员熟知，在此不再赘述。
S103、根据稳定的4D点云数据解算出自我平台的三维体速度。自我平台的三维体速度基于单次雷达扫描，每次扫描由雷达帧中的3D点和从多普勒频移获
得的相应速度组成。
自我平台的三维体速度的估计可以基于最小平方解进行
其中，给定目标位置pr，通过归一化得到方向rr，测得的多普勒速度是方向rr与雷达速度vr的标量积：
如图2所示，图2为本发明实施例中提供的解算出自我平台的三维体速度方法的流程示意图，根据稳定的4D点云数据解算出自我平台的三维体速度包括：
S201、通过稳定的4D点云数据的多维信息构建最小二乘求解模型。
具体的，最小二乘求解模型为
式中，是测得的多普勒速度，N为测量值总个数，r代表雷达坐标系，vr表示雷达速度。
对于雷达速度vr，雷达到机体系的转换需要给定雷达的外部校准结果(平移和旋转/>)，机身速度vb的测量方程为：
使用IMU测量的角速度wb和估计的陀螺偏置bg，则机体速度测量值建模为：
是高斯白噪声过程。实际中对滤波器更新的测量噪声使用基于ODR方法估计协方差。机体速度是滤波器状态的一部分，则线性化后可产生测量雅可比Hr，然后执行EKF的更新。
S202、利用线性最小二乘求解方法对最小二乘求解模型进行求解获得自我平台的三维体速度。
利用线性最小二乘(LSQ)解可得到如下解：
式中，为求得的平台三维速度，H矩阵为最小二乘求解模型中含rx,N、ry,N、rz,N的矩阵，yr为测得的多普勒速度/>
用所有的测量方法求解此最小二乘问题都容易出错。环境不能假设是静态的，所以噪声、反射或重影而产生的异常值必须被删除，前述的点云预处理是为了解决此问题。
S104、融合稳定的4D点云数据以及惯导器件的位姿数据，优化解算出自我平台的第二里程计信息。
S105、融合自我平台的第一里程计信息和第二里程计信息和三维体速度，优化解算出最优里程计信息，同时根据稳定的4D点云数据和最优里程计信息绘制出环境点云地图。具体的，稳定的4D点云数据和最优里程计信息进行投影和拼接绘制出环境点云地图。
本发明的基于红外视觉、毫米波雷达和IMU融合的定位与建图方法，通过融合红外视觉、毫米波雷达和IMU的数据，具有更全面、多样化的环境感知能力，且能在光照不足、恶劣天气和动态环境等挑战性条件下表现出良好的鲁棒性。
在一个实施例中，图像对齐处理为通过构建图像金字塔模型，利用IMU的姿态数据作为信息先验，通过直接法执行红外图像的配准，根据配准结果计算出自我平台的位姿信息。
如图3所示，图3是本发明实施例提供的图像金字塔模型结构示意图，为了对齐并建立两幅红外图像之间的对应关系，将它们之间的辐射误差降至最低，这种图像对齐处理属于直接法的类别，但基于光度误差最小化的直接视觉里程计方法需要对大量点进行优化，因此计算成本很高，但本实施例中跟踪一组稀疏的点来执行图像对齐。给定参考图像中的一组点，将它们投影到传入图像中，如下所示：
p′＝K(RK-1(p,d-1)+t) (10)
式中，p和p′在分别是以像素为单位的原始和新投影点位置，K是固有相机矩阵，R和t分别是帧之间的旋转矩阵和平移向量，d是点的估计深度。由于R和t是未知的，通过最小化点之间的辐射测量误差来估计它们。如果一个点的深度从其先前与3D地标的关联中已知，则使用它，否则仅按比例估计t。为了使辐射测量误差最小化过程更加鲁棒，在每个点周围的一个小邻域内计算误差：
式中，eradio是在每个点的邻域Ni上计算的一组跟踪点的辐射测量误差的平方和，T代表14位分辨率中每个点的热值。然后进行加权高斯牛顿优化，以估计两幅图像之间的变换参数。然而，对大量点执行优化在计算非常耗时。但本实施例中以自上而下的方式在图像金字塔上执行对齐操作，其中两个图像首先进行粗对齐，并且它们的变换估计成为下一级别的对齐先验。这样可以以非常少的迭代中收敛于图像金字塔的最低级别。此外，本实施例中使用IMU提供的转换先验并在最低的级别开始对齐，一旦估计了帧之间的变换参数，参考图像中的点就被投影到新图像中，并且被认为是匹配点。然后将这些匹配点用于下一图像的图像对准。
在一个可选的实施例中，点初始化处理为将红外图像划分为32×32个像素块，在像素块中通过设定阈值筛选出图像关键帧。
具体的，为了选择跟踪点，首先计算整个图像上的图像梯度。然后，将图像划分为32×32个像素块，并计算每个块中的中值梯度(gmed)。将恒定偏移(goff)添加到每个块的gmed，并且块中低于该gmed+goff值的所有梯度值抑制。为了确保在场景中选择良好的点分布，首先在每个块中拾取具有最大非抑制梯度的点，并将它们插入与图像大小相等的2D点选择网格中。这种2D点选择网格能快速检查下一个候选点是否与之前选择的点有足够的距离，而无需明确计算距离度量。这样可以确保所选择的点具有足够的梯度值，并且在图像上分布良好。以这种方式在图像尺度-空间上选择点，使得在给定级别上选择的点的数量是在其以下级别上选择点的数量的一半。只有当跟踪点的数量低于特定设定阈值时，才会在图像中初始化新点。在这种情况下，在插入新点之前，首先将成功跟踪的点插入2D网格中。每次初始化新点时都会将该图像设置为关键帧。
在其中一个实施例中，点位优化处理为通过细化像素块位置计算残差和比率消除不受约束的点。
一旦参考图和新图像对齐，并确定了新图像中点的像素位置，就可以进一步细化这些像素位置，以了解点在其邻域中的定位效果。通过在以点为中心的搜索窗口中计算点沿其运动矢量的辐射残差来计算点的定位质量，如图3所示。然后计算搜索窗口中最低残差与第二最低残差的比率，如果该比率低于阈值，则拒绝该点。此步骤确保受到充分约束的点能够细化，同时消除由于对齐错误而从角或边位置分离到均匀平面表面上并在其位置变得不受约束的点。
在其中一个实施例中，地标初始化和剪枝处理为根据红外图像中设定的对应关系点确定初始化3D地标，同时根据红外图像位置和质量优化剪枝算法对深度不一致的地标进行剪枝。
一旦建立了图像之间的点对应关系，就将它们初始化为3D地标。使用OpenGV库对点进行三角测量，并检查从点发出的光线是否不平行。如果可以成功地对一个点进行三角测量，则会为其分配一个地标ID。只有在时间窗口中当前帧和最新帧之间的图像对齐成功时，才会初始化新的地标。然而，对于当前帧与时间窗口中最旧帧和关键帧的每次成功对齐，都会在已经初始化的地标上添加新的观测值。如果在两帧之间可以观察到足够多的地标，则通过使用地标的深度估计来检查地标的质量，将其像素位置从一帧反向投影到另一帧，并将反向投影的像素位置与图像对齐和点细化过程后获得的像素位置进行比较。深度不一致的地标在其像素位置上有更大的误差，并且可以被剪枝。如果观测次数最少，则将剩余的地标添加到优化后端，用于里程计估计。
在一个具体的实施例中，如图4所示，图4是本发明实施例提供的红外-惯性里程计优化方法框架示意图之一，红外-惯性里程计优化方法用于融合优化解算出第一里程计信息，本实施例中，将热成像相机的直接辐射数据与惯性测量数据融合，以扩展无人机在GPS受限和视觉恶劣环境中的导航能力，包括在黑暗条件下和存在尘土、雾和烟雾等空中遮挡物的情况下，使用了一种基于优化的方法，将3D地标的重投影误差和惯性测量误差联合最小化，此外还使用了一种基于关键帧的里程计估计方法，该方法使用单眼LWIR热像仪的直接14位辐射数据来建立连续图像之间的对应关系。直接处理14位辐射数据使本实施例的方法能够克服与图像重新缩放相关的许多问题，在不依赖为视觉系统设计的特征检测和描述方法的情况下进行操作，并在不同的热条件下保持对各种环境的可推广性。类似地，由于其基于优化的估计性质，本实施例的方法能够保持对数据中断的鲁棒性。此外，本实施例集成了惯性测量单元(IMU)的测量结果，为本实施例的热惯性里程计估计方法制定了联合成本函数。集成惯性测量单元的原因有三方面。首先，IMU测量为图像对准过程提供了变换先验。其次，它们提供了对两个旋转状态的直接观测，将未观测到的姿态自由度从6个减少到4个。第三，在单目视觉的情况下，它们提供了更好的尺度估计。
在一个优选的实施例中，如图5所示，图5是本发明实施例提供的基于红外视觉、毫米波雷达和IMU融合的定位与建图方法的流程示意图之一，本实施例涉及的是如何将循环处理输出的红外相机的位姿数据与惯导器件的位姿数据融合，优化解算出自我平台的第一里程计信息，在上述实施例的基础上，将循环处理输出的红外相机的位姿数据与惯导器件的位姿数据融合，优化解算出自我平台的第一里程计信息包括：
S501、根据惯导器件的测量值和循环处理输出的红外相机的位姿数据建立重投影误差模型。
给定一组3D地标和图像帧之间的惯性测量值，通过解决非线性优化问题来估计机器人的姿态，该问题最大限度地减少了观测到的地标的重投影误差，同时符合惯性约束。地标的重投影误差可以写成：
式中，pc是相机坐标系中的点坐标，是世界坐标系中对应的3D地标位置，/>是从IMU帧中IMU到相机坐标系的变换，/>是IMU在世界坐标系中的变换，K是固有相机矩阵。
S502、利用重投影误差模型构建非线性优化问题。
为了描述惯性测量误差模型，首先定义了IMU状态方程：
x＝[r q v ba bω]′ (12)
其中r和q分别是IMU在W中的位置和方向，v是IMU的速度，ba和bω是加速度计和陀螺仪的估计偏差。状态的微分方程定义为
其中是旋转矩阵IMU到世界坐标系的变换、×表示向量的斜对称矩阵，g是重力向量，/>和/>表示偏置校正的适当加速度和角速度，并给出如下：
其中和/>是未补偿的IMU测量值。因此，两个帧之间惯性测量的误差项可以写成：
其中，xk表示在帧k时间处的IMU状态，并且表示在帧k+1时间的IMU的预测状态。使用前向欧拉积分方案来计算预测的IMU状态。给定重投影和惯性测量误差方程，非线性优化问题即联合热惯性问题的成本函数可以为
式中，k表示在包含时间帧和关键帧的滑动窗口中正在处理的帧，L(k)表示在帧k中可观察到的一组地标，表示在帧k中可见的每个地标l的重投影误差的叠加向量，表示帧k中地标测量的协方差矩阵，/>表示IMU测量的协方差矩阵。
S503、根据非线性优化问题求解出自我平台的里程计信息。
具体的，使用Ceres优化方法框架将成本函数最小化，并产生自我平台的里程计信息。
在其中一个实施例中，融合稳定的4D点云数据以及惯导器件的位姿数据以及融合第一里程计信息和第二里程计信息和三维体速度均采用图优化模型以滑动窗口方式进行融合。具体的，如图6所示，图6为本发明实施例中提供的雷达、IMU融合里程计结构示意图，可以将稳定的4D点云数据、惯导器件的位姿数据以及第一里程计信息和第二里程计信息和三维体速度置于一个多传感器融合SLAM框架中以滑动窗口方式进行融合。
在融合过程中，时间t时滑动窗口中包含的状态定义为其中在时间t内滑动窗口中活动IMU状态。Tt表示t处的IMU量测集合。
IMU状态为
式中，是一个单位四元数，表示从世界帧{w}到IMU帧{I}的旋转。/>和/>分别是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的鲁棒性。
基于同样的发明创造，本发明还提供了一种基于红外视觉、毫米波雷达和IMU融合的定位与建图系统，系统包括：
循环解算模块，用于将红外图像进行循环处理，同时将循环处理输出的红外相机的位姿数据与惯导器件的位姿数据融合，优化解算出自我平台的第一里程计信息，循环处理包括图像对齐、点初始化、点位优化、地标初始化和剪枝处理。
点云预处理模块，用于将4D毫米波雷达获得的4D点云数据通过点云预处理算法进行去噪获得稳定的4D点云数据。点云预处理模块输入是ros点云数据类型，来自雷达点云驱动，输出是ros点云数据类型，发送给NDT点云匹配模块和雷达自运动估计模块。
雷达自运动估计模块，用于根据稳定的4D点云数据解算出自我平台的三维体速度。雷达自运动估计模块是通过4D毫米波雷达点云中的多普勒速度信息，计算雷达自身运动速度。输入是经过点云预处理模块处理后发送的ros点云数据，输出是雷达当前的瞬时速度。其中速度结果发送给gtsam。
位姿解算模块，用于融合稳定的4D点云数据以及惯导器件的位姿数据，优化解算出自我平台的第二里程计信息。输入是经过点云预处理模块处理后发送的ros点云数据，输出是雷达当前六自由度位姿和投影到世界坐标系下的全局点云地图。其中位姿结果发送给gtsam。
基于图优化的多传感器融合SLAM模块，用于融合第一里程计信息和第二里程计信息和三维体速度，优化解算出最优里程计信息，同时根据稳定的4D点云数据和最优里程计信息绘制出环境点云地图。
本实施例的基于红外视觉、毫米波雷达和IMU融合的定位与建图系统，能够在实时性要求下运行，相对于传统SLAM系统，计算资源需求相对较低。另外，该系统对周围环境的感知能力强，适应性、精度和稳定性高。
以上所述实施例仅表达了本发明的几种实施方式，其描述较为具体和详细，但并不能因此而理解为对本发明专利范围的限制。应当指出的是，对于本领域的普通技术人员来说，在不脱离本发明构思的前提下，还可以做出若干变形和改进，这些都属于本发明的保护范围。因此，本发明的保护范围应以所附权利要求为准。
Claims (10)
1.一种基于红外视觉、毫米波雷达和IMU融合的定位与建图方法，其特征在于，包括以下步骤：
将红外图像进行循环处理，同时将循环处理输出的红外相机的位姿数据与惯导器件的位姿数据融合，优化解算出自我平台的第一里程计信息，所述循环处理包括图像对齐、点初始化、点位优化、地标初始化和剪枝处理；
将4D毫米波雷达获得的4D点云数据通过点云预处理算法进行去噪获得稳定的4D点云数据；
根据所述稳定的4D点云数据解算出自我平台的三维体速度；
融合所述稳定的4D点云数据以及惯导器件的位姿数据，优化解算出自我平台的第二里程计信息；
融合所述自我平台的第一里程计信息和第二里程计信息和三维体速度，优化解算出最优里程计信息，同时根据稳定的4D点云数据和最优里程计信息绘制出环境点云地图。
2.根据权利要求1所述的基于红外视觉、毫米波雷达和IMU融合的定位与建图方法，其特征在于，图像对齐处理为通过构建图像金字塔模型，利用IMU的姿态数据作为信息先验，通过直接法执行红外图像的配准，根据配准结果计算出自我平台的位姿信息。
3.根据权利要求1所述的基于红外视觉、毫米波雷达和IMU融合的定位与建图方法，其特征在于，点初始化处理为将红外图像划分为32×32个像素块，在所述像素块中通过设定阈值筛选出图像关键帧。
4.根据权利要求1所述的基于红外视觉、毫米波雷达和IMU融合的定位与建图方法，其特征在于，点位优化处理为通过细化像素块位置计算残差和比率消除不受约束的点。
5.根据权利要求1所述的基于红外视觉、毫米波雷达和IMU融合的定位与建图方法，其特征在于，地标初始化和剪枝处理为根据红外图像中设定的对应关系点确定初始化3D地标，同时根据红外图像位置和质量优化剪枝算法对深度不一致的地标进行剪枝。
6.根据权利要求2至5任意一项所述的基于红外视觉、毫米波雷达和IMU融合的定位与建图方法，其特征在于，将循环处理输出的红外相机的位姿数据与惯导器件的位姿数据融合，优化解算出自我平台的第一里程计信息包括：
根据惯导器件的测量值和循环处理输出的红外相机的位姿数据建立重投影误差模型；
利用重投影误差模型构建非线性优化问题；
根据非线性优化问题求解出自我平台的里程计信息。
7.根据权利要求6所述的基于红外视觉、毫米波雷达和IMU融合的定位与建图方法，其特征在于，融合所述稳定的4D点云数据以及惯导器件的位姿数据以及融合所述第一里程计信息和第二里程计信息和三维体速度均采用图优化模型以滑动窗口方式进行融合。
8.根据权利要求7所述的基于红外视觉、毫米波雷达和IMU融合的定位与建图方法，其特征在于，所述重投影误差模型为
式中，是相机坐标系中的点坐标，/>是世界坐标系中对应的3D地标位置，/>是从IMU帧中IMU到相机坐标系的变换，/>是IMU在世界坐标系中的变换，K是固有相机矩阵。
9.根据权利要求8所述的基于红外视觉、毫米波雷达和IMU融合的定位与建图方法，其特征在于，非线性优化问题的成本函数为
式中，k表示在包含时间帧和关键帧的滑动窗口中正在处理的帧，L(k)表示在帧k中可观察到的一组地标，表示在帧k中可见的每个地标l的重投影误差的叠加向量，/>表示帧k中地标测量的协方差矩阵，/>表示IMU测量的协方差矩阵。
10.一种基于红外视觉、毫米波雷达和IMU融合的定位与建图系统，其特征在于，所述系统包括：
循环解算模块，用于将红外图像进行循环处理，同时将循环处理输出的红外相机的位姿数据与惯导器件的位姿数据融合，优化解算出自我平台的第一里程计信息，所述循环处理包括图像对齐、点初始化、点位优化、地标初始化和剪枝处理；
点云预处理模块，用于将4D毫米波雷达获得的4D点云数据通过点云预处理算法进行去噪获得稳定的4D点云数据；
雷达自运动估计模块，用于根据所述稳定的4D点云数据解算出自我平台的三维体速度；
位姿解算模块，用于融合所述稳定的4D点云数据以及惯导器件的位姿数据，优化解算出自我平台的第二里程计信息；
基于图优化的多传感器融合SLAM模块，用于融合所述第一里程计信息和第二里程计信息和三维体速度，优化解算出最优里程计信息，同时根据稳定的4D点云数据和最优里程计信息绘制出环境点云地图。
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202311083869.2A CN117330052A (zh) | 2023-08-25 | 2023-08-25 | 基于红外视觉、毫米波雷达和imu融合的定位与建图方法及系统 |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202311083869.2A CN117330052A (zh) | 2023-08-25 | 2023-08-25 | 基于红外视觉、毫米波雷达和imu融合的定位与建图方法及系统 |
Publications (1)
| Publication Number | Publication Date |
|---|---|
| CN117330052A true CN117330052A (zh) | 2024-01-02 |
Family
ID=89276175
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202311083869.2A Pending CN117330052A (zh) | 2023-08-25 | 2023-08-25 | 基于红外视觉、毫米波雷达和imu融合的定位与建图方法及系统 |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN117330052A (zh) |
-
2023
- 2023-08-25 CN CN202311083869.2A patent/CN117330052A/zh active Pending
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN111156998B (zh) | 一种基于rgb-d相机与imu信息融合的移动机器人定位方法 | |
| CN109887057B (zh) | 生成高精度地图的方法和装置 | |
| US10515458B1 (en) | Image-matching navigation method and apparatus for aerial vehicles | |
| Li et al. | NRLI-UAV: Non-rigid registration of sequential raw laser scans and images for low-cost UAV LiDAR point cloud quality improvement | |
| Panahandeh et al. | Vision-aided inertial navigation based on ground plane feature detection | |
| Doer et al. | An ekf based approach to radar inertial odometry | |
| US8295547B1 (en) | Model-based feature tracking in 3-D and 2-D imagery | |
| Deng et al. | Global optical flow-based estimation of velocity for multicopters using monocular vision in GPS-denied environments | |
| Ramezani et al. | Pose estimation by omnidirectional visual-inertial odometry | |
| Ramezani et al. | Omnidirectional visual-inertial odometry using multi-state constraint Kalman filter | |
| Veth et al. | Stochastic constraints for efficient image correspondence search | |
| Alliez et al. | Real-time multi-SLAM system for agent localization and 3D mapping in dynamic scenarios | |
| Li et al. | Aerial-triangulation aided boresight calibration for a low-cost UAV-LiDAR system | |
| Xian et al. | Fusing stereo camera and low-cost inertial measurement unit for autonomous navigation in a tightly-coupled approach | |
| Wu et al. | AFLI-Calib: Robust LiDAR-IMU extrinsic self-calibration based on adaptive frame length LiDAR odometry | |
| CN116184430B (zh) | 一种激光雷达、可见光相机、惯性测量单元融合的位姿估计算法 | |
| Qayyum et al. | Inertial-kinect fusion for outdoor 3d navigation | |
| Aminzadeh et al. | Implementation and performance evaluation of optical flow navigation system under specific conditions for a flying robot | |
| Klavins et al. | Unmanned aerial vehicle movement trajectory detection in open environment | |
| CN115930948A (zh) | 一种果园机器人融合定位方法 | |
| CN115574816A (zh) | 仿生视觉多源信息智能感知无人平台 | |
| CN114842224A (zh) | 一种基于地理底图的单目无人机绝对视觉匹配定位方案 | |
| CN117330052A (zh) | 基于红外视觉、毫米波雷达和imu融合的定位与建图方法及系统 | |
| Li-Chee-Ming et al. | Augmenting visp’s 3d model-based tracker with rgb-d slam for 3d pose estimation in indoor environments | |
| Hu et al. | Efficient Visual-Inertial navigation with point-plane map |
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 |