Track planning method and system of automatic guided vehicle applied to dynamic narrow scene

Abstract

The invention relates to the technical field of robot navigation, and discloses a track planning method and system for an automated guided vehicle applied to a dynamic narrow scene, and the method comprises the steps: sensing the surrounding environment through the automated guided vehicle in the dynamic scene, planning and optimizing the surrounding environment into a safe and smooth motion track, and achieving the purpose of achieving the dynamic narrow scene. And the carrying vehicle is driven to approach a target point, and the important application value is achieved in material carrying, local obstacle avoidance and the like.

Description

Technical Field

[0001] The present invention relates to the field of robot navigation technology, and in particular to a trajectory planning method and system for an unmanned guided vehicle applied to dynamic narrow scenes. Background Art

[0002] Unmanned guided vehicles are an indispensable and efficient handling equipment in modern logistics, warehousing and manufacturing. With the continuous development of autonomous navigation technology, unmanned guided vehicles are gradually coming into the public eye. Autonomous navigation is the core part of unmanned guided vehicles, providing technical support for the autonomous execution of tasks by the guided vehicles, further improving the automation and intelligence level of the guided vehicles. As the core component of the autonomous navigation system, the local perception and planner aims to digitally depict the three-dimensional physical space environment in the local space and generate a smooth, safe and drivable trajectory in the short term. It is a key research direction in academia and industry, and has important application value in material handling, local obstacle avoidance and other conveniences.

[0003] Although the current local planners for unmanned guided vehicles are very mature in dealing with static and open scenes, they face dynamic changes and various uncertainties in complex, narrow and changing environments, such as the sudden movement of pedestrians, the appearance and disappearance of obstacles, and the shrinkage of drivable space caused by the stacking of obstacles. The current local planners still have technical bottlenecks and problems in highly dynamic and narrow scenes, which are mainly manifested in the following aspects:

[0004] 1. In the current field of navigation technology, the classic local planning strategy generally relies on real-time perception technology, which integrates the real-time scene information captured by multiple sensors to build a local map model. Although this method shows high efficiency and accuracy when dealing with static environments, its limitations gradually become apparent when facing more complex dynamic scenes.

[0005] Due to the inherent limitations of the sensor's field of view and the intermittent occlusion of objects in a dynamic environment, some key obstacles may not be effectively and continuously detected, resulting in the existence of perception blind spots. The local real-time perception mechanism is particularly inadequate in dealing with such dynamic changes, because it only relies on the sensor data of the current frame and has difficulty capturing and integrating continuous changes in time series. The locality and immediacy of this information make the constructed local map have deviations in reflecting the real environment state and cannot fully and accurately describe the dynamic evolution of the surrounding environment.

[0006] 2. Classic local planners for mobile robots, such as DWA (dynamic window method) and TEB (time elastic band), have certain limitations when dealing with highly complex and narrow environments. These algorithms fall into the dilemma of local optimal solutions in narrow scenes due to the limitations of the algorithm nature or parameter settings, and then generate trajectories that are not smooth or have poor stability.

[0007] The local planning method based on piecewise polynomial trajectory optimization representation is a relatively practical technology in open scenes, but it also faces challenges for the application of transport vehicles in narrow scenes. Due to the high density of the location and distribution of obstacles in narrow environments, the safe and collision-free front-end discrete path generated by this method may conflict with other constraints due to real-time environmental information in the subsequent optimization stage, resulting in no solution to the optimization problem or low quality of the solution, thus causing potential collision problems. Summary of the invention

[0008] The present invention provides a trajectory planning method and system for an unmanned guided vehicle applied to dynamic narrow scenes, so as to solve the problem in the prior art that there is a lack of the ability to plan a safe and smooth motion trajectory of an unmanned guided vehicle in dynamic narrow scenes.

[0009] In order to achieve the above object, the present invention is implemented by the following technical solutions:

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

[0011] S1: acquiring point cloud data of a laser radar sensor in a dynamic narrow scene, and performing preprocessing on the point cloud data to obtain preprocessed point cloud data;

[0012] S2: Calculate coordinate transformation parameters of the preprocessed point cloud data by using a normal distribution transformation registration algorithm, introduce a sliding window queue to store the point cloud data, and construct a local dynamic voxel map by splicing and eliminating point cloud data of different frames;

[0013] S3: Projecting the target point into the boundary of the local dynamic voxel map, and obtaining a front-end piecewise linear path based on the target point;

[0014] S4: constructing an ellipsoid based on the front-end piecewise linear path expansion, generating a polyhedron based on the ellipsoid, and optimizing and calculating the polyhedron to finally generate a safe driving area represented by a polyhedron set;

[0015] S5: constructing a back-end optimization model based on the safe driving area, and obtaining a motion trajectory of the unmanned guided vehicle in a dynamic narrow scene based on the back-end optimization model.

[0016] In a second aspect, the present application provides a trajectory planning system for an unmanned guided vehicle applied to dynamic narrow scenes, comprising a memory, a processor, and a computer program stored in the memory and executable on the processor, wherein the processor implements the steps of the method described in the first aspect when executing the computer program.

[0017] Beneficial effects:

[0018] The present application provides a trajectory planning method for an unmanned guided vehicle applied to dynamic narrow scenes. The guided vehicle senses the surrounding environment in the dynamic scene, plans and optimizes it into a safe and smooth motion trajectory, thereby realizing the dynamic narrow scene and driving the guided vehicle to approach the target point, thereby improving the automation and intelligence level of the guided vehicle. BRIEF DESCRIPTION OF THE DRAWINGS

[0019] Figure 1 It is a flow chart of a trajectory planning method for an unmanned guided vehicle applied to a dynamic narrow scene according to a preferred embodiment of the present invention;

[0020] Figure 2 The overall flow chart of realizing dynamic scene perception and local planning of the preferred embodiment of the present invention;

[0021] Figure 3 A flow chart of ground point filtering according to a preferred embodiment of the present invention;

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

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

[0024] Figure 6 A schematic diagram of a sliding window in a preferred embodiment of the present invention;

[0025] Figure 7 A diagram showing the steps for constructing an ellipsoid according to a preferred embodiment of the present invention;

[0026] Figure 8 A diagram showing the steps of constructing a polyhedron according to a preferred embodiment of the present invention;

[0027] Fig. 9 A schematic diagram of dynamic time allocation according to a preferred embodiment of the present invention;

[0028] Fig.10 This is a diagram showing the optimal trajectory generation effect of a preferred embodiment of the present invention. DETAILED DESCRIPTION

[0029] The technical solution of the present invention is described clearly and completely below. Obviously, the described embodiments are only part of the embodiments of the present invention, not all of the embodiments. Based on the embodiments of the present invention, all other embodiments obtained by ordinary technicians in this field without creative work are within the scope of protection of the present invention.

[0030] Unless otherwise defined, the technical terms or scientific terms used in the present invention shall have the usual meanings understood by persons with ordinary skills in the field to which the present invention belongs. The words "first", "second" and similar words used in the present invention do not indicate any order, quantity or importance, but are only used to distinguish different components. Similarly, words such as "one" or "one" do not indicate quantity restrictions, but indicate the existence of at least one. Words such as "connect" or "connected" are not limited to physical or mechanical connections, but may include electrical connections, whether direct or indirect. "Up", "down", "left", "right" and the like are only used to indicate relative positional relationships. When the absolute position of the object being described changes, the relative positional relationship also changes accordingly.

[0031] See also Figure 1-Figure 2 , the present application provides a trajectory planning method for an unmanned guided vehicle applied to a dynamic narrow scene, comprising:

[0032] S1: acquiring point cloud data of a laser radar sensor in a dynamic narrow scene, and performing preprocessing on the point cloud data to obtain preprocessed point cloud data;

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

[0034] S2: Calculate the coordinate transformation parameters of the preprocessed point cloud data by using a normal distribution transform (NDT) registration algorithm, introduce a sliding window queue to store the point cloud data, and construct a local dynamic voxel map by splicing and eliminating point cloud data of different frames;

[0035] S3: Projecting the target point into the boundary of the local dynamic voxel map, and obtaining a front-end piecewise linear path based on the target point;

[0036] S4: constructing an ellipsoid based on the front-end piecewise linear path expansion, generating a polyhedron based on the ellipsoid, and optimizing and calculating the polyhedron to finally generate a safe driving area represented by a polyhedron set;

[0037] S5: constructing a back-end optimization model based on the safe driving area, and obtaining a motion trajectory of the unmanned guided vehicle in a dynamic narrow scene based on the back-end optimization model.

[0038] The above-mentioned trajectory planning method for an unmanned guided vehicle applied to dynamic narrow scenes proposes a trajectory planning method for an unmanned guided vehicle applied to dynamic narrow scenes to address the problem in the prior art that there is a lack of a safe and smooth motion trajectory for an unmanned guided vehicle in dynamic narrow scenes. The method enables the guided vehicle to perceive the surrounding environment in a dynamic scene, plan and optimize it into a safe and smooth motion trajectory, thereby realizing a dynamic narrow scene and driving the guided vehicle to approach the target point, thereby improving the automation and intelligence level of the guided vehicle.

[0039] The steps of the above method are described in detail below with a complete embodiment.

[0040] 1. Obtain the point cloud data of the laser radar sensor in a dynamic narrow scene, perform preprocessing on the point cloud data, and obtain the preprocessed point cloud data

[0041] Step 1: Randomly select three points in the laser radar sensor point cloud data, and build a model through these three points to satisfy the following relationship:

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

[0043] Among them, a, b, c, d are the coefficients of the equation for constructing the three-dimensional plane model; x, y, z are the coordinates of each point in the point cloud;

[0044] Step 2: Assume that all points except three randomly selected points in the laser radar sensor point cloud data are remaining points, calculate the distance between the remaining points and the model, compare the distance with a first set threshold, and if it is less than the first set threshold, it is regarded as an inner point and saved, otherwise it is classified into the outer point range, and the number of inner points under the model is counted;

[0045] In this optional implementation, the first set threshold may be between 0.1m and 0.5m. This is only an example and not a limitation. In other feasible implementations, it may 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 inliers in the model is greater than the maximum number of inliers that has been saved, update the model and always keep the model parameters with the largest number of inliers.

[0047] Step 4: Repeat Step 1, Step 2 and Step 3 for iterative operation. Assume that the proportion of the inliers in the point cloud data of the laser radar sensor is o. When the model uses k points each time, the probability of selecting a point as the inliers is o. k , the probability that the selected point is an external point is 1-o k, calculate the probability of failure of iteration S times, satisfying the following relationship:

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

[0049] Determine the final number of iterations required to satisfy the following relationship:

[0050]

[0051] The model parameters with the largest number of inliers are obtained through the iterative operation, and the model parameters are estimated again using the inliers to obtain the final point cloud data after segmenting the ground points, such as Figure 3 , Figure 4 , Figure 5 As shown;

[0052] Step 5: Perform range filtering on the point cloud data after segmenting the ground points, assuming that the coordinates of each point in the single-frame point cloud after filtering out the ground points in the point cloud data after segmenting the ground points are y p , calculate the vector modulus length of each point coordinate, compare the vector modulus length with a second set threshold, retain the vector modulus length if it is less than the second set threshold, otherwise ignore it, and finally obtain the preprocessed point cloud data;

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

[0054] 2. The coordinate transformation parameters of the pre-processed point cloud data are calculated based on the NDT registration algorithm, and a sliding window queue is introduced to store the point cloud data. The local dynamic voxel map is constructed by splicing and eliminating point cloud data of different frames.

[0055] Step 1: The product of the registration change amount of the preprocessed point cloud data of the current positioning and the preprocessed point cloud data of the past two adjacent points is used as the initial registration prediction, and the prediction matrix satisfies the following relationship:

[0056]

[0057] Among them, Q is the current pose matrix, ΔT is the change in the past two adjacent registrations, is the pose matrix of the current frame;

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

[0059] Step 2: Grid the pre-processed point cloud data to obtain processed grid data, and calculate the normal distribution probability density function for each processed grid data. The grid mean satisfies the following relationship:

[0060]

[0061] in, is the coordinate vector of each laser scanning point in the grid, is the mean vector of all points in the grid, which is used to represent the concentrated position of the points in the grid, and m is the total number of laser points in the grid;

[0062] The corresponding covariance matrix Σ is calculated using the grid mean i , satisfying the following relationship:

[0063]

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

[0065] Normal distribution probability density function for each grid Satisfies the following relationship:

[0066]

[0067] Step 3: Take the probability product of each point as the target likelihood function, construct the maximum target likelihood function and find the optimal transformation parameters, and achieve the optimal match by finding the coordinate transformation relationship when the probability product is maximized, satisfying the following relationship:

[0068]

[0069] Among them, Φ is the target likelihood function, which is 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;

[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] Assume that the optimal transformation parameter at the previous moment is Then the transformation amount of the two nearest adjacent frames satisfies the following relationship:

[0075]

[0076] If the current frame is the second frame, let ΔT = 1;

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

[0078] Step 4: using the preprocessed point cloud data and the optimal transformation parameters, transforming the preprocessed point cloud data into a global coordinate system;

[0079] Step 5: Within the time threshold δ, set the total number of sliding window queue point clouds M = {F i |i=t-δ,t-δ-1...,t} and point cloud threshold φ, when the total number M of the queue point cloud 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 optional implementation, δ may be 10, and φ may be 5000 for a 16-line radar.

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

[0082] The dynamic voxel map is constructed by dynamically retaining the environment perception sequence with a length of 10 frames.

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

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

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

[0086] Among them, the initial point X init is the current position of the robot;

[0087] Step 3: Get the sampling point X through random sampling strategy rand , if the sampling point X rand If it is inside the obstacle, execute this step again;

[0088] Step 4: Traverse the sampling point X rand The distance between all points in the generated node set τ and the nearest node X near , from the nearest node X near , at the sampling point X rand The closest node X near StepSize to the sampling point X rand, thereby generating a new node X new ; If the new node X new and the nearest node X near The connecting line segment E i If there is an intersection with an obstacle, it means that the road is blocked and the path generation fails, and this step needs to be repeated;

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

[0090] Step 5: For the new node X new Repeated iterations generate a random expansion tree to test the new node X new Whether to reach the local target point X goal If it is reached, then find a path from the initial point X in the random expansion tree. init To local target point X goal The path is used as the final front-end piecewise linear path;

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

[0092] 4. Based on the front-end segmented linear path expansion, an ellipsoid is constructed, a polyhedron is generated based on the ellipsoid, and the polyhedron is optimized and calculated to finally generate a safe driving area represented by a polyhedron set.

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

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

[0095] Step 2: Two P i The length of the linear path L between the points is the diameter, and an initial sphere with the same length on the three axes is constructed;

[0096] Step 3: Fix the x-axis length of the initial sphere and make it completely coincide with L, 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 touches the point Get the ellipsoid ζ i ,like Figure 7 (a)

[0098] Step 5: In the ellipsoid ζ i Generate a hyperplane on the tangent plane like Figure 8 As shown in (a), after calculating the hyperplane H i After the parameters are set, all the point cloud data outside the range of the hyperplane are deleted, such as Figure 8 (a)

[0099] Where p is the coordinate vector representing the point, which indicates the coordinates of the point that needs to be verified to be located in the polyhedron; A is a matrix containing the parameters of each hyperplane, and each column of it represents the normal vector of a hyperplane, that is, each hyperplane H i The direction of the vector b; each element of the vector b represents the hyperplane H i The offset of

[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), at the same time, a new hyperplane H is generated i+1 ,like Figure 8 (b)

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

[0103]

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

[0105] a i and b i are 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, is the coordinate of the point in contact with the ellipsoid during the expansion process, d is the coordinate of the center point of the ellipsoid, a i is the hyperplane H i The normal vector of

[0108] Step 8: Based on the polyhedron set C, it is obtained that any point x is located in the polyhedron and satisfies the following relationship:

[0109] Ax≤b;

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

[0111] The area formed by all the points x that satisfy Ax≤b is the safe driving area.

[0112] 5. Construct a back-end optimization model based on the safe driving area, and obtain the motion trajectory of the unmanned guided vehicle in a dynamic narrow scene based on the back-end optimization model

[0113] Step 1: Determine the optimization objective function to satisfy the following relationship:

[0114]

[0115] Among them, j n is the acceleration of the nth time step, N is the total number of discrete time steps, ||j n || 2 is the square norm of the jerk at the nth time step;

[0116] In this alternative embodiment, N may be 10;

[0117] Step 2: Let the control point of the nth segment of the third-order Bezier curve be r n0 ,r n1 ,r n2 ,r n3 , using the convex hull property of the Bezier curve, the control point is restricted to the safe space, then the trajectory constrained by the control point must also be within the safe space; let the binary optimization variable b np The control point representing the nth trajectory falls into the pth polyhedron of the safe driving area, and the binary optimization variable b np Satisfies the following relationship:

[0118]

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

[0120] For the binary optimization variable b np Apply the safety corridor linear inequality constraint to satisfy the following relationship:

[0121]

[0122] Step 3: Apply the deterministic position equality constraint of the endpoint of the overall trajectory to the front-end piecewise linear path, satisfying the following relationship:

[0123]

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

[0125] Step 4: Apply the vehicle body dynamics inequality constraint to the front end piecewise linear path to satisfy the following relationship:

[0126]

[0127] Among them, v n (0) ∞ is the initial speed of the nth path, a n (0) ∞ is the initial acceleration of the nth path, j n∞ is the acceleration of the nth path, v max is the maximum permissible speed, a max is the maximum permissible acceleration, j max is the maximum permissible acceleration, for any;

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

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

[0130] Among them, x n+1 (0) is the initial position of the n+1th path, x n (dt) is the end point position of the nth path, and N is the number of trajectory segments;

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

[0132]

[0133] in, and Add constraints v respectively max and a max After that, there is a constant input motion solution on each axis i = {x, y, z}; η ≥ 1 is the time dynamic adjustment factor, and its value depends on the solution of the previous attempt dt; the specific feasible and converged factor in the k-1th planning is denoted as η con,k-1 , in the kth planning, the dynamic adjustment factor will be changed from the interval [η con,k-1 -ε,η con,k-1 +ε′] in increasing 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 Fig. 9 As shown in Figure 2, the optimization results in a safe and smooth motion trajectory of the unmanned guided vehicle in a dynamic narrow scene, as shown in Figure 2. Fig.10 shown.

[0136] The embodiment of the present application also provides a trajectory planning system for an unmanned guided vehicle applied to dynamic narrow scenes, including a memory, a processor, and a computer program stored in the memory and executable on the processor, and the processor implements the steps of the above method when executing the computer program. The trajectory planning system for an unmanned guided vehicle applied to dynamic narrow scenes can implement various embodiments of the above method and achieve the same beneficial effects, which will not be described in detail here.

[0137] The preferred specific embodiments of the present invention are described in detail above. It should be understood that a person skilled in the art can make many modifications and changes based on the concept of the present invention without creative work. Therefore, any technical solution that can be obtained by a person skilled in the art through logical analysis, reasoning or limited experiments based on the concept of the present invention on the basis of the prior art should be within the scope of protection determined by the claims.

Claims

1. A trajectory planning method for an unmanned guided vehicle in a dynamic narrow scene, characterized in that: include: S1: acquiring point cloud data of a laser radar sensor in a dynamic narrow scene, and performing preprocessing on the point cloud data to obtain preprocessed point cloud data; S2: Calculate coordinate transformation parameters of the preprocessed point cloud data by using a normal distribution transformation registration algorithm, introduce a sliding window queue to store the point cloud data, and construct a local dynamic voxel map by splicing and eliminating point cloud data of different frames; S3: Projecting the target point into the boundary of the local dynamic voxel map, and obtaining a front-end piecewise linear path based on the target point; S4: constructing an ellipsoid based on the front-end piecewise linear path expansion, generating a polyhedron based on the ellipsoid, and optimizing and calculating the polyhedron to finally generate a safe driving area represented by a polyhedron set; S5: constructing a back-end optimization model based on the safe driving area, and obtaining a motion trajectory of the unmanned guided vehicle in a dynamic narrow scene based on the back-end optimization model.

2. The trajectory planning method for an unmanned guided vehicle applied to dynamic narrow scenes according to claim 1 is characterized in that: The S1 includes: S11: Randomly select three points in the laser radar sensor point cloud data, and build a model through these three points to satisfy the following relationship: ax+by+cz+d=0; Among them, a, b, c, d are the coefficients of the equation for constructing the three-dimensional plane model; x, y, z are the coordinates of each point in the point cloud; S12: Assuming that all points except the three randomly selected points in the laser radar sensor point cloud data are remaining points, calculating the distance between the remaining points and the model, comparing the distance with a first set threshold, if the distance is less than the first set threshold, it is regarded as an inner point and saved, otherwise it is classified into an outer point range, and the number of inner points under the model is counted; S13: Repeat S11 and S12, and if the number of inliers in the model is greater than the maximum number of inliers that has been saved, update the model and always keep the model parameters with the largest number of inliers; S14: Repeat S11, S12 and S13 to perform an iterative operation. Assuming that the proportion of the interior points in the laser radar sensor point cloud data is o, when the model uses k points each time, the probability of selecting a point as the interior point is o. k , the probability that the selected point is an external point is 1-o k , calculate the probability of failure of iteration S times, satisfying the following relationship: 1-o=(1-o k ) S ; Determine the final number of iterations required to satisfy the following relationship: The model parameters with the largest number of inliers are obtained through the iterative operation, and the model parameters are estimated again using the inliers to obtain the final point cloud data after segmenting the ground points; S15: performing range filtering on the point cloud data after the ground points are segmented, assuming that the coordinates of each point in the single frame point cloud after the ground points are filtered out from the point cloud data after the ground points are segmented is y p , calculate the vector modulus of each point coordinate, compare the vector modulus with a second set threshold, if the vector modulus is less than the second set threshold, retain it, otherwise ignore it, and finally obtain the preprocessed point cloud data.

3. The trajectory planning method for an unmanned guided vehicle applied to dynamic narrow scenes according to claim 1 is characterized in that: The S2 includes: S21: using the point cloud after segmenting the ground, the coordinate transformation parameters of the point cloud are calculated based on the normal distribution transformation registration algorithm, wherein the source point cloud is the current frame point cloud, and the target point cloud is the local dynamic incremental point cloud; S22: The product of the registration change of the currently located point cloud data and the point cloud data of the two adjacent points in the past is used as the initial registration prediction, and the prediction matrix satisfies the following relationship: Among them, Q is the current pose matrix, ΔT is the change in the past two adjacent registrations, is the pose matrix of the current frame; If the current frame is the second frame, then S23: Gridding the pre-processed point cloud data to obtain processed grid data, and performing normal distribution probability density function calculation on each processed grid data, and the grid mean satisfies the following relationship: in, is the coordinate vector of each laser scanning point in the grid, is the mean vector of all points in the grid, which is used to represent the concentrated position of the points in the grid, and m is the total number of laser points in the grid; The corresponding covariance matrix Σ is calculated using the grid mean i , satisfying the following relationship: in, is the mean vector of all points in the grid; Calculate the normal distribution probability density function for each grid Satisfies the following relationship: S24: Taking the probability product of each point as the target likelihood function, constructing the maximum target likelihood function and finding the optimal transformation parameters, and achieving the optimal match by finding the coordinate transformation relationship when the probability product is maximized, satisfying the following relationship: Among them, Φ is the target likelihood function, which is used to evaluate the matching degree between the source point cloud and the target point cloud, k is the point cloud quantity 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Φ; Assume that the optimal transformation parameter at the previous moment is Then the transformation amount of the two nearest adjacent frames satisfies the following relationship: If the current frame is the second frame, let ΔT = 1; If the current frame is the first frame, the optimal transformation parameter T=1; S25: using the preprocessed point cloud data and the optimal transformation parameters, transforming the preprocessed point cloud data into a global coordinate system; S26: Within the time threshold δ, set the total number of sliding window queue point clouds M = {F i |i=t-δ,t-δ-1...,t} and point cloud threshold φ, when the total number M of the queue point cloud 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 eight-transport tree to construct a local dynamic voxel map.

4. The trajectory planning method for an unmanned guided vehicle applied to dynamic narrow scenes according to claim 1 is characterized in that: The 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 operation and set the state sampling space B; where the initial point X init is the current position of the robot; S33: Get sampling point X through random sampling strategy rand , if the sampling point X rand If it is inside the obstacle, execute this step again; S34: traverse the sampling point X rand The distance between all points in the generated node set τ and the nearest node X near , from the nearest node X near , at the sampling point X rand The closest node X near StepSize to the sampling point X rand , thereby generating a new node X new ; If the new node X new and the nearest node X near The connecting line segment E i If there is an intersection with an obstacle, it means that the road is blocked and the path generation fails, and this step needs to be executed again; <StepSize<1; S35: For the new node X new Repeated iterations generate a random expansion tree to test the new node X new Whether to reach the local target point X goal If it is reached, then find a path from the initial point X in the random expansion tree. init To local target point X goal , as the final front-end piecewise linear path.

5. The trajectory planning method for an unmanned guided vehicle applied to dynamic narrow scenes according to claim 1 is characterized in that: The S4 includes: S41: A series of new nodes X will be generated new It is represented as P = {p0, p1, p2, ..., p n }, from point Pi to point P i+1 The linear path is represented by L i = {P i →P i+1 }; Among them, P i For a new node X new ; S42: With two P i The length of the linear path L between the points is the diameter, and an initial sphere with the same length on the three axes is constructed; S43: fixing the x-axis length of the initial sphere and completely coinciding with L, scaling the initial sphere to obtain an ellipsoid; S44: Traverse all obstacles and find the point closest to the center point of the ellipsoid And adjust the lengths of the other two axes so that the ellipsoid touches the point Get the ellipsoid ζ i ; S45: Generate a hyperplane H on the tangent plane of the ellipsoid ζi i ={p|A T p < b}, after calculating the parameters of the hyperplane H i delete the point cloud data filtered within the range outside all hyperplanes; Where p is the coordinate vector representing the point, which indicates the coordinates of the point that needs to be verified to be located in the polyhedron; A is a matrix containing the parameters of each hyperplane, and each column of it represents the normal vector of a hyperplane, that is, each hyperplane H i The direction of the vector b; each element of the vector b represents the hyperplane H i The offset of S46: Repeat S44 and S45 to find the new nearest point And generate a new ellipsoid ζ i+1 , and at the same time, generate a new hyperplane H i+1 ; S47: Repeat S46. Assume that after n iterations, the final ellipsoid ζ is obtained. n , and we get the hyperplane sequence H = {H1, H2, H3, ..., H n }, then the set number of hyperplanes intersect to obtain a polyhedron, and the polyhedron set satisfies the following relationship: Where A is the matrix containing the parameters of each hyperplane, and b is the matrix containing the offset of each hyperplane; a i and b i are 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, is the coordinate of the point in contact with the ellipsoid during the expansion process, d is the coordinate of the center point of the ellipsoid, a i is the hyperplane H i The normal vector of S48: Based on the polyhedron set C, it is obtained that any point x is located in the polyhedron and satisfies the following relationship: Ax≤b; Where A is the matrix containing the parameters of each hyperplane; The area formed by all the points x that satisfy Ax≤b is regarded as a safe driving area.

6. The method for designing a perception and local planner for an unmanned guided vehicle in a dynamic narrow scene according to claim 1, characterized in that: The S5 includes: S51: Determine the optimization objective function to satisfy the following relationship: Among them, j n is the acceleration of the nth time step, N is the total number of discrete time steps, ||j n || 2 is the square norm of the jerk at the nth time step; S52: Let the control point of the nth segment of the third-order Bezier curve be r n0 ,r n1 ,r n2 ,r n3 , using the convex hull property of the Bezier curve, the control point is restricted to the safe space, then the trajectory constrained by the control point must also be within the safe space; let the binary optimization variable b np The control point representing the nth trajectory falls into the pth polyhedron of the safe driving area, and the binary optimization variable b np Satisfies the following relationship: Among them, A p is the parameter matrix of each hyperplane of the pth polyhedron; c p is the boundary value of each hyperplane of the p-th polyhedron; For the binary optimization variable b np Apply the safety corridor linear inequality constraint to satisfy the following relationship: S53: Applying a deterministic position equality constraint of the endpoint of the overall trajectory to the front-end piecewise linear path to satisfy the following relationship: Among them, x0(0) is the trajectory point at the initial time t = 0, x N-1 (dt) is the trajectory point at the final time t = dt; S54: Applying a vehicle body dynamics inequality constraint to the front end piecewise linear path to satisfy the following relationship: Among them, v n (0) ∞ is the initial speed of the nth path, a n (0) ∞ is the initial acceleration of the nth path, j n∞ is the acceleration of the nth path, v max is the maximum permissible speed, a max is the maximum permissible acceleration, j max is the maximum permissible jerk, for any; S55: Applying a segmented trajectory continuity constraint to the front segmented linear path to satisfy the following relationship: x n+1 (0)=x n (dt)n=0:N-2; Among them, x n+1 (0) is the initial position of the n+1th path, x n (dt) is the end point position of the nth path, and N is the number of trajectory segments; S56: Dynamically adjust the time allocation, that is, reasonably estimate the time dt. The allocation of each time interval dt satisfies the following relationship: in, and Add constraints v respectively max and a max After that, there is 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 motion trajectory of the unmanned guided vehicle in dynamic and narrow scenes is optimized.

7. A trajectory planning system for an unmanned guided vehicle in a dynamic narrow scene, 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, the steps of the method described in any one of claims 1 to 6 are implemented.

Images