A collision warning method based on lidar in unstructured environments
By collecting point cloud data with lidar and combining it with Kalman filter and obstacle clustering detection, the problem of low collision warning accuracy in unstructured environments has been solved, achieving higher accuracy in obstacle recognition and collision warning, and enhancing vehicle driving safety.
Patent Information
- Authority / Receiving Office
- CN · China
- Patent Type
- Patents(China)
- Current Assignee / Owner
- BEIJING MECHANICAL EQUIP INST
- Filing Date
- 2024-06-25
- Publication Date
- 2026-07-17
AI Technical Summary
Existing collision warning methods have low accuracy and effectiveness in unstructured environments, failing to effectively ensure vehicle driving safety. This is mainly due to the lack of road markings, uneven road surfaces, and the diverse and irregular types of obstacles, which make obstacle identification and motion prediction difficult.
Point cloud data is collected using LiDAR. By estimating the vehicle's pose and speed, a predicted vehicle trajectory is generated. Ground point cloud filtering and obstacle clustering detection are performed. Combined with Kalman filter, smooth updates are made, RSS distance is calculated, and a collision warning is triggered.
It improves the accuracy of obstacle recognition and collision warning, enhances vehicle driving safety, reduces false alarm rate, has strong adaptability, and improves the robustness and applicability of the system.
Smart Images
Figure CN121208859B_ABST
Abstract
Description
Technical Field
[0001] This invention relates to the field of vehicle driving safety technology, and in particular to a collision warning method based on lidar (LiDAR) in unstructured environments. Background Technology
[0002] In known traffic accident cases, collisions account for a large proportion of vehicle safety issues, with causes involving a variety of factors such as illegal driving, driving environment, and technical limitations. Driving in unstructured environments, due to factors such as a lack of road markings, uneven road surfaces, and non-standard obstacles, increases the risk of natural collisions. Therefore, equipping vehicles with collision warning systems is essential to improving driving safety.
[0003] Vehicle collision warning systems use onboard sensors to detect the vehicle's surroundings, analyze and assess potential collision risks in real time, and send audible or tactile warnings to the driver when necessary to reduce the probability of accidents and improve driving safety. In structured urban environments, vehicle collision warning systems typically analyze collision risks based on information such as the vehicle's lane position, self-vehicle planned trajectory, obstacle types, and trajectory changes. However, in unstructured environments, due to the lack of road markings, vehicle collision warning systems generally plan their route in limited open spaces, and the types of obstacles are not all known pre-trained objects to be identified, making it difficult to analyze and predict the movement direction and purpose of obstacles and establish corresponding trajectory prediction models. Due to the unevenness of the road surface, vehicles may experience tire spin and slippage during driving, increasing the error in predicting the vehicle's motion trend and affecting the accuracy of the collision warning system's predictive analysis. Therefore, conventional collision warning methods are limited in unstructured environments and cannot fully guarantee vehicle driving safety.
[0004] Existing conventional collision warning methods are limited in their use in unstructured environments and cannot fully guarantee vehicle driving safety. Due to the lack of road markings, uneven road surfaces, and diverse and irregular obstacle types in unstructured environments, the accuracy and effectiveness of the prediction and analysis of existing collision warning methods are reduced. Summary of the Invention
[0005] Based on the above analysis, the embodiments of the present invention aim to provide a collision warning method based on lidar in unstructured environments, in order to solve the technical problem of low accuracy and effectiveness of existing methods for collision warning of vehicles in unstructured environments.
[0006] This invention provides a collision warning method based on lidar in unstructured environments, comprising the following steps:
[0007] Step S1: In the vehicle coordinate system, use lidar to collect point cloud data in an unstructured environment, use the point cloud data to estimate the vehicle's motion pose, and obtain the vehicle's motion pose and speed.
[0008] Step S2: Generate a predicted trajectory of vehicle motion based on the vehicle's motion pose and speed, and perform ground point cloud filtering and obstacle clustering detection on the point cloud data in the predicted driving area of the vehicle to obtain the obstacle clustering result.
[0009] Step S3: Based on the obstacle clustering results, perform obstacle geometric collision checks to determine the nearest obstacle vertex to the vehicle within the vehicle's predicted trajectory range. Estimate the movement speed of the nearest obstacle vertex and calculate the RSS distance. If the RSS distance is less than a preset safe distance threshold, trigger a collision warning.
[0010] Furthermore, the obstacle vertex closest to the current vehicle position is selected as a candidate point for collision detection, and the obstacle's speed is estimated as follows:
[0011] d pos =norm(p cur -p pre )
[0012]
[0013] v obj =v norm *cos(yaw diff )+v ego
[0014] Where, p cur and p pre These are the position vectors of the nearest obstacle vertex in the current frame and the previous frame, respectively. diff The heading angle of the estimated vehicle's motion pose is used to estimate the deviation between the heading angle of the obstacle's displacement vector. dt is the time difference between the timestamps of the nearest obstacle vertex in the current frame and the timestamps of that vertex in the previous frame. pos Let p be the position vector cur and p pre The second norm, v norm The relative velocity of the obstacle; v ego v is the linear velocity of the vehicle. obj The velocity of the obstacle in the 2D plane.
[0015] Furthermore, the RSS distance is calculated as follows:
[0016]
[0017] Among them, tresponse It is the response time when the vehicle begins to decelerate, a ego_max With a obj_max These are the maximum decelerations of the vehicle and the obstacle, respectively, α. sense This is the adjustment factor for the early warning sensitivity coefficient, and offset is the preset measurement bias value.
[0018] Furthermore, a time-delay model is used to extend the Kalman filter with hysteresis. Based on the point cloud data, the vehicle's motion pose and velocity are estimated using the Kalman filter with hysteresis extension. The vehicle's motion pose and velocity estimated by the Kalman filter are then smoothly updated to obtain the smoothly updated vehicle motion pose and velocity. Specifically, estimating the vehicle's motion pose and velocity using the Kalman filter with hysteresis extension includes:
[0019] The latest position and velocity measurements are used as the latest observations and are input into the Kalman filter for observation hysteresis update; wherein, the latest position measurement includes x and y axis coordinates and heading angle; the latest velocity measurement includes linear velocity and angular velocity;
[0020] The Mahalanobis distance between the predicted value and the latest observation is calculated to detect outliers, and the latest observation with large outlier deviations is filtered out; the latest pose and velocity measurements that are retained are used as the vehicle's running pose and velocity.
[0021] Furthermore, based on the assumption of uniform motion, a 2D vehicle kinematic model is constructed to estimate the vehicle's motion pose. The vehicle pose includes six state variables: the vehicle's x and y axis coordinates, heading angle, heading angle correction term, linear velocity, and angular velocity.
[0022] The Kalman filter is extended with hysteresis using a delay model, as follows:
[0023]
[0024] Where A is the state prediction transfer matrix corresponding to the 2D vehicle kinematics model, I is the identity matrix, and Y is the vector representation of the six state variables.
[0025] Furthermore, vehicle motion prediction trajectories within a certain historical time range are generated at fixed time intervals. At each position point of the prediction trajectory, based on the vehicle's own size and shape, the vertices of the vehicle profile, i.e., the expansion profile points, are generated with that position point as the vehicle's reference center point. Connecting all the expansion profile points forms the trajectory expansion range, which is the vehicle's predicted driving area.
[0026] Furthermore, the point cloud data within the vehicle's predicted driving area is filtered by ground point cloud to obtain the remaining point cloud after filtering.
[0027] Euclidean clustering is used to perform obstacle clustering on the remaining point cloud after filtering;
[0028] The 2D polygonal envelope of obstacles is obtained based on the obstacle clustering results;
[0029] The vertices of the obstacle are obtained based on the 2D polygonal envelope of the obstacle.
[0030] Furthermore, the lateral distance between each obstacle vertex and the vehicle's predicted trajectory is calculated to determine whether the obstacle vertex is within the range of the vehicle's predicted trajectory. If so, the obstacle vertex closest to the current vehicle position is selected as a candidate point for collision detection, and the obstacle's running speed is estimated.
[0031] Collision checks between vehicles and obstacles are performed based on estimated obstacle speeds, and collision assessments are made by calculating RSS distances.
[0032] Further, the point cloud data within the vehicle's predicted driving area is subjected to ground point cloud filtering, including:
[0033] The predicted vehicle trajectory range is divided into a two-dimensional grid of a preset size; the point cloud within the predicted vehicle trajectory range is projected onto the two-dimensional grid area.
[0034] For each grid, perform ground equation fitting, select the point with the lowest height as the initial ground plane representative point, and use the points within the preset height threshold as ground plane points, and iteratively fit the ground plane equation;
[0035] Calculate the tilt and flatness of each grid region;
[0036] Based on the elevation, slope and flatness in the ground plane equation, the grid area closest to the vehicle is taken as the search starting point. The local concavity and convexity connectivity between the search starting point and the grid areas adjacent to it is calculated. The grid areas that satisfy the connectivity relationship are identified as ground plane areas.
[0037] Points within the aforementioned ground plane area are filtered out as ground points; the remaining non-ground point cloud is then aggregated for obstacle clustering detection.
[0038] Further, step S1 includes:
[0039] Point cloud data is acquired using lidar; the point cloud data is then sequentially subjected to motion distortion correction and downsampling to obtain the processed point cloud.
[0040] Based on the processed point cloud, a local map within a limited historical range is constructed. A hash table is used to accelerate the acquisition of the nearest neighbor matching point in the local map of each point in the currently collected point cloud and the corresponding point in the previous time step, resulting in a set of nearest neighbor matching point pairs. Based on the set of nearest neighbor matching point pairs, ICP is used to calculate the latest pose measurement and speed measurement of the current vehicle.
[0041] Based on the latest pose and velocity measurements obtained from ICP, the local map is updated using the current measured point cloud data; and the vehicle's pose and velocity are obtained by estimating the vehicle's motion pose.
[0042] Compared with the prior art, the present invention can achieve at least one of the following beneficial effects:
[0043] 1. Improved accuracy of obstacle warnings during vehicle operation, enhancing vehicle safety: Vehicle motion pose estimation is performed using point cloud data collected by LiDAR, improving the accuracy of vehicle motion state prediction, especially in unstructured environments. Geometric collision checks are conducted based on predicted vehicle motion trajectories and obstacle clustering detection, enabling timely warnings when potential collision risks occur, thus enhancing vehicle safety.
[0044] 2. Optimized obstacle recognition: By filtering ground point clouds and clustering obstacles, the system effectively distinguishes between ground points and obstacles, improving the accuracy of obstacle recognition and providing reliable obstacle information for collision warning.
[0045] 3. Improved positioning robustness and dynamic prediction capability: By employing a Kalman filter and its hysteresis extension, the vehicle's motion pose and speed are estimated and smoothly updated in real time, ensuring the positioning robustness of the collision warning system; by calculating the obstacle's motion speed and RSS distance, the dynamic behavior of the obstacle is predicted, enhancing the forward-looking nature of the collision warning.
[0046] 4. High adaptability and reduced false alarm rate for collision warnings: This method can effectively adapt to and provide accurate collision warnings for complex road conditions and diverse obstacles in unstructured environments, improving the applicability and robustness of the system; through precise obstacle clustering and collision checking algorithms, the possibility of false alarms is reduced, ensuring the reliability of the warning system.
[0047] In this invention, the above-described technical solutions can be combined with each other to achieve more preferred combinations. Other features and advantages of this invention will be set forth in the following description, and some advantages may become apparent from the description or be learned by practicing the invention. The objects and other advantages of this invention can be realized and obtained from what is particularly pointed out in the description and drawings. Attached Figure Description
[0048] The accompanying drawings are for illustrative purposes only and are not intended to limit the invention. Throughout the drawings, the same reference numerals denote the same parts.
[0049] Figure 1 This is a flowchart of a collision warning method based on lidar in an unstructured environment.
[0050] Figure 2 Flowchart for estimating vehicle motion pose changes and calculating vehicle speed based on point cloud data;
[0051] Figure 3 Flowchart for estimating vehicle motion state using a hysteresis extended Kalman filter;
[0052] Figure 4 Flowchart for obtaining obstacle clustering results from ground point cloud segmentation and obstacle clustering detection;
[0053] Figure 5 This is a flowchart for obstacle geometric collision checking based on obstacle clustering results. Detailed Implementation
[0054] Preferred embodiments of the present invention will now be described in detail with reference to the accompanying drawings, which form part of this application and are used together with the embodiments of the present invention to illustrate the principles of the present invention, but are not intended to limit the scope of the present invention.
[0055] Unstructured environments typically refer to areas lacking defined or fixed road structures, traffic rules, or standardized road signage. In such environments, road conditions may be irregular and unpredictable, and various obstacles may be present. Examples include mountain trails, fields, forests, or undeveloped areas. Specifically, unstructured environments include the following characteristics:
[0056] (1) Lack of road signs: There are no clear lane lines, traffic signals or signs to guide vehicles;
[0057] (2) Uneven road surface: There may be potholes, gravel, mud or other irregular terrain;
[0058] (3) Diversity of obstacles: such as rocks, trees, ruins, animals, etc. These obstacles are irregular in size, shape and location, and may also be moving or in motion.
[0059] To address the technical challenges of ensuring the accuracy and effectiveness of collision warnings for vehicles in unstructured environments, this invention employs lidar as a detection sensor and proposes a lidar-based collision warning method for unstructured environments.
[0060] The system uses lidar to estimate the vehicle's motion and pose changes, calculating the vehicle's linear velocity and yaw rate. Based on the vehicle's self-motion trend, a predicted trajectory is generated. LiDAR point clouds within a certain range near the predicted trajectory are extracted. After ground point cloud segmentation, non-standard obstacle point clouds are clustered and detected, and the external polygon envelope information corresponding to different obstacles is calculated. Geometric collision checks are performed on different obstacles, calculating the obstacle vertex closest to the vehicle and estimating its corresponding velocity. The RSS (Remaining Safe Stopping) distance information is calculated to detect the safe distance between the vehicle and obstacles, determining whether to trigger a vehicle collision warning.
[0061] This invention uses a lidar sensor and a domestically produced hardware computing board as the system hardware components to complete sensor data acquisition and program deployment and computation. It conducts computer program design using a collision warning system design method for unstructured environments to form the system software components. The combination of software and hardware constitutes a lidar-based collision warning system for unstructured environments.
[0062] A specific embodiment of the present invention discloses a collision warning method based on lidar in unstructured environments, such as... Figure 1 As shown, it includes the following steps:
[0063] Step S1: In the vehicle coordinate system, use lidar to collect point cloud data in an unstructured environment, use the point cloud data to estimate the vehicle's motion pose, and obtain the vehicle's running pose and speed.
[0064] Step S2: Generate a predicted trajectory of vehicle motion based on the vehicle's motion pose and speed, and perform ground point cloud filtering and obstacle clustering detection on the point cloud data in the predicted driving area of the vehicle to obtain the obstacle clustering result.
[0065] Step S3: Perform obstacle geometric collision check based on the obstacle clustering results, determine the obstacle vertex closest to the vehicle within the vehicle's predicted trajectory range, estimate the movement speed of the obstacle vertex and calculate the RSS distance; if the RSS distance is less than a preset safe distance threshold, trigger a collision warning.
[0066] The lidar is installed on the front bumper or the front of the roof of the vehicle to ensure that it covers the important field of vision in front of the vehicle, and the forward orientation of the lidar is consistent with the direction of the vehicle's movement and parallel to the vehicle body; the scanning plane of the lidar is parallel to the ground.
[0067] The positional deviation between the lidar and the vehicle reference center (the vehicle's center of mass or geometric center) is measured to obtain the external parameters between the lidar coordinate system and the vehicle coordinate system; based on the external parameters, the lidar coordinate system is transformed to the vehicle coordinate system.
[0068] The lidar is fixed horizontally to ensure that its scanning plane is parallel to the ground, which is crucial for accurately measuring the distance and height of objects.
[0069] Proper installation and calibration of the lidar are crucial. After installation, the lidar must be calibrated to determine the spatial relationship between the lidar coordinate system and the vehicle coordinate system. This involves measuring the deviation between the lidar and the vehicle's reference center (such as the center of mass or geometric center).
[0070] The calibration process of the lidar obtains the external parameters between the lidar coordinate system and the vehicle coordinate system. These parameters are essential for accurately converting the point cloud data collected by the lidar into the vehicle coordinate system.
[0071] The lidar must be securely installed to withstand vibrations and impacts that the vehicle may encounter while driving in unstructured environments, ensuring the stability of the lidar sensor and the reliability of the measurement data.
[0072] In this invention, all subsequent steps are performed using point cloud data in the vehicle coordinate system.
[0073] Step S1 includes:
[0074] Point cloud data is acquired using lidar; the point cloud data is then sequentially subjected to motion distortion correction and downsampling to obtain the processed point cloud.
[0075] Based on the processed point cloud, a local map within a limited historical range is constructed. A hash table is used to accelerate the acquisition of the nearest neighbor matching point in the local map of each point in the currently collected point cloud and the corresponding point in the previous time step, resulting in a set of nearest neighbor matching point pairs. Based on the set of nearest neighbor matching point pairs, ICP is used to calculate the latest pose measurement and speed measurement of the current vehicle.
[0076] Based on the latest pose and velocity measurements obtained from ICP, the local map is updated using the current measured point cloud data; and the vehicle's pose and velocity are obtained by estimating the vehicle's motion pose.
[0077] like Figure 2 As shown, the specific steps are as follows:
[0078] (1) LiDAR collects point cloud data.
[0079] Point cloud data of the vehicle's surrounding environment is collected using a lidar sensor. The data for each point in the point cloud includes the point cloud's three-dimensional coordinates (x, y, z) and the point cloud acquisition timestamp.
[0080] (2) Normalize the timestamps of the laser point cloud:
[0081] Subtract the timestamp of the start time of the point cloud scan from the measurement timestamp of each point in the point cloud, and then divide the result by the point cloud scan time interval of the frame to obtain the normalized timestamp of each point.
[0082] (3) Point cloud motion distortion correction:
[0083] The vehicle's motion pose change estimation results are read within the previous scan interval (the time interval between two frames of point cloud data). The motion pose change estimation results are obtained using laser odometry technology. The laser odometry obtains the vehicle's pose (x, y, z), attitude (pitch angle, yaw angle, roll angle), and velocity (linear velocity and angular velocity) based on the point cloud data.
[0084] Based on the assumption of uniform motion, the motion offset corresponding to each point is calculated to complete the motion distortion correction.
[0085] The laser point cloud data and the laser odometry at the current and previous moments are used to calculate the estimated motion pose and velocity. Interpolation is performed based on the timestamp of each point in the point cloud data to obtain the pose interpolation amount at the corresponding moment. The pose interpolation amount is multiplied by the point cloud coordinates to obtain the measurement point after motion distortion correction.
[0086] Motion status reading: Read the estimated results of the vehicle's motion pose changes within the previous scan interval, provided by the laser odometry.
[0087] Uniform motion assumption: Between two consecutive scan intervals, it is assumed that the vehicle moves at a uniform speed. Based on this assumption, the pose of each point in the point cloud when it is measured is estimated.
[0088] Pose interpolation: Using the vehicle's pose information at the current and previous moments, and combining it with the normalized time proportion of each point, interpolation is performed to obtain the pose interpolation amount at the measurement moment of that point.
[0089] For example, the scanning time of one frame is 0.01s, the interval between frames is 0.1s, the interpolation is 0.01s / 0.1s, one-tenth; the interpolation is a proportional value relative to the pose change; the one-tenth pose change is multiplied by the point cloud coordinates to obtain the corrected point cloud coordinates.
[0090] Multiply the pose interpolation by the point cloud coordinates to obtain the measurement point after motion distortion correction (corrected point cloud coordinates).
[0091] (4) Laser point cloud downsampling:
[0092] Laser point cloud downsampling is performed using a 3D voxel grid. The 3D space is divided into a series of uniform voxel grids, each with the same size and shape.
[0093] For example, one point is reserved in each voxel grid.
[0094] The selection of this point: using coordinates, for example, the point cloud coordinate values (x, y, z) are all divided by the side length of the voxel grid cube. Dividing the point cloud coordinates by the side length of the voxel grid yields the spatial voxel grid coordinates corresponding to the point cloud; this means that each point is assigned to a specific voxel based on its coordinates.
[0095] For each voxel grid, retain a representative measurement point. This ensures that there is only one point within each grid cell. This reduces subsequent computational overhead and improves computational efficiency.
[0096] The point cloud data after motion distortion correction is downsampled using a voxel grid. The point cloud coordinates are divided by the side length of the three-dimensional voxel grid to obtain the spatial voxel grid coordinates corresponding to the point cloud, ensuring that there is an observation point in each grid, thereby completing the laser point cloud downsampling.
[0097] By downsampling the point cloud data points, the original high-density point cloud data is downsampled into a sparser point cloud.
[0098] Laser point cloud downsampling simplifies subsequent processing steps by reducing the amount of data, while preserving as much important structural information of the environment as possible.
[0099] The benefits of downsampling include: reducing the complexity of data processing and the demand for computing resources; reducing data storage space; and preserving the main characteristics of the environment to facilitate subsequent processing steps.
[0100] Choosing the right voxel grid size is crucial. If the voxel grid is too large, important information may be lost, while if the voxel grid is too small, the downsampling effect will be insignificant.
[0101] (5) Perform ICP matching between laser point cloud and local map.
[0102] The currently acquired laser point cloud is matched with the local map of the previous moment using ICP (Iterative ClosestPoint) matching. First, the nearest neighbor matching point of the current point cloud is indexed.
[0103] The local map is constructed by accumulating historically measured point cloud information within a limited time range (point cloud multiplied by pose estimated by laser odometry), and the local map serves as a reference for ICP matching.
[0104] For example,
[0105] At time t1, the laser odometry outputs the pose at time t1 to construct a local map;
[0106] At time t2, the coordinates of the point cloud are multiplied by the pose at time t2, and the local map at time t1 is updated. The local map is continuously updated.
[0107] ICP matching is used to align current laser point cloud data with a local map. During ICP matching, the 3D coordinate difference between each point in the current point cloud and a point in the local map is calculated, and the point with the smallest difference is selected as the nearest neighbor matching point.
[0108] Hash tables are used to accelerate the storage and indexing of voxel grids, speeding up ICP pairing. The hash table accelerates the ICP pairing search process. The hash function quickly locates the voxel grid where the point in the point cloud is located and finds points within a certain range around it. The point with the closest Euclidean distance to the current laser point cloud and the local map is calculated. It is determined whether the distance between the matched point pairs meets the preset distance threshold requirement; otherwise, the matched point pairs are discarded.
[0109] Each point in the current point cloud data has only one pairing. By using the points in the current point cloud and their pairing with ICPs in the local map, the local map is continuously updated.
[0110] (6) Use a hysteresis extended Kalman filter to estimate the vehicle's pose and speed.
[0111] A time-delay model is used to extend the Kalman filter with hysteresis. Based on the point cloud data, the vehicle's motion pose and velocity are estimated using the Kalman filter with hysteresis extension. The estimated vehicle motion pose and velocity are then smoothly updated to obtain the smoothed vehicle motion pose and velocity. Specifically, estimating the vehicle motion pose and velocity using the Kalman filter with hysteresis extension includes:
[0112] The latest position and velocity measurements are used as the latest observations, and are input into the Kalman filter for observation hysteresis update to detect and obtain the predicted value; wherein, the latest position measurement includes x and y axis coordinates, and heading angle; the latest velocity measurement includes linear velocity and angular velocity;
[0113] The Mahalanobis distance between the predicted value and the latest observation is calculated to detect outliers, and the latest observation with large outlier deviations is filtered out; the latest pose and velocity measurements that are retained are used as the vehicle's running pose and velocity.
[0114] After ICP pairing, the output attitude and velocity are improved to account for the time delay. The results estimated using the hysteresis extended Kalman filter are smoother.
[0115] The coordinates estimated by the laser odometry are three-dimensional xyz. When using the Kalman filter, z is abandoned and the coordinates are reduced in dimension.
[0116] The laser odometer outputs three angles: yaw angle, roll angle, and pitch angle. The Kalman filter uses the yaw angle.
[0117] Hysteresis extended Kalman filters are used to estimate vehicle pose and velocity, such as Figure 3 As shown.
[0118] A hysteresis Kalman filter is used for pose and velocity prediction and updating, enabling high-frequency pose and velocity estimation and publishing.
[0119] Initial position: The initial position is when the vehicle is stationary and has not yet started moving.
[0120] Next, the Kalman filter is initialized.
[0121] Then, state model prediction is performed, as follows:
[0122] Based on the assumption of uniform motion, a 2D vehicle kinematic model is constructed to estimate the vehicle's motion pose. The vehicle pose includes six state variables: vehicle x and y axis coordinates, heading angle, heading angle correction term, linear velocity, and angular velocity.
[0123] The Kalman filter is extended with hysteresis using a delay model, as follows:
[0124]
[0125] Where A is the state prediction transfer matrix corresponding to the 2D vehicle kinematics model, I is the identity matrix, and Y is the vector representation of the set of six state variables, with subscripts indicating time.
[0126] The 2D vehicle kinematics model, including six state variables—vehicle position coordinates, heading angle, heading angle correction term, linear velocity, and acceleration—is constructed as follows:
[0127] x k+1 =x k +v k CoS(θ k +b k )dt
[0128] y k+1 =y k +v k sin(θ k +b k )dt
[0129] θ k+1 =θk +ω k dt
[0130] b k+1 =b k
[0131] v k+1 =vk
[0132] ω k+1 =ω k
[0133] Where, x k+1 and y k+1 x k and y k θ represents the x and y coordinates of the vehicle at times k+1 and k, respectively. k+1 θ k Let b represent the heading angles at times k+1 and k, respectively. k+1 b k This represents the heading angle correction term at times k+1 and k, v k+1 v k and ω k+1 ω k These represent the linear velocity and angular velocity of the vehicle at times k+1 and k, respectively.
[0134] A time-delay model is used for smooth updates of the Kalman filter. In the time-delay model, the variable corresponding to Y is a vector representation of the set of six state variables (i.e., xy-axis coordinates, heading angle, heading angle correction term, linear velocity, and acceleration) in the kinematic model of the Kalman filter. It is a 6-row, 1-column vector.
[0135] Figure 3 The latest position measurement and the latest velocity measurement are used as inputs to the Kalman filter. The latest position measurement is the x-axis and y-axis coordinates and yaw angle output by the laser odometry. The latest velocity measurement is the linear velocity and angular velocity output by the laser odometry. The latest pose measurement and the latest velocity measurement are used as inputs to the Kalman filter.
[0136] Measurement updates are as follows:
[0137] The vehicle's position (x, y coordinates), yaw angle, angular velocity, and linear velocity measured by the laser odometer are used as the observation update values for the filter.
[0138] The input data is first subjected to maximum hysteresis detection. The hysteresis extended Kalman filter only accepts data within a certain time range for effective observation updates. If the timestamp corresponding to the current observation update exceeds a preset time tolerance threshold, no further calculation or update is performed.
[0139] The filter prediction value closest to the observation update timestamp is read. Outlier detection is performed by calculating the Mahalanobis distance between the observation update and the filter prediction value, thereby filtering out observation updates with large outlier deviations. Observation updates with small outlier deviations are retained as pose updates and velocity updates and published.
[0140] The Kalman filter performs pose prediction at a fixed frequency. For the lidar odometry observation update with lag input, the observation hysteresis is first detected, and the Kalman filter only updates the observations within a finite time delay range.
[0141] The filter has a fixed frequency, for example, 10Hz, and it is recommended to keep it consistent with the LiDAR point cloud measurement frequency.
[0142] Then, the current observation update (current x and y axis coordinates, heading angle, linear velocity, and angular velocity) and the filter prediction value (x) are calculated. k+1 y k+1 θ k+1 v k+1 ω k+1 Outlier detection is performed using Mahalanobis distance between the observations, and updates for outliers (observations with large deviations) are rejected.
[0143] By compensating for the time delay of the latest pose and velocity measurements, adaptive integration of input observations can be achieved, enabling smooth updates and publication of measurement results.
[0144] The result obtained in step S1 is the vehicle's operating pose and velocity, where the pose includes the published position ( xy Coordinates and heading angle) and velocity (linear velocity and angular velocity).
[0145] Step S2: Generate a predicted trajectory of vehicle motion based on the vehicle's motion pose and speed, and perform ground point cloud filtering and obstacle clustering detection on the point cloud data in the predicted driving area of the vehicle to obtain the obstacle clustering result.
[0146] Step S2 includes steps S21-S22.
[0147] Step S21: Generate a predicted trajectory for vehicle motion based on the vehicle's pose and speed;
[0148] Vehicle motion prediction trajectories within a certain historical time range are generated at fixed time intervals. At each location point on the prediction trajectory, based on the vehicle's own size and shape, the vertices of the vehicle profile, i.e., the expansion profile points, are generated with that location point as the vehicle's reference center point. Connecting all the expansion profile points forms the trajectory expansion range, which is the vehicle's predicted driving area.
[0149] like Figure 4 As shown.
[0150] Vehicle pose (x) y The coordinates and heading angle) and velocity (linear velocity, angular velocity) are used as inputs.
[0151] After obtaining the input vehicle pose and speed information, vehicle trajectory prediction is performed.
[0152] The predicted trajectory of the vehicle is calculated using the following method:
[0153] x k+1 =x k +v k cos(θ k )dt
[0154] y k+1 =y k +v k sin(θ k )dt
[0155] θ k+1 =θ k +ω k dt
[0156] Predicted trajectories within a certain time range are generated at fixed time intervals.
[0157] For example, the fixed time interval is 0.1s, and the certain time range is 2s.
[0158] Connecting the predicted trajectories of vehicles to form a range is called the trajectory expansion range.
[0159] Then, at each location point on the predicted trajectory, based on the vehicle's size and shape, the vertices of the vehicle's profile, i.e., the expansion profile points, are generated with that location as the measurement center point. These points represent the space that the vehicle may occupy at the predicted time.
[0160] Connect all the points of the expansion contour to form the trajectory expansion range, which is the vehicle's predicted driving area.
[0161] Point cloud extraction within the predicted trajectory range: Only point clouds within the predicted trajectory range are extracted, reducing the amount of data.
[0162] Step S22: Filter the point cloud data in the predicted driving area of the vehicle by ground point cloud to obtain the remaining point cloud after filtering.
[0163] Euclidean clustering is used to perform obstacle clustering on the remaining point cloud after filtering;
[0164] The 2D polygonal envelope of obstacles is obtained based on the obstacle clustering results;
[0165] The vertices of the obstacle are obtained based on the 2D polygonal envelope of the obstacle.
[0166] Step S22 includes steps S221-S222.
[0167] Step S221: Filter the ground point cloud within the vehicle's driving range.
[0168] Ground point cloud filtering is performed on the point cloud data within the vehicle's predicted driving area, including:
[0169] The predicted vehicle trajectory range is divided into a two-dimensional grid of a preset size; the point cloud within the predicted vehicle trajectory range is projected onto the two-dimensional grid area.
[0170] For each grid, perform ground equation fitting, select the point with the lowest height as the initial ground plane representative point, and use the points within the preset height threshold as ground plane points, and iteratively fit the ground plane equation;
[0171] Calculate the tilt and flatness of each grid region;
[0172] Based on the elevation, slope and flatness in the ground plane equation, the grid area closest to the vehicle is taken as the search starting point. The local concavity and convexity connectivity between the search starting point and the grid areas adjacent to it is calculated. The grid areas that satisfy the connectivity relationship are identified as ground plane areas.
[0173] Points within the aforementioned ground plane area are filtered out as ground points; the remaining non-ground point cloud is then aggregated for obstacle clustering detection.
[0174] Ground point cloud filtering within vehicle driving range
[0175] First, the vehicle's predicted driving area is divided into different two-dimensional grid regions of a fixed size. Then, based on the three-dimensional coordinates of the point cloud in the point cloud data, the point cloud within the predicted trajectory range is projected onto the corresponding two-dimensional grid region.
[0176] For each region's point cloud, ground equation fitting is performed. The point with the lowest height in each region is selected as the initial ground plane representative point. Principal component analysis is used to fit the three-dimensional ground plane equation. Then, points within a preset height threshold obtained from distance fitting are selected as ground plane representative points. The corresponding ground plane equation is obtained through multiple rounds of iterative calculation.
[0177] For example, the preset height threshold is 0.3 meters.
[0178] The fitted ground plane equation is as follows:
[0179] Ax + By + Cz - d = 0
[0180] Where d represents the ground elevation, and [A, B, C] represents the normal vector of the ground.
[0181] The system screens and verifies whether a real ground plane actually exists within different grid areas, and uses ground plane elevation, tilt, flatness, and local concavity / convexity connectivity for joint diagnosis.
[0182] The inclination of the ground plane is calculated as follows:
[0183]
[0184] Where θ is the inclination.
[0185] Flatness σ n The calculation is as follows:
[0186]
[0187] Where λ represents the eigenvalue of the vector obtained by SVD (Singular Value Decomposition) during the fitting plane solution. 1，n , λ 2，n , λ 3，n These are the eigenvalues obtained when fitting the model in a plane.
[0188] The local concavity-convexity connectivity of the ground plane is calculated as follows:
[0189] |n i ·n j |>1-sin(||d ij ||ε)
[0190] Where n i and n j These represent the normal vectors of the fitted plane within the two adjacent regions, ||d ij || represents the distance between the center points of two planes, and ε is an adjustment threshold used to control the discrimination of local concavity and convexity connectivity.
[0191] The elevation, inclination, and local concavity / convexity in the ground plane equation are used for preliminary screening of the ground plane region. The region closest to the vehicle body is used as the search starting point. A breadth-first traversal method is used to calculate the local concavity / convexity connectivity between the current plane region and its neighboring regions. Regions that meet the connectivity relationship are identified as ground plane regions until all grid regions have been traversed.
[0192] The above steps are used to filter and obtain effective planar grid regions. Within each grid, points within a certain height range from the ground plane equation are calculated and used as planar points for further filtering. The remaining point clouds are then aggregated for subsequent obstacle clustering.
[0193] Step S222: Clustering of obstacles within the vehicle's driving range.
[0194] Select the point cloud within the driving range after filtering by ground point cloud, and divide the point cloud into grid regions in order of distance. Then, use Euclidean clustering to cluster obstacles within each region, setting different Euclidean clustering thresholds depending on the distance of the region. After obtaining different obstacle point clouds, calculate their 2D polygon envelopes.
[0195] Calculate the 2D polygon envelope for all inflated contour points. Based on the polygon envelope, the predicted range of the vehicle's future driving path is obtained (the generated contour points are aggregated, and the polygon envelope of this set of points is calculated again using mathematical methods; the calculated polygon envelope is the predicted driving path range). Select point clouds located within the predicted path range for subsequent obstacle extraction.
[0196] The predicted driving space of the vehicle is divided into uniform grids of quadrilaterals with a fixed resolution, and the extracted point cloud is projected onto the corresponding grids. These are typically quadrilateral grids.
[0197] Next, region-level ground fitting was performed for each grid region. Point cloud points with heights within a certain height threshold were selected as initial ground representative points, and PCA (Principal Component Analysis) was used to fit the region plane.
[0198] Then, iterative sampling is used to obtain point clouds for plane fitting and to calculate the plane equation.
[0199] Based on the set threshold, the planar grids initially divided into effective surface regions are filtered. Then, the connectivity between the ground within different grids is calculated through breadth-first traversal search, that is, the concavity and convexity of the ground connection in the local area. The grids that satisfy the connectivity relationship are selected as the ground point cloud segmentation regions.
[0200] Finally, points outside a certain threshold of ground height are selected as candidate obstacle point clouds.
[0201] A multi-threshold Euclidean clustering method is used to cluster candidate obstacle point clouds. Different clustering thresholds are set for point clouds at different distances, and the 2D polygon envelope of each obstacle is calculated.
[0202] The point cloud data obtained by clustering each obstacle is fitted into a 2D polygon envelope, which represents the spatial outline of the obstacle.
[0203] The output of step S2 is the obstacle clustering result, i.e., the 2D polygon envelope.
[0204] Step S3 includes steps S31-S33.
[0205] like Figure 5 As shown, this step performs obstacle geometric collision checks based on obstacle clustering results.
[0206] Step S31: Determine the nearest obstacle vertex to the vehicle within the predicted trajectory range and estimate the obstacle's speed.
[0207] The 2D polygons based on obstacle clustering results are used to obtain the vertices of the 2D envelope of obstacles.
[0208] Calculate the lateral distance between each 2D envelope vertex and the predicted vehicle trajectory. This is the distance perpendicular to the vehicle's direction of travel.
[0209] Determine whether the vehicle is within a safe driving range, filter the 2D envelope vertices of the obstacles closest to the vehicle's current position, and confirm whether the vertices of the polygon envelope are within the vehicle's safe driving range. Count all obstacle vertices that may collide with the vehicle.
[0210] Estimate the running speed of the 2D envelope vertices of the nearest obstacle.
[0211] Select the obstacle vertex closest to the current vehicle position as the candidate point for collision detection, and estimate the obstacle's speed as follows:
[0212] d pos =norm(p cur -p pre )
[0213]
[0214] v obj =v norm *cos(yaw diff )+v ego
[0215] Where, p cur and p pre These are the position vectors of the nearest obstacle vertex in the current frame and the previous frame, respectively, yaW diff The heading angle of the estimated vehicle's motion pose is used to estimate the deviation between the heading angle of the obstacle's displacement vector. dt is the time difference between the timestamps of the nearest obstacle vertex in the current frame and the timestamps of that vertex in the previous frame. pos Let p be the position vector cur and p pre The second norm, v norm The relative velocity of the obstacle; v ego v is the linear velocity of the vehicle. obj The velocity of the obstacle in the 2D plane.
[0216] Where dt=cur timestamp -pre timestamp ,cur timestamp With pre timestamp The timestamps corresponding to the nearest obstacle represented by the current nearest vertex and the nearest obstacle in the previous frame's point cloud are respectively detected. norm The relative velocity of the obstacle is incorporated into the vehicle's velocity, vehicle motion, and the relative velocity of the obstacle relative to the vehicle. `norm(.)` calculates the velocity of two vectors p. cur and p pre The L2 norm of the difference.
[0217] Step S32: Calculate the RSS distance.
[0218] Calculate the lateral distance between each obstacle vertex and the vehicle's predicted trajectory, determine whether the obstacle vertex is within the range of the vehicle's predicted trajectory, and if so, select the obstacle vertex closest to the current vehicle position as a candidate point for collision detection and estimate the obstacle's running speed.
[0219] Collision checks between vehicles and obstacles are performed based on estimated obstacle speeds, and collision assessments are made by calculating RSS distances.
[0220] RSS distance is the safe stopping distance required to avoid a vehicle colliding with an obstacle.
[0221] The RSS distance is calculated as follows:
[0222]
[0223] Among them, t response It is the response time when the vehicle begins to decelerate, a ego_max With a obj_max These are the maximum decelerations of the vehicle and the obstacle, respectively, α. sense This is the adjustment factor for the early warning sensitivity coefficient, and offset is the preset measurement bias value.
[0224] Early warning sensitivity coefficient adjustment factor α sense Used to adjust the warning sensitivity of the collision warning system. The minimum offset is 0.
[0225] If the calculated RSS distance is less than the preset distance threshold, indicating a risk of collision between the vehicle and the obstacle, the system will activate a collision warning and issue an emergency signal.
[0226] If the RSS distance is less than a preset distance threshold, it is determined that the vehicle is currently at risk of collision. This means that, based on the current movement of the vehicle and the obstacle, the vehicle may not be able to stop safely in front of the obstacle.
[0227] The method of this invention can be applied to both autonomous and manned driving scenarios.
[0228] The preset distance threshold is set based on safety standards and vehicle performance.
[0229] For example, emergency signals may include visual, auditory, or tactile warning signals to alert the driver to take action.
[0230] Visual signals: such as warning lights on the dashboard;
[0231] Auditory signals: voice prompts, beeps, or other alarm sounds;
[0232] Tactile feedback: such as steering wheel vibration or seat vibration.
[0233] After an emergency signal is issued, the driver responds by taking necessary measures to avoid a collision, such as braking, changing lanes, or stopping. If the driver does not respond in time, the system may prepare to activate Automatic Emergency Braking (AEB, also known as Automatic Collision Prevention) or Anti-lock Braking System (ABS) or other intervention measures to reduce the likelihood of a collision.
[0234] In summary, the collision warning method based on lidar in an unstructured environment according to embodiments of the present invention has the following beneficial effects:
[0235] 1. Improved accuracy of obstacle warnings during vehicle operation, enhancing vehicle safety: Vehicle motion pose estimation is performed using point cloud data collected by LiDAR, improving the accuracy of vehicle motion state prediction, especially in unstructured environments. Geometric collision checks are conducted based on predicted vehicle motion trajectories and obstacle clustering detection, enabling timely warnings when potential collision risks occur, thus enhancing vehicle safety.
[0236] 2. Optimized obstacle recognition: By filtering ground point clouds and clustering obstacles, the system effectively distinguishes between ground points and obstacles, improving the accuracy of obstacle recognition and providing reliable obstacle information for collision warning.
[0237] 3. Improved real-time performance and dynamic prediction capabilities: By employing a Kalman filter and its hysteresis extension, the vehicle's motion pose and speed are estimated and smoothly updated in real time, ensuring the rapid response capability of the collision warning system; by calculating the obstacle's motion speed and RSS distance, the dynamic behavior of the obstacle is predicted, enhancing the forward-looking nature of the collision warning.
[0238] 4. High adaptability and reduced false alarm rate for collision warnings: This method can effectively adapt to and provide accurate collision warnings for complex road conditions and diverse obstacles in unstructured environments, improving the applicability and robustness of the system; through precise obstacle clustering and collision checking algorithms, the possibility of false alarms is reduced, ensuring the reliability of the warning system.
[0239] The above description is only a preferred embodiment of the present invention, but the scope of protection of the present invention is not limited thereto. Any changes or substitutions that can be easily conceived by those skilled in the art within the scope of the technology disclosed in the present invention should be included within the scope of protection of the present invention.
Claims
1. A collision warning method based on lidar in unstructured environments, characterized in that, Includes the following steps: Step S1: In the vehicle coordinate system, use lidar to collect point cloud data in an unstructured environment, use the point cloud data to estimate the vehicle's motion pose, and obtain the vehicle's motion pose and speed. A time-delay model is used to extend the Kalman filter with hysteresis. Based on the point cloud data, the vehicle's motion pose and velocity are estimated using the Kalman filter with hysteresis extension. Step S2: Generate a predicted vehicle motion trajectory based on the vehicle's motion pose and velocity; The range of the vehicle motion prediction trajectory is divided into a two-dimensional grid of a preset size; the point cloud within the range of the vehicle motion prediction trajectory is projected onto the two-dimensional grid area; For each grid, perform ground equation fitting, select the point with the lowest height as the initial ground plane representative point, and use the points within the preset height threshold as ground plane points, and iteratively fit the ground plane equation; The point cloud data within the vehicle's predicted driving area is then filtered for ground point cloud data and subjected to obstacle clustering detection to obtain obstacle clustering results. The point cloud data in the predicted driving area of the vehicle is filtered by ground point cloud to obtain the remaining point cloud after filtering. Euclidean clustering is used to perform obstacle clustering on the remaining point cloud after filtering; The 2D polygonal envelope of obstacles is obtained based on the obstacle clustering results; The vertices of the obstacle are obtained based on the 2D polygonal envelope of the obstacle; Step S3: Based on the obstacle clustering results, perform obstacle geometric collision checks to determine the nearest obstacle vertex to the vehicle within the vehicle's predicted trajectory range, estimate the movement speed of the nearest obstacle vertex, and calculate the RSS distance; if the RSS distance is less than a preset safe distance threshold, trigger a collision warning. The RSS distance is calculated as follows: in, It is the response time when the vehicle begins to decelerate. and These are the maximum decelerations of the vehicle and the obstacle, respectively. As an adjustment factor for the early warning sensitivity coefficient, This is the preset measurement bias value.
2. The method according to claim 1, characterized in that, Select the obstacle vertex closest to the current vehicle position as the candidate point for collision detection, and estimate the obstacle's speed as follows: in, and These are the position vectors of the nearest obstacle vertex in the current frame and the previous frame, respectively. To estimate the deviation between the heading angle of the vehicle's motion pose and the heading angle of the obstacle's displacement vector, The time difference between the timestamps of the nearest obstacle vertex in the current frame and the timestamps of that vertex in the previous frame. Position vector and L2 norm, The relative velocity of the obstacle; For the linear velocity of the vehicle, The velocity of the obstacle in the 2D plane.
3. The method according to claim 1, characterized in that, The vehicle motion pose and velocity estimated by the Kalman filter are then smoothly updated to obtain the smoothed updated vehicle motion pose and velocity; wherein, the estimation of the vehicle motion pose and velocity using a Kalman filter with hysteresis spread includes: The latest position and velocity measurements are used as the latest observations and are input into the Kalman filter for observation hysteresis update; wherein, the latest position measurement includes x and y axis coordinates and heading angle; the latest velocity measurement includes linear velocity and angular velocity; Outlier detection is performed by calculating the Mahalanobis distance between the predicted value and the latest observation, and the latest observation with large outlier deviations is filtered out; the latest pose and velocity measurements that are retained are used as the vehicle's running pose and velocity.
4. The method according to claim 3, characterized in that, Based on the assumption of uniform motion, a 2D vehicle kinematic model is constructed to estimate the vehicle's motion pose. The vehicle pose includes six state variables: vehicle x and y axis coordinates, heading angle, heading angle correction term, linear velocity, and angular velocity. The Kalman filter is extended with hysteresis using a delay model, as follows: Where A is the state prediction transfer matrix corresponding to the 2D vehicle kinematics model, I is the identity matrix, and Y is the vector representation of the six state variables.
5. The method according to claim 1, characterized in that, Vehicle motion prediction trajectories within a certain historical time range are generated at fixed time intervals. At each location point on the prediction trajectory, based on the vehicle's own size and shape, the vertices of the vehicle profile, i.e., the expansion profile points, are generated with that location point as the vehicle's reference center point. Connecting all the expansion profile points forms the trajectory expansion range, which is the vehicle's predicted driving area.
6. The method according to claim 1, characterized in that, Calculate the lateral distance between each obstacle vertex and the vehicle's predicted trajectory, determine whether the obstacle vertex is within the range of the vehicle's predicted trajectory, and if so, select the obstacle vertex closest to the current vehicle position as a candidate point for collision detection and estimate the obstacle's running speed. Collision checks between vehicles and obstacles are performed based on estimated obstacle speeds, and collision assessments are made by calculating RSS distances.
7. The method according to claim 1, characterized in that, Ground point cloud filtering is performed on the point cloud data within the vehicle's predicted driving area, including: Calculate the tilt and flatness of each grid region; Based on the elevation, slope and flatness in the ground plane equation, the grid area closest to the vehicle is taken as the search starting point. The local concavity and convexity connectivity between the search starting point and the grid areas adjacent to it is calculated. The grid areas that satisfy the connectivity relationship are identified as ground plane areas. Points within the aforementioned ground plane area are filtered out as ground points; the remaining non-ground point cloud is then aggregated for obstacle clustering detection.
8. The method according to claim 1, characterized in that, Step S1 includes: Point cloud data is acquired using lidar; the point cloud data is then sequentially subjected to motion distortion correction and downsampling to obtain the processed point cloud. Based on the processed point cloud, a local map within a limited historical range is constructed. A hash table is used to accelerate the acquisition of the nearest neighbor matching point in the local map of each point in the currently collected point cloud and the corresponding point in the previous time step, resulting in a set of nearest neighbor matching point pairs. Based on the set of nearest neighbor matching point pairs, ICP is used to calculate the latest pose measurement and speed measurement of the current vehicle. Based on the latest pose and velocity measurements obtained from ICP, the local map is updated using the current measured point cloud data; and the vehicle's pose and velocity are obtained by estimating the vehicle's motion pose.