Four-way shuttle robot obstacle avoidance and safety control system based on laser radar

Abstract

The present application relates to shuttle robot obstacle avoidance technical field, especially in four-way shuttle robot obstacle avoidance and safety control system based on laser radar, including: the system obtains original three-dimensional point cloud data, through comparing before and after time point cloud segmentation dynamic point cloud cluster and static background, using static background matching coarse registration matrix, and according to scene dynamic degree adjustment geometric feature weighting coefficient solution fine pose, effectively exclude dynamic noise interference, simultaneously, the system is based on state vector prediction barrier motion trajectory probability distribution, with entropy as trajectory uncertainty measure;Combined with trajectory probability distribution and robot discrete four-way kinematics model constructs time series kinematics threat field, through along the planned path to threat field is integrated, when the integral value exceeds the dynamic safety threshold based on uncertainty adjustment brake instruction is generated.The present application effectively solves the problem of positioning drift, frequent false stop machine and brake lag, improves the obstacle avoidance accuracy and running safety.

Description

Technical Field

[0001] This invention relates to the field of obstacle avoidance technology for shuttle robots, and more particularly to an obstacle avoidance and safety control system for four-way shuttle robots based on lidar. Background Technology

[0002] In automated storage and retrieval systems (AS / RS), four-way shuttles serve as the core material handling and storage equipment. They typically operate along a track network and exhibit discrete four-way kinematic characteristics, including straight-line travel and right-angle direction changes. To achieve real-time pose estimation and environmental perception within the storage environment, shuttles usually rely on 3D vision sensors (such as LiDAR) to collect point cloud data, detect obstacles ahead of the track, and execute obstacle avoidance strategies.

[0003] However, in industrial warehousing scenarios, the operating environment is highly dynamic, containing not only static structures like fixed shelving but also dynamic interference objects such as mobile equipment, workers, and goods being handled. In such scenarios, if the original point cloud containing dynamic objects is directly used for registration and localization, the moving objects will act as noise interference in feature matching, disrupting the stability of the point cloud correspondence and leading to decreased shuttle registration accuracy and localization drift. Furthermore, in dense, dynamic warehousing environments, the location, direction of movement, and future trajectory of moving obstacles exhibit significant randomness and uncertainty. Existing shuttle obstacle avoidance strategies often rely on static distance thresholds at the current moment for braking decisions. Lacking prediction and assessment of the future spatiotemporal evolution of dynamic obstacles, the system struggles to accurately measure the true threat posed by obstacles to the shuttle's planned path within future time windows. This often results in overly conservative shuttle obstacle avoidance control, leading to frequent and unnecessary shutdowns, severely reducing the logistics efficiency of the automated warehouse, or causing collisions due to delayed response.

[0004] To mitigate collision issues with warehouse robots, Chinese patent document CN115140481B discloses a dynamic obstacle avoidance method and device for a four-way shuttle. This patent utilizes onboard sensors on the four-way shuttle to detect obstacles in the track ahead in real time and sets a certain static safety distance threshold. When the detection module determines that the distance between the four-way shuttle and a moving obstacle is less than the preset static threshold, the system directly sends a command to the shuttle's underlying control mechanism to drive the motor to decelerate or stop the vehicle in an emergency, thereby achieving collision protection within the logistics channel.

[0005] While the aforementioned patent documents provide a basic obstacle avoidance method for four-way shuttles, they fail to segment and extract dynamic point cloud clusters from static background point clouds during sensor data acquisition. This makes them susceptible to interference from moving noise in highly dynamic warehouse scenarios, leading to positioning drift. Furthermore, their reliance on static distance thresholds for braking judgment fails to predict the probability distribution and uncertainty of dynamic obstacle trajectories, and they do not integrate this trajectory uncertainty with the four-way shuttle's proprietary kinematic model to construct a temporal kinematic threat field. Therefore, when faced with complex dynamic disturbances in warehouses, the inability to dynamically adjust safety thresholds still results in overly conservative obstacle avoidance control, potentially causing warehouse logistics disruptions, or collisions due to delayed response. This makes it difficult to achieve more accurate, timely, and reliable obstacle avoidance and braking control. Summary of the Invention

[0006] To address the technical problems of positioning drift caused by interference from existing dynamic point clouds and the lack of obstacle trajectory prediction, which leads to frequent false stops or delayed responses in obstacle avoidance, this invention provides an obstacle avoidance and safety control system for a four-way shuttle robot based on lidar, comprising: a calculation module for acquiring the original three-dimensional point cloud data and relative pose estimation data at the current moment, combining the relative pose estimation data with the original three-dimensional point cloud data at the previous moment to segment dynamic point cloud clusters and static background point clouds, and extracting the state vector of the dynamic point cloud cluster containing the centroid coordinates and translational velocity vector;

[0007] The solution module is used to obtain a coarse registration transformation matrix by matching the static background point cloud with the global environment map point cloud, calculate the scene dynamics based on the magnitude of the translation velocity vector and the number of dynamic point cloud clusters, adjust the weighting coefficients of Euclidean distance, normal vector angle and principal curvature difference based on the scene dynamics, calculate the weighted sum of Euclidean distance, normal vector angle and principal curvature difference between the source point and the target point using the adjusted weighting coefficients to find corresponding point pairs, and solve the fine transformation matrix based on the corresponding point pairs to obtain the pose. The generation module is used to predict the probability distribution of motion trajectory based on the state vector, sample the probability distribution of motion trajectory on a preset spatial grid to obtain a spatial occupancy probability array, calculate the information entropy of the spatial occupancy probability array as a trajectory uncertainty measure, combine the probability distribution of motion trajectory with a discrete four-way kinematics model to construct a temporal kinematic threat field and integrate it along the planned path, and generate a braking command when the integral value exceeds the dynamic safety threshold adjusted based on the trajectory uncertainty measure.

[0008] This invention effectively eliminates the disruption of feature matching caused by dynamic objects in dense storage by stripping dynamic point cloud clusters, thus improving the stability of pose calculation in highly dynamic environments. During point cloud registration, the system introduces scene dynamics to dynamically adjust the weighting coefficient of geometric feature differences, enhancing the accuracy of corresponding point pair selection. The system uses state vectors to predict the probability distribution of future trajectories, quantifies the uncertainty of motion trajectories through information entropy, and deeply integrates the trajectory evolution trend with the discrete four-way kinematics model of the four-way shuttle robot to construct a temporal kinematic threat field, achieving accurate assessment of the spatiotemporal risks of the planned path. When the uncertainty of obstacle trajectories increases, the system adaptively lowers the dynamic safety threshold, enabling the robot to trigger braking in advance when facing unreliable trajectory predictions, avoiding collisions caused by response lag. Simultaneously, the system reduces the rate of unnecessary downtime through comprehensive evaluation of path integrals, ensuring both operational safety and operational efficiency.

[0009] Preferably, the calculation module converts the original three-dimensional point cloud data of the previous time and the current time into three-dimensional voxel grids respectively, compares the difference in average coordinates of point clouds within the same three-dimensional voxel grid to retain moving points, and uses a density-based spatial clustering algorithm to merge moving points whose distance is less than the clustering search radius, and discards groups whose number of points is less than the minimum number of core points, in order to obtain the dynamic point cloud cluster.

[0010] This invention can quickly and accurately screen out moving points by voxel grid downsampling and average coordinate difference comparison. Then, it uses a density-based spatial clustering algorithm to remove isolated noise points, ensuring the connectivity and independence of the segmented dynamic point cloud clusters and improving the noise resistance of environmental perception.

[0011] Preferably, the calculation module calculates the average value of the coordinates of each point in the dynamic point cloud cluster to obtain the centroid coordinates. Based on the nearest neighbor principle of the centroid coordinates, when the Euclidean distance between the centroid coordinates at the current time and the centroid coordinates at the previous time is less than a preset maximum association distance threshold, a data matching relationship between the dynamic point cloud clusters at the previous and next times is established. The centroid displacement vector is obtained by subtracting the associated centroid coordinates at the previous time from the centroid coordinates at the current time. The translational velocity vector is obtained by dividing the centroid displacement vector by the sampling time interval.

[0012] This invention uses the nearest neighbor distance principle to achieve accurate tracking and association of dynamic obstacles between consecutive frames. Based on basic coordinate geometry operations, it can quickly calculate the translational velocity vector with physical meaning, reducing the overall computational complexity and providing reliable state input data for subsequent trajectory prediction.

[0013] Preferably, the solution module performs normalization processing on the Euclidean distance, the angle between normal vectors, and the principal curvature difference, and then calculates a weighted sum. The calculation factor of the scene dynamics is formed by multiplying the sum of the translational velocity vector magnitudes of each dynamic point cloud cluster by a first empirical coefficient, and adding it to the number of dynamic point cloud clusters by a second empirical coefficient. The calculation factor is then mapped to the interval between 0 and 1 to obtain the scene dynamics.

[0014] This invention unifies geometric features from multiple dimensions into a dimensionless range and comprehensively assesses the severity of scene changes by combining obstacle speed and quantity. It can adaptively adjust feature matching strategies, prevent the failure of simple distance metrics, and ensure the stability of the registration algorithm in complex environments.

[0015] Preferably, the solution module calculates the centroid coordinates of the source point and the target point in each corresponding point pair to construct a cross-correlation matrix, decomposes the cross-correlation matrix using singular value decomposition, calculates the optimal rotation matrix by taking the product of the decomposed orthogonal matrices, calculates the translation column vector by combining the centroid coordinates, and synthesizes the optimal rotation matrix and the translation column vector to obtain the fine transformation matrix.

[0016] This invention constructs a rigorous matrix operation model by using centroid decentering and singular value decomposition, which can minimize the mean square error between corresponding point sets and calculate a high-precision rigid body space transformation mapping relationship, thereby improving the relative pose estimation accuracy of the robot.

[0017] Preferably, the generation module establishes a state transition equation using a constant velocity motion model as the basic model, calculates the mean predicted coordinates for each discrete prediction time based on the state vector and the state transition equation, calculates the spatial covariance matrix for each discrete prediction time using a preset process noise covariance matrix, solves for the mean predicted coordinates and the spatial covariance matrix using a multivariate Gaussian function, and outputs a continuous spatial position probability density distribution as the motion trajectory probability distribution.

[0018] This invention incorporates process noise and measurement error based on the Kalman recursion principle. It uses a multivariate Gaussian distribution to mathematically model and extrapolate the uncertainty of the future spatial position of obstacles. The output continuous probability density truly reflects the spatiotemporal randomness of the object's motion.

[0019] Preferably, the generation module parses the predicted arrival time of the four-way shuttle robot to each grid cell from the discrete four-way kinematics model, extracts the probability distribution of the motion trajectory under the predicted arrival time, multiplies the extracted probability distribution of the motion trajectory by an intensity reduction factor that decreases exponentially with the distance between the grid cell and the mean predicted coordinate, and records it to a three-dimensional floating-point array to construct the temporal kinematic threat field.

[0020] This invention links the probability distribution with the arrival time of the shuttle robot and uses a negative exponential decay model to simulate the decreasing trend of collision risk with distance. It constructs a three-dimensional threat matrix that includes spatiotemporal two-dimensional characteristics, enabling the system to clearly quantify the future risk cost of any grid.

[0021] Preferably, the generation module performs numerical integration weighted summation on the sequence interpolation nodes and corresponding obstacle threat assignment data on the planned path according to the Simpson quadrature criterion to obtain the integral value, multiplies the trajectory uncertainty measure by a preset dimension conversion coefficient to align the dimensions and obtain the safety margin compensation value, and subtracts the safety margin compensation value from the pre-calibrated basic safety threshold to obtain the dynamic safety threshold.

[0022] This invention utilizes the composite Simpson quadrature formula to perform high-precision numerical integration on discrete nodes, accurately summarizing the comprehensive collision risk of the entire trajectory. By converting the information entropy into a compensation basis for the safety threshold through dimensional transformation, it achieves seamless integration of risk assessment and braking control.

[0023] Preferably, it further includes a low-level programmable logic controller (PLC) with a communication connection. The low-level PLC is used to receive the braking command issued by the generation module and convert the target deceleration contained in the braking command into a corresponding braking torque to control the motor to perform emergency braking.

[0024] By using a programmable logic controller to accurately convert logic instructions into the physical braking torque of the motor, a rapid response and stable execution of emergency braking actions are ensured, forming a complete hardware protection mechanism.

[0025] Preferably, the system further includes a memory and a processor communicatively connected to the memory. The memory stores computer execution instructions, and the processor instantiates the operating logic of the computing module, the solving module, and the generating module when executing the computer execution instructions.

[0026] It provides reliable hardware computing power and data storage support for the issuance of various core algorithms and control commands of the system, ensuring the real-time response requirements of high-frequency point cloud processing and complex matrix numerical operations.

[0027] The technical solution of the present invention has the following beneficial technical effects: This invention effectively reduces redundant data processing by extracting dynamic point cloud clusters of 3D point clouds at adjacent time points and constructing incremental encoded data. In the point cloud registration stage, the weighting coefficients of Euclidean distance, normal vector angle and principal curvature difference are adaptively adjusted based on scene dynamics to reduce moving noise interference and ensure the pose calculation and global mapping accuracy of the four-way shuttle robot in a highly dynamic warehouse environment.

[0028] Furthermore, information entropy is introduced as a quantitative measure of the uncertainty in predicting dynamic obstacle trajectories, and a spatiotemporal threat field is constructed by combining it with the discrete kinematics model specific to the four-way shuttle robot. By performing high-precision numerical integration along the planned path and using trajectory uncertainty to adaptively adjust the dynamic safety threshold, the response lag and frequent false triggering of traditional fixed-distance obstacle avoidance schemes are avoided, and more accurate and reliable collision avoidance safety control is achieved. Attached Figure Description

[0029] Figure 1 This is a graph showing the exponential decay of the threat field intensity with distance. Figure 2 This is a schematic diagram of the dynamic safety threshold change curve based on trajectory information entropy; Figure 3 This is a bar chart illustrating the performance comparison of different obstacle avoidance schemes. Detailed Implementation

[0030] The technical solutions of the present invention will be clearly and completely described below with reference to the accompanying drawings of the embodiments of the present invention. Obviously, the described embodiments are some embodiments of the present invention, but not all embodiments.

[0031] Reference Figure 1 The obstacle avoidance and safety control system for a four-way shuttle robot based on lidar includes a calculation module, a solution module, and a generation module, which are described in detail below.

[0032] The calculation module is used to acquire the original 3D point cloud data and relative pose estimation data of the four-way shuttle robot at the current moment; the calculation module combines the relative pose estimation data with the original 3D point cloud data of the previous moment and the original 3D point cloud data of the current moment to segment dynamic point cloud clusters and static background point clouds; the calculation module calculates the state vector of the dynamic point cloud cluster, which includes the centroid coordinates and translation velocity vector, and the state vector and the static background point cloud constitute incremental encoded data.

[0033] The computation module acquires raw 3D point cloud data in real time using a lidar sensor. It then uses voxel mesh downsampling filtering to remove redundant noise from the raw 3D point cloud data. Simultaneously, the inertial measurement unit (IMU), in conjunction with a wheeled odometry system, employs an extended Kalman filter (EDF) algorithm for state estimation, obtaining the relative pose estimation data of the four-way shuttle robot at the current moment. Specifically, the system sets its state vector to include the shuttle robot's 3D position, velocity, and attitude angles. During the recursive process, the system uses the linear velocity and angular velocity data output from the wheeled odometry system as input to the kinematic model for state prediction, and uses the acceleration and angular velocity data acquired in real time by the IMU as observations for Kalman updates, thereby continuously outputting high-precision relative pose estimation data.

[0034] Based on the relative pose estimation data, a homogeneous transformation matrix containing quaternions and three-dimensional translation vectors is constructed. The homogeneous transformation matrix is ​​used to perform rigid body transformation compensation on the original three-dimensional point cloud data of the previous moment, so that the original three-dimensional point cloud data of the previous moment is aligned with the coordinate system of the current moment.

[0035] Furthermore, the calculation module converts the compensated original 3D point cloud data from the previous moment and the original 3D point cloud data from the current moment into 3D voxel meshes. The calculation module calculates the arithmetic mean of the coordinates of all points within each 3D voxel mesh. The size of the 3D voxel mesh is set to 0.05m × 0.05m × 0.05m. The calculation module compares the average coordinate difference of 3D voxel meshes at the same spatial absolute position at two moments. When the average coordinate difference of 3D voxel meshes at the same spatial absolute position at two moments is greater than the distance determination constant, the calculation module marks the point set within the 3D voxel mesh as a moving point. The distance determination constant is obtained through a system-preset spatial distance distinction threshold value. In this embodiment, its preset value is 0.03m.

[0036] After acquiring the moving points, the calculation module uses a density-based spatial clustering algorithm to gradually aggregate and merge moving points whose adjacent distances are less than the clustering search radius to form multiple candidate clusters. The density-based spatial clustering algorithm automatically divides the point cloud into clusters according to the density of the point cloud in space. The clustering search radius is obtained through the parameter configuration of the density-based spatial clustering algorithm; in this embodiment, its preset value is 0.15m. The calculation module discards candidate clusters whose number of moving points is less than the minimum inclusion constant of the core points. The minimum inclusion constant of the core points is also obtained through the parameter configuration of the density-based spatial clustering algorithm; in this embodiment, its preset value is 20. The calculation module treats the candidate clusters that are not discarded as independent and connected dynamic point cloud clusters. In the data structure of the original 3D point cloud data at the current moment, the calculation module uses a Boolean index to mask the point sets already assigned to dynamic point cloud clusters. The calculation module extracts all remaining points in the original 3D point cloud data at the current moment that are not assigned to dynamic point cloud clusters and outputs them as a static background point cloud.

[0037] Furthermore, for each dynamic point cloud cluster generated by clustering at the current moment, the calculation module traverses all data points within the dynamic point cloud cluster and calculates the arithmetic mean of the X-axis, Y-axis, and Z-axis coordinates of all data points to form the centroid coordinates of each dynamic point cloud cluster at the current moment. The calculation module extracts the centroid coordinate set of all dynamic point cloud clusters stored at the previous moment and transforms the centroid coordinate set of the previous moment to the coordinate system of the current moment using relative pose estimation data. Based on the nearest neighbor principle of centroid coordinates, the calculation module establishes a data matching association relationship between the current dynamic point cloud cluster and the previous dynamic point cloud cluster. If the Euclidean distance between the current centroid coordinate and all previous centroid coordinates in the previous centroid coordinate set is greater than the maximum correlation distance threshold, then the dynamic point cloud cluster corresponding to the current centroid coordinate is determined to be a newly created obstacle, and the initial translational velocity vector of the newly created obstacle is assigned to 0. The maximum correlation distance threshold is obtained by the maximum allowable offset space distance for target tracking set by the system. In this embodiment, its value is preset to 0.5m.

[0038] For a pair of dynamic point cloud clusters that have successfully established a data matching relationship, the calculation module obtains the centroid displacement vector by subtracting the centroid coordinates of the previously associated dynamic point cloud cluster from the centroid coordinates of the current dynamic point cloud cluster at the same time. The calculation module then calculates the translational velocity vector by dividing the centroid displacement vector by the sampling time interval from the previous time to the current time. The sampling time interval represents the time difference between the previous time and the current time, and is obtained by acquiring the rotation period of the lidar; in this embodiment, its value is preset to 0.1s. The calculation module encapsulates the translational velocity vector and the current centroid coordinates together into a state vector containing six data dimensions, and stores the state vector in the incremental encoded data sequence of the current time.

[0039] The computation module combines the state vector containing obstacle motion information with the static background point cloud representing the geometric features of the environment into an in-memory data structure to complete the construction of incremental encoded data.

[0040] The solution module is used to obtain a coarse registration transformation matrix by matching the static background point cloud with the target point cloud. Based on the coarse registration transformation matrix, the solution module finds corresponding point pairs by calculating the minimum weighted sum of the geometric feature differences between the source point and the target point. The weighting coefficients are adjusted according to the scene dynamics based on the speed and quantity of the dynamic point cloud clusters. The solution module calculates the fine transformation matrix based on the corresponding point pairs to obtain the pose.

[0041] In one embodiment, the solving module uses the static background point cloud as the source point cloud and the pre-constructed global environment map point cloud as the target point cloud. The solving module uses a fast point feature histogram algorithm to extract key point feature descriptors from the source point cloud and the target point cloud. The solving module uses a sample consistency initial registration algorithm to perform correspondence matching in the feature space and solves to obtain a coarse registration transformation matrix that provides coarse alignment for translation and rotation.

[0042] Based on the initial position space mapping provided by the coarse registration transformation matrix, the solution module uses a tree structure search algorithm to find local nearest neighbor points in the target point cloud as candidate target points. The search domain is set as a spherical space with a radius of 0.3m centered on the sampling point. The solution module calculates the Euclidean distance between the source point and the candidate target point, the angle between the normal vectors obtained by the dot product inverse cosine of the local surface normal vectors at the two points, and the absolute value of the difference in principal curvature between the two points estimated based on the eigenvalues ​​of the covariance matrix.

[0043] The solution module uses the minimax normalization formula to map the absolute values ​​of Euclidean distance, the angle between normal vectors, and the difference in principal curvature to a dimensionless interval of 0 to 1. The solution module sets the objective function as a weighted sum of normalized feature quantities. The solution module selects the candidate target point that minimizes the weighted sum as the corresponding point pair of the source point. The weighting coefficients must satisfy the constraint that the sum of the Euclidean distance weighting coefficient, the angle between normal vectors weighting coefficient, and the difference in principal curvature weighting coefficient is equal to 1.

[0044] The solution module calculates the total number of dynamic point cloud clusters and the translational velocity magnitude of each cluster. The solution module then uses the following formula... Calculate the scene dynamic factor.

[0045] in, Indicates the scene dynamics factor; Indicates the first empirical coefficient; This represents the translational velocity magnitude of each dynamic point cloud cluster, which is obtained by calculating the magnitude of the translational velocity vector, and its unit is preset to m / s; The first and second empirical coefficients are obtained through parameter scanning calibration experiments conducted by the system in a highly dynamic warehousing environment. The calibration conditions are as follows: multiple dynamic obstacle vehicles with irregular movement patterns are arranged in the test area; the lidar sampling rate is 10Hz; the scene dynamic factor, after normalization, falls within the 0-1 range without saturation truncation; and the root mean square error (RMSE) of the pose calculated by the fine transformation matrix relative to the reference true value is used as the evaluation index. A gridded parameter scan is performed on the first and second empirical coefficients, and the set of values ​​that minimizes the RMS error is selected as the calibration result. Under the calibration conditions of this embodiment, the value range of the first empirical coefficient is 0.1 to 0.4, and the value range of the second empirical coefficient is 0.05 to 0.2. In this embodiment, the first empirical coefficient is preferably 0.2, and the second empirical coefficient is preferably 0.1. This represents the total number of dynamic point cloud clusters, which is obtained by counting the number of dynamic point cloud clusters segmented at the current moment.

[0046] The solution module truncates or normalizes the calculated scene dynamics factor to the range of 0 to 1 to obtain the scene dynamics. When the scene dynamics is greater than 0.6, the solution module lowers the Euclidean distance weighting coefficient from the default value of 0.6 to 0.3. At the same time, the solution module increases the weighting coefficients of the normal vector angle and the principal curvature difference from the default value of 0.2 to 0.35 respectively. Lowering the Euclidean distance weighting coefficient can reduce the distance misleading caused by moving noise, while increasing other parameters can increase the weight of local topological features in the selection of corresponding point pairs.

[0047] After finding corresponding point pairs, the calculation module removes erroneous corresponding point pairs whose spatial distance exceeds a set distance threshold. The calculation module sets an error loss function to minimize the sum of squared distances between corresponding point pairs, and the error loss function is expressed as follows: .

[0048] in, The mean squared error value is obtained through iterative calculation using the error loss function. This represents the total number of corresponding point pairs, obtained by counting the number of valid corresponding point pairs found. Indicates the coordinates of the source point. Indicates the coordinates of the target point. Represents the optimal rotation matrix in three-dimensional space. Represents a translation column vector in three-dimensional space.

[0049] The solution module calculates the centroid coordinates of the source and target point sets respectively. For each corresponding point pair, the module performs a decentroiding operation by subtracting the centroid of both the source and target point sets, obtaining the center-aligned source and target point coordinates. The module then calculates a 3×3 cross-correlation matrix by multiplying the center-aligned source and target point coordinates. The cross-correlation matrix calculation formula is as follows: .

[0050] in, The cross-correlation matrix is ​​obtained by summing decentralized coordinates. Indicates the coordinates of the source point after center alignment; This represents the coordinates of the target point after center alignment.

[0051] The solution module uses singular value decomposition to decompose the cross-correlation matrix into... .

[0052] in, This represents a left singular orthogonal matrix with a dimension of 3×3; This represents a right singular orthogonal matrix with dimensions 3×3. The solution module calculates the optimal rotation matrix by taking the product of the decomposed orthogonal matrices, using the following formula: After calculation, the solution module checks the determinant of the optimal rotation matrix. If the determinant is -1, it indicates a reflection transformation has occurred. In this case, the solution module will convert the right singular orthogonal matrix... The last column is multiplied by -1 for pure rotational property correction.

[0053] Substitute the relation into the solution module By combining the centroid coordinates of the source and target point sets and the optimal rotation matrix, a 3×1 translation column vector is calculated. The translation column vector in three-dimensional space is obtained through matrix operations in the solution module; The centroid coordinates of the target point set are obtained by calculating the arithmetic mean of the coordinates within the target point set. The optimal rotation matrix in three-dimensional space is obtained through the singular value decomposition of the cross-correlation matrix; The coordinates of the centroid of the source point set are obtained by calculating the arithmetic mean of the coordinates within the source point set. The solution module integrates the obtained optimal rotation matrix and translation column vectors into a homogeneous pose transformation matrix of dimension 4×4. The homogeneous pose transformation matrix directly represents the pose of the four-way shuttle robot relative to the global coordinate system at the current moment. The solution module outputs the homogeneous pose transformation matrix as the fine transformation matrix.

[0054] Furthermore, the generation module is used to predict the probability distribution of the motion trajectory of dynamic obstacles within a future time window based on the state vector. The generation module uses the entropy value representing the probability distribution of the motion trajectory as a measure of trajectory uncertainty. The generation module combines the probability distribution of the motion trajectory with the discrete four-way kinematics model of the four-way shuttle robot to construct a temporal kinematic threat field. The generation module integrates the temporal kinematic threat field along the current planned path of the four-way shuttle robot. When the path integral value exceeds the dynamic safety threshold adjusted based on the trajectory uncertainty measure, the generation module generates a braking command.

[0055] The generation module uses the centroid coordinates and translational velocity vector from the extracted state vector as the observation inputs to the state transition model, establishing an extended Kalman prediction framework based on a constant velocity motion model and the Gaussian noise perturbation assumption. The generation module recursively calculates the mean three-dimensional position and spatial covariance matrix of the dynamic obstacle over multiple discrete time steps within a set future time window using the extended Kalman prediction framework. Furthermore, the generation module outputs a continuous spatial position probability density distribution based on a three-dimensional multivariate Gaussian probability density distribution function, thus obtaining the trajectory probability distribution of the dynamic obstacle. The extended Kalman prediction framework is an estimation algorithm based on Kalman filtering, used for state prediction and uncertainty recursion in nonlinear motion systems.

[0056] The generation module uses a constant velocity motion model as the basic model to establish the state transition equations for the obstacle. The generation module sets a 6×1 column vector containing position and velocity as the state variables, and uses the translational velocity vector and centroid coordinates extracted from the incrementally encoded data as the initial state variables at time 0. The generation module sets the prediction time step, preset to 0.2s, and the total prediction future time window length, preset to 2s. The generation module divides the total prediction future time window length into 10 discrete prediction steps.

[0057] The generation module uses a 6×6 state transition matrix to perform matrix multiplication and recursively calculate the mean predicted coordinates of the dynamic obstacle at each discrete time step in the future. The generation module sets the state covariance matrix at time zero, which is constructed based on the LiDAR centroid measurement error and velocity estimation error, or it can be set as a preset 6×6 diagonal matrix. In the state covariance matrix at time zero, the position variance is preset to 0.0025m. 2 The velocity variance is preset to 0.01. At each time step of the mean prediction coordinates, the generation module uses a preset process noise covariance matrix to represent the unmodeled motion acceleration disturbance, and the position variance on the diagonal of the process noise covariance matrix is ​​preset to 0.0025m. 2 The velocity variance on the diagonal of the process noise covariance matrix is ​​preset to 0.01.

[0058] The generation module uses matrix equations Synchronous recursive accumulation.

[0059] in, The state covariance matrix at the current moment is obtained recursively through the extended Kalman prediction framework; The state transition matrix is ​​represented and obtained through a constant velocity model. The state covariance matrix of the previous time step is obtained by storing historical time-series data. The process noise covariance matrix is ​​obtained through a system-preset fixed variance value. The generation module extracts a 3×3 submatrix from the top left corner of the state covariance matrix as the spatial covariance matrix of the position at each discrete prediction time. The generation module uses the obtained mean prediction coordinates as the Gaussian center and substitutes the spatial covariance matrix as a distribution morphology parameter into a three-dimensional multivariate Gaussian probability density distribution function to output the continuous position probability density value of the dynamic obstacle in space as uncertainty diffuses over time.

[0060] The generation module samples the probability density distribution of continuous spatial positions at each discrete prediction time into a three-dimensional grid point matrix, and normalizes the probability density values ​​of each grid cell in the three-dimensional grid point matrix to obtain a spatial occupancy probability array. The generation module uses the Shannon entropy calculation formula to calculate the information entropy of the spatial occupancy probability array, and uses the maximum information entropy value obtained at each time step as a trajectory uncertainty metric parameter.

[0061] The generation module uses the geometric center of the four-way shuttle robot chassis as the origin and uniformly divides the local three-dimensional space around the robot's operating area into a cubic mesh matrix. The preset dimensions of the local three-dimensional space are 10m forward, 10m laterally, and 2m high. The preset dimensions of the three-dimensional cubic mesh are 0.1m × 0.1m × 0.1m. The generation module inputs the discrete four-way kinematics model of the four-way shuttle robot and integrates the kinematic constraint parameters of the four-way shuttle robot. The kinematic constraint parameters include the maximum longitudinal acceleration, the maximum cruising speed, and the lifting delay of the mechanical reversing mechanism. The maximum longitudinal acceleration is preset to 1.5m / s². 2 The maximum cruising speed is preset to 2 m / s, and the lifting delay of the mechanical reversing mechanism is preset to 0.5 s. The generation module performs forward analytical deduction to predict the arrival time required for the four-way shuttle robot to reach each three-dimensional mesh cell within the coverage area under the current given speed and acceleration control commands.

[0062] The generation module iterates through each 3D mesh cell within the coverage area. For a specific 3D mesh cell in a specific coordinate system, the generation module extracts the predicted arrival time corresponding to the four-way shuttle robot covering it. Using the predicted arrival time as an index, the generation module queries the preceding output data to obtain the probability density distribution of the dynamic obstacle's position at the specific 3D mesh cell coordinates at that predicted time. The generation module sets an intensity reduction coefficient that exhibits a negative exponential decay relationship as the Euclidean distance between the center of the 3D mesh cell and the predicted mean center of the dynamic obstacle increases. The formula for the intensity reduction coefficient is:

[0063] in, This represents the strength conversion factor. The distance attenuation constant represents the Euclidean distance between the center of the grid cell and the center of the predicted mean of the dynamic obstacle. It is obtained by calculating the difference of three-dimensional spatial coordinates. The distance attenuation constant controls the speed at which the threat decreases as the distance increases. Its value is positively correlated with the robot's safe braking space margin and is a conventional engineering design choice based on safety control in this field. In this embodiment, the distance attenuation constant is preset to 0.5. For shuttles of different specifications, the distance attenuation constant can be adjusted according to the braking capacity. It is preferably half of the safe stopping distance required for the robot to perform emergency braking at its maximum cruising speed. The safe stopping distance is determined by the robot's maximum cruising speed, maximum braking deceleration, and system response delay. That is, the safe stopping distance is equal to the product of the maximum cruising speed and the system response delay, plus the quotient obtained by dividing the square of the maximum cruising speed by twice the maximum braking deceleration. In this embodiment, the maximum cruising speed is 2 m / s, the target deceleration is -2 m / s², and the lidar sampling time interval is 0.1 s. Based on this, the calculated safe stopping distance is 1 m, so the distance attenuation constant is taken as 0.5. For a shuttle with a maximum cruising speed of 4 m / s, the safe stopping distance calculated according to the same formula is increased accordingly, and the distance attenuation constant is also increased accordingly, so that the spatial influence radius of the threat field matches the braking capacity of the shuttle. The generation module multiplies the intensity conversion coefficient by the extracted position probability density distribution result to generate a composite hazard weight, and the generation module records it into a three-dimensional floating-point array according to time and spatial coordinates to construct a temporal kinematic threat field intensity distribution table. The exponential decay curve of the threat field intensity with distance is shown in the figure. Figure 1 As shown.

[0064] The generation module divides the current planned path given by the system into an even number of line segments. The local planned path length is preset to 5m. The generation module divides the local planned path into 20 even-numbered line segments at equal intervals, thereby generating 21 discrete interpolation nodes with three-dimensional coordinate information. The generation module obtains the path start point, the connection point of each adjacent line segment, and the path end point as discrete interpolation nodes. The generation module combines the velocity profile curve of the four-way shuttle robot to calculate the corresponding time when the geometric center of the four-way shuttle robot moves to each discrete interpolation node. The generation module uses the three-dimensional spatial coordinates and corresponding time of each discrete interpolation node as a composite index. In the constructed temporal kinematic threat field strength distribution table, the obstacle threat assignment data at each discrete interpolation node is read through the trilinear interpolation algorithm.

[0065] The generation module performs numerical integral weighted summation calculation on the discrete interpolation nodes and corresponding obstacle threat assignment data of the sequence according to the complex Simpson quadrature criterion, and outputs the path integral value representing the comprehensive collision risk of the current trajectory. The weighted algorithm of the complex Simpson quadrature is expressed as: divide the line segment step length by 3, multiply by the sum of the threat values ​​of the starting point, the sum of the threat values ​​of odd-numbered nodes, multiply by 4, the sum of the threat values ​​of even-numbered nodes, multiply by 2, and the sum of the threat values ​​of the ending point. When executing obstacle avoidance decisions, the generation module retrieves a pre-calibrated basic safety threshold. This basic safety threshold is a critical value used to determine whether the spatiotemporal collision risk integrated along the planned path is acceptable. It is a pre-calibrated dimensionless constant reflecting the maximum risk tolerance allowed by the system in the absence of dynamic obstacle uncertainty. For example, the basic safety threshold is preset to 0.8. The generation module then retrieves a trajectory uncertainty measurement parameter and multiplies it by a preset dimension conversion coefficient to calculate a safety margin compensation value. The dimension conversion coefficient is a scaling factor used to linearly map the trajectory uncertainty measurement in bits to the safety threshold compensation value. It can be determined based on the ratio of the maximum acceptable safety margin loss to the estimated maximum trajectory information entropy. For example, in this embodiment, the preset dimension conversion coefficient is 0.05. The generation module subtracts the safety margin compensation value from the basic safety threshold to obtain the dynamic safety threshold. The more unpredictable the obstacle movement, the lower the dynamic safety threshold.

[0066] The generation module uses digital comparator logic to compare the path integral value with the dynamic safety threshold in real time. When the path integral value exceeds the dynamic safety threshold, the generation module sends a low-level hardware stop control frame message containing the target deceleration parameter to the underlying programmable logic controller (PLC) connected in the communication connection, generating and executing an emergency braking command. The target deceleration is preset to -2 m / s². 2 The underlying programmable logic controller converts the target deceleration into a corresponding braking torque to control the motor braking of the four-way shuttle robot, such as... Figure 2As shown, the dynamic safety threshold has a linear negative correlation with the trajectory information entropy. The basic safety threshold is 0.8, and the threshold drops to 0.55 when the entropy increases from 0 to 5.

[0067] This experiment serves as a comparative test to verify the performance of the obstacle avoidance decision-making module. The experiment was conducted in a densely packed warehouse testing area for four-way shuttle robots, measuring 30m in length and 15m in width. Five dynamic obstacle robots with irregular movement patterns were randomly placed in the environment. The airborne radar sampling rate was configured at 10Hz, and the 3D voxel mesh size was uniformly set to 0.05m. The maximum cruising speed of the four-way shuttle robots was set to 2m / s, and the local planned path length for each test was 5m. A total of 500 autonomous navigation tests were conducted in a high-dynamic environment. The control group ran a traditional distance judgment scheme based on a fixed safety threshold and without considering trajectory uncertainty, while the experimental group ran the comprehensive strategy proposed in this application, based on temporal kinematic threat field integrals and dynamic safety thresholds with information entropy.

[0068] In 500 dynamic obstacle avoidance tests, the control group encountered 15 collisions when facing highly dynamic obstacles with abrupt changes in trajectory, with an average obstacle avoidance response time of 0.68s. Simultaneously, interference from surrounding irrelevant moving noise caused 28 false emergency braking triggers, resulting in an overall obstacle avoidance success rate of 97%. After adopting the experimental group, due to the use of an adjustment mechanism, the total number of collisions decreased to 2, and the average obstacle avoidance response time was shortened to 0.39s. Benefiting from the probabilistic constraints of the temporal kinematic threat field generated by the spatial correspondence array, the experimental group reduced the number of false alarms and false stops to 6, and the overall obstacle avoidance success rate increased to 99.6%. Figure 3 As shown, the experimental group outperformed the control group in both the number of collisions and the number of mis-braking incidents. The number of collisions decreased from 15 to 2, and the number of mis-braking incidents decreased from 28 to 6.

[0069] By utilizing a multivariate Gaussian function to deduce the probability distribution in continuous space and construct a temporal kinematic threat field, a representation of collision risks that evolve with time and space is achieved, reducing false alarms caused by the fixed safety radius dead zone in conventional solutions. By incorporating the information entropy of dynamic obstacle trajectories as an uncertainty metric into the obstacle avoidance decision-making process, the safety threshold is tightened when facing unreliable trajectory predictions. This ensures the robot can trigger deceleration braking control earlier, which is then converted into corresponding braking torque by the underlying motor controller, improving obstacle avoidance stability and decision predictability in high-risk scenarios.

[0070] It should be noted that those skilled in the art can make various modifications and improvements without departing from the inventive concept, and these all fall within the scope of protection of this invention. Therefore, the scope of protection of this patent should be determined by the appended claims.

Claims

1. A four-way shuttle robot obstacle avoidance and safety control system based on lidar, characterized in that, include: The calculation module is used to acquire the original 3D point cloud data and relative pose estimation data at the current moment. It combines the relative pose estimation data with the original 3D point cloud data at the previous moment and the current moment to segment the dynamic point cloud clusters and the static background point cloud. It extracts the state vector of the dynamic point cloud cluster containing the centroid coordinates and translation velocity vector. The solution module is used to obtain a coarse registration transformation matrix by matching the static background point cloud with the global environment map point cloud. It calculates the scene dynamics based on the magnitude of the translation velocity vector and the number of dynamic point cloud clusters. Based on the scene dynamics, it adjusts the weighting coefficients of the Euclidean distance, the angle between the normal vectors and the difference in principal curvature. It uses the adjusted weighting coefficients to calculate the weighted sum of the Euclidean distance, the angle between the normal vectors and the difference in principal curvature between the source point and the target point to find corresponding point pairs. Based on the corresponding point pairs, it calculates the fine transformation matrix to obtain the pose. The generation module is used to predict the probability distribution of motion trajectory based on the state vector. It samples the probability distribution of motion trajectory on a preset spatial grid to obtain a spatial occupancy probability array, calculates the information entropy of the spatial occupancy probability array as a trajectory uncertainty measure, and constructs a temporal kinematic threat field by combining the motion trajectory probability distribution with a discrete four-way kinematics model and integrates it along the planned path. When the integral value exceeds the dynamic safety threshold adjusted based on the trajectory uncertainty measure, a braking command is generated. Specifically, the generation module uses a constant velocity motion model as the basic model to establish a state transition equation. Based on the state vector and the state transition equation, it calculates the mean predicted coordinates for each discrete prediction time. It analyzes the predicted arrival time of the four-way shuttle robot to each grid cell from the discrete four-way kinematics model, extracts the motion trajectory probability distribution at the predicted arrival time, and multiplies the extracted motion trajectory probability distribution by an intensity reduction coefficient that decreases exponentially with the distance between the grid cell and the mean predicted coordinates and records it to a three-dimensional floating-point array to construct a temporal kinematic threat field. The trajectory uncertainty measure is multiplied by a preset dimension conversion coefficient to align the dimensions and obtain a safety margin compensation value. The dynamic safety threshold is obtained by subtracting the safety margin compensation value from the pre-calibrated basic safety threshold.

2. The obstacle avoidance and safety control system for a four-way shuttle robot based on lidar according to claim 1, characterized in that, The calculation module converts the original three-dimensional point cloud data of the previous time and the current time into three-dimensional voxel grids respectively, compares the difference in average coordinates of point clouds within the same three-dimensional voxel grid to retain moving points, and uses a density-based spatial clustering algorithm to merge moving points whose distance is less than the cluster search radius, and discards groups whose number of points is less than the minimum number of core points, in order to obtain the dynamic point cloud cluster.

3. The obstacle avoidance and safety control system for a four-way shuttle robot based on lidar as described in claim 1, characterized in that, The calculation module calculates the average value of the coordinates of each point in the dynamic point cloud cluster to obtain the centroid coordinates. Based on the nearest neighbor principle of the centroid coordinates, when the Euclidean distance between the centroid coordinates at the current time and the centroid coordinates at the previous time is less than the preset maximum association distance threshold, a data matching relationship between the dynamic point cloud clusters at the previous and next time times is established. The centroid displacement vector is obtained by subtracting the associated centroid coordinates at the previous time from the centroid coordinates at the current time. The translational velocity vector is obtained by dividing the centroid displacement vector by the sampling time interval.

4. The obstacle avoidance and safety control system for a four-way shuttle robot based on lidar according to claim 1, characterized in that, The solution module normalizes the Euclidean distance, the angle between normal vectors, and the difference in principal curvature, and then calculates a weighted sum. The calculation factor of the scene dynamics is formed by multiplying the sum of the translational velocity vector magnitudes of each dynamic point cloud cluster by a first empirical coefficient, and adding it to the number of dynamic point cloud clusters by a second empirical coefficient. The calculation factor is then mapped to the interval between 0 and 1 to obtain the scene dynamics.

5. The obstacle avoidance and safety control system for a four-way shuttle robot based on lidar according to claim 1, characterized in that, The solution module calculates the centroid coordinates of the source point and the target point in each corresponding point pair to construct a cross-correlation matrix. It then decomposes the cross-correlation matrix using singular value decomposition and calculates the optimal rotation matrix by taking the product of the decomposed orthogonal matrices. Finally, it calculates the translation column vector by combining the centroid coordinates and synthesizes the optimal rotation matrix and the translation column vector to obtain the fine transformation matrix.

6. The obstacle avoidance and safety control system for a four-way shuttle robot based on lidar according to claim 1, characterized in that, The generation module uses a preset process noise covariance matrix to calculate the spatial covariance matrix for each discrete prediction time, and uses a multivariate Gaussian function to solve for the mean prediction coordinates and the spatial covariance matrix, outputting a continuous spatial position probability density distribution as the motion trajectory probability distribution.

7. The obstacle avoidance and safety control system for a four-way shuttle robot based on lidar according to claim 1, characterized in that, The generation module calculates the integral value by performing numerical integration weighted summation on the sequence interpolation nodes and corresponding obstacle threat assignment data on the planned path according to the Simpson quadrature criterion.

8. The obstacle avoidance and safety control system for a four-way shuttle robot based on lidar according to any one of claims 1 to 7, characterized in that, It also includes a low-level programmable logic controller with a communication connection. The low-level programmable logic controller is used to receive the braking command issued by the generation module and convert the target deceleration contained in the braking command into a corresponding braking torque to control the motor to perform emergency braking.

9. The obstacle avoidance and safety control system for a four-way shuttle robot based on lidar according to any one of claims 1 to 7, characterized in that, It also includes a memory and a processor communicatively connected to the memory. The memory stores computer execution instructions, and the processor instantiates the operating logic of the computing module, the solving module, and the generating module when executing the computer execution instructions.

Images