An Automatic Emergency Braking System for Unmanned Low-Speed ​​Freight Vehicles Based on LiDAR

Abstract

This invention relates to an automatic emergency braking system for unmanned low-speed freight vehicles based on lidar, belonging to the field of vehicle driving safety, and solves the technical problems of low accuracy in safety warnings and low effectiveness in emergency control when dealing with sudden obstacles during freight vehicle operation. It includes a lidar system deployed on the front bumper or front roof of the freight vehicle to collect environmental point cloud data in real time in the vehicle coordinate system during operation; a computing module that receives the environmental point cloud data collected by the lidar in real time via an onboard Ethernet; estimates the freight vehicle's state based on the point cloud data, obtaining the freight vehicle's motion pose and speed; performs obstacle detection and collision warning based on the freight vehicle's motion pose and speed; if a collision risk is determined, a control command is sent to the freight vehicle chassis VCU; the freight vehicle chassis VCU is used to dynamically control the freight vehicle chassis based on the control command. This enables the unmanned low-speed freight vehicle to perform emergency braking when a collision risk occurs.

Description

Technical Field

[0001] This invention relates to the field of vehicle driving safety technology, and in particular to an automatic emergency braking system for unmanned low-speed freight vehicles based on lidar. Background Technology

[0002] In the field of freight transportation and material delivery support, unmanned trucks, as an innovative material transportation solution, are gradually becoming a hot topic in research and application. Unmanned truck transportation systems can autonomously navigate and transport goods in unstructured environments, which not only improves logistics efficiency but also reduces labor costs. However, the complexity of the delivery environment poses significant challenges to the navigation and safe operation of unmanned trucks.

[0003] Unstructured environments typically refer to environments without fixed patterns or rules, such as unmarked city streets, dirt roads in the countryside, open industrial parks, or rural paths. These environments may contain various obstacles, such as pedestrians, vehicles, animals, and temporary roadblocks, whose positions and behaviors are often unpredictable. In such environments, autonomous trucks need to possess advanced perception, decision-making, and response speeds to ensure safe and accurate delivery.

[0004] As a key component of autonomous trucks, collision warning systems are designed to issue timely warnings when potential collision risks are detected and, if necessary, activate automatic emergency braking systems to avoid or mitigate collisions. However, existing collision warning systems face numerous technical challenges in unstructured environments, such as the accuracy of obstacle recognition, the reliability of predicted trajectories, and real-time processing capabilities in complex traffic scenarios.

[0005] Existing conventional collision warning methods are limited in their use in unstructured environments and cannot fully guarantee the driving safety of trucks. 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, and their real-time performance is poor. Summary of the Invention

[0006] Based on the above analysis, the present invention aims to provide an automatic emergency braking system for unmanned low-speed freight vehicles based on lidar, in order to solve the technical problems of low accuracy in safety warnings and low effectiveness in emergency braking of unmanned low-speed freight vehicles during freight transport in existing methods.

[0007] This invention provides an automatic emergency braking system for unmanned low-speed freight vehicles based on lidar, comprising:

[0008] LiDAR, deployed on the front bumper or front of the roof of a truck, is used to collect environmental point cloud data in real time in the vehicle coordinate system during the operation of the truck.

[0009] The computing module receives environmental point cloud data collected by the LiDAR in real time via the vehicle Ethernet; estimates the truck's state based on the point cloud data to obtain the truck's motion pose and speed; performs obstacle detection and collision warning based on the truck's motion pose and speed; and sends control commands to the truck chassis VCU if a collision risk is determined.

[0010] The truck chassis VCU is used to dynamically control the truck chassis based on the control commands.

[0011] Furthermore, based on the truck's motion posture and speed, the truck performs obstacle detection and collision warning, including:

[0012] Based on the truck's motion pose and speed, a predicted trajectory for the truck is generated, and ground point cloud filtering and obstacle clustering detection are performed on the point cloud data within the predicted driving area of ​​the truck to obtain the obstacle clustering results.

[0013] Based on the obstacle clustering results, an obstacle geometric collision check is performed to determine the nearest obstacle vertex to the truck within the truck's predicted trajectory range. The movement speed of the nearest obstacle vertex is estimated to calculate the RSS distance. Based on the RSS distance, it is determined whether a collision warning is triggered. If so, the calculation module sends a control command to the truck chassis VCU.

[0014] Furthermore, the computing module sends the control commands to the vehicle chassis VCU via the CAN bus;

[0015] The control commands include emergency braking commands, deceleration commands, and speed limit commands;

[0016] If the RSS distance is greater than or equal to the preset speed limit threshold, a speed limit command is sent.

[0017] Otherwise, if the RSS distance is less than the preset speed reduction threshold, a speed reduction command is sent.

[0018] Otherwise, if the RSS distance is less than a preset emergency braking threshold, an emergency braking command is sent.

[0019] Furthermore, the obstacle vertex closest to the current truck position is selected as a candidate point for collision checking, and the obstacle's speed is estimated as follows:

[0020] d pos =norm(p cur -p pre )

[0021]

[0022] vobj =v norm *cos(yaw diff )+v ego

[0023] 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 truck'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 truck. obj The velocity of the obstacle in the 2D plane;

[0024] Based on the linear velocity of the truck and the speed of the obstacle on the plane, the RSS distance is calculated as follows:

[0025]

[0026] Among them, t response It is the response time when the truck begins to decelerate, a ego_max With a obj_max These are the maximum negative accelerations of the truck and the obstacle, respectively, α. sense This is the adjustment factor for the early warning sensitivity coefficient, and offset is the preset measurement bias value.

[0027] Furthermore, a time-delay model is used to extend the Kalman filter with hysteresis. Based on the point cloud data, the motion pose and velocity of the truck are estimated using the Kalman filter with hysteresis extension. The truck motion pose and velocity estimated by the Kalman filter are then smoothly updated to obtain the smoothly updated truck motion pose and velocity. Specifically, estimating the truck motion pose and velocity using the Kalman filter with hysteresis extension includes:

[0028] Using the latest position and velocity measurements as the latest observations, the Kalman filter is input to perform observation hysteresis detection to 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;

[0029] 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 truck's running pose and velocity.

[0030] Furthermore, based on the assumption of uniform motion, a 2D truck kinematic model is constructed to estimate the truck's motion pose. The truck's pose includes six state variables: the truck's x and y axis coordinates, heading angle, heading angle correction term, linear velocity, and acceleration.

[0031] The Kalman filter is extended with hysteresis using a delay model, as follows:

[0032]

[0033] Where A is the state prediction transfer matrix corresponding to the 2D truck kinematics model, I is the identity matrix, and Y is the vector representation of the six state variables.

[0034] Furthermore, a truck motion prediction trajectory within a certain historical time range is generated at fixed time intervals. At each location point of the prediction trajectory, based on the truck's own size and shape, the vertices of the truck's profile, i.e., the expansion profile points, are generated with that location point as the truck's reference center point. Connecting all the expansion profile points forms the trajectory expansion range, which is the truck's predicted driving area.

[0035] The point cloud data within the predicted driving area of ​​the truck is filtered by ground point cloud to obtain the remaining point cloud after filtering.

[0036] Euclidean clustering is used to perform obstacle clustering on the remaining point cloud after filtering;

[0037] The 2D polygonal envelope of obstacles is obtained based on the obstacle clustering results;

[0038] The vertices of the obstacle are obtained based on the 2D polygonal envelope of the obstacle.

[0039] Furthermore, the lateral distance between each obstacle vertex and the predicted trajectory of the truck is calculated to determine whether the obstacle vertex is within the range of the predicted trajectory of the truck. If so, the obstacle vertex closest to the current position of the truck is selected as a candidate point for collision detection, and the speed of the obstacle is estimated.

[0040] The collision detection between the truck and the obstacle is performed based on the estimated obstacle speed, and the collision assessment is made by calculating the RSS distance.

[0041] Further, the point cloud data within the predicted driving area of ​​the truck is subjected to ground point cloud filtering, including:

[0042] Divide the predicted trajectory range of the truck into a two-dimensional grid of a preset size; project the point cloud within the predicted trajectory range of the truck onto the two-dimensional grid area;

[0043] 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;

[0044] Calculate the tilt and flatness of each grid region;

[0045] Based on the elevation, slope and flatness in the ground plane equation, the grid area closest to the truck is taken as the search starting point. The local concavity and convexity connectivity between the search starting point and the adjacent grid areas is calculated. The grid areas that satisfy the connectivity relationship are identified as ground plane areas.

[0046] 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.

[0047] Furthermore, the system also includes a chassis battery for powering the lidar, computing module, and vehicle chassis VCU.

[0048] Compared with the prior art, the present invention can achieve at least one of the following beneficial effects:

[0049] 1. This invention achieves real-time, high-precision detection of potential obstacles in the operating environment by integrating LiDAR and advanced computing modules into an unmanned low-speed freight vehicle. Compared with traditional sensors, LiDAR provides richer and more accurate point cloud data, enabling the system to more accurately estimate the vehicle's motion state and the location of obstacles;

[0050] 2. The computing module utilizes the high-speed data transmission capability of the vehicle-mounted Ethernet to quickly process point cloud data, enabling timely collision warnings and automatic emergency braking decisions, which significantly improves the system's response speed and overall safety.

[0051] 3. The ground point cloud filtering, obstacle clustering detection, RSS distance calculation, and Kalman filter with hysteresis extension based on point cloud data in this invention significantly improve the accuracy of obstacle detection and collision warning.

[0052] 4. By predicting the truck's trajectory and generating the trajectory expansion range, the system can more comprehensively assess potential collision risks. Furthermore, the methods for estimating obstacle speeds and performing collision checks further enhance the system's ability to identify and react to dynamic obstacles, thus providing more reliable collision prevention measures in unstructured environments. This improves the accuracy of obstacle detection and collision warning.

[0053] 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

[0054] 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.

[0055] Figure 1 This is a schematic diagram of an automatic emergency braking system for an unmanned low-speed freight vehicle based on lidar.

[0056] Figure 2 Flowchart for calculating the motion pose and velocity of a truck based on point cloud data to estimate its motion pose changes;

[0057] Figure 3 Flowchart for estimating the motion state of a truck using a hysteresis extended Kalman filter;

[0058] Figure 4 Flowchart for obtaining obstacle clustering results from ground point cloud segmentation and obstacle clustering detection;

[0059] Figure 5 This is a flowchart for obstacle geometric collision checking based on obstacle clustering results. Detailed Implementation

[0060] 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.

[0061] Truck delivery environments may be unstructured. Unstructured environments typically refer to areas without clear or fixed road structures, traffic rules, or standardized road markings. Road conditions are irregular and unpredictable, and may contain various obstacles. Examples include: embankments, excavations, retaining walls, equipment, pedestrians, and other traffic factors. Specifically, unstructured environments include the following characteristics:

[0062] (1) Lack of road markings: There are no clear lane lines, traffic signals or signs to guide trucks;

[0063] (2) Uneven road surface: There may be potholes, gravel, mud, excavation pits or other irregular terrain;

[0064] (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.

[0065] To address the technical challenges of accurate and effective collision warning for trucks in unstructured environments, this invention employs lidar as a detection sensor and proposes an automatic emergency braking system for unmanned low-speed freight vehicles based on lidar, comprising:

[0066] LiDAR, deployed on the front bumper or front of the roof of a truck, is used to collect environmental point cloud data in real time in the vehicle coordinate system during the operation of the truck.

[0067] The computing module receives environmental point cloud data collected by the LiDAR in real time via the vehicle Ethernet; estimates the truck's state based on the point cloud data to obtain the truck's motion pose and speed; performs obstacle detection and collision warning based on the truck's motion pose and speed; and sends control commands to the truck chassis VCU (Vehicle Control Unit) if a collision risk is determined.

[0068] The truck chassis VCU is used to dynamically control the truck chassis based on the control commands.

[0069] The system also includes a chassis battery for powering the lidar, computing module, and VCU (Vehicle Controller Unit). The power supply is provided in parallel.

[0070] For example, the speed of the unmanned low-speed truck is limited to below 20 km / h.

[0071] The lidar is installed on the front bumper or the front of the roof of the truck to ensure that it covers the important field of vision in front of the truck and that the forward orientation of the lidar is consistent with the direction of travel of the truck; the scanning plane of the lidar is parallel to the ground.

[0072] The positional deviation between the lidar and the truck's reference center (the truck's centroid 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.

[0073] 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.

[0074] 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 truck's reference center (such as the center of mass or geometric center).

[0075] During the operation, the unmanned truck first calibrates the lidar to obtain 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.

[0076] The lidar must be securely installed to withstand the bumps, vibrations and impacts that the truck may encounter during operation, ensuring the stability of the lidar sensor and the reliability of the measurement data.

[0077] The subsequent parts of this invention are all based on point cloud data in the vehicle coordinate system.

[0078] Ensure that the speed limit for trucks is within the preset speed threshold.

[0079] Point cloud data of the truck's driving environment is collected using lidar sensors. The data for each point in the point cloud includes the three-dimensional coordinates (x, y, z) of the point cloud and the point cloud acquisition timestamp.

[0080] The truck's state is estimated based on the point cloud data, and the truck's motion pose and speed are obtained.

[0081] The point cloud data is sequentially subjected to motion distortion correction and downsampling to obtain the processed point cloud;

[0082] Based on the processed point cloud, a local map within a limited historical range is constructed. A hash table is used to accelerate the ICP matching of each point in the currently collected point cloud with the local map of the corresponding point in the previous time step, resulting in ICP matching point pairs. The local map is continuously updated based on the ICP matching point pairs.

[0083] After performing ICP matching, the latest current position and speed of the truck are obtained based on the point cloud data;

[0084] Based on the latest position and velocity measurements of the truck, the truck's pose and velocity are obtained by estimating its motion pose.

[0085] like Figure 2 As shown, the details are as follows:

[0086] (1) Normalize the timestamps of the environmental laser point cloud collected by the lidar:

[0087] 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.

[0088] (2) Point cloud motion distortion correction:

[0089] The motion pose change estimation results of the truck within the previous scan interval (the time interval between two frames of point cloud data) are read. The motion pose change estimation results are obtained using laser odometry technology. The laser odometry obtains the truck'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.

[0090] Based on the assumption of uniform motion, the motion offset corresponding to each point is calculated to complete the motion distortion correction.

[0091] 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.

[0092] Motion status reading: Read the estimated results of the truck's motion pose changes within the previous scanning interval, provided by the laser odometer.

[0093] Uniform motion assumption: Between two consecutive scan intervals, it is assumed that the truck moves at a uniform speed. Based on this assumption, the pose of each point in the point cloud when it is measured is estimated.

[0094] Pose interpolation: Using the pose information of the truck at the current time and the previous time, the normalized timestamp of each point is interpolated to obtain the pose interpolation amount at the measurement time of that point.

[0095] 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; one-tenth is multiplied by the point cloud coordinates to obtain the corrected point cloud coordinates.

[0096] Multiply the pose interpolation by the point cloud coordinates to obtain the measurement point after motion distortion correction (corrected point cloud coordinates).

[0097] (3) Laser point cloud downsampling:

[0098] 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.

[0099] For example, one point is reserved in each voxel grid.

[0100] 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.

[0101] 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.

[0102] 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.

[0103] By downsampling the point cloud data points, the original high-density point cloud data is downsampled into a sparser point cloud.

[0104] 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.

[0105] 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.

[0106] 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.

[0107] (4) Perform ICP matching between laser point cloud and local map.

[0108] 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.

[0109] 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.

[0110] For example,

[0111] At time t1, the laser odometry outputs the pose at time t1 to construct a local map;

[0112] 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.

[0113] 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.

[0114] 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.

[0115] 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.

[0116] (5) Use a hysteresis extended Kalman filter to estimate the pose and speed of the truck.

[0117] A time-delay model is used to extend the Kalman filter with hysteresis. Based on the point cloud data, the motion pose and velocity of the truck are estimated using the Kalman filter with hysteresis extension. The estimated motion pose and velocity of the truck are then smoothly updated to obtain the smoothed updated motion pose and velocity of the truck. The process of estimating the motion pose and velocity of the truck using the Kalman filter with hysteresis extension includes:

[0118] Using the latest position and velocity measurements as the latest observations, the Kalman filter is input to perform observation hysteresis detection to 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;

[0119] 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 truck's running pose and velocity.

[0120] 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.

[0121] 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.

[0122] The laser odometer outputs three angles: yaw angle, roll angle, and pitch angle. The Kalman filter uses the yaw angle.

[0123] Hysteresis extended Kalman filter estimates the pose and velocity of a truck, such as Figure 3 As shown.

[0124] A hysteresis Kalman filter is used for pose and velocity prediction and updating, enabling high-frequency pose and velocity estimation and publishing.

[0125] Initial pose: The initial pose is when the truck is stationary and has not yet started moving.

[0126] Next, the Kalman filter is initialized.

[0127] Then, state model prediction is performed, as follows:

[0128] Based on the assumption of uniform motion, a 2D truck kinematic model is constructed to estimate the truck's motion pose. The truck's pose includes six state variables: the truck's x and y axis coordinates, heading angle, heading angle correction term, linear velocity, and angular velocity.

[0129] The Kalman filter is extended with a delay model, as shown in equation (1):

[0130]

[0131] Where A is the state prediction transfer matrix corresponding to the 2D truck kinematics model, I is the identity matrix, and Y is the vector representation of the set of six state variables, with subscripts indicating time.

[0132] The kinematic model of the Kalman filter includes six state variables: the truck's position coordinates, heading angle, heading angle correction term, linear velocity, and acceleration, as shown in (2) to (7):

[0133] x k+1 =x k v k CoS(θ k +b k Formula (2)

[0134] y k+1 =y k +v k sin(θ k +b k Formula (3)

[0135] θ k+1 =θ k +ω k Formula (4)

[0136] b k+1 =b k Formula (5)

[0137] v k+1 =v k Formula (6)

[0138] ωk+1 =ω k Formula (7)

[0139] Where, x k+1 and y k+1 x k and y k θ and y represent the x and y coordinates of the truck 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 Let x and y represent the linear velocity and angular velocity of the truck body at times k+1 and k, respectively.

[0140] 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.

[0141] 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.

[0142] Measurement updates are as follows:

[0143] The truck'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.

[0144] 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.

[0145] 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.

[0146] 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.

[0147] The filter has a fixed frequency, for example, 10Hz, and it is recommended to keep it consistent with the LiDAR point cloud measurement frequency.

[0148] 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.

[0149] 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.

[0150] Obtain the truck's operating pose and speed, the pose including the published position (x). y Coordinates and heading angle) and velocity (linear velocity and angular velocity).

[0151] Based on the truck's movement posture and speed, the truck performs obstacle detection and collision warning;

[0152] Based on the truck's motion pose and speed, a predicted trajectory for the truck's motion is generated;

[0153] At fixed time intervals, a truck motion prediction trajectory within a certain historical time range is generated. At each location point on the prediction trajectory, based on the truck's own size and shape, the location point is used as the truck's reference center point to generate the vertices of the truck's profile, i.e., the expansion profile points. Connecting all the expansion profile points forms the trajectory expansion range, which is the truck's predicted driving area.

[0154] like Figure 4 As shown. The truck's pose (xy coordinates and heading angle) and velocity (linear velocity and angular velocity) are used as inputs.

[0155] After obtaining the input truck pose and speed information, the truck trajectory is predicted.

[0156] The predicted trajectory of the truck is calculated using formulas (8) to (10):

[0157] x k+1 =x k +v k cos(θk Formula (8)

[0158] y k+1 =y k +v k sin(θ k Formula (9)

[0159] θ k+1 =θ k +ω k Formula (10)

[0160] Predicted trajectories within a certain time range are generated at fixed time intervals.

[0161] For example, the fixed time interval is 0.1s, and the certain time range is 2s.

[0162] Connecting the predicted trajectories of trucks to form a range is called the trajectory expansion range.

[0163] Then, at each location point on the predicted trajectory, based on the truck's size and shape, the vertices of the truck'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 truck may occupy at the predicted time.

[0164] Connect all the points of the expansion contour to form the trajectory expansion range, which is the predicted driving area of ​​the truck.

[0165] Point cloud extraction within the predicted trajectory range: Only point clouds within the predicted trajectory range are extracted, reducing the amount of data.

[0166] Based on the truck's motion posture and speed, the truck performs obstacle detection and collision warning, including:

[0167] Based on the truck's motion pose and speed, a predicted trajectory for the truck is generated, and ground point cloud filtering and obstacle clustering detection are performed on the point cloud data within the predicted driving area of ​​the truck to obtain the obstacle clustering results.

[0168] Based on the obstacle clustering results, an obstacle geometric collision check is performed to determine the nearest obstacle vertex to the truck within the truck's predicted trajectory range. The movement speed of the nearest obstacle vertex is estimated to calculate the RSS distance. Based on the RSS distance, it is determined whether a collision warning is triggered. If so, the calculation module sends a control command to the truck chassis VCU.

[0169] The point cloud data within the predicted driving area of ​​the truck is filtered by ground point cloud to obtain the remaining point cloud after filtering.

[0170] Euclidean clustering is used to perform obstacle clustering on the remaining point cloud after filtering;

[0171] The 2D polygonal envelope of obstacles is obtained based on the obstacle clustering results;

[0172] The vertices of the obstacle are obtained based on the 2D polygonal envelope of the obstacle.

[0173] Filtering of ground point clouds within the driving range of trucks.

[0174] Ground point cloud filtering is performed on the point cloud data within the predicted driving area of ​​the truck, including:

[0175] Divide the predicted trajectory range of the truck into a two-dimensional grid of a preset size; project the point cloud within the predicted trajectory range of the truck onto the two-dimensional grid area;

[0176] 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;

[0177] Calculate the tilt and flatness of each grid region;

[0178] Based on the elevation, slope and flatness in the ground plane equation, the grid area closest to the truck is taken as the search starting point. The local concavity and convexity connectivity between the search starting point and the adjacent grid areas is calculated. The grid areas that satisfy the connectivity relationship are identified as ground plane areas.

[0179] 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.

[0180] Ground point cloud filtering within the truck's driving range.

[0181] First, the predicted driving area of ​​the truck is divided into different two-dimensional grid areas of fixed size. 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 area.

[0182] 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.

[0183] For example, the preset height threshold is 0.3 meters.

[0184] The fitted ground plane equation is shown in formula (11):

[0185] Ax+By+Cz-d=0 formula (11)

[0186] Where d represents the ground elevation, and [A, B, C] represents the normal vector of the ground.

[0187] 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.

[0188] The inclination of the ground plane is calculated as shown in formula (12):

[0189]

[0190] Where θ is the inclination.

[0191] Flatness σ n The calculation is shown in formula (13):

[0192]

[0193] 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.

[0194] The local concavity and convexity connectivity of the ground plane is calculated as shown in formula (14):

[0195] |n i ·n j |>1-sin(||d ij ||ε) Formula (14)

[0196] 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.

[0197] 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.

[0198] 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.

[0199] Trucks are predicted to cluster obstacles within their driving range.

[0200] 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.

[0201] Calculate the 2D polygon envelope for all inflated contour points. Based on the polygon envelope, the predicted range of the truck's future travel 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 travel path range). Select point clouds located within the predicted path range for subsequent obstacle extraction.

[0202] The predicted driving space of the truck 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.

[0203] 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.

[0204] Then, iterative sampling is used to obtain point clouds for plane fitting and to calculate the plane equation.

[0205] 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.

[0206] Finally, points outside a certain threshold of ground height are selected as candidate obstacle point clouds.

[0207] 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.

[0208] The point cloud data obtained by clustering each obstacle is fitted into a 2D polygon envelope, which represents the spatial outline of the obstacle.

[0209] The obstacle clustering result is a 2D polygon envelope.

[0210] like Figure 5 As shown, this section performs obstacle geometric collision checks based on obstacle clustering results.

[0211] Identify the nearest obstacle vertex to the truck within the predicted trajectory range and estimate the obstacle's speed.

[0212] The 2D polygons based on obstacle clustering results are used to obtain the vertices of the 2D envelope of obstacles.

[0213] Calculate the lateral distance between each 2D envelope vertex and the predicted trajectory of the truck. This is the distance perpendicular to the truck's direction of travel.

[0214] Determine whether the vehicle is within a safe driving range, filter the 2D envelope vertices of the obstacles closest to the truck's current position, and confirm whether the vertices of the polygon envelope are within the truck's safe driving range. Count all obstacle vertices that may collide with the truck.

[0215] Estimate the running speed of the 2D envelope vertices of the nearest obstacle.

[0216] Select the obstacle vertex closest to the current truck position as the candidate point for collision detection, and estimate the obstacle's speed as shown in formulas (15) to (17):

[0217] d pos =norm(p cur -p pre ) Formula (15)

[0218]

[0219] v obj =v norm *cos(yaw diff )+v ego Formula (17)

[0220] 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 truck'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; vego v is the linear velocity of the truck. obj The velocity of the obstacle in the 2D plane.

[0221] 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.

[0222] Calculate the RSS distance.

[0223] Calculate the lateral distance between each obstacle vertex and the predicted trajectory of the truck, determine whether the obstacle vertex is within the range of the predicted trajectory of the truck, and if so, select the obstacle vertex closest to the current position of the truck as a candidate point for collision detection and estimate the speed of the obstacle.

[0224] The collision detection between the truck and the obstacle is performed based on the estimated obstacle speed, and the collision assessment is made by calculating the RSS distance.

[0225] RSS distance is the safe stopping distance required to avoid a truck colliding with an obstacle.

[0226] The RSS distance is calculated as shown in formula (18):

[0227]

[0228] Among them, t response It is the response time when the truck begins to decelerate, a ego_max With a obj_max These are the maximum decelerations of the truck and the obstacle, respectively, α sense This is the adjustment factor for the early warning sensitivity coefficient, and offset is the preset measurement bias value.

[0229] Early warning sensitivity coefficient adjustment factor α sense Used to adjust the warning sensitivity of the collision warning system. The minimum offset is 0.

[0230] If the calculated RSS distance is less than the preset distance threshold, indicating a risk of collision between the truck and the obstacle, the system will activate a collision warning and issue an emergency signal.

[0231] If the RSS distance is less than a preset distance threshold, it is determined that the truck is at risk of collision. This means that, based on the current movement of the truck and the obstacle, the truck may not be able to stop safely in front of the obstacle.

[0232] The computing module sends the control commands to the vehicle chassis VCU via the CAN bus;

[0233] The control commands include emergency braking commands, deceleration commands, and speed limit commands;

[0234] The control commands include emergency braking commands, deceleration commands, and speed limit commands;

[0235] If the RSS distance is greater than or equal to the preset speed limit threshold, a speed limit command is sent.

[0236] Otherwise, if the RSS distance is less than the preset speed reduction threshold, a speed reduction command is sent.

[0237] Otherwise, if the RSS distance is less than a preset emergency braking threshold, an emergency braking command is sent.

[0238] The subsequent trucks maintain a safe driving distance and adjust their speed based on the real-time truck status published by the trucks mentioned above;

[0239] Emergency braking command is used in emergency situations to instruct the truck to stop immediately;

[0240] The deceleration command is used to provide a deceleration instruction to the truck in the event of a malfunction.

[0241] Speed ​​limit commands are used to set or adjust the speed limit for trucks, so that the trucks are below the preset speed limit threshold.

[0242] The method of this invention is applicable to both autonomous driving and driving scenarios with drivers.

[0243] The preset distance threshold is set based on safety standards and truck performance.

[0244] In summary, the automatic emergency braking system for unmanned low-speed freight vehicles based on lidar according to embodiments of the present invention has the following beneficial effects:

[0245] 1. This invention achieves real-time, high-precision detection of potential obstacles in the operating environment by integrating LiDAR and advanced computing modules into an unmanned low-speed freight vehicle. Compared with traditional sensors, LiDAR provides richer and more accurate point cloud data, enabling the system to more accurately estimate the vehicle's motion state and the location of obstacles;

[0246] 2. The computing module utilizes the high-speed data transmission capability of the vehicle-mounted Ethernet to quickly process point cloud data, enabling timely collision warnings and automatic emergency braking decisions, which significantly improves the system's response speed and overall safety.

[0247] 3. The ground point cloud filtering, obstacle clustering detection, RSS distance calculation, and Kalman filter with hysteresis extension based on point cloud data in this invention significantly improve the accuracy of obstacle detection and collision warning.

[0248] 4. By predicting the truck's trajectory and generating the trajectory expansion range, the system can more comprehensively assess potential collision risks. Furthermore, the methods for estimating obstacle speeds and performing collision checks further enhance the system's ability to identify and react to dynamic obstacles, thus providing more reliable collision prevention measures in unstructured environments. This improves the accuracy of obstacle detection and collision warning.

[0249] 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. An automatic emergency braking system for unmanned low-speed freight vehicles based on lidar, characterized in that, include: LiDAR, deployed on the front bumper or front of the roof of a truck, is used to collect environmental point cloud data in real time in the vehicle coordinate system during the truck's operation. The computing module receives environmental point cloud data collected by the LiDAR in real time via the vehicle Ethernet; estimates the truck's state based on the point cloud data to obtain the truck's motion pose and speed; and performs obstacle detection and collision warning based on the truck's motion pose and speed. If a collision risk is determined, a control command is sent to the truck chassis VCU; The truck chassis VCU is used for dynamic control of the truck chassis based on the control commands. Based on the truck's motion posture and speed, the truck performs obstacle detection and collision warning, including: Based on the truck's motion pose and speed, a predicted trajectory for the truck is generated, and ground point cloud filtering and obstacle clustering detection are performed on the point cloud data within the predicted driving area of ​​the truck to obtain the obstacle clustering results. Based on the obstacle clustering results, an obstacle geometric collision check is performed to determine the nearest obstacle vertex to the truck within the truck's predicted trajectory range. The movement speed of the nearest obstacle vertex is estimated and the RSS distance is calculated. Based on the RSS distance, it is determined whether a collision warning is triggered. If so, the calculation module sends a control command to the truck chassis VCU. Select the obstacle vertex closest to the current truck 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 truck'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 speed of the truck, The velocity of the obstacle in the 2D plane; Based on the linear velocity of the truck and the speed of the obstacle on the plane, the RSS distance is calculated as follows: in, It is the response time when the truck begins to decelerate. and These are the maximum negative accelerations of the truck and the obstacle, respectively. As an adjustment factor for the early warning sensitivity coefficient, This is the preset measurement bias value; A time-delay model is used to extend the Kalman filter with hysteresis. Based on the point cloud data, the motion pose and velocity of the truck are estimated using the Kalman filter with hysteresis extension. The estimated motion pose and velocity of the truck are then smoothly updated to obtain the smoothed updated motion pose and velocity of the truck. The process of estimating the motion pose and velocity of the truck using the Kalman filter with hysteresis extension includes: Using the latest position and velocity measurements as the latest observations, the Kalman filter is input to perform observation hysteresis detection to 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; 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 position and velocity measurements are retained as the motion pose and velocity of the truck.

2. The system according to claim 1, characterized in that, The computing module sends the control commands to the truck chassis VCU via the CAN bus; The control commands include emergency braking commands, deceleration commands, and speed limit commands; If the RSS distance is greater than or equal to the preset speed limit threshold, a speed limit command is sent. Otherwise, if the RSS distance is less than the preset speed reduction threshold, a speed reduction command is sent. Otherwise, if the RSS distance is less than a preset emergency braking threshold, an emergency braking command is sent.

3. The system according to claim 1, characterized in that, Based on the assumption of uniform motion, a 2D truck kinematic model is constructed to estimate the truck's pose. The truck pose includes six state variables: the truck's x and y axis coordinates, heading angle, heading angle correction term, linear velocity, and acceleration. 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 truck kinematics model, I is the identity matrix, and Y is the vector representation of the six state variables.

4. The system according to claim 1, characterized in that, At fixed time intervals, predict the movement trajectory of a truck within a certain historical time range. At each location point of the predicted trajectory, based on the size and shape of the truck itself, the vertices of the truck profile, i.e., the expansion profile points, are generated with that location point as the truck reference center point. Connecting all the expansion profile points forms the trajectory expansion range, which is the predicted driving area of ​​the truck. The point cloud data within the predicted driving area of ​​the truck 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.

5. The system according to claim 4, characterized in that, Calculate the lateral distance between each obstacle vertex and the predicted trajectory of the truck, determine whether the obstacle vertex is within the range of the predicted trajectory of the truck, and if so, select the obstacle vertex closest to the current position of the truck as a candidate point for collision detection and estimate the speed of the obstacle. The collision detection between the truck and the obstacle is performed based on the estimated obstacle speed, and the collision assessment is made by calculating the RSS distance.

6. The system according to claim 1, characterized in that, Ground point cloud filtering is performed on the point cloud data within the predicted driving area of ​​the truck, including: Divide the predicted trajectory range of the truck into a two-dimensional grid of a preset size; project the point cloud within the predicted trajectory range of the truck 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; 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 truck is taken as the search starting point. The local concavity and convexity connectivity between the search starting point and the adjacent grid areas 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.

7. The system according to any one of claims 1-6, characterized in that, The system also includes a chassis battery for powering the lidar, computing module, and vehicle chassis VCU.

Images