A doppler velocity-based 4D millimeter wave radar mounting angle calibration method and device

Abstract

This invention discloses a method and apparatus for calibrating the installation angle of a 4D millimeter-wave radar based on Doppler velocity, comprising: (1) obtaining information such as the Doppler velocity, pitch angle, heading angle, and distance of an object through detection by the 4D millimeter-wave radar, and calculating the self-velocity of the 4D millimeter-wave radar using a least squares algorithm; (2) comparing the velocity obtained by least squares with the Doppler velocity of the 4D millimeter-wave radar point cloud, filtering out dynamic objects, and iteratively obtaining a high-precision self-velocity; (3) comparing the high-precision self-velocity of the 4D millimeter-wave radar obtained by iteration with the odometer velocity of a vehicle carrier, and estimating the pitch angle and heading angle of the 4D millimeter-wave radar and the vehicle carrier. This invention is applicable to the estimation of the installation angle of a 4D millimeter-wave radar and a vehicle carrier. Without the need for a specific calibration object, it estimates the pitch angle and heading angle of the 4D millimeter-wave radar and the vehicle carrier with a calibration accuracy of approximately 0.5°.

Description

Technical Field

[0001] This invention belongs to the field of vehicle navigation and positioning, specifically relating to a method and device for angular calibration between a 4D millimeter-wave radar based on Doppler velocity and a vehicle carrier. Background Technology

[0002] Autonomous driving, as a key area of ​​the intelligent world, integrates multiple sensor and algorithm platforms to replace drivers in perceiving, making decisions, and controlling the environment. The vehicle's environmental perception and self-positioning serve as prior information for subsequent decisions and control; their accuracy determines the safety and reliability of autonomous driving. Currently, the most mature solution for autonomous vehicle positioning relies on a combination of Global Navigation Satellite System (GNSS) and Inertial Navigation System (INS) to provide high-precision positioning results. However, in complex urban environments such as tunnels, underground parking garages, overpasses, and urban canyons, GNSS satellite signals are easily blocked and interfered with, compromising the positioning accuracy provided by the combined navigation system. Therefore, introducing sensors with different characteristics to fuse with GNSS and IMU for positioning, thereby improving the accuracy and stability of autonomous vehicle positioning in diverse environments, is the current development trend of autonomous driving.

[0003] Currently, mainstream in-vehicle autonomous driving systems utilize sensors such as cameras, vehicle odometers, LiDAR, and millimeter-wave radar. In in-vehicle autonomous driving navigation systems, to accurately fuse data from various sensors for multi-source integrated navigation and positioning, it is essential to obtain the spatial positions and angular relationships between each sensor, including the linkages and mounting angles. While linkages can be directly measured physically, mounting angles are not easily obtained through direct measurement and require more rigorous algorithms for calibration and estimation. Current algorithms for calibrating the mounting angles of cameras, LiDAR, and other sensors relative to the vehicle or IMU are relatively mature. However, for millimeter-wave radar, because its point cloud angular and distance resolutions are far inferior to those of cameras and LiDAR, estimating the mounting angle between millimeter-wave radar and the vehicle requires various large calibration objects, and accuracy cannot be guaranteed, making its widespread use in in-vehicle integrated navigation and positioning systems difficult.

[0004] However, calibrating the mounting angle between existing 4D millimeter-wave radars and vehicle carriers remains challenging. Traditional calibration methods rely on specific calibration objects or external equipment, are complex to operate, and cannot be completed online while the vehicle is in motion. Summary of the Invention

[0005] To address the difficulty in calibrating the installation angle between existing 4D millimeter-wave radars and vehicle carriers, this invention provides a 4D millimeter-wave radar installation angle calibration method based on Doppler velocity. By utilizing the Doppler velocity, heading angle, and pitch angle information from the point cloud data detected by the 4D millimeter-wave radar, an iterative least squares algorithm is used to estimate the radar's own velocity vector. This vector is then matched with the vehicle's motion velocity vector obtained from the vehicle's odometer, and the rotation matrix from the radar coordinate system to the vehicle carrier coordinate system is calculated. This allows for the determination of the installation angle between the 4D millimeter-wave radar and the vehicle carrier, achieving high-precision calibration of the installation angle without the need for a specific calibration object.

[0006] According to one aspect of the present invention, a method for calibrating the mounting angle of a 4D millimeter-wave radar based on Doppler velocity is provided, comprising: Acquire point cloud data detected by 4D millimeter-wave radar during vehicle operation, wherein the point cloud data includes the Doppler velocity, heading angle and pitch angle of the object; Based on the point cloud data, the current self-velocity vector of the 4D millimeter-wave radar is estimated using the least squares algorithm; Based on the current velocity vector, the points corresponding to the dynamic objects are filtered and removed from the point cloud data to obtain the updated point cloud data. Based on the updated point cloud data, the estimation and filtering steps are iteratively executed until the preset iteration conditions are met, and the target's own velocity vector of the 4D millimeter-wave radar is obtained. Based on the target's own velocity vector and the vehicle's motion velocity vector obtained from the vehicle's odometer, calculate the rotation matrix from the 4D millimeter-wave radar coordinate system to the vehicle carrier coordinate system. Based on the rotation matrix, the installation angle between the 4D millimeter-wave radar and the vehicle carrier is calculated.

[0007] As a further technical solution, based on the current velocity vector, filtering and removing points corresponding to dynamic objects from the point cloud data includes: Based on the current velocity vector and the direction vector of each point in the point cloud data, calculate the theoretical Doppler velocity corresponding to each point; Calculate the difference between the theoretical Doppler velocity and the measured Doppler velocity in the point cloud data for each point; Points with a difference greater than a preset threshold are identified as points corresponding to dynamic objects and removed from the point cloud data.

[0008] As a further technical solution, based on the target's own velocity vector and the vehicle's motion velocity vector obtained from the vehicle's odometer, a rotation matrix from the 4D millimeter-wave radar coordinate system to the vehicle's coordinate system is calculated, including: Construct a first velocity matrix and a second velocity matrix, wherein the first velocity matrix is ​​composed of the target's own velocity vector at multiple moments, and the second velocity matrix is ​​composed of the vehicle's motion velocity vector at multiple moments; Calculate the product matrix of the transpose of the first velocity matrix and the second velocity matrix; Perform singular value decomposition on the product matrix to obtain a left singular matrix and a right singular matrix; The rotation matrix is ​​determined by the product of the right singular matrix and the transpose of the left singular matrix.

[0009] As a further technical solution, the installation angle includes pitch angle and yaw angle, and the method further includes: The roll angle between the 4D millimeter-wave radar and the vehicle carrier is preset to zero. Based on the rotation matrix, the pitch angle and heading angle are calculated by using the relationship between the matrix elements and trigonometric functions.

[0010] As a further technical solution, before calculating the rotation matrix from the 4D millimeter-wave radar coordinate system to the vehicle carrier coordinate system based on the target's own velocity vector and the vehicle's motion velocity vector obtained from the vehicle odometer, the method further includes: forming multiple sets of velocity pairs from the target's own velocity vector and the vehicle's motion velocity vector obtained at multiple times.

[0011] As a further technical solution, the estimation step and the filtering and elimination step together perform two least squares estimations, wherein: the first time, the current velocity vector is estimated using the original point cloud data and the filtering and elimination step is performed to obtain the updated point cloud data; the second time, the least squares estimation is performed again using the updated point cloud data to obtain the target's own velocity vector.

[0012] According to one aspect of the present invention, a 4D millimeter-wave radar mounting angle calibration device based on Doppler velocity is provided, comprising: The data acquisition module is used to acquire point cloud data detected by 4D millimeter-wave radar during vehicle operation. The point cloud data includes the Doppler velocity, heading angle and pitch angle of the object. The first estimation module is used to estimate the current self-velocity vector of the 4D millimeter-wave radar based on the point cloud data using a least squares algorithm. The point cloud filtering module is used to filter and remove points corresponding to dynamic objects from the point cloud data based on the current velocity vector, so as to obtain updated point cloud data. The second estimation module is used to iteratively perform estimation and filtering based on the updated point cloud data until the preset iteration conditions are met, so as to obtain the target's own velocity vector of the 4D millimeter-wave radar. The matrix calculation module is used to calculate the rotation matrix from the 4D millimeter-wave radar coordinate system to the vehicle carrier coordinate system based on the target's own velocity vector and the vehicle's motion velocity vector obtained from the vehicle odometer. An angle calculation module is used to calculate the installation angle between the 4D millimeter-wave radar and the vehicle carrier based on the rotation matrix.

[0013] As a further technical solution, the point cloud filtering module includes: The theoretical velocity calculation unit is used to calculate the theoretical Doppler velocity corresponding to each point based on the current self-velocity vector and the direction vector of each point in the point cloud data; The difference calculation unit is used to calculate the difference between the theoretical Doppler velocity at each point and the measured Doppler velocity in the point cloud data; The dynamic point removal unit is used to identify points whose difference is greater than a preset threshold as points corresponding to dynamic objects and remove them from the point cloud data.

[0014] According to one aspect of the present invention, a vehicle is provided, comprising: 4D millimeter-wave radar is used to detect point cloud data; Odometer, used to provide the vehicle's velocity vector; The controller includes a memory and a processor. The memory stores a computer program, and the processor executes the computer program to implement the steps of the 4D millimeter-wave radar mounting angle calibration method based on Doppler velocity.

[0015] According to one aspect of the present invention, a computer-readable storage medium is provided having a computer program stored thereon, which, when executed by a processor, implements the steps of the described 4D millimeter-wave radar mounting angle calibration method based on Doppler velocity.

[0016] Compared with the prior art, the beneficial effects of the present invention are as follows: 1. This invention is used to estimate the installation angle between a 4D millimeter-wave radar and a vehicle carrier. It uses the Doppler velocity information of the 4D millimeter-wave radar to calculate the radar's own velocity and compares and analyzes it with the vehicle carrier's odometer information to estimate the installation angle between the 4D millimeter-wave radar and the vehicle carrier.

[0017] 2. The calibration algorithm involved in this invention is easy to implement. Compared with the traditional method that requires various external calibration objects to estimate the installation angle using point cloud positions, this invention does not require additional calibration objects and is more suitable for vehicle-mounted integrated navigation and positioning systems.

[0018] 3. Experiments and results analysis were conducted on a real vehicle-mounted integrated navigation and positioning platform: Under normal vehicle road operation conditions, high-precision pitch and heading angles can be calibrated. Attached Figure Description

[0019] To more clearly illustrate the technical solutions in the embodiments of the present invention or the prior art, the accompanying drawings used in the description of the embodiments or the prior art will be briefly introduced below. Obviously, the accompanying drawings described below are some embodiments of the present invention. For those skilled in the art, other drawings can be obtained based on these drawings without creative effort.

[0020] Figure 1 A schematic diagram of a 4D millimeter-wave radar installation angle calibration method based on Doppler velocity provided in an embodiment of the present invention; Figure 2 The installation angle calibration method provided in this embodiment of the invention obtains the pitch angle θ and yaw angle ψ in the installation angle under real road operation, and provides a time convergence diagram. Figure 3 This invention provides a comparison chart of position errors obtained by using 4D millimeter-wave radar-assisted integrated navigation positioning before and after calibrating the installation angle under actual road operation. Figure 4 This is a speed error comparison chart of the positioning results obtained by using 4D millimeter-wave radar-assisted integrated navigation positioning before and after calibrating the installation angle under actual road operation, provided as an embodiment of the present invention. Detailed Implementation

[0021] It should be noted that: The advent of the latest 4D millimeter-wave radar provides more accurate Doppler velocity sensing capabilities, opening up new possibilities for the spatial relationship calibration of vehicle-mounted sensors. However, existing technologies lack an effective online calibration method that utilizes the Doppler velocity information of 4D millimeter-wave radar to perform online calibration of the radar's mounting angle with the vehicle platform without the need for calibration objects. Therefore, this invention provides a method for calibrating the mounting angle of 4D millimeter-wave radar with the vehicle platform using Doppler velocity, which has significant practical implications for vehicle-mounted integrated navigation and positioning systems.

[0022] The terms “comprising” and “having”, and any variations thereof, in the specification, claims, and accompanying drawings of this invention are intended to cover a non-exclusive inclusion, such as a process, method, system, product, or apparatus that includes a series of steps or units, not necessarily limited to those explicitly listed, but may include other steps or units not explicitly listed or inherent to such processes, methods, products, or apparatus.

[0023] To make the objectives, technical solutions, and advantages of the embodiments of the present invention clearer, the technical solutions of the embodiments of the present invention will be clearly and completely described below with reference to the accompanying drawings. Obviously, the described embodiments are only some embodiments of the present invention, not all embodiments. Based on the embodiments of the present invention, all other embodiments obtained by those skilled in the art without creative effort are within the scope of protection of the present invention. In addition, the technical features of the various embodiments or individual embodiments provided by the present invention can be arbitrarily combined to form new technical solutions. Such combinations are not bound by the order of steps and / or structural composition patterns, but must be based on the ability of those skilled in the art to implement them. When the combination of technical solutions is contradictory or cannot be implemented, it should be considered that such a combination of technical solutions does not exist and is not within the scope of protection claimed by the present invention.

[0024] This invention provides a method for calibrating the installation angle of a 4D millimeter-wave radar based on Doppler velocity. This method uses Doppler velocity information detected by the 4D millimeter-wave radar, combined with speed information from a vehicle odometer, to achieve high-precision calibration of the installation angle between the radar and the vehicle carrier. The method includes the following steps.

[0025] Step 1: Acquire 4D millimeter-wave radar point cloud data

[0026] 4D millimeter-wave radar can detect information such as the Doppler velocity, heading angle, pitch angle, and distance of an object. This invention utilizes 4D millimeter-wave radar to identify static objects and then uses least squares and iterative iteration to perform self-velocity estimation, providing the velocity information of the carrier.

[0027] During vehicle operation, the 4D millimeter-wave radar detects the surrounding environment in real time, and the raw observations obtained are the millimeter-wave radar points at... Momentary Cloud Collection , The corresponding millimeter-wave radar point cloud is ,

[0028]

[0029] In the formula, It is the polar coordinate representation of the detected object in the radar coordinate system. It detects the distance of an object relative to the center point of a 4D millimeter-wave radar array. and It detects the heading and elevation angles of an object in the radar's own coordinate system. This is the Doppler velocity of the detected object. To facilitate subsequent calculations, the entire point cloud is first converted to the millimeter-wave radar coordinate system. The following section discusses the transformation between Cartesian and polar coordinates.

[0030]

[0031] exist Under this system, the Cartesian coordinates of the radar point are represented as follows:

[0032]

[0033] radar points The corresponding Doppler velocity scalar is

[0034]

[0035] Finally, the Cartesian coordinates of the 4D millimeter-wave radar point cloud were obtained.

[0036]

[0037] Step 2: Estimate the current self-velocity vector of the 4D millimeter-wave radar.

[0038] Using the least squares algorithm, the radar's own velocity is initially estimated based on the point cloud data of the current frame. The least squares solution of the 4D millimeter-wave radar's own velocity vector in the r-coordinate system can be obtained as follows:

[0039]

[0040] In the formula, yes The Doppler velocity scalar of all point clouds in the point cloud set at any given moment; The matrix is ​​the Cartesian coordinate representation of the corresponding point cloud in the 4D millimeter-wave radar coordinate system; yes The 4D millimeter-wave radar's own ground velocity vector at any given moment The projection under the system.

[0041] Step 3: Filter and remove points corresponding to dynamic objects based on their current velocity vector.

[0042] Using the current self-velocity vector estimated in the previous step, the theoretical Doppler velocity corresponding to each point is calculated in reverse and compared with the measured Doppler velocity in order to identify and eliminate dynamic points.

[0043] Specifically, the 4D millimeter-wave radar's own velocity vector estimation is obtained through the first least squares method. This velocity result is obtained by performing least-squares calculations on all millimeter-wave radar point clouds, without any noise reduction processing on the 4D millimeter-wave radar data. Utilizing... As a vector velocity reference value, the 4D millimeter-wave radar point corresponding to each stationary object is calculated in reverse. scalar velocity relative to the center of the 4D millimeter-wave radar .

[0044]

[0045]

[0046] By comparing the Doppler velocity of point clouds from the original 4D millimeter-wave radar observations... Velocity calculated with direction This is used to remove noise from the Doppler velocity anomalies in the original 4D millimeter-wave radar observations and update the point cloud set. .

[0047] In the formula, T is and The velocity difference threshold (e.g., T=2m / s) will vary depending on the scenario. The specific threshold value should be set according to the velocity accuracy level of the scenario in which the 4D millimeter-wave radar is located. Update Then, repeat equation (6) to finally obtain the 4D millimeter-wave radar's estimate of its own velocity. .

[0048] Step 4: Iteratively perform the estimation and filtering steps to obtain the target's own velocity vector.

[0049] First, the initial radar velocity vector is estimated using the least squares algorithm based on the original point cloud, and a filtering and elimination step (Equation (7)-(9)) is performed to remove point clouds with Doppler velocity anomalies, thus obtaining updated point cloud data.

[0050] Then, the updated point cloud is used as input, and the least squares algorithm (Equation 6) is used again to estimate the final target's own velocity vector.

[0051] In this embodiment, two least squares estimations are performed, namely, a two-step method of "coarse estimation, removal of dynamic points, and fine estimation". The specific process is as follows:

[0052] First estimation: Based on the entire original point cloud, the initial velocity vector is obtained by least squares, and dynamic targets and noise points are filtered out to obtain the updated static point cloud set.

[0053] Second estimation: Based on the updated point cloud set, least squares estimation is performed again to obtain a high-precision radar self-velocity vector.

[0054] This two-step method can effectively suppress the interference of dynamic targets and noise points on velocity estimation, obtain the accurate radar's own velocity vector, and provide reliable input for subsequent installation angle calibration.

[0055] Step 5: Obtain the speed vector from the vehicle's odometer.

[0056] The speed obtained by the millimeter-wave radar through self-velocity estimation This is the velocity vector of the 4D millimeter-wave radar in the millimeter-wave radar coordinate system. Because the 4D millimeter-wave radar is rigidly connected and fixed to the vehicle carrier, this velocity is equal to the velocity vector projected onto the vehicle's velocity in the 4D millimeter-wave radar coordinate system. .

[0057]

[0058] The vehicle's odometer can be used to obtain the vehicle's velocity vector in the vehicle's coordinate system at the same moment. The carrier velocity calculated by 4D millimeter-wave radar. The vehicle's speed, obtained from the odometer on the vehicle carrier, is These are different representations of the same vector in the 4D millimeter-wave radar coordinate system and the carrier coordinate system, representing the vehicle's motion state.

[0059] In this system, the origin of the radar coordinate system is the center of the antenna array, the x-axis points to the normal direction of the antenna array plane, the y-axis points to the left side of the antenna array plane, and the z-axis is a right-handed coordinate system with respect to the x and y axes. The origin of the vehicle coordinate system is the center of the vehicle, the x-axis points forward of the vehicle's motion, the y-axis points to the right of the vehicle's motion, and the z-axis is a right-handed coordinate system with respect to the x and y axes. The rotation matrix from the radar coordinate system to the vehicle coordinate system is... The rotation sequence is .

[0060] Step 6: Calculate the rotation matrix from the radar coordinate system to the vehicle carrier coordinate system, and solve for the installation angle.

[0061] and The conversion relationship between them is determined by the 4D millimeter-wave radar coordinate system and the vehicle coordinate system. Considering the installation angle between the 4D millimeter-wave radar and the vehicle vehicle, we have:

[0062]

[0063] This represents the rotation matrix from the 4D millimeter-wave radar coordinate system to the vehicle carrier coordinate system. and Let represent the vehicle's odometer speed and the 4D millimeter-wave radar estimated speed at time i, respectively. Specifically, it is expressed as:

[0064]

[0065] In an ideal situation, the above equation holds strictly. Due to observation noise, the error function is minimized using the least squares method:

[0066]

[0067]

[0068]

[0069] By solving the least squares method to minimize the error function (13), the rotation matrix from the 4D millimeter-wave radar coordinate system to the vehicle carrier coordinate system can be obtained. This allows for the determination of the installation angle information between the 4D millimeter-wave radar and the vehicle carrier.

[0070] Since the roll angle can usually be precisely aligned (approximately 0) when the radar and carrier are installed, while the pitch and yaw angles are uncertain due to factors such as installation tolerances and vehicle load variations, and since the vehicle carrier and 4D millimeter-wave radar are more sensitive to roll and pitch angles in the installation direction, in actual calibration, the roll angle in the three installation angle directions is assumed to be 0, and the yaw and pitch angles are estimated based on this. The least squares formula of equation (13) is solved by SVD.

[0071]

[0072]

[0073] In equation (15), the vehicle odometer speed matrix And 4D millimeter-wave radar estimated velocity matrix Represented as

[0074]

[0075]

[0076] For equation (15) Perform singular value decomposition on the matrix.

[0077] Rotation matrix The solution is then:

[0078]

[0079] It is a rotation matrix from the 4D millimeter-wave radar coordinate system to the vehicle-mounted coordinate system, and the roll angle is set to 0 during the solution process. It can be determined by the pitch angle. and heading angle Represented as:

[0080]

[0081] Solving equation (20) yields the installation pitch angles of the 4D millimeter-wave radar and the vehicle-mounted platform. and heading angle .

[0082] Figure 1 A method for determining the installation angle of a 4D millimeter-wave radar and a vehicle carrier based on the Doppler velocity of a 4D millimeter-wave radar is presented. The box shows the method for solving the radar's own velocity using the Doppler velocity of the 4D millimeter-wave radar. After obtaining the point cloud of the 4D millimeter-wave radar, the initial 4D millimeter-wave radar's own velocity vector result is obtained by least-squares calculation of the 4D millimeter-wave radar point cloud according to equations (2)-(3). The velocity vector result is then substituted into the 4D millimeter-wave radar point cloud set for judgment. The Doppler velocity abnormal radar point cloud is eliminated by equations (7)-(9). The process is iterated repeatedly until the velocity difference is less than the threshold (see equation (9)) to obtain the final 4D millimeter-wave radar velocity vector. Then, the installation angle is estimated by combining this vector with the wheel carrier odometer. Equations (11)-(13) give the least-squares algorithm for estimating the installation angle, and equations (14)-(19) give the specific derivation steps for least-squares calculation using the 4D millimeter-wave radar velocity vector and the wheel carrier odometer velocity vector. The installation angle estimation result is obtained by solving the rotation matrix obtained by the least-squares algorithm.

[0083] As a preferred embodiment, this invention utilizes a vehicle-mounted data acquisition platform equipped with a vehicle odometer, 4D millimeter-wave radar, IMU, GNSS receiver, and high-precision integrated navigation system, based on real data. Data acquisition, installation angle estimation, and 4D millimeter-wave radar-assisted integrated navigation calculations are performed. The effectiveness of the installation angle estimation is evaluated by comparing the positioning results of the assisted integrated navigation before and after the 4D millimeter-wave radar installation angle estimation.

[0084] Figure 2 The convergence plots of the pitch and yaw angle estimation results versus the vehicle's motion time are presented during the installation angle estimation. The red curve represents the estimated pitch angle, and the red dashed line represents the final calibrated pitch angle value. The blue curve represents the estimated yaw angle, and the blue dashed line represents the final calibrated yaw angle value. The curves show that after convergence, the estimated yaw angle is 8.78° and the estimated pitch angle is -5.12°, with the curve fluctuation range within 0.5°.

[0085] Figure 3The graphs showing the north-east ground velocity error of GNSS / IMU / 4D millimeter-wave radar during a 40-second GNSS interruption are presented before and after installation angle estimation correction. The red curve represents the velocity error of the navigation result after installation angle estimation, and the blue curve represents the velocity error of the navigation result without installation angle estimation. Statistical error calculations were performed on multiple sets of data and multiple interruptions. After correcting the installation angle between the 4D millimeter-wave radar and the vehicle, the RMS statistical error of the northbound velocity decreased from 0.18 m / s to 0.04 m / s during the GNSS interruption. The RMS statistical error of the eastbound velocity decreased from 0.22 m / s to 0.03 m / s. This significantly improves the accuracy of 4D millimeter-wave radar-assisted vehicle-mounted integrated navigation and positioning.

[0086] Figure 4 The diagram presents the NR (northeast) position error of GNSS / IMU / 4D millimeter-wave radar during a 40-second GNSS interruption, before and after installation angle estimation correction for a single set of data. The red curve represents the position error of the navigation result after installation angle estimation, and the blue curve represents the position error of the navigation result without installation angle estimation. Statistical error calculations were performed on multiple sets of data and multiple interruptions. After correcting the installation angle between the 4D millimeter-wave radar and the vehicle carrier, the RMS (Real Mean Squared Error) of the northward position decreased from 3.14m to 0.32m during the GNSS interruption. The RMS of the eastward position decreased from 3.62m to 0.45m.

[0087] The above results demonstrate that the calibration method proposed in this invention can effectively improve the positioning accuracy of 4D millimeter-wave radar-assisted vehicle-mounted integrated navigation.

[0088] Corresponding to the above method embodiments, this invention provides a 4D millimeter-wave radar mounting angle calibration device based on Doppler velocity, which includes the following modules:

[0089] Data acquisition module: used to acquire point cloud data detected by 4D millimeter-wave radar during vehicle operation. The point cloud data includes the Doppler velocity, heading angle and pitch angle of the object.

[0090] First estimation module: used to estimate the current velocity vector of the 4D millimeter-wave radar using the least squares algorithm based on the point cloud data.

[0091] Point cloud filtering module: Used to filter and remove points corresponding to dynamic objects from the point cloud data based on the current velocity vector, so as to obtain updated point cloud data.

[0092] The second estimation module is used to iteratively perform estimation and filtering based on the updated point cloud data until the preset iteration conditions are met, thereby obtaining the target's own velocity vector for the 4D millimeter-wave radar. In this embodiment, a two-step least squares estimation is performed: a coarse estimation followed by dynamic point removal, and then a fine estimation.

[0093] Matrix calculation module: This module calculates the rotation matrix from the 4D millimeter-wave radar coordinate system to the vehicle carrier coordinate system based on the target's own velocity vector and the vehicle's velocity vector obtained from the vehicle odometer. Specifically, it constructs a first velocity matrix and a second velocity matrix, calculates the product matrix, performs singular value decomposition, and determines the rotation matrix based on the product of the right singular matrix and the transpose of the left singular matrix.

[0094] Angle calculation module: used to calculate the installation angle between the 4D millimeter-wave radar and the vehicle carrier, including the pitch angle and the heading angle, based on the rotation matrix.

[0095] Furthermore, the point cloud filtering module further includes:

[0096] Theoretical velocity calculation unit: used to calculate the theoretical Doppler velocity corresponding to each point based on the current self-velocity vector and the direction vector of each point in the point cloud data;

[0097] Difference calculation unit: used to calculate the difference between the theoretical Doppler velocity and the measured Doppler velocity in the point cloud data for each point;

[0098] Dynamic point removal unit: used to identify points whose difference is greater than a preset threshold as points corresponding to dynamic objects and remove them from the point cloud data.

[0099] The device provided in this embodiment addresses the technical problems of difficulty in calibrating the installation angle of existing 4D millimeter-wave radar on vehicle carriers, the need for specific calibration objects, and the inability to calibrate online. By utilizing the Doppler velocity information of the 4D millimeter-wave radar and combining it with the vehicle odometer velocity information, and employing iterative least squares estimation and rotation matrix matching, high-precision online calibration of the installation angle is achieved without the need for external calibration objects. This results in high calibration accuracy, low computational load, and real-time operation.

[0100] Based on the same inventive concept as any of the foregoing embodiments, this embodiment of the invention provides a vehicle that can achieve online, high-precision self-calibration of the installation angle between the 4D millimeter-wave radar and the vehicle carrier without the need for external calibration objects or specific calibration sites.

[0101] The vehicles include:

[0102] A 4D millimeter-wave radar, installed on a vehicle, is used to detect point cloud data of the surrounding environment in real time while the vehicle is in motion. The point cloud data includes the Doppler velocity, heading angle, and pitch angle of objects.

[0103] An odometer is used to obtain the vehicle's own velocity vector. The odometer includes, but is not limited to, wheel speedometers, inertial measurement units, or integrated navigation systems.

[0104] The controller includes a memory and a processor. The memory stores a computer program, and the processor executes the computer program to implement the steps of the 4D millimeter-wave radar mounting angle calibration method based on Doppler velocity as described in any of the above method embodiments.

[0105] During vehicle operation, the 4D millimeter-wave radar and the odometer collect data respectively. The controller processes the collected data and outputs the installation angle calibration results between the radar and the vehicle carrier in real time. The calibration results are then used for subsequent autonomous driving functions such as multi-sensor fusion positioning and environmental perception.

[0106] Based on the same inventive concept as any of the foregoing embodiments, this embodiment of the invention provides a computer-readable storage medium storing a computer program thereon, which, when executed by a processor, implements the steps of the 4D millimeter-wave radar installation angle calibration method based on Doppler velocity as described in any of the above method embodiments.

[0107] The computer-readable storage medium may include, but is not limited to, read-only memory (ROM), random access memory (RAM), disk, optical disk, flash memory, solid-state drive (SSD), etc. When the computer program runs on the processor, it can perform the following steps: acquire point cloud data of 4D millimeter-wave radar; estimate the radar's current self-velocity vector using a least squares algorithm; filter and remove dynamic points based on the current self-velocity vector to obtain updated point cloud data; iteratively execute the estimation and filtering steps to obtain the target's self-velocity vector; calculate the rotation matrix based on the target's self-velocity vector and the vehicle's motion velocity vector obtained from the vehicle odometer; and calculate the installation angle based on the rotation matrix.

[0108] By applying the computer-readable storage medium to a vehicle controller or other computing device, high-precision calibration of the mounting angle of the 4D millimeter-wave radar to the vehicle carrier can be achieved without the need for a specific calibration object.

[0109] In summary, this invention discloses a 4D millimeter-wave radar installation angle calibration method based on Doppler velocity, specifically including: (1) obtaining information such as the Doppler velocity, pitch angle, heading angle, and distance of an object through the detection of the object by the 4D millimeter-wave radar, and calculating the self-velocity of the 4D millimeter-wave radar using the least squares algorithm. (2) comparing the velocity obtained by least squares with the Doppler velocity of the 4D millimeter-wave radar point cloud, filtering out dynamic objects, and iteratively obtaining a high-precision self-velocity. (3) using the high-precision self-velocity of the 4D millimeter-wave radar obtained through iteration, comparing it with the odometer velocity of the vehicle carrier, estimating the pitch angle and heading angle of the 4D millimeter-wave radar and the vehicle carrier (calibration accuracy is approximately 0.5°), significantly improving the accuracy level of 4D millimeter-wave radar-assisted positioning. This invention is applicable to the estimation of the installation angle of 4D millimeter-wave radar and vehicle carrier. Without the need for specific calibration objects, it estimates the pitch and heading angles of 4D millimeter-wave radar and vehicle carrier, providing accurate installation angle information for the fusion positioning of 4D millimeter-wave radar and vehicle sensor data, and realizing spatial synchronization of multi-sensor fusion positioning.

[0110] Finally, it should be noted that the above embodiments are only used to illustrate the technical solutions of the present invention, and not to limit them; although the present invention has been described in detail with reference to the foregoing embodiments, those skilled in the art should understand that modifications can still be made to the technical solutions described in the foregoing embodiments, or equivalent substitutions can be made to some or all of the technical features therein; and these modifications or substitutions do not cause the essence of the corresponding technical solutions to deviate from the technical solutions of the embodiments of the present invention.

Claims

1. A method for calibrating the mounting angle of a 4D millimeter-wave radar based on Doppler velocity, characterized in that, include: Acquire point cloud data detected by 4D millimeter-wave radar during vehicle operation, wherein the point cloud data includes the Doppler velocity, heading angle and pitch angle of the object; Based on the point cloud data, the current self-velocity vector of the 4D millimeter-wave radar is estimated using the least squares algorithm; Based on the current velocity vector, the points corresponding to the dynamic objects are filtered and removed from the point cloud data to obtain the updated point cloud data. Based on the updated point cloud data, the estimation and filtering steps are iteratively executed until the preset iteration conditions are met, and the target's own velocity vector of the 4D millimeter-wave radar is obtained. Based on the target's own velocity vector and the vehicle's motion velocity vector obtained from the vehicle's odometer, calculate the rotation matrix from the 4D millimeter-wave radar coordinate system to the vehicle carrier coordinate system. Based on the rotation matrix, the installation angle between the 4D millimeter-wave radar and the vehicle carrier is calculated.

2. The Doppler velocity based 4D mmWave radar mounting angle calibration method according to claim 1, characterized in that, Based on the current velocity vector, filtering and removing points corresponding to dynamic objects from the point cloud data includes: Based on the current velocity vector and the direction vector of each point in the point cloud data, calculate the theoretical Doppler velocity corresponding to each point; Calculate the difference between the theoretical Doppler velocity and the measured Doppler velocity in the point cloud data for each point; Points with a difference greater than a preset threshold are identified as points corresponding to dynamic objects and removed from the point cloud data. 3.The Doppler velocity based 4D mmWave radar mounting angle calibration method of claim 1, wherein, Based on the target's own velocity vector and the vehicle's velocity vector obtained from the vehicle's odometer, calculate the rotation matrix from the 4D millimeter-wave radar coordinate system to the vehicle's coordinate system, including: Construct a first velocity matrix and a second velocity matrix, wherein the first velocity matrix is ​​composed of the target's own velocity vector at multiple moments, and the second velocity matrix is ​​composed of the vehicle's motion velocity vector at multiple moments; Calculate the product matrix of the transpose of the first velocity matrix and the second velocity matrix; Perform singular value decomposition on the product matrix to obtain a left singular matrix and a right singular matrix; The rotation matrix is ​​determined by the product of the right singular matrix and the transpose of the left singular matrix. 4.The Doppler velocity based 4D mmWave radar mounting angle calibration method of claim 1, wherein, The installation angle includes pitch angle and yaw angle, and the method further includes: The roll angle between the 4D millimeter-wave radar and the vehicle carrier is preset to zero. Based on the rotation matrix, the pitch angle and heading angle are calculated by using the relationship between the matrix elements and trigonometric functions. 5.The Doppler velocity based 4D mmWave radar mounting angle calibration method of claim 1, wherein, Before calculating the rotation matrix from the 4D millimeter-wave radar coordinate system to the vehicle carrier coordinate system based on the target's own velocity vector and the vehicle's motion velocity vector obtained from the vehicle odometer, the method further includes: forming multiple sets of velocity pairs from the target's own velocity vector and the vehicle's motion velocity vector obtained at multiple times.

6. The Doppler velocity based 4D mmWave radar mounting angle calibration method according to claim 1, characterized in that, The estimation and filtering / removal steps involve two least-squares estimations: the first estimation uses the original point cloud data to estimate the current velocity vector and performs the filtering / removal step to obtain updated point cloud data; the second estimation uses the updated point cloud data to perform least-squares estimation again to obtain the target's velocity vector. 7.A Doppler velocity based 4D millimeter wave radar installation angle calibration device, characterized by, include: The data acquisition module is used to acquire point cloud data detected by 4D millimeter-wave radar during vehicle operation. The point cloud data includes the Doppler velocity, heading angle and pitch angle of the object. The first estimation module is used to estimate the current self-velocity vector of the 4D millimeter-wave radar based on the point cloud data using a least squares algorithm. The point cloud filtering module is used to filter and remove points corresponding to dynamic objects from the point cloud data based on the current velocity vector of the object itself, so as to obtain updated point cloud data. The second estimation module is used to iteratively perform estimation and filtering based on the updated point cloud data until the preset iteration conditions are met, so as to obtain the target's own velocity vector of the 4D millimeter-wave radar. The matrix calculation module is used to calculate the rotation matrix from the 4D millimeter-wave radar coordinate system to the vehicle carrier coordinate system based on the target's own velocity vector and the vehicle's motion velocity vector obtained from the vehicle odometer. An angle calculation module is used to calculate the installation angle between the 4D millimeter-wave radar and the vehicle carrier based on the rotation matrix.

8. The Doppler velocity based 4D mm-wave radar mounting angle calibration apparatus according to claim 7, characterized by, The point cloud filtering module includes: The theoretical velocity calculation unit is used to calculate the theoretical Doppler velocity corresponding to each point based on the current self-velocity vector and the direction vector of each point in the point cloud data; The difference calculation unit is used to calculate the difference between the theoretical Doppler velocity at each point and the measured Doppler velocity in the point cloud data; The dynamic point removal unit is used to identify points whose difference is greater than a preset threshold as points corresponding to dynamic objects and remove them from the point cloud data.

9. A vehicle characterized by comprising: include: 4D millimeter-wave radar is used to detect point cloud data; Odometer, used to provide the vehicle's velocity vector; A controller, comprising a memory and a processor, wherein the memory stores a computer program, and the processor executes the computer program to implement the steps of the 4D millimeter-wave radar mounting angle calibration method based on Doppler velocity as described in any one of claims 1 to 6.

10. A computer-readable storage medium having stored thereon a computer program, characterized in that, When the computer program is executed by the processor, it implements the steps of the 4D millimeter-wave radar mounting angle calibration method based on Doppler velocity as described in any one of claims 1 to 6.

Images