A collision avoidance path planning method and system for a four-way shuttle robot

Abstract

The application provides a kind of four-way shuttle robot anti-collision path planning method and system, it is related to warehousing robot technical field, wherein, the method comprises: collecting the three-dimensional point cloud data of four-way shuttle robot and the robot attitude data containing inertial measurement data and real-time pose information;Then based on inertial measurement data, motion distortion correction is carried out to point cloud data, and the corrected three-dimensional point cloud data and real-time pose information are fused into fusion environment data;Then fusion environment data is input into improved PointNet++ network, the network identifies dynamic obstacles and estimates its motion information by analyzing data, and outputs obstacle list;Finally, according to the current position of four-way shuttle robot, target position and the list, combined with the predicted trajectory of dynamic obstacle, node expansion is carried out, and anti-collision path is planned.The application improves the safety obstacle avoidance and path planning efficiency of four-way shuttle robot in complex dynamic environment.

Description

Technical Field

[0001] This application relates to the field of warehouse robot technology, and in particular to a collision avoidance path planning method and system for a four-way shuttle robot. Background Technology

[0002] With the rapid development of automated warehousing and logistics, four-way shuttle robots, as core handling equipment, rely heavily on precise anti-collision path planning methods for safe operation in high-density dynamic environments. The application prospects of this method are directly related to the operational efficiency and safety of warehouses.

[0003] Existing collision avoidance methods for four-way shuttle robots mainly rely on sensors such as LiDAR to acquire environmental information, construct static environmental maps and plan preset paths, and supplement them with real-time detection for emergency obstacle avoidance during execution; some methods also attempt to introduce simple dynamic object detection to deal with slowly moving obstacles in the environment.

[0004] However, while these methods are effective in handling fixed routes or single dynamic targets, they are limited in terms of safety and efficiency when faced with complex scenarios involving high-speed movement between high-level shelves, multi-robot interaction, and frequent temporary obstacles. Therefore, existing technologies suffer from insufficient safety and adaptability in path planning in highly dynamic and complex environments. Summary of the Invention

[0005] This application provides a collision avoidance path planning method and system for a four-way shuttle robot, which solves the problems of poor obstacle avoidance and low path planning efficiency of four-way shuttle robots in complex dynamic environments in the prior art.

[0006] To address the aforementioned technical problems, in a first aspect, this application provides a collision avoidance path planning method for a four-way shuttle robot, comprising:

[0007] Collect three-dimensional point cloud data and robot posture data of a four-way shuttle robot. The robot posture data includes inertial measurement data and real-time pose information.

[0008] Based on the inertial measurement data, the motion distortion of the three-dimensional point cloud data is corrected using a quaternion spherical linear interpolation algorithm;

[0009] The corrected 3D point cloud data is fused with the real-time pose information to form fused environmental data;

[0010] The fused environment data is input into an improved PointNet++ network. The improved PointNet++ network analyzes the fused environment data, identifies dynamic obstacles in the environment, estimates the motion information of the dynamic obstacles, and outputs a list of obstacles containing the motion information.

[0011] Based on the current position and target position of the four-way shuttle robot and the obstacle list, a path planning algorithm is adopted to perform path planning. During the planning process, the dynamic RRT algorithm expands the path nodes by combining the predicted trajectories of dynamic obstacles in the obstacle list in order to plan a collision avoidance path.

[0012] Optionally, the step of using a dynamic RRT algorithm for path planning based on the current position of the four-way shuttle robot, the target position, and the obstacle list, wherein the dynamic RRT algorithm expands the path nodes by combining the predicted trajectories of dynamic obstacles in the obstacle list during the planning process, in order to plan an anti-collision path, includes:

[0013] A dynamic RRT algorithm is used to construct a path search tree with the current position as the starting point and the target position as the ending point.

[0014] Within the drivable area of ​​the four-way shuttle robot, a location point is randomly selected as a candidate point, and the tree node closest to the candidate point is determined from the path search tree.

[0015] In the direction from the nearest tree node to the candidate point, a new path segment and a new tree node are generated, and dynamic conflict checks are performed on the new path segment.

[0016] If the check result is that no conflict is found, the new tree node and the new path segment are added to the path search tree;

[0017] The steps of random selection, determination, trial generation, dynamic conflict checking, and addition are executed repeatedly until the path search tree extends to the endpoint.

[0018] Extract the complete path connecting the starting point and the ending point from the path search tree, and use it as the collision avoidance path.

[0019] Optionally, the dynamic conflict check on the new path segment includes:

[0020] During the attempt, based on the motion information of the dynamic obstacles in the obstacle list, the motion trajectory of the dynamic obstacle is predicted within the time period during which the new path segment may be executed.

[0021] An LSTM-based trajectory prediction module is introduced to analyze the historical motion state sequence of dynamic obstacles in the obstacle list in order to correct the predicted trajectory.

[0022] Based on the corrected predicted trajectory, a cone-shaped conflict zone that varies with time within the time period is calculated for each dynamic obstacle using the speed obstacle method.

[0023] The results of the inspection are obtained by checking whether the new path segment will conflict with any obstacles or the cone-shaped conflict area during the time period.

[0024] Optionally, the fused environment data is input into an improved PointNet++ network. The improved PointNet++ network analyzes the fused environment data, identifies dynamic obstacles in the environment, estimates the motion information of the dynamic obstacles, and outputs a list of obstacles containing the motion information, including:

[0025] The fused environment data is input into the feature extraction module of the improved PointNet++ network. Through the multilayer perceptron and max pooling layer in the feature extraction module, the depth features of each point cloud point are extracted from the fused environment data frame by frame to form a point cloud feature map for each frame.

[0026] The point cloud feature maps of multiple consecutive frames are input into the temporal attention module of the improved PointNet++ network. In the temporal attention module, the correlation weights of corresponding feature regions between different frames are calculated, and the features of multiple frames are weighted, fused, and context-associated based on the correlation weights to generate an enhanced feature map with temporal correlation.

[0027] The enhanced feature map is input into the classification and regression module of the improved PointNet++ network. The fully connected layer and softmax layer in the classification and regression module are used to classify each feature region in the enhanced feature map to distinguish between static obstacles and dynamic obstacles.

[0028] For key feature regions identified as dynamic obstacles, the motion information of the dynamic obstacle is determined by the regression sub-network in the classification and regression module based on the position offset of the key feature regions in the enhanced feature maps of consecutive frames.

[0029] The improved PointNet++ network output module aggregates the categories, key information, and motion information of all obstacles to generate an obstacle list.

[0030] Optionally, the step of correcting motion distortion of the three-dimensional point cloud data using a quaternion spherical linear interpolation algorithm based on the inertial measurement data includes:

[0031] Based on the angular velocity and acceleration information in the inertial measurement data, and combined with the robot kinematics model, the robot motion of the four-way shuttle robot during the acquisition of the three-dimensional point cloud data is calculated. The robot motion includes angular change and displacement change.

[0032] Based on the scanning cycle of the rotating lidar, the angle change and the displacement change are allocated to the acquisition time corresponding to each point cloud in the three-dimensional point cloud data to obtain the instantaneous motion at each acquisition time.

[0033] Based on the instantaneous motion at each acquisition moment, the attitude interpolation at each acquisition moment relative to the acquisition start moment is calculated using the quaternion spherical linear interpolation algorithm, and combined with the displacement change, the coordinate transformation parameters corresponding to each acquisition moment are obtained.

[0034] For each point cloud in the three-dimensional point cloud data, the original coordinates of the point cloud are inversely compensated according to the coordinate transformation parameters corresponding to the acquisition time, so as to obtain the corrected coordinates of the point cloud in the coordinate system at the acquisition start time of the three-dimensional point cloud data.

[0035] The corrected coordinates of all the point clouds are aggregated to form the corrected 3D point cloud data.

[0036] Optionally, fusing the corrected 3D point cloud data with the real-time pose information to form fused environmental data includes:

[0037] Based on the predetermined coordinate transformation relationship between the rotating lidar and the four-way shuttle robot, the corrected 3D point cloud data is transformed into the robot coordinate system of the four-way shuttle robot.

[0038] The position information, attitude information, and corresponding acquisition time of the four-way shuttle robot recorded in the real-time pose information are obtained.

[0039] By using a state estimator based on Kalman filtering, the position information and the attitude information are smoothed and compensated to obtain optimized robot pose state information;

[0040] The 3D point cloud data transformed into the robot coordinate system is merged with the optimized robot pose state information corresponding to the acquisition time.

[0041] The point cloud in the merged data is processed into a voxel grid to generate point cloud map blocks, and the point cloud map block corresponding to each acquisition time is used as a basic data unit.

[0042] Multiple basic data units that are consecutive in time are aggregated to form fused environmental data.

[0043] Optionally, after planning the collision avoidance path, the following may also be included:

[0044] The gradient descent optimization algorithm is used to reduce the path curvature and the rate of change of acceleration. The node positions of the anti-collision path are iteratively adjusted to obtain a smooth trajectory.

[0045] Using a time-optimal trajectory planning model, under the conditions of satisfying the motor torque, hub speed and structural strength constraints of the four-way shuttle robot, the travel time is allocated to the smooth trajectory to determine the speed curve of each segment of the smooth trajectory. The smooth trajectory is then combined with the corresponding speed curve to obtain the optimized collision avoidance path.

[0046] Secondly, this application provides a collision avoidance path planning system for a four-way shuttle robot, comprising:

[0047] The acquisition module is used to acquire three-dimensional point cloud data and robot posture data of the four-way shuttle robot. The robot posture data includes inertial measurement data and real-time pose information.

[0048] The correction module is used to correct motion distortion of the three-dimensional point cloud data based on the inertial measurement data using a quaternion spherical linear interpolation algorithm.

[0049] The fusion module is used to fuse the corrected 3D point cloud data with the real-time pose information to form fused environmental data;

[0050] An input module is used to input the fused environment data into an improved PointNet++ network. The improved PointNet++ network analyzes the fused environment data, identifies dynamic obstacles in the environment, estimates the motion information of the dynamic obstacles, and outputs a list of obstacles containing the motion information.

[0051] The planning module is used to perform path planning using a dynamic RRT algorithm based on the current position, target position, and obstacle list of the four-way shuttle robot. During the planning process, the dynamic RRT algorithm expands the path nodes by combining the predicted trajectories of dynamic obstacles in the obstacle list to plan an anti-collision path.

[0052] Thirdly, this application provides an electronic device, comprising:

[0053] Memory, used to store computer programs;

[0054] A processor is configured to implement the steps of the collision avoidance path planning method for a four-way shuttle robot as described in the first aspect above when executing the computer program.

[0055] Fourthly, this application provides a computer-readable storage medium storing a computer program that, when executed by a processor, can implement the steps of the collision avoidance path planning method for a four-way shuttle robot as described in the first aspect above.

[0056] This application provides a collision avoidance path planning method for a four-way shuttle robot. The method includes: acquiring three-dimensional point cloud data and robot posture data of the four-way shuttle robot, wherein the robot posture data includes inertial measurement data and real-time pose information; performing motion distortion correction on the three-dimensional point cloud data using a quaternion spherical linear interpolation algorithm based on the inertial measurement data; fusing the corrected three-dimensional point cloud data with the real-time pose information to form fused environment data; inputting the fused environment data into an improved PointNet++ network, wherein the improved PointNet++ network analyzes the fused environment data, identifies dynamic obstacles in the environment, estimates the motion information of the dynamic obstacles, and outputs an obstacle list containing the motion information; and performing path planning using a dynamic RRT algorithm based on the current position, target position, and obstacle list of the four-way shuttle robot. During the planning process, the dynamic RRT algorithm expands the path nodes by incorporating the predicted trajectories of the dynamic obstacles in the obstacle list to plan a collision avoidance path.

[0057] The technical solution provided in this application has the following beneficial effects:

[0058] This application effectively eliminates point cloud distortion caused by the robot's own motion by collecting 3D point cloud and robot posture data and using interpolation algorithms for motion distortion correction, thereby improving the accuracy of environmental perception data. Secondly, fusing the corrected point cloud with real-time pose information generates spatiotemporally unified fused environmental data, providing a consistent data foundation for subsequent processing. Then, analyzing the fused data through an improved neural network accurately identifies dynamic obstacles and estimates their motion state, enhancing the system's understanding of the dynamic environment. Finally, by combining a list containing dynamic obstacle motion information and employing a dynamic path planning algorithm for node expansion and conflict checking, the future positions of dynamic obstacles can be avoided in advance during the path planning stage, thus planning a safer and more reliable collision avoidance path.

[0059] Furthermore, this application constructs a path search tree with the current location as the starting point and the target location as the ending point. By randomly sampling in the drivable area and determining the nearest tree node, it attempts to generate new path segments and tree nodes, and performs a conflict check on the path segment based on dynamic obstacle prediction. If it is safe, it is added to the search tree and this process is repeated until the search tree reaches the end point. Finally, the complete path is extracted as the collision avoidance path.

[0060] Therefore, this application integrates the predicted trajectory of dynamic obstacles into the growth process of the path search tree in real time, so that the planned path can not only avoid static obstacles, but also proactively avoid the movement routes of dynamic obstacles, thereby improving the safety and adaptability of path planning in dynamic and complex environments.

[0061] These or other aspects of this application will become more apparent in the following description of the embodiments. Attached Figure Description

[0062] To more clearly illustrate the technical solutions in the embodiments of this application or the prior art, the drawings used in the description of the embodiments or the prior art will be briefly introduced below. Obviously, the drawings described below are some embodiments of this application. For those skilled in the art, other drawings can be obtained based on these drawings without creative effort.

[0063] Figure 1 A flowchart illustrating a collision avoidance path planning method for a four-way shuttle robot, provided as an embodiment of this application;

[0064] Figure 2 A schematic diagram illustrating a specific implementation of a collision avoidance path planning method for a four-way shuttle robot provided in this application embodiment;

[0065] Figure 3 This is a schematic diagram of the anti-collision path planning system for a four-way shuttle robot provided in an embodiment of this application. Detailed Implementation

[0066] To address the aforementioned issues of insufficient safety and adaptability in path planning within highly dynamic warehousing environments, this application proposes a collision avoidance path planning method for a four-way shuttle robot. The core idea of ​​this method is as follows: First, it integrates the robot's own 3D point cloud and inertial measurement attitude data, correcting motion distortion through specific interpolation to obtain an accurate environmental perception basis. Then, it fuses the corrected perception data with the robot's real-time pose to form a unified spatiotemporal reference for the fused environment description. Next, it utilizes an improved neural network to perform deep analysis on the fused data, identifying dynamic obstacles and accurately estimating their motion trends, generating an obstacle list containing motion state information. Finally, the planning algorithm, combined with this dynamic list, proactively avoids obstacles by considering the predicted trajectories of dynamic obstacles in real-time during path expansion, thereby directly planning a safe global path.

[0067] Therefore, this method, through a collaborative processing chain from precise perception and motion understanding to forward planning, directly embeds the future state of dynamic obstacles into the path decision-making process. This effectively overcomes the limitations of existing technologies in planning lag or overly conservative approaches when facing complex dynamic scenarios such as high-speed shuttles and multi-target interactions, thereby improving the safety level and path efficiency of four-way shuttle robot operations.

[0068] To enable those skilled in the art to better understand the present application, the present application will be further described in detail below with reference to the accompanying drawings and specific embodiments. Obviously, the described embodiments are merely some embodiments of the present application, and not all embodiments. Based on the embodiments in this application, all other embodiments obtained by those skilled in the art without creative effort are within the scope of protection of this application.

[0069] The core of this application is to provide a collision avoidance path planning method for a four-way shuttle robot, and a flowchart of one specific implementation is shown below. Figure 1 As shown, the method includes:

[0070] Step 101: Collect 3D point cloud data and robot posture data of the four-way shuttle robot. The robot posture data includes inertial measurement data and real-time pose information.

[0071] In step 101, the three-dimensional point cloud data refers to a set of dense spatial points generated by the rotating lidar installed on the top of the four-way shuttle robot after scanning the surrounding environment. Each point has precise three-dimensional coordinate information, which together outline the contours of the shelves, other robots and obstacles around the robot.

[0072] Robot posture data is used to describe the motion state and spatial orientation of the four-way shuttle robot; among them, inertial measurement data is provided by the on-board inertial measurement unit, which records the robot's real-time changes in angular velocity and linear acceleration; real-time pose information indicates the robot's specific position and orientation in the global coordinate system.

[0073] In this embodiment, a rotating LiDAR mounted on a vehicle is used to scan and acquire three-dimensional point cloud data that depicts the contours of the environment in real time. At the same time, the robot's own inertial measurement unit and positioning system are used to collect inertial measurement data that reflects the instantaneous changes in the robot's motion state and real-time pose information that characterizes the robot's global position and orientation.

[0074] Step 102: Based on the inertial measurement data, perform motion distortion correction on the three-dimensional point cloud data using a quaternion spherical linear interpolation algorithm.

[0075] In this embodiment, step 102 includes the following process:

[0076] Step 1021: Based on the angular velocity and acceleration information in the inertial measurement data, and combined with the robot kinematic model, calculate the robot motion of the four-way shuttle robot during the acquisition of the three-dimensional point cloud data. The robot motion includes angular change and displacement change.

[0077] In step 1021, angular velocity information is used to describe how fast the robot rotates, while acceleration information is used to describe how fast the robot's velocity changes. The robot's motion quantity is a comprehensive index describing the robot's motion state over a period of time, where the change in angle reflects how much the robot has rotated in orientation, and the change in displacement reflects how far the robot's position has moved.

[0078] A robot kinematics model is a mathematical formula used to describe the motion laws and geometric constraints of a specific type of robot, such as a four-way shuttle robot. Furthermore, this application does not impose specific limitations on the expression of the mathematical formula used in the robot kinematics model, and can set it accordingly based on the actual situation.

[0079] In this embodiment, the angular velocity information of the robot's rotation speed and the acceleration information of the speed of velocity change are first recorded in the collected inertial measurement data. This information is then input into a robot kinematic model that can accurately characterize the motion characteristics of the four-way shuttle robot for integral calculation, thereby accurately calculating the total angular change and total displacement change experienced by the robot itself during the time when the lidar scans a whole frame of point cloud.

[0080] Step 1022: Based on the scanning cycle of the rotating lidar, the angle change and the displacement change are allocated to the acquisition time corresponding to each point cloud in the three-dimensional point cloud data to obtain the instantaneous motion at each acquisition time.

[0081] In step 1022, the scanning cycle refers to the time required for the lidar to complete one complete environmental scan, the acquisition time is the exact time when the lidar scans a specific point in the environment and generates the corresponding point cloud data point; the instantaneous motion refers to the minute angle change and minute position movement experienced by the robot within a very short time point or time period.

[0082] In this embodiment of the application, based on the fixed cycle of the lidar completing one scan, the total angle change and total displacement change calculated in step 1021 are decomposed and allocated linearly or nonlinearly according to time. Specifically, based on the precise time when each point cloud data point is acquired by the lidar, a corresponding small angle change and small displacement are assigned to that point, thereby obtaining the instantaneous motion corresponding to each point cloud data point.

[0083] Step 1023: Based on the instantaneous motion at each acquisition moment, use the quaternion spherical linear interpolation algorithm to calculate the attitude interpolation at each acquisition moment relative to the acquisition start moment, and combine it with the displacement change to obtain the coordinate transformation parameters corresponding to each acquisition moment.

[0084] In step 1023, the acquisition start time refers to the starting time point when the lidar begins scanning the current frame of point cloud; the coordinate transformation parameters are a set of mathematical parameters used to transform the coordinates of a point in one three-dimensional coordinate system to another three-dimensional coordinate system. The coordinate transformation parameters usually include rotation parameters and translation parameters.

[0085] The explanation and specific implementation of the quaternion spherical linear interpolation algorithm can be found in relevant technologies, and will not be elaborated here.

[0086] In this embodiment, the instantaneous motion data obtained in step 1022 at each acquisition moment, which includes minute attitude change information, is used to calculate the intermediate interpolation results of the attitude smoothly changing from the initial acquisition moment to the attitude at each subsequent acquisition moment using a quaternion spherical linear interpolation algorithm. Then, the attitude interpolation results are combined with the minute displacement changes corresponding to each acquisition moment to construct a corresponding coordinate transformation parameter for each acquisition moment. This parameter fully defines how to transform the point cloud coordinates acquired at that moment back to the coordinate system of the initial moment.

[0087] Step 1024: For each point cloud in the three-dimensional point cloud data, based on the coordinate transformation parameters corresponding to the acquisition time, perform inverse compensation calculation on the original coordinates of the point cloud to obtain the corrected coordinates of the point cloud in the coordinate system at the start time of the acquisition of the three-dimensional point cloud data.

[0088] In step 1024, the original coordinates refer to the three-dimensional coordinates of the point cloud data points directly generated in the coordinate system of the lidar itself, based on the time when they were scanned; the inverse compensation calculation refers to the process of using mathematical inverse transformation to correct the coordinates affected by motion to the unaffected reference coordinate system; the corrected coordinates refer to the three-dimensional coordinates obtained after the inverse compensation calculation, located in a unified reference coordinate system, and free from the effects of motion distortion.

[0089] In this embodiment of the application, for each point cloud in the three-dimensional point cloud data, the coordinate transformation parameters pre-calculated in step 1023 are found according to its corresponding acquisition time. Then, the original coordinates of the point cloud are subjected to a mathematical transformation operation that is opposite to the coordinate transformation parameters. Through this reverse compensation calculation, the point cloud coordinates are uniformly transformed from the actual coordinate system of their respective acquisition time to the coordinate system defined by the acquisition start time of the point cloud in this frame, thereby obtaining more accurate corrected coordinates of spatial position.

[0090] Step 1025: Aggregate the corrected coordinates of all the point clouds to form corrected 3D point cloud data.

[0091] In this embodiment of the application, the correction coordinates calculated for each point cloud data point in step 1024 are reorganized and set according to their logical relationship in the environment, thereby generating a brand new frame of three-dimensional point cloud data as the output of the correction result. The coordinates of all points in this frame of data are based on the same static reference system, which effectively eliminates the spatiotemporal inconsistency problem introduced by robot motion.

[0092] This application effectively eliminates the distortion of environmental perception point cloud data caused by the robot's own movement during high-speed movement through the above process, providing a high-quality input data foundation for subsequent accurate fusion and recognition.

[0093] Step 103: Fuse the corrected 3D point cloud data with the real-time pose information to form fused environmental data.

[0094] Among them, fused environmental data is a data structure that unifies the precise three-dimensional structural information of the environment with the precise spatiotemporal state information of the robot itself. Its purpose is to provide a spatiotemporally consistent and information-complete environmental description basis for subsequent analysis and processing.

[0095] In this embodiment, step 103 includes the following process:

[0096] Step 1031: Based on the predetermined coordinate transformation relationship between the rotating lidar and the four-way shuttle robot, the corrected 3D point cloud data is transformed into the robot coordinate system of the four-way shuttle robot.

[0097] In step 1031, the predetermined coordinate transformation relationship between the rotating lidar and the four-way shuttle robot is a set of fixed mathematical parameters that are accurately measured and calculated in the calibration field beforehand. This coordinate transformation relationship describes the spatial geometric relationship between the installation position and orientation of the lidar and the center point of the robot. The robot coordinate system is a three-dimensional Cartesian coordinate system established with the robot itself as the center and moving with the robot.

[0098] In this embodiment, a rigid mathematical coordinate transformation is first performed on each point cloud coordinate in the corrected 3D point cloud data output in step 102 using a fixed coordinate transformation relationship that has been calibrated before the robot leaves the factory and describes the physical position of the LiDAR relative to the robot. This transforms all the point cloud data that was originally located in the LiDAR's own coordinate system into a robot coordinate system with the robot itself as the reference center.

[0099] Step 1032: Obtain the position information, attitude information, and corresponding acquisition time of the four-way shuttle robot recorded in the real-time pose information.

[0100] In step 1032, the position information is used to indicate the specific three-dimensional coordinates of the four-way shuttle robot in the global environment map or world coordinate system, and the attitude information is used to indicate the direction of the four-way shuttle robot's front or the angle of its body at the current moment. The corresponding acquisition time refers to the specific time point at which the position information and attitude information are acquired.

[0101] Step 1033: Using a Kalman filter-based state estimator, smooth and compensate the position information and the pose information to obtain optimized robot pose state information.

[0102] In step 1033, the state estimator based on Kalman filtering is an algorithm module that combines the system motion model with sensor observation data. This algorithm module estimates the system state that is closer to the true value by iteratively predicting and correcting the noisy sensor data.

[0103] This application does not impose specific limitations on the internal structure and parameter design of the state estimator based on Kalman filtering; these can be set according to the actual situation.

[0104] In this embodiment of the application, the position information and attitude information extracted in step 1032 are input into a pre-built Kalman filter state estimator. The estimator predicts the pose based on the robot's motion model and corrects it by combining the latest sensor observations, thereby smoothing and compensating for noise, jumps or brief loss that may exist in the original data, and finally outputs a set of more stable and continuous optimized robot pose state information.

[0105] Step 1034: Merge the 3D point cloud data converted to the robot coordinate system with the optimized robot pose state information corresponding to the acquisition time.

[0106] In this embodiment of the application, for each frame of 3D point cloud data, the robot pose state information that is the same as or closest to its acquisition timestamp and has been optimized in step 1033 is found. Then, the point cloud data of that frame converted to the robot coordinate system is logically associated and encapsulated with this optimized pose state information to form a data package containing the local environment structure and the global robot state.

[0107] Step 1035: Perform voxel meshing on the point cloud in the merged data to generate point cloud map blocks, and take the point cloud map block corresponding to each acquisition time as a basic data unit.

[0108] In step 1035, voxel meshing is the process of dividing a continuous three-dimensional space into regular small cubic meshes and approximating all point cloud points falling into the same small cubic mesh with their center point or other statistical features; the point cloud map block is a point cloud data block with a regular spatial index structure obtained after voxel meshing.

[0109] In this embodiment, the 3D point cloud data in the merged data packet after step 1034 is processed using a voxel meshing algorithm. Specifically, by setting a spatial size parameter, the space containing the point cloud is divided into numerous small, regular cubic grids, and the point cloud within each grid is aggregated and simplified. This significantly reduces the amount of data while preserving the main structural features of the environment, ultimately generating a structured point cloud map tile. Each such point cloud map tile, along with its associated optimized robot pose state information, is collectively defined as a basic data unit to be processed in subsequent processes. Furthermore, this embodiment does not limit the specific design of the spatial size parameter.

[0110] Step 1036: Aggregate multiple time-sequential basic data units to form fused environmental data.

[0111] In this embodiment of the application, multiple basic data units that are sequential in time are arranged and combined according to the order of collection time, thereby forming a fusion environment data sequence that is continuous in the time dimension and unified in the information dimension.

[0112] This application constructs a spatiotemporally unified, structurally regular, and higher-quality environmental representation through the above process, providing reliable and efficient data input for subsequent dynamic obstacle recognition and motion estimation based on neural networks.

[0113] Step 104: Input the fused environment data into the improved PointNet++ network. The improved PointNet++ network analyzes the fused environment data, identifies dynamic obstacles in the environment, estimates the motion information of the dynamic obstacles, and outputs a list of obstacles containing the motion information.

[0114] The improved PointNet++ network, based on the original point cloud processing architecture, introduces a temporal attention module as the core improvement for dynamic warehouse scenarios. Its overall structure consists of a feature extraction module, a temporal attention module, and a classification and regression module. The feature extraction module adopts the hierarchical point set abstraction design of the original PointNet++, and specifically extracts the local geometric features of the point cloud step by step through sampling, grouping, and multilayer perceptron, and uses max pooling to aggregate the global features.

[0115] The newly added temporal attention module is located after the feature extraction module. Its structure can be designed as a network layer based on a self-attention mechanism. This layer receives point cloud feature maps from multiple consecutive frames as input, generates a correlation weight matrix by calculating the similarity of feature vectors between different frames, and then uses this matrix to weight and fuse the features of multiple frames, so that the features of the current frame can be associated with and reinforce the temporal change patterns of the same spatial region in past frames. The classification and regression module contains a fully connected softmax branch for obstacle classification and a fully connected branch for motion information regression. The regression branch receives enhanced features rich in temporal information from the output of the temporal attention module and regresses the velocity and direction by analyzing the coordinate offset of the same feature region in multiple frames.

[0116] The main improvement of this network is the introduction of the temporal attention module. This temporal attention module enables the network to not only analyze the static shape of a single frame point cloud, but also to actively capture and utilize motion cues between consecutive frames. In this way, in complex scenarios where multiple robots shuttle at high speed between warehouse high-level shelves, the network can effectively distinguish between static shelves and dynamic robots / pedestrians, and accurately estimate the motion state of dynamic targets.

[0117] The training process of this model uses a dataset with dynamic obstacle category labels and ground truth values ​​for motion speed and direction. First, the network is pre-trained end-to-end to learn basic feature extraction and classification capabilities. Then, samples containing continuous frame sequences are used for specialized training of the temporal attention module and regression branch. The network parameters are iteratively optimized until convergence by minimizing the weighted sum of the classification cross-entropy loss and the mean squared error loss of motion parameter regression.

[0118] It should be noted that the above structure is exemplary. This application does not impose specific limitations on the structural design, parameter design, training process, etc. of the improved PointNet++ network internal structure, and corresponding settings can be made according to the actual situation.

[0119] Dynamic obstacles refer to objects in the environment that move relative to the four-way shuttle robot, such as other moving robots or pedestrians; motion information is used to describe the instantaneous movement characteristics of dynamic obstacles, which usually includes movement speed and direction; the obstacle list is a structured data set that summarizes all identified obstacles in the environment at the current moment.

[0120] In this embodiment, step 104 includes the following process:

[0121] Step 1041: Input the fused environment data into the feature extraction module of the improved PointNet++ network. Through the multilayer perceptron and max pooling layer in the feature extraction module, extract the depth features of each point cloud point from the fused environment data frame by frame to form a point cloud feature map for each frame.

[0122] In step 1041, the feature extraction module is a component of the neural network specifically designed to extract useful information from the raw data;

[0123] A multilayer perceptron is a network structure composed of multiple neurons connected in layers. It can perform nonlinear transformations on input data to learn complex features. A max pooling layer is an operation used for downsampling and enhancing feature robustness. This max pooling layer selects the maximum value in a local region as the output. Deep features refer to information learned by a neural network from the point cloud data perceived by a four-way shuttle robot. This information can characterize high-level environmental semantics such as shelf structure, robot outline, and obstacle movement trends. The point cloud feature map is a structured data representation formed by mapping these learned features one-to-one with their three-dimensional spatial positions. This point cloud feature map directly serves the subsequent process of identifying dynamic obstacles and estimating motion.

[0124] In this embodiment, each frame of structured point cloud data generated in step 103 is first input into the feature extraction module of the improved PointNet++ network. This module analyzes the spatial distribution of each point around the four-way shuttle robot through a multilayer perceptron, learns key geometric attributes such as shelf surface, other robot edges and temporary obstacles, and uses a max pooling layer to aggregate key local area information and filter out redundant data. Thus, it extracts high-dimensional depth features that can characterize the structure of the warehouse environment and the shape of objects for each frame of point cloud. These features are organized into a point cloud feature map corresponding to the spatial location to support subsequent dynamic obstacle identification and motion state analysis.

[0125] Step 1042: Input the point cloud feature maps of multiple consecutive frames into the temporal attention module of the improved PointNet++ network. In the temporal attention module, calculate the correlation weights of corresponding feature regions between different frames, and perform weighted fusion and context association on the features of multiple frames based on the correlation weights to generate an enhanced feature map with temporal correlation.

[0126] In step 1042, the temporal attention module is a component in the neural network specifically designed to process time-series data and capture temporal dependencies. The correlation weight is a numerical value used to quantify the strength of the correlation between features at different time steps. The enhanced feature map is a feature map with stronger feature representation capabilities obtained by incorporating temporal information from multiple frames.

[0127] In this embodiment of the application, the continuous multi-frame point cloud features output in step 1041 are... Figure 1 The input is fed into the temporal attention module, which first calculates the similarity of the feature vectors of the same physical location in the warehouse environment at different times to determine the correlation weight, thereby capturing the appearance and displacement patterns of dynamic obstacles. Then, based on these weights, the features of multiple frames are weighted and fused so that the features at the current time can be associated with and strengthen the motion cues of the same obstacle in the historical frames. Finally, an enhanced feature map that integrates temporal evolution information is generated, which provides a key basis for accurately distinguishing between static shelves and dynamic robots and estimating the latter's motion trend.

[0128] Step 1043: Input the enhanced feature map into the classification and regression module of the improved PointNet++ network. Through the fully connected layer and softmax layer in the classification and regression module, classify each feature region in the enhanced feature map to distinguish between static obstacles and dynamic obstacles.

[0129] In step 1043, the classification and regression module is the component in the neural network responsible for outputting specific task results; the fully connected layer is a network layer in which each neuron is connected to all neurons in the previous layer, used to integrate all feature information; the softmax layer is a function layer that converts the raw output of neurons into a probability distribution, often used for multi-classification tasks;

[0130] A feature region refers to a set of features in an enhanced feature map that corresponds to a specific local spatial location within the warehouse environment. This specific local spatial location can be a gap between shelves, a section of a travel aisle, or the area where a mobile robot is located. This set of features collectively describes the structural attributes and motion state of that location in the spatiotemporal dimensions.

[0131] In this embodiment, the enhanced feature map output by the temporal attention module is input into the classification and regression module. The module first performs comprehensive analysis and calculation on each feature region in the feature map through a fully connected layer, and then sends the output of the fully connected layer into the softmax layer. The softmax layer calculates the probability that each feature region belongs to different categories such as "static obstacle", "dynamic obstacle" or "background", and completes the classification based on the maximum probability value, thereby distinguishing static obstacles and dynamic obstacles in the environment at the pixel or point level.

[0132] Step 1044: For the key feature regions identified as dynamic obstacles, the motion information of the dynamic obstacles is determined by the regression sub-network in the classification and regression module based on the position offset of the key feature regions in the enhanced feature maps of consecutive frames.

[0133] In step 1044, the regression subnetwork is a network branch within the classification and regression module specifically designed for predicting continuous values; the key feature region refers to those feature regions that are classified as dynamic obstacles in the enhanced feature map; the position offset refers to the amount of coordinate change of the same object in the spatial position corresponding to multiple consecutive frames of enhanced feature maps.

[0134] In this embodiment of the application, for each key feature region classified as a dynamic obstacle in step 1043, the regression sub-network in the classification and regression module starts working. The sub-network receives the coordinate information of the key feature region in the current frame and the enhanced feature maps of previous consecutive frames. By analyzing the change trajectory of these coordinates between multiple frames, the position offset of the dynamic obstacle in three-dimensional space is calculated, and then its movement speed and movement direction are deduced according to the time interval. This information together constitutes the movement information of the dynamic obstacle.

[0135] Step 1045: Through the output module of the improved PointNet++ network, summarize the categories, key information of all obstacles and the motion information of dynamic obstacles to generate an obstacle list.

[0136] In step 1045, the output module is a component of the improved PointNet++ network responsible for formatting and outputting the final results; key information includes the categories and spatial locations of the identified obstacles.

[0137] In this embodiment, the output module of the improved PointNet++ network collects all results from the classification and regression module, including the category label of each identified obstacle, its three-dimensional position coordinates in the environment, and the motion speed and direction calculated specifically for dynamic obstacles. This information is organized and summarized according to a preset structure to finally generate an obstacle list containing rich dynamic information that can be directly used by the subsequent path planning module.

[0138] This application achieves accurate identification and motion state estimation of dynamic obstacles in complex warehouse environments through the above process, providing key and reliable dynamic environment perception input for subsequent safe path planning.

[0139] Step 105: Based on the current position and target position of the four-way shuttle robot and the obstacle list, a dynamic RRT algorithm is used for path planning. During the planning process, the dynamic RRT algorithm expands the path nodes by combining the predicted trajectories of dynamic obstacles in the obstacle list to plan an anti-collision path.

[0140] Among them, the predicted trajectory is an estimate of the possible movement path of a dynamic obstacle in the future; path node expansion is the process by which the RRT algorithm selects a node from the currently grown path search tree and attempts to add a new node to it to extend the path tree.

[0141] In this embodiment, step 105 includes the following process, such as... Figure 2 As shown:

[0142] Step 1051: Using the dynamic RRT algorithm, construct a path search tree with the current position as the starting point and the target position as the ending point.

[0143] In step 1051, the path search tree is a data structure used to represent all possible paths explored from the starting point, where nodes represent points on the path and lines between nodes represent passable path segments.

[0144] In this embodiment of the application, the initialization step of path planning is to take the specific location coordinates of the four-way shuttle robot in the global map as the starting point of path exploration, take the destination coordinates specified by the task as the ending point, then create an empty path search tree data structure, and add the node representing the starting point as the first and only node of the tree, thereby starting to build the path search tree.

[0145] In practical applications, assuming the current location coordinates of the four-way shuttle robot on the warehouse map are [10.0 m, 5.0 m, 0.0 m], and the coordinates of its target shelf entrance are [50.0 m, 30.0 m, 0.0 m], then the starting node coordinates of the path search tree are [10.0, 5.0, 0.0], and the ending node coordinates are set to [50.0, 30.0, 0.0].

[0146] Step 1052: Randomly select a location point as a candidate point within the drivable area of ​​the four-way shuttle robot, and determine the tree node closest to the candidate point from the path search tree.

[0147] In step 1052, the drivable area refers to all the spatial range in the warehouse environment that the four-way shuttle robot is allowed to pass through, which is usually predefined by the management map; the candidate point is a coordinate point randomly generated within the drivable area to guide the growth direction of the path tree; the tree node is an existing node on the path search tree that represents the explored path point.

[0148] In each iteration of the algorithm, a three-dimensional coordinate point is first generated as a candidate point within the space of the entire warehouse map where the robot can travel. Then, the Euclidean distance from all existing nodes on the current path search tree to this random candidate point is calculated, and the existing node with the closest distance is selected. This node will be used as the starting point for this expansion.

[0149] In practical applications, assuming the coordinates of a randomly selected candidate point are [25.0 meters, 18.0 meters, 0.0 meters], and there are 3 nodes in the current path search tree with coordinates [10.0, 5.0, 0.0], [15.0, 8.0, 0.0], and [20.0, 12.0, 0.0], the node [20.0, 12.0, 0.0] has the smallest distance to the candidate point, for example, 9.8 meters. Therefore, this node is determined to be the nearest tree node.

[0150] Step 1053: Generate a new path segment and a new tree node in the direction from the nearest tree node to the candidate point, and perform dynamic conflict checking on the new path segment.

[0151] Here, a new path segment refers to a straight path extending from the nearest tree node towards the candidate point, and a new tree node refers to the end point of this new path segment.

[0152] Step 1053 may specifically include the following steps:

[0153] A1: When making an attempt, based on the motion information of the dynamic obstacles in the obstacle list, predict the motion trajectory of the dynamic obstacle during the time period when the new path segment may be executed.

[0154] In step A1, the possible execution time period refers to the time interval required for the four-way shuttle robot to travel along this new path segment; the predicted motion trajectory is a linear or nonlinear extrapolation of the movement path of the dynamic obstacle within this time period.

[0155] In this embodiment of the application, after a new path segment is generated, the time required for the four-way shuttle robot to travel the path at a preset cruising speed is first estimated to form a future time period. Then, the current velocity vectors of all dynamic obstacles are read from the obstacle list. Assuming that they maintain uniform linear motion during the time period, the movement path of each dynamic obstacle from the current moment to the end of the future time period is calculated, thus obtaining the preliminary predicted motion trajectory.

[0156] In practical applications, assuming the new path segment is 2.2 meters long and the four-way shuttle robot has a preset cruising speed of 1.0 meter per second, the travel time is 2.2 seconds. The obstacle list contains a dynamic obstacle A, currently located at [22.0, 10.0, 0.0], with a speed of 1.5 meters per second and a direction of due east. In this case, its predicted trajectory over the next 2.2 seconds is: starting from [22.0, 10.0, 0.0], moving 3.3 meters in a straight line due east.

[0157] A2: Introduce a trajectory prediction module based on LSTM network to analyze the historical motion state sequence of dynamic obstacles in the obstacle list in order to correct the predicted trajectory.

[0158] In step A2, the LSTM network trajectory prediction module is a trained long short-term memory recurrent neural network model used to process time series data and predict future values; the historical motion state sequence refers to the position, speed and other state data of the dynamic obstacle recorded at multiple consecutive moments in the past.

[0159] This application does not impose specific limitations on the model type, internal structure design, parameter design, training process, etc. of the trajectory prediction module based on LSTM network, and can be set accordingly according to the actual situation.

[0160] In this embodiment of the application, in order to further improve the prediction accuracy, the preliminary prediction result obtained in step A1 and the historical position and velocity sequence of the dynamic obstacle in the past few seconds are input into a pre-trained LSTM trajectory prediction module. This module can learn the motion pattern and consider possible acceleration, deceleration or turning, and output a corrected prediction trajectory that is closer to the real motion trend. This trajectory may be a curve rather than a simple straight line.

[0161] In practical applications, the LSTM module receives the position sequence of obstacle A over the past 5 time points as input, combines it with the preliminary linear prediction in step A1, and outputs the corrected predicted trajectory. This trajectory may show that obstacle A will move in a curved direction in the east-northeast direction within the next 2.2 seconds.

[0162] A3: Based on the corrected predicted trajectory, a cone-shaped conflict zone that changes over time within the time period is calculated for each dynamic obstacle using the speed obstacle method.

[0163] In step A3, the explanation and implementation process of the speed obstacle method can be referred to relevant technologies, and will not be repeated here; the cone-shaped conflict area is a three-dimensional cone space swept by the current position of the dynamic obstacle as the vertex and the range of its possible future positions as the generatrix. This space represents all positions that the obstacle may occupy within a specified time period.

[0164] In this embodiment of the application, for each dynamic obstacle, based on the direction of motion and speed range described by the predicted trajectory after step A2, the principle of the speed obstacle method is applied to calculate the spatial region formed by all possible positions that the obstacle can reach from the current moment to the end of a specified time period in the future. This region is usually represented by a cone-shaped volume that increases over time. This cone-shaped conflict area is the spatiotemporal range that needs to be avoided.

[0165] In practical applications, for obstacle A, a conical conflict zone is calculated based on its corrected predicted trajectory and velocity range. The cross-section of this zone expands linearly with time, for example, the radius is 0.3 meters at 0.5 seconds and 1.0 meters at 2.2 seconds, forming a conical space that expands eastward and northward from the point [22.0, 10.0, 0.0].

[0166] A4: Check whether the new path segment will conflict with any obstacle or the cone-shaped conflict area during the time period, and obtain the check results.

[0167] In step A4, checking for conflicts means determining whether the space occupied by the new path segment planned by the four-way shuttle robot will overlap with the space of static obstacles or the cone-shaped conflict area of ​​dynamic obstacles in time and space during the execution period.

[0168] In this embodiment of the application, the new path segment is modeled as a line segment that moves over time. The spatial position occupied by the four-way shuttle robot at each time point when it moves along this line segment is calculated. Then, it is determined whether this spatial position intersects with the known static obstacle spatial model or whether it intersects with the spatial range of the cone-shaped conflict area of ​​each dynamic obstacle calculated in step A3 at the corresponding time. If any intersection exists, the result is that there is a conflict; otherwise, no conflict is found.

[0169] In practical applications, the new path segment [20.0, 12.0, 0.0] to [21.0, 13.5, 0.0] is discretized into multiple points over time, and each point's corresponding robot bounding box is checked sequentially to see if it intersects with the model of the shelf or the cone-shaped conflict area of ​​obstacle A. If no intersection occurs at any time point, the check result is that no conflict was found.

[0170] Step 1054: If the check result is that no conflict is found, then add the new tree node and the new path segment to the path search tree.

[0171] In this embodiment of the application, if the result returned by the dynamic conflict check in step 1053 is safe, that is, no conflict is found, then the newly generated tree node and the path segment from the parent node to the tree node are considered valid. At this time, the new tree node is added as a child node to the path search tree, and it is connected to the nearest tree node with an edge representing the new path segment, thereby expanding the path search tree.

[0172] In practical applications, since the check result shows no conflict, the new node [21.0, 13.5, 0.0] is officially added to the path tree, and its parent node is [20.0, 12.0, 0.0]. The line between the two represents a safe candidate path segment.

[0173] Step 1055: Repeatedly execute the steps of random selection, determination, attempt generation, dynamic conflict checking and addition until the path search tree extends to the endpoint.

[0174] In this embodiment, steps 1052, 1053, and 1054 constitute one iterative loop of the algorithm, which needs to be executed repeatedly. In each iteration, random sampling is performed, the nearest node is found, an attempt is made to expand, and a strict dynamic conflict check is conducted. Only safe expansions are retained. The loop continues until a newly expanded node enters a spherical region with a preset radius centered on the target location. At this point, the path search tree is considered to have successfully extended to the endpoint.

[0175] In practical applications, after thousands of iterations, the algorithm generates thousands of nodes and path segments. Finally, a new node is located at coordinates [49.8, 30.1, 0.0]. The distance between this point and the target location [50.0, 30.0, 0.0] is less than the preset tolerance of 0.5 meters, and the loop terminates.

[0176] Step 1056: Extract the complete path connecting the starting point and the ending point from the path search tree as the collision avoidance path.

[0177] In this embodiment of the application, when the loop termination condition is met, the final node that enters the destination region is found from the path search tree. Then, starting from this node, the path is traced back along its parent node pointer in the tree until the starting node is reached. This broken line path connected by the node sequence is a complete anti-collision path from the starting point to the end point, which has undergone dynamic conflict checking.

[0178] In practical applications, starting from the final node [49.8, 30.1, 0.0], backtracking through the nodes [45.0, 28.0, 0.0], [40.0, 25.0, 0.0], ..., [10.0, 5.0, 0.0], and connecting these node coordinates in sequence, a complete collision avoidance path from the starting point to the ending point is obtained.

[0179] In this embodiment, after step 105, the method further includes the following steps:

[0180] B1: Using the gradient descent optimization algorithm to reduce the path curvature and acceleration change rate, the node positions of the anti-collision path are iteratively adjusted to obtain a smooth trajectory.

[0181] In step B1, path curvature is used to measure the degree of tortuosity of the path, and the rate of change of acceleration is used to measure the drastic change in acceleration when the robot moves along the path. A smooth trajectory refers to a path with gentle curvature that is kinematically easier for the robot to follow. This application embodiment does not limit the expression of the gradient descent optimization algorithm; it can be set according to the actual situation.

[0182] In this embodiment of the application, the anti-collision path obtained in step 1056 is regarded as an initial solution composed of a series of node coordinates. An objective function is defined, which simultaneously penalizes the total curvature of the path and the rate of change of acceleration of the nodes. Then, the gradient descent method is used to calculate the partial derivative of the objective function with respect to each node coordinate, and the position of each node is gradually adjusted according to a learning rate to continuously reduce the value of the objective function. After multiple iterations, a trajectory with optimized node positions and a smoother overall path is obtained.

[0183] In practical applications, the initial anti-collision path has 50 nodes. The optimization objective is to minimize the sum of squares of the angles of the changes in direction of adjacent path segments and the sum of squares of the node accelerations. After 100 gradient descent iterations, the node positions are fine-tuned, the sharp corners on the path are softened, and finally a smooth curved trajectory is obtained.

[0184] B2: Using the time-optimal trajectory planning model, under the conditions of satisfying the motor torque, hub speed and structural strength constraints of the four-way shuttle robot, the travel time is allocated to the smooth trajectory to determine the speed curve of each segment of the smooth trajectory. The smooth trajectory is combined with the corresponding speed curve to obtain the optimized collision avoidance path.

[0185] In step B2, the velocity curve describes the functional relationship between the robot's velocity and time or position as it moves along a smooth trajectory.

[0186] The explanation of the time-optimal trajectory planning model can be found in relevant technologies, and will not be repeated here; furthermore, this embodiment does not limit the content of each constraint.

[0187] In this embodiment of the application, an optimization problem with the goal of minimizing the total time is established for the smooth trajectory generated in step B1. The constraints include the maximum torque that the four-way shuttle robot motor can provide, the maximum speed allowed by the wheel hub, and the maximum acceleration that the vehicle body structure can withstand. Then, this constrained optimization problem is solved to allocate the optimal travel time to each path segment on the smooth trajectory, thereby determining the speed value that the robot should have at each path point. Finally, a time-optimal speed curve that matches the smooth trajectory is generated, and this smooth trajectory with the speed curve attached is used as the final executable and optimized collision avoidance path.

[0188] In practical applications, the maximum driving acceleration of the four-way shuttle robot is known to be 2 m / s², the maximum braking deceleration to be 3 m / s², and the linear velocity corresponding to the highest hub rotation speed to be 3 m / s. Using Pontryagin's maximum principle or numerical optimization methods, a velocity curve is obtained that allows the robot to accelerate to its maximum permissible speed on straightaways and decelerate in advance before curves, reducing the total travel time by 20% compared to constant speed travel.

[0189] This application realizes a collision avoidance path planning method through the above process, which can proactively avoid dynamic obstacles and generate smooth, efficient and executable trajectories, thereby improving the operational safety and efficiency of four-way shuttle robots in highly dynamic and complex warehouse environments.

[0190] Figure 3 This is a schematic diagram of the structure of a collision avoidance path planning system for a four-way shuttle robot provided in an embodiment of this application, as shown below. Figure 3 As shown, the system includes:

[0191] The acquisition module 31 is used to acquire three-dimensional point cloud data and robot posture data of the four-way shuttle robot. The robot posture data includes inertial measurement data and real-time pose information.

[0192] The correction module 32 is used to correct motion distortion of the three-dimensional point cloud data based on the inertial measurement data using a quaternion spherical linear interpolation algorithm.

[0193] The fusion module 33 is used to fuse the corrected 3D point cloud data with the real-time pose information to form fused environmental data.

[0194] Input module 34 is used to input the fused environment data into an improved PointNet++ network. The improved PointNet++ network analyzes the fused environment data, identifies dynamic obstacles in the environment, estimates the motion information of the dynamic obstacles, and outputs a list of obstacles containing the motion information.

[0195] The planning module 35 is used to perform path planning using a dynamic RRT algorithm based on the current position, target position, and obstacle list of the four-way shuttle robot. During the planning process, the dynamic RRT algorithm expands the path nodes by combining the predicted trajectories of dynamic obstacles in the obstacle list to plan an anti-collision path.

[0196] The collision avoidance path planning system for the four-way shuttle robot in this application embodiment is used to implement the aforementioned collision avoidance path planning method for the four-way shuttle robot. Therefore, the specific implementation of the collision avoidance path planning system for the four-way shuttle robot can be found in the embodiment section of the collision avoidance path planning method for the four-way shuttle robot mentioned above. The specific implementation can be referred to the description of the corresponding embodiments, which will not be repeated here.

[0197] This application also provides an electronic device, including: a memory for storing a computer program; and a processor for executing the computer program to implement the steps of the collision avoidance path planning method for any of the above-described four-way shuttle robots.

[0198] This application also provides a computer-readable storage medium storing a computer program, which, when executed by a processor, implements the steps of the collision avoidance path planning method for any of the four-way shuttle robots described above.

[0199] In one exemplary embodiment, the aforementioned computer-readable storage medium may include, but is not limited to, various media capable of storing computer programs, such as USB flash drives, read-only memory, random access memory, portable hard drives, magnetic disks, or optical disks.

[0200] The embodiments of this application also provide a computer program product, which includes a computer program that, when executed by a processor, implements the steps in any of the above embodiments of the anti-collision path planning method for a four-way shuttle robot.

[0201] Those skilled in the art will further recognize that the units and algorithm steps of the various examples described in conjunction with the embodiments disclosed herein can be implemented in electronic hardware, computer software, or a combination of both. To clearly illustrate the interchangeability of hardware and software, the components and steps of the various examples have been generally described in terms of functionality in the foregoing description. Whether these functions are implemented in hardware or software depends on the specific application and design constraints of the technical solution. Those skilled in the art can use different methods to implement the described functions for each specific application, but such implementation should not be considered beyond the scope of this application.

[0202] It should be noted that the user information (including but not limited to user device information, user personal information, etc.) and data (including but not limited to data used for analysis, data stored, data displayed, etc.) involved in one or more embodiments of this specification are all information and data authorized by the user or fully authorized by all parties. Furthermore, the collection, use and processing of related data must comply with relevant laws, regulations and standards, and corresponding operation entry points are provided for users to choose to authorize or refuse.

[0203] The above provides a detailed description of the collision avoidance path planning method and system for a four-way shuttle robot provided in this application. Specific examples have been used to illustrate the principles and implementation methods of this application. The descriptions of the embodiments above are only for the purpose of helping to understand the method and its core ideas. It should be noted that those skilled in the art can make several improvements and modifications to this application without departing from the principles of this application, and these improvements and modifications also fall within the protection scope of this application.

Claims

1. A collision avoidance path planning method for a four-way vehicle, characterized in that, include: Collect three-dimensional point cloud data and vehicle attitude data of the four-way vehicle, wherein the vehicle attitude data includes inertial measurement data and real-time pose information; Based on the inertial measurement data, the motion distortion of the three-dimensional point cloud data is corrected using a quaternion spherical linear interpolation algorithm; The corrected 3D point cloud data is fused with the real-time pose information to form fused environmental data; The fused environment data is input into an improved PointNet++ network. The improved PointNet++ network analyzes the fused environment data, identifies dynamic obstacles in the environment, estimates the motion information of the dynamic obstacles, and outputs a list of obstacles containing the motion information. Based on the current position and target position of the four-way vehicle and the obstacle list, a dynamic RRT algorithm is used for path planning. During the planning process, the dynamic RRT algorithm expands the path nodes by combining the predicted trajectories of dynamic obstacles in the obstacle list in order to plan an anti-collision path. The method involves using a dynamic RRT algorithm for path planning based on the current position of the four-way vehicle, the target position, and the obstacle list. During the planning process, the dynamic RRT algorithm expands the path nodes by incorporating the predicted trajectories of dynamic obstacles in the obstacle list to plan an anti-collision path, including: A dynamic RRT algorithm is used to construct a path search tree with the current position as the starting point and the target position as the ending point. A location point is randomly selected as a candidate point within the drivable area of ​​the four-way vehicle, and the tree node closest to the candidate point is determined from the path search tree. In the direction from the nearest tree node to the candidate point, a new path segment and a new tree node are generated, and dynamic conflict checks are performed on the new path segment. If the check result is that no conflict is found, the new tree node and the new path segment are added to the path search tree; The steps of random selection, determination, trial generation, dynamic conflict checking, and addition are executed repeatedly until the path search tree extends to the endpoint. Extract the complete path connecting the starting point and the ending point from the path search tree, and use it as the collision avoidance path.

2. The method according to claim 1, characterized in that, The dynamic conflict check of the new path segment includes: During the attempt, based on the motion information of the dynamic obstacles in the obstacle list, the motion trajectory of the dynamic obstacle is predicted within the time period during which the new path segment may be executed. An LSTM-based trajectory prediction module is introduced to analyze the historical motion state sequence of dynamic obstacles in the obstacle list in order to correct the predicted trajectory. Based on the corrected predicted trajectory, a cone-shaped conflict zone that varies with time within the time period is calculated for each dynamic obstacle using the speed obstacle method. The results of the inspection are obtained by checking whether the new path segment will conflict with any obstacles or the cone-shaped conflict area during the time period.

3. The method according to claim 1, characterized in that, The fused environment data is input into an improved PointNet++ network. The improved PointNet++ network analyzes the fused environment data, identifies dynamic obstacles in the environment, estimates the motion information of the dynamic obstacles, and outputs a list of obstacles containing the motion information, including: The fused environment data is input into the feature extraction module of the improved PointNet++ network. Through the multilayer perceptron and max pooling layer in the feature extraction module, the depth features of each point cloud point are extracted from the fused environment data frame by frame to form a point cloud feature map for each frame. The point cloud feature maps of multiple consecutive frames are input into the temporal attention module of the improved PointNet++ network. In the temporal attention module, the correlation weights of corresponding feature regions between different frames are calculated, and the features of multiple frames are weighted, fused, and context-associated based on the correlation weights to generate an enhanced feature map with temporal correlation. The enhanced feature map is input into the classification and regression module of the improved PointNet++ network. The fully connected layer and softmax layer in the classification and regression module are used to classify each feature region in the enhanced feature map to distinguish between static obstacles and dynamic obstacles. For key feature regions identified as dynamic obstacles, the motion information of the dynamic obstacle is determined by the regression sub-network in the classification and regression module based on the position offset of the key feature regions in the enhanced feature maps of consecutive frames. The improved PointNet++ network output module aggregates the categories, key information, and motion information of all obstacles to generate an obstacle list.

4. The method according to claim 1, characterized in that, The step of correcting motion distortion in the 3D point cloud data based on the inertial measurement data using a quaternion spherical linear interpolation algorithm includes: Based on the angular velocity and acceleration information in the inertial measurement data, and combined with the vehicle kinematics model, the vehicle motion of the four-way vehicle during the acquisition of the three-dimensional point cloud data is calculated. The vehicle motion includes angular change and displacement change. Based on the scanning cycle of the rotating lidar, the angle change and the displacement change are allocated to the acquisition time corresponding to each point cloud in the three-dimensional point cloud data to obtain the instantaneous motion at each acquisition time. Based on the instantaneous motion at each acquisition moment, the attitude interpolation at each acquisition moment relative to the acquisition start moment is calculated using the quaternion spherical linear interpolation algorithm, and combined with the displacement change, the coordinate transformation parameters corresponding to each acquisition moment are obtained. For each point cloud in the three-dimensional point cloud data, the original coordinates of the point cloud are inversely compensated according to the coordinate transformation parameters corresponding to the acquisition time, so as to obtain the corrected coordinates of the point cloud in the coordinate system at the acquisition start time of the three-dimensional point cloud data. The corrected coordinates of all the point clouds are aggregated to form the corrected 3D point cloud data.

5. The method according to claim 1, characterized in that, The process of fusing the corrected 3D point cloud data with the real-time pose information to form fused environmental data includes: Based on the predetermined coordinate transformation relationship between the rotating lidar and the four-way vehicle, the corrected three-dimensional point cloud data is transformed into the vehicle body coordinate system of the four-way vehicle. Obtain the position information, attitude information, and corresponding acquisition time of the four-way vehicle recorded in the real-time pose information; By using a state estimator based on Kalman filtering, the position information and the attitude information are smoothed and compensated to obtain optimized vehicle pose state information. The 3D point cloud data converted to the vehicle coordinate system is merged with the optimized vehicle pose state information corresponding to the acquisition time. The point cloud in the merged data is processed into a voxel grid to generate point cloud map blocks, and the point cloud map block corresponding to each acquisition time is used as a basic data unit. Multiple basic data units that are consecutive in time are aggregated to form fused environmental data.

6. The method according to claim 1, characterized in that, After planning the collision avoidance path, it also includes: The gradient descent optimization algorithm is used to reduce the path curvature and the rate of change of acceleration. The node positions of the anti-collision path are iteratively adjusted to obtain a smooth trajectory. Using a time-optimal trajectory planning model, under the conditions of satisfying the constraints of motor torque, wheel hub speed and structural strength of the four-way vehicle, the travel time is allocated to the smooth trajectory to determine the speed curve of each segment of the smooth trajectory. The smooth trajectory is then combined with the corresponding speed curve to obtain the optimized collision avoidance path.

7. A collision avoidance path planning system for a four-way vehicle, characterized in that, include: The acquisition module is used to acquire three-dimensional point cloud data and vehicle attitude data of the four-way vehicle. The vehicle attitude data includes inertial measurement data and real-time pose information. The correction module is used to correct motion distortion of the three-dimensional point cloud data based on the inertial measurement data using a quaternion spherical linear interpolation algorithm. The fusion module is used to fuse the corrected 3D point cloud data with the real-time pose information to form fused environmental data; An input module is used to input the fused environment data into an improved PointNet++ network. The improved PointNet++ network analyzes the fused environment data, identifies dynamic obstacles in the environment, estimates the motion information of the dynamic obstacles, and outputs a list of obstacles containing the motion information. The planning module is used to perform path planning using a dynamic RRT algorithm based on the current position of the four-way vehicle, the target position, and the obstacle list. During the planning process, the dynamic RRT algorithm expands the path nodes by combining the predicted trajectories of dynamic obstacles in the obstacle list to plan an anti-collision path. The method involves using a dynamic RRT algorithm for path planning based on the current position of the four-way vehicle, the target position, and the obstacle list. During the planning process, the dynamic RRT algorithm expands the path nodes by incorporating the predicted trajectories of dynamic obstacles in the obstacle list to plan an anti-collision path, including: A dynamic RRT algorithm is used to construct a path search tree with the current position as the starting point and the target position as the ending point. A location point is randomly selected as a candidate point within the drivable area of ​​the four-way vehicle, and the tree node closest to the candidate point is determined from the path search tree. In the direction from the nearest tree node to the candidate point, a new path segment and a new tree node are generated, and dynamic conflict checks are performed on the new path segment. If the check result is that no conflict is found, the new tree node and the new path segment are added to the path search tree; The steps of random selection, determination, trial generation, dynamic conflict checking, and addition are executed repeatedly until the path search tree extends to the endpoint. Extract the complete path connecting the starting point and the ending point from the path search tree, and use it as the collision avoidance path.

8. An electronic device, characterized in that, include: Memory, used to store computer programs; A processor, configured to implement the steps of the collision avoidance path planning method for a four-way vehicle as described in any one of claims 1 to 6 when executing the computer program.

9. A computer-readable storage medium, characterized in that, The computer-readable storage medium stores a computer program that, when executed by a processor, enables the implementation of the collision avoidance path planning method for a four-way vehicle as described in any one of claims 1 to 6.

Images