Method, device, processor and storage medium for implementing vehicle-side 4D millimeter-wave radar SLAM processing in combination with fisheye camera

Abstract

The present invention relates to a method for realizing vehicle-side 4D millimeter-wave radar SLAM processing in combination with a fisheye camera, wherein the method comprises the following steps: (1) an intelligent driving vehicle collects point cloud data of a 4D millimeter-wave radar and image data of a fisheye camera in real time, and preprocesses the data of the two; (2) pose estimation processing is performed on the point cloud data and the image data simultaneously, and the current pose of the vehicle based on the 4D millimeter-wave radar and the current pose of the vehicle based on the fisheye image are obtained respectively; (3) the two poses are subjected to EKF filtering processing to obtain a more accurate pose; (4) the precise pose of the vehicle is transformed with the point cloud key frame of the 4D millimeter-wave radar to obtain a point cloud map; (5) the precise pose and the point cloud map are output as SLAM. The present invention also relates to a corresponding device, a processor and a storage medium thereof. The method, device, processor and storage medium of the present invention are adopted to greatly improve the mapping quality and positioning accuracy.

Description

Technical Field

[0001] The present invention relates to the field of intelligent driving technology, in particular to the field of millimeter-wave radar and SLAM technology, and specifically refers to a method, device, processor and computer-readable storage medium thereof for realizing vehicle-side 4D millimeter-wave radar SLAM processing in combination with a fisheye camera. Background Art

[0002] In the field of intelligent driving, vehicles often need to perceive their surroundings and determine their own position in real time to achieve accurate perception of the surrounding environment and output it to the motion planning and decision-making module. This requires SLAM technology to build a map of the vehicle's surroundings while determining the vehicle's position in the global reference coordinate system.

[0003] Currently, vehicle-based SLAM technology using pure 4D millimeter-wave radar is limited by the sparseness of millimeter-wave radar's point cloud and the inaccurate measurement of its altitude information and pitch angles. This results in poor quality environmental maps (object outlines are unclear and there are ghosting images) and poor position accuracy (significantly different from the actual position). This phenomenon is particularly noticeable when the vehicle is turning. Poor map quality and low positioning accuracy often lead to jitter in subsequent motion control, resulting in a poor driving experience for passengers. Summary of the Invention

[0004] The purpose of the present invention is to overcome the shortcomings of the above-mentioned prior art and provide a method, device, processor and computer-readable storage medium thereof for improving mapping quality and positioning accuracy by combining a fisheye camera to realize vehicle-side 4D millimeter-wave radar SLAM processing.

[0005] To achieve the above objectives, the present invention provides a method, device, processor, and computer-readable storage medium for implementing vehicle-side 4D millimeter-wave radar SLAM processing in combination with a fisheye camera.

[0006] The method for realizing vehicle-side 4D millimeter-wave radar SLAM processing by combining a fisheye camera is characterized in that the method comprises the following steps:

[0007] (1) The intelligent driving vehicle collects point cloud data from the 4D millimeter-wave radar and image data from the fisheye camera in real time and preprocesses the data;

[0008] (2) Performing pose estimation on the pre-processed point cloud data and image data simultaneously to obtain the current pose of the vehicle based on the 4D millimeter-wave radar and the current pose of the vehicle based on the fisheye image, respectively;

[0009] (3) Perform EKF filtering on the two poses and obtain a more accurate pose after filtering;

[0010] (4) Transforming the precise position of the vehicle with the point cloud key frame of the 4D millimeter wave radar to obtain the key frame point cloud in the world coordinate system, and then splicing the key frame point clouds of several frames to obtain a point cloud map;

[0011] (5) The precise pose and point cloud map obtained above are output as SLAM.

[0012] Preferably, the step (1) specifically includes the following steps:

[0013] (1.1) Time-synchronize the data collected from each 4D millimeter-wave radar and combine them into a 360-degree point cloud around the vehicle in a sequential fusion manner;

[0014] (1.2) Time-align and synchronize the point cloud data of the 4D millimeter-wave radar and the image data of the fisheye camera, and align them according to the time of the 4D millimeter-wave radar point cloud.

[0015] Preferably, the step (2) of performing pose estimation based on 4D millimeter wave radar specifically includes the following steps:

[0016] (2.1.a) Extracting point cloud keyframes: Based on the vehicle's motion information, i.e., the distance Δd or the rotation angle Δα from the last keyframe extracted, if Δd or Δα is greater than a certain threshold θ, the current 4D millimeter-wave radar point cloud P is extracted. i {x0,x1…x m}Add as a keyframe, denoted as K i ;

[0017] (2.2.a) Matching between point cloud key frames: Key frame K i and the previous keyframe K i-1 Perform normal distribution transformation ndt matching and transform the key frame K i Point cloud P i {x0,x1…x m} and keyframe K i-1 Point cloud P i-1 {x0,x1…x n} is divided into a voxel unit voxel, and the normal distribution p of all points in each voxel unit is estimated. voxel ~N(μ,σ), where μ is the mean of the normal distribution and σ is the covariance of the normal distribution; and solve a transformation so that the key frame K i The point is transformed and falls to the key frame K i-1 The probability value p of the voxel cell voxel where the point is located voxel The maximum value is solved as follows:

[0018]

[0019] Finally, the relative translation transformation between 4D millimeter wave radar frames is obtained and relative rotation transformation

[0020] (2.3.a) Use the following formula to calculate the current vehicle posture based on 4D millimeter wave radar

[0021]

[0022] in, is the pose estimate of the vehicle in the previous frame.

[0023] Preferably, the step (2) of performing pose estimation based on the fisheye image specifically includes the following steps:

[0024] (2.1.b) Extract visual features: Extract visual ORB features from each fisheye image frame, and record the features extracted from the current image frame as F i , the feature extracted from the image frame at the previous moment is recorded as F i-1 , the pose of the current frame of the fisheye image is The pose of the image frame at the previous moment is

[0025] (2.2.b) Visual feature matching: Based on feature F i and F i-1 The Hamming distance between the two frames is used to find the corresponding matching points between the two frames, and the PnP is used to calculate the relative translation transformation of the current frame relative to the previous frame. and relative rotation transformation

[0026] (2.3.b) Use the following formula to calculate the current pose based on the fisheye image

[0027]

[0028] And from the fisheye image pose Remove the rotating part and get

[0029] Preferably, the step (3) is specifically as follows:

[0030] To obtain the pose estimated based on 4D millimeter wave radar and pose estimation based on fisheye images The rotating part After that, the two poses are subjected to EKF filtering, specifically according to and The Kalman gain is updated based on the covariance of the posture, thereby updating the posture state and obtaining a more accurate posture X after filtering. i .

[0031] Preferably, the step (4) specifically includes the following steps:

[0032] (4.1) The key frame K of the 4D millimeter wave radar is extracted i The corresponding point cloud P i {x0,x1…x m} and the exact pose X i The keyframe point cloud is calculated according to the following formula

[0033]

[0034] Among them, the key frame point cloud The key frame point cloud in the world coordinate system is obtained by transforming to the global reference coordinate system;

[0035] (4.2) Add up the transformed keyframe point clouds to form a point cloud map of the entire environment

[0036] Preferably, the step (5) is specifically as follows:

[0037] The exact pose X i With the point cloud map M i As SLAM output.

[0038] The main features of the device for implementing vehicle-side 4D millimeter-wave radar SLAM processing in combination with a fisheye camera are as follows:

[0039] a processor configured to execute computer-executable instructions;

[0040] A memory stores one or more computer-executable instructions. When the computer-executable instructions are executed by the processor, the various steps of the method for realizing vehicle-side 4D millimeter-wave radar SLAM processing in combination with a fisheye camera are implemented.

[0041] The main feature of the processor that combines a fisheye camera to implement vehicle-side 4D millimeter-wave radar SLAM processing is that the processor is configured to execute computer-executable instructions. When the computer-executable instructions are executed by the processor, the various steps of the above-mentioned method of combining a fisheye camera to implement vehicle-side 4D millimeter-wave radar SLAM processing are implemented.

[0042] The main feature of this computer-readable storage medium is that a computer program is stored thereon, and the computer program can be executed by a processor to implement the various steps of the above-mentioned method of realizing vehicle-side 4D millimeter-wave radar SLAM processing in combination with a fisheye camera.

[0043] The present invention employs a method, device, processor, and computer-readable storage medium for vehicle-side 4D millimeter-wave radar SLAM processing in conjunction with a fisheye camera. Using the standard fisheye camera in intelligent driving vehicles as the VO, the method filters and fuses its rotation R with the pose estimated by pure millimeter-wave radar, resulting in more accurate pose estimation and improved map quality. This is because the fisheye camera VO method utilizes richer environmental information, and the estimated pose rotation R is more accurate than that of pure millimeter-wave radar. However, due to the limitation of a monocular camera that cannot estimate depth, the translation component of the VO is not integrated. The fisheye camera's estimated R is integrated to correct and update the pure millimeter-wave radar pose, thereby achieving higher pose estimation accuracy. This improved pose accuracy, in turn, improves the quality of map stitching. Furthermore, this technical solution has excellent adaptability and scalability. Image frame-based pose estimation can be extended to pinhole cameras, not just fisheye cameras, and can be applied to various SLAM methods for intelligent driving vehicles, resulting in more accurate pose estimation. BRIEF DESCRIPTION OF THE DRAWINGS

[0044] Figure 1 This is a flow chart of the method of the present invention for realizing vehicle-side 4D millimeter-wave radar SLAM processing in combination with a fisheye camera. DETAILED DESCRIPTION

[0045] In order to more clearly describe the technical content of the present invention, further description is given below in conjunction with specific embodiments.

[0046] Before describing in detail embodiments according to the present invention, it should be noted that, hereinafter, the terms "comprises," "comprising," or any other variations thereof are intended to cover non-exclusive inclusion, whereby a process, method, article, or apparatus comprising a list of elements includes not only those elements, but also other elements not expressly listed or inherent to such process, method, article, or apparatus.

[0047] Before explaining this technical solution in detail, the relevant abbreviations involved in this case are further explained:

[0048] SLAM: Simultaneous localization and mapping

[0049] VO: Visual Odometry

[0050] ndt: normal distribution transform

[0051] t: translation in pose

[0052] R: Rotation in pose

[0053] Radar: radar, here specifically refers to 4D millimeter wave radar

[0054] EKF: Extended Kalman Filter

[0055] ORB: Oriented FAST and Rotated BRIEF

[0056] PnP: Perspective-n-Points

[0057] Voxel: Voxel cell

[0058] w: World reference coordinate system, short for world

[0059] ●r:radar coordinate system

[0060] M: Map, short for Map

[0061] See also Figure 1 As shown, the method for realizing vehicle-side 4D millimeter-wave radar SLAM processing in combination with a fisheye camera, wherein the method comprises the following steps:

[0062] (1) The intelligent driving vehicle collects point cloud data from the 4D millimeter-wave radar and image data from the fisheye camera in real time and preprocesses the data;

[0063] (2) Performing pose estimation on the pre-processed point cloud data and image data simultaneously to obtain the current pose of the vehicle based on the 4D millimeter-wave radar and the current pose of the vehicle based on the fisheye image, respectively;

[0064] (3) Perform EKF filtering on the two poses and obtain a more accurate pose after filtering;

[0065] (4) Transforming the precise position of the vehicle with the point cloud key frame of the 4D millimeter wave radar to obtain the key frame point cloud in the world coordinate system, and then splicing the key frame point clouds of several frames to obtain a point cloud map;

[0066] (5) The precise pose and point cloud map obtained above are output as SLAM.

[0067] As a preferred embodiment of the present invention, the step (1) specifically includes the following steps:

[0068] (1.1) Time-synchronize the data collected from each 4D millimeter-wave radar and combine them into a 360-degree point cloud around the vehicle in a sequential fusion manner;

[0069] (1.2) Time-align and synchronize the point cloud data of the 4D millimeter-wave radar and the image data of the fisheye camera, and align them according to the time of the 4D millimeter-wave radar point cloud.

[0070] As a preferred embodiment of the present invention, the step (2) of performing pose estimation based on 4D millimeter wave radar specifically includes the following steps:

[0071] (2.1.a) Extracting point cloud keyframes: Based on the vehicle's motion information, i.e., the distance Δd or the rotation angle Δα from the last keyframe extracted, if Δd or Δα is greater than a certain threshold θ, the current 4D millimeter-wave radar point cloud P is extracted. i {x0,x1…x m}Add as a keyframe, denoted as K i ;

[0072] (2.2.a) Matching between point cloud key frames: Key frame K i and the previous keyframe K i-1 Perform normal distribution transformation ndt matching and transform the key frame K i Point cloud P i {x0,x1…x m} and keyframe K i-1 Point cloud P i-1 {x0,x1…x n} is divided into a voxel unit voxel, and the normal distribution p of all points in each voxel unit is estimated. voxel ~N(μ,σ), where μ is the mean of the normal distribution and σ is the covariance of the normal distribution; and solve a transformation so that the key frame K i The point is transformed and falls to the key frame K i-1 The probability value p of the voxel cell voxel where the point is located voxel The maximum value is solved as follows:

[0073]

[0074] Finally, the relative translation transformation between 4D millimeter wave radar frames is obtained and relative rotation transformation

[0075] (2.3.a) Use the following formula to calculate the current vehicle posture based on 4D millimeter wave radar

[0076]

[0077] in, is the pose estimate of the vehicle in the previous frame.

[0078] As a preferred embodiment of the present invention, the step (2) of performing pose estimation based on the fisheye image specifically includes the following steps:

[0079] (2.1.b) Extract visual features: Extract visual ORB features from each fisheye image frame, and record the features extracted from the current image frame as F i , the feature extracted from the image frame at the previous moment is recorded as F i-1 , the pose of the current frame of the fisheye image is The pose of the image frame at the previous moment is

[0080] (2.2.b) Visual feature matching: Based on feature F i and F i-1 The Hamming distance between the two frames is used to find the corresponding matching points between the two frames, and the motion transformation Δt of the current frame relative to the previous frame is calculated using PnP. i and ΔR i ;

[0081] (2.3.b) Use the following formula to calculate the current pose based on the fisheye image

[0082]

[0083] And from the fisheye image pose Remove the rotating part and get

[0084] As a preferred embodiment of the present invention, the step (3) is specifically as follows:

[0085] To obtain the pose estimated based on 4D millimeter wave radar and pose estimation based on fisheye images The rotating part After that, the two poses are subjected to EKF filtering, specifically according to and The Kalman gain is updated based on the covariance of the posture, thereby updating the posture state and obtaining a more accurate posture X after filtering. i .

[0086] As a preferred embodiment of the present invention, the step (4) specifically includes the following steps:

[0087] (4.1) The key frame K of the 4D millimeter wave radar is extracted i The corresponding point cloud P i {x0,x1…x m} and the exact pose X i The keyframe point cloud is calculated according to the following formula

[0088]

[0089] Among them, the key frame point cloud The key frame point cloud in the world coordinate system is obtained by transforming to the global reference coordinate system;

[0090] (4.2) Add up the transformed keyframe point clouds to form a point cloud map of the entire environment

[0091] As a preferred embodiment of the present invention, the step (5) is specifically as follows:

[0092] The exact pose X i With the point cloud map M i As SLAM output.

[0093] In practical applications, the main steps of combining fisheye cameras and 4D millimeter-wave radar for SLAM in this technical solution include:

[0094] 1) Data input. The intelligent driving vehicle collects point cloud data from six 4D millimeter-wave radars and image data from one fisheye camera in real time and preprocesses the data. The preprocessing includes:

[0095] a. Synchronize the time of each millimeter-wave radar data and combine them into a car according to the fusion method of which one is integrated first.

[0096] 360-degree point cloud around the vehicle;

[0097] b. Time-align and synchronize the millimeter-wave radar point cloud data and fisheye image data, aligning them according to the time of the radar point cloud;

[0098] 2) Pose estimation. That is, to estimate the translation t and rotation R of the vehicle at the current time i, the pose estimation based on the millimeter-wave radar based on the point cloud and the pose estimation based on the fisheye image are performed simultaneously and are calculated in parallel. Therefore, from the perspective of the framework, it is a post-processing method. The pose estimation based on the millimeter-wave radar is obtained respectively. and position estimation based on fisheye images

[0099] a. Pose estimation based on millimeter-wave radar includes the following steps:

[0100] i. Extract key frames. Based on the vehicle's motion information, that is, the distance Δd from the last key frame extracted

[0101] Or rotate the angle Δα, if Δd or Δα is greater than a certain threshold θ, the current millimeter wave radar point cloud P i {x0,x1…x m}Add as a keyframe, denoted as K i ;

[0102] ii. Matching between point cloud key frames. That is, key frame K i and the previous keyframe K i-1 Perform ndt matching, ndt is based on the voxel normal distribution assumption, and the key frame K i Point cloud P i {x0,x1…x m} and keyframe K i-1 Point cloud P i-1 {x0,x1…x n} is divided into a voxel unit voxel, and the normal distribution p of all points in each voxel unit is estimated. voxel ~N(μ,σ), μ is the mean of the normal distribution, σ is the covariance of the normal distribution. Solve a transformation so that the key frame K i The point is transformed and falls to the key frame K i-1 The probability value p of the voxel where the point is located voxel Maximum, and finally the relative motion transformation Δt between frames is obtained i and ΔR i ;

[0103] iii. Use the following formula

[0104]

[0105] Get the current position of the vehicle based on millimeter wave radar

[0106] b. Pose estimation based on fisheye images includes the following steps:

[0107] i. Extract visual features. Extract visual ORB features from each frame of fisheye image and record the features extracted from the current image frame as F i , the feature extracted from the image frame at the previous moment is recorded as F i-1 , the pose of the current frame of the fisheye image is The pose of the image frame at the previous moment is

[0108] ii. Visual feature matching. According to feature F i and F i-1 The Hamming distance between the two frames is used to find the corresponding matching points between the two frames, and the motion transformation Δt of the current frame relative to the previous frame is calculated using PnP. i and ΔR i ;

[0109] iii.Use the formula:

[0110]

[0111] Get the current pose estimate based on the fisheye image But only the rotation part of the pose is output Only rotation is used to assist the millimeter-wave radar in estimating the pose. This is because the millimeter-wave radar estimates the translation part more accurately and does not require other translation-assisted estimations.

[0112] 3) Posture fusion. After step 2), the pose estimated based on the millimeter wave radar is obtained and estimated based on fisheye images The rotating part After that, the two poses are subjected to EKF filtering, specifically according to and The Kalman gain is updated based on the pose covariance size, thereby updating the pose state and obtaining a more accurate pose X after filtering. i ;

[0113] 4) Map stitching. Get the vehicle’s precise position X i Then, the key frame K extracted from 2) i The corresponding point cloud P i {x0,x1…x m} and pose X i According to the formula:

[0114]

[0115] Transform to the global reference coordinate system to obtain the key frame point cloud in the world coordinate system Then add up the transformed keyframe point clouds to form a map of the entire environment The resulting map is a millimeter-wave radar point cloud map, not an image feature point map, and is a relatively dense map.

[0116] 5) SLAM output. The accurate pose X obtained after filtering in step 3) is i (t i ,R i) and the map after step 4) map stitching The output is used as the output of the SLAM of this technical solution.

[0117] In practical applications, this technical solution has the following advantages over the existing technology:

[0118] (1) This technical solution proposes a vehicle-side 4D millimeter-wave radar SLAM method combined with fisheye, which greatly improves the mapping quality and positioning accuracy compared with the original pure 4D millimeter-wave radar SLAM method;

[0119] (2) This technical solution has great adaptability and can be used on smart driving vehicles of different types and structural designs;

[0120] (3) This technical solution combines fisheye sensors to maximize the use of various sensors on the vehicle, improves the utilization rate of hardware equipment, and has a high cost-effectiveness;

[0121] (4) In this technical solution, only the rotation R in the fisheye VO is taken out and fused with the pose estimated by the 4D millimeter-wave radar; no visual closed-loop detection and global pose optimization are required; the rotation R in the fisheye VO plays a complementary role in the pose estimation of the millimeter-wave radar, making the pose estimation more robust;

[0122] (5) The method proposed in this technical solution is based on 4D millimeter wave radar and is not applicable to lidar sensors;

[0123] (6) The SLAM method described in this technical solution is a loosely coupled framework based on 4D millimeter wave radar pose estimation and supplemented by fisheye vision estimation, which is a post-fusion processing method;

[0124] (7) The 4D millimeter-wave radar point cloud in this technical solution is obtained through multiple radar time synchronization, external parameter calibration, filtering and other operations;

[0125] (8) The fusion result of this technical solution is the millimeter wave point cloud around the smart car, not a panoramic image. The visual sensor only needs a fisheye sensor with a small viewing angle;

[0126] (9) This technical solution is based on the features of 4D millimeter-wave radar point cloud, and proposes a method based on voxel probability maximization to estimate the pose.

[0127] The device for implementing vehicle-side 4D millimeter-wave radar SLAM processing in combination with a fisheye camera, wherein the device comprises:

[0128] a processor configured to execute computer-executable instructions;

[0129] A memory stores one or more computer-executable instructions. When the computer-executable instructions are executed by the processor, the various steps of the method for realizing vehicle-side 4D millimeter-wave radar SLAM processing in combination with a fisheye camera are implemented.

[0130] The processor that implements vehicle-side 4D millimeter-wave radar SLAM processing in combination with a fisheye camera, wherein the processor is configured to execute computer-executable instructions. When the computer-executable instructions are executed by the processor, the various steps of the above-mentioned method for implementing vehicle-side 4D millimeter-wave radar SLAM processing in combination with a fisheye camera are implemented.

[0131] The computer-readable storage medium stores a computer program thereon, which can be executed by a processor to implement the various steps of the above-mentioned method for realizing vehicle-side 4D millimeter-wave radar SLAM processing in combination with a fisheye camera.

[0132] Any process or method description in a flowchart or otherwise described herein may be understood to represent a module, segment or portion of code comprising one or more executable instructions for implementing the steps of a specific logical function or process, and the scope of the preferred embodiments of the present invention includes alternative implementations in which functions may be performed out of the order shown or discussed, including performing functions in a substantially simultaneous manner or in the reverse order depending on the functions involved, which should be understood by those skilled in the art to which the embodiments of the present invention pertain.

[0133] It should be understood that each part of the present invention can be implemented by hardware, software, firmware or a combination thereof. In the above embodiments, multiple steps or methods can be implemented by software or firmware stored in a memory and executed by a suitable instruction execution device.

[0134] Those skilled in the art will understand that all or part of the steps of the method for implementing the above-mentioned embodiment can be completed by instructing the relevant hardware through a program, and the program can be stored in a computer-readable storage medium. When the program is executed, it includes one of the steps of the method embodiment or a combination thereof.

[0135] The storage medium mentioned above can be a read-only memory, a magnetic disk or an optical disk, etc.

[0136] Throughout this specification, references to terms such as "one embodiment," "some embodiments," "example," "specific example," or "embodiment" indicate that the specific features, structures, materials, or characteristics described in conjunction with that embodiment or example are included in at least one embodiment or example of the present invention. In this specification, schematic representations of the above terms do not necessarily refer to the same embodiment or example. Furthermore, the specific features, structures, materials, or characteristics described may be combined in any suitable manner in any one or more embodiments or examples.

[0137] Although the embodiments of the present invention have been shown and described above, it will be understood that the above embodiments are illustrative and are not to be construed as limitations on the present invention. A person skilled in the art may change, modify, replace and modify the above embodiments within the scope of the present invention.

[0138] This technical solution is based on the imaging characteristics and point cloud output features of 4D imaging millimeter wave radar, and the designed radar is used as the primary vision and the radar vision fusion SLAM system is supplemented by it. It has certain novelty, and its SLAM output effect is far better than that of traditional radar.

[0139] The method mentioned in this technical solution only uses a fisheye camera with a small viewing angle, rather than a more complex wide-angle panoramic camera, to complete the estimation of the rotation part of the smart car's posture, and complements the posture estimation of the millimeter-wave radar, making the posture estimation of the smart vehicle more accurate and the obtained environmental map effect better.

[0140] The SLAM method in this technical solution is essentially a loosely coupled post-processing framework based on 4D millimeter-wave radar point cloud and supplemented by fisheye images. It is not a tightly coupled pre-processing framework based on panoramic vision and supplemented by lidar in the fusion of panoramic vision and lidar. This loosely coupled framework is more robust and the system is more robust. The failure of the visual sensor will not cause the entire system to crash or fail.

[0141] The SLAM method proposed based on this technical solution obtains higher-precision posture, greatly improving the accuracy of vehicle motion planning and control, making the passengers' driving experience, comfort and safety better.

[0142] The present invention employs a method, device, processor, and computer-readable storage medium for vehicle-side 4D millimeter-wave radar SLAM processing in conjunction with a fisheye camera. Using the standard fisheye camera in intelligent driving vehicles as the VO, the method filters and fuses its rotation R with the pose estimated by pure millimeter-wave radar, resulting in more accurate pose estimation and improved map quality. This is because the fisheye camera VO method utilizes richer environmental information, and the estimated pose rotation R is more accurate than that of pure millimeter-wave radar. However, due to the limitation of a monocular camera that cannot estimate depth, the translation component of the VO is not integrated. The fisheye camera's estimated R is integrated to correct and update the pure millimeter-wave radar pose, thereby achieving higher pose estimation accuracy. This improved pose accuracy, in turn, improves the quality of map stitching. Furthermore, this technical solution has excellent adaptability and scalability. Image frame-based pose estimation can be extended to pinhole cameras, not just fisheye cameras, and can be applied to various SLAM methods for intelligent driving vehicles, resulting in more accurate pose estimation.

[0143] In this specification, the present invention has been described with reference to specific embodiments thereof. However, it will be apparent that various modifications and variations may be made without departing from the spirit and scope of the present invention. Accordingly, the specification and drawings are to be regarded as illustrative rather than restrictive.

Claims

1. A method for realizing vehicle-side 4D millimeter-wave radar SLAM processing in combination with a fisheye camera, characterized in that: The method comprises the following steps: (1) The intelligent driving vehicle collects point cloud data from the 4D millimeter-wave radar and image data from the fisheye camera in real time and preprocesses the data; (2) Performing pose estimation on the pre-processed point cloud data and image data simultaneously to obtain the current pose of the vehicle based on the 4D millimeter-wave radar and the current pose of the vehicle based on the fisheye image, respectively; (3) Perform EKF filtering on the two poses and obtain a more accurate pose after filtering; (4) Transforming the precise position of the vehicle with the point cloud key frame of the 4D millimeter wave radar to obtain the key frame point cloud in the world coordinate system, and then splicing the key frame point clouds of several frames to obtain a point cloud map; (5) Output the above-obtained precise pose and point cloud map as SLAM; The step (1) specifically includes the following steps: (1.1) Time-synchronize the data collected from each 4D millimeter-wave radar and combine them into a 360-degree point cloud around the vehicle in a sequential fusion manner; (1.2) Time-align and synchronize the 4D millimeter-wave radar point cloud data and the fisheye camera image data, and align them according to the time of the 4D millimeter-wave radar point cloud; The step (2) of performing pose estimation based on 4D millimeter wave radar specifically includes the following steps: (2.1.a) Extracting point cloud keyframes: Based on the vehicle's motion information, i.e., the distance Δd or the rotation angle Δα from the last keyframe extracted, if Δd or Δα is greater than a certain threshold θ, the current 4D millimeter-wave radar point cloud P is extracted. i {x0,x1…x m }Add as a keyframe, denoted as K i ; (2.2.a) Matching between point cloud key frames: Key frame K i and the previous keyframe K i-1 Perform normal distribution transformation ndt matching and transform the key frame K i Point cloud P i {x0,x1…x m } and keyframe K i-1 Point cloud P i-1 {x0,x1…x n } is divided into a voxel unit voxel, and the normal distribution p of all points in each voxel unit is estimated. voxel ~N(μ,σ), where μ is the mean of the normal distribution and σ is the covariance of the normal distribution; and solve a transformation T i (R i ,t i ) makes the key frame K i The point is transformed and falls to the key frame K i-1 The probability value p of the voxel cell voxel where the point is located voxel The maximum value is solved as follows: Finally, the relative translation transformation between 4D millimeter wave radar frames is obtained and relative rotation transformation (2.3.a) Use the following formula to calculate the current vehicle posture based on 4D millimeter wave radar in, is the pose estimate of the vehicle in the previous frame.

2. The method for realizing vehicle-side 4D millimeter-wave radar SLAM processing in combination with a fisheye camera according to claim 1, characterized in that: The step (2) of performing pose estimation based on the fisheye image specifically includes the following steps: (2.1.b) Extract visual features: Extract visual ORB features from each fisheye image frame, and record the features extracted from the current image frame as F i , the feature extracted from the image frame at the previous moment is recorded as F i-1 , the pose of the current frame of the fisheye image is The pose of the image frame at the previous moment is (2.2.b) Visual feature matching: Based on feature F i and F i-1 The Hamming distance between the two frames is used to find the corresponding matching points between the two frames, and the PnP is used to calculate the relative translation transformation of the current frame relative to the previous frame. and relative rotation transformation (2.3.b) Use the following formula to calculate the current pose based on the fisheye image And from the fisheye image pose Remove the rotating part and get 3. The method for realizing vehicle-side 4D millimeter-wave radar SLAM processing in combination with a fisheye camera according to claim 2, characterized in that: The step (3) is specifically as follows: To obtain the pose estimated based on 4D millimeter wave radar and pose estimation based on fisheye images The rotating part After that, the two poses are subjected to EKF filtering, specifically according to and The Kalman gain is updated based on the covariance of the posture, thereby updating the posture state and obtaining a more accurate posture X after filtering. i .

4. The method for realizing vehicle-side 4D millimeter-wave radar SLAM processing in combination with a fisheye camera according to claim 3, characterized in that: The step (4) specifically includes the following steps: (4.1) The key frame K of the 4D millimeter wave radar is extracted i The corresponding point cloud P i {x0,x1…x m } and the exact pose X i The keyframe point cloud is calculated according to the following formula Among them, the key frame point cloud The key frame point cloud in the world coordinate system is obtained by transforming to the global reference coordinate system; (4.2) Add up the transformed keyframe point clouds to form a point cloud map of the entire environment 5. The method for realizing vehicle-side 4D millimeter-wave radar SLAM processing in combination with a fisheye camera according to claim 4, characterized in that: The step (5) is specifically as follows: The exact pose X i With the point cloud map M i As SLAM output.

6. A device for implementing vehicle-side 4D millimeter-wave radar SLAM processing in combination with a fisheye camera, characterized in that: The device comprises: a processor configured to execute computer-executable instructions; A memory storing one or more computer-executable instructions, which, when executed by the processor, implement the steps of the method for implementing vehicle-side 4D millimeter-wave radar SLAM processing in combination with a fisheye camera as described in any one of claims 1 to 5.

7. A processor that combines a fisheye camera to implement vehicle-side 4D millimeter-wave radar SLAM processing, characterized in that: The processor is configured to execute computer-executable instructions. When the computer-executable instructions are executed by the processor, the various steps of the method for realizing vehicle-side 4D millimeter-wave radar SLAM processing in combination with a fisheye camera as described in any one of claims 1 to 5 are implemented.

8. A computer-readable storage medium, characterized in that A computer program is stored thereon, and the computer program can be executed by a processor to implement the various steps of the method for realizing vehicle-side 4D millimeter-wave radar SLAM processing in combination with a fisheye camera as described in any one of claims 1 to 5.

Images