A trajectory planning method and system of an unmanned carrier applied to a dynamic narrow scene

Abstract

The application relates to the technical field of robot navigation, and discloses a trajectory planning method and system of an unmanned carrier applied to a dynamic narrow scene. The method perceives the surrounding environment of the carrier under the dynamic scene, plans and optimizes a safe and smooth motion trajectory, so that the dynamic narrow scene is realized, the carrier is driven to approach a target point, and the method has important application value in material carrying, local obstacle avoidance and the like.

Description

Technical Field

[0001] This invention relates to the field of robot navigation technology, and in particular to a trajectory planning method and system for unmanned transport vehicles applied in dynamic and narrow scenarios. Background Technology

[0002] Automated guided vehicles (AGVs) are indispensable high-efficiency handling equipment in modern logistics warehousing and manufacturing. With the continuous development of autonomous navigation technology, AGVs are gradually gaining public attention. Autonomous navigation is the core component of AGVs, providing technical support for the autonomous execution of tasks and further improving the automation and intelligence level of AGVs. As a core component of the autonomous navigation system, the local perception and planner aims to digitally depict the three-dimensional physical space environment within a local area, generating a smooth and safe drivable trajectory within a short range. This is a key research direction in academia and industry, and has significant application value in material handling and local obstacle avoidance.

[0003] While current local planners for automated guided vehicles (AGVs) are quite mature in handling static, open scenarios, they face dynamic changes and various uncertainties in complex, narrow, and variable environments. These uncertainties include sudden pedestrian movement, the appearance and disappearance of obstacles, and the shrinking of drivable space due to obstacle stacking. Current local planners still have technical bottlenecks and problems in highly dynamic, narrow scenarios, mainly in the following aspects:

[0004] 1. In the current field of navigation technology, classic local planning strategies generally rely on real-time perception technology, which integrates real-time scene information captured by multiple sensors to construct a local map model. Although this method has shown high efficiency and accuracy in handling static environments, its limitations are becoming increasingly apparent when facing more complex dynamic scenes.

[0005] Due to the inherent limitations of sensor field of view and the intermittent occlusion of objects in dynamic environments, some key obstacles may not be effectively and continuously detected, resulting in perception blind spots. Local real-time perception mechanisms are particularly inadequate in handling such dynamic changes because they rely solely on sensor data from the current frame, making it difficult to capture and integrate continuous changes over time. This locality and immediacy of information leads to biases in the constructed local maps reflecting the true state of the environment, failing to comprehensively and accurately describe the dynamic evolution of the surrounding environment.

[0006] 2. Classical mobile robot local planners such as DWA (Dynamic Window Method) and TEB (Time-Flexible Band) have certain limitations when dealing with highly complex and spatially confined environments. Due to the inherent limitations of the algorithm or the constraints of parameter settings, these algorithms get stuck in local optima in narrow scenarios, resulting in trajectories that are not smooth enough or have poor stability.

[0007] Local planning methods based on piecewise polynomial trajectory optimization are a practical technique in open environments, but they face challenges when applied to transport vehicles in confined spaces. Due to the high density of obstacles in narrow environments, the safe, collision-free discrete paths generated by this method may fail in subsequent optimization stages due to conflicts between real-time environmental information and other constraints, leading to unsolvable problems or poor-quality solutions, and potentially causing collisions. Summary of the Invention

[0008] This invention provides a trajectory planning method and system for unmanned transport vehicles applied in dynamic and narrow scenarios, in order to solve the problem in the prior art of lacking the ability to plan safe and smooth motion trajectories for unmanned transport vehicles in dynamic and narrow scenarios.

[0009] To achieve the above objectives, the present invention employs the following technical solution:

[0010] In a first aspect, the present invention provides a trajectory planning method for an unmanned transport vehicle applied in dynamic, narrow scenarios, comprising:

[0011] S1: Acquire point cloud data from a lidar sensor in a dynamic narrow scene, perform preprocessing on the point cloud data, and obtain preprocessed point cloud data;

[0012] S2: The coordinate transformation parameters of the preprocessed point cloud data are calculated by the normal distribution transformation registration algorithm. At the same time, a sliding window queue is introduced to store the point cloud data. By splicing and removing point cloud data from different frames, a local dynamic voxel map is constructed.

[0013] S3: Project the target point into the boundary of the local dynamic voxel map, and obtain the front-end segmented linear path based on the target point;

[0014] S4: Construct an ellipsoid based on the segmented linear path expansion at the front end, generate a polyhedron based on the ellipsoid, and optimize the calculation of the polyhedron to finally generate a safe driving area represented by a set of polyhedra;

[0015] S5: Construct a backend optimization model based on the safe driving area, and obtain the motion trajectory of the unmanned transport vehicle in a dynamic narrow scenario based on the backend optimization model.

[0016] Secondly, this application provides a trajectory planning system for an unmanned transport vehicle applied in dynamic narrow scenarios, including a memory, a processor, and a computer program stored in the memory and executable on the processor. When the processor executes the computer program, it implements the steps of the method described in the first aspect above.

[0017] Beneficial effects:

[0018] This application provides a trajectory planning method for unmanned transport vehicles applied in dynamic and narrow scenarios. By enabling the transport vehicle to perceive its surrounding environment in dynamic scenarios, it plans and optimizes a safe and smooth movement trajectory, thereby driving the transport vehicle to approach the target point in dynamic and narrow scenarios and improving the automation and intelligence level of the transport vehicle. Attached Figure Description

[0019] Figure 1 A flowchart illustrating a preferred embodiment of the trajectory planning method for an unmanned transport vehicle applied in a dynamic, narrow scenario according to the present invention;

[0020] Figure 2 This is a flowchart illustrating the overall process of implementing dynamic scene perception and local planning in a preferred embodiment of the present invention.

[0021] Figure 3 This is a flowchart of a preferred embodiment of the ground point filtering method of the present invention;

[0022] Figure 4 This is a schematic diagram of the ground point segmentation process according to a preferred embodiment of the present invention;

[0023] Figure 5 This is a comparison image of the ground point cloud segmentation before and after a preferred embodiment of the present invention;

[0024] Figure 6 This is a schematic diagram of a sliding window according to a preferred embodiment of the present invention;

[0025] Figure 7 This is a diagram illustrating the steps of constructing an ellipsoid according to a preferred embodiment of the present invention;

[0026] Figure 8 This is a diagram illustrating the polyhedron construction steps of a preferred embodiment of the present invention;

[0027] Figure 9 This is a schematic diagram of dynamic time allocation according to a preferred embodiment of the present invention;

[0028] Figure 10 The diagram shows the optimal trajectory generation effect of a preferred embodiment of the present invention. Detailed Implementation

[0029] The technical solution of the present invention will be clearly and completely described below. Obviously, the described embodiments are only some embodiments of the present invention, and not all embodiments. Based on the embodiments of the present invention, all other embodiments obtained by those skilled in the art without creative effort are within the scope of protection of the present invention.

[0030] Unless otherwise defined, the technical or scientific terms used in this invention shall have the ordinary meaning understood by one of ordinary skill in the art to which this invention pertains. The terms "first," "second," and similar terms used in this invention do not indicate any order, quantity, or importance, but are merely used to distinguish different components. Similarly, the terms "an" or "a" and similar terms do not indicate a quantity limitation, but rather indicate the presence of at least one. The terms "connected" or "linked" and similar terms are not limited to physical or mechanical connections, but can include electrical connections, whether direct or indirect. "Up," "down," "left," "right," etc., are used only to indicate relative positional relationships; when the absolute position of the described object changes, the relative positional relationship also changes accordingly.

[0031] Please see Figures 1-2 This application provides a trajectory planning method for unmanned transport vehicles applied in dynamic and narrow scenarios, including:

[0032] S1: Acquire point cloud data from a lidar sensor in a dynamic narrow scene, perform preprocessing on the point cloud data, and obtain preprocessed point cloud data;

[0033] In this optional implementation, the dynamic narrow scenario can be that there are moving obstacles in the passage, such as people, other moving robots, vehicles, etc., and the width of the passage that the robot can walk on is narrower than the width of the robot itself, about 1.5 to 2 times the width of the robot.

[0034] S2: The coordinate transformation parameters of the preprocessed point cloud data are calculated by the registration algorithm based on Normal Distributions Transform (NDT). At the same time, a sliding window queue is introduced to store the point cloud data. By stitching and removing point cloud data from different frames, a local dynamic voxel map is constructed.

[0035] S3: Project the target point into the boundary of the local dynamic voxel map, and obtain the front-end segmented linear path based on the target point;

[0036] S4: Construct an ellipsoid based on the segmented linear path expansion at the front end, generate a polyhedron based on the ellipsoid, and optimize the calculation of the polyhedron to finally generate a safe driving area represented by a set of polyhedra;

[0037] S5: Construct a backend optimization model based on the safe driving area, and obtain the motion trajectory of the unmanned transport vehicle in a dynamic narrow scenario based on the backend optimization model.

[0038] The aforementioned trajectory planning method for unmanned transport vehicles (ARTVs) applied in dynamic, confined environments addresses the lack of existing technologies capable of planning safe and smooth motion trajectories for ARTVs in such conditions. This method proposes a trajectory planning method for ARTVs in dynamic, confined environments. By enabling the transport vehicle to perceive its surroundings in dynamic scenarios, it plans and optimizes a safe and smooth motion trajectory, thereby driving the transport vehicle closer to the target point in such situations and improving the automation and intelligence level of the transport vehicle.

[0039] The steps of the above method will now be described in detail with reference to a complete embodiment.

[0040] 1. Acquire point cloud data from a lidar sensor in a dynamic, narrow scene; perform preprocessing on the point cloud data to obtain preprocessed point cloud data.

[0041] Step 1: Randomly select three points from the point cloud data of the lidar sensor, and construct a model using these three points that satisfies the following relationship:

[0042] ax + by + cz + d = 0;

[0043] Where a, b, c, d are the equation coefficients for constructing the 3D planar model; x, y, z are the coordinates of each point in the point cloud;

[0044] Step 2: Assuming that all points in the point cloud data of the lidar sensor, except for three randomly selected points, are the remaining points, calculate the distance from the remaining points to the model, compare the distance with a first set threshold, if it is less than the first set threshold, it is regarded as an interior point and saved; otherwise, it is included in the range of exterior points, and the number of interior points under the model is counted.

[0045] In this optional implementation, the first set threshold can be between 0.1m and 0.5m. This is only an example and is not a limitation. In other feasible implementations, it can also be set to other values, depending on the actual scenario and complexity.

[0046] Step 3: Repeat Step 1 and Step 2. If the number of interior points in the model is greater than the maximum number of interior points that have been saved, update the model and always retain the model parameter with the largest number of interior points.

[0047] Step 4: Repeat Step 1, Step 2, and Step 3 iteratively. Assuming the proportion of the interior point in the LiDAR sensor point cloud data is 0, and the probability of selecting a point as the interior point is 0 when the model uses k points each time. k The probability of selecting a point being an exterior point is 1-0. kCalculate the probability of failure after S iterations, satisfying the following relationship:

[0048] 1-o=(1-o k ) S ;

[0049] The final number of iterations required is determined, satisfying the following relationship:

[0050]

[0051] The model parameter with the most inliers is obtained through the iterative operation. These inliers are then used to estimate the model parameter again, resulting in the final point cloud data after segmenting the ground points. Figure 3 , Figure 4 , Figure 5 As shown;

[0052] Step 5: Perform range filtering on the point cloud data after segmenting the ground points. Let the coordinates of each point in a single frame of the point cloud data after filtering out the ground points be y. p The vector magnitude of each point coordinate is calculated, and the vector magnitude is compared with a second set threshold. If the vector magnitude is less than the second set threshold, it is retained; otherwise, it is omitted, and the preprocessed point cloud data is finally obtained.

[0053] In this optional implementation, the second set threshold can be 5m.

[0054] 2. The coordinate transformation parameters of the preprocessed point cloud data are calculated using the NDT registration algorithm. A sliding window queue is introduced to store the point cloud data. By stitching and removing points from different frames, a local dynamic voxel map is constructed.

[0055] Step 1: Using the product of the registration changes between the currently located preprocessed point cloud data and the two previous adjacent preprocessed point cloud data, the prediction matrix satisfies the following relationship:

[0056]

[0057] Where Q is the current pose matrix, and ΔT is the change between the two previous registrations. This is the pose matrix for the current frame;

[0058] If the current frame is the second frame, then let

[0059] Step 2: The preprocessed point cloud data is gridded to obtain processed grid data. The normal distribution probability density function is calculated for each processed grid data. The grid mean satisfies the following relationship:

[0060]

[0061] in, Let be the coordinate vector of each laser scanning point in the grid. is the mean vector of all points in the grid, used to represent the concentration of points in the grid, and m is the total number of laser points in the grid;

[0062] The covariance matrix Σ is calculated using the grid mean. i It satisfies the following relationship:

[0063]

[0064] in, Let be the mean vector of all points in the grid;

[0065] Normal probability density function for each grid cell The following relationship must be satisfied:

[0066]

[0067] Step 3: Use the product of probabilities for each point as the objective likelihood function, construct the maximum objective likelihood function and find the optimal transformation parameters, and achieve the optimal matching by finding the coordinate transformation relationship that maximizes the probability product, satisfying the following relationship:

[0068]

[0069] Wherein, Φ is the target likelihood function, used to evaluate the matching degree between the source point cloud and the target point cloud, k is the point cloud index, n is the total number of points in the source point cloud, and f(y') is the normal distribution probability density function.

[0070] The initial prediction of y' satisfies the following relationship:

[0071]

[0072] The optimal transformation parameter T satisfies the following relationship:

[0073] T = maxΦ;

[0074] Let the optimal transformation parameters at the previous time step be... The transform values ​​of the two nearest adjacent frames satisfy the following relationship:

[0075]

[0076] If we define the current frame as the second frame, then let ΔT = 1;

[0077] If the current frame is the first frame, then the optimal transform parameter T = 1;

[0078] Step 4: Using the preprocessed point cloud data and the optimal transformation parameters, transform the preprocessed point cloud data to the global coordinate system;

[0079] Step 5: Within the time threshold δ, set the total number of point clouds in the sliding window queue to M = {F}. i |i=t-δ,t-δ-1...,t} and a point cloud threshold φ, when the total number M of the queued point clouds exceeds the point cloud threshold φ, the registration algorithm will automatically remove redundant information starting from time t-δ according to the time threshold δ, such as Figure 6 As shown;

[0080] In this alternative implementation, δ can be 10, and φ can be 5000 in a 16-line radar.

[0081] Step 6: Represent the sliding window queue point cloud in the form of an octotree to construct a local dynamic voxel map;

[0082] The dynamic voxel map is constructed from an environment-aware sequence with a dynamic retention length of 10 frames.

[0083] 3. Project the target point onto the boundary of the local dynamic voxel map, and obtain the front-end segmented linear path based on the target point.

[0084] Step 1: Calculate the intersection point between the target point and the center of the local dynamic voxel map, and use the intersection point as the projected local target point X. goal ;

[0085] Step 2: Set the initial point X init With the local target point X goal Perform initialization operations and set the state sampling space B;

[0086] Where, the initial point X init This is the robot's current position.

[0087] Step 3: Obtain sampling point X using a random sampling strategy rand If the sampling point X rand If inside the obstacle, repeat this step;

[0088] Step 4: Traverse the sampling points X rand The nearest node X is obtained by finding the distance between it and all points in the already generated set of nodes τ. near From the nearest node X near At the sampling point X rand With the nearest node X near The sampling point X is approached by a step size StepSize. randThis generates a new node X. new If the new node X new With the nearest node X near The line segment E i If there is an intersection with an obstacle, it means that the path is blocked and path generation failed, and this step needs to be repeated.

[0089] Among them, 0 <StepSize<1;

[0090] Step 5: For the new node X new By iterating repeatedly, a randomly expanded tree is generated to test the new node X. new Has the local target point X been reached? goal If the target is nearby, then find a path in the randomly expanded tree that originates from the initial point X. init To local target point X goal The path is used as the final front-end segmented linear path;

[0091] In this optional implementation, the local target point X goal The vicinity can be a local target point X. goal Within a 1m range.

[0092] 4. Construct an ellipsoid based on the segmented linear path expansion at the front end, generate a polyhedron based on the ellipsoid, and optimize the calculation of the polyhedron to finally generate a safe driving area represented by a set of polyhedra.

[0093] Step 1: Generate a series of new nodes X new Represented as P = {p0, p1, p2, ..., p} n}, from point Pi to point P i+1 The linear path is represented as L i ={P i →P i+1};

[0094] Among them, P i For a new node X new ;

[0095] Step 2: Using two Ps i The length of the linear path L between points is the diameter, and an initial sphere with the same length on all three axes is constructed.

[0096] Step 3: Fix the x-axis length of the initial sphere and make it completely coincide with L, then scale the initial sphere to obtain an ellipsoid;

[0097] Step 4: Traverse all obstacles and find the point closest to the center of the ellipsoid. And adjust the lengths of the other two axes so that the ellipsoid contacts the point. Obtain the ellipsoid ζ i ,like Figure 7 As shown in (a);

[0098] Step 5: On the ellipsoid ζ i Generate a hyperplane on the cut surface. like Figure 8 As shown in (a), after calculating the hyperplane H i After setting the parameters, delete all filtered point cloud data outside the specified range, such as... Figure 8 As shown in (a);

[0099] Where p is the coordinate vector of a point, representing the coordinates of the point whose location within the polyhedron needs to be verified; A is a matrix containing the parameters of each hyperplane, where each column represents the normal vector of a hyperplane, i.e., the normal vector of each hyperplane H. i The direction; each element of vector b represents the hyperplane H. i The offset;

[0100] Among them, vector b determines the specific position of the hyperplane in the coordinate system;

[0101] Step 6: Repeat Step 4 and Step 5 to find the new nearest point. And generate a new ellipsoid ζ i+1 ,like Figure 7 As shown in (b), a new hyperplane H is generated simultaneously. i+1 ,like Figure 8 As shown in (b);

[0102] Step 7: Repeat Step 6, assuming that after a set number of iterations, the final ellipsoid ζ is obtained. n ,like Figure 7 As shown in (c), the hyperplane sequence H = {H1, H2, H3, ..., H} is obtained simultaneously. n If the set number of hyperplanes intersect, a polyhedron can be obtained, such as... Figure 8 As shown in (c), the set of polyhedra satisfies the following relationship:

[0103]

[0104] Where A is a matrix containing the parameters of each hyperplane, and b is a matrix containing the offset of each hyperplane;

[0105] a i and b i Let A and B be the i-th column element of matrix A and the i-th element of vector B, respectively, satisfying the following relationship:

[0106]

[0107] Where E represents the shape matrix of the ellipsoid. Let d be the coordinates of the point in contact with the ellipsoid during the expansion process, d be the coordinates of the center point of the ellipsoid, and a be the coordinates of the point in contact with the ellipsoid. i For the hyperplane H i The normal vector;

[0108] Step 8: Based on the polyhedron set C, obtain the following relationship for any point x within the polyhedron:

[0109] Ax≤b;

[0110] Where A is a matrix containing the parameters of each hyperplane;

[0111] The region consisting of all points x that satisfy Ax≤b is the safe driving area.

[0112] 5. Construct a backend optimization model based on the safe driving area, and obtain the motion trajectory of the unmanned transport vehicle in a dynamic narrow scenario based on the backend optimization model.

[0113] Step 1: Determine the objective function to be optimized, satisfying the following relationship:

[0114]

[0115] Where, j n Let ||j| be the jerk at the nth time step, where N is the total number of discrete time steps. n || 2 Let be the square norm of the accelerometer at the nth time step;

[0116] In this optional implementation, N can be 10;

[0117] Step 2: Let r be the control point of the nth segment of the third-order Bézier curve. n0 ,r n1 ,r n2 ,r n3 By utilizing the convex hull property of Bézier curves, the control points are restricted to a safe space. Therefore, the trajectory constrained by these control points must also be within the safe space. Let the binary optimization variable b... np The control point representing the nth trajectory segment falls into the pth polyhedron of the safe driving area, and the binary optimization variable b np The following relationship must be satisfied:

[0118]

[0119] Among them, A p c is the parameter matrix for each hyperplane of the p-th polyhedron; p Let be the boundary value of each hyperplane of the p-th polyhedron;

[0120] For the binary optimization variable b np Applying linear inequality constraints to the safety corridor, the following relationship is satisfied:

[0121]

[0122] Step 3: Apply the deterministic positional constraint of the endpoints of the overall trajectory to the segmented linear path at the front end, satisfying the following relationship:

[0123]

[0124] Where x0(0) is the trajectory point at the initial time t=0, x N-1 (dt) represents the trajectory point at the final time t = dt;

[0125] Step 4: Apply vehicle dynamics inequality constraints to the segmented linear path at the front end, satisfying the following relationship:

[0126]

[0127] Among them, v n (0) ∞ Let a be the initial velocity of the nth path segment. n (0) ∞ Let j be the initial acceleration of the nth path segment. n∞ v is the jerk of the nth path segment. max For the maximum permissible speed, a max For the maximum allowable acceleration, j max For the maximum permissible jerk, For any;

[0128] Step 5: Apply segmented trajectory continuity constraints to the segmented linear path at the front end.

[0129] x n+1 (0)=x n (dt)n=0:N-2;

[0130] Where, x n+1 (0) represents the initial position of the (n+1)th path segment, x n (dt) represents the endpoint of the nth path segment, and N represents the number of path segments;

[0131] Step 6: Dynamically adjust the time allocation, that is, make a reasonable estimate of time dt. The allocation of each time interval dt satisfies the following relationship:

[0132]

[0133] in, and These are adding constraints v max and a max Then, there exists a constant input motion solution on each axis i = {x, y, z}; η ≥ 1 is the time dynamic adjustment factor, the value of which depends on the solution of the previous attempt dt; the factor that is specifically feasible and convergent during the (k-1)th planning is expressed as η. con,k-1 In the k-th planning iteration, the dynamic adjustment factor will be adjusted from the interval [η]. con,k-1 -ε,η con,k-1 The values ​​in [+ε′] are taken in ascending order until the mixed integer quadratic programming problem converges.

[0134] Among them, ε and ε′ are determined by specific experiments;

[0135] By dynamically adjusting time allocation, such as Figure 9 As shown, the optimized trajectory of the unmanned transport vehicle in a dynamic, narrow scenario is a safe and smooth movement. Figure 10 As shown.

[0136] This application also provides a trajectory planning system for an unmanned transport vehicle applied in dynamic, narrow scenarios, including a memory, a processor, and a computer program stored in the memory and executable on the processor. When the processor executes the computer program, it implements the steps of the above-described method. This trajectory planning system for an unmanned transport vehicle applied in dynamic, narrow scenarios can implement various embodiments of the above-described method and achieve the same beneficial effects; further details are omitted here.

[0137] The preferred embodiments of the present invention have been described in detail above. It should be understood that those skilled in the art can make numerous modifications and variations based on the concept of the present invention without creative effort. Therefore, all technical solutions that can be obtained by those skilled in the art based on the concept of the present invention through logical analysis, reasoning, or limited experimentation on the basis of existing technology should be within the scope of protection defined by the claims.

Claims

1. A trajectory planning method for unmanned transport vehicles applied in dynamic and confined scenarios, characterized in that, include: S1: Acquire point cloud data from a lidar sensor in a dynamic narrow scene, perform preprocessing on the point cloud data, and obtain preprocessed point cloud data; S2: The coordinate transformation parameters of the preprocessed point cloud data are calculated by the normal distribution transformation registration algorithm. At the same time, a sliding window queue is introduced to store the point cloud data. By splicing and removing point cloud data from different frames, a local dynamic voxel map is constructed. S3: Project the target point into the boundary of the local dynamic voxel map, and obtain the front-end segmented linear path based on the target point; S4: Construct an ellipsoid based on the segmented linear path expansion at the front end, generate a polyhedron based on the ellipsoid, and optimize the calculation of the polyhedron to finally generate a safe driving area represented by a set of polyhedra; S5: Construct a backend optimization model based on the safe driving area, and obtain the motion trajectory of the unmanned transport vehicle in a dynamic narrow scenario based on the backend optimization model; S3 includes: S31: Calculate the intersection point between the target point and the center of the local dynamic voxel map, and use the intersection point as the projected local target point X. goal ; S32: For the initial point X init With the local target point X goal Perform initialization operations and set the state sampling space B; where the initial point X init This is the robot's current position. S33: Obtain sampling point X through a random sampling strategy rand If the sampling point X rand If inside the obstacle, repeat this step; S34: Traverse the sampling points X rand The nearest node X is obtained by finding the distance between it and all points in the already generated set of nodes τ. near From the nearest node X near At the sampling point X rand With the nearest node X near The sampling point X is approached by a step size StepSize. rand This generates a new node X. new If the new node X new With the nearest node X near The line segment E i If the path intersects with an obstacle, it means the path is blocked and path generation failed, and this step needs to be repeated; where 0 < StepSize < 1; S35: For the new node X new By iterating repeatedly, a randomly expanded tree is generated to test the new node X. new Has the local target point X been reached? goal If the target is nearby, then find a path in the randomly expanded tree that originates from the initial point X. init To local target point X goal The path is used as the final front-end segmented linear path; S4 includes: S41: A series of new nodes X will be generated. new Represented as P = {p0, p1, p2, ..., p} n }, from point P i To point P i+1 The linear path is represented as L i ={P i →P i+1 }; where P i For a new node X new ; S42: With two P i The length of the linear path L between points is the diameter, and an initial sphere with the same length on all three axes is constructed. S43: Fix the x-axis length of the initial sphere and make it completely coincide with L, then scale the initial sphere to obtain an ellipsoid; S44: Traverse all obstacles and find the point closest to the center of the ellipsoid. And adjust the lengths of the other two axes so that the ellipsoid contacts the point. Obtain the ellipsoid ζ i ; S45: In the ellipsoid ζ i A hyperplane H is generated on the cross surface. i ={p|A T p < b}, in calculating the hyperplane H i After setting the parameters, delete all filtered point cloud data outside the hyperplane. Where p is the coordinate vector of a point, representing the coordinates of the point whose location within the polyhedron needs to be verified; A is a matrix containing the parameters of each hyperplane, where each column represents the normal vector of a hyperplane, i.e., the normal vector of each hyperplane H. i The direction; each element of vector b represents the hyperplane H. i The offset; S46: Repeat S44 and S45 to find the new nearest point. And generate a new ellipsoid ζ i+1 At the same time, a new hyperplane H is generated. i+1 ; S47: Repeat S46, assuming that after n iterations, the final ellipsoid ζ is obtained. n Simultaneously, the hyperplane sequence H = {H1, H2, H3, ..., H} is obtained. n If the set number of hyperplanes intersect, a polyhedron can be obtained, and the set of polyhedra satisfies the following relationship: Where A is a matrix containing the parameters of each hyperplane, and b is a matrix containing the offset of each hyperplane; a i and b i Let A and B be the i-th column element of matrix A and the i-th element of vector B, respectively, satisfying the following relationship: Where E represents the shape matrix of the ellipsoid. Let d be the coordinates of the point in contact with the ellipsoid during the expansion process, d be the coordinates of the center point of the ellipsoid, and a be the coordinates of the point in contact with the ellipsoid. i For the hyperplane H i The normal vector; S48: Based on the set of polyhedra C, any point x within the polyhedra satisfies the following relationship: Ax≤b; Where A is a matrix containing the parameters of each hyperplane; The region consisting of all points x that satisfy Ax≤b is designated as the safe driving area.

2. The trajectory planning method for unmanned transport vehicles applied in dynamic narrow scenarios according to claim 1, characterized in that, S1 includes: S11: Randomly select three points from the point cloud data of the lidar sensor, and construct a model using these three points that satisfies the following relationship: ax + by + cz + d = 0; Where a, b, c, d are the equation coefficients for constructing the 3D planar model; x, y, z are the coordinates of each point in the point cloud; S12: Assuming that all points in the point cloud data of the lidar sensor, except for three randomly selected points, are the remaining points, calculate the distance from the remaining points to the model, compare the distance with a first set threshold, if it is less than the first set threshold, it is regarded as an interior point and saved; otherwise, it is included in the range of exterior points, and the number of interior points under the model is counted. S13: Repeat S11 and S12. If the number of interior points in the model is greater than the maximum number of interior points already saved, update the model and always retain the model parameter with the largest number of interior points. S14: Repeat S11, S12, and S13 for iterative operations. Assuming the proportion of the interior point in the lidar sensor point cloud data is o, and the probability of selecting a point as the interior point is o when the model uses k points each time. k The probability of selecting a point being an exterior point is 1-0. k Calculate the probability of failure after S iterations, satisfying the following relationship: 1-o=(1-o k ) S ; The final number of iterations required is determined, satisfying the following relationship: The model parameter with the most inliers is obtained through the iterative operation, and the model parameter is estimated again using the inliers to obtain the final point cloud data after segmenting the ground points. S15: Perform range filtering on the point cloud data after segmenting the ground points. Let the coordinates of each point in a single frame of the point cloud data after filtering out the ground points be y. p The vector magnitude of each point coordinate is calculated, and the vector magnitude is compared with a second set threshold. If the vector magnitude is less than the second set threshold, it is retained; otherwise, it is omitted, and the preprocessed point cloud data is finally obtained.

3. The trajectory planning method for unmanned transport vehicles applied in dynamic narrow scenarios according to claim 1, characterized in that, S2 includes: S21: Using the point cloud after segmenting the ground, calculate the coordinate transformation parameters of the point cloud based on the normal distribution transformation registration algorithm, where the source point cloud is the current frame point cloud and the target point cloud is a local dynamic incremental point cloud; S22: Using the product of the registration changes of the currently located point cloud data and the two previous adjacent point cloud data as the initial registration prediction, the prediction matrix satisfies the following relationship: Where Q is the current pose matrix, and ΔT is the change between the two previous registrations. This is the pose matrix for the current frame; If the current frame is the second frame, then let S23: The preprocessed point cloud data is gridded to obtain processed grid data. The normal distribution probability density function is calculated for each processed grid data. The grid mean satisfies the following relationship: in, Let be the coordinate vector of each laser scanning point in the grid. is the mean vector of all points in the grid, used to represent the concentration of points in the grid, and m is the total number of laser points in the grid; The covariance matrix Σ is calculated using the grid mean. i It satisfies the following relationship: in, Let be the mean vector of all points in the grid; Calculate the normal probability density function for each grid cell. The following relationship must be satisfied: S24: Using the product of probabilities at each point as the objective likelihood function, construct the maximum objective likelihood function and find the optimal transformation parameters. Then, achieve the optimal matching by finding the coordinate transformation relationship that maximizes the product of probabilities, satisfying the following relationship: Where Φ is the target likelihood function, used to evaluate the matching degree between the source point cloud and the target point cloud, k is the point cloud number index, n is the total number of points in the source point cloud, and f(y') is the normal distribution probability density function. The initial prediction of y' satisfies the following relationship: The optimal transformation parameter T satisfies the following relationship: T = maxΦ; Let the optimal transformation parameters at the previous time step be... The transform values ​​of the two nearest adjacent frames satisfy the following relationship: If we define the current frame as the second frame, then let ΔT = 1; If the current frame is the first frame, then the optimal transform parameter T = 1; S25: Using the preprocessed point cloud data and the optimal transformation parameters, transform the preprocessed point cloud data to the global coordinate system; S26: Within the time threshold δ, set the total number of point clouds in the sliding window queue to M = {F}. i |i=t-δ,t-δ-1...,t} and point cloud threshold φ, when the total number M of the queue point clouds exceeds the point cloud threshold φ, the registration algorithm will automatically remove redundant information starting from time t-δ according to the time threshold δ; S27: Represent the sliding window queue point cloud in the form of an octotree and construct a local dynamic voxel map.

4. The trajectory planning method for unmanned transport vehicles applied in dynamic narrow scenarios according to claim 1, characterized in that, S5 includes: S51: Determine the objective function to be optimized, satisfying the following relationship: Where, j n Let |j| be the acceleration at the nth time step, and N be the total number of discrete time steps. n || 2 Let be the square norm of the acceleration at the nth time step; S52: Let r be the control point of the nth segment of the third-order Bézier curve. n0 ,r n1 ,r n2 ,r n3 By utilizing the convex hull property of Bézier curves, the control points are restricted to a safe space. Therefore, the trajectory constrained by these control points must also be within the safe space. Let the binary optimization variable b... np The control point representing the nth trajectory segment falls into the pth polyhedron of the safe driving area, and the binary optimization variable b np The following relationship must be satisfied: Among them, A p c is the parameter matrix for each hyperplane of the p-th polyhedron; p Let be the boundary value of each hyperplane of the p-th polyhedron; For the binary optimization variable b np Applying linear inequality constraints to the safety corridor, the following relationship is satisfied: S53: Apply the overall trajectory endpoint deterministic position equation constraint to the aforementioned front-end segmented linear path, satisfying the following relationship: Where x0(0) is the trajectory point at the initial time t=0, x N-1 (dt) represents the trajectory point at the final time t = dt; S54: Apply vehicle dynamics inequality constraints to the segmented linear path at the front end, satisfying the following relationship: Among them, v n (0) ∞ Let a be the initial velocity of the nth path segment. n (0) ∞ Let j be the initial acceleration of the nth path segment. n∞ Let v be the acceleration of the nth path segment. max For the maximum permissible speed, a max For the maximum allowable acceleration, j max For the maximum permissible acceleration, For any; S55: Apply segmented trajectory continuity constraints to the segmented linear path at the front end, satisfying the following relationship: x n+1 (0)=x n (dt)n=0:N-2; Where, x n+1 (0) represents the initial position of the (n+1)th path segment, x n (dt) represents the endpoint of the nth path segment, and N represents the number of path segments; S56: Dynamically adjusting the time allocation, i.e., making a reasonable estimate of time dt, the allocation of each time interval dt satisfies the following relationship: in, and These are adding constraints v max and a max Then, there exists a constant input motion solution on each axis i = {x, y, z}; η is the time dynamic adjustment factor; By dynamically adjusting the time allocation, the movement trajectory of the unmanned transport vehicle in a dynamic and confined scenario is optimized.

5. A trajectory planning system for an unmanned transport vehicle applied in dynamic, narrow scenarios, comprising a memory, a processor, and a computer program stored in the memory and executable on the processor, characterized in that, When the processor executes the computer program, it implements the steps of the method described in any one of claims 1 to 4.

Images