Mapping method and device based on 4d millimeter wave radar and computer readable medium

Abstract

The application discloses a mapping method and device based on a 4D millimeter wave radar and a computer readable medium, the method comprising: performing noise reduction processing on 4D millimeter wave radar point clouds based on coordinate information and RCS information of the 4D millimeter wave radar point clouds in a preset sliding window; performing dynamic point cloud filtering processing on the preprocessed point clouds based on Doppler velocity to obtain static point clouds; for any current frame point cloud in the static point clouds: performing conversion processing on the current frame point cloud based on a pose conversion matrix, performing feature point registration on the converted current frame point cloud and a previous frame submap, and generating a current frame submap. In this embodiment, the RCS information and the Doppler velocity are used to perform filtering processing on the point clouds collected by the 4D millimeter wave radar; then, the static point clouds obtained after the filtering are subjected to feature point registration to form a local submap. Thus, the point clouds in a target scene can be subjected to high-precision registration, and high-precision positioning of an unmanned device can be realized.

Description

Technical Field

[0001] This invention belongs to the technical field of high-precision positioning, and particularly relates to a mapping method, device and computer-readable medium based on 4D millimeter-wave radar. Background Technology

[0002] 4D millimeter-wave radar is a multi-sensor array system based on millimeter-wave frequency bands (e.g., 24GHz, 77GHz, or 79GHz). It consists of a transmitting antenna, a receiving antenna array, a signal processing unit, and a deep learning algorithm. It is used to perform high-precision perception and imaging of the four-dimensional spatial characteristics of a target object, including distance, velocity, horizontal azimuth angle, and vertical height.

[0003] Existing lidar and visual SLAM solutions are significantly affected by the environment and cannot be used in harsh conditions such as rain, snow, and fog, while ordinary 3D millimeter-wave radar lacks altitude information. 4D millimeter-wave radar is gaining attention, but the sparsity and noise of point clouds limit its application. Traditional lidar algorithms are not applicable to 4D millimeter-wave radar. Although 4D millimeter-wave radar SLAM systems exist, they often suffer from poor positioning accuracy and fail to fully utilize the Doppler velocity and radar cross-section values ​​provided by the radar.

[0004] Therefore, in order to solve the positioning problem of unmanned equipment in some harsh environments, there is an urgent need to provide a high-precision positioning method and mapping method based on 4D millimeter-wave radar. Summary of the Invention

[0005] This invention provides a mapping method, apparatus, and computer-readable medium based on 4D millimeter-wave radar. This method enables high-precision positioning of unmanned equipment through high-precision registration of 4D millimeter-wave radar point clouds.

[0006] According to a first aspect of the present invention, a mapping method based on 4D millimeter-wave radar is provided. The method includes: acquiring a 4D millimeter-wave radar point cloud of a target scene; performing noise reduction processing on the 4D millimeter-wave radar point cloud based on the coordinate information and RCS information of the 4D millimeter-wave radar point cloud within a preset sliding window to generate a preprocessed point cloud; performing dynamic point cloud filtering processing on the preprocessed point cloud based on the Doppler velocity of the preprocessed point cloud to obtain a static point cloud; for any current frame point cloud in the static point cloud: determining the pose transformation matrix corresponding to the current frame point cloud; and performing transformation processing on the current frame point cloud based on the pose transformation matrix; and performing feature point registration between the transformed current frame point cloud and the previous frame sub-map corresponding to the adjacent previous frame point cloud to generate a current frame sub-map corresponding to the current frame point cloud.

[0007] Optionally, the step of performing noise reduction processing on the 4D millimeter-wave radar point cloud based on the coordinate information and RCS information of the 4D millimeter-wave radar point cloud within a preset sliding window to generate a preprocessed point cloud includes: for any original point in the 4D millimeter-wave radar point cloud within the preset sliding window: obtaining the height value and radar scattering interface of the original point in the spatial coordinate system; determining the shortest distance between the original point and the radar center point based on the spatial coordinate information of the original point and the spatial coordinate information of the radar center point; using the shortest distance not greater than a distance threshold, the height value greater than a height threshold, and the radar scattering interface greater than an RCS threshold as the filtering conditions for the original point; selecting the original points that meet the filtering conditions from the 4D millimeter-wave radar point cloud within the preset sliding window as preprocessed points to generate a preprocessed point cloud.

[0008] Optionally, the step of performing dynamic point cloud filtering on the preprocessed point cloud based on the Doppler velocity of the preprocessed point cloud to obtain a static point cloud includes: for any target point in the preprocessed point cloud: constructing a functional relationship between the Doppler velocity of the target point and the radar velocity based on the Doppler velocity of the target point and the unit direction vector of the target point relative to the radar; and using RANSAC iteration to remove all dynamic points in the preprocessed point cloud based on the functional relationship to obtain a static point cloud.

[0009] Optionally, determining the pose transformation matrix corresponding to the current frame point cloud includes: for any target point in the current frame point cloud: obtaining the previous frame sub-map corresponding to the previous frame point cloud adjacent to the current frame point cloud; selecting a quasi-reference point corresponding to the target point from the previous frame sub-map, and determining the combination of the quasi-reference point and the target point as the effective point pair corresponding to the target point; generating several effective point pairs based on each effective point pair corresponding to the target point in the current frame point cloud; and obtaining the pose transformation matrix corresponding to the current frame point cloud by fitting a least squares function based on the target point weight, target point spatial coordinate information, quasi-reference point spatial coordinate information, and the displacement of the optimal radar velocity in the Z direction for each of the several effective point pairs.

[0010] Optionally, the method further includes: acquiring a plurality of registration points in the previous frame sub-map; calculating the absolute value of the RCS difference between each registration point and the target point; selecting from the plurality of registration points a registration point whose absolute value of the RCS difference is not greater than a fourth preset threshold as a candidate reference point, thereby obtaining at least one candidate reference point; calculating the spatial distance between each candidate reference point and the target point; and selecting from the at least one candidate reference point the candidate reference point with the shortest spatial distance as a quasi-reference point.

[0011] Optionally, the method further includes: obtaining the maximum RCS value and the minimum RCS value corresponding to the current frame point cloud; determining the absolute value of the RCS difference between the quasi-reference point and the target point in the effective point pair; and determining the target point weight corresponding to the target point based on the maximum RCS value, the minimum RCS value, and the absolute value of the RCS difference.

[0012] Optionally, the method further includes: constructing a least-squares objective function based on the Doppler velocities of N static points in the static point cloud to obtain the optimal radar velocity of the 4D millimeter-wave radar; determining the velocity component of the optimal radar velocity in the Z direction; and determining the displacement of the optimal radar velocity in the Z direction based on the velocity component in the Z direction and the time difference between two adjacent frames of point cloud.

[0013] Optionally, the method further includes: outputting the pose trajectory of the unmanned device based on the pose transformation matrix corresponding to each current frame point cloud within a preset sliding window; and generating a radar point cloud map based on the current frame sub-map corresponding to each current frame point cloud within the preset sliding window.

[0014] According to a second aspect of the present invention, a mapping device based on 4D millimeter-wave radar is also provided. The device includes: an acquisition module for acquiring a 4D millimeter-wave radar point cloud of a target scene; a noise reduction module for performing noise reduction processing on the 4D millimeter-wave radar point cloud based on the coordinate information and RCS information of the 4D millimeter-wave radar point cloud within a preset sliding window to generate a preprocessed point cloud; a filtering processing module for performing dynamic point cloud filtering processing on the preprocessed point cloud based on the Doppler velocity of the preprocessed point cloud to obtain a static point cloud; and a feature point registration module for: determining the pose transformation matrix corresponding to the current frame point cloud for any current frame point cloud in the static point cloud; performing transformation processing on the current frame point cloud based on the pose transformation matrix; and performing feature point registration between the transformed current frame point cloud and the previous frame sub-map corresponding to the adjacent previous frame point cloud to generate a current frame sub-map corresponding to the current frame point cloud.

[0015] According to a third aspect of the present invention, an electronic device is also provided, comprising: a processor; a memory for storing executable instructions of the processor; the processor being configured to read the executable instructions from the memory and execute the instructions to implement the method as described in the first aspect.

[0016] According to a fourth aspect of the present invention, a computer-readable medium is also provided, on which a computer program is stored, wherein the program, when executed by a processor, implements the method described in the first aspect.

[0017] This invention provides a mapping method, apparatus, and computer-readable medium based on 4D millimeter-wave radar. The method includes at least the following steps: First, acquiring a 4D millimeter-wave radar point cloud of a target scene; second, performing noise reduction processing on the 4D millimeter-wave radar point cloud based on the coordinate information and RCS information of the 4D millimeter-wave radar point cloud within a preset sliding window to generate a preprocessed point cloud; then, performing dynamic point cloud filtering processing on the preprocessed point cloud based on the Doppler velocity of the preprocessed point cloud to obtain a static point cloud; finally, for any current frame point cloud in the static point cloud: determining the pose transformation matrix corresponding to the current frame point cloud; and performing coordinate system transformation processing on the current frame point cloud based on the pose transformation matrix; and performing feature point registration between the transformed current frame point cloud and the previous frame sub-map corresponding to the adjacent previous frame point cloud to generate the current frame sub-map corresponding to the current frame point cloud. This embodiment filters out noise points in the 4D millimeter-wave radar point cloud based on RCS information, and then performs dynamic point filtering on the preprocessed point cloud based on Doppler velocity to obtain a static point cloud; finally, it performs registration based on the static point cloud and accumulates to form a local sub-map; thus, high-precision positioning of unmanned equipment is achieved through high-precision registration of point clouds in the target scene. Attached Figure Description

[0018] The following sections will describe some specific embodiments of the invention in a detailed manner by way of example and not limitation, with reference to the accompanying drawings. The same reference numerals in the drawings denote the same or similar parts or portions. Those skilled in the art should understand that these drawings are not necessarily drawn to scale. In the drawings:

[0019] Figure 1 This is a flowchart illustrating a mapping method based on 4D millimeter-wave radar according to an embodiment of the present invention.

[0020] Figure 2 This is a flowchart illustrating the process of determining the pose transformation matrix corresponding to the point cloud in the current frame in one embodiment of the present invention;

[0021] Figure 3 This is a schematic diagram of a mapping device based on 4D millimeter-wave radar, provided as an embodiment of the present invention. Detailed Implementation

[0022] To make the objectives, features, and advantages of this invention more apparent and understandable, the technical solutions of the embodiments of this invention will be clearly and completely described below with reference to the accompanying drawings. Obviously, the described embodiments are only a part of the embodiments of this invention, and not all of them. All other embodiments obtained by those skilled in the art based on the embodiments of this invention without creative effort are within the scope of protection of this invention.

[0023] The radar cross section (RCS) is a physical quantity that describes a target's ability to reflect radar waves, characterizing the intensity of the echo generated by the target under radar wave illumination.

[0024] Doppler velocity is the radial velocity of a target relative to the radar, measured based on the Doppler effect; that is, the velocity component of the target along the direction of radar wave propagation.

[0025] like Figure 1 The diagram shown is a flowchart illustrating a mapping method based on 4D millimeter-wave radar according to an embodiment of the present invention.

[0026] A mapping method based on 4D millimeter-wave radar includes at least the following steps:

[0027] S101, acquire 4D millimeter-wave radar point cloud of the target scene;

[0028] S102, Based on the coordinate information and RCS information of the 4D millimeter-wave radar point cloud within the preset sliding window, the 4D millimeter-wave radar point cloud is denoised to generate a pre-processed point cloud.

[0029] S103, Based on the Doppler velocity of the preprocessed point cloud, perform dynamic point cloud filtering on the preprocessed point cloud to obtain a static point cloud;

[0030] S104, for any current frame point cloud in the static point cloud: determine the pose transformation matrix corresponding to the current frame point cloud; and perform transformation processing on the current frame point cloud based on the pose transformation matrix; perform feature point registration between the transformed current frame point cloud and the previous frame sub-map corresponding to the adjacent previous frame point cloud to generate the current frame sub-map corresponding to the current frame point cloud.

[0031] In S101, 4D millimeter-wave radar point clouds of the target scene are acquired using 4D millimeter-wave radar.

[0032] In S102, based on the coordinate information and RCS information of the 4D millimeter-wave radar point cloud within the preset sliding window, the 4D millimeter-wave radar point cloud is subjected to noise reduction processing based on preset rules or existing algorithm models to generate a pre-processed point cloud.

[0033] For example, the step of performing noise reduction processing on the 4D millimeter-wave radar point cloud based on the coordinate information and RCS information of the 4D millimeter-wave radar point cloud within a preset sliding window to generate a preprocessed point cloud includes: for any original point in the 4D millimeter-wave radar point cloud within the preset sliding window: obtaining the height value and radar scattering interface of the original point in the spatial coordinate system; determining the shortest distance between the original point and the radar center point based on the spatial coordinate information of the original point and the spatial coordinate information of the radar center point; using the shortest distance not greater than a distance threshold, the height value greater than a height threshold, and the radar scattering interface greater than an RCS threshold as the filtering conditions for the original point; selecting the original points that meet the filtering conditions from the 4D millimeter-wave radar point cloud within the preset sliding window as preprocessed points to generate a preprocessed point cloud.

[0034] Specifically, for any original point in the 4D millimeter-wave radar point cloud within a preset sliding window: obtain the height value of the original point in the spatial coordinate system and the radar scattering interface; select original points from the 4D millimeter-wave radar point cloud whose height value is greater than a height threshold and whose radar scattering interface is greater than an RCS threshold as candidate points; generate a candidate point cloud; for any candidate point in the candidate point cloud: based on the spatial coordinate information of the candidate point and the spatial coordinate information of the radar center point, determine the shortest distance between the candidate point and the radar center point; select candidate points from the candidate point cloud whose shortest distance is not greater than a distance threshold as preprocessed points, and generate a preprocessed point cloud. Alternatively, for any original point in the 4D millimeter-wave radar point cloud within a preset sliding window: based on the spatial coordinate information of the original point and the spatial coordinate information of the radar center point, determine the shortest distance between the original point and the radar center point; select original points in the 4D millimeter-wave radar point cloud whose shortest distance is not greater than a distance threshold as candidate points to generate a candidate point cloud; for any candidate point in the candidate point cloud: obtain the height value of the candidate point in the spatial coordinate system and the radar scattering interface; select candidate starting points in the candidate point cloud whose height value is greater than a height threshold and whose radar scattering interface is greater than an RCS threshold as preprocessed points to generate a preprocessed point cloud.

[0035] For example: 4D millimeter-wave radar point cloud P = {p i}, i∈{1,2,...,n}, where n is the number of target points in the 4D millimeter-wave radar point cloud; each target point is represented as p in the radar coordinate system. i =(x i y i ,z i Doppler, RCS)

[0036] Where, x i y i , z iThe location information of the target point is represented by Doppler, where Doppler represents the Doppler velocity of the target point, and RCS represents the radar cross section of the target point.

[0037] Here, the height threshold, distance threshold, and RCS threshold are all empirical values.

[0038] This embodiment can effectively remove noise points below the ground and in the sky by setting height and distance thresholds, thereby obtaining effective points within the observation range of 4D millimeter-wave radar; by setting RCS threshold, it can filter point clouds with deterministic characteristics, improve the quality of point cloud data, and thus facilitate high-precision positioning of unmanned equipment.

[0039] In S103, based on the Doppler velocity of the preprocessed point cloud, dynamic point cloud filtering is performed on the preprocessed point cloud according to preset rules or existing algorithm models to obtain a static point cloud.

[0040] For example, the step of performing dynamic point cloud filtering on the preprocessed point cloud based on the Doppler velocity of the preprocessed point cloud to obtain a static point cloud includes: for any target point in the preprocessed point cloud: constructing a functional relationship between the Doppler velocity of the target point and the radar velocity based on the Doppler velocity of the target point and the unit direction vector of the target point relative to the radar; and using RANSAC iteration to remove all dynamic points in the preprocessed point cloud based on the functional relationship to obtain a static point cloud.

[0041] For example: S1, construct the radar velocity function relationship of the target point.

[0042] For the i-th target point in the preprocessed point cloud, the radar velocity function relationship of the target point is constructed based on the Doppler velocity of the target point and the unit direction vector of the target point relative to the radar, as shown in Equation (1).

[0043] v d,i =-(u i ) T v r

[0044] Equation (1);

[0045] Among them, v d，i It is the Doppler velocity of the i-th target point. It is the unit direction vector of the target point relative to the radar, v r It is the radar velocity, including components in the x, y, and z axes.

[0046] S2, Fitting the initial model

[0047] Three target points are randomly sampled from the preprocessed point cloud, and these three target points are assumed to be static points. Substitute the three static points into equation (1) to construct the initial model, as shown in equation (2):

[0048]

[0049] Let matrix The initial model can then be written as If A is full rank (i.e., the three target points are not collinear), then the candidate radar velocity can be obtained by matrix calculation.

[0050] S3, Dynamic Point Removal

[0051] For any target point in the preprocessed point cloud: based on candidate radar velocity Based on formula (1), predict the Doppler velocity of the target point and generate the predicted Doppler velocity. As shown in equation (3).

[0052]

[0053] Predicting Doppler velocity Compared with the radar-measured Doppler velocity υ d，i If a comparison is made, If the target point conforms to the initial model, then the target point is determined to be a static point; if The target point does not conform to the initial model, therefore the target point is determined to be a dynamic point.

[0054] Count the number of static points obtained in this iteration.

[0055] S4. Repeat steps S1 to S3, iterating at most k times, and record the number of static points in each iteration. After the iteration, select the candidate radar velocity corresponding to the iteration with the most static points as the quasi-radar velocity. Based on the quasi-radar velocity, remove all dynamic points and retain the set of static points corresponding to the quasi-radar velocity. The set of static points contains the unit direction vector and Doppler velocity corresponding to the static point.

[0056] Therefore, in this embodiment, based on the functional relationship between the Doppler velocity and the radar velocity of the target point, all dynamic points in the preprocessed point cloud are eliminated through RANSAC iteration to obtain a static point cloud.

[0057] In S104, for example: at the initial moment, the first frame point cloud is used as the previous frame sub-map; the second frame point cloud is used as the current frame point cloud. Based on the first and second frame point clouds, the pose transformation matrix corresponding to the second frame point cloud is determined according to preset rules or algorithm models; the pose transformation matrix is ​​used to transform the second frame point cloud to the coordinate system corresponding to the first frame point cloud, generating the transformed second frame point cloud. Feature point registration is performed between the transformed second frame point cloud and the first frame point cloud to obtain the second frame sub-map composed of several registration points. Then, the third frame point cloud is used as the current frame point cloud, and the second frame sub-map is used as the previous frame sub-map, repeating the above steps to generate the pose transformation matrix corresponding to the third frame sub-map and the third frame point cloud.

[0058] Based on the pose transformation matrix corresponding to the point cloud of each current frame within the preset sliding window, the pose trajectory of the unmanned device is output; based on the sub-map of the current frame corresponding to the point cloud of each current frame within the preset sliding window, a radar point cloud map is generated.

[0059] This embodiment filters out noise points in the 4D millimeter-wave radar point cloud based on RCS information, and then performs dynamic point filtering on the preprocessed point cloud based on Doppler velocity to obtain a static point cloud; finally, it performs registration based on the static point cloud and accumulates to form a local sub-map; thus, high-precision positioning of unmanned equipment is achieved through high-precision registration of point clouds in the target scene; and it solves the problem of poor positioning accuracy of the existing 4D millimeter-wave radar SLAM system in harsh environments such as rain, snow and smoke.

[0060] like Figure 2 The diagram shown is a flowchart illustrating the process of determining the pose transformation matrix corresponding to the point cloud in the current frame in one embodiment of the present invention.

[0061] Determining the pose transformation matrix corresponding to the point cloud in the current frame includes at least the following steps:

[0062] S201, For any target point in the current frame point cloud: obtain the previous frame sub-map corresponding to the previous frame point cloud adjacent to the current frame point cloud; select the quasi-reference point corresponding to the target point from the previous frame sub-map, and determine the combination of the quasi-reference point and the target point as the valid point pair corresponding to the target point;

[0063] S202, Based on the valid point pairs corresponding to each target point in the current frame point cloud, generate several valid point pairs;

[0064] S203, based on the target point weight, target point spatial coordinate information, quasi-reference point spatial coordinate information, and the displacement of the optimal radar velocity in the Z direction of each effective point pair, the pose transformation matrix corresponding to the current frame point cloud is obtained by fitting the least squares function.

[0065] In S201, for example, several registration points in the previous frame sub-map are obtained; the absolute value of the RCS difference between each registration point and the target point is calculated; registration points whose absolute value of the RCS difference is not greater than a fourth preset threshold are selected from the several registration points as candidate reference points to obtain at least one candidate reference point; the spatial distance between each candidate reference point and the target point is calculated; and the candidate reference point with the shortest spatial distance is selected from the at least one candidate reference point as a quasi-reference point.

[0066] For example: P represents the point cloud of the current frame, and Q represents the sub-map of the previous frame; p i Let q be the target point. i For registration points; p i ∈P,q i ∈Q, i, j = 1, 2, ...

[0067] p i With q i The absolute value of the RCS difference between them is shown in equation (4).

[0068]

[0069] p i With q i The spatial distance between them is shown in equation (5).

[0070] d i =||q i -p i ||2 Equation (5).

[0071] In S203, the weight of the target point is determined based on the attention mechanism or the RCS value.

[0072] For example, the maximum and minimum RCS values ​​corresponding to the current frame point cloud are obtained; the absolute value of the RCS difference between the quasi-reference point and the target point in the effective point pair is determined; and the target point weight corresponding to the target point is determined based on the maximum RCS value, the minimum RCS value, and the absolute value of the RCS difference.

[0073] For example: target point p i The corresponding target point weight ω i The calculation formula is shown in equation (6) below.

[0074]

[0075] in, These represent the minimum and maximum RCS values ​​in the current frame's point cloud, respectively.

[0076] For example, based on the Doppler velocities of N static points in the static point cloud, a least-squares objective function is constructed to obtain the optimal radar velocity of the 4D millimeter-wave radar; the velocity component of the optimal radar velocity in the Z direction is determined; based on the velocity component in the Z direction and the time difference between two adjacent frames of point cloud, the displacement of the optimal radar velocity in the Z direction is determined.

[0077] For example: Selecting N static points from a static point cloud, theoretically these points should all satisfy... However, radar measurements are subject to noise (such as v). d，i Measurement error, u i (Angle error), in practice (Slight errors exist.)

[0078] Construct the least squares objective function as shown in equation (7);

[0079]

[0080] The core of constructing the least squares objective function is to find an optimal radar velocity v. r This minimizes the sum of squared errors at all static points, thereby offsetting the effects of measurement noise.

[0081] Displacement of optimal radar velocity in the Z direction in, For v r The component in direction, Δt, is the time interval between two adjacent frame point clouds.

[0082] Therefore, by first separating the dynamic and static points through RANSAC, and then minimizing the error of the static points through least squares fitting, the optimal radar speed is inferred. The whole process makes full use of the "four-dimensional detection capability" (speed, distance, horizontal angle, and vertical angle) of the 4D radar, and has strong robustness to dynamic interference (such as pedestrians and vehicles) and measurement noise.

[0083] When performing point cloud registration for lidar point clouds using existing methods, it is difficult to register the observation points in the Z direction. Therefore, this embodiment innovatively proposes to constrain the distance in the Z direction when constructing the least squares function, which can effectively improve the accuracy of point cloud registration.

[0084] Determine the valid point pairs corresponding to the current frame point cloud and the previous frame sub-map to obtain n valid point pairs. Based on the target point weight, target point spatial coordinate information, quasi-reference point spatial coordinate information, and the displacement of the optimal radar velocity in the Z direction for each of the n valid point pairs, construct a weight-based least squares function as shown in Equation (8).

[0085]

[0086] Among them, T i This represents the transformation relationship between the point cloud in the current frame and the sub-map in the previous frame. For T i The component related to displacement in the Z direction.

[0087] The pose transformation matrix corresponding to the point cloud of the current frame is obtained by fitting the formula (8).

[0088] Using the pose transformation matrix T corresponding to the point cloud in each current frame i N consecutive frames of scanned point clouds are aligned and accumulated to form a local sub-map. When the number of scanned frames exceeds N, a sliding window mechanism is used to remove the oldest scanned point cloud and add the latest scanned point cloud to ensure that the sub-map is always composed of the latest N frames of point clouds. The sub-map is then updated using a registration-optimized pose transformation matrix to improve the map's accuracy and consistency.

[0089] This embodiment introduces RCS information as a weighting factor for point-pair matching and constructs a weighted least squares function by combining it with distance constraints in the Z direction. This provides a quantitative calculation framework for point cloud registration, effectively solving the problems of single weights and insufficient constraints in point-pair matching, and improving the accuracy of point cloud registration. By dynamically adjusting the weights of point pairs through RCS differences, the weight allocation is made more suitable for the characteristics of different targets, further enhancing the robustness and adaptability of the registration algorithm. This ensures that the algorithm can still run stably in scenarios with diverse target characteristics, thereby improving the accuracy of sub-map construction and facilitating high-precision positioning of unmanned equipment. It also solves the problem of poor positioning accuracy in existing 4D millimeter-wave radar SLAM systems under harsh environments such as rain, snow, and fog.

[0090] The following section provides a detailed description of a mapping method based on 4D millimeter-wave radar provided in this embodiment, using specific application scenarios as examples.

[0091] A mapping method based on 4D millimeter-wave radar includes at least the following steps:

[0092] S1, acquire the 4D millimeter-wave radar point cloud of the target scene.

[0093] S2, for any original point in the 4D millimeter-wave radar point cloud within a preset sliding window: obtain the height value of the original point in the spatial coordinate system and the radar scattering interface; based on the spatial coordinate information of the original point and the spatial coordinate information of the radar center point, determine the shortest distance between the original point and the radar center point. Use the following criteria for original points: the shortest distance must not exceed a distance threshold, the height value must exceed a height threshold, and the radar scattering interface must exceed an RCS threshold. Select original points that meet the selection criteria from the 4D millimeter-wave radar point cloud within the preset sliding window as preprocessed points to generate a preprocessed point cloud.

[0094] S3. For any target point in the preprocessed point cloud: Based on the Doppler velocity of the target point and the unit direction vector of the target point relative to the radar, construct the functional relationship between the Doppler velocity of the target point and the radar velocity; Based on the functional relationship, use RANSAC iteration to remove all dynamic points in the preprocessed point cloud to obtain the static point cloud.

[0095] S4, for any target point in any current frame point cloud in the static point cloud: obtain the previous frame sub-map corresponding to the previous frame point cloud adjacent to the current frame point cloud; obtain several registration points in the previous frame sub-map; calculate the absolute value of the RCS difference between each registration point and the target point; select registration points whose absolute value of the RCS difference is not greater than a fourth preset threshold from the several registration points as candidate reference points, and obtain at least one candidate reference point; calculate the spatial distance between each candidate reference point and the target point; select the candidate reference point with the shortest spatial distance from the at least one candidate reference point as a quasi-reference point, and determine the combination of the quasi-reference point and the target point as the effective point pair corresponding to the target point; generate several effective point pairs based on the effective point pairs corresponding to each target point in the current frame point cloud.

[0096] S5, for any valid point pair among the plurality of valid point pairs: obtain the maximum RCS value and the minimum RCS value corresponding to the point cloud of the current frame; determine the absolute value of the RCS difference between the reference point and the target point in the valid point pair; and determine the target point weight corresponding to the target point based on the maximum RCS value, the minimum RCS value, and the absolute value of the RCS difference.

[0097] S6. Based on the Doppler velocities of N static points in the static point cloud, construct a least-squares objective function to obtain the optimal radar velocity of the 4D millimeter-wave radar; determine the velocity component of the optimal radar velocity in the Z direction; based on the velocity component in the Z direction and the time difference between two adjacent frames of point cloud, determine the displacement of the optimal radar velocity in the Z direction.

[0098] S7. Based on the target point weight, target point spatial coordinate information, quasi-reference point spatial coordinate information, and the displacement of the optimal radar velocity in the Z direction for each effective point pair, the pose transformation matrix corresponding to the current frame point cloud is obtained by fitting the least squares function.

[0099] S8. Based on the pose transformation matrix, perform coordinate system transformation on the current frame point cloud; perform feature point registration between the transformed current frame point cloud and the previous frame sub-map corresponding to the adjacent previous frame point cloud to generate the current frame sub-map corresponding to the current frame point cloud.

[0100] S9, based on the pose transformation matrix corresponding to the point cloud of each current frame within the preset sliding window, outputs the pose trajectory corresponding to the unmanned device;

[0101] S10, Generate a radar point cloud map based on the current frame sub-map corresponding to each current frame point cloud in a preset sliding window.

[0102] This embodiment uses a point cloud registration method based on RCS and Doppler velocity assistance. First, it uses RCS information to filter out noise and insignificant points in the point cloud. Then, it obtains the radar velocity based on the Doppler velocity of the point cloud, thus providing prior information for point cloud registration. Finally, it uses the magnitude of the RCS value to assign weights to point pairs during registration, thereby achieving high-precision registration of the point cloud of the target scene and ultimately realizing high-precision positioning of unmanned equipment.

[0103] like Figure 3 The diagram shown is a schematic representation of a mapping device based on 4D millimeter-wave radar according to an embodiment of the present invention.

[0104] A mapping device based on 4D millimeter-wave radar, the device 300 comprising at least: an acquisition module 301 for acquiring 4D millimeter-wave radar point clouds of a target scene; a noise reduction module 302 for performing noise reduction processing on the 4D millimeter-wave radar point clouds based on coordinate information and RCS information within a preset sliding window to generate a preprocessed point cloud; a filtering processing module 303 for performing dynamic point cloud filtering processing on the preprocessed point cloud based on the Doppler velocity of the preprocessed point cloud to obtain a static point cloud; and a feature point registration module 304 for, for any current frame point cloud in the static point cloud: determining the pose transformation matrix corresponding to the current frame point cloud; performing coordinate system transformation processing on the current frame point cloud based on the pose transformation matrix; and performing feature point registration between the transformed current frame point cloud and the previous frame sub-map corresponding to the adjacent previous frame point cloud to generate a current frame sub-map corresponding to the current frame point cloud.

[0105] In a preferred embodiment of this example, the noise reduction module includes: a first determining unit, configured to: acquire the height value of the original point in the spatial coordinate system and the radar scattering interface for any original point in the 4D millimeter-wave radar point cloud within a preset sliding window; determine the shortest distance between the original point and the radar center point based on the spatial coordinate information of the original point and the spatial coordinate information of the radar center point; and a first selecting unit, configured to use the shortest distance not greater than a distance threshold, the height value greater than a height threshold, and the radar scattering interface greater than an RCS threshold as the filtering conditions for the original point; and select the original points that meet the filtering conditions from the 4D millimeter-wave radar point cloud within the preset sliding window as preprocessed points to generate a preprocessed point cloud.

[0106] In a preferred embodiment of this example, the filtering processing module includes: a construction unit, configured to, for any target point in the preprocessed point cloud, construct a functional relationship between the Doppler velocity of the target point and the radar velocity based on the Doppler velocity of the target point and the unit direction vector of the target point relative to the radar; and a filtering unit, configured to, based on the functional relationship, use RANSAC iteration to remove all dynamic points in the preprocessed point cloud to obtain a static point cloud.

[0107] In a preferred embodiment of this example, the feature point registration module includes: a third determining unit, configured to, for any target point in the current frame point cloud,: acquire a previous frame sub-map corresponding to the previous frame point cloud adjacent to the current frame point cloud; select a quasi-reference point corresponding to the target point from the previous frame sub-map, and determine the combination of the quasi-reference point and the target point as a valid point pair corresponding to the target point; a generating unit, configured to generate several valid point pairs based on each valid point pair corresponding to the target point in the current frame point cloud; and a fitting unit, configured to, based on the target point weight, target point spatial coordinate information, quasi-reference point spatial coordinate information, and the displacement of the optimal radar velocity in the Z direction of each of the several valid point pairs, obtain the pose transformation matrix corresponding to the current frame point cloud by fitting it with a least squares function.

[0108] In a preferred embodiment of this example, the third determining unit further includes: an acquisition subunit, used to acquire a plurality of registration points in the previous frame sub-map; a first calculation subunit, used to calculate the absolute value of the RCS difference between each registration point and the target point; a first selection subunit, used to select from the plurality of registration points a registration point whose absolute value of the RCS difference is not greater than a fourth preset threshold as a candidate reference point, thereby obtaining at least one candidate reference point; a second calculation subunit, used to calculate the spatial distance between each candidate reference point and the target point; and a second selection subunit, used to select from the at least one candidate reference point the candidate reference point with the shortest spatial distance as a quasi-reference point.

[0109] In a preferred embodiment of this example, the feature point registration module further includes: a first acquisition unit, used to acquire the maximum RCS value and the minimum RCS value corresponding to the current frame point cloud; a fourth determination unit, used to determine the absolute value of the RCS difference between the quasi-reference point and the target point in the effective point pair; and a fifth determination unit, used to determine the target point weight corresponding to the target point based on the maximum RCS value, the minimum RCS value, and the absolute value of the RCS difference.

[0110] In a preferred embodiment of this example, the feature point registration module further includes: a construction unit, used to construct a least-squares objective function based on the Doppler velocities of N static points in the static point cloud to obtain the optimal radar velocity of the 4D millimeter-wave radar; a sixth determination unit, used to determine the velocity component of the optimal radar velocity in the Z direction; and a seventh determination unit, used to determine the displacement of the optimal radar velocity in the Z direction based on the velocity component in the Z direction and the time difference between two adjacent frames of point cloud.

[0111] In a preferred embodiment of this invention, the device further includes: an output module, configured to output the pose trajectory of the unmanned device based on the pose transformation matrix corresponding to each current frame point cloud within a preset sliding window; and a generation module, configured to generate a radar point cloud map based on the current frame sub-map corresponding to each current frame point cloud within the preset sliding window.

[0112] The above-described mapping apparatus can execute a mapping method based on 4D millimeter-wave radar provided in an embodiment of the present invention, and has the corresponding functional modules and beneficial effects for executing a mapping method based on 4D millimeter-wave radar. Technical details not described in detail in this embodiment can be found in the mapping method based on 4D millimeter-wave radar provided in an embodiment of the present invention.

[0113] The present invention also provides an electronic device, comprising: a processor; a memory for storing executable instructions of the processor; the processor being configured to read the executable instructions from the memory and execute the instructions to implement a mapping method based on 4D millimeter-wave radar as described in the present invention.

[0114] In addition to the methods and apparatus described above, embodiments of this application may also be computer program products, which include computer program instructions that, when executed by a processor, cause the processor to perform the steps in the methods according to various embodiments of this application described in the "Exemplary Methods" section above.

[0115] The computer program product can be written in any combination of one or more programming languages ​​to perform the operations of the embodiments of this application. The programming languages ​​include object-oriented programming languages ​​such as Java and C++, as well as conventional procedural programming languages ​​such as C or similar languages. The program code can be executed entirely on the user's computing device, partially on the user's computing device, as a standalone software package, partially on the user's computing device and partially on a remote computing device, or entirely on a remote computing device or server.

[0116] Furthermore, embodiments of this application may also be computer-readable storage media storing computer program instructions thereon, which, when executed by a processor, cause the processor to perform the steps in the methods according to the following embodiments of this application described in the "Exemplary Methods" section above.

[0117] The computer-readable storage medium may be any combination of one or more readable media. A readable medium may be a readable signal medium or a readable storage medium. A readable storage medium may, for example, include, but is not limited to, electrical, magnetic, optical, electromagnetic, infrared, or semiconductor systems, apparatuses, or devices, or any combination thereof. More specific examples of readable storage media (a non-exhaustive list) include: electrical connections having one or more wires, portable disks, hard disks, random access memory (RAM), read-only memory (ROM), erasable programmable read-only memory (EPROM or flash memory), optical fibers, portable compact disk read-only memory (CD-ROM), optical storage devices, magnetic storage devices, or any suitable combination thereof.

[0118] The basic principles of this application have been described above with reference to specific embodiments. However, it should be noted that the advantages, benefits, and effects mentioned in this application are merely examples and not limitations, and should not be considered as essential features of each embodiment of this application. Furthermore, the specific details disclosed above are for illustrative and facilitative purposes only, and are not limitations. These details do not limit the application to the necessity of employing the aforementioned specific details for implementation.

[0119] The block diagrams of devices, apparatuses, devices, and systems involved in this application are merely illustrative examples and are not intended to require or imply that they must be connected, arranged, or configured in the manner shown in the block diagrams. As those skilled in the art will recognize, these devices, apparatuses, devices, and systems can be connected, arranged, and configured in any manner. Words such as “comprising,” “including,” “having,” etc., are open-ended terms meaning “including but not limited to,” and are used interchangeably with them. The terms “or” and “and” as used herein refer to the terms “and / or,” and are used interchangeably with them unless the context clearly indicates otherwise. The term “such as” as used herein refers to the phrase “such as but not limited to,” and is used interchangeably with it.

[0120] It should also be noted that in the apparatus, equipment, and methods of this application, the components or steps can be disassembled and / or recombined. These disassemblies and / or recombinations should be considered as equivalent solutions of this application.

[0121] The above description of the disclosed aspects is provided to enable any person skilled in the art to make or use this application. Various modifications to these aspects will be readily apparent to those skilled in the art, and the general principles defined herein can be applied to other aspects without departing from the scope of this application. Therefore, this application is not intended to be limited to the aspects shown herein, but rather to be accorded the widest scope consistent with the principles and novel features disclosed herein.

[0122] The above description has been given for purposes of illustration and description. Furthermore, this description is not intended to limit the embodiments of this application to the forms disclosed herein. Although numerous exemplary aspects and embodiments have been discussed above, those skilled in the art will recognize certain variations, modifications, alterations, additions, and sub-combinations thereof.

[0123] In the description of this specification, references to terms such as "one embodiment," "some embodiments," "example," "specific example," or "some examples," etc., indicate that a specific feature, structure, material, or characteristic described in connection with that embodiment or example is included in at least one embodiment or example of the present invention. Furthermore, the specific features, structures, materials, or characteristics described may be combined in any suitable manner in one or more embodiments or examples. Moreover, without contradiction, those skilled in the art can combine and integrate the different embodiments or examples described in this specification, as well as the features of those different embodiments or examples.

[0124] Furthermore, the terms "first" and "second" are used for descriptive purposes only and should not be construed as indicating or implying relative importance or implicitly specifying the number of technical features indicated. Thus, a feature defined as "first" or "second" may explicitly or implicitly include at least one of that feature. In the description of this invention, "a plurality of" means two or more, unless otherwise explicitly specified.

[0125] The above description is merely a specific embodiment of the present invention, but the scope of protection of the present invention is not limited thereto. Any variations or substitutions that can be easily conceived by those skilled in the art within the technical scope disclosed in the present invention should be included within the scope of protection of the present invention. Therefore, the scope of protection of the present invention should be determined by the scope of the claims.

Claims

1. A 4D millimeter wave radar based mapping method, characterized in that, include: Acquire 4D millimeter-wave radar point cloud of the target scene; Based on the coordinate information and RCS information of the 4D millimeter-wave radar point cloud within a preset sliding window, the 4D millimeter-wave radar point cloud is subjected to noise reduction processing to generate a pre-processed point cloud. Based on the Doppler velocity of the preprocessed point cloud, dynamic point cloud filtering is performed on the preprocessed point cloud to obtain a static point cloud. For any current frame point cloud in the static point cloud: determine the pose transformation matrix corresponding to the current frame point cloud; and perform coordinate system transformation processing on the current frame point cloud based on the pose transformation matrix; perform feature point registration between the transformed current frame point cloud and the previous frame sub-map corresponding to the adjacent previous frame point cloud to generate the current frame sub-map corresponding to the current frame point cloud. Determining the pose transformation matrix corresponding to the current frame point cloud includes: For any target point in the current frame point cloud: obtain the previous frame sub-map corresponding to the previous frame point cloud adjacent to the current frame point cloud; A quasi-reference point corresponding to the target point is selected from the previous frame sub-map, and the combination of the quasi-reference point and the target point is determined as the effective point pair corresponding to the target point; based on the effective point pairs corresponding to each target point in the current frame point cloud, several effective point pairs are generated; based on the target point weight, target point spatial coordinate information, quasi-reference point spatial coordinate information, and the displacement of the optimal radar velocity in the Z direction of each of the several effective point pairs, the pose transformation matrix corresponding to the current frame point cloud is obtained by fitting with a least squares function.

2. The method of claim 1, wherein, The method involves performing noise reduction processing on the 4D millimeter-wave radar point cloud based on the coordinate information and RCS information within a preset sliding window to generate a pre-processed point cloud; including: For any original point in the 4D millimeter-wave radar point cloud within a preset sliding window: obtain the height value of the original point in the spatial coordinate system and the radar scattering interface; based on the spatial coordinate information of the original point and the spatial coordinate information of the radar center point, determine the shortest distance between the original point and the radar center point. The criteria for selecting the original points are: the shortest distance not greater than a distance threshold, the height value greater than a height threshold, and the radar scattering interface greater than an RCS threshold. The original points that meet the criteria are selected from the 4D millimeter-wave radar point cloud within the preset sliding window as preprocessed points to generate the preprocessed point cloud.

3. The method of claim 1, wherein, The step of performing dynamic point cloud filtering on the preprocessed point cloud based on the Doppler velocity of the preprocessed point cloud to obtain a static point cloud includes: For any target point in the preprocessed point cloud: based on the Doppler velocity of the target point and the unit direction vector of the target point relative to the radar, construct a functional relationship between the Doppler velocity of the target point and the radar velocity; Based on the aforementioned functional relationship, RANSAC iteration is used to remove all dynamic points in the preprocessed point cloud to obtain a static point cloud.

4. The method of claim 1, wherein, Also includes: Obtain several registration points from the previous frame sub-map; Calculate the absolute value of the RCS difference between each registration point and the target point; From the plurality of registration points, registration points whose absolute value of the RCS difference is not greater than a fourth preset threshold are selected as candidate reference points to obtain at least one candidate reference point; Calculate the spatial distance between each candidate reference point and the target point; The candidate reference point with the shortest spatial distance from the at least one candidate reference point is selected as the quasi-reference point.

5. The method of claim 1, wherein, Also includes: Obtain the maximum RCS value and minimum RCS value corresponding to the point cloud of the current frame; Determine the absolute value of the RCS difference between the quasi-reference point and the target point in the effective point pair; The target point weight corresponding to the target point is determined based on the maximum RCS value, the minimum RCS value, and the absolute value of the RCS difference.

6. The method of claim 1, wherein, Also includes: Based on the Doppler velocities of N static points in the static point cloud, a least-squares objective function is constructed to obtain the optimal radar velocity for the 4D millimeter-wave radar. Determine the velocity component of the optimal radar velocity in the Z direction; Based on the velocity component in the Z direction and the time difference between two adjacent point cloud frames, the displacement of the optimal radar velocity in the Z direction is determined.

7. The method according to claim 1, characterized in that, Also includes: Based on the pose transformation matrix corresponding to each current frame point cloud within a preset sliding window, the pose trajectory of the unmanned device is output. A radar point cloud map is generated based on the current frame sub-map corresponding to each current frame point cloud in the preset sliding window.

8. A mapping device based on 4D millimeter-wave radar, characterized in that, include: The acquisition module is used to acquire 4D millimeter-wave radar point clouds of the target scene; The noise reduction module is used to perform noise reduction processing on the 4D millimeter-wave radar point cloud based on the coordinate information and RCS information of the 4D millimeter-wave radar point cloud within a preset sliding window, and generate a pre-processed point cloud. The filtering processing module is used to perform dynamic point cloud filtering processing on the preprocessed point cloud based on the Doppler velocity of the preprocessed point cloud to obtain a static point cloud; The feature point registration module is used to: determine the pose transformation matrix corresponding to any current frame point cloud in the static point cloud; and perform transformation processing on the current frame point cloud based on the pose transformation matrix. The transformed current frame point cloud is registered with the previous frame sub-map corresponding to the adjacent previous frame point cloud to generate the current frame sub-map corresponding to the current frame point cloud. The feature point registration module includes: a third determining unit, used for obtaining, for any target point in the current frame point cloud: the previous frame sub-map corresponding to the previous frame point cloud adjacent to the current frame point cloud; A quasi-reference point corresponding to the target point is selected from the previous frame sub-map, and the combination of the quasi-reference point and the target point is determined as the effective point pair corresponding to the target point; a generation unit is used to generate several effective point pairs based on each effective point pair corresponding to the target point in the current frame point cloud; a fitting unit is used to obtain the pose transformation matrix corresponding to the current frame point cloud by fitting the target point weight, target point spatial coordinate information, quasi-reference point spatial coordinate information, and the displacement of the optimal radar velocity in the Z direction of each of the several effective point pairs through a least squares function.

9. A computer-readable medium having a computer program stored thereon, which, when executed by a processor, implements the method as described in any one of claims 1-7.

Images