Highway automatic driving vehicle dynamic control method based on multi-mode LiDAR point cloud data fusion

Abstract

The invention discloses an expressway automatic driving vehicle dynamic control method based on multi-mode LiDAR point cloud data fusion. According to the technical scheme, for the problem of obstacle avoidance delay caused by moving target trajectory prediction deviation in an existing expressway scene, effective three-dimensional point cloud data are extracted, and a three-dimensional laser point cloud multi-target real-time tracking and collaborative decision-making framework is innovatively constructed. Kalman filtering and PID (Proportion Integration Differentiation) algorithms are adopted, vehicle dynamics constraints, road curvature constraints and safety envelope boundary conditions are integrated, and a planning problem under a multi-target collaborative optimization linear system is constructed. Through multi-modal fusion of camera and radar data, vehicle obstacle avoidance with higher precision and higher speed is realized.

Description

Technical Field

[0001] This invention relates to motion control problems in autonomous driving technology, particularly control based on lidar (LiDAR). Background Technology

[0002] LiDAR (Light Detection and Ranging) is an optical remote sensing technology based on laser pulses or continuous waves. It emits laser light towards a target and receives the reflected signal, combining principles such as Time of Flight (ToF) or phase difference to accurately measure the target's position, distance, velocity, and three-dimensional shape. Its core technologies combine optics, electronics, signal processing, and algorithm analysis, and it is widely used in fields such as autonomous driving, geographic mapping, environmental monitoring, and robot navigation.

[0003] LiDAR has the following unique advantages compared to other sensors (such as cameras and millimeter-wave radar): (1) High precision and high resolution: The laser wavelength (usually 905 nm or 1550 nm) is short and highly directional, which can achieve centimeter-level ranging accuracy and spatial resolution up to millimeter level.

[0004] (2) Applicable to the detection of small targets (such as stones on the road, details of pedestrian limbs).

[0005] (3) Strong anti-interference ability: The laser beam has a short wavelength and a small divergence angle (<1°), and is less affected by ambient light (such as sunlight and car lights), and can work under complex lighting conditions.

[0006] (4) Three-dimensional point cloud imaging: Through multi-beam scanning or area array detection, three-dimensional point cloud data of the target is generated, supporting advanced applications such as object recognition and terrain modeling.

[0007] (5) All-weather adaptability: It performs better than cameras (but weaker than millimeter-wave radar) in severe weather conditions such as fog, rain, and snow.

[0008] (6) Large dynamic range: It can detect targets at close range (0.1 m) and long range (more than 200 m) simultaneously, covering multiple scenario requirements.

[0009] The commonly used obstacle avoidance methods in existing lidar technologies mainly include: Dynamic Window Method (DWA), Vector Field Histogram (VFH), and Artificial Potential Field Method (APF). These algorithms have low accuracy, large computational load, and are prone to false detection and false negative detection.

[0010] This study significantly improves data processing efficiency and reduces computational resource consumption by introducing an optimized adaptive radius filtering algorithm for point cloud data. Based on this, the Iterative Closest Point (ICP) point cloud registration algorithm is combined to effectively suppress the negative impact of noise interference on point cloud matching accuracy, thereby achieving a significant improvement in object recognition rate in complex noisy environments. This technical solution systematically solves the problem of reduced recognition performance due to noise sensitivity in traditional methods through the synergistic optimization of filtering and registration. Summary of the Invention

[0011] In intelligent transportation systems, accurate tracking and localization of vehicles is crucial for achieving functions such as autonomous driving, traffic flow management, and safety protection. Traditional vehicle tracking and localization methods mainly rely on GPS and visual sensors; however, these methods suffer from limitations in positioning accuracy and susceptibility to environmental interference. In recent years, existing LiDAR-based detection methods still need improvement in data processing efficiency, detection accuracy, and adaptability to different scenarios.

[0012] The vehicle control method based on multimodal LiDAR point cloud data fusion significantly enhances the robustness and spatial resolution of environmental perception through spatiotemporal alignment and feature-level fusion of heterogeneous point cloud data. This method effectively suppresses dynamic noise interference, improves the accuracy of target detection and pose estimation, and strengthens the reliability of perception in complex scenarios (such as low-light and rain / fog environments) through multi-source data redundancy and complementarity. It provides high-confidence environmental modeling input for the vehicle decision-making and control modules, thereby optimizing path planning response speed and driving safety.

[0013] To address the aforementioned problems, this invention proposes a technical solution for dynamic control of autonomous vehicles on highways based on multimodal LiDAR point cloud data fusion. This includes: S1. Obtain raw point cloud data; S2. Perform adaptive filtering on the raw point cloud data; S3. Obtain point cloud data of the detected object through 3D-3D pose estimation; S4. Use the DBSCAN algorithm to extract features from the image point cloud data; S5. Based on the acquired point cloud data, use the KF algorithm to estimate the position and state. S6. Based on the acquired point cloud data, a PID algorithm is used for speed control; S7. Perform hierarchical fusion of radar point cloud features and visual semantic features.

[0014] Furthermore, the raw point cloud data obtained in S1 can be used to adjust radar parameters using a user-adjustable platform; Before starting the second step, the obtained minDistance needs to be processed to determine whether the target distance exceeds the radar range in order to achieve adaptive detection and accurately obtain the distance of the object, ensuring that the vehicle does not detect the position of an object that is stationary relative to the vehicle.

[0015] If a certain position is clearly outside the scanning range of the adaptive detection, it indicates that the vehicle has detected the position of an object that is stationary relative to the vehicle. Otherwise, the car will follow the target.

[0016] Furthermore, the adaptive filtering operation on the raw point cloud data described in S2 includes the following steps: S2-1: Voxel filtering effectively reduces the number of point clouds while preserving their shape features, thereby improving the algorithm's running speed.

[0017] S2-2: A minimum number of points constraint is added to the voxel filtering, that is, the number of points in each voxel grid must reach the minimum constraint value MP, otherwise the voxel grid range is expanded until the number of points is not less than MP.

[0018] S2-3: Further processing is performed on the speed-up filtered point cloud data. By fusing point cloud density features, the radius filtering is optimized into parameter adaptive adjustment. Large radius filtering models and small radius filtering models are constructed.

[0019] Furthermore, the 3D-3D pose estimation described in S3 to acquire point cloud data of the probed object includes the following steps: S3-1: Obtain paired 3D point cloud data and pair them using the two points with the closest Euclidean distance.

[0020] S3-2: Due to noise and mismatch (e.g.) The existence of (which do not actually correspond to the same point in space, but the feature matching algorithm mistakenly considers them to be the same point) means that the pose transformation formula does not always hold. Therefore, we need to use the least squares method to minimize the objective function as follows: , in, It's the camera rotating. It's the camera panning. pi The first set of point cloud data coordinate system, qi This is the coordinate system for the second set of point cloud data.

[0021] Furthermore, as described in S4, the DBSCAN algorithm is used to extract features from the image point cloud data. The algorithm steps are as follows: S4-1: Specify appropriate e and Minpoints (e is similar to the radius, and Minpoint is the minimum number of samples, for the subsequent determination of the core objects and their clusters). S4-2: Calculate all sample points. If point p has more than Minpoints points in its neighborhood, create a new cluster with p as the core point (p is the core object).

[0022] S4-3: Repeatedly find points that are directly density-reachable (and then density-reachable) from these core points, add them to the corresponding clusters, and merge clusters where the core points are density-connected.

[0023] S4-4: The algorithm terminates when no new points can be added to any cluster.

[0024] Furthermore, in step S6, a PID algorithm is used for speed control based on the acquired point cloud data, as shown in the following formula: , Where e(t) represents the data at the current position, Kp This is the proportionality coefficient; T t The integral time constant; T D is the differential time constant.

[0025] Furthermore, in order to improve the system's robustness under interference environments, , Substituting, we get: , in: T f The filtering time constant controls the filtering intensity. T f The larger the value, the stronger the filtering effect; s is the Laplace variable.

[0026] Furthermore, the hierarchical fusion operation of radar point cloud features and visual semantic features described in S6 includes the following steps: To achieve spatial alignment between the camera and the 3D LiDAR, joint calibration is required to obtain the projection matrix from the LiDAR frame to the camera plane. The extrinsic parameter matrix between the camera and LiDAR coordinate systems is calculated using the following formula to achieve spatial consistency between the two coordinate systems.

[0027] am,i:θm,iX+dm,i=0 an,i:θn,iX+dn,i=0 In the formula: am,i and an,i are the planar models of the camera and the LiDAR, respectively; θ is the plane normal vector; and d is the distance from the plane to the far point.

[0028] S6-2 minimizes the objective function by calculating the projected position of each point in the sample point cloud onto the camera image, as well as the error between the predicted and actual positions. Then, the errors of all points are weighted and summed, with the weight being the number of points in the point cloud. By minimizing this error, the optimal extrinsic parameter matrix can be obtained, thus achieving joint calibration of the camera and LiDAR.

[0029] , In the formula: R and T are the rotation and translation parts of the extrinsic parameter matrix from the lidar coordinate system to the camera coordinate system, respectively, and n is the number of samples. l ( i ) is the first i The number of points in a sample point cloud. For the first i The dimensional coordinates of each sample. θ m,i and d m,i , where are the projection angle and distance of the sample in the camera image, respectively, and m is a parameter in the camera intrinsic parameter matrix.

[0030] The beneficial effects of the above-mentioned technical solution of the present invention are as follows: LiDAR and visual multimodal fusion technology demonstrates significant advantages in dynamic obstacle avoidance. Leveraging LiDAR's centimeter-to-millimeter high-precision ranging capabilities and dense 3D point cloud data (over 100 points per square meter), the system can accurately construct the geometric contours of obstacles and track the motion trajectories of dynamic targets (such as pedestrians and vehicles) in real time. Meanwhile, depth cameras supplement the semantic details of obstacles with color and texture information (such as identifying traffic signs and vehicle types). Together, they achieve comprehensive environmental understanding and accurate intent prediction. In complex scenarios, LiDAR, with its resistance to light interference (such as strong light and nighttime), stably detects obstacle positions (e.g., using the ICP algorithm). Visual sensors, through deep learning models, correct point cloud noise in rain and snow. Combined with robust fusion algorithms (such as spatiotemporal alignment, incomplete differential PID, and Kalman filtering), this effectively reduces false detection and false negative rates, ensuring reliable obstacle avoidance in occlusion or sudden situations (such as pedestrians emerging from behind trees or vehicles suddenly changing lanes). In addition, the high-frequency synchronous processing of multimodal data (10-20Hz for LiDAR, 30-60fps for vision) keeps the system response latency within 50 milliseconds, balancing real-time performance and energy consumption optimization. It can be flexibly applied to scenarios such as urban traffic congestion, unmanned warehousing and logistics, and driving in adverse weather conditions, providing high-precision, all-scenario, and highly safe redundancy technical support for dynamic obstacle avoidance. Attached Figure Description

[0031] Figure 1 A detailed flowchart of the radar obstacle avoidance technology solution provided in the embodiments of the present invention; Figure 2 This is a schematic diagram of a radar obstacle avoidance user parameter adjustment platform provided in an embodiment of the present invention; Figure 3 A radar detection range limitation diagram provided for an embodiment of the present invention; Figure 4 This is a schematic diagram illustrating the positional relationship between the vehicle camera and the radar provided in an embodiment of the present invention; Figure 5 This is a flowchart of the standard Kalman filter update provided in an embodiment of the present invention; Figure 6 The Kalman filter error analysis diagram provided in this embodiment of the invention.

[0032] Figure 7 The system error analysis diagram provided in the embodiment of the present invention. Detailed Implementation

[0033] To make the technical problems, technical solutions and advantages of the present invention clearer, a detailed description will be given below in conjunction with the accompanying drawings and specific embodiments.

[0034] This invention addresses the problems of low detection accuracy, high false alarm rate, slow calculation speed, limited positioning accuracy, and susceptibility to environmental interference in existing radar-based obstacle avoidance systems. It provides a dynamic control method for highway autonomous vehicles based on multimodal LiDAR point cloud data fusion. Example 1

[0035] To address the aforementioned problems, this invention proposes a technical solution for dynamic control of autonomous vehicles on highways based on multimodal LiDAR point cloud data fusion. This includes: S1. Obtain raw point cloud data; S2. Perform adaptive filtering on the raw point cloud data; S3. Obtain point cloud data of the detected object through 3D-3D pose estimation; S4. Use the DBSCAN algorithm to extract features from the image point cloud data; S5. Based on the acquired point cloud data, use the KF algorithm to estimate the position and state. S6. Based on the acquired point cloud data, a PID algorithm is used for speed control; S7. Perform hierarchical fusion of radar point cloud features and visual semantic features.

[0036] The technical solution for dynamic control of autonomous vehicles on highways based on multimodal LiDAR point cloud data fusion described in this invention significantly improves the feature extraction accuracy of point cloud data for unmanned vehicles by adaptively filtering the original point cloud data, combining it with 3D-3D pose estimation to obtain the point cloud data of the detected object, and using the DBSCAN algorithm to extract features from the image point cloud data. This method can handle complex environmental changes and effectively address problems such as low detection accuracy, high false alarm rate, and slow computation speed. By introducing the KF algorithm, the positioning accuracy under linear systems is improved, thus solving the problem of limited positioning accuracy. The introduction of the PID algorithm improves the robustness of motion control. By extracting and fusing radar point cloud features with visual semantic features, the system's detection accuracy for objects in complex environments is improved.

[0037] To better understand the technical solution for dynamic control of highway autonomous vehicles based on multimodal LiDAR point cloud data fusion as described in the embodiments of the present invention, it is described in detail below. The method specifically may include: S1, Obtain raw point cloud data: S1-1. In this study, a combined hardware solution of LiDAR and depth camera was adopted to achieve more efficient target object position detection. This solution combines real-time vehicle monitoring and data recording functions, aiming to improve road safety and the efficiency of intelligent transportation systems. The depth camera used is Orbbec ASTRAS, which can not only accurately locate objects but also display real-time vehicle speed, with a resolution of up to 1280x1024 and an error of 1mm / m.

[0038] S1-2. Further practical and platform-based processing of the originally fixed radar measurement data; the improved platform is as follows: Figure 2 As shown, users can achieve the desired effect by optimizing parameters such as diagonal resolution (the angular interval between two adjacent point clouds of the LiDAR), field of view (the field of view that the LiDAR can detect), frame rate (the number of scans that the LiDAR completes per second), point frequency (the number of point clouds that the LiDAR can acquire per second), and reflectivity (the proportion of the point cloud that can be returned after the LiDAR scans the target object).

[0039] S1-3. Based on the platform, an intelligent limitation function was developed. The effective detection boundary is set by distance_min=10.0m and distance_max=360.0m. The is_parted parameter is used to activate the multi-area scanning mode (such as simultaneously monitoring the 40°-50° area of ​​the main road and the 130°-140° area of ​​the auxiliary road). range_preco=0.5 is used to preset the target motion prediction compensation amount. Based on the P_v / P_w differential term parameter, high-speed target trajectory jitter is suppressed. At the same time, environmental anti-interference is realized. The reflectivity filtering algorithm automatically removes noise points below the threshold (D_v=0.0) and dynamically adjusts angle_start / end to avoid strong reflection interference sources (such as traffic signs).

[0040] S1-4 Simultaneously, the platform is also equipped with features to limit the radar detection range, preventing irrelevant factors from affecting experimental results. For example... Figure 3 As shown.

[0041] S2, performs adaptive filtering on the raw point cloud data: S2-1. By using voxel filtering and the voxelization method, the number of point clouds can be effectively reduced while preserving the shape features of the point clouds, thereby improving the running speed of the algorithm.

[0042] S2-2. A minimum number of points constraint is added to the voxel filtering, that is, the number of points in each voxel grid must reach the minimum constraint value MP, otherwise the voxel grid range is expanded until the number of points is not less than MP.

[0043] S2-3. The density of a point cloud is defined as the minimum Euclidean distance between any point in the point cloud and every other point, and then the average of these minimum distances is taken. The voxel-filtered point cloud data is further processed by fusing point cloud density features, optimizing radius filtering into an adaptive parameter adjustment mechanism. Large-radius filtering and small-radius filtering models are constructed, with the following functions: , In the formula: F(R L ,N L () represents a set of numerical pairs of the large filtering radius and the nearest neighbor points. R L For filtering large radius, N L α is the number of nearest neighbors; α is the large filter coefficient; M S It is a multiple of density; This represents the density of point clouds.

[0044] The filtering accuracy can be improved by changing the value of the large radius filter coefficient α; let's take α=110. Since the filter radius is set larger than the point cloud density, the value of ML is in the range [20, 50]. Furthermore, setting ML to the center segment of this range (30-40) is sufficient for most situations; the non-center segment is only used as an alternative value to expand applicability.

[0045] Since different point cloud data have different point cloud densities, each point should have α·ML nearest neighbor points within a circle with the point cloud density as the radius. If there are none, it means that the density of the point is less than the average value and should be judged as an isolated noise point.

[0046] S2-4. After the minimum number of points constraint and adaptive large radius filtering, outlier noise points and outlier noise clusters will be largely eliminated. Then, adaptive small radius filtering is added to remove small-scale noise.

[0047] , In the formula: F(R S ,N S ) For a set of numerical pairs of small filter radius and nearest neighbor points. R S For filtering with a small radius, N S Number of nearest neighbors; β Small filter coefficients; M S It is a multiple of density.

[0048] In the small radius filtering model, β=120 is chosen to meet the accuracy requirements. Since the point cloud density is not recalculated after the large radius filtering, the point cloud density value is slightly inflated when incorporated into the small radius filtering model. Furthermore, since β amplifies more than α, the density factor MS should not be too small when selecting it. Therefore, the value of MS is in the range of [30, 70], and the middle segment of the range of 40 to 60 is usually sufficient.

[0049] The algorithm's performance is shown in the table below: Table 1

[0050] S3, obtains point cloud data of the detected object through 3D-3D pose estimation: The first set of point cloud data was obtained using lidar. The second set of point cloud data was obtained after pose transformation. To obtain the camera's rotation Peaceful relocation Without error, from Coordinate system transformation to The formula is:

[0051] qi is point cloud data pi represents point cloud data. R is the radar rotation matrix, and t is the translation matrix.

[0052] However, due to noise and mismatches (such as...) The existence of (which do not actually correspond to the same point in space, but the feature matching algorithm mistakenly assumes they are the same point) means that the above equation does not always hold true. Therefore, the objective function we want to minimize is:

[0053] The optimal solution is obtained by decomposing the solution using the SVD method, as follows: We take the centroids of two sets of point clouds. And the following actions were taken: , , , , , make The objective function simplifies to: , set up To find the optimal solution, the optimization problem can be divided into two steps: , , After SVD decomposition, we obtain: , , This completes the point cloud data preprocessing.

[0054] After detecting the position of objects in space, the next step is to process the detected data.

[0055] S4, the DBSCAN algorithm is used to extract features from the image point cloud data; The algorithm steps are as follows: S4-1: Specify appropriate e and Minpoints (e is similar to the radius, and Minpoint is the minimum number of samples, for the subsequent determination of the core objects and their clusters). S4-2: Calculate all sample points. If point p has more than Minpoints points in its neighborhood, create a new cluster with p as the core point (p is the core object).

[0056] S4-3: Repeatedly find points that are directly density-reachable (and then density-reachable) from these core points, add them to the corresponding clusters, and merge clusters where the core points are density-connected.

[0057] S4-4: The algorithm terminates when no new points can be added to any cluster.

[0058] S5. Based on the acquired point cloud data, the KF algorithm is used to estimate the position and state: The Kalman filter algorithm is used to estimate the position and motion state of the target object, thereby enabling the prediction and updating of the target trajectory.

[0059] The prediction model, update model, and Kalman gain formula for Kalman filtering are as follows: , The ICP matching results of target point cloud clusters in two adjacent frames are used as the state variables of the Kalman filter, and the target tracking results are used as the observations to predict and update the trajectory. The state variables are represented as follows: , In the formula, (xk, yk, zk) are the coordinates of the centroid of the point cloud cluster after clustering and target recognition at time k in the roadside LiDAR coordinate system; the transition matrix is: , The observations are: , The observation matrix is: , The standard Kalman filter update process is as follows: Figure 5 As shown, by iterating between prediction (system model) and update (measurement value), the weights are dynamically adjusted to balance the credibility of model prediction and actual measurement. This effectively suppresses measurement noise and system noise, fuses multi-source information (such as sensor data), and outputs an estimation result closer to the true value. The Kalman gain is dynamically adjusted using the covariance matrix. By balancing the weights of the model's predictions and measurements, the optimal solution for the location can be found. The resulting errors are shown in Table 1, and the error analysis is as follows: Figure 6 As shown.

[0060] Table 2 , S6 uses a PID algorithm for speed control based on the acquired point cloud data. S6-1. Since the radar position is opposite to the actual position, we need to linearly invert it, as shown in the following formula: , in, The angle read by the car.

[0061] Next, to prevent unnecessary energy loss during the car's movement, we adjusted the radar angle as follows: , , Ultimately, after multiple learning and adjustments, the system will be able to accurately detect objects and precisely plan their movement trajectories, reducing energy consumption and providing strong protection for subsequent vehicle driving safety.

[0062] S6-2. The PID algorithm has three main components: the proportional term (the controller immediately takes control action once a deviation occurs, causing the control quantity to change in the direction of reducing the deviation), the integral term (to eliminate the system deviation), and the derivative term (not only to respond immediately to the deviation, but also to provide appropriate correction in advance based on the trend of the deviation). The PID formula is as follows: , in, e(t) For the data at the current location, K P This is the proportionality coefficient; T t The integral time constant; T D is the differential time constant.

[0063] Furthermore, in order to improve the system's robustness under interference environments, , Substituting, we get:

[0064] in: T f The filtering time constant controls the filtering intensity. T f The larger the value, the stronger the filtering effect. s is a Laplace variable.

[0065] This approach significantly reduces the impact of noise on the control output, improves the system's robustness in noisy environments, and makes the system more stable when tracking rapidly changing setpoints, reducing the risk of overshoot and oscillation. By adjusting the filter time constant, a trade-off can be struck between noise suppression and differential response speed to adapt to different scenario requirements. Error analysis is as follows: Figure 7 As shown.

[0066] S7. Perform hierarchical fusion of radar point cloud features and visual semantic features: To achieve spatial alignment between the camera and the 3D LiDAR, joint calibration is required to obtain the projection matrix from the LiDAR frame to the camera plane. The relative positions of the LiDAR and the depth camera are as follows: Figure 4 As shown.

[0067] The extrinsic parameter matrix between the camera and the lidar coordinate system is calculated according to equation (2) to achieve spatial consistency between the two coordinate systems.

[0068] am,i:θm,iX+dm,i=0 an,i:θn,iX+dn,i=0 (2) In the formula: am,i and an,i are the planar models of the camera and the LiDAR, respectively; θ is the plane normal vector; and d is the distance from the plane to the far point.

[0069] By minimizing the objective function, we find the random variables R and T that minimize the objective function (3). For each point in the sample point cloud, we calculate its projected position on the camera image and the error between the predicted and actual positions. Then, we sum the errors of all points using a weighted average, where the weight is the number of points in the point cloud. By minimizing this error, we can obtain the optimal extrinsic parameter matrix, thereby achieving joint calibration of the camera and lidar.

[0070]

[0071] In the formula: R and T are the rotation and translation parts of the extrinsic parameter matrix from the lidar coordinate system to the camera coordinate system, respectively, and n is the number of samples. l ( i ) is the first i The number of points in a sample point cloud. For the first i The dimensional coordinates of each sample. θ m,i and d m,i , where are the projection angle and distance of the sample in the camera image, respectively, and m is a parameter in the camera intrinsic parameter matrix.

[0072] The ICP algorithm described in this embodiment of the invention acquires point cloud data of the area to be detected and further processes the obtained point cloud data; it uses an incomplete differential PID algorithm, incorporating the filtering time constant and Laplace variable into the standard PID formula to establish a control system function; based on the established control system function, it detects external obstacles to be detected and determines the obstacle range. Thus, by converting radar and visual cameras, radar and depth cameras can cooperate, thereby improving the speed and accuracy of obstacle detection; the use of Kalman filtering improves the accuracy of object localization.

[0073] The above description represents the preferred embodiments of the present invention. It should be noted that those skilled in the art can make various improvements and modifications without departing from the principles of the present invention, and these improvements and modifications should also be considered within the scope of protection of the present invention.

Claims

1. A method for dynamic control of an automatic driving vehicle on a highway based on multi-modal LiDAR point cloud data fusion, comprising: obtaining original point cloud data; performing adaptive filtering operation on the original point cloud data; obtaining point cloud data of a detected object through 3D-3D pose estimation; extracting features from the image point cloud data using a DBSCAN algorithm; performing position state estimation using a KF algorithm based on the obtained point cloud data; performing speed control using a PID algorithm based on the obtained point cloud data; performing hierarchical fusion of radar point cloud features and visual semantic features.

2. The highway autonomous vehicle dynamic control method of claim 1, wherein An adaptive radius filtering operation is introduced, and the method comprises: using a voxelization method to effectively reduce the number of point clouds while retaining the shape features of the point clouds through voxel filtering, thereby improving the running speed of the algorithm; adding a minimum point number constraint to the voxel filtering, i.e., the number of points in each voxel grid must reach a minimum limit MP, otherwise the voxel grid range is expanded until the number of points is not less than MP; further processing the point cloud data after voxel filtering by fusing point cloud density features to optimize the radius filtering to parameter adaptive adjustment; a large radius filtering model and a small radius filtering model are constructed, and their functions are as follows: ， where F R L ,N L is a set of numerical pairs of large filtering radius and near neighbor points, R L is a large filtering radius, N L is a number of near neighbor points; and M S is a density multiplier; represents a point cloud density. After the minimum point number constraint and adaptive large radius filtering, the outlying isolated noise points and outlying noise clusters are largely removed, and then adaptive small radius filtering is added to remove small scale noise; ， where F R S ,N S is a set of numerical pairs of small filter radius and near neighbor points, R S is a small filter radius, N S is a number of near neighbor points; β is a small filter coefficient; M S is a density multiplier.

3. The highway autonomous vehicle dynamic control method of claim 1, wherein performing 3D-3D target object pose state estimation using an ICP algorithm, comprising: obtaining paired 3D point cloud data, which is paired by the two points with the shortest Euclidean distance, and the relative position function is as follows: ， wherein, q i for point cloud data coordinates, p i for point cloud data coordinates, R is a radar rotation matrix, and t is a translation matrix; solving R and t that make the error sum of squares reach a minimum by constructing a least squares problem, and the loss function is as follows: 。 4. The highway autonomous vehicle dynamic control method of claim 1, wherein extracting features from the data using a DBSCAN algorithm, which includes the following content: a density-based method that clusters based on the density of data objects and stops when the density exceeds a certain threshold, which can discover irregularly shaped data cluster groups; and constructing the shape of the target object point cloud data.

5. The highway autonomous vehicle dynamic control method of claim 1, wherein performing position state estimation, which includes the following content: using the ICP matching results of the target point cloud clusters of adjacent two frames as the state quantity of Kalman filtering, and using the target tracking results as the observation value, to predict and update the trajectory; the state quantity is represented as: ， ， In the formula, x k ,y k ,z k is the coordinate of the centroid point of the point cloud cluster after clustering and target recognition of the target object point cloud at time k in the roadside LiDAR coordinate system.

6. The highway autonomous vehicle dynamic control method of claim 1, wherein performing speed control using an incomplete differential PID algorithm, and the speed control system function is: ， Wherein, T f is a filter time constant, controlling filter strength, T f The greater, the stronger the filtering effect; s is a Laplace variable; K p is a proportional coefficient; K i is an integral time constant; K d is a differential time constant.

7. The highway autonomous vehicle dynamic control method of claim 1, wherein after obtaining the point cloud data, using Kalman filtering and PID algorithm to obtain position information and control speed, the method further comprises: a depth camera synchronously obtains RGB-D data through structured light or ToF technology, combines three-dimensional reconstruction and real-time processing, and realizes precise environment perception and obstacle avoidance navigation; minimizing the objective function, for each point in each sample point cloud, calculating its projection position on the camera image and the error between the predicted position and the actual position; then, the errors of all points are weighted and summed, and the weight is the number of each point in the point cloud; by minimizing this error, the optimal extrinsic parameter matrix can be obtained, thereby realizing the joint calibration of the camera and the laser radar; ， wherein R and T are rotation and translation from the laser radar coordinate system to the camera coordinate system, n is the number of samples, l i is the number of points of the i th sample point cloud, is the dimension coordinate of the i th sample, ​ m,i and d m,i are the projection angle and distance of the sample in the camera image respectively, and m is the parameter in the camera intrinsic matrix.​

Images