Millimeter wave radar assisted monocular vision odometer

Abstract

The invention is applicable to the fields of positioning, multi-sensor fusion and visual odometers, and provides a millimeter-wave radar-assisted monocular visual odometer, which comprises the following steps: body motion estimation and point cloud screening based on millimeter-wave radar point cloud; millimeter wave radar assisted monocular initial scale calculation and three-dimensional map point recovery; millimeter-wave radar-assisted monocular vision odometer scale drift correction is carried out; according to the method, the speed estimation reliability is improved, the problem that a monocular speedometer lacks real speed reference is solved through the processing flow of building a radar speed model, eliminating dynamic points through RANSAC, carrying out speed least square fitting and screening static point cloud, accurate input is provided for initial scale calculation, and the situation that dynamic interference influences a positioning datum is avoided; the platform application limitation is broken through; and the scale drift error is reduced.

Description

Technical Field

[0001] This invention belongs to the fields of positioning, multi-sensor fusion, and visual odometry, and particularly relates to a millimeter-wave radar-assisted monocular visual odometry. Background Technology

[0002] The scale limitation of monocular visual odometry: Monocular cameras are the core sensors for autonomous driving and robotics. Visual odometry based on continuous images can estimate the platform's motion trajectory. However, monocular images do not contain real-world scale information, which causes the odometry to also lack absolute scale and cannot correspond to the real environment's spatial position, making it difficult to meet the requirements for accurate positioning (such as the trajectory error of traditional monocular ORB-SLAM accumulates with the movement distance). Application limitations of monocular and IMU fusion: Although existing monocular camera and IMU fusion odometry can provide absolute scale, initialization and scale recovery require large motion excitation (such as rapid platform turning and acceleration). However, platforms such as vehicles and ships have small motion amplitudes when starting or traveling at low speeds, which cannot meet this condition, thus limiting its application on such platforms. Advantages of millimeter-wave radar: Millimeter-wave radar is inexpensive and resistant to environmental interference such as wind, rain, and fog. It can also measure Doppler velocity and provide an estimate of the platform's true motion velocity. However, existing technologies do not fully utilize its velocity information to solve the scale problem of monocular odometry, which prevents the radar from fully realizing its advantages. Therefore, a millimeter-wave radar-assisted monocular visual odometry is needed to solve the above problems. Summary of the Invention

[0003] The purpose of this invention is to provide a millimeter-wave radar-assisted monocular visual odometry to solve the problems mentioned in the background art.

[0004] To achieve the above objectives, the present invention provides the following technical solution: A millimeter-wave radar-assisted monocular visual odometry method includes the following steps: S1. Body motion estimation and point cloud selection based on millimeter-wave radar point cloud; S2. Millimeter-wave radar-assisted monocular initial scale calculation and 3D map point restoration; S3, millimeter-wave radar-assisted monocular vision odometry scale drift correction.

[0005] A further technical solution, step S1 includes the following steps: S11. Construct a velocity estimation model for millimeter-wave radar point clouds at time t. , ( Distance to point i For horizontal azimuth, For vertical azimuth, For Doppler velocity, (Total number of point clouds), static target points With radar three-axis velocity satisfy ; S12. After excluding dynamic point clouds using the RANSAC algorithm, the estimated radar three-axis velocities are obtained by fitting using the least squares algorithm. ; S13, based on Calculating the theoretical speed of point clouds ,reserve ( , The point cloud (with radar velocity resolution) is used to obtain the filtered point cloud. .

[0006] In a further technical solution, in step S12, the RANSAC algorithm fits a velocity model by repeatedly sampling a subset of static point clouds, discarding dynamic point clouds with residuals exceeding a threshold, and the least squares algorithm minimizes... ( , Solve for the coefficient matrix in the velocity model. ; In step S13, the theoretical speed of the point cloud pass Calculation.

[0007] Step S1 effectively filters out dynamic target point clouds and clutter point clouds through mathematical model construction and algorithm screening, avoiding velocity estimation errors caused by dynamic interference. The standard deviation is ≤0.05m / s, providing accurate velocity input for subsequent scale calculations and ensuring the reliability of the scale reference.

[0008] A further technical solution, step S2 includes the following steps: S21. Initialize the monocular visual odometry using the ORB-SLAM algorithm to obtain the pose at time 0. ( It is the identity matrix. (zero vector), scale-free pose at time n and scaleless map points ; S22. Using the Direct Linear Transform (DLT) method for the PnP problem, match the image at time i between 0 and n. and Triangulated feature points to recover the scale-free pose at time i. ; S23, Rotation matrix via radar to camera Will Convert to camera speed Combined with image time intervals There is a cumulative scale displacement from time 0 to n. Calculate the scaling factor Restore a measured posture With scaled map points .

[0009] In a further technical solution, in step S21, the initialization conditions for the ORB-SLAM algorithm are: feature point matching number ≥ 80 and disparity ≥ 0.05. ( This represents the total number of map points. (Coordinates of points on a scaleless map); In step S22, the DLT algorithm by... The coordinates of two-dimensional feature points and Substitute the three-dimensional coordinates into the system of linear equations and solve. rotation matrix With translation matrix ; In step S23, there are scaled map points. ( for (points in the middle).

[0010] Step S2 can restore the absolute scale of the monocular odometry without relying on large motion excitations, overcoming the application limitations of monocular and IMU fusion—in low-speed vehicle start-up scenarios, the initialization success rate is increased from 30% with monocular + IMU fusion to 95%, and and It can accurately correspond to the real environment and space, providing a reliable scale benchmark for subsequent positioning.

[0011] A further technical solution, step S3 includes the following steps: S31. Use the ORB-SLAM algorithm to match the image at subsequent time j. With keyframes The initial pose at time j is obtained through the DLT algorithm. ; S32, based on time j-1 Calculate the change in displacement with scale Compare the changes in visual displacement Displacement correction The pose is updated at time j with a scale. .

[0012] A further technical solution, in step S31, is the key frame. To and The most recent frame with ≥50 matching feature points; In step S32, Determined by the camera frame rate; when the camera frame rate is 10fps, .

[0013] Step S3 constrains visual displacement in real time by radar velocity, effectively correcting the scale drift of monocular odometry. In data segment 1, the average ATE of this method is only 0.14m (4.43m for monocular odometry), and in data segment 2, the average ATE is 0.16m (3.78m for monocular odometry). When the velocity is ≥1.0m / s, the average ATE is stable at 0.37-0.40m, significantly improving positioning accuracy and stability.

[0014] Compared with the prior art, the beneficial effects of the present invention are: This invention improves the reliability of velocity estimation: through a processing flow of "constructing a radar velocity model → RANSAC to exclude dynamic points → least squares fitting of velocity → screening of static point clouds," interference from dynamic targets and clutter point clouds is filtered out, ultimately yielding the radar's three-axis velocity. With a standard deviation of ≤0.05m / s, it solves the problem of lack of real velocity reference in monocular odometers, provides accurate input for initial scale calculation, and avoids dynamic interference affecting the positioning reference. This invention breaks through platform application limitations: through the processing flow of "ORB-SLAM initialization → PnP frame pose recovery → radar velocity to camera velocity → calculation of scale displacement and scale factor", the scale recovery of monocular odometry can be completed without large motion excitation. On low-speed starting or small-amplitude motion platforms such as vehicles and ships, the initialization success rate is increased from 30% of monocular + IMU fusion to 95%, solving the application limitations of existing methods. This invention reduces scale drift error: through the processing flow of "ORB-SLAM to estimate initial pose → radar velocity to calculate corrected displacement → update scaled pose", the scale deviation of visual odometry is corrected in real time. The average ATE of data segment 1 is 0.14m (4.43m for monocular odometry), and the average ATE of data segment 2 is 0.16m (3.78m for monocular odometry). When the velocity is ≥1.0m / s, the average ATE is stable at 0.37-0.40m, which significantly improves the positioning accuracy and stability.

[0015] To more clearly illustrate the structural features and effects of the present invention, the present invention will be described in detail below with reference to the accompanying drawings and specific embodiments. Attached Figure Description

[0016] Figure 1 This is a schematic diagram of the overall process of the present invention; Figure 2 This is a schematic diagram comparing the odometer results on the data segment of this invention; Figure 3This is a schematic diagram illustrating the error analysis under different speed scenarios of the present invention. Detailed Implementation

[0017] 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.

[0018] The specific implementation of the present invention will be described in detail below with reference to specific embodiments.

[0019] Example 1 like Figure 1-3 As shown, this embodiment of the invention provides a millimeter-wave radar-assisted monocular visual odometry, comprising the following steps: S1. Body motion estimation and point cloud selection based on millimeter-wave radar point clouds: S11. Construct a velocity model, radar point cloud at time t. ( , ), static target point and Satisfies the velocity model formula; S12. The RANSAC algorithm is used for 100 iterations, with 3 points sampled each time to fit the velocity model. After removing dynamic points, the velocity model is obtained through least squares fitting. , , ; S13, Assume radar velocity resolution , , calculation ,reserve The 165 points are obtained ; S2. Radar-assisted monocular initial scale calculation and map point restoration: S21. Initialize using ORB-SLAM to obtain... , and ( ); S22. Use the DLT algorithm to match frames between 0 and n. Feature points are used to recover the scale-free pose of each frame. ; S23, Let It is a unit matrix (camera and radar are mounted coaxially). , ,calculate , ,recover and ; S3, Radar-assisted scale drift correction: S31. Use ORB-SLAM to match the image at subsequent time j. With keyframes The result obtained through the DLT algorithm ; S32, Calculation , , ,renew .

[0020] In this embodiment, a vehicle-mounted millimeter-wave radar (maximum speed 4.5 m / s) is used. The method was tested with a monocular camera (10fps) on urban road data segment 1 (X-axis 0-35m): the average ATE was 0.14m, and the average ATE of the monocular odometry was 4.43m; on suburban road data segment 2 (X-axis -15-35m): the average ATE was 0.16m, and the average ATE of the monocular odometry was 3.78m; the average ATE was 0.52m at a speed of 0.2m / s, 0.40m at 1.0m / s, and stable at 0.37-0.40m at ≥1.0m / s; the initialization process does not require large motion excitation, and scale recovery can be completed within 3s after the vehicle starts, making it suitable for unmanned vehicle localization on urban and suburban roads.

[0021] Example 2 The difference between this embodiment and Embodiment 1 is that: In step S13 In step 2.3 (The camera and radar are mounted horizontally at a 90° angle and are not coaxial.)

[0022] In this embodiment, Smaller values ​​result in stricter removal of dynamic points when filtering point clouds—in suburban road scenarios with low pedestrian traffic (dynamic targets account for 10%). The proportion of dynamic points decreased from 3% in Example 1 to 1.5%, and the standard deviation of radar velocity estimation was 0.03 m / s (0.05 m / s in Example 1). The method adapts to the 90° mounting method of cameras and radar, solving the application needs of small robots (with limited installation space, sensors cannot be mounted coaxially). The average ATE of data segment 1 is 0.15m and the average ATE of data segment 2 is 0.17m, which is only 0.01m higher than that of Example 1. The average ATE of data segment 2 is 0.41m at a speed of 1.0m / s, which is still far lower than the 4.43m of monocular odometry, proving the adaptability of the method in scenarios where sensors are not mounted coaxially.

[0023] Example 3 The difference between this embodiment and Embodiment 2 is that: In step 2.2, the PnP problem uses the EPnP algorithm instead of the DLT algorithm; in step 3.1, the keyframes... Selection and The most recent frame with ≥60 feature point matches (≥50 in Example 2).

[0024] In this embodiment, the EPnP algorithm improves the pose recovery success rate between frames 0 and n in a sparsely textured underground parking garage scene (40 triangulated feature points per frame, compared to 80 in Embodiment 2) from 85% in Embodiment 2 to 98%, avoiding pose estimation failures due to a small number of feature points; keyframes The match count threshold was increased to 60, making and The average number of matches reached 75 (60 in Example 2), and the initial pose at time j. The estimated standard deviation is 0.06m (0.08m in Example 2); when applied to an AGV robot in an underground parking garage, the average ATE is 0.18m, and the average ATE is 0.50m at a speed of 0.2m / s (0.53m in Example 2), which solves the problem of unstable pose estimation in sparse texture scenes and expands the application scope of the method.

[0025] Working principle and usage process of this invention: With "millimeter-wave radar velocity providing scale constraints" as its core, this method addresses the scale limitations of monocular visual odometry through a three-level process: "radar point cloud processing → monocular scale recovery → real-time drift correction." The specific working principle is as follows: Phase 1: Millimeter-wave radar point cloud processing (acquiring reliable velocity and static point clouds); Point cloud acquisition and model building: Millimeter-wave radar acquires environmental point clouds at time t at a frequency of 10Hz, generating... each Include (distance), (Horizontal azimuth) (Vertical azimuth) (Doppler velocity); Based on the physical characteristics that the Doppler velocity of a static target is solely generated by the radar's own motion, a velocity estimation model is constructed and established. With radar three-axis velocity The quantitative correlation (formula in claim 1); On-body velocity estimation: Dynamic target point clouds (such as pedestrians and vehicles) in the scene can disrupt the linear relationship of the velocity model. Therefore, the RANSAC algorithm is used to randomly sample subsets of the point cloud multiple times, fit preliminary velocities, and calculate residuals, discarding dynamic points with large residuals. For the remaining static point cloud, the least squares algorithm is used to minimize the sum of squared residuals to solve for the radar's three-axis velocity. This ensures that the speed reflects the platform's true motion. Static point cloud filtering: based on radar velocity resolution set up (Balancing noise and dynamic point removal effects), based on Calculate the theoretical speed for each point cloud. ;contrast and The retained difference is less than The point cloud was obtained (High-quality static point cloud) provides data support for subsequent environmental mapping, while avoiding interference from dynamic points in subsequent calculations; Phase 2: Initial scale recovery of monocular visual odometry (establishing an absolute scale reference). Monocular odometry initialization: The monocular camera acquires continuous images at 10fps, and the ORB-SLAM algorithm is used to detect ORB feature points in the images (possessing rotation and scale invariance); when the nth frame image... Initialization is completed when the initialization conditions of "feature point matching number ≥ 80 and disparity ≥ 0.05" are met, and the reference pose at time 0 is obtained. (The rotation matrix is ​​the identity matrix, and the translation matrix is ​​the zero vector), scale-free pose at time n. and scale-free 3D map points ; Intermediate frame pose recovery: Frames between 0 and n have undetermined poses because they did not meet the initialization conditions; these frames are matched with the intermediate frame poses using the DLT method of the PnP algorithm. The triangulated feature points (establishing the correspondence between 2D image points and 3D map points) are substituted into the DLT linear equations to solve for the scale-free pose of each frame. This forms a complete scale-free pose sequence from time 0 to n. Velocity conversion and scale factor calculation: Pre-calibrate the rotation matrix based on the installation angles of the radar and camera. , radar speed Velocity converted to camera coordinate system Combined with image time intervals Rotation matrix of pose in each frame Calculate the scaled displacements from time 0 to n. ; Calculate the scaling factor ( for (scale-free translation matrix), Multiplying by the scale-free translation matrix of each frame and the scale-free map point coordinates yields the scaled pose. and scaled map points Complete the initial scale recovery; Third stage: Real-time correction of scale drift in monocular visual odometry (maintaining scale stability). Initial pose estimation for subsequent frames: for images at time j>n ORB-SLAM algorithm is used to select and The most recent keyframe (Feature point matching count ≥ 50, ensuring matching reliability); Matching and Feature points are solved using the DLT algorithm. initial pose (including rotation matrix) and scale-free translation matrix ); Displacement correction and pose update: using the final calibrated pose at time j-1 (including translation matrix) ) and camera speed Calculate the scaled displacement change from time j-1 to time j. Displacement changes obtained from computational visual odometry ; Calculate the corrected displacement based on the ratio of their modulus lengths. ,Will and Addition, combination Obtain the final sized pose at time j. To achieve scale drift correction; Loop iteration and trajectory generation; During platform movement, the radar and camera continuously acquire point cloud data and images at times t+1, t+2, etc., repeating the above process of "radar point cloud processing → monocular scale recovery → scale drift correction" to update the data sequentially. ; All moments By connecting the data in chronological order, the continuous motion trajectory of the platform in the real world can be obtained, thus completing the monocular visual odometry function with absolute scale.

[0026] The above description is only a preferred embodiment of the present invention and is not intended to limit the present invention. Any modifications, equivalent substitutions, and improvements made within the spirit and principles of the present invention should be included within the protection scope of the present invention.

Claims

1. A millimeter-wave radar-assisted monocular visual odometry system, characterized in that, Includes the following steps: S1. Body motion estimation and point cloud selection based on millimeter-wave radar point cloud; S2. Millimeter-wave radar-assisted monocular initial scale calculation and 3D map point restoration; S3, millimeter-wave radar-assisted monocular vision odometry scale drift correction.

2. The millimeter-wave radar-assisted monocular visual odometry according to claim 1, characterized in that, Step S1 includes the following steps: S11. Construct a velocity estimation model for millimeter-wave radar point clouds at time t. , ( Distance to point i For horizontal azimuth, For vertical azimuth, For Doppler velocity, (Total number of point clouds), static target points With radar three-axis velocity satisfy ; S12. After excluding dynamic point clouds using the RANSAC algorithm, the estimated radar three-axis velocities are obtained by fitting using the least squares algorithm. ; S13, based on Calculating the theoretical speed of point clouds ,reserve ( , The point cloud (with radar velocity resolution) is used to obtain the filtered point cloud. .

3. The millimeter-wave radar-assisted monocular visual odometry according to claim 2, characterized in that, In step S12, the RANSAC algorithm fits a velocity model by repeatedly sampling a subset of static point clouds, discarding dynamic point clouds with residuals exceeding a threshold. The least squares algorithm minimizes... ( , Solve for the coefficient matrix in the velocity model. ; In step S13, the theoretical speed of the point cloud pass Calculation.

4. The millimeter-wave radar-assisted monocular visual odometry according to claim 1, characterized in that, Step S2 includes the following steps: S21. Initialize the monocular visual odometry using the ORB-SLAM algorithm to obtain the pose at time 0. ( It is the identity matrix. (zero vector), scale-free pose at time n and scaleless map points ; S22. Using the Direct Linear Transform (DLT) method for the PnP problem, match the image at time i between 0 and n. and Triangulated feature points to recover the scale-free pose at time i. ; S23, Rotation matrix via radar to camera Will Convert to camera speed Combined with image time intervals There is a cumulative scale displacement from time 0 to n. Calculate the scaling factor Restore a measured posture With scaled map points .

5. The millimeter-wave radar-assisted monocular visual odometry according to claim 4, characterized in that, In step S21, the initialization conditions for the ORB-SLAM algorithm are: feature point matching number ≥ 80 and disparity ≥ 0.

05. ( This represents the total number of map points. (Coordinates of points on a scaleless map); In step S22, the DLT algorithm by... The coordinates of two-dimensional feature points and Substitute the three-dimensional coordinates into the system of linear equations and solve. rotation matrix With translation matrix ; In step S23, there are scaled map points. .

6. The millimeter-wave radar-assisted monocular visual odometry according to claim 1, characterized in that, Step S3 includes the following steps: S31. Use the ORB-SLAM algorithm to match the image at subsequent time j. With keyframes The initial pose at time j is obtained through the DLT algorithm. ; S32, based on time j-1 Calculate the change in displacement with scale Compare the changes in visual displacement Displacement correction Update the pose with scale at time j. .

7. The millimeter-wave radar-assisted monocular visual odometry according to claim 6, characterized in that, In step S31, keyframe To and The most recent frame with at least 50 matching feature points; In step S32, Determined by the camera frame rate; when the camera frame rate is 10fps, .

Images