Positioning and mapping method and system based on fusion of infrared vision, millimeter wave radar and IMU

Abstract

This invention relates to a positioning and mapping method and system based on the fusion of infrared vision, millimeter-wave radar, and IMU. First, infrared images are processed cyclically, and the pose data from the infrared camera output from this cyclic processing is fused with the pose data from the inertial navigation system (INS) to optimize and calculate the first odometry information of the self-platform. Then, 4D point cloud data obtained from 4D millimeter-wave radar is denoised using a point cloud preprocessing algorithm to obtain stable 4D point cloud data. The three-dimensional volume velocity of the self-platform is then calculated based on the stable 4D point cloud data. Next, the stable 4D point cloud data and the pose data from the INS are fused to optimize and calculate the second odometry information of the self-platform. Finally, the first and second odometry information are fused with the three-dimensional volume velocity to optimize and calculate the optimal odometry information. Simultaneously, an environmental point cloud map is drawn based on the stable 4D point cloud data and the optimal odometry information. This invention features more comprehensive environmental perception and better dynamic object processing capabilities.

Description

Technical Field

[0001] This invention relates to the field of autonomous navigation and environmental perception technology for unmanned systems, and in particular to a positioning and mapping method and system based on the fusion of infrared vision, millimeter-wave radar and IMU. Background Technology

[0002] SLAM, which stands for Simultaneous Localization and Mapping, is a technology that enables robots or autonomous vehicles to autonomously locate themselves and build maps of their surroundings in unknown environments. SLAM aims to use sensors on a mobile robot to simultaneously determine its own position and build a map of its environment, without requiring prior map information. This is crucial for applications such as autonomous navigation, environmental perception, and path planning.

[0003] In traditional SLAM systems, commonly used sensors include LiDAR, cameras, and IMUs. LiDAR can provide high-precision distance measurement, but it has limitations for surfaces with low reflectivity or objects with special materials. Cameras can acquire rich visual information, but perform poorly in low-light conditions or with weak textures. IMUs can provide high-frequency attitude and motion state measurements, but they suffer from drift issues. Summary of the Invention

[0004] Therefore, it is necessary to provide a localization and mapping method and system based on the fusion of infrared vision, millimeter-wave radar and IMU, which has more comprehensive environmental perception and better dynamic object processing capabilities, to address the above-mentioned technical problems.

[0005] In a first aspect, the present invention provides a localization and mapping method based on the fusion of infrared vision, millimeter-wave radar and IMU, comprising the following steps:

[0006] The infrared images are processed in a loop, and the pose data of the infrared camera output by the loop processing is fused with the pose data of the inertial navigation device to optimize and calculate the first odometry information of the self-platform. The loop processing includes image alignment, point initialization, point optimization, landmark initialization and pruning.

[0007] The 4D point cloud data obtained by 4D millimeter-wave radar is denoised using a point cloud preprocessing algorithm to obtain stable 4D point cloud data.

[0008] The three-dimensional velocity of the self-platform is calculated based on stable 4D point cloud data;

[0009] By integrating stable 4D point cloud data and pose data from inertial navigation devices, the second odometry information of the self-platform is optimized and calculated.

[0010] By integrating the first and second odometer information from the self-platform and the 3D volume velocity, the optimal odometer information is calculated and optimized. At the same time, an environmental point cloud map is drawn based on stable 4D point cloud data and the optimal odometer information.

[0011] In one embodiment, the image alignment process involves constructing an image pyramid model, using the pose data of the IMU as prior information, performing infrared image registration via a direct method, and calculating the pose information of the self-platform based on the registration result.

[0012] In one embodiment, the point initialization process involves dividing the infrared image into 32×32 pixel blocks and filtering out keyframes of the image within the pixel blocks by setting a threshold.

[0013] In one embodiment, the point optimization process involves calculating residuals and ratios by refining pixel block positions to eliminate unconstrained points.

[0014] In one embodiment, the landmark initialization and pruning process involves determining the initialization of 3D landmarks based on the corresponding points set in the infrared image, and simultaneously optimizing the pruning algorithm based on the position and quality of the infrared image to prune landmarks with inconsistent depths.

[0015] In one embodiment, the pose data from the infrared camera output by the cyclic processing is fused with the pose data from the inertial navigation device to optimize the calculation of the first odometry information of the self-platform, including:

[0016] A reprojection error model is established based on the measurements from the inertial navigation device and the pose data from the infrared camera output by the loop processing.

[0017] Construct a nonlinear optimization problem using a reprojection error model;

[0018] The odometer information of the self-platform was obtained by solving a nonlinear optimization problem.

[0019] In one embodiment, the fusion of stable 4D point cloud data, pose data from inertial navigation devices, first odometer information, second odometer information, and three-dimensional volume velocity is performed using a graph optimization model with a sliding window approach.

[0020] In one embodiment, the reprojection error model is:

[0021]

[0022] In the formula, These are the coordinates of a point in the camera coordinate system. It is the corresponding 3D landmark location in the world coordinate system. It is the transformation from the IMU to the camera coordinate system in the IMU frame. It is the transformation of the IMU in the world coordinate system, and K is the intrinsic camera matrix.

[0023] In one embodiment, the cost function of the nonlinear optimization problem is:

[0024]

[0025] In the formula, k represents the frame being processed within a sliding window that includes time frames and keyframes, and L(k) represents a set of landmarks observable in frame k. This represents the superposition vector of reprojection errors for each landmark l visible in frame k. This represents the covariance matrix of landmark measurements in frame k. This represents the covariance matrix of the IMU measurements.

[0026] Secondly, the present invention also provides a positioning and mapping system based on the fusion of infrared vision, millimeter-wave radar, and IMU, the system comprising:

[0027] The loop calculation module is used to process the infrared image in a loop, and fuse the pose data of the infrared camera output by the loop processing with the pose data of the inertial navigation device to optimize and calculate the first odometry information of the self-platform. The loop processing includes image alignment, point initialization, point optimization, landmark initialization and pruning.

[0028] The point cloud preprocessing module is used to denoise the 4D point cloud data obtained by the 4D millimeter-wave radar through the point cloud preprocessing algorithm to obtain stable 4D point cloud data.

[0029] The radar self-motion estimation module is used to calculate the three-dimensional volume velocity of the self-platform based on stable 4D point cloud data.

[0030] The pose calculation module is used to fuse stable 4D point cloud data and pose data from inertial navigation devices to optimize and calculate the second odometry information of the self-platform.

[0031] A graph-optimized multi-sensor fusion SLAM module is used to fuse first and second odometer information and 3D volume velocity to optimize and calculate the optimal odometer information. At the same time, an environmental point cloud map is drawn based on stable 4D point cloud data and the optimal odometer information.

[0032] The beneficial effects of this invention are:

[0033] (1) By fusing data from infrared vision, millimeter-wave radar, and IMU, this invention can obtain a more comprehensive and diverse environmental perception capability. Infrared vision provides unique thermal feature perception, millimeter-wave radar provides high-precision distance and velocity measurement, and IMU provides fast attitude and motion state estimation, thereby enhancing the system's perception capability of the surrounding environment.

[0034] (2) In this invention, infrared vision can provide reliable environmental perception in poor lighting conditions, millimeter-wave radar has strong penetration capability, and IMU provides fast response and attitude correction capability, which together improve the adaptability and stability of the system and can exhibit good robustness under challenging conditions such as insufficient lighting, bad weather and dynamic environment.

[0035] (3) In this invention, the data fusion of infrared vision and millimeter-wave radar can supplement the limitations of traditional visual SLAM systems. The attitude correction and state estimation of the IMU improve the accuracy and stability of positioning, and can provide more accurate and stable positioning results.

[0036] (4) In this invention, by fusing information from infrared vision and millimeter-wave radar, dynamic objects and static environments can be distinguished. The dynamic object processing capability is good, thereby avoiding attitude drift or map building errors. Attached Figure Description

[0037] Figure 1 This is one of the flowcharts of the localization and mapping method based on the fusion of infrared vision, millimeter-wave radar and IMU provided in the embodiments of the present invention;

[0038] Figure 2 This is a flowchart illustrating the method for calculating the three-dimensional volume velocity of a self-platform provided in an embodiment of the present invention.

[0039] Figure 3 This is a schematic diagram of the image pyramid model structure provided in an embodiment of the present invention;

[0040] Figure 4 This is one of the schematic diagrams of the infrared-inertial odometry optimization method framework provided in the embodiments of the present invention;

[0041] Figure 5 This is one of the flowcharts of the localization and mapping method based on the fusion of infrared vision, millimeter-wave radar and IMU provided in the embodiments of the present invention;

[0042] Figure 6 This is a schematic diagram of the radar and IMU fusion odometer structure provided in an embodiment of the present invention. Detailed Implementation

[0043] To make the objectives, technical solutions, and advantages of this invention clearer, the invention will be further described in detail below with reference to the accompanying drawings and embodiments. It should be understood that the specific embodiments described herein are merely illustrative and not intended to limit the invention.

[0044] In one embodiment, such as Figure 1 As shown, Figure 1This is one of the flowcharts illustrating the localization and mapping method based on the fusion of infrared vision, millimeter-wave radar, and IMU provided in this embodiment of the invention. The localization and mapping method based on the fusion of infrared vision, millimeter-wave radar, and IMU in this embodiment includes the following steps:

[0045] S101. The infrared image is processed in a loop, and the pose data of the infrared camera output by the loop processing is fused with the pose data of the inertial navigation device to optimize and calculate the first odometer information of the self-platform. The loop processing includes image alignment, point initialization, point optimization, landmark initialization and pruning processing.

[0046] Infrared vision provides unique thermal signature sensing, enabling reliable environmental awareness even in low-light conditions. Inertial navigation devices (IMUs) offer rapid attitude and motion state estimation, as well as fast response and attitude correction capabilities.

[0047] The self-platform can be, but is not limited to, drones and unmanned vehicles.

[0048] S102. The 4D point cloud data obtained by the 4D millimeter-wave radar is denoised using a point cloud preprocessing algorithm to obtain stable 4D point cloud data.

[0049] Specifically, the 4D point cloud data obtained by 4D millimeter-wave radar contains noisy point clouds, which can be removed using point cloud preprocessing algorithms. It should be noted that point cloud preprocessing algorithms are well-known to those skilled in the art and will not be elaborated upon here.

[0050] S103. Calculate the 3D volume velocity of the self-platform based on stable 4D point cloud data. The 3D volume velocity of the self-platform is based on a single radar scan, each scan consisting of 3D points in the radar frame and data obtained from the Doppler frequency shift.

[0051] The corresponding velocity composition is obtained.

[0052] The 3D volume velocity of the self-platform can be estimated based on the least squares solution.

[0053]

[0054] Wherein, given the target position p r The direction r is obtained through normalization. r The measured Doppler velocity It is the direction r r With radar speed v r scalar product:

[0055]

[0056] like Figure 2 As shown, Figure 2This is a flowchart illustrating the method for calculating the three-dimensional volume velocity of a self-platform provided in this embodiment of the invention. Calculating the three-dimensional volume velocity of the self-platform based on stable 4D point cloud data includes:

[0057] S201. Construct a least squares solution model using multidimensional information from stable 4D point cloud data.

[0058] Specifically, the least squares solution model is as follows:

[0059]

[0060] In the formula, This is the measured Doppler velocity, where N is the total number of measurements, r represents the radar coordinate system, and v... r Indicates radar speed.

[0061] For radar speed v r The conversion of radar to an electronic system requires the external calibration results (translation) of the radar. and rotation ), fuselage speed v b The measurement equation is:

[0062]

[0063] Angular velocity w measured using an IMU b and estimated gyroscope bias b g Then the measured value of the body speed The model is as follows:

[0064]

[0065] It is a Gaussian white noise process. In practice, the covariance of the measurement noise for filter updates is estimated using an ODR-based method. Since the body velocity is part of the filter state, linearization can produce the measurement Jacobian H. r Then perform the EKF update.

[0066]

[0067] S202. The three-dimensional velocity of the self-platform is obtained by solving the least squares model using the linear least squares solution method.

[0068] Using the linear least squares (LSQ) solution, we can obtain the following solution:

[0069]

[0070] In the formula, To obtain the three-dimensional velocity of the platform, the H matrix is ​​the least squares solution model containing r. x,N ry,N r z,N The matrix, y r For the measured Doppler velocity

[0071] Solving this least-squares problem using any measurement method is prone to errors. The environment cannot be assumed to be static, so outliers caused by noise, reflections, or ghosting must be removed; the aforementioned point cloud preprocessing is to address this issue.

[0072] S104 integrates stable 4D point cloud data and pose data from inertial navigation devices to optimize and calculate the second odometer information of its own platform.

[0073] S105 integrates the first and second odometer information from the self-platform with the 3D volume velocity to optimize and calculate the optimal odometer information. Simultaneously, an environmental point cloud map is drawn based on stable 4D point cloud data and the optimal odometer information. Specifically, the stable 4D point cloud data and the optimal odometer information are projected and stitched together to create the environmental point cloud map.

[0074] The localization and mapping method based on the fusion of infrared vision, millimeter-wave radar and IMU of the present invention has a more comprehensive and diversified environmental perception capability by fusing data from infrared vision, millimeter-wave radar and IMU, and can exhibit good robustness under challenging conditions such as insufficient light, severe weather and dynamic environment.

[0075] In one embodiment, the image alignment process involves constructing an image pyramid model, using the IMU's pose data as prior information, performing infrared image registration via a direct method, and calculating the pose information of the self-platform based on the registration result.

[0076] like Figure 3 As shown, Figure 3 This is a schematic diagram of the image pyramid model structure provided in an embodiment of the present invention. To align and establish the correspondence between two infrared images and minimize their radiometric errors, this image alignment process belongs to the direct method category. However, the direct visual odometry method based on minimizing photometric errors requires optimization of a large number of points, thus incurring high computational costs. In this embodiment, however, image alignment is performed by tracking a sparse set of points. Given a set of points in a reference image, they are projected onto the input image, as shown below:

[0077] p′=K(RK -1 (p,d -1 )+t) (10)

[0078] In the formula, p and p′ are the original and new projected point positions in pixels, respectively, K is the intrinsic camera matrix, R and t are the rotation matrix and translation vector between frames, respectively, and d is the estimated depth of the point. Since R and t are unknown, they are estimated by minimizing the radiometric error between points. If the depth of a point is known from its previous association with a 3D landmark, it is used; otherwise, t is estimated only proportionally. To make the radiometric error minimization process more robust, the error is calculated in a small neighborhood around each point:

[0079]

[0080] In the formula, e radio It is the neighborhood N of each point i The sum of squared radiometric measurement errors of a set of tracking points is calculated, where T represents the thermal value of each point in 14-bit resolution. Weighted Gaussian-Newton optimization is then performed to estimate the transformation parameters between the two images. However, performing optimization on a large number of points is computationally very time-consuming. This embodiment, however, performs alignment on the image pyramid in a top-down manner, where the two images are first coarsely aligned, and their transformation estimates become the alignment priors for the next level. This allows convergence to the lowest level of the image pyramid in very few iterations. Furthermore, this embodiment uses the transformation prior provided by the IMU and starts alignment at the lowest level. Once the transformation parameters between frames are estimated, points in the reference image are projected into the new image and considered as matching points. These matching points are then used for image alignment in the next image.

[0081] In an optional embodiment, the point initialization process involves dividing the infrared image into 32×32 pixel blocks and filtering out keyframes of the image within the pixel blocks by setting a threshold.

[0082] Specifically, to select tracking points, the image gradient across the entire image is first calculated. Then, the image is divided into 32×32 pixel blocks, and the median gradient (g) in each block is calculated. med ). Constant offset (g) off Add to g in each block med And the block is lower than that g med +g offAll gradient values ​​are suppressed. To ensure a good distribution of points in the scene, points with the largest unsuppressed gradients are first picked in each block and inserted into a 2D point selection grid of the same size as the image. This 2D point selection grid quickly checks whether the next candidate point is sufficiently far from the previously selected points without explicitly calculating the distance metric. This ensures that the selected points have sufficient gradient values ​​and are well distributed across the image. Points are selected in this image scale-space manner such that the number of points selected at a given level is half the number selected at the levels below it. New points are only initialized in the image when the number of tracked points falls below a certain set threshold. In this case, successfully tracked points are first inserted into the 2D grid before inserting new points. The image is set as a keyframe each time new points are initialized.

[0083] In one embodiment, the point optimization process involves calculating residuals and ratios by refining pixel block positions to eliminate unconstrained points.

[0084] Once the reference image and the new image are aligned, and the pixel positions of points in the new image are determined, these pixel positions can be further refined to understand the localization quality of the points within their neighborhoods. The localization quality of a point is calculated by computeding the radiative residual of the point along its motion vector within a search window centered on the point. Figure 3 As shown. Then, the ratio of the lowest residual to the second lowest residual in the search window is calculated. If this ratio is lower than a threshold, the point is rejected. This step ensures that sufficiently constrained points are refined while eliminating points that have separated from their corner or edge positions onto a uniform plane surface due to alignment errors and become unconstrained in their positions.

[0085] In one embodiment, the landmark initialization and pruning process involves determining the initialization of 3D landmarks based on the corresponding points set in the infrared image, and simultaneously optimizing the pruning algorithm based on the position and quality of the infrared image to prune landmarks with inconsistent depths.

[0086] Once the point correspondences between images are established, they are initialized as 3D landmarks. Triangulation of the points is performed using the OpenGV library, and the parallelism of rays emanating from the points is checked. If a point can be successfully triangulated, it is assigned a landmark ID. New landmarks are initialized only when image alignment between the current frame and the latest frame within the time window is successful. However, for each successful alignment of the current frame with the oldest and keyframes within the time window, new observations are added to the already initialized landmarks. If enough landmarks can be observed between two frames, the quality of the landmarks is checked by backprojecting their pixel positions from one frame to another using depth estimation, and comparing the backprojected pixel positions with those obtained after image alignment and point refinement. Landmarks with inconsistent depth have larger errors in their pixel positions and can be pruned. If the number of observations is minimal, the remaining landmarks are added to the optimization backend for odometry estimation.

[0087] In a specific embodiment, such as Figure 4 As shown, Figure 4 This is one of the schematic diagrams of the infrared-inertial odometry optimization method framework provided in this embodiment of the invention. The infrared-inertial odometry optimization method is used to fuse and optimize the calculation of the first odometry information. In this embodiment, direct radiometric data from a thermal imaging camera is fused with inertial measurement data to extend the navigation capabilities of UAVs in GPS-limited and visually challenging environments, including in dark conditions and in the presence of aerial obstructions such as dust, fog, and smoke. An optimization-based method is used to jointly minimize the reprojection error of 3D landmarks and the inertial measurement error. In addition, a keyframe-based odometry estimation method is used, which uses direct 14-bit radiometric data from a monocular LWIR thermal imager to establish the correspondence between consecutive images. Direct processing of 14-bit radiometric data enables the method of this embodiment to overcome many problems related to image rescaling, operate without relying on feature detection and description methods designed for vision systems, and maintain generalizability to various environments under different thermal conditions. Similarly, due to its optimization-based estimation nature, the method of this embodiment can maintain robustness to data interruptions. Furthermore, this embodiment integrates the measurement results of the inertial measurement unit (IMU) to establish a joint cost function for the thermal odometry estimation method of this embodiment. There are three reasons for integrating the IMU. First, IMU measurements provide a transformed prior for the image alignment process. Second, they provide direct observation of the two rotational states, reducing the unobserved attitude degrees of freedom from six to four. Third, in the case of monocular vision, they provide better scale estimation.

[0088] In a preferred embodiment, such as Figure 5 As shown, Figure 5This is one of the flowcharts illustrating a localization and mapping method based on the fusion of infrared vision, millimeter-wave radar, and IMU provided in this embodiment of the invention. This embodiment relates to how to fuse the pose data of the infrared camera output from the cyclic processing with the pose data of the inertial navigation device to optimize and calculate the first odometry information of the self-platform. Based on the above embodiment, fusing the pose data of the infrared camera output from the cyclic processing with the pose data of the inertial navigation device to optimize and calculate the first odometry information of the self-platform includes:

[0089] S501. Establish a reprojection error model based on the measured values ​​of the inertial navigation device and the pose data of the infrared camera output by the cyclic processing.

[0090] Given a set of inertial measurements between 3D landmarks and image frames, the robot's pose is estimated by solving a nonlinear optimization problem that minimizes the reprojection error of the observed landmarks while satisfying inertial constraints. The reprojection error of the landmarks can be written as:

[0091]

[0092] In the formula, p c These are the coordinates of a point in the camera coordinate system. It is the corresponding 3D landmark location in the world coordinate system. It is the transformation from the IMU to the camera coordinate system in the IMU frame. It is the transformation of the IMU in the world coordinate system, and K is the intrinsic camera matrix.

[0093] S502. Construct a nonlinear optimization problem using the reprojection error model.

[0094] To describe the inertial measurement error model, the IMU state equations are first defined:

[0095] x=[rqvb a b ω ]′ (12)

[0096] Where r and q are the position and orientation of the IMU in W, respectively, v is the velocity of the IMU, and b is the velocity of the IMU. a and b ω This refers to the estimation bias from the accelerometer and gyroscope. The differential equation for the state is defined as follows:

[0097]

[0098]

[0099]

[0100]

[0101]

[0102] in It represents the transformation from the IMU to the world coordinate system by the rotation matrix, × represents the skew-symmetric matrix of the vector, and g is the gravity vector. and The appropriate acceleration and angular velocity for bias correction are given as follows:

[0103]

[0104]

[0105] in and These are uncompensated IMU measurements. Therefore, the error term for inertial measurements between two frames can be written as:

[0106]

[0107] Where, x k This represents the IMU state at time k in frame k, and This represents the predicted state of the IMU at time k+1. The predicted IMU state is calculated using a forward Euler integration scheme. Given the reprojection and inertial measurement error equations, the cost function of the nonlinear optimization problem, i.e., the joint thermal inertial problem, can be:

[0108]

[0109] In the formula, k represents the frame being processed within a sliding window that includes time frames and keyframes, and L(k) represents a set of landmarks observable in frame k. This represents the superposition vector of reprojection errors for each landmark l visible in frame k. This represents the covariance matrix of landmark measurements in frame k. This represents the covariance matrix of the IMU measurements.

[0110] S503. Solve the odometer information of the self-platform based on the nonlinear optimization problem.

[0111] Specifically, the Ceres optimization framework is used to minimize the cost function and generate odometry information for the self-platform.

[0112] In one embodiment, the fusion of stable 4D point cloud data, pose data from inertial navigation devices, and the fusion of first and second odometry information and three-dimensional volume velocity are all performed using a graph optimization model in a sliding window manner. Specifically, as shown... Figure 6 As shown, Figure 6The diagram below shows the radar and IMU fusion odometry structure provided in this embodiment of the invention. Stable 4D point cloud data, inertial navigation device pose data, first odometry information, second odometry information, and three-dimensional volume velocity can be fused in a multi-sensor fusion SLAM framework using a sliding window method.

[0113] During the fusion process, the states contained in the sliding window at time t are defined as follows: in The active IMU state within a sliding window over time t. t Let t represent the set of IMU measurements at point t.

[0114] IMU status is

[0115]

[0116] In the formula, It is a unit quaternion representing the rotation from world frame {w} to IMU frame {I}. and These are the IMU position and velocity, b g b a These are the random walk biases of the gyroscope and accelerometer, respectively.

[0117] Using the definition of state, the goal is to minimize the cost function of residuals generated by different measurements in the above equation.

[0118]

[0119] The first term is the residual based on the IMU, r I,m Define the measurement residual between frames m and m+1. The second term is the odometer measurement residual, and the last term is the velocity residual. t V and Σ are a set of measurements within a sliding window at time t. i Σj and Σj are the covariances of the two measurements, respectively.

[0120] The optimized solution after fusion is typically solved using an iterative least squares solver with linear approximation. In this embodiment, the solution is specifically, but not limited to, based on GTSAM.

[0121] In this embodiment, the factor modeling results of the graph optimization model include the IMU pre-integration factor, the odometer factor, and the velocity prior factor.

[0122] Preferably, the IMU pre-integration factor is

[0123]

[0124] In the formula, the operator vec(·) is used to extract the vector part of the quaternion, and m represents the m-th frame. Represents a rotation matrix. b represents the system's position, velocity, and attitude states, respectively. a,m b g,m This represents the bias of the accelerometer and gyroscope. Represents the pre-integral quantity, g w Represents the gravitational parameter, Δτ m The parameters are Gaussian white noise.

[0125] Specifically, the IMU pre-integration factor r I,m This includes the error term containing relative motion constraints between radar keyframes. By using IMU pre-integration, based on the known IMU state variables from the previous moment, the linear acceleration and angular velocity measured by the IMU are integrated to obtain the current state variables. Ultimately, this enables the acquisition of odometry information at the IMU frequency from the IMU data, based on the radar pose obtained through point cloud matching.

[0126] Preferably, the odometer factor is

[0127]

[0128] In the formula, The quaternion multiplication operator is vec(·), which extracts the vector part of a quaternion. Let Δp represent the inverse pose and the predicted pose of frame m, respectively. m and These represent the change in location and the predicted change in location, respectively.

[0129] Preferably, the speed prior factor is

[0130]

[0131] In the formula, v radar It is a priori velocity estimate. It's an estimated speed.

[0132] Specifically, the velocity prior factor restricts the estimation of robot velocity to improve the robustness of SLAM.

[0133] Based on the same invention, this invention also provides a positioning and mapping system based on the fusion of infrared vision, millimeter-wave radar, and IMU, the system comprising:

[0134] The loop calculation module is used to process infrared images in a loop, and simultaneously fuse the pose data of the infrared camera output by the loop processing with the pose data of the inertial navigation device to optimize and calculate the first odometry information of the self-platform. The loop processing includes image alignment, point initialization, point optimization, landmark initialization and pruning.

[0135] The point cloud preprocessing module is used to denoise the 4D point cloud data obtained by the 4D millimeter-wave radar using a point cloud preprocessing algorithm to obtain stable 4D point cloud data. The input of the point cloud preprocessing module is a ROS point cloud data type from the radar point cloud driver, and the output is a ROS point cloud data type, which is sent to the NDT point cloud matching module and the radar self-motion estimation module.

[0136] The radar self-motion estimation module is used to calculate the 3D volumetric velocity of the self-platform based on stable 4D point cloud data. This module calculates the radar's own velocity using Doppler velocity information from the 4D millimeter-wave radar point cloud. The input is ROS point cloud data processed by the point cloud preprocessing module, and the output is the radar's current instantaneous velocity. The velocity result is then sent to GTSMAM.

[0137] The pose calculation module fuses stable 4D point cloud data and pose data from inertial navigation devices to optimize and calculate the platform's second odometry information. The input is ROS point cloud data processed by the point cloud preprocessing module, and the output is the radar's current six-DOF pose and a global point cloud map projected onto the world coordinate system. The pose result is then sent to GTSMAM.

[0138] A graph-optimized multi-sensor fusion SLAM module is used to fuse first and second odometer information and 3D volume velocity to optimize and calculate the optimal odometer information. At the same time, an environmental point cloud map is drawn based on stable 4D point cloud data and the optimal odometer information.

[0139] The localization and mapping system based on the fusion of infrared vision, millimeter-wave radar, and IMU in this embodiment can operate under real-time requirements and has relatively lower computational resource requirements compared to traditional SLAM systems. Furthermore, this system has strong environmental perception capabilities and exhibits high adaptability, accuracy, and stability.

[0140] The embodiments described above are merely illustrative of several implementations of the present invention, and while the descriptions are specific and detailed, they should not be construed as limiting the scope of the present invention. It should be noted that those skilled in the art can make various modifications and improvements without departing from the concept of the present invention, and these modifications and improvements all fall within the scope of protection of the present invention. Therefore, the scope of protection of the present invention should be determined by the appended claims.

Claims

1. A localization and mapping method based on the fusion of infrared vision, millimeter-wave radar, and IMU, characterized in that, Includes the following steps: The infrared images are processed in a loop, and the pose data of the infrared camera output by the loop processing is fused with the pose data of the inertial navigation device to optimize and calculate the first odometry information of the self-platform. The loop processing includes image alignment, point initialization, point optimization, landmark initialization and pruning. The 4D point cloud data obtained by 4D millimeter-wave radar is denoised using a point cloud preprocessing algorithm to obtain stable 4D point cloud data. The three-dimensional velocity of the self-platform is calculated based on the stable 4D point cloud data. By integrating the stable 4D point cloud data and the pose data of the inertial navigation device, the second odometry information of the self-platform is optimized and calculated. By integrating the first and second odometer information and the three-dimensional volume velocity of the self-platform, the optimal odometer information is calculated and optimized. At the same time, an environmental point cloud map is drawn based on stable 4D point cloud data and the optimal odometer information. The pose data from the infrared camera, processed in a loop, is fused with the pose data from the inertial navigation system to optimize and calculate the first odometry information for the self-platform, including: A reprojection error model is established based on the measurements from the inertial navigation device and the pose data from the infrared camera output by the loop processing. Construct a nonlinear optimization problem using a reprojection error model; The odometer information of the self-platform is obtained by solving a nonlinear optimization problem; The reprojection error model is as follows: 　　　（1） In the formula, These are the coordinates of a point in the camera coordinate system. It is the corresponding 3D landmark location in the world coordinate system. It is the transformation from the IMU to the camera coordinate system in the IMU frame. It is the transformation of the IMU in the world coordinate system, and K is the intrinsic camera matrix; The cost function of the nonlinear optimization problem is 　　　（2） In the formula, This indicates the frame being processed within a sliding window that includes time frames and keyframes. Indicates in frame A group of landmarks that can be observed in the middle, Indicates in frame Every landmark visible in The superposition vector of reprojection errors. Representing a frame The covariance matrix of the Chinese landmark measurement, This represents the covariance matrix of the IMU measurements.

2. The localization and mapping method based on the fusion of infrared vision, millimeter-wave radar, and IMU as described in claim 1, characterized in that, Image alignment processing involves constructing an image pyramid model, using IMU pose data as prior information, performing infrared image registration via a direct method, and calculating the pose information of the self-platform based on the registration results.

3. The localization and mapping method based on the fusion of infrared vision, millimeter-wave radar, and IMU as described in claim 1, characterized in that, The point initialization process involves dividing the infrared image into 32×32 pixel blocks, and then filtering out keyframes of the image from these pixel blocks by setting a threshold.

4. The localization and mapping method based on the fusion of infrared vision, millimeter-wave radar, and IMU as described in claim 1, characterized in that, Point optimization processing involves calculating residuals and ratios by refining pixel block positions to eliminate unconstrained points.

5. The localization and mapping method based on the fusion of infrared vision, millimeter-wave radar, and IMU according to claim 1, characterized in that, Landmark initialization and pruning are performed by determining the initial 3D landmarks based on the corresponding points set in the infrared image, and optimizing the pruning algorithm based on the position and quality of the infrared image to prune landmarks with inconsistent depth.

6. The localization and mapping method based on the fusion of infrared vision, millimeter-wave radar, and IMU according to claim 1, characterized in that, The fusion of the stable 4D point cloud data, the pose data of the inertial navigation device, the first odometer information, the second odometer information, and the three-dimensional volume velocity is all performed using a graph optimization model in a sliding window manner.

7. A positioning and mapping system based on the fusion of infrared vision, millimeter-wave radar, and IMU, used to perform the method as described in any one of claims 1 to 6, characterized in that, The system includes: The loop calculation module is used to process the infrared image in a loop, and simultaneously fuse the pose data of the infrared camera output by the loop processing with the pose data of the inertial navigation device to optimize and calculate the first odometry information of the self-platform. The loop processing includes image alignment, point initialization, point optimization, landmark initialization and pruning. The point cloud preprocessing module is used to denoise the 4D point cloud data obtained by the 4D millimeter-wave radar through the point cloud preprocessing algorithm to obtain stable 4D point cloud data. The radar self-motion estimation module is used to calculate the three-dimensional volume velocity of the self-platform based on the stable 4D point cloud data. The pose calculation module is used to fuse the stable 4D point cloud data and the pose data of the inertial navigation device to optimize and calculate the second odometry information of the self-platform. The graph-optimized multi-sensor fusion SLAM module is used to fuse the first odometer information, the second odometer information, and the three-dimensional volume velocity to optimize and calculate the optimal odometer information. At the same time, it draws an environmental point cloud map based on stable 4D point cloud data and the optimal odometer information.

Images