Unmanned vehicle environment perception and obstacle avoidance control method in complex dynamic scene

Abstract

The invention discloses an unmanned vehicle environment perception and obstacle avoidance control method in a complex dynamic scene, relates to the technical field of unmanned vehicle positioning and navigation, and is used for solving the problems of low positioning accuracy, low stability, poor real-time response capability and the like in a dynamic complex environment in an existing method. The method is specifically achieved through an unmanned vehicle environment sensing and obstacle avoidance control system, the unmanned vehicle environment sensing and obstacle avoidance control system is provided with a sensor module, a vehicle-mounted computer module and a chassis control module, and the vehicle-mounted computer module is connected with the sensor module and the chassis control module. The sensor module is provided with a solid-state laser radar, a multi-line mechanical laser radar and an inertial measurement unit which are installed on the unmanned vehicle, and the chassis control module is provided with an accelerator and brake calibration meter and a driving execution unit. The method has the advantages that the positioning accuracy, stability and real-time response capability of the unmanned vehicle in a dynamic complex environment are effectively improved.

Description

TECHNICAL FIELD

[0001] The present application relates to the field of unmanned vehicle positioning and navigation technology, in particular to an unmanned vehicle environment perception and obstacle avoidance control method in a complex dynamic scene. BACKGROUND

[0002] With the wide application of intelligent technology in industrial and civil fields, the demand for autonomous operation of unmanned vehicles in complex dynamic environments is increasing. In tasks such as logistics transportation, emergency rescue, and park inspection, unmanned vehicles can effectively replace human labor to perform repetitive or high-risk tasks, improving task efficiency and safety. However, actual operating environments often have high dynamicity and uncertainty, such as varying terrain, complex obstacle shapes, severe dust interference, and frequent multi-source signal interference, which pose serious challenges to the environment perception, path planning, and real-time obstacle avoidance capabilities of unmanned vehicles.

[0003] Existing technical solutions rely on single sensor information or path planning algorithms based on static environment assumptions, making it difficult to respond to sudden dynamic obstacles and complex working condition changes in the environment. Such methods not only lead to incomplete environment modeling and high dynamic obstacle detection rate, but also may cause obstacle avoidance failure, motion jitter, and even system out-of-control due to mismatch between planned path and actual control response, severely limiting the reliable application of unmanned vehicles in real complex scenes. Therefore, there is an urgent need for an unmanned vehicle environment perception and obstacle avoidance control method that can integrate multi-source sensor information and has high robustness and real-time response capability to support precise positioning, stable navigation, and efficient task execution in dynamic complex environments.

[0004] The existing technical solution has the following disadvantages: the existing unmanned vehicle environment perception and obstacle avoidance control method has low positioning accuracy, low stability, and poor real-time response capability in dynamic complex environments. SUMMARY

[0005] The unmanned vehicle environment perception and obstacle avoidance control method in a complex dynamic scene provided by the present application can improve the positioning accuracy of unmanned vehicles in dynamic complex environments.

[0006] To achieve the above-mentioned purpose, the unmanned vehicle environment perception and obstacle avoidance control method in a complex dynamic scene provided by the present application comprises the following steps: step S1: constructing an unmanned vehicle environment perception and obstacle avoidance control system, which is provided with a sensor module, a vehicle-mounted computer module, and a chassis control module, the vehicle-mounted computer module is connected to the sensor module and the chassis control module respectively, the sensor module is provided with a solid-state laser radar, a multi-line mechanical laser radar, and an inertial measurement unit installed on the unmanned vehicle, and the chassis control module is provided with a throttle brake calibration table and a driving execution unit;

[0007] The vehicle-mounted computer module is provided with an initialization unit, a point cloud fusion unit, a data optimization unit, a feature extraction unit, an environment modeling and updating unit, a path planning unit, and a control and drive unit connected in sequence.

[0008] Step S2: The unmanned vehicle is stationary at the starting position and powered on for initialization. The initialization unit acquires the initial data of the sensor module and then obtains the external parameters between the solid-state lidar coordinate system and the multi-line mechanical lidar coordinate system through the iterative nearest point algorithm ICP calibration, thus completing the initial calibration of the sensor module.

[0009] Step S3: The unmanned vehicle starts up, and the solid-state lidar and multi-line mechanical lidar acquire point cloud data around the unmanned vehicle in real time. And transmit it to the on-board computer module;

[0010] The inertial measurement unit acquires the linear acceleration of the unmanned vehicle in real time. and angular acceleration Data is then transmitted to the onboard computer module.

[0011] Step S4: The point cloud fusion unit in the vehicle-mounted computer module processes the point cloud data. Perform coordinate alignment and point cloud fusion operations to generate a preliminary fused point cloud. And pass it to the data optimization unit;

[0012] Step S5: The data optimization unit optimizes the preliminary fused point cloud. EKF denoising, Laplacian optimization, and dynamic modeling are performed sequentially to obtain the final fused point cloud. And pass it to the feature extraction unit and the environment modeling update unit;

[0013] Step S6: The feature extraction unit uses a method combining local ground height and global plane equations to extract the final fused point cloud. The non-ground points in the data are dynamically filtered and then optimized to obtain the obstacle point set. And pass it to the environment modeling update unit;

[0014] Step S7: The environment modeling update unit updates the environment based on the set of obstacle points. and the final fusion point cloud Perform environmental modeling and real-time map updates;

[0015] Step S8: The path planning unit updates the real-time map constructed by the environment modeling unit based on the linear acceleration. and angular acceleration The data is used to perform global path search and local obstacle avoidance planning, generate a planned path, and transmit it to the control drive unit;

[0016] Step S9: The control drive unit, based on the control algorithm of the throttle and brake calibration table, controls the drive execution unit to drive the unmanned vehicle to run according to the planned path.

[0017] Through the above design, this invention uses a solid-state lidar for forward scanning and a 16-line mechanical lidar for ring scanning, and integrates an inertial measurement unit (IMU) to obtain multi-source fusion data. First, the external parameters of the multiple lidars are calibrated, and the coordinates of the multi-source point clouds are aligned and weighted for fusion. Dynamic height filtering is used to remove ground points during fusion. Then, obstacle points are projected into a two-dimensional occupied grid map, and global path search and local obstacle avoidance are performed on this map. Finally, the chassis actuator is driven by a throttle / brake calibration table to achieve closed-loop path tracking control.

[0018] The method provided by this invention balances real-time performance and robustness, and is suitable for high-precision positioning and safe obstacle avoidance in complex dynamic scenarios.

[0019] Preferably, the unmanned vehicle is placed in an open and flat area, and the initialization unit acquires the initial point cloud data collected by the solid-state lidar and the multi-line mechanical lidar respectively, and then uses the iterative nearest point algorithm (ICP) to perform point cloud registration.

[0020] The ICP algorithm iteratively searches for the nearest point pair between the point cloud to be registered and the reference point cloud, and then optimizes the extrinsic parameters using the least squares error function. The extrinsic parameters include rotation and translation matrices.

[0021] The Gauss-Newton method is used to optimize the residual function during the iteration process, resulting in the following formula for the least squares iteration objective of ICP:

[0022] ;

[0023] in, For multi-line mechanical lidar point clusters, For solid-state lidar point aggregation; This is the pose transformation matrix, i.e., the rotation matrix or the translation matrix; This is the index of the point pair currently being calculated. This represents the number of point pairs participating in the matching.

[0024] The above design avoids getting trapped in local optima and ensures rapid convergence of the registration process, thereby obtaining the optimal rotation matrix between the two types of lidar point clouds and achieving accurate alignment of point cloud data in different coordinate systems.

[0025] Preferably, in step S4, the point cloud fusion unit first uses a rigid transformation matrix. The point cloud data Mapping to the robot's unified coordinate system and completing the coordinate alignment operation, the expression is as follows:

[0026] ;

[0027] in, For the first Radar point cloud coordinates, for The corresponding point cloud coordinates in the fused coordinate system The optimized rotation matrix, This is the optimized translation matrix;

[0028] The point cloud fusion unit then uses a loosely coupled approach to perform weighted fusion on the aligned point cloud data to generate a preliminary fused point cloud. The expression is:

[0029] ;

[0030] in, For the first The confidence weights of the radar point clouds after fusion and unification in the X, Y, and Z directions satisfy the following: , This indicates the number of radar point clouds participating in the fusion, which is dynamically adjusted through point cloud density. The expression is:

[0031] ;

[0032] in, For the first The radar number The confidence weights of points in the XYZ directions. For the first The radar number The distance from a point to the center of its local neighborhood. This is an adjustment coefficient used to enhance the fusion weights in sparse or dense regions.

[0033] The above design aligns the point cloud data collected by each radar, effectively eliminating coordinate differences caused by different radar installation positions and attitudes, and improving the accuracy and reliability of subsequent data fusion.

[0034] Preferably, in step S5, to improve the accuracy and reliability of the fused point cloud, the data optimization unit uses an error-spreading Kalman filter (EKF) to process the preliminary fused point cloud. The state update model for denoising and outlier removal is as follows:

[0035] ;

[0036] in, This is the estimated point cloud coordinates after filtering at the current time. This represents the predicted state at the previous moment. These are the point cloud observations at the current moment. Here is the Kalman gain matrix. For the observation matrix, Indicates time; after EKF processing, the denoised point cloud output is obtained as follows: ;

[0037] Secondly, to enhance the local smoothness and sparsity of the point cloud, the data optimization unit outputs the denoised point cloud. Perform Laplace constraint regularization, with the optimization objective being:

[0038] ;

[0039] in, The point cloud optimized by Laplacian constraint regularization, This represents the Hilbert-Schmidt norm (Frobenius norm). The Laplacian matrix of the point cloud; The weighting coefficients are smoothed to ensure that outliers are eliminated while maintaining the overall structure of the point cloud. This represents the optimal estimate that minimizes the cost. This represents the original point cloud matrix collected by each sensor. Represents topological regularization terms;

[0040] Finally, to account for the dynamic characteristics of point cloud changes over time, the data optimization unit will optimize the obtained data... As input for system dynamic modeling, a dynamic evolution model of the point cloud with continuous temporal sequence is established, and the model expression is:

[0041] ;

[0042] in, , Here is the system state transition matrix. To control the input, For system noise, Indicates system process noise; To measure noise, Indicates observation noise; This represents the predicted state vector for the next time step. This represents the system state vector at the current moment. Represents the observation matrix. It represents the observation at the next moment.

[0043] This invention overcomes the limitations of single-radar perception by fusion of multiple radar point clouds through coordinate alignment and loose coupling; and improves point cloud quality by removing noise and outliers from the fused point cloud through error Kalman filtering.

[0044] Through the above-mentioned coordinate alignment, weighted fusion, EKF denoising, Laplacian optimization, and dynamic modeling, a high-precision fused point cloud can be obtained. It is used for subsequent environmental mapping, feature extraction and high-precision robot positioning.

[0045] This invention effectively improves the environmental perception accuracy and robustness of the system by performing data fusion and optimization processing on multi-radar point clouds.

[0046] Preferably, in step S6, the feature extraction unit employs a dynamic height filtering method that combines local ground height estimation with global plane equation compensation to dynamically filter and optimize non-ground points, thereby enhancing the extraction accuracy and robustness of obstacle points in complex environments. Specifically, this includes the following steps:

[0047] S-A1: The feature extraction unit will fuse the final point cloud Divided into a set of ground points Set of non-ground points To improve the efficiency of ground height estimation, and to set up ground points... A KD-Tree structure is built on top of this to quickly search for neighboring ground points; let the total number of the original point cloud be... Then we have:

[0048] ;

[0049] S-A2: For any non-ground point In the local neighborhood Select the K nearest ground points Local ground height is calculated by weighted average. The expression is:

[0050] ;

[0051] in, This represents a local ground elevation estimate. Indicates the weighting coefficient. This represents the height value of the j-th neighboring ground point, where j represents the neighbor index number. Indicates index iteration;

[0052] Weighting coefficient The calculation expression is:

[0053] ;

[0054] in, The Gaussian kernel bandwidth parameter is used to ensure that neighboring points contribute more and distant points have less influence, thereby improving the stability of the estimation.

[0055] S-A3: If not a ground point Non-ground points in the local neighborhood If there are no valid points within the surface, the global plane equation is used to calculate the ground height, expressed as:

[0056] ;

[0057] in, b, c, and d are the global ground model coefficients obtained by fitting using the least squares method, where b controls the tilt direction, c determines the normal component, and d is the offset; this ensures the robustness of the algorithm in sparse or noisy scenes.

[0058] S-A4: Calculate non-ground points relative height difference The expression is:

[0059] ;

[0060] in, This represents the relative height difference, where z represents the actual height coordinate of the current point;

[0061] To accommodate ground fluctuations in different regions, a dynamic threshold based on statistical characteristics is set as follows:

[0062] ;

[0063] ;

[0064] in, This represents the average of the local relative heights. The standard deviation of the local relative height. This is the adjustment coefficient;

[0065] If not a ground point relative height difference The following conditions must be met:

[0066] ;

[0067] Non-ground points Retained as an obstacle point;

[0068] S-A5: Calculated set of obstacle points :

[0069] ;

[0070] And remove ground points by combining outlier points by radius:

[0071] ;

[0072] in, Indicated by point Centered on, The set of the radius's neighborhood; This represents the minimum number of neighbors threshold;

[0073] And Euclidean clustering constraints:

[0074] ;

[0075] in, To optimize the 3D spatial coordinate vector of the m-th point in the previous point cloud, To and The three-dimensional spatial coordinate vector of the nth adjacent point, Indicates the cluster radius threshold;

[0076] The final optimized set of obstacle points is obtained. This provides high-quality input for subsequent mapping.

[0077] Through the above design, a method combining local ground height and global plane equations is used to dynamically filter non-ground points, thereby enhancing the accuracy of obstacle point extraction.

[0078] Preferably, in step S7, the environment modeling update unit updates the obstacle point set. and the final fusion point cloud Environmental modeling and real-time map updates involve the following steps:

[0079] S-B1: The environment modeling and updating unit first calculates the obstacle point cloud. Find the boundary, and obtain the minimum and maximum points. , And based on this, determine the size of the two-dimensional raster map:

[0080] ;

[0081] in, For raster resolution, The width of the map. The height of the map. The x-axis is... The vertical axis is used as the coordinate.

[0082] S-B2: The environment modeling update unit maps point cloud coordinates to a two-dimensional raster index, completing the environment modeling.

[0083] ;

[0084] in, Indicates column index, This indicates the row index and marks the corresponding raster as occupied.

[0085] S-B3: To accelerate the projection process from point cloud to raster, the environment modeling update unit updates the obstacle point set according to Euclidean distance and point density characteristics. Data segmentation is performed to obtain s subsets of obstacle point clouds, where s ≥ 2. Then, a multi-threaded framework (such as OpenMP) is used to traverse all obstacle point cloud subsets in parallel. Each thread independently processes one subset of obstacle point clouds, and atomic operations or critical sections are used to protect shared raster data to complete the real-time update of the 2D raster map.

[0086] ;

[0087] in, This indicates that two occupies the position in the grid map at the th position. OK, The value of a cell in a column is 100, which indicates that the cell is occupied by an obstacle. Unvisited cells are set to -1.

[0088] Based on the above design, after obstacle point extraction, the 3D point cloud needs to be mapped into a 2D occupancy grid map for environmental modeling and path planning of unmanned vehicles. To meet real-time requirements, this invention proposes an occupancy grid generation method based on multi-threaded parallel computing, which significantly improves the grid generation speed and enables efficient environmental modeling and real-time map updates in the ROS environment (the software framework environment of the robot operating system), meeting the needs of unmanned vehicles for rapid mapping and navigation in complex environments.

[0089] Preferably, in step S8, the path planning unit performs global path search and local obstacle avoidance planning, specifically including the following steps:

[0090] S-C1: The path planning unit is based on the pose of the unmanned vehicle at the previous moment. and the linear acceleration measured by the inertial measurement unit. and angular acceleration are Perform prediction updates to obtain the initial pose. The expression is:

[0091] ;

[0092] ;

[0093] ;

[0094] in, This represents the initial pose of the unmanned vehicle at time t. The sampling period is The linear velocity at the previous moment. The angular velocity at the previous moment is obtained by integrating the IMU acceleration.

[0095] Then, using Kalman filtering or nonlinearization, the observation pose provided by LiDAR or visual SLAM is transformed. As an observation To perform an update, the update expression is:

[0096] ;

[0097] in, For the updated observation pose, For Kalman gain, For observation model parameters;

[0098] S-C2: The path planning unit uses the A* search algorithm to perform a global path search on the two-dimensional grid map to obtain the global path. ;

[0099] Global path cost function for:

[0100] ;

[0101] in, This represents the distance from the current location to the target point. The cost of grid occupancy state, , These are the weighting coefficients. This is the node row index at the k-th iteration. This is the node column index at the k-th iteration;

[0102] S-C3: During the movement of the unmanned vehicle, the sensor module detects dynamic obstacles in real time, and the path planning unit uses the dynamic obstacles to plan the path on the local map. Obstacle avoidance adjustments are made, and the local path planning cost function is:

[0103] ;

[0104] in, This represents the distance from the current location to the global path. For dynamic obstacle grid cost, , These are the weighting coefficients;

[0105] After completing multi-sensor data acquisition and obstacle point extraction, pose estimation needs to be corrected using inertial measurement unit (IMU) data, and the 3D point cloud needs to be mapped into a 2D occupancy grid map for mapping and path planning of unmanned vehicles in complex dynamic scenarios. To meet the real-time and robustness requirements in complex environments, this invention uses a localization and path planning method based on multi-sensor fusion and multi-threaded parallel computing. This method calculates the locally optimal path using the Temporally Elastic Band Local Path Planning (TEB) algorithm. By combining global paths, continuous path optimization and real-time obstacle avoidance are achieved.

[0106] S-C4: The path planning unit will determine the actual pose of the unmanned vehicle. With planned path Compare and generate path deviations Used for controller feedback:

[0107] ;

[0108] in, For speed and steering control variables, and This is the gain for PID control.

[0109] Through the above design, the unmanned vehicle combines inertial measurement unit data during the mapping process, uses a fusion algorithm to correct the pose estimation, and runs a path planning algorithm on the generated grid map, realizing global path search and local obstacle avoidance.

[0110] Preferably, in step S9, the control drive unit drives the drive execution unit based on the control algorithm of the throttle and brake calibration table, and the specific steps are as follows:

[0111] S-D1: When the unmanned vehicle is stationary, the control drive unit applies different throttle amounts in stages. The experimental dataset was obtained by recording the vehicle's speed and acceleration response curves over time.

[0112] ;

[0113] S-D2: The control drive unit constructs a three-dimensional mapping relationship between throttle position, speed, and acceleration based on the collected vehicle speed and acceleration data, and obtains the calibration surface equation through surface fitting methods (such as least squares fitting or polynomial regression):

[0114] ;

[0115] in, For throttle input, The vehicle's current speed, The vehicle's current acceleration, This indicates the relationship between throttle input, such as speed and acceleration.

[0116] S-D3: Analogous to the throttle modeling process, at the vehicle's maximum speed, the control drive unit applies different braking amounts step by step to form a brake gauge, which is used for vehicle deceleration and emergency braking control.

[0117] Through the above design, the present invention drives the chassis actuators by using a control algorithm based on the throttle and brake calibration table, thereby achieving consistency between the planned path and the actual vehicle movement, and thus supporting the highly robust obstacle avoidance of unmanned vehicles in complex dynamic environments.

[0118] The beneficial effects of this invention are: This invention provides an environmental perception and obstacle avoidance control method for unmanned vehicles in complex dynamic scenarios. This method can achieve accurate positioning of the vehicle itself in complex dynamic scenarios, and can also quickly, accurately and stably complete the tasks of environmental perception and obstacle avoidance, effectively improving the positioning accuracy, stability and real-time response capability of unmanned vehicles in dynamic and complex environments. Attached Figure Description

[0119] Figure 1 This is a schematic diagram of the method flow of the present invention;

[0120] Figure 2 This is a block diagram of the unmanned vehicle environmental perception and obstacle avoidance control system in the embodiment.

[0121] Figure 3 This is a flowchart of multi-threaded parallel computing in the embodiment;

[0122] Figure 4 This is a two-dimensional grayscale image of the throttle calibration curve in the embodiment;

[0123] Figure 5 This is a three-dimensional grayscale image of the throttle calibration curve in the embodiment. Detailed Implementation

[0124] The present invention will be further described in detail below with reference to the accompanying drawings and specific examples. The following embodiments or drawings are used to illustrate the present invention, but are not intended to limit the scope of the present invention.

[0125] like Figure 1 As shown, a method for environmental perception and obstacle avoidance control of unmanned vehicles in complex dynamic scenarios includes the following steps:

[0126] Step S1: Construct an environmental perception and obstacle avoidance control system for unmanned vehicles, such as... Figure 2 As shown, the unmanned vehicle environmental perception and obstacle avoidance control system is equipped with a sensor module, an on-board computer module, and a chassis control module. The on-board computer module is connected to the sensor module and the chassis control module respectively. The sensor module is equipped with a solid-state lidar, a multi-line mechanical lidar, and an inertial measurement unit installed on the unmanned vehicle. The chassis control module is equipped with a throttle and brake calibration table and a drive execution unit.

[0127] The vehicle-mounted computer module is provided with an initialization unit, a point cloud fusion unit, a data optimization unit, a feature extraction unit, an environment modeling and updating unit, a path planning unit, and a control and drive unit connected in sequence.

[0128] Step S2: The unmanned vehicle is stationary at the starting position and powered on for initialization. The initialization unit acquires the initial data of the sensor module and then obtains the external parameters between the solid-state lidar coordinate system and the multi-line mechanical lidar coordinate system through the iterative nearest point algorithm ICP calibration, thus completing the initial calibration of the sensor module.

[0129] Step S3: The unmanned vehicle starts up, and the solid-state lidar and multi-line mechanical lidar acquire point cloud data around the unmanned vehicle in real time. And transmit it to the on-board computer module;

[0130] The inertial measurement unit acquires the linear acceleration of the unmanned vehicle in real time. and angular acceleration Data is then transmitted to the onboard computer module.

[0131] Step S4: The point cloud fusion unit in the vehicle-mounted computer module processes the point cloud data. Perform coordinate alignment and point cloud fusion operations to generate a preliminary fused point cloud. And pass it to the data optimization unit;

[0132] Step S5: The data optimization unit optimizes the preliminary fused point cloud. EKF denoising, Laplacian optimization, and dynamic modeling are performed sequentially to obtain the final fused point cloud. And pass it to the feature extraction unit and the environment modeling update unit;

[0133] Step S6: The feature extraction unit uses a method combining local ground height and global plane equations to extract the final fused point cloud. The non-ground points in the data are dynamically filtered and then optimized to obtain the obstacle point set. And pass it to the environment modeling update unit;

[0134] Step S7: The environment modeling update unit updates the environment based on the set of obstacle points. and the final fusion point cloud Perform environmental modeling and real-time map updates;

[0135] Step S8: The path planning unit updates the real-time map constructed by the environment modeling unit based on the linear acceleration. and angular acceleration The data is used to perform global path search and local obstacle avoidance planning, generate a planned path, and transmit it to the control drive unit;

[0136] Step S9: The control drive unit, based on the control algorithm of the throttle and brake calibration table, controls the drive execution unit to drive the unmanned vehicle to run according to the planned path.

[0137] Furthermore, the unmanned vehicle is placed in an open and flat area, and the initialization unit acquires the initial point cloud data collected by the solid-state lidar and the multi-line mechanical lidar, and then uses the iterative nearest point algorithm (ICP) to perform point cloud registration.

[0138] The ICP algorithm iteratively searches for the nearest point pair between the point cloud to be registered and the reference point cloud, and then optimizes the extrinsic parameters using the least squares error function. The extrinsic parameters include rotation and translation matrices.

[0139] The Gauss-Newton method is used to optimize the residual function during the iteration process, resulting in the following formula for the least squares iteration objective of ICP:

[0140] ;

[0141] in, For multi-line mechanical lidar point clusters, For solid-state lidar point aggregation; This is the pose transformation matrix, i.e., the rotation matrix or the translation matrix; This is the index of the point pair currently being calculated. This represents the number of point pairs participating in the matching.

[0142] The above design avoids getting trapped in local optima and ensures rapid convergence of the registration process, thereby obtaining the optimal rotation matrix between the two types of lidar point clouds and achieving accurate alignment of point cloud data in different coordinate systems.

[0143] Furthermore, in step S4, the point cloud fusion unit first uses a rigid transformation matrix... The point cloud data Mapping to the robot's unified coordinate system and completing the coordinate alignment operation, the expression is as follows:

[0144] ;

[0145] in, For the first Radar point cloud coordinates, for The corresponding point cloud coordinates in the fused coordinate system The optimized rotation matrix, This is the optimized translation matrix;

[0146] The point cloud fusion unit then uses a loosely coupled approach to perform weighted fusion on the aligned point cloud data to generate a preliminary fused point cloud. The expression is:

[0147] ;

[0148] in, For the first The confidence weights of each radar point cloud in the XYZ directions satisfy the following conditions: , This indicates the number of radar point clouds participating in the fusion, which is dynamically adjusted through point cloud density. The expression is:

[0149] ;

[0150] in, For the first The radar number The confidence weights of points in the XYZ directions. For the first The radar number The distance from a point to the center of its local neighborhood. This is an adjustment coefficient used to enhance the fusion weights in sparse or dense regions.

[0151] The above design aligns the point cloud data collected by each radar, effectively eliminating coordinate differences caused by different radar installation positions and attitudes, and improving the accuracy and reliability of subsequent data fusion.

[0152] Furthermore, in step S5, firstly, the data optimization unit uses an error-spreading Kalman filter (EKF) to process the initially fused point cloud. The state update model for denoising and outlier removal is as follows:

[0153] ;

[0154] in, This is the estimated point cloud coordinates after filtering at the current time. This represents the predicted state at the previous moment. These are the point cloud observations at the current moment. Here is the Kalman gain matrix. For the observation matrix, Indicates time; after EKF processing, the denoised point cloud output is obtained as follows: ;

[0155] Secondly, the data optimization unit outputs the denoised point cloud. Perform Laplace constraint regularization, with the optimization objective being:

[0156] ;

[0157] in, The point cloud optimized by Laplacian constraint regularization, This represents the Hilbert-Schmidt norm (Frobenius norm). The Laplacian matrix of the point cloud; The weighting coefficients are smoothed to ensure that outliers are eliminated while maintaining the overall structure of the point cloud. This represents the optimal estimate that minimizes the cost. This represents the original point cloud matrix collected by each sensor. Represents topological regularization terms;

[0158] Finally, the data optimization unit will optimize the obtained data. As input for system dynamic modeling, a dynamic evolution model of the point cloud with continuous temporal sequence is established, and the model expression is:

[0159] ;

[0160] in, , Here is the system state transition matrix. To control the input, For system noise, Indicates system process noise; To measure noise, Indicates observation noise; This represents the predicted state vector for the next time step. This represents the system state vector at the current moment. Represents the observation matrix. It represents the observation at the next moment.

[0161] Through the above-mentioned coordinate alignment, weighted fusion, EKF denoising, Laplacian optimization, and dynamic modeling, a high-precision fused point cloud can be obtained. It is used for subsequent environmental mapping, feature extraction and high-precision robot positioning.

[0162] Furthermore, in step S6, the feature extraction unit employs a dynamic height filtering method that combines local ground height estimation with global plane equation compensation to dynamically filter and optimize non-ground points, enhancing the extraction accuracy and robustness of obstacle points in complex environments. Specifically, this includes the following steps:

[0163] S-A1: The feature extraction unit will fuse the final point cloud Divided into a set of ground points Set of non-ground points To improve the efficiency of ground height estimation, and to set up ground points... A KD-Tree structure is built on top of this to quickly search for neighboring ground points; let the total number of the original point cloud be... Then we have:

[0164] ;

[0165] S-A2: For any non-ground point In the local neighborhood Select the K nearest ground points Local ground height is calculated by weighted average. The expression is:

[0166] ;

[0167] in, This represents a local ground elevation estimate. Indicates the weighting coefficient. This represents the height value of the j-th neighboring ground point, where j represents the neighbor index number. Indicates index iteration;

[0168] Weighting coefficient The calculation expression is:

[0169] ;

[0170] in, The Gaussian kernel bandwidth parameter is used to ensure that neighboring points contribute more and distant points have less influence, thereby improving the stability of the estimation.

[0171] S-A3: If not a ground point Non-ground points in the local neighborhood If there are no valid points within the surface, the global plane equation is used to calculate the ground height, expressed as:

[0172] ;

[0173] in, b, c, and d are the global ground model coefficients obtained by fitting using the least squares method, where b controls the tilt direction, c determines the normal component, and d is the offset; this ensures the robustness of the algorithm in sparse or noisy scenes.

[0174] S-A4: Calculate non-ground points relative height difference The expression is:

[0175] ;

[0176] in, This represents the relative height difference, where z represents the actual height coordinate of the current point;

[0177] To accommodate ground fluctuations in different regions, a dynamic threshold based on statistical characteristics is set as follows:

[0178] ;

[0179] ;

[0180] in, This represents the average of the local relative heights. The standard deviation of the local relative height. This is the adjustment coefficient;

[0181] If not a ground point relative height difference The following conditions must be met:

[0182] ;

[0183] Non-ground points Retained as an obstacle point;

[0184] S-A5: Calculated set of obstacle points :

[0185] ;

[0186] And remove ground points by combining outlier points by radius:

[0187] ;

[0188] in, Indicated by point Centered on, The set of the radius's neighborhood; This represents the minimum number of neighbors threshold;

[0189] And Euclidean clustering constraints:

[0190] ;

[0191] in, To optimize the 3D spatial coordinate vector of the m-th point in the previous point cloud, To and The three-dimensional spatial coordinate vector of the nth adjacent point, Indicates the cluster radius threshold;

[0192] The final optimized set of obstacle points is obtained. This provides high-quality input for subsequent mapping.

[0193] Furthermore, in step S7, the environment modeling update unit updates the environment based on the set of obstacle points. and the final fusion point cloud Environmental modeling and real-time map updates involve the following steps:

[0194] S-B1: The environment modeling and updating unit first calculates the obstacle point cloud. Find the boundary, and obtain the minimum and maximum points. , And based on this, determine the size of the two-dimensional raster map:

[0195] ;

[0196] in, For raster resolution, The width of the map. The height of the map. The x-axis is... The vertical axis is used as the coordinate.

[0197] S-B2: The environment modeling update unit maps point cloud coordinates to a two-dimensional raster index, completing the environment modeling.

[0198] ;

[0199] in, Indicates column index, This indicates the row index and marks the corresponding raster as occupied.

[0200] S-B3: such as Figure 3As shown, to accelerate the projection process from point cloud to grid, the environment modeling update unit updates the obstacle point set according to Euclidean distance and point density characteristics. Data segmentation is performed to obtain s subsets of obstacle point clouds, where s ≥ 2. Then, a multi-threaded framework (such as OpenMP) is used to traverse all obstacle point cloud subsets in parallel. Each thread independently processes one subset of obstacle point clouds, and atomic operations or critical sections are used to protect shared raster data to complete the real-time update of the 2D raster map.

[0201] ;

[0202] in, This indicates that two occupies the position in the grid map at the th position. OK, The value of a cell in a column is 100, which indicates that the cell is occupied by an obstacle. Unvisited cells are set to -1.

[0203] Through the above design and parallel computing, the grid generation speed is significantly improved, enabling real-time updates and continuous publishing of maps in the ROS environment, thus meeting the needs of unmanned vehicles and people for rapid mapping and navigation in complex environments.

[0204] Furthermore, in step S8, the path planning unit performs global path search and local obstacle avoidance planning, specifically including the following steps:

[0205] S-C1: The path planning unit is based on the pose of the unmanned vehicle at the previous moment. and the linear acceleration measured by the inertial measurement unit. and angular acceleration are Perform prediction updates to obtain the initial pose. The expression is:

[0206] ;

[0207] ;

[0208] ;

[0209] in, This represents the initial pose of the unmanned vehicle at time t. The sampling period is The linear velocity at the previous moment. The angular velocity at the previous moment is obtained by integrating the IMU acceleration.

[0210] Then, using Kalman filtering or nonlinearization, the observation pose provided by LiDAR or visual SLAM is transformed. As an observation To perform an update, the update expression is:

[0211] ;

[0212] in, For the updated observation pose, For Kalman gain, For observation model parameters;

[0213] S-C2: The path planning unit uses the A* search algorithm to perform a global path search on the two-dimensional grid map to obtain the global path. ;

[0214] Global path cost function for:

[0215] ;

[0216] in, This represents the distance from the current location to the target point. The cost of grid occupancy state, , These are the weighting coefficients. This is the node row index at the k-th iteration. This is the node column index at the k-th iteration;

[0217] S-C3: During the movement of the unmanned vehicle, the sensor module detects dynamic obstacles in real time, and the path planning unit uses the dynamic obstacles to plan the path on the local map. Obstacle avoidance adjustments are made, and the local path planning cost function is:

[0218] ;

[0219] in, This represents the distance from the current location to the global path. For dynamic obstacle grid cost, , These are the weighting coefficients;

[0220] The path planning unit calculates the locally optimal path using the Time-Flexible Local Path Planning (TEB) algorithm. It also combines global path to achieve continuous optimization and real-time obstacle avoidance.

[0221] S-C4: The path planning unit will determine the actual pose of the unmanned vehicle. With planned path Compare and generate path deviations Used for controller feedback:

[0222] ;

[0223] in, For speed and steering control variables, and This is the gain for PID control.

[0224] Furthermore, in step S9, the control drive unit drives the drive execution unit based on the control algorithm of the throttle and brake calibration table, and the specific steps are as follows:

[0225] S-D1: When the unmanned vehicle is stationary, the control drive unit applies different throttle amounts in stages. The experimental dataset was obtained by recording the vehicle's speed and acceleration response curves over time.

[0226] ;

[0227] S-D2: The control drive unit constructs a three-dimensional mapping relationship between throttle position, speed, and acceleration based on the collected vehicle speed and acceleration data, and obtains the calibration surface equation through surface fitting methods (such as least squares fitting or polynomial regression):

[0228] ;

[0229] in, For throttle input, The vehicle's current speed, The vehicle's current acceleration, This indicates the relationship between throttle input, such as speed and acceleration.

[0230] S-D3: Analogous to the throttle modeling process, at the vehicle's maximum speed, the control drive unit applies different braking amounts step by step to form a brake gauge, which is used for vehicle deceleration and emergency braking control.

[0231] Figure 4 This image shows a two-dimensional grayscale image of the throttle calibration curve in this embodiment. Figure 5 A three-dimensional grayscale image of the throttle calibration curve in this embodiment is shown.

[0232] The above description is merely a preferred embodiment of the present invention and is not intended to limit the invention. Various modifications and variations can be made to the present invention by those skilled in the art. Any modifications, equivalent substitutions, improvements, etc., made within the spirit and principles of the present invention should be included within the scope of protection of the present invention.

Claims

1. A method for environmental perception and obstacle avoidance control of unmanned vehicles in complex dynamic scenarios, characterized in that, Includes the following steps: Step S1: Construct an unmanned vehicle environmental perception and obstacle avoidance control system. This unmanned vehicle environmental perception and obstacle avoidance control system is equipped with a sensor module, an on-board computer module, and a chassis control module. The on-board computer module is connected to the sensor module and the chassis control module respectively. The sensor module is equipped with a solid-state LiDAR, a multi-line mechanical LiDAR, and an inertial measurement unit installed on the unmanned vehicle. The chassis control module is equipped with a throttle and brake calibration table and a drive execution unit. The vehicle-mounted computer module is provided with an initialization unit, a point cloud fusion unit, a data optimization unit, a feature extraction unit, an environment modeling and updating unit, a path planning unit, and a control and drive unit connected in sequence. Step S2: The unmanned vehicle is stationary at the starting position and powered on for initialization. The initialization unit acquires the initial data of the sensor module and then obtains the external parameters between the solid-state lidar coordinate system and the multi-line mechanical lidar coordinate system through the iterative nearest point algorithm ICP calibration, thus completing the initial calibration of the sensor module. Step S3: The unmanned vehicle starts up, and the solid-state lidar and multi-line mechanical lidar acquire point cloud data around the unmanned vehicle in real time. And transmit it to the on-board computer module; The inertial measurement unit acquires the linear acceleration of the unmanned vehicle in real time. and angular acceleration Data is then transmitted to the onboard computer module. Step S4: The point cloud fusion unit in the vehicle-mounted computer module processes the point cloud data. Perform coordinate alignment and point cloud fusion operations to generate a preliminary fused point cloud. And pass it to the data optimization unit; Step S5: The data optimization unit optimizes the preliminary fused point cloud. EKF denoising, Laplacian optimization, and dynamic modeling are performed sequentially to obtain the final fused point cloud. And pass it to the feature extraction unit and the environment modeling update unit; Step S6: The feature extraction unit uses a method combining local ground height and global plane equations to extract the final fused point cloud. The non-ground points in the data are dynamically filtered and then optimized to obtain the obstacle point set. And pass it to the environment modeling update unit; Step S7: The environment modeling update unit updates the environment based on the set of obstacle points. and the final merged point cloud Perform environmental modeling and real-time map updates; Step S8: The path planning unit updates the real-time map constructed by the environment modeling unit based on the linear acceleration. and angular acceleration The data is used to perform global path search and local obstacle avoidance planning, generate a planned path, and transmit it to the control drive unit; Step S9: The control drive unit, based on the control algorithm of the throttle and brake calibration table, controls the drive execution unit to drive the unmanned vehicle to run according to the planned path.

2. The method for environmental perception and obstacle avoidance control of unmanned vehicles in complex dynamic scenarios according to claim 1, characterized in that: The initialization unit acquires the initial point cloud data collected by the solid-state lidar and the multi-line mechanical lidar, and then performs point cloud registration using the Iterative Closest Point (ICP) algorithm. The ICP algorithm iteratively searches for the nearest point pair between the point cloud to be registered and the reference point cloud, and then optimizes the extrinsic parameters using the least squares error function. The extrinsic parameters include rotation and translation matrices. The Gauss-Newton method is used to optimize the residual function during the iteration process, resulting in the following formula for the least squares iteration objective of ICP: ； in, For multi-line mechanical lidar point clusters, For solid-state lidar point aggregation; This is the pose transformation matrix, i.e., the rotation matrix or the translation matrix; This is the index of the point pair currently being calculated. This represents the number of point pairs participating in the matching.

3. The method for environmental perception and obstacle avoidance control of unmanned vehicles in complex dynamic scenarios according to claim 1, characterized in that: In step S4, the point cloud fusion unit first uses a rigid transformation matrix. The point cloud data Mapping to the robot's unified coordinate system and completing the coordinate alignment operation, the expression is as follows: ； in, For the first Radar point cloud coordinates, for The corresponding point cloud coordinates in the fused coordinate system The optimized rotation matrix, This is the optimized translation matrix; The point cloud fusion unit then uses a loosely coupled approach to perform weighted fusion on the aligned point cloud data to generate a preliminary fused point cloud. The expression is: ； in, For the first The confidence weights of each radar point cloud in the XYZ directions satisfy the following conditions: , This indicates the number of radar point clouds participating in the fusion, which is dynamically adjusted through point cloud density. The expression is: ； in, For the first The radar number The confidence weights of points in the XYZ directions. For the first The radar number The distance from a point to the center of its local neighborhood. This is the adjustment coefficient.

4. The method for environmental perception and obstacle avoidance control of unmanned vehicles in complex dynamic scenarios according to claim 1, characterized in that: In step S5, firstly, the data optimization unit uses an error-spreading Kalman filter (EKF) to process the initially fused point cloud. The state update model for denoising and outlier removal is as follows: ； in, This is the estimated point cloud coordinates after filtering at the current time. This represents the predicted state at the previous moment. These are the point cloud observations at the current moment. Here is the Kalman gain matrix. For the observation matrix, Indicates time; after EKF processing, the denoised point cloud output is obtained as follows: ; Secondly, the data optimization unit outputs the denoised point cloud. Perform Laplace constraint regularization, with the optimization objective being: ； in, The point cloud optimized by Laplacian constraint regularization, Denotes the Hilbert-Schmidt norm. The Laplacian matrix of the point cloud; For smoothing weighting coefficients; This represents the optimal estimate with the minimum cost. This represents the original point cloud matrix collected by each sensor. Represents a topological regularization term; Finally, the data optimization unit will optimize the obtained data. As input for system dynamic modeling, a dynamic evolution model of the point cloud with continuous temporal sequence is established, and the model expression is: ； in, , Here is the system state transition matrix. To control the input, ; For system noise, Indicates system process noise; To measure noise, Indicates observation noise; This represents the predicted state vector for the next time step. This represents the system state vector at the current moment. Represents the observation matrix. It represents the observation at the next moment.

5. The method for environmental perception and obstacle avoidance control of unmanned vehicles in complex dynamic scenarios according to claim 1, characterized in that: In step S6, the feature extraction unit uses local ground height estimation and global plane equation compensation to dynamically filter and optimize non-ground points, specifically including the following steps: S-A1: The feature extraction unit will fuse the final point cloud Divided into a set of ground points Set of non-ground points and assemble at the ground point. A KD-Tree structure is built on top of this to search for neighboring ground points; let the total number of the original point cloud be... Then we have: ； S-A2: For any non-ground point In the local neighborhood Select the K nearest ground points Local ground height is calculated by weighted average. The expression is: ； in, This represents a local ground elevation estimate. Indicates the weighting coefficient. This represents the height value of the j-th neighboring ground point, where j represents the neighbor index number. Indicates index iteration; Weighting coefficient The calculation expression is: ； in, This refers to the Gaussian kernel bandwidth parameter; S-A3: If not a ground point Non-ground points in the local neighborhood If there are no valid points within the surface, the global plane equation is used to calculate the ground height, expressed as: ； in, b, c, and d are the global ground model coefficients obtained by fitting using the least squares method, where b controls the tilt direction, c determines the normal component, and d is the offset. S-A4: Calculate non-ground points relative height difference The expression is: ； in, This represents the relative height difference, where z represents the actual height coordinate of the current point; The dynamic threshold based on statistical characteristics is set as follows: ； ； in, This represents the average of the local relative heights. The standard deviation of the local relative height. This is the adjustment coefficient; If not a ground point relative height difference The following conditions must be met: ； Non-ground points Retained as an obstacle point; S-A5: Calculated set of obstacle points : ； And remove ground points by combining outlier points by radius: ； in, Indicated by point Centered on, The set of the radius's neighborhood; This represents the minimum number of neighbors threshold; And Euclidean clustering constraints: ； in, To optimize the 3D spatial coordinate vector of the m-th point in the previous point cloud, To and The three-dimensional spatial coordinate vector of the nth adjacent point, Indicates the cluster radius threshold; The final optimized set of obstacle points is obtained. .

6. The method for environmental perception and obstacle avoidance control of unmanned vehicles in complex dynamic scenarios according to claim 1, characterized in that: In step S7, the environment modeling update unit updates the environment based on the set of obstacle points. and the final merged point cloud Environmental modeling and real-time map updates involve the following steps: S-B1: The environment modeling and updating unit first calculates the obstacle point cloud. Find the boundary, and obtain the minimum and maximum points. , And based on this, determine the size of the two-dimensional raster map: ； in, For raster resolution, The width of the map. The height of the map. The x-axis is... The vertical axis is used as the coordinate. S-B2: The environment modeling update unit maps point cloud coordinates to a two-dimensional raster index, completing the environment modeling. ； in, Indicates column index, This indicates the row index and marks the corresponding raster as occupied. S-B3: The environment modeling update unit updates the obstacle point set according to Euclidean distance and point density characteristics. Data segmentation is performed to obtain s subsets of obstacle point clouds, where s ≥ 2. Then, a multi-threaded framework is used to traverse all obstacle point cloud subsets in parallel, with each thread processing one subset independently. Atomic operations or critical sections are used to protect shared raster data to achieve real-time updates of the 2D raster map. ； in, This indicates that two occupies the position in the grid map at the th position. OK, The value of a cell in a column is 100, which indicates that the grid cell is occupied by an obstacle. The value of an unvisited cell is set to -1.

7. The method for environmental perception and obstacle avoidance control of unmanned vehicles in complex dynamic scenarios according to claim 6, characterized in that: In step S8, the path planning unit performs global path search and local obstacle avoidance planning, specifically including the following steps: S-C1: The path planning unit is based on the pose of the unmanned vehicle at the previous moment. and the linear acceleration measured by the inertial measurement unit. and angular acceleration are Perform prediction updates to obtain the initial pose. The expression is: ； ； ； in, This represents the initial pose of the unmanned vehicle at time t. The sampling period is The linear velocity at the previous moment. The angular velocity at the previous moment; Then, using Kalman filtering or nonlinearization, the observation pose provided by the lidar is... As an observation To perform an update, the update expression is: ； in, For the updated observation pose, For Kalman gain, These are the parameters of the observation model; S-C2: The path planning unit uses the A* search algorithm to perform a global path search on the two-dimensional grid map to obtain the global path. ; Global path cost function for: ； in, This represents the distance from the current location to the target point. The cost of grid occupancy state, , These are the weighting coefficients. This is the node row index at the k-th iteration. This is the node column index at the k-th iteration; S-C3: During the movement of the unmanned vehicle, the sensor module detects dynamic obstacles in real time, and the path planning unit uses the dynamic obstacles to plan the path on the local map. Obstacle avoidance adjustments are made, and the local path planning cost function is: ； in, This represents the distance from the current location to the global path. For dynamic obstacle grid cost, , These are the weighting coefficients; S-C4: The path planning unit will determine the actual pose of the unmanned vehicle. With planned path Compare and generate path deviations Used for controller feedback: ； in, For speed and steering control variables, and This is the gain for PID control.

8. The method for environmental perception and obstacle avoidance control of unmanned vehicles in complex dynamic scenarios according to claim 1, characterized in that: In step S9, the control drive unit drives the drive execution unit based on the control algorithm of the throttle and brake calibration table. The specific steps are as follows: S-D1: When the unmanned vehicle is stationary, the control drive unit applies different throttle amounts in stages. The experimental dataset was obtained by recording the vehicle's speed and acceleration response curves over time. ； S-D2: The control drive unit constructs a three-dimensional mapping relationship between throttle position, speed, and acceleration based on the collected vehicle speed and acceleration data, and obtains the calibration surface equation through a surface fitting method. ； in, For throttle input, The vehicle's current speed, The vehicle's current acceleration, This indicates the relationship between throttle input, such as speed and acceleration. S-D3: Analogous to the throttle modeling process, at the vehicle's maximum speed, the control drive unit applies different braking amounts step by step to form a brake gauge, which is used for vehicle deceleration and emergency braking control.

Images