An unmanned vehicle intelligent obstacle avoidance method and system based on a multi-modal sensor

Abstract

The application discloses an unmanned vehicle intelligent obstacle avoidance method and system based on a multi-modal sensor, and relates to the field of automatic driving and intelligent traffic.The technical points of the application are as follows: laser radar and depth camera data are aligned by a space-time registration module; laser radar point clouds are preprocessed and obstacle point clouds are segmented, fusion clustering is implemented in combination with three-dimensional semantic seed points output by the depth camera, and obstacle clusters with semantic labels are generated; three-dimensional oriented bounding boxes (OBB) of obstacles are calculated, a safety distance is dynamically adjusted according to the speed of the vehicle, and the OBB of the vehicle is expanded, and collision detection is implemented by using the separate axis theorem.When a collision risk is detected, a first-level warning and a second-level emergency braking instruction are triggered in sequence, and safe and efficient dynamic obstacle avoidance of the unmanned vehicle is implemented.The method fully utilizes the advantages of multiple sensors, effectively improves the obstacle detection accuracy and reaction speed, and provides reliable technical support for the application of the unmanned driving technology in a complex road environment.

Description

Technical Field

[0001] This invention relates to the fields of autonomous driving and intelligent transportation, specifically to an intelligent obstacle avoidance method for unmanned vehicles based on multimodal sensor data fusion, which is particularly suitable for achieving high-precision obstacle perception and graded safety braking through multi-source data fusion in complex dynamic environments. Background Technology

[0002] Existing obstacle avoidance systems for autonomous vehicles mostly rely on a single sensor (such as LiDAR or a camera) for obstacle detection. However, a single sensor often has limitations when dealing with different environmental conditions (changing lighting, occlusion, multipath interference, etc.). To address this, multimodal sensor data fusion technology has been proposed to integrate the distance accuracy of LiDAR with the semantic information of depth cameras, in order to achieve more accurate and robust obstacle perception and collision warning.

[0003] The prior art, document number CN107065890B, discloses an obstacle avoidance system for unmanned vehicles based on reinforcement learning. It includes a perception part, a decision part, a control part, and an execution part. The perception part detects obstacle areas using a single-line lidar to acquire obstacle information. The decision part determines whether the unmanned vehicle needs to avoid obstacles in the next moment based on the obstacle detection. If so, the control part inputs the state information from the perception part into the reinforcement learning model. After trial and error learning, the model converges to a stable state and calculates the steering wheel angle required by the unmanned vehicle. This angle is then executed by the execution part of the unmanned vehicle to achieve the obstacle avoidance function.

[0004] The prior art, document CN112859871B, discloses a dynamic obstacle avoidance control method based on model prediction. First, it acquires information such as the speed and pose of dynamic obstacles using radar and cameras. This information is then input into a model prediction layer, using the given vehicle speed and heading angle as model input parameters. Based on model prediction theory, the method identifies and judges whether an obstacle will enter a potential collision domain. If it is determined that the obstacle will enter a potential collision domain, it is input into an improved artificial potential field method. This improvement solves the problem of getting trapped in local optima inherent in the classic artificial potential field method. This prior art utilizes model prediction theory to model the autonomous vehicle and obstacles separately, and delineates potential collision domains. It performs two-layer identification and judgment for the entire dynamic obstacle avoidance process, significantly reducing the computational load. Furthermore, its improvement and optimization of the artificial potential field method greatly enhances the safety and reliability of obstacle avoidance planning for autonomous vehicles.

[0005] However, how to leverage the complementary advantages of multimodal sensor data to achieve accurate obstacle detection, semantic annotation, and dynamic collision detection has become a significant technical challenge in improving the safety of autonomous vehicles, and current technologies do not offer a solution. Summary of the Invention

[0006] The purpose of this invention is to provide an intelligent obstacle avoidance method for unmanned vehicles based on multimodal sensors. By spatiotemporal registration of LiDAR and depth camera data, it can achieve accurate obstacle detection and semantic annotation as well as dynamic collision detection. Combined with a graded braking strategy, it can achieve real-time and accurate detection and avoidance of obstacles in complex environments, thereby improving the safe driving performance of unmanned vehicles.

[0007] To achieve the above objectives, the present invention adopts the following technical solution:

[0008] An intelligent obstacle avoidance method for autonomous vehicles based on multimodal sensors includes the following steps:

[0009] Step 1: Multimodal sensor data fusion: The LiDAR point cloud data and depth camera RGB-D data are aligned in time and space through the spatiotemporal registration module, and the two types of sensor data are uniformly converted to the vehicle coordinate system;

[0010] Step 2, Obstacle Detection and Semantic Labeling: The point cloud data collected by the LiDAR is preprocessed and segmented to extract the obstacle point cloud. The depth camera data is processed by the target detection module to output the target bounding box and semantic label. Based on the depth information and camera intrinsic parameters, the 3D semantic seed points of the target are generated.

[0011] Step 3: Fusion clustering based on semantic and geometric features: Based on the three-dimensional semantic seed points, perform region growing clustering on the obstacle point cloud data to generate semantic obstacle clusters, perform Euclidean distance clustering on the remaining unclustered point cloud to generate geometric obstacle clusters, and calculate the three-dimensional directed bounding box (OBB) of each cluster.

[0012] Step 4, Dynamic Collision Detection and Graded Braking: Adjust the dynamic safety distance according to the real-time speed of the vehicle to expand the vehicle's OBB size. Transform the expanded vehicle OBB and obstacle OBB to the world coordinate system, use the separation axis theorem to detect the collision risk of the two, and execute graded braking strategy according to the risk level.

[0013] Furthermore, the spatiotemporal registration module includes:

[0014] ① The time synchronization unit adopts an approximate time synchronization strategy based on message queues to ensure that the maximum time difference does not exceed 0.1 seconds;

[0015] ② Spatial alignment unit: using the pre-calibrated extrinsic matrix between the lidar and the depth camera, the depth camera data is converted to the lidar coordinate system and then uniformly converted to the vehicle coordinate system;

[0016] Furthermore, the lidar point cloud preprocessing includes:

[0017] ① A statistical filter was used to remove outliers from the original point cloud, where the number of neighboring points used to calculate the mean distance was set to 40 to 60, and the standard deviation coefficient threshold was set to 0.8 to 1.2.

[0018] ② The point cloud after outlier removal is downsampled using a voxel grid filter, with the voxel size set to 0.08 to 0.12 meters;

[0019] ③ROI region acquisition: Define the first cube region based on the given coordinates (minPoint, maxPoint) as the region of interest, and retain the point cloud data within this region;

[0020] ④ Vehicle point cloud removal: Define a second cube region based on the given coordinates (car_minPoint, car_maxPoint) to represent the spatial range of the vehicle body, and remove point clouds that fall into this region;

[0021] The lidar point cloud segmentation includes:

[0022] The Random Sample Consensus (RANSAC) algorithm is used to segment the point cloud after the above processing, dividing it into ground point cloud and obstacle point cloud. The obstacle point cloud is retained for subsequent processing, and the algorithm iterates no less than 80 times, with a planar distance threshold of no more than 0.15 meters.

[0023] Furthermore, the depth camera data processing includes:

[0024] ① The target detection module processes the images acquired by the depth camera and outputs the target bounding box and corresponding semantic label, wherein the confidence threshold is limited to the range of 0.4 to 0.6;

[0025] ②Based on the pixel coordinates of the target bounding box center and its corresponding depth value in the depth map, and combined with the pre-determined camera intrinsic parameters, the target center point is accurately transformed from the two-dimensional image coordinates to the vehicle coordinate system to obtain the three-dimensional semantic seed point.

[0026] Furthermore, the fusion clustering includes:

[0027] ① Perform region growing clustering on semantic seed points, with the neighborhood search radius preferably being 0.2-0.4 meters and the minimum number of cluster points preferably being 5-15;

[0028] ② Perform Euclidean distance clustering on the remaining point cloud, with a neighborhood radius preferably of 0.4-0.6 meters and a minimum number of cluster points preferably of 3-8;

[0029] The calculation steps for the three-dimensional directed bounding box (OBB) of the point cloud cluster include:

[0030] ① Vertical dimension extreme value extraction: Traverse the point cloud cluster to obtain the minimum vertical coordinate Z_min and the maximum vertical coordinate Z_max;

[0031] ② Planar projection processing: Set the vertical coordinates of all points in the point cloud cluster to the reference value 0, and retain only the horizontal coordinates (x,y) to generate a two-dimensional projected point cloud;

[0032] ③ Moment of inertia OBB calculation: Based on the two-dimensional projection point cloud computing bounding box center position, two-dimensional dimensions (length, width) and rotation matrix;

[0033] ④ 3D reconstruction: Assign the extracted vertical extrema to the bounding box, where the vertical center coordinate of the bounding box is Z_center=(Z_min+Z_max) / 2, and the vertical dimension is H=Z_max-Z_min;

[0034] ⑤ Output: The output includes the center position vector, rotation quaternion, and bounding box description parameters of the three-dimensional dimensions.

[0035] The bounding box description parameters include:

[0036] The center position coordinates are (x, y, z_center); the three-dimensional dimensions are (L, W, H); and the rotation quaternion (q_x, q_y, q_z, q_w) represents the azimuth angle of the bounding box; where q_w is the real part (scalar part) of the quaternion; and q_x, q_y, and q_z are the imaginary parts (vector parts) of the quaternion.

[0037] Furthermore, the calculation of the moment of inertia OBB includes:

[0038] ① Moment of inertia estimation: Compute the centroid coordinates and covariance matrix of the input point cloud, and perform eigenvalue decomposition on the covariance matrix to extract eigenvectors as the main direction of the point cloud data, thereby determining the main distribution trend of the point cloud;

[0039] ② Construction of local coordinate system: Establish a local coordinate system along the principal direction with the calculated centroid as the origin;

[0040] ③ Rotation matrix acquisition: Calculate the rotation matrix from the original point cloud coordinate system to the local coordinate system to achieve coordinate transformation of the point cloud data;

[0041] ④Extremal projection: The point cloud data is transformed to a local coordinate system using the rotation matrix, and the minimum and maximum projection values ​​on each coordinate axis are recorded to determine the extreme boundaries of the bounding box in the local coordinate system.

[0042] Furthermore, the collision detection step includes:

[0043] ① Dynamic safety distance extension: Based on the vehicle's real-time speed v, according to formula d safe=k·|v|+b to calculate the safety distance, where k∈[0.15,0.25], b∈[0.4,0.6], and the safety distance is uniformly extended to the boundary of the vehicle's OBB bounding box size;

[0044] ② Candidate separation axis generation: The normal directions of each side of the expanded vehicle OBB and the obstacle OBB are used as candidate separation axes, and a total of 4 separation axes are selected;

[0045] ③ The collision determination module takes the lower left corner vertex of the expanded vehicle OBB as the origin, connects all vertices of the two bounding boxes to the origin to form a vector, and calculates the projection of the vector onto the selected separation axis. The minimum and maximum values ​​of the projection line segments of each OBB are recorded through the projection points. When there is an overlap of projection line segments on any candidate separation axis, it indicates that the expanded vehicle OBB intersects with the obstacle OBB, which is determined as a collision risk and triggers a graded braking command. Otherwise, the determination continues until there is no overlap of projection line segments on all candidate separation axes, indicating that the expanded vehicle OBB does not intersect with the obstacle OBB, which is determined as no collision risk and exits the detection.

[0046] Furthermore, the graded braking strategy includes:

[0047] ① Level 1 warning: When a collision risk is detected, the vehicle speed is limited to 1.0±0.2 m / s, and the dynamic safety distance is switched to a fixed compensation value d_fix=0.15±0.05 m;

[0048] ② Level 2 Emergency Braking: After switching to a fixed compensation safety distance, collision detection is repeated. If the overlap of the body boxes is detected again, the emergency stop procedure is initiated, reducing the vehicle speed to zero and activating the collision warning signal within 0.3±0.1 seconds.

[0049] An intelligent obstacle avoidance system for unmanned vehicles based on multimodal sensors is provided. This system has program modules corresponding to the steps of the above-mentioned technical solution, and executes the steps in the intelligent obstacle avoidance method for unmanned vehicles based on multimodal sensors when running.

[0050] A computer-readable storage medium storing a computer program configured to implement the steps of a multimodal sensor-based intelligent obstacle avoidance method for unmanned vehicles when invoked by a processor.

[0051] The present invention has the following beneficial technical effects:

[0052] This invention discloses an intelligent obstacle avoidance method for unmanned vehicles based on multimodal sensors, comprising: aligning LiDAR and depth camera data through a spatiotemporal registration module; preprocessing the LiDAR point cloud and segmenting it into obstacle point clouds, performing fusion clustering with 3D semantic seed points output by the depth camera to generate obstacle clusters with semantic labels; calculating the 3D directed bounding box (OBB) of obstacles, dynamically adjusting the safety distance and expanding the vehicle's OBB according to the vehicle's speed, and using the separating axis theorem for collision detection. When a collision risk is detected, the system sequentially triggers a first-level warning and a second-level emergency braking command to achieve safe and efficient dynamic obstacle avoidance for unmanned vehicles. This invention fully utilizes the advantages of multiple sensors, effectively improving obstacle detection accuracy and reaction speed, and providing reliable technical support for the application of unmanned driving technology in complex road environments.

[0053] This invention, through the collaborative work of its modules, achieves high-precision obstacle detection and accurate collision prediction in complex dynamic environments, and can promptly trigger graded braking, significantly improving the driving safety and obstacle avoidance response speed of autonomous vehicles. Actual operation verification has demonstrated the superior obstacle detection accuracy of this invention, providing reliable technical support for autonomous driving applications in complex road environments. Attached Figure Description

[0054] Figure 1 This is a flowchart illustrating the overall process of this invention.

[0055] Figure 2 Flowchart for preprocessing LiDAR point clouds;

[0056] Figure 3 Image target detection results;

[0057] Figure 4 A schematic diagram of AABB and OBB;

[0058] Figure 5a The results of point cloud fusion clustering and OBB calculation are shown from a top-down perspective;

[0059] Figure 5b The results of point cloud fusion clustering and OBB calculation from a side-view perspective;

[0060] Figure 6 A schematic diagram illustrating the expansion of the OBB dimensions of a vehicle;

[0061] Figure 7 This is a schematic diagram of the principle of the separation axis theorem.

[0062] Figure 8 A schematic diagram of selecting rectangular candidate separation axes;

[0063] Figure 9a This is a top-down view of the actual operation of the invention;

[0064] Figure 9b This is a side view of the actual operation effect of the invention. Detailed Implementation

[0065] The specific embodiments of the present invention will be described in detail below with reference to the accompanying drawings:

[0066] This invention proposes an intelligent obstacle avoidance method for unmanned vehicles based on multimodal sensors, the overall flowchart of which is shown below. Figure 1 As shown, this method includes aligning LiDAR and depth camera data through a spatiotemporal registration module; preprocessing the LiDAR point cloud and segmenting it into obstacle point clouds, then performing fusion clustering with the 3D semantic seed points output by the depth camera to generate obstacle clusters with semantic labels; calculating the 3D directed bounding box (OBB) of obstacles, dynamically adjusting the safety distance and expanding the vehicle's OBB according to the vehicle's speed, and using the separating axis theorem for collision detection. When a collision risk is detected, the system sequentially triggers a first-level warning and a second-level emergency braking command to achieve safe and efficient dynamic obstacle avoidance for the unmanned vehicle, specifically including:

[0067] 1. Multimodal sensor data fusion

[0068] A spatiotemporal registration module is used to achieve temporal and spatial alignment between LiDAR point cloud data and depth camera RGB-D data. The spatiotemporal registration module includes:

[0069] ① Time synchronization unit: An approximate time synchronization strategy based on message queues is adopted to ensure that the maximum time difference between the two types of data does not exceed 0.1 seconds;

[0070] ② Spatial Alignment Unit: Using the extrinsic parameter matrix between the pre-calibrated LiDAR and depth camera, the depth camera data is converted to the LiDAR coordinate system, and then uniformly converted to the vehicle coordinate system.

[0071] 2. Obstacle detection and semantic annotation

[0072] The lidar point cloud is preprocessed (including outlier removal, voxel mesh downsampling, ROI region acquisition, and vehicle point cloud removal), and then the obstacle point cloud is segmented using the RANSAC algorithm. At the same time, the target detection module processes the images acquired by the depth camera, outputs the target bounding box and semantic label, and generates the target 3D semantic seed points by combining the camera intrinsic parameters and depth information.

[0073] 3. Clustering based on semantic and geometric features

[0074] The generated 3D semantic seed points are used to perform region growing and clustering on the obstacle point cloud to form semantic obstacle clusters; the remaining unclustered point cloud is clustered using Euclidean distance to generate geometric obstacle clusters, and the 3D directed bounding box (OBB) of each cluster is calculated. The calculation of the OBB includes horizontal plane projection, moment of inertia calculation and 3D reconstruction.

[0075] 4. Dynamic collision detection and graded braking

[0076] Based on the real-time speed of the vehicle, the formula d is used. safe =k·|v|+b (where k∈[0.15,0.25], b∈[0.4,0.6]) calculates the dynamic safety distance, and this safety distance is uniformly extended to the size of the vehicle's OBB. After transforming the extended vehicle OBB and obstacle OBB to the world coordinate system, collision detection is performed through candidate separation axes (composed of the normal directions of each side of the OBB, a total of 4). When an overlap of the OBB projection line segments is detected on any separation axis, a collision risk is determined, and a graded braking command is triggered.

[0077] ① Level 1 warning braking: Speed ​​is limited to approximately 1.0 ± 0.2 m / s, and the dynamic safety distance is switched to a fixed compensation value d_fix = 0.15 ± 0.05 m;

[0078] ② Level 2 Emergency Braking: If OBB overlap is detected again after the warning braking, the vehicle speed will be rapidly reduced to zero within 0.3±0.1 seconds and the collision warning signal will be activated.

[0079] 5. Additional Technical Details

[0080] ① The preprocessing of the lidar point cloud adopts statistical filtering (the number of neighboring points is set to 40 to 60, and the standard deviation coefficient threshold is 0.8 to 1.2), voxel mesh filtering (voxel size 0.08 to 0.12 meters), ROI region acquisition and vehicle point cloud removal; and the RANSAC algorithm (the number of iterations is not less than 80, and the plane distance threshold is not greater than 0.15 meters) is used to segment the point cloud into ground point cloud and obstacle point cloud.

[0081] ② The depth camera data processing module uses a target detection algorithm (confidence threshold in the range of 0.4 to 0.6). By combining the pixel coordinates of the target bounding box center and the depth map data with the pre-determined camera intrinsic parameters, it achieves accurate conversion from two-dimensional image coordinates to three-dimensional semantic seed points in the vehicle coordinate system.

[0082] ③ Semantic clustering adopts region growing clustering (the neighborhood search radius is preferably 0.2-0.4 meters, and the minimum number of cluster points is 5-15), and geometric clustering adopts Euclidean distance clustering (the neighborhood radius is preferably 0.4-0.6 meters, and the minimum number of cluster points is 3-8);

[0083] ④ The calculation of the 3D directed bounding box is achieved by extracting the vertical extrema of the point cloud cluster, constructing a 2D projected point cloud, calculating the moment of inertia to obtain the center position, 2D size and rotation matrix of the OBB, and then performing 3D reconstruction to obtain a complete OBB description (including center coordinates (x,y,z_center), 3D size (L,W,H) and rotation quaternion (q_x,q_y,q_z,q_w)).

[0084] ⑤ The OBB moment of inertia calculation process includes centroid and covariance matrix calculation, eigenvalue decomposition, local coordinate system construction, and rotation matrix acquisition, to ensure that the OBB can accurately reflect the point cloud distribution characteristics. Specific implementation examples:

[0086] I. Multimodal sensor data fusion

[0087] In this embodiment, the system collects environmental data using a LiDAR and a depth camera mounted on the autonomous vehicle. A spatiotemporal registration module aligns the original LiDAR point cloud data and the depth camera RGB-D data in both time and space, transforming the two types of sensor data into the vehicle coordinate system to obtain aligned LiDAR point cloud data and depth camera RGB-D data.

[0088] The spatiotemporal registration module includes a time synchronization unit and a spatial synchronization unit. The time synchronization unit uses a message queue to ensure that the time difference between the two data acquisitions is less than 0.1 seconds. The spatial alignment unit converts the RGB-D data acquired by the depth camera to the lidar coordinate system based on the pre-calibrated extrinsic parameter matrix, and further unifies it to the vehicle coordinate system.

[0089] 1.1 Time Synchronization Unit

[0090] The time synchronization unit employs a double-buffered queue mechanism to achieve time alignment of multimodal sensor data. Its core design includes the following three technical elements:

[0091] ① Asynchronous data acquisition architecture

[0092] • Dual-queue storage: Independent data buffers (buf1 / buf2) are created for the LiDAR and depth camera respectively, and the std::deque double-ended queue structure is used to achieve fast head and tail operations;

[0093] • Non-blocking write: Zero-copy enqueue operation of sensor data is implemented through the callback functions of each sensor, and the write process does not interrupt the main thread.

[0094] • Timestamp binding: Each data packet carries a standard ROS header structure, recording the collection timestamp (header.stamp) accurate to the nanosecond level.

[0095] ② Time window comparison strategy

[0096] • First frame comparison: In the data processing thread, the timestamps of the first frame data of the two queues are continuously extracted, and the absolute time difference Δt = |t1-t2| is calculated;

[0097] • Eliminate lagging data: When Δt > 0.1 seconds, discard sensor data with earlier timestamps (remove the first element of the queue using pop()) until a data pair that satisfies Δt ≤ 0.1 seconds is found;

[0098] • Dynamic compensation mechanism: Set a maximum queue depth threshold (default 100 frames). When any queue backlog exceeds the threshold, a data discarding policy is triggered to avoid memory overflow.

[0099] ③ Technological advantages

[0100] • Low latency: The time complexity of queue operations is O(1), and the time consumption of a single synchronization is <2ms (tested on Jetson Xavier platform);

[0101] • High robustness: Tolerates momentary data loss from a single sensor (automatically resumed with subsequent data);

[0102] • Scalability: Supports expansion to more sensor synchronization (such as millimeter-wave radar, ultrasound, etc.) by increasing the number of queues.

[0103] The pseudocode description of the workflow of the time synchronization unit is as follows:

[0104]

[0105] 1.2 Spatial Synchronization Unit

[0106] This method achieves spatial synchronization using the ROS TensorFlow library, relying on a pre-calibrated coordinate relationship between the radar and depth camera. During the calibration phase, the transformations (including translation and rotation) between the two sensor coordinate systems are determined. In the program, `tf::TransformBroadcaster` is used to broadcast this calibrated transformation information. High-frequency, looping broadcasts ensure real-time updates of the coordinate transformation information. Other nodes can obtain these transformations through `tf::TransformListener`, achieving synchronization and fusion of data from different sensors. Its technical implementation includes the following core elements:

[0107] ①External parameter calibration pre-storage

[0108] • Calibration data acquisition: The lidar and depth camera are calibrated together using a checkerboard calibration board, and the rigid transformation matrix (extrinsic parameter matrix) between the two sensors is solved using a feature point matching algorithm (such as SVD decomposition method).

[0109] • Parameter persistence: The translation vector (x,y,z) and rotation quaternion (qx,qy,qz,qw) obtained from calibration are stored in the YAML configuration file as the initial parameters for coordinate transformation.

[0110] ② Dynamic coordinate broadcasting mechanism

[0111] • Coordinate tree construction: Based on the ROS tf library, a multi-level coordinate system relationship is established, defining the vehicle coordinate system (body) as the root node, and the LiDAR (lidar_link) and depth camera (camera_color_optical_frame) as child nodes;

[0112] • Transformation broadcast: The coordinate transformation from the vehicle coordinate system to the lidar coordinate system and from the vehicle coordinate system to the depth camera coordinate system is continuously broadcast at high frequency via tf::TransformBroadcaster.

[0113] II. Obstacle Detection and Semantic Labeling

[0114] The point cloud data acquired by LiDAR is preprocessed and segmented to extract obstacle point clouds. A target detection module is used on the depth camera data to output target bounding boxes and semantic labels, and 3D semantic seed points for the targets are generated based on depth information and camera intrinsic parameters.

[0115] 2.1 Point Cloud Data Preprocessing

[0116] The flowchart of the lidar point cloud preprocessing is as follows: Figure 2 As shown, the process includes using a statistical filtering algorithm to remove outliers from the aligned LiDAR point cloud data, using a voxel grid filter to downsample the point cloud data after removing outliers to reduce its data volume, retaining LiDAR point cloud data within the preset ROI region, and removing the vehicle body point cloud data scanned by LiDAR by setting the coordinate range of the vehicle's location.

[0117] 2.2 Point Cloud Data Segmentation

[0118] The lidar point cloud segmentation includes using the Random Sample Consensus (RANSAC) algorithm to segment the preprocessed point cloud data into ground point clouds and obstacle point clouds, wherein the obstacle point clouds are retained as the basis data for subsequent processing.

[0119] Specifically, the process involves first randomly selecting a minimum number of sample points (usually 3) from the preprocessed point cloud and using these 3 points to construct a candidate plane model. Then, the distance from all points to the plane is calculated, and points with a distance within a preset threshold are considered interior points. After multiple iterations, the plane with the most interior points is selected as the ground model, thus classifying the points in the plane as ground points and the remaining points as obstacle points, achieving effective segmentation of ground point clouds and obstacle point clouds in complex scenes.

[0120] 2.3 Depth Camera Data Processing and Semantic Seed Point Generation

[0121] Image data acquired by the depth camera is processed by the target detection module, which outputs target bounding boxes and corresponding semantic labels. Based on the center pixel of the target bounding box and its corresponding depth value, combined with camera intrinsic parameters, a mathematical transformation is performed to obtain the 3D semantic seed point of the target in the vehicle coordinate system, providing prior information for subsequent semantic clustering. The specific steps are as follows:

[0122] ①Target detection and parameter extraction

[0123] Image data acquired by a depth camera is processed by an object detection module (such as YOLOv5) to output detection results containing the following elements:

[0124] • Target bounding box: Calculate the pixel coordinates (u,v) of the center point of the target bounding box.

[0125] • Semantic label: category identifier class_id (e.g., pedestrian = 1, vehicle = 2).

[0126] • Detection confidence: confidence∈[0,1].

[0127] ② 3D coordinate calculation of the camera coordinate system

[0128] The mapping from 2D pixel coordinates in the pixel coordinate system to 3D coordinates in the camera coordinate system is achieved through a pre-calibrated camera intrinsic parameter matrix.

[0129] • Intrinsic parameter matrix:

[0130] In camera calibration, the intrinsic parameter matrix is ​​used to describe the camera's internal characteristics, and its form is:

[0131]

[0132] Among them, f x and f y These represent the camera's focal length in the horizontal and vertical directions, respectively, in pixels. They are determined by the camera's physical focal length and the physical size of each pixel. x and c yThese represent the horizontal and vertical coordinates of the image center (principal point), respectively, in pixels. Typically, the principal point is located at the geometric center of the image, but in some cases it may be offset.

[0133] • Inverse projection calculation:

[0134] Given the depth value Zc (in meters) at (u,v) in the depth map, the coordinates Pc(Xc,Yc,Zc) of the target point in the camera coordinate system are calculated using the following formula:

[0135]

[0136] ③ Multi-sensor coordinate system

[0137] The transformation from the camera coordinate system to the vehicle coordinate system is achieved by using pre-calibrated extrinsic parameter matrices of the LiDAR and camera.

[0138] Rigid transformation

[0139] The calibration yielded the following extrinsic parameter matrix:

[0140]

[0141] Where R∈SO(3) is the rotation matrix. Translation vector

[0142] • Perform coordinate transformation

[0143] The three-dimensional coordinates P in the camera coordinate system c Transform to three-dimensional coordinates P in the vehicle coordinate system body The conversion formula is as follows:

[0144] P body =RP c +t

[0145] ④ Semantic-geometric association

[0146] Generate 3D seed points with semantic labels:

[0147] Seed sem = <P body ,class_id,confidence

[0148] ·P body : Three-dimensional coordinates in the vehicle coordinate system

[0149] • class_id: Semantic category inherited from the object detection result

[0150] • Confidence: Detects confidence level

[0151] Object detection results from depth camera images, such as Figure 3 As shown in the figure. To highlight the experimental results, the target detection module only detects the 'chair' category, masking other categories. The bounding boxes in the figure represent the detected targets, the 'chair' label in the upper left corner indicates the target category, and the coordinates (-0.19, -0.22, 1.43) in the lower left corner are the calculated 3D coordinates of the target in the camera coordinate system.

[0152] III. Fusion Clustering Based on Semantic and Geometric Features

[0153] Based on the three-dimensional semantic seed points, region growing clustering is performed on the obstacle point cloud data to generate semantic obstacle clusters. Euclidean distance clustering is performed on the remaining unclustered point clouds to generate geometric obstacle clusters, and the three-dimensional directed bounding box (OBB) of each cluster is calculated.

[0154] 3.1 Fusion Clustering

[0155] Semantic seed points are used to perform region growing clustering on obstacle point clouds to form obstacle clusters with clear semantic labels; for points that cannot be divided by semantic clustering, Euclidean distance clustering is used to generate geometric obstacle clusters.

[0156] ①Regional growth clustering

[0157] Point cloud multi-subregion growth clustering is a segmentation method based on spatial proximity. Its basic principle is to start with an initial seed point and recursively group neighboring points into the same cluster. This method first selects multiple seed points as the core regions of the initial clusters, and then gradually expands the cluster range according to a growth criterion. The growth criterion is typically whether the distance between neighboring points is less than a preset threshold. By continuously expanding the seed regions, multiple point cloud clusters that are separate yet tightly connected are eventually formed, achieving spatial segmentation of obstacles. Since each seed point corresponds to the semantic information of a target, the single obstacle obtained through this clustering method also carries semantic information, including the obstacle's category.

[0158] ② Euclidean distance clustering

[0159] Euclidean distance clustering of point clouds is a spatial distance-based segmentation method. Its principle is to use the three-dimensional coordinates of points in the point cloud and calculate the Euclidean distance between any two points to determine their spatial proximity. This algorithm typically starts with an unclassified point and sequentially searches for points whose distance is less than a preset threshold, grouping these points into the same cluster and marking them as visited points. This process is then repeated for newly added neighboring points until the current cluster can no longer be expanded. The next unclassified point is then selected, and the operation is repeated until all points are clustered. Euclidean distance clustering can quickly divide a point cloud into multiple density-connected point cloud clusters, each cluster corresponding to an independent obstacle.

[0160] 3.2 OBB Calculation Process

[0161] A 3D directed bounding box (OBB) is calculated for each point cloud cluster. The calculation process first extracts the minimum and maximum values ​​of the point cloud cluster in the vertical direction, then projects the point cloud cluster onto a horizontal plane, obtains the 2D parameters of the OBB using the moment of inertia method, and finally obtains the complete descriptive parameters of the OBB through 3D reconstruction. The specific steps are as follows:

[0162] ①AABB and OBB

[0163] AABB (Axis-Aligned Bounding Box) and OBB (Oriented Bounding Box) are two commonly used 3D bounding box types, widely applied in computer graphics, robotics, and collision detection, to represent the smallest bounding box of an object in space. Schematic diagrams of AABB and OBB are shown below. Figure 4 As shown, an AABB (Axis-Aligned Bounding Box) is a rectangular bounding box aligned with the coordinate axes. In 3D space, the six faces of an AABB are parallel to the X, Y, and Z coordinate axes, meaning each face of the bounding box is parallel to the coordinate axes. An OBB (Operated Bounding Box) is a bounding box that is not necessarily aligned with the coordinate axes. It customizes the orientation of its bounding box according to the shape and pose of the object, allowing its bounding box to more tightly enclose the object without being affected by gaps caused by rotation. In summary, AABB is a computationally efficient bounding box, but it leads to wasted space. OBB uses rotational orientation to minimize the size of the bounding box, thereby improving space utilization, but it has a higher computational cost. Considering that objects may have complex shapes and irregular rotations, this system uses OBB (Operated Bounding Box) as the collision detection scheme. OBB can dynamically adjust the orientation and size of the bounding box according to the actual pose and geometry of the object. Compared with AABB (Axis-Aligned Bounding Box), it can enclose the object more tightly, reducing unnecessary blank areas, thereby improving the accuracy and efficiency of collision detection. For scenarios requiring high-precision collision detection, OBB provides higher space utilization, especially suitable for objects with rotation or complex shapes.

[0164] ② OBB Calculation Steps

[0165] Vertical dimension extreme value extraction

[0166] First, iterate through all points in the point cloud cluster and extract the minimum and maximum vertical coordinates, denoted as Zmin and Zmax, respectively. These extreme values ​​will help in subsequent steps to calculate the vertical dimensions of the bounding box.

[0167] • Plane projection processing

[0168] To simplify bounding box calculations, the vertical coordinates of all points in the point cloud cluster are uniformly set to a baseline value of 0, retaining only the horizontal plane coordinates (x, y) to form a two-dimensional projected point cloud. This approach reduces computational complexity, allowing focus on bounding box calculations on the horizontal plane.

[0169] ·Moment of inertia OBB solution

[0170] Based on a 2D projected point cloud, the center position, 2D dimensions (length and width), and rotation matrix of its bounding box are calculated. These parameters will help determine the geometry and orientation of the bounding box.

[0171] ·3D reconstruction

[0172] Based on the 2D bounding box calculation, the extracted vertical extrema Z_min and Z_max are assigned to the vertical direction of the bounding box, so that the vertical center coordinate of the bounding box is Z_center = (Z_min + Z_max) / 2, and the vertical dimension is H = Z_max - Z_min. In this way, the final 3D bounding box has a vertical dimension.

[0173] • Output Results

[0174] The final calculated bounding box description parameters are output, including the center position vector (x,y,Z_center), 3D dimensions (L,W,H), and rotation quaternions (q_x,q_y,q_z,q_w). These parameters can comprehensively describe the spatial position and orientation of the point cloud cluster.

[0175] ③ Steps for calculating the moment of inertia (OBB)

[0176] Moment of inertia estimation

[0177] First, the input point cloud data is processed to calculate the centroid coordinates and covariance matrix of the point cloud. Eigenvalue decomposition of the covariance matrix is ​​then performed to extract eigenvectors, which serve as the principal orientation of the point cloud data. This step helps determine the main distribution trend of the point cloud data, thereby determining the orientation of the bounding box.

[0178] Local coordinate system construction

[0179] Based on the calculated centroid coordinates, a local coordinate system is established with the centroid as the origin and along the principal directions. This local coordinate system will be helpful for subsequent rotation matrix calculations and coordinate transformations.

[0180] • Obtaining the rotation matrix

[0181] The coordinate transformation of point cloud data is achieved by calculating a rotation matrix from the original point cloud coordinate system to the local coordinate system. This rotation matrix is ​​used to transform the point cloud data to the new coordinate system, thereby allowing for better calculation of the shape and orientation of the bounding box.

[0182] Extreme value projection

[0183] After transforming the point cloud data to a local coordinate system using a rotation matrix, the minimum and maximum projection values ​​on each coordinate axis are recorded. These projection values ​​allow us to determine the extreme boundaries of the bounding box in the local coordinate system, thereby accurately calculating the size and position of the 3D oriented bounding box.

[0184] ④ Bounding box description parameters

[0185] • The center position coordinates (x, y, Z_center) represent the position of the bounding box in the vehicle coordinate system;

[0186] • Three-dimensional dimensions (L, W, H) represent the length, width, and height of the bounding box;

[0187] • The rotation quaternion (q_x,q_y,q_z,q_w) represents the rotation angle of the bounding box relative to the vehicle coordinate system.

[0188] The point cloud fusion clustering and OBB calculation results are shown in Figure 5. Figure 5a From a bird's-eye view, Figure 5b This is a side view. The image shows the obstacle clustering results and the calculated OBB (On-Board Balance) of the obstacles and the extended vehicle. Each obstacle's OBB has two lines of text above it: the first line displays 'Label', indicating the obstacle's category; the second line displays 'Confidence', indicating the object detection confidence score. From 'Label:chair' and 'Confidence:0.830506' above the obstacle in the image, we can see that the obstacle's category is 'chair' and its confidence score is 0.830506.

[0189] IV. Dynamic Collision Detection and Graded Braking Strategy

[0190] The dynamic safety distance is adjusted according to the real-time speed of the vehicle to expand the vehicle's OBB size. The expanded vehicle OBB and the obstacle OBB are transformed to the world coordinate system. The collision risk of the two is detected by the separation axis theorem, and a graded braking strategy is executed according to the risk level.

[0191] 4.1 OBB Size Extension for Self-Driving Vehicles

[0192] The system acquires the autonomous vehicle's speed in real time and calculates the dynamic safety distance based on the formula dsafe=k·|v|+b, thus expanding the size of the autonomous vehicle's OBB. A schematic diagram of the autonomous vehicle's OBB size expansion is shown below. Figure 6 As shown in the figure, the inner solid rectangle represents the vehicle's original OBB dimensions, and the outer dashed rectangle represents the expanded OBB dimensions. The distance between the two is the dynamic safety distance calculated from the speed.

[0193] 4.2 Dynamic Collision Detection

[0194] The expanded vehicle OBB and the OBBs of each obstacle are transformed to the world coordinate system. Projection vectors are constructed using candidate separation axes (OBB edge normals) to determine whether there is overlap in the projection intervals of each OBB on the separation axis. If overlap is detected, a first-level warning command is triggered (speed is limited and the dynamic safety distance is switched to a fixed compensation value). If overlap is still detected under the warning state, a second-level emergency braking procedure is initiated to achieve rapid stopping and activate the collision warning signal.

[0195] ① Separation Axis Theorem

[0196] The core principle of the Separation Axis Theorem (SAT) is: for two convex polygons (in two-dimensional space) or two convex polyhedra (in three-dimensional space), if there exists a straight line (or a hyperplane in higher-dimensional space) that can separate the two objects, then the two objects must not intersect. If no such line can be found, then they must intersect. A schematic diagram of the Separation Axis Theorem is shown below. Figure 7 As shown in the diagram, the thick dashed line is called the separating line, and the thick solid line perpendicular to it is called the separating axis. The thin dashed lines represent the projections of the polygons onto the separating axis. If two projections of the polygons onto a separating axis do not overlap, it means that there exists a separating line that separates the two polygons, and the two polygons do not intersect.

[0197] ② Candidate separation axis generation

[0198] Four candidate separation axes were selected, using the normal directions of each side of the expanded vehicle's OBB and the obstacle's OBB as candidate separation axes. These separation axes will be used to subsequently determine whether a collision exists between the two bounding boxes. A schematic diagram of the rectangular candidate separation axis selection is shown below. Figure 8 As shown in the figure, ①②③④ are candidate separation axes.

[0199] ③ Collision Detection

[0200] Using the lower left corner vertex of the expanded vehicle's OBB as the origin, a vector is formed by connecting all vertices of the two bounding boxes to the origin. The projection of this vector onto the selected separation axis is calculated, and the minimum and maximum values ​​of the projection line segments of each OBB are recorded through the projection points. When there is an overlap of projection line segments on any candidate separation axis, it indicates that the expanded vehicle's OBB intersects with the obstacle's OBB, which is determined as a collision risk and triggers a graded braking command. Otherwise, the judgment continues until there is no overlap of projection line segments on all candidate separation axes, indicating that the expanded vehicle's OBB does not intersect with the obstacle's OBB, which is determined as no collision risk and exits the detection.

[0201] ④Graded braking strategy

[0202] Based on the collision detection results, the following graded braking measures will be implemented:

[0203] • Level 1 Warning: When a collision risk is detected, the vehicle's speed is first limited to 1.0 ± 0.2 m / s, and the dynamic safety distance is switched to a fixed compensation value d_fix = 0.15 ± 0.05 m. This measure is used to reduce the risk of collision and simultaneously alert the driver or the system to enter a warning state.

[0204] • Level 2 Emergency Braking: After switching to the fixed compensation safety distance, the system performs collision detection again; if the expanded vehicle OBB is still detected to overlap with the obstacle OBB, the emergency braking procedure is initiated, reducing the vehicle speed to zero within 0.3±0.1 seconds and activating the collision warning signal to ensure that accidents are avoided to the greatest extent possible.

[0205] Through the above steps, this embodiment can calculate the extended dimensions of the vehicle's OBB in real time, generate candidate separation axes, and accurately detect collision risks using a vector projection overlap judgment method. Combined with a graded braking strategy, the system can take warning or emergency braking measures based on real-time detection results, thereby achieving safe and efficient vehicle collision protection.

[0206] V. Effects of the Implementation Examples

[0207] Through the collaborative work of various modules, this invention achieves high-precision obstacle detection and accurate collision prediction in complex dynamic environments, and can trigger graded braking in a timely manner, greatly improving the driving safety and obstacle avoidance response speed of unmanned vehicles. Figure 9a and Figure 9b The diagram shows the actual operating effect, demonstrating the excellent obstacle detection accuracy of the invention and providing reliable technical support for autonomous driving applications in complex road environments.

Claims

1. A multi-modal sensor based intelligent obstacle avoidance method for unmanned vehicles, characterized in that, Includes the following steps: Step 1: Multimodal sensor data fusion: The LiDAR point cloud data and depth camera RGB-D data are aligned in time and space through the spatiotemporal registration module, and the two types of sensor data are uniformly converted to the vehicle coordinate system; Step 2, Obstacle Detection and Semantic Labeling: The point cloud data collected by the LiDAR is preprocessed and segmented to extract the obstacle point cloud. The depth camera data is processed by the target detection module to output the target bounding box and semantic label. Based on the depth information and camera intrinsic parameters, the 3D semantic seed points of the target are generated. Step 3: Fusion clustering based on semantic and geometric features: Based on the three-dimensional semantic seed points, perform region growing clustering on the obstacle point cloud data to generate semantic obstacle clusters, perform Euclidean distance clustering on the remaining unclustered point clouds to generate geometric obstacle clusters, and calculate the three-dimensional directed bounding box (OBB) of each cluster. Step 4, Dynamic Collision Detection and Graded Braking: Adjust the dynamic safety distance according to the real-time speed of the vehicle to expand the vehicle's OBB size, transform the expanded vehicle OBB and obstacle OBB to the world coordinate system, use the separation axis theorem to detect the collision risk of the two, and execute graded braking strategy according to the risk level. The fusion clustering includes: ① Perform region growing clustering on semantic seed points, with a neighborhood search radius of 0.2-0.4 meters and a minimum number of cluster points of 5-15; ② Perform Euclidean distance clustering on the remaining point cloud, with a neighborhood radius of 0.4-0.6 meters and a minimum number of cluster points of 3-8; The calculation steps for the 3D directed bounding box (OBB) of the point cloud cluster include: ① Vertical dimension extreme value extraction: Traverse the point cloud cluster to obtain the minimum vertical coordinate Z_min and the maximum vertical coordinate Z_max; ② Planar projection processing: Set the vertical coordinates of all points in the point cloud cluster to the reference value 0, and retain only the horizontal coordinates (x,y) to generate a two-dimensional projected point cloud; ③ Moment of inertia OBB calculation: Based on the two-dimensional projection point, the bounding box center position, two-dimensional dimensions, and rotation matrix are calculated; the two-dimensional dimensions are length and width; ④ 3D reconstruction: Assign the extracted vertical extrema to the bounding box, where the vertical center coordinate of the bounding box is Z_center=(Z_min+Z_max) / 2, and the vertical dimension is H=Z_max-Z_min; ⑤ Output: The output includes the center position vector, rotation quaternion, and bounding box description parameters of the three-dimensional dimensions; The bounding box description parameters include: The center position coordinates are (x, y, z_center); the three-dimensional dimensions are (L, W, H); and the rotation quaternion (q_x, q_y, q_z, q_w) represents the azimuth angle of the bounding box; where q_w is the real part of the quaternion; q_x, q_y, and q_z are the imaginary parts of the quaternion; the real part is the scalar part, and the imaginary part is the vector part. The calculation of the moment of inertia OBB includes: ① Moment of inertia estimation: Compute the centroid coordinates and covariance matrix of the input point cloud, and perform eigenvalue decomposition on the covariance matrix to extract eigenvectors as the main direction of the point cloud data, thereby determining the main distribution trend of the point cloud; ② Construction of local coordinate system: Establish a local coordinate system along the principal direction with the calculated centroid as the origin; ③ Rotation matrix acquisition: Calculate the rotation matrix from the original point cloud coordinate system to the local coordinate system to achieve coordinate transformation of the point cloud data; ④Extremal projection: The point cloud data is transformed to a local coordinate system using the rotation matrix, and the minimum and maximum projection values ​​on each coordinate axis are recorded to determine the extreme boundaries of the bounding box in the local coordinate system.

2. The method of claim 1, wherein, The spatiotemporal registration module includes: ① The time synchronization unit adopts an approximate time synchronization strategy based on message queues to ensure that the maximum time difference does not exceed 0.1 seconds; ② Spatial alignment unit: using the pre-calibrated extrinsic parameter matrix between the LiDAR and the depth camera, the depth camera data is converted to the LiDAR coordinate system and then uniformly converted to the vehicle coordinate system.

3. The method according to claim 1 or 2, characterized in that, LiDAR point cloud preprocessing includes: ① A statistical filter was used to remove outliers from the original point cloud, where the number of neighboring points used to calculate the mean distance was set to 40 to 60, and the standard deviation coefficient threshold was set to 0.8 to 1.2; ② The point cloud after outlier removal is downsampled using a voxel grid filter, with the voxel size set to 0.08 to 0.12 meters; ③ROI region acquisition: Define the first cube region based on the given coordinates (minPoint, maxPoint) as the region of interest, and retain the point cloud data within this region; ④ Vehicle point cloud removal: Define a second cube region based on the given coordinates (car_minPoint, car_maxPoint) to represent the spatial range of the vehicle body, and remove point clouds that fall into this region; The lidar point cloud segmentation includes: The point cloud after the above processing is segmented using a random sampling consensus algorithm, which divides it into ground point cloud and obstacle point cloud. The obstacle point cloud is retained for subsequent processing, and the algorithm iterates no less than 80 times, with a plane distance threshold of no more than 0.15 meters.

4. The method of claim 3, wherein, Depth camera data processing includes: ① The target detection module processes the images acquired by the depth camera and outputs the target bounding box and corresponding semantic label, wherein the confidence threshold is limited to the range of 0.4 to 0.6; ②Based on the pixel coordinates of the target bounding box center and its corresponding depth value in the depth map, and combined with the pre-determined camera intrinsic parameters, the target center point is accurately transformed from the two-dimensional image coordinates to the vehicle coordinate system to obtain the three-dimensional semantic seed point.

5. The method according to claim 1, characterized in that, The dynamic collision detection steps include: ① Dynamic safety distance extension: Based on the vehicle's real-time speed v, according to formula d safe = k·|v| + b to calculate the safety distance, where k∈[0.15,0.25], b∈[0.4,0.6], and the safety distance is uniformly extended to the bounding box dimension boundary of the vehicle's OBB; ② Candidate separation axis generation: The normal directions of each side of the expanded vehicle OBB and the obstacle OBB are used as candidate separation axes, and a total of 4 separation axes are selected; ③ The collision determination module takes the lower left corner vertex of the expanded vehicle OBB as the origin, connects all vertices of the two bounding boxes to the origin to form a vector, and calculates the projection of the vector onto the selected separation axis. The minimum and maximum values ​​of the projection line segments of each OBB are recorded through the projection points. When there is an overlap of projection line segments on any candidate separation axis, it indicates that the expanded vehicle OBB intersects with the obstacle OBB, which is determined as a collision risk and triggers a graded braking command. Otherwise, the determination continues until there is no overlap of projection line segments on all candidate separation axes, indicating that the expanded vehicle OBB does not intersect with the obstacle OBB, which is determined as no collision risk and exits the detection.

6. The method of claim 5, wherein, The graded braking strategy includes: ① Level 1 warning: When a collision risk is detected, the vehicle speed is limited to 1.0±0.2 m / s, and the dynamic safety distance is switched to a fixed compensation value d_fix=0.15±0.05 m; ② Level 2 Emergency Braking: After switching to a fixed compensation safety distance, collision detection is repeated. If the overlap of the body boxes is detected again, the emergency stop procedure is initiated, reducing the vehicle speed to zero and activating the collision warning signal within 0.3±0.1 seconds.

7. A multi-modal sensor based intelligent obstacle avoidance system for unmanned vehicles, characterized in that: The system has a program module corresponding to the steps of any one of the claims 1-6 above, and executes the steps in the intelligent obstacle avoidance method for unmanned vehicles based on multimodal sensors when it is run.

8. A computer-readable storage medium, characterized in that: The computer-readable storage medium stores a computer program configured to implement, when invoked by a processor, the steps of the intelligent obstacle avoidance method for unmanned vehicles based on multimodal sensors according to any one of claims 1-6.

Images