Unmanned vehicle intelligent obstacle avoidance method and system based on multi-mode sensor

Abstract

The invention 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. According to the technical key points of the invention, data of a laser radar and a depth camera are aligned through a space-time registration module; the laser radar point cloud is preprocessed, obstacle point cloud is segmented, fusion clustering is carried out in combination with three-dimensional semantic seed points output by a depth camera, and an obstacle cluster with a semantic tag is generated; and calculating a three-dimensional directed bounding box (OBB) of an obstacle, dynamically adjusting a safety distance according to the speed of the vehicle, expanding the OBB of the vehicle, and carrying out collision detection by adopting a separation axis theorem. When a collision risk is detected, the system sequentially triggers a first-level early warning instruction and a second-level emergency braking instruction, and safe and efficient unmanned vehicle dynamic obstacle avoidance is achieved. According to the method, the advantages of multiple sensors are fully utilized, the obstacle detection precision and the response speed are effectively improved, and reliable technical support is provided for application of the unmanned driving technology in a complex road environment.

Description

Technical Field

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

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

[0003] The prior art with document number CN107065890B discloses an unmanned vehicle obstacle avoidance system based on reinforcement learning, which includes a perception part, a decision part, a control part and an execution part. The perception part detects the obstacle area through a single-line laser radar to obtain obstacle information. The decision part decides whether the unmanned vehicle needs to avoid obstacles at the next moment based on the obstacle detection situation. If necessary, the control part inputs the state information of the perception part into the reinforcement learning model. The model converges to a stable state through trial and error learning, calculates the steering wheel angle required for the unmanned vehicle, and is executed by the unmanned vehicle execution part, thereby realizing the obstacle avoidance function.

[0004] The prior art with document number CN112859871B discloses a dynamic obstacle avoidance control method based on model prediction. First, the speed, posture and other information of the dynamic obstacle are obtained through radar, camera, etc. After obtaining the information, it is input into the model prediction layer. The given vehicle speed and heading angle are used as model input parameters. The obstacle is judged and identified based on the model prediction theory to determine whether it will enter the possible collision domain. If it is judged to enter the possible collision domain, it is input into the improved artificial potential field method. Through the improvement, the problem of falling into the local optimum in the classic artificial potential field method is solved. This prior art uses the theory of model prediction to model the unmanned vehicle and the obstacle separately, and divide the possible collision domain. The two-layer recognition and judgment of the entire dynamic obstacle avoidance process can greatly reduce the computational amount of the entire obstacle avoidance process. It improves and optimizes the artificial potential field method, greatly improving the safety and reliability of the unmanned vehicle obstacle avoidance planning.

[0005] However, how to leverage the complementary advantages of multimodal sensor data to achieve accurate obstacle detection, semantic labeling, and dynamic collision detection has become an important technical challenge in improving the safety of unmanned vehicles, and existing technologies currently do not provide a solution. Summary of the Invention

[0006] The purpose of the present invention is to provide an intelligent obstacle avoidance method for unmanned vehicles based on multimodal sensors. By temporal and spatial alignment of lidar and depth camera data, accurate obstacle detection and semantic labeling as well as dynamic collision detection are achieved. Combined with a graded braking strategy, real-time and accurate detection and avoidance of obstacles in complex environments are achieved, thereby improving the safe driving performance of the unmanned vehicle.

[0007] To achieve the above object, the present invention adopts the following technical solutions:

[0008] An intelligent obstacle avoidance method for an unmanned vehicle based on a multimodal sensor comprises the following steps:

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

[0010] Step 2: Obstacle Detection and Semantic Labeling: Preprocess and segment the point cloud data collected by the lidar to extract the obstacle point cloud. Use the object detection module to process the depth camera data, output the object bounding box and semantic label, and generate the 3D semantic seed point of the object based on the depth information and camera intrinsic parameters.

[0011] Step 3: Fusion clustering based on semantic and geometric features: Based on the 3D 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 3D directed bounding box (OBB) of each cluster is calculated.

[0012] Step 4: Dynamic Collision Detection and Graded Braking: The dynamic safety distance is adjusted based on the real-time speed of the ego vehicle to expand the size of the ego vehicle's OBB. The expanded ego vehicle OBB and the obstacle OBB are converted to the world coordinate system. The separating axis theorem is used to detect the collision risk between the two, and a graded braking strategy is implemented based on the risk level.

[0013] Furthermore, the spatiotemporal registration module includes:

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

[0015] ② The spatial alignment unit uses the pre-calibrated extrinsic matrix between the LiDAR and the depth camera to convert the depth camera data into the LiDAR coordinate system, and then uniformly converts it to the vehicle coordinate system;

[0016] Furthermore, the laser radar point cloud preprocessing includes:

[0017] ① A statistical filter is used to remove abnormal points in the original point cloud, where the number of neighborhood points used to calculate the mean distance is set to 40 to 60, and the standard deviation coefficient threshold is set to 0.8 to 1.2;

[0018] ② Use voxel grid filter to downsample the point cloud after removing outliers, and set the voxel size to 0.08 to 0.12 meters;

[0019] ③ROI area acquisition: define the first cube area according to the given coordinates (minPoint, maxPoint) as the region of interest, and retain the point cloud data within the area;

[0020] ④ Ego-car point cloud culling: Define a second cubic region based on the given coordinates (car_minPoint, car_maxPoint) to represent the spatial range of the ego-car, and then cull the point cloud that falls into this region;

[0021] The laser radar point cloud segmentation includes:

[0022] The RANSAC algorithm is used to segment the processed point cloud into ground point cloud and obstacle point cloud. The obstacle point cloud is retained for subsequent processing, and the algorithm iteration number is not less than 80 times, and the plane distance threshold is not greater than 0.15 meters.

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

[0024] ① Use the object detection module to process the image captured by the depth camera and output the object bounding box and corresponding semantic label, where 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, combined with the predetermined camera intrinsic parameters, the target center point is accurately converted from the 2D image coordinates to the vehicle coordinate system to obtain the 3D semantic seed point.

[0026] Furthermore, the fusion clustering includes:

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

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

[0029] The step of calculating the three-dimensional oriented bounding box (OBB) of the point cloud cluster includes:

[0030] ① Extraction of vertical dimension extreme values: traverse the point cloud cluster to obtain the minimum vertical coordinate Z_min and the maximum vertical coordinate Z_max;

[0031] ② Plane projection processing: The vertical coordinates of all points in the point cloud cluster are uniformly set to the reference value 0, and only the horizontal coordinates (x, y) are retained to generate a two-dimensional projection point cloud;

[0032] ③ Inertia moment OBB solution: Based on the two-dimensional projection point cloud computing bounding box center position, two-dimensional size (length, width) and rotation matrix;

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

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

[0035] The bounding box description parameters include:

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

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

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

[0039] ② Local coordinate system construction: With the calculated center of mass as the origin, a local coordinate system is established along the main direction;

[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] ④ Extreme projection: Use the rotation matrix to transform the point cloud data into the local coordinate system, and record the minimum and maximum projection values ​​on each coordinate axis to determine the extreme boundary of the bounding box in the local coordinate system.

[0042] Furthermore, the collision detection step includes:

[0043] ① Dynamic safety distance expansion: According to the real-time speed v of the vehicle, 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 expanded to the bounding box size boundary of the ego vehicle OBB;

[0044] ②Generate candidate separation axes: The normal directions of the edges of the expanded ego vehicle OBB and the obstacle OBB are used as candidate separation axes, and a total of four separation axes are selected;

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

[0046] Furthermore, the hierarchical braking strategy includes:

[0047] ① Level 1 warning: When a collision risk is detected, the vehicle's 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] ② Secondary emergency braking: After switching to a fixed compensation safety distance, the collision detection is repeated. If the bounding box overlap is detected again, the emergency stop procedure is initiated, the vehicle speed is reduced to zero within 0.3±0.1 seconds, and the collision warning signal is activated.

[0049] An intelligent obstacle avoidance system for an unmanned vehicle based on a multimodal sensor has a program module corresponding to the steps of the above technical solution, and executes the steps of the intelligent obstacle avoidance method for an unmanned vehicle based on a multimodal sensor when running.

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

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

[0052] The present invention discloses an intelligent obstacle avoidance method for unmanned vehicles based on multimodal sensors, which includes: aligning the laser radar and depth camera data through a spatiotemporal registration module; preprocessing the laser radar point cloud and segmenting the obstacle point cloud, implementing fusion clustering in combination with the three-dimensional semantic seed points output by the depth camera to generate obstacle clusters with semantic labels; calculating the three-dimensional directed bounding box (OBB) of the obstacle, dynamically adjusting the safety distance and expanding the OBB of the vehicle according to the speed of the vehicle, and using the separating axis theorem for collision detection. When a collision risk is detected, the system triggers the first-level warning and the second-level emergency braking command in sequence to achieve safe and efficient dynamic obstacle avoidance for the unmanned vehicle. The method of the present invention makes full use of the advantages of multiple sensors, effectively improves the obstacle detection accuracy and reaction speed, and provides reliable technical support for the application of unmanned driving technology in complex road environments.

[0053] Through the coordinated operation of various modules, this invention achieves high-precision obstacle detection and accurate collision prediction in complex and dynamic environments, and can promptly trigger graded braking, significantly improving the driving safety and obstacle avoidance response speed of unmanned vehicles. Practical operation has demonstrated the invention's superior obstacle detection accuracy, providing reliable technical support for unmanned driving applications in complex road environments. BRIEF DESCRIPTION OF THE DRAWINGS

[0054] Figure 1 It is a flowchart of the overall process of the present invention;

[0055] Figure 2 This is the flowchart for preprocessing the lidar point cloud;

[0056] Figure 3 is the image target detection result;

[0057] Figure 4 Schematic diagram of AABB and OBB;

[0058] Figure 5a The results of point cloud fusion clustering and OBB calculation from a bird's-eye view;

[0059] Figure 5b Point cloud fusion clustering and OBB calculation results under side view;

[0060] Figure 6 Schematic diagram of the OBB size expansion of the ego vehicle;

[0061] Figure 7 This is a schematic diagram of the principle of the separating axis theorem;

[0062] Figure 8 Schematic diagram for selecting candidate separating axes for rectangles;

[0063] Figure 9a This is a diagram showing the actual operation of the present invention from a top-down perspective;

[0064] Figure 9b This is a diagram showing the actual operation effect of the present invention from a side view. DETAILED DESCRIPTION

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

[0066] The embodiment of the present invention proposes an intelligent obstacle avoidance method for unmanned vehicles based on multimodal sensors, the overall flow chart of which is as follows: Figure 1 As shown in the figure, the method includes aligning LiDAR and depth camera data through a spatiotemporal registration module; preprocessing the LiDAR point cloud and segmenting the obstacle point cloud, which is then clustered using the 3D semantic seed points output by the depth camera to generate obstacle clusters with semantic labels; calculating the obstacle's 3D directed bounding box (OBB), dynamically adjusting the safety distance based on the vehicle's speed and expanding the OBB, and using the separating axis theorem for collision detection. When a collision risk is detected, the system sequentially triggers a Level 1 warning and a Level 2 emergency braking command, enabling safe and efficient dynamic obstacle avoidance for the unmanned vehicle. Specifically, it includes:

[0067] 1. Multimodal sensor data fusion

[0068] The temporal and spatial alignment of the lidar point cloud data and the depth camera RGB-D data is achieved through the spatiotemporal registration module. The spatiotemporal registration module includes:

[0069] ① Time synchronization unit: uses an approximate time synchronization strategy based on a message queue to ensure that the maximum time difference between two types of data does not exceed 0.1 seconds;

[0070] ② Spatial alignment unit: Use the pre-calibrated extrinsic matrix between the lidar and the depth camera to convert the depth camera data into the lidar coordinate system, and then uniformly convert it to the vehicle coordinate system.

[0071] 2. Obstacle Detection and Semantic Labeling

[0072] The lidar point cloud is preprocessed (including outlier point removal, voxel grid downsampling, ROI area acquisition and ego-vehicle point cloud removal), and then the obstacle point cloud is segmented using the RANSAC algorithm. At the same time, the target detection module is used to process the depth camera image, output the target bounding box and semantic label, and generate the target 3D semantic seed point by combining the camera intrinsic parameters and depth information.

[0073] 3. Fusion clustering based on semantic and geometric features

[0074] The generated 3D semantic seed points are used to perform region growing 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 OBB calculation includes horizontal plane projection, moment of inertia solution and 3D reconstruction.

[0075] 4. Dynamic collision detection and graded braking

[0076] According to the real-time speed of the vehicle, the formula d safe = k·|v|+b (where k∈[0.15,0.25] and b∈[0.4,0.6]) to calculate the dynamic safety distance and evenly scale it to the size of the ego vehicle's OBB. After converting the scaled ego vehicle's OBB and the obstacle's OBB to the world coordinate system, collision detection is performed using candidate separation axes (formed by the directions of the normal lines of each OBB edge, a total of four). If overlap of the OBB projected line segments on any separation axis is detected, a collision risk is determined and a graded braking command is triggered:

[0077] ① Level 1 warning braking: The 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] ② Secondary emergency braking: After the warning braking, if OBB overlap is detected again, the vehicle speed will be quickly reduced to zero within 0.3±0.1 seconds and the collision warning signal will be activated.

[0079] 5. Additional technical details

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

[0081] ② The depth camera data processing module uses an object detection algorithm (with a confidence threshold in the range of 0.4 to 0.6) to accurately convert the 2D image coordinates to the 3D semantic seed points in the vehicle coordinate system through the pixel coordinates of the target bounding box center and the depth map data, combined with the predetermined camera intrinsic parameters.

[0082] ③ Semantic clustering uses 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 uses 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 three-dimensional directed bounding box is done by extracting the vertical extreme value of the point cloud cluster, constructing a two-dimensional projected point cloud, and calculating the moment of inertia to obtain the center position, two-dimensional size and rotation matrix of the OBB. Then, three-dimensional reconstruction is performed to obtain a complete OBB description (including center coordinates (x, y, z_center), three-dimensional size (L, W, H) and rotation quaternion (q_x, q_y, q_z, q_w));

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

[0086] 1. Multimodal Sensor Data Fusion

[0087] In this embodiment, the system collects environmental data using a LiDAR and depth camera installed on an unmanned vehicle. A spatiotemporal registration module performs temporal and spatial alignment on the raw LiDAR point cloud data and the depth camera RGB-D data. Both sensor data types are uniformly converted to the vehicle coordinate system, resulting in 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 acquisition times is less than 0.1 seconds. The spatial alignment unit converts the RGB-D data collected by the depth camera into the LiDAR coordinate system based on a pre-calibrated extrinsic parameter matrix, and further unifies it into the vehicle coordinate system.

[0089] 1.1 Time Synchronization Unit

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

[0091] ①Asynchronous data acquisition architecture

[0092] Dual-queue storage: Create independent data buffers (buf1 / buf2) for the lidar and depth camera, respectively, and use the std::deque double-ended queue structure to achieve fast head-to-tail operations;

[0093] Non-blocking write: Zero-copy enqueueing of sensor data is achieved through each sensor callback function, and the writing process does not interrupt the main thread;

[0094] Timestamp binding: Each data packet carries the ROS standard header structure, recording the acquisition 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 frames of data from the two queues are continuously extracted and the absolute time difference Δt = |t1-t2| is calculated;

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

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

[0099] ③Technological advantages

[0100] Low latency: Queue operations have an O(1) complexity, and a single synchronization takes less than 2ms (measured on the Jetson Xavier platform).

[0101] High robustness: Tolerates instantaneous data loss from a single sensor (automatically reconnects with subsequent data);

[0102] Scalability: Supports synchronization with more sensors (such as millimeter-wave radar and ultrasonic sensors) by increasing the number of queues.

[0103] The pseudo code of the workflow of the time synchronization unit is described as follows:

[0104]

[0105] 1.2 Space synchronization unit

[0106] This method uses the ROS tf library to achieve spatial synchronization and relies on the pre-calibrated coordinate relationship between the radar and depth camera. During the calibration phase, the transformation (including translation and rotation) between the two sensor coordinate systems is determined. In the program, this calibrated transformation information is broadcast using tf::TransformBroadcaster. Through high-frequency cyclic broadcasting, the coordinate transformation information is updated in real time. Other nodes can obtain these transformations through tf::TransformListener, realizing the synchronization and fusion of different sensor data. Its technical implementation includes the following core elements:

[0107] ① External parameter calibration pre-storage

[0108] Calibration data acquisition: Jointly calibrate the LiDAR and depth camera using a checkerboard calibration plate, using a feature point matching algorithm (such as SVD decomposition) to solve the rigid transformation matrix (extrinsic parameter matrix) between the two sensors;

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

[0110] ②Dynamic coordinate broadcast mechanism

[0111] Coordinate tree construction: Establish a multi-level coordinate system relationship based on the ROS tf library, define the vehicle body coordinate system (body) as the root node, and the lidar (lidar_link) and depth camera (camera_color_optical_frame) as child nodes;

[0112] Transformation publishing: Continuously 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 at a high frequency through tf::TransformBroadcaster.

[0113] 2. Obstacle Detection and Semantic Labeling

[0114] The point cloud data collected by the LiDAR is preprocessed and segmented to extract obstacle point clouds. The object detection module is used on the depth camera data to output the target bounding box and semantic label, and generate the target's 3D semantic seed points based on the depth information and camera intrinsic parameters.

[0115] 2.1 Point cloud data preprocessing

[0116] The flowchart of the laser radar point cloud preprocessing is as follows Figure 2 As shown, the method includes using a statistical filtering algorithm to remove outliers in 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 the lidar point cloud data in the region of interest according to a preset ROI area, and removing the vehicle body point cloud data scanned by the lidar by setting the coordinate range of the space where the vehicle is located.

[0117] 2.2 Point Cloud Data Segmentation

[0118] The lidar point cloud segmentation includes using the random sampling consensus (RANSAC) algorithm to segment the pre-processed point cloud data into a ground point cloud and an obstacle point cloud, wherein the obstacle point cloud is retained as basic data for subsequent processing.

[0119] The specific implementation is to first randomly extract a minimum number of sample points (usually 3) from the preprocessed point cloud, and use these 3 points to build a candidate plane model; then calculate the distance of all points to the plane, and regard the points whose distance is within a preset threshold as inliers; after multiple iterations, select the plane with the largest number of inliers as the ground model, so that the points in the plane are classified as ground points, and the remaining points are regarded as obstacle points, realizing the effective segmentation of ground point cloud and obstacle point cloud in complex scenes.

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

[0121] The image data captured by the depth camera is processed by the object detection module, which outputs the target bounding box and corresponding semantic label. Based on the center pixel of the target bounding box and its corresponding depth value, combined with the camera's intrinsic parameters, a mathematical transformation is performed to obtain the target's 3D semantic seed point 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] The image data collected by the depth camera is processed by the target detection module (such as YOLOv5), and the output detection results include 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: class identifier class_id (e.g. pedestrian = 1, vehicle = 2).

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

[0127] ② Calculation of three-dimensional coordinates of the camera coordinate system

[0128] The mapping from two-dimensional pixel coordinates in the pixel coordinate system to three-dimensional coordinates in the camera coordinate system is achieved through the pre-calibrated camera intrinsic parameter matrix:

[0129] Internal parameter matrix:

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

[0131]

[0132] Among them, f x and f y Represents the horizontal and vertical focal lengths of the camera, in pixels. They are determined by the physical focal length of the camera and the physical size of each pixel. x and c yThe horizontal and vertical coordinates of the center of the image (the principal point) are expressed in pixels. Usually, the principal point is located at the geometric center of the image, but in some cases it may be offset.

[0133] Back 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 as follows:

[0135]

[0136] ③Multi-sensor coordinate system 1

[0137] The conversion from the camera coordinate system to the vehicle coordinate system is achieved through the pre-calibrated external parameter matrix of the lidar and camera:

[0138] Rigid transformation

[0139] The external parameter matrix obtained by calibration is as follows:

[0140]

[0141] Where R∈SO(3) is the rotation matrix, is the translation vector

[0142] Perform coordinate transformation

[0143] The three-dimensional coordinate P in the camera coordinate system c Convert to the three-dimensional coordinate 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: The semantic category inherited from the target detection result

[0150] Confidence: detection confidence

[0151] The target detection results of the depth camera image are as follows Figure 3 To highlight the experimental results, the object detection module only detects the 'chair' category and blocks other categories. The bounding box in the figure represents the detected object. The 'chair' labeled in the upper left corner indicates the target category, and the coordinates (-0.19, -0.22, 1.43) labeled in the lower left corner are the calculated 3D coordinates of the target in the camera coordinate system.

[0152] 3. 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 a 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 the obstacle point cloud to form obstacle clusters with clear semantic labels. For points that fail to pass semantic clustering, Euclidean distance clustering is used to generate geometric obstacle clusters.

[0156] ①Region growth clustering

[0157] Multi-seed region growing clustering of point clouds is a segmentation method based on spatial proximity. Its basic principle is to recursively group neighboring points into the same cluster, starting from an initial seed point. This method first selects multiple seed points as the core region of the initial cluster and then gradually expands the cluster range based on a growth criterion. The growth criterion is usually whether the distance between neighboring points is less than a preset threshold. By continuously expanding the seed region, multiple point cloud clusters with separate points and close internal connections are eventually formed, achieving spatial segmentation of obstacles. Because 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 category of the obstacle.

[0158] ②Euclidean distance clustering

[0159] Euclidean distance clustering of point clouds is a segmentation method based on spatial distance. Its principle is to use the three-dimensional coordinates of each point in the point cloud and determine their spatial proximity by calculating the Euclidean distance between two points. The algorithm usually starts with an unclassified point and searches for points whose distance to it is less than a preset threshold. These points are grouped into the same cluster and marked as visited points. Subsequently, this process is repeated for newly added neighboring points until the current cluster can no longer be expanded. Then, the next unclassified point is selected and the operation is repeated until all points are clustered. The Euclidean distance clustering method can quickly divide the 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 the horizontal plane, uses the moment of inertia method to obtain the 2D parameters of the OBB, and finally obtains the complete description 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 types of three-dimensional bounding boxes, which are widely used in computer graphics, robotics and collision detection. They are used to represent the minimum bounding box of an object in space. The schematic diagrams of AABB and OBB are as follows: Figure 4 As shown in Figure 2, an AABB is an axis-aligned rectangular box. In three-dimensional space, the six faces of an AABB are parallel to the X, Y, and Z axes, meaning that each face of the bounding box is parallel to an axis. An OBB is a bounding box that is not necessarily axis-aligned. It customizes the orientation of its bounding box based on the object's shape and pose, allowing it to more tightly enclose the object without being affected by gaps caused by rotation. In summary, an AABB is computationally efficient, but it can lead to space waste. OBBs minimize the size of the bounding box through rotational orientation, thereby improving space utilization. However, this comes at a high computational cost. Given that objects may have complex shapes and irregular rotations, this system uses OBBs (Oriented Bounding Boxes) as a collision detection solution. OBBs dynamically adjust the orientation and size of the bounding box based on the object's actual pose and geometry. Compared to AABBs (Axis-Aligned Bounding Boxes), they provide a tighter bounding box, reducing unnecessary white space, thereby improving collision detection accuracy and efficiency. For scenarios requiring high-precision collision detection, OBBs offer greater space utilization, especially when objects are rotated or have complex shapes.

[0164] ②OBB calculation steps

[0165] Vertical dimension extreme value extraction

[0166] First, we traverse all points in the point cloud cluster and extract their minimum and maximum vertical coordinates, which we record as Zmin and Zmax respectively. These extreme values ​​will help us calculate the vertical size of the bounding box in the subsequent steps.

[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 base value of 0, retaining only the horizontal coordinates (x, y) to form a two-dimensional projected point cloud. This reduces computational complexity and allows us to focus on bounding box calculations on the horizontal plane.

[0169] Moment of inertia OBB solution

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

[0171] 3D reconstruction

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

[0173] Result output

[0174] Output the final calculated bounding box description parameters, including the bounding box's center position vector (x, y, Z_center), three-dimensional dimensions (L, W, H), and rotation quaternion (q_x, q_y, q_z, q_w). These parameters can fully describe the spatial position and orientation of the point cloud cluster.

[0175] ③ Steps for calculating moment of inertia OBB

[0176] Moment of inertia estimation

[0177] First, the input point cloud data is processed to calculate the point cloud's centroid coordinates and covariance matrix. By performing eigenvalue decomposition on the covariance matrix, eigenvectors are extracted as the principal directions of the point cloud data. This step helps determine the primary distribution trend of the point cloud data and, in turn, the orientation of the bounding box.

[0178] Local coordinate system construction

[0179] Based on the calculated center of mass coordinates, a local coordinate system is established with the center of mass as the origin and along the main directions. This local coordinate system will help with subsequent rotation matrix calculations and coordinate transformations.

[0180] Rotation matrix acquisition

[0181] The coordinate transformation of the point cloud data is achieved by calculating the 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 into the new coordinate system to better calculate the shape and orientation of the bounding box.

[0182] Extreme projection

[0183] After transforming the point cloud data into a local coordinate system using a rotation matrix, the minimum and maximum projection values ​​on each coordinate axis are recorded. These projection values ​​can be used 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] Center position coordinates (x, y, z_center), which represent the position of the bounding box in the vehicle coordinate system;

[0186] · Three-dimensional size (L, W, H), which represents the length, width, and height of the bounding box;

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

[0188] The results of point cloud fusion clustering and OBB calculation are shown in Figure 5. Figure 5a For a bird's-eye view, Figure 5b This is a side view. The figure shows the obstacle clustering results and the calculated observable bounding box (OBB) for the obstacle and the expanded ego-vehicle. Above each obstacle's OBB are two lines of text: the first line displays 'Lable,' indicating the obstacle's category; the second line displays 'Confidence,' indicating the confidence score for the object detection. The text 'Lable: chair' and 'Confidence: 0.830506' above the obstacle in the figure indicates that the obstacle's category is 'chair' and its confidence score is 0.830506.

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

[0190] The dynamic safety distance is adjusted according to the real-time speed of the ego vehicle to expand the size of the ego vehicle's OBB. The expanded ego vehicle OBB and the obstacle OBB are converted to the world coordinate system. The separating axis theorem is used to detect the collision risk between the two, and a graded braking strategy is implemented according to the risk level.

[0191] 4.1 Expansion of the OBB Size of Self-Driving Vehicles

[0192] The system obtains the speed of the unmanned vehicle in real time, and calculates the dynamic safety distance according to the formula dsafe=k·|v|+b based on the speed, and expands the size of the ego vehicle OBB. The schematic diagram of the ego vehicle OBB size expansion is shown as follows Figure 6 In the figure, the inner solid rectangle represents the original OBB size of the ego vehicle, and the outer dashed rectangle represents the expanded OBB size of the ego vehicle. The distance between the two is the dynamic safety distance calculated based on speed.

[0193] 4.2 Dynamic Collision Detection

[0194] The expanded ego vehicle OBB and each obstacle OBB are transformed into the world coordinate system. A projection vector is constructed using the candidate separating axis (OBB edge normals). The system then determines whether the projection intervals of the OBBs on the separating axis overlap. If overlap is detected, a Level 1 warning is triggered (limiting speed and switching the dynamic safety distance to a fixed compensation value). If overlap is still detected during the warning state, a Level 2 emergency braking procedure is initiated, achieving a rapid stop and activating the collision warning signal.

[0195] ① Separating axis theorem

[0196] The core principle of the Separation Axis Theorem (SAT) is that for two convex polygons (in two-dimensional space) or two convex polyhedrons (in three-dimensional space), if there exists a straight line (or a hyperplane in high-dimensional space) that can separate the two objects, then the two objects must not intersect. If such a straight line cannot be found, then they must intersect. Figure 7 As shown in the figure, the thick dashed line is called the separating line, the thick solid line perpendicular to it is called the separating axis, and the thin dashed line represents the projection of the polygon onto the separating axis. If the two projected line segments of a polygon on a separating axis do not overlap, it means that such a separating line separates the two polygons and the two polygons do not intersect.

[0197] ②Generation of candidate separation axes

[0198] The normal directions of each side of the expanded ego vehicle OBB and the obstacle OBB are used as candidate separation axes, and a total of four candidate separation axes are selected. These separation axes will be used to determine whether there is a collision between the two bounding boxes. The schematic diagram of the selection of rectangular candidate separation axes is shown in the figure below. Figure 8 As shown in the figure, ①②③④ are candidate separation axes.

[0199] ③Collision determination

[0200] Taking the lower left corner vertex of the expanded ego vehicle OBB as the origin, connect all the vertices of the two bounding boxes with the origin to form a vector, and calculate the projection of the vector on the selected separating axis. Record the minimum and maximum values ​​of each OBB projection line segment through the projection point; when there is overlap of projection line segments on any candidate separating axis, it indicates that the expanded ego vehicle OBB intersects with the obstacle OBB, and a collision risk is determined and a graded braking command is triggered; otherwise, the judgment continues until there is no overlap of projection line segments on all candidate separating axes, indicating that the expanded ego vehicle OBB does not intersect with the obstacle OBB, and it is determined that there is no collision risk and the detection is exited.

[0201] ④Graded braking strategy

[0202] Based on the collision detection results, the following graded braking measures are taken:

[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, of 0.15 ± 0.05 m. This measure is intended to reduce the risk of collision while alerting the driver or the system to enter a warning state.

[0204] Level 2 Emergency Braking: After switching to a fixed compensation safety distance, the system performs another collision detection. If the expanded self-vehicle OBB is still detected to overlap with the obstacle OBB, the system initiates an emergency stop sequence, reducing the vehicle's speed to zero within 0.3±0.1 seconds and activating the collision warning signal to minimize the risk of an accident.

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

[0206] V. Effects of the Implementation

[0207] Through the coordinated work of various modules, the present 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 the unmanned vehicle. Figure 9a and Figure 9b This is a diagram of actual operation effects, showing the excellent obstacle detection accuracy of the present invention, providing reliable technical support for unmanned driving applications in complex road environments.

Claims

1. An intelligent obstacle avoidance method for unmanned vehicles based on multimodal sensors, characterized in that: The following steps are involved: Step 1: Multimodal sensor data fusion: The spatiotemporal registration module is used to align the lidar point cloud data and the depth camera RGB-D data in time and space, and the two types of sensor data are uniformly converted to the vehicle coordinate system. Step 2: Obstacle Detection and Semantic Labeling: Preprocess and segment the point cloud data collected by the lidar to extract the obstacle point cloud. Use the object detection module to process the depth camera data, output the object bounding box and semantic label, and generate the 3D semantic seed point of the object based on the depth information and camera intrinsic parameters. Step 3: Fusion clustering based on semantic and geometric features: Based on the 3D 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 3D directed bounding box (OBB) of each cluster is calculated. Step 4: Dynamic Collision Detection and Graded Braking: The dynamic safety distance is adjusted based on the real-time speed of the ego vehicle to expand the size of the ego vehicle's OBB. The expanded ego vehicle OBB and the obstacle OBB are converted to the world coordinate system. The separating axis theorem is used to detect the collision risk between the two, and a graded braking strategy is implemented based on the risk level.

2. The method according to claim 1, characterized in that The spatiotemporal registration module includes: ① The time synchronization unit adopts an approximate time synchronization strategy based on a message queue to ensure that the maximum time difference does not exceed 0.1 seconds; ② The spatial alignment unit uses the pre-calibrated extrinsic matrix between the lidar and the depth camera to convert the depth camera data into the lidar coordinate system, and then uniformly converts it to the vehicle coordinate system.

3. The method according to claim 1 or 2, characterized in that The laser radar point cloud preprocessing includes: ① A statistical filter is used to remove abnormal points in the original point cloud, where the number of neighborhood points used to calculate the mean distance is set to 40 to 60, and the standard deviation coefficient threshold is set to 0.8 to 1.2; ② Use voxel grid filter to downsample the point cloud after removing outliers, and set the voxel size to 0.08 to 0.12 meters; ③ROI area acquisition: define the first cube area according to the given coordinates (minPoint, maxPoint) as the region of interest, and retain the point cloud data within the area; ④ Ego-car point cloud culling: Define a second cubic region based on the given coordinates (car_minPoint, car_maxPoint) to represent the spatial range of the ego-car, and then cull the point cloud that falls into this region; The laser radar point cloud segmentation includes: The RANSAC algorithm is used to segment the processed point cloud into ground point cloud and obstacle point cloud. The obstacle point cloud is retained for subsequent processing, and the algorithm iteration number is not less than 80 times, and the plane distance threshold is not greater than 0.15 meters.

4. The method according to claim 3, characterized in that The depth camera data processing includes: ① Use the object detection module to process the image captured by the depth camera and output the object bounding box and corresponding semantic label, where 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, combined with the predetermined camera intrinsic parameters, the target center point is accurately converted from the 2D image coordinates to the vehicle coordinate system to obtain the 3D semantic seed point.

5. The method according to claim 4, characterized in that The fusion clustering includes: ① Perform region growing clustering on the semantic seed points, with a neighborhood search radius of preferably 0.2-0.4 meters and a minimum cluster point number of preferably 5-15; ② Perform Euclidean distance clustering on the remaining point cloud, with a neighborhood radius of preferably 0.4-0.6 meters and a minimum cluster point number of preferably 3-8; The step of calculating the three-dimensional directed bounding box OBB of the point cloud cluster includes: ① Extraction of vertical dimension extreme values: traverse the point cloud cluster to obtain the minimum vertical coordinate Z_min and the maximum vertical coordinate Z_max; ② Plane projection processing: The vertical coordinates of all points in the point cloud cluster are uniformly set to the reference value 0, and only the horizontal coordinates (x, y) are retained to generate a two-dimensional projection point cloud; ③ Inertia moment OBB solution: Based on the two-dimensional projection point cloud computing bounding box center position, two-dimensional size (length, width) and rotation matrix; ④ 3D reconstruction: assign the extracted vertical extrema to a bounding box, where the vertical center coordinate of the bounding box Z_center = (Z_min + Z_max) / 2 and the vertical size H = Z_max - Z_min; ⑤Result output: Output includes the center position vector, rotation quaternion and bounding box description parameters of three-dimensional size; The bounding box description parameters include: The center position coordinates (x, y, z_center); the three-dimensional dimensions (L, W, H); and the rotation quaternion (q_x, q_y, q_z, q_w) representing the orientation of the bounding box; where q_w is the real part (scalar part) of the quaternion; q_x, q_y, q_z are the imaginary part (vector part) of the quaternion.

6. The method according to claim 5, characterized in that The moment of inertia OBB solution includes: ① Moment of inertia estimation: Calculate the centroid coordinates and covariance matrix of the input point cloud, and perform eigenvalue decomposition on the covariance matrix to extract the eigenvector as the main direction of the point cloud data and determine the main distribution trend of the point cloud; ② Local coordinate system construction: With the calculated center of mass as the origin, a local coordinate system is established along the main direction; ③ 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; ④ Extreme projection: Use the rotation matrix to transform the point cloud data into the local coordinate system, and record the minimum and maximum projection values ​​on each coordinate axis to determine the extreme boundary of the bounding box in the local coordinate system.

7. The method according to claim 6, characterized in that The collision detection step comprises: ① Dynamic safety distance expansion: According to the real-time speed v of the vehicle, 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 expanded to the bounding box size boundary of the ego vehicle OBB; ②Generate candidate separation axes: The normal directions of the edges of the expanded ego vehicle OBB and the obstacle OBB are used as candidate separation axes, and a total of four separation axes are selected; ③ The collision judgment module takes the lower left corner vertex of the expanded ego vehicle OBB as the origin, connects all the vertices of the two bounding boxes with the origin to form a vector, and calculates the projection of the vector on the selected separation axis. The minimum and maximum values ​​of each OBB projection line segment are recorded through the projection point. When there is an overlap of projection line segments on any candidate separation axis, it indicates that the expanded ego vehicle OBB intersects with the obstacle OBB, and a collision risk is determined and a graded braking command is triggered. Otherwise, the judgment continues until there is no overlap of projection line segments on all candidate separation axes, indicating that the expanded ego vehicle OBB does not intersect with the obstacle OBB, and it is determined that there is no collision risk and the detection exits.

8. The method according to claim 7, characterized in that The graded braking strategy includes: ① Level 1 warning: When a collision risk is detected, the vehicle's 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; ② Secondary emergency braking: After switching to a fixed compensation safety distance, the collision detection is repeated. If the bounding box overlap is detected again, the emergency stop procedure is initiated, the vehicle speed is reduced to zero within 0.3±0.1 seconds, and the collision warning signal is activated.

9. An intelligent obstacle avoidance system for unmanned vehicles based on multimodal sensors, characterized by: The system has a program module corresponding to the steps of any one of claims 1 to 9 above, and executes the steps in the intelligent obstacle avoidance method for an unmanned vehicle based on a multimodal sensor when running.

10. A computer-readable storage medium, characterized in that: The computer-readable storage medium stores a computer program, and the computer program is configured to implement the steps of an unmanned vehicle intelligent obstacle avoidance method based on a multimodal sensor according to any one of claims 1 to 9 when called by a processor.

Images