Local path planning method, device and equipment of AGV and storage medium

Abstract

The invention discloses an AGV local path planning method and device, equipment and a storage medium. The method comprises the following steps: acquiring point cloud data of an area around an AGV and pose information of the AGV to construct a local occupation grid map, and mapping a global path of the AGV and a road width constraint thereof to the local occupation grid map; performing spline curve fitting on a reference path of the global path in the local occupation grid map based on the pose information to obtain an initial path regression to the reference path; based on the initial path and the local occupation grid map, local A star search is carried out on obstacles on the initial path, and an initial collision-free path of local obstacle avoidance is obtained; and optimizing the initial collision-free path based on the B-spline curve, and taking the optimized collision-free path as a local path of the AGV.

Description

Technical Field

[0001] The embodiments of the present application relate to the field of robotics technology, and in particular to a local path planning method, device, equipment and storage medium for AGV. Background Art

[0002] An automated guided vehicle (AGV) is a transport vehicle equipped with electromagnetic or optical guidance devices that can accurately travel along a predetermined guidance path and complete a series of operational tasks under computer monitoring according to operational requirements. With the rapid development of flexible manufacturing systems, automated factories, and automated warehouses, AGV, as an important equipment for material and cargo transportation, has received more and more attention.

[0003] Most existing AGVs follow structured paths for path tracking. AGV path planning usually involves searching for the shortest path on a manually drawn road network using algorithms such as Dijkstra or A*. This path planning method does not take into account the AGV's autonomous obstacle avoidance. Therefore, during the path tracking process, when an obstacle is encountered, the AGV can only stop and wait for the obstacle to leave before continuing to drive, which greatly reduces the efficiency of AGV transportation. Summary of the invention

[0004] The local path planning method, device, equipment and storage medium of AGV provided in the embodiments of the present application can solve or partially solve the above-mentioned deficiencies in the prior art or other deficiencies in the prior art.

[0005] According to the first aspect of the present application, a local path planning method for an AGV is provided, comprising: acquiring point cloud data of an area surrounding the AGV and the posture information of the AGV to construct a local occupancy grid map, and mapping the global path of the AGV and its road width constraint to the local occupancy grid map; performing quintic spline curve fitting on a reference path of the global path in the local occupancy grid map based on the posture information to obtain an initial path for regressing the reference path; performing a local A-star search for obstacles on the initial path based on the initial path and the local occupancy grid map to obtain an initial collision-free path for local obstacle avoidance; and optimizing the initial collision-free path based on a B-spline curve, and using the optimized collision-free path as the local path of the AGV.

[0006] In one embodiment of the present application, point cloud data of the area around the AGV and the posture information of the AGV are obtained to construct a local occupancy grid map, and the global path of the AGV and its road width constraint are mapped to the local occupancy grid map, including: obtaining point cloud data of the area around the AGV and the posture information of the AGV for point cloud mapping to construct a local point cloud map; determining the reference path and its road width boundary curve corresponding to the global path of the AGV and its road width constraint based on the local point cloud map; and rasterizing the local point cloud map to generate the local occupancy grid map, and mapping the reference path and its road width boundary curve to the local occupancy grid map.

[0007] In one embodiment of the present application, the local occupancy grid map includes a dilated local occupancy grid map.

[0008] In one embodiment of the present application, a spline curve is fitted on a reference path of the global path in the local occupancy grid map based on the posture information to obtain an initial path for regressing the reference path, including: determining a starting point, an end point, and a time for passing through the spline curve on the reference path based on the posture information; determining parameters of the spline curve based on the starting point, the end point, and the time for passing through the spline curve, and generating a corresponding spline curve; and combining the generated spline curve with the reference path to obtain an initial path for regressing the reference path.

[0009] In one embodiment of the present application, the starting point, end point and time of passing through the spline curve are determined on the reference path based on the posture information, including: determining the reference point closest to the AGV on the reference path; determining the position of the AGV on the reference path as the starting point of the spline curve, and determining the heading angle of the starting point based on the reference point; determining the end point of the spline curve on the reference path based on the reference point and a preset forward sampling distance; and determining the time of passing through the spline curve based on the starting point, the end point and the maximum acceleration of the AGV.

[0010] In one embodiment of the present application, the spline curve includes a quintic spline curve.

[0011] In one embodiment of the present application, a local A-star search is performed for obstacles on the initial path based on the initial path and the local occupancy grid map to obtain an initial collision-free path for local obstacle avoidance, including: determining the starting point and end point of the local A-star search on the initial path based on the expanded local occupancy grid map; performing a local A-star search on unoccupied and travel-allowed grids in the expanded local occupancy grid map based on the starting point and end point of the local A-star search to obtain a local obstacle avoidance path; and combining the local obstacle avoidance path with the initial path to obtain the initial collision-free path for local obstacle avoidance.

[0012] In one embodiment of the present application, the starting point and the end point of the local A-star search are determined on the initial path based on the expanded local occupancy grid map, including: traversing the points on the initial path in sequence, and detecting the states of the points on the initial path based on the expanded local occupancy grid map; determining the current point as the starting point of the local A-star search based on the current point on the initial path being in an occupied state and the previous point being in an unoccupied state; and determining the current point as the end point of the local A-star search based on the current point on the initial path being in an unoccupied state and the previous point being in an occupied state.

[0013] In one embodiment of the present application, the local occupancy grid map further includes a non-expanded local occupancy grid map; the method further includes: constructing a non-expanded local Euclidean signed distance field map based on the non-expanded local occupancy grid map.

[0014] In one embodiment of the present application, the initial collision-free path is optimized based on a B-spline curve, and the optimized collision-free path is used as a local path of the AGV, including: fitting the initial collision-free path with a quasi-uniform B-spline curve to obtain an initial B-spline curve and its control points; constructing a target cost function based on the non-expanded local Euclidean signed distance field map and the control points of the initial B-spline curve; and optimizing the control points of the initial B-spline curve based on the target cost function to obtain an optimized B-spline curve as the local path of the AGV.

[0015] In one embodiment of the present application, a target cost function is constructed based on the non-expanded local Euclidean signed distance field map and the control points of the initial B-spline curve, including: constructing a smoothness cost function and a dynamic cost function based on the control points of the initial B-spline curve; constructing an obstacle cost function based on the control points of the initial B-spline curve and the non-expanded local Euclidean signed distance field map; and combining the smoothness cost function, the dynamic cost function and the obstacle cost function to obtain the target cost function.

[0016] In one embodiment of the present application, the control points of the initial B-spline curve are optimized based on the target cost function to obtain an optimized B-spline curve as the local path of the AGV, including: iteratively optimizing the target cost function through the LD_LBFGS algorithm to obtain optimized control points, and generating an optimized B-spline curve as the local path of the AGV.

[0017] According to the second aspect of the present application, a local path planning device for an AGV is provided, comprising: a map construction module, configured to acquire point cloud data of an area around the AGV and the posture information of the AGV to construct a local occupancy grid map, and map the global path of the AGV and its road width constraint to the local occupancy grid map; a path planning module, configured to perform quintic spline curve fitting on a reference path of the global path in the local occupancy grid map based on the posture information to obtain an initial path for regressing the reference path; a local obstacle avoidance module, configured to perform a local A-star search for obstacles on the initial path based on the initial path and the local occupancy grid map to obtain an initial collision-free path for local obstacle avoidance; and a path optimization module, configured to optimize the initial collision-free path based on a B-spline curve, and use the optimized collision-free path as the local path of the AGV.

[0018] The automated logistics equipment provided according to the third aspect of the present application may include: at least one processor; and a memory communicatively connected to the at least one processor; wherein the memory stores instructions executable by the at least one processor, and the instructions are executed by the at least one processor so that the at least one processor can execute the local path planning method of the AGV described in the first aspect of the present application.

[0019] The computer-readable storage medium provided according to the fourth aspect of the present application stores a computer program, and when the computer program is executed by a processor, the local path planning method of the AGV described in the first aspect of the present application is implemented.

[0020] According to the local path planning method, device, equipment and storage medium of AGV provided by the implementation method of the present application, by adding road width constraints to the local occupancy grid map constructed by the AGV, local path planning can be carried out within the set allowed driving area; by combining spline curve fitting with local A-star search for obstacles to generate an initial collision-free path, a reasonable initial value that can converge quickly can be provided for subsequent path optimization through B-spline curves, and by limiting the A-star search to generating local obstacle avoidance paths for obstacles, the consumption of computing power can be reduced and the efficiency of generating initial collision-free paths can be improved; by optimizing the initial collision-free path through B-spline curves, a smooth and safe ideal local path suitable for AGV driving can be obtained; so that the path planned by the local path planning method can meet the requirements of high efficiency, reliability and low cost while achieving autonomous obstacle avoidance, and is suitable for processors of AGVs with limited computing power.

[0021] It should be understood that the content described in this section is not intended to identify the key or important features of the embodiments of the present application, nor is it intended to limit the scope of the present application. Other features of the present application will become easily understood through the following description. BRIEF DESCRIPTION OF THE DRAWINGS

[0022] Other features, purposes and advantages of the present application will become more apparent by reading the detailed description of the non-limiting embodiments made with reference to the following drawings. The drawings are used to better understand the present solution and do not constitute a limitation of the present application. Among them:

[0023] Figure 1 is a flow chart of a local path planning method for an AGV according to an embodiment of the present application;

[0024] Figure 2 is a schematic diagram of an embodiment of a local occupancy grid map constructed according to the present application;

[0025] Figure 3 is a schematic diagram of a process for constructing a local occupancy grid map according to an implementation method of the present application;

[0026] Figure 4 is a schematic diagram of a process of generating an initial path of a regression reference path according to an embodiment of the present application;

[0027] Figure 5 is a schematic diagram of an embodiment of an initial path of a regression reference path generated according to the present application;

[0028] Figure 6 is a schematic diagram of a process for generating an initial collision-free path according to an embodiment of the present application;

[0029] Figure 7is a schematic diagram of an embodiment of determining the starting point and end point of a local A-star search according to the present application;

[0030] Figure 8 is a schematic diagram of an embodiment of an initial collision-free path generated according to the present application;

[0031] Fig. 9 is a schematic diagram of generating a corresponding ESDF map according to the non-expanded local occupancy grid map of the present application;

[0032] Fig.10 is a schematic diagram of a process of performing path optimization by using a B-spline curve according to an embodiment of the present application;

[0033] Fig.11 is a schematic diagram of an initial B-spline curve and its control points obtained by fitting the initial collision-free path according to the present application;

[0034] Fig.12 is a schematic diagram of an optimized B-spline curve and its control points obtained by optimizing the initial B-spline curve according to the present application;

[0035] Fig.13 is a schematic diagram of an embodiment of a local path planned according to the local path planning method of the AGV of the present application;

[0036] Fig.14 is a block diagram of a local path planning device for an AGV according to an embodiment of the present application;

[0037] Fig.15 It is a block diagram of automated logistics equipment used to implement the local path planning method of the AGV in the implementation mode of the present application. DETAILED DESCRIPTION

[0038] The following is a description of exemplary embodiments of the present application in conjunction with the accompanying drawings, including various details of the embodiments of the present application to facilitate understanding, which should be considered as merely exemplary. Therefore, it should be recognized by those of ordinary skill in the art that various changes and modifications can be made to the embodiments described herein without departing from the scope and spirit of the present application. Similarly, for the sake of clarity and conciseness, the description of well-known functions and structures is omitted in the following description.

[0039] It should be noted that, in the absence of conflict, the embodiments and features of the embodiments in the present application can be combined with each other. The present application will be described in detail below with reference to the accompanying drawings and in combination with the embodiments.

[0040] An automated guided vehicle is a transport vehicle equipped with electromagnetic or optical guidance devices that can accurately travel along a predetermined guidance path and complete a series of operational tasks under computer monitoring according to the operating requirements. With the rapid development of flexible manufacturing systems, automated factories, automated warehouses, etc., AGV, as an important equipment for the transportation of materials and goods, can replace repetitive human labor to a certain extent, reduce labor costs, and improve transportation efficiency, and therefore has received more and more attention.

[0041] Most existing AGVs track paths along structured paths. Among them, the path planning of AGVs usually searches for the shortest path on a manually drawn road network through algorithms such as Dijkstra or A*. This path planning method does not consider the autonomous obstacle avoidance of AGVs. Therefore, in the process of path tracking, when encountering an obstacle, the AGV can only stop driving and wait for the obstacle to leave before continuing to drive, which greatly reduces the efficiency of AGV transportation. The path planning method based on the TEB algorithm, which is currently widely used in mobile robots, can achieve autonomous obstacle avoidance, but the TEB algorithm uses soft constraint optimization, which may result in a local optimal solution, resulting in the planned path not meeting the requirements of smoothness and safety. At the same time, the path planning method based on the TEB algorithm has a high frequency of path planning, and it is difficult to achieve successful path planning on the processor of the AGV with limited computing power. The path planning method currently used in unmanned driving technology is also used for structured roads and can achieve autonomous obstacle avoidance, but the path planning method used in unmanned driving technology is more complex in calculation, the algorithm framework is more complicated, and it is more difficult to achieve path planning on the processor of the AGV with limited computing power.

[0042] Based on this, in order to solve the above problems, an embodiment of the present application provides a local path planning method 1000 for an AGV.

[0043] Figure 1 The flowchart of the local path planning method 1000 of the AGV according to the embodiment of the present application is shown. The local path planning method 1000 of the AGV according to the embodiment of the present application can be executed by the local path planning device of the AGV, and the local path planning device of the AGV can be set in the automated logistics equipment such as the AGV, but the embodiment of the present application is not limited to this. Figure 1 As shown, the local path planning method 1000 of the AGV may include the following steps:

[0044] S100, acquiring point cloud data of the area around the AGV and the position information of the AGV to construct a local occupancy grid map, and mapping the global path of the AGV and its road width constraint to the local occupancy grid map.

[0045] S200 , performing spline curve fitting on a reference path of a global path in a local occupancy grid map based on the position information to obtain an initial path of a regression reference path.

[0046] S300 , performing a local A-star search for obstacles on the initial path based on the initial path and the local occupancy grid map to obtain an initial collision-free path for local obstacle avoidance.

[0047] S400: Optimize the initial collision-free path based on the B-spline curve, and use the optimized collision-free path as the local path of the AGV.

[0048] In the process of AGV tracking along the structured path, the AGV can obtain the point cloud data of the surrounding area and its own posture information through the sensors set by itself, and construct a local occupancy grid map (Occupancy map) based on the obtained point cloud data and posture information. The local occupancy grid map reflects the environmental status within a certain range around the current position of the AGV, and maps the pre-planned global path and the corresponding road width constraint to the local occupancy grid map. The obtained local occupancy grid map is as follows: Figure 2 As shown. Figure 2 In the illustrated embodiment, the local occupancy grid map of the AGV includes the AGV 201, the mapping 202 of the global path in the local occupancy grid map, the mapping 203 and 204 of the road width constraint in the local occupancy grid map, and the mapping 205 of the obstacle in the local occupancy grid map. Figure 2 After obtaining the local occupancy grid map shown in FIG. 1 , the AGV can use the path of the global path in the local occupancy grid map as a reference path, and perform spline curve fitting on the reference path according to its own posture information to obtain the corresponding spline curve, and regress the obtained spline curve on the reference path to obtain the initial path. Then the AGV can use the initial path and Figure 2 The local occupancy grid map shown performs a local A-star search for obstacles 205 on the initial path to obtain a corresponding local obstacle avoidance path, and combines the obtained local obstacle avoidance path with the initial path to obtain an initial collision-free path. After obtaining the initial collision-free path, the AGV can optimize the initial collision-free path through a B-spline curve to obtain an optimized collision-free path, and use the obtained optimized collision-free path as the local path of the AGV for path tracking.

[0049] From the above, it can be seen that the implementation method of the present application can make local path planning within the set allowed driving area by adding road width constraints to the local occupancy grid map constructed by AGV; by combining spline curve fitting with local A-star search for obstacles to generate an initial collision-free path, a reasonable initial value that can converge quickly can be provided for subsequent path optimization through B-spline curves, and by limiting the A-star search to generating local obstacle avoidance paths for obstacles, the consumption of computing power can be reduced and the efficiency of generating initial collision-free paths can be improved; by optimizing the initial collision-free path through B-spline curves, a smooth and safe ideal local path suitable for AGV driving can be obtained; so that the path planned by the local path planning method can meet the requirements of high efficiency, reliability and low cost while realizing autonomous obstacle avoidance, and is suitable for processors of AGVs with limited computing power.

[0050] It should be understood that the steps shown in method 1000 are not exclusive, and other steps may be performed before, after, or between any of the steps shown. In addition, some of the steps shown may be performed simultaneously, or may be performed in different steps. Figure 1 Executed in the order shown.

[0051] It should be noted that in the embodiments of the present application, a path refers to a sequence of points or a curve connecting a starting position and an end position. Path planning refers to a strategy for forming a path. Path tracking refers to a robot or AGV being able to travel along a planned geometric path at a preset speed from a known initial state through motion control. An occupancy grid map refers to a map in which each grid indicates whether the corresponding area is occupied by an object.

[0052] The following is a detailed description of each step of the local path planning method 1000 for AGV in the implementation manner of the present application.

[0053] Step S100

[0054] In step S100, point cloud data of the area around the AGV and the posture information of the AGV are obtained to construct a local occupancy grid map, and the global path of the AGV and its road width constraint are mapped to the local occupancy grid map. Figure 3 FIG. 1 is a flow chart showing some implementation methods of constructing a local occupancy grid map according to the present application. Figure 3 As shown, step S100 may include the following steps:

[0055] S110, obtaining point cloud data of the area around the AGV and the position information of the AGV for point cloud mapping, and constructing a local point cloud map.

[0056] S120, determining the global path of the AGV and the reference path and the road width boundary curve corresponding to the road width constraint thereof based on the local point cloud map.

[0057] S130 , rasterizing the local point cloud map to generate a local occupancy grid map, and mapping the reference path and its road width boundary curve into the local occupancy grid map.

[0058] AGV can collect point cloud data of the surrounding area through its own environmental perception sensors, such as environmental perception sensors that may include but are not limited to laser radar, millimeter wave radar, ultrasonic radar, camera, etc., and obtain the current posture information through its own posture sensors, such as posture sensors that may include but are not limited to inertial measurement units (IMU), encoders, etc., and combine point cloud data with posture information for point cloud mapping to construct a local point cloud map. Then AGV can determine the path corresponding to the global path in the local point cloud map as a reference path based on the local point cloud map and the pre-planned global path of the AGV. For example, the reference path can be a Bezier curve, and the corresponding road width boundary curve is calculated based on the road width constraint of the reference path. After that, AGV can rasterize the local point cloud map to generate a corresponding local occupancy grid map, and map the determined reference path and its road width boundary curve to the generated local occupancy grid map.

[0059] Optionally, the range of the local occupancy grid map can be determined based on the environmental perception sensor set by the AGV for collecting point cloud data, or can also be set to be smaller than the range of the collected point cloud data as needed, and the embodiments of the present application are not limited to this. Usually, the local occupancy grid map can include a range of a certain distance in front of the current position of the AGV and a range of a certain distance behind the current position of the AGV, and the range of a certain distance in front of the current position of the AGV included in the local occupancy grid map is larger than the range of a certain distance behind the current position of the AGV, so as to plan the path for the AGV to travel forward. Optionally, the local occupancy grid map can include an expanded local occupancy grid map. By expanding objects in the local occupancy grid map, such as AGVs, obstacles, etc., the space occupied by the volume of the objects is taken into account when planning the path according to the local occupancy grid map, which can improve the accuracy of the path planning.

[0060] It should be noted that the global path of the AGV can be obtained by existing path planning methods, such as searching for the shortest path on a manually drawn road network by using algorithms such as Dijkstra or A*, and the implementation methods of this application are not limited to this. Obtaining a local occupancy grid map based on a local point cloud map can be achieved by an existing rasterization method, and the implementation methods of this application are not limited to this.

[0061] Step S200

[0062] In step S200, a spline curve is fitted to a reference path of the global path in the local occupancy grid map based on the posture information to obtain an initial path of the regression reference path. Figure 4 FIG. 1 is a flow chart showing some implementation methods of generating an initial path of a regression reference path according to the present application. Figure 4 As shown, step S200 may include the following steps:

[0063] S210 . Determine a starting point, an end point, and a time for passing through the spline curve on the reference path based on the posture information.

[0064] S220 , determining parameters of the quintic spline curve based on the starting point, the end point and the time of passing through the spline curve, and generating a corresponding spline curve.

[0065] S230: Combining the generated spline curve with the reference path to obtain an initial path of the regression reference path.

[0066] After obtaining the reference path of the global path in the local occupancy grid map, the AGV can optimize the reference path through the spline curve. First, the starting point start, end point end and time T of the spline curve passing through the spline curve can be determined on the reference path according to the posture information of the AGV, the preset forward sampling distance of the spline curve and the maximum acceleration of the AGV. Then, the starting point, end point and time of passing through the spline curve are used as constraints to solve the parameters in the expression of the spline curve. After obtaining the parameters in the expression of the spline curve, the corresponding spline curve can be obtained. By splicing the obtained spline curve with the reference path, the spline curve is regressed to the reference path, and the spliced ​​path is used as the initial path to obtain the initial path of the regressed reference path.

[0067] Optionally, step S210 of determining the starting point, end point and time of passing the spline curve may also include the following steps: determining the reference point closest to the AGV on the reference path; determining the position of the AGV on the reference path as the starting point start of the spline curve, and determining the heading angle of the starting point start based on the reference point; determining the end point end of the spline curve on the reference path based on the reference point and the preset forward sampling distance; and determining the time T of passing the spline curve based on the starting point start, the end point end and the maximum acceleration of the AGV. Wherein, when determining the starting point start of the spline curve, the AGV uses the coordinates of its position on the reference path as the coordinates of the starting point of the spline curve, such as the x coordinate and the y coordinate in the plane rectangular coordinate system, and the heading angle of the starting point start of the spline curve is determined by comparing the heading angle of the AGV at the current position with the heading angle of the reference point. When determining the end point end of the spline curve, the AGV samples on the reference path starting from the reference point with the preset forward sampling distance. Optionally, step S200 may also include intercepting a path of corresponding length on the reference path according to a preset path length. When combining the generated spline curve with the reference path, the AGV can splice the resulting spline curve with the intercepted reference path.

[0068] In an optional example, the spline curve includes a quintic spline curve, and a one-dimensional expression of the quintic spline curve is as follows:

[0069] x(t)=c5t 5 +c4t 4 +c3t 3 +c2t 2 +c1t+c0 (Formula 1)

[0070] Substituting the starting point start, the end point end and the time T boundary constraint, we can get the following matrix expression:

[0071]

[0072] Among them, c1 to c5 are parameters; a is the coordinate of the starting point start; b is the coordinate of the end point end; in the matrix expression, the first row represents the position of the starting point start, the second row represents the position of the end point end, the third row represents the speed of the starting point start, the fourth row represents the speed of the end point end, the fifth row represents the acceleration of the starting point start, and the sixth row represents the acceleration of the end point end.

[0073] In an optional example, the initial path of the regression reference path obtained by splicing the obtained quintic spline curve with the intercepted reference path is as follows: Figure 5 As shown. Figure 5In the figure, 501 is the AGV, 502 is the fitted quintic spline curve, 503 is the intercepted reference path, 504 is the obstacle, 505 is the starting point of the quintic spline curve, 506 is the end point of the quintic spline curve, 507 and 508 are the road width boundary curves, and 502+503 constitutes the initial path of the regression reference path.

[0074] It should be noted that, in the implementation of the present application, the values ​​of the preset path length and the preset forward sampling distance can be set according to the configuration information of the AGV, the actual application field, etc., and the implementation of the present application does not limit this. In the implementation of the present application, the curve can be a cubic spline curve, a quartic spline curve, or a quintic spline curve, etc., and the implementation of the present application does not limit this.

[0075] Step S300

[0076] In step S300, a local A-star search is performed for obstacles on the initial path based on the initial path and the local occupancy grid map to obtain an initial collision-free path for local obstacle avoidance. Figure 6 FIG. 2 shows a schematic diagram of some implementation methods of generating an initial collision-free path according to the present application. Figure 6 As shown, step S300 may include the following steps:

[0077] S310 , determining a start point and an end point of a local A-star search on an initial path based on an expanded local occupancy grid map.

[0078] S320 , performing a local A-star search on unoccupied and travel-allowed grids in the expanded local occupied grid map based on the starting point and the end point of the local A-star search, to obtain a local obstacle avoidance path.

[0079] S330: Combine the local obstacle avoidance path with the initial path to obtain an initial collision-free path for local obstacle avoidance.

[0080] After optimizing the reference path through the spline curve to obtain the initial path, the AGV can plan a local obstacle avoidance path for obstacles on the initial path through local A-star search. First, the starting point and end point of the local A-star search can be determined on the initial path according to the initial path and the expanded local occupancy grid map. Then, according to the determined starting point and end point of the local A-star search, a local A-star search is performed in the expanded local occupancy grid map to obtain a local obstacle avoidance path. Among them, the local A-star search is performed in the grid that is not occupied by obstacles in the expanded local occupancy grid map and is allowed to travel as set by the road width constraint. By splicing the obtained local obstacle avoidance path with the initial path, the initial collision-free path for local obstacle avoidance can be obtained.

[0081] Optionally, step S310 of determining the starting point and end point of the local A-star search may also include the following steps: traversing the points on the initial path in sequence, and detecting the states of the points on the initial path based on the expanded local occupancy grid map; determining the current point as the starting point start of the local A-star search based on the current point on the initial path being in an occupied state and the previous point being in an unoccupied state; and determining the current point as the end point end of the local A-star search based on the current point on the initial path being in an unoccupied state and the previous point being in an occupied state. In an optional example, the process of determining the starting point and end point of the local A-star search in the expanded local occupancy grid map may be as follows: Figure 7 As shown. Figure 7 In the figure, 701 is the initial path, 702 is the obstacle, 703 is the starting point of the local A star search, and 704 is the end point of the local A star search. Optionally, when combining the local obstacle avoidance path with the initial path, the AGV can also discretize the initial path according to a preset discrete spacing, and splice the discretized initial path with the local obstacle avoidance path.

[0082] In an optional example, the initial collision-free path obtained by concatenating the obtained local obstacle avoidance path with the initial path is as follows: Figure 8 As shown. Figure 8 In the figure, 801 is the AGV, 802 is the local obstacle avoidance path obtained by the local A-star search, 803 is the fitted quintic spline curve, 804 is the intercepted reference path, 805 is the obstacle, 806 is the starting point of the local A-star search, 807 is the end point of the local A-star search, 808 and 809 are road width boundary curves, and 803+802+804 constitute the initial collision-free path.

[0083] It should be noted that the local A-star search can be implemented by the existing A-star search method, and the implementation of the present application does not limit this. In the implementation of the present application, the number of obstacles in the initial path can be one, or two, three, or other numbers, and the implementation of the present application does not limit this.

[0084] Step 400

[0085] In step S400, the initial collision-free path is optimized based on the B-spline curve, and the optimized collision-free path is used as the local path of the AGV. The local occupancy grid map obtained in step S100 may also include a non-expanded local occupancy grid map, and the local path planning method 1000 of the AGV may also include: constructing a non-expanded local Euclidean Signed Distance Functions (ESDF) map based on the non-expanded local occupancy grid map, and each grid in the ESDF map also indicates the distance of the obstacle closest to the grid.

[0086] In an optional example, a non-expanded local occupancy grid map and its corresponding ESDF map are as follows: Fig. 9 As shown. Fig. 9 901 is a non-expanded local occupancy grid map, 902 is an ESDF map obtained based on the non-expanded local occupancy grid map, and 903 is an ESDF map showing a gradient obtained based on the ESDF map.

[0087] Fig.10 FIG. 1 is a flow chart showing some implementation methods of path optimization using B-spline curves according to the present application. Fig.10 As shown, step S400 may include the following steps:

[0088] S410 , performing quasi-uniform B-spline curve fitting on the initial collision-free path to obtain an initial B-spline curve and its control points.

[0089] S420, constructing a target cost function based on the non-expanded local Euclidean signed distance field map and the control points of the initial B-spline curve.

[0090] S430 , optimizing the control points of the initial B-spline curve based on the target cost function to obtain an optimized B-spline curve as a local path of the AGV.

[0091] After obtaining the initial collision-free path, the AGV can optimize the initial collision-free path through the B-spline curve. First, the matrix equation of the quasi-uniform B-spline curve can be constructed, and the constructed matrix equation can be solved according to the preset spacing of the quasi-uniform B-spline curve node vectors and the discretized initial collision-free path, so that the control points of the quasi-uniform B-spline curve can be obtained. The control points of the obtained quasi-uniform B-spline curve are used as the control points of the initial B-spline curve, and the initial B-spline curve that fits the initial collision-free path can be obtained. Then, the target cost function can be constructed according to the ESDF map and the control points of the initial B-spline curve, and the control points of the initial B-spline curve are optimized according to the constructed target cost function, and the optimized B-spline curve is generated according to the optimized control points, and the optimized B-spline curve is used as the local path of the AGV.

[0092] Optionally, step S420 constructing the target cost function may also include the following steps: constructing a smoothness cost function and a dynamics cost function based on the control points of the initial B-spline curve; constructing an obstacle cost function based on the control points of the initial B-spline curve and the non-expanded local Euclidean signed distance field map; combining the smoothness cost function, the dynamics cost function and the obstacle cost function to obtain the target cost function. Optionally, step S430 optimizing the control points of the initial B-spline curve through the target cost function may also include: iteratively optimizing the target cost function through the LD_LBFGS algorithm to obtain the optimized control points, and generating the optimized B-spline curve as the local path of the AGV.

[0093] In an alternative example, the degree of the B-spline curve is p b , with N+1 control points {Q0 Q1 ... Q N}, the node vector is [t0 t1 ... t M ], where M = N + p b +1, the value of the B-spline curve is determined by the parameter t, t∈[t pb , t M-p b], the expression of the B-spline curve is as follows:

[0094]

[0095] Among them, B i,p (t) is the Bernstein basis function, and its expression is as follows:

[0096]

[0097] When the nodes are evenly distributed along the parameter axis, that is, When , it is a uniform B-spline curve. Examples of the node vectors of a uniform B-spline curve are as follows:

[0098] t=(0,1,2,3,4,5,6)

[0099] The difference between a uniform B-spline curve and a uniform B-spline curve is that the two end nodes have a repetition k+1 times, where k is the degree of the B-spline curve. Such a node vector defines the node vector of a quasi-uniform B-spline curve. An example of a node vector of a quasi-uniform B-spline curve is as follows:

[0100] t=(0,0,0,01,2,3,4,5,5,5,5)

[0101] The uniform B-spline curve and the uniform B-spline curve can be expressed in matrix form as follows:

[0102]

[0103] The fitting input of the uniform B-spline curve is the discrete points {P0 P1 … P k-1}, there are k points in total, the node spacing is Δt, the node vector can be expressed as [0 0 0 Δt … (k-1)Δt (k)Δt (k)Δt (k)Δt], and the number of uniform B-spline node vectors is k+6. The output is k+2 node vectors, and the expression of k matrices can be obtained as follows:

[0104]

[0105] The matrix expression obtained after expansion is as follows:

[0106]

[0107] For the convenience of calculation, the following form is taken:

[0108]

[0109] Simplified to the following form:

[0110]

[0111] In an optional example, the initial B-spline curve and its control points obtained by fitting the initial collision-free path with a quasi-uniform B-spline curve are as follows: Fig.11 As shown. Fig.11 In FIG. 1 , 1101 is the AGV, 1102 is the initial B-spline curve and its control points, 1103 is the obstacle, and 1104 and 1105 are the road width boundary curves.

[0112] In an optional example, the constructed objective cost function is as follows:

[0113] f total =λ1f s+λ2f c +λ s (f v +f a ) (Formula 9)

[0114] Among them, f total is the target cost function, f c is the obstacle cost function, f s is the smoothness cost function, f θ +f a is the dynamic cost function.

[0115] The expression of the obstacle cost function is as follows:

[0116]

[0117]

[0118] The expression of the smoothness cost function is as follows:

[0119]

[0120] The expression of the dynamics cost function is as follows:

[0121]

[0122]

[0123] Among them, when the end point of the B-spline curve is also the end point of the reference path, the end point of the B-spline curve is not included in the optimization, otherwise it is included in the optimization.

[0124] In an optional example, the optimized B-spline curve and its control points obtained by optimizing the initial B-spline curve by using the constructed target cost function are as follows: Fig.12 As shown. Fig.12 In the figure, 1201 is the AGV, 1202 is the optimized B-spline curve and its control points, 1203 is the initial B-spline curve and its control points, 1204 is an obstacle, and 1205 and 1206 are road width boundary curves.

[0125] It should be noted that, in the embodiments of the present application, the spacing of the node vectors of the quasi-uniform B-spline curve can be set according to the actual application field, the accuracy requirements of the path planning, etc., and the embodiments of the present application do not limit this. In addition to using the LD_LBFGS algorithm, the method for optimizing the control points of the initial B-spline curve according to the target cost function can also use other optimization algorithms, and the embodiments of the present application do not limit this.

[0126] In a verification example, the method 1000 of the above embodiment of the present application is used to perform local path planning, and the processor used is Core TM i7-9700 CPU@3.00GHz×8, memory is 16G, Fig.13 The local path planned in the environment shown is 1302, where 1301 is the AGV, 1303 is the initial B-spline curve, 1304 is the obstacle, and 1305 and 1306 are the road width boundary curves. Fig.13 It can be seen that an obstacle avoidance path is planned within the specified road width, and the time overhead of a single path planning of the local path planning is about 0.0011s, indicating that the method 1000 of the above-mentioned implementation mode of the present application can be used for planning a fast local obstacle avoidance path.

[0127] In a verification example, on a Rockchip RK3399 with dual-core Cortex-A72, quad-core Contex-A53 and a maximum main frequency of 1.8 GHz, the method 1000 of the above implementation of the present application is run on a real vehicle to perform local path planning, and its CPU occupancy is about 27%, and the time overhead of a single path planning of the local path planning is about 0.05 s, indicating that the method 1000 of the above implementation of the present application can achieve efficient, reliable and low-cost local path planning on a low computing power platform.

[0128] like Fig.14 As shown, the embodiment of the present application also provides a local path planning device 2000 for an AGV. Fig.14 FIG. 2 shows a block diagram of a local path planning device 2000 for an AGV according to an embodiment of the present application. Fig.14 As shown, the local path planning device 2000 of the AGV according to the embodiment of the present application can execute the local path planning method 1000 of the AGV, and the local path planning device 2000 of the AGV can include:

[0129] The map construction module 510 is configured to acquire point cloud data of the area around the AGV and the posture information of the AGV to construct a local occupancy grid map, and map the global path of the AGV and its road width constraint to the local occupancy grid map.

[0130] The path planning module 520 is configured to perform spline curve fitting on the reference path of the global path in the local occupancy grid map based on the posture information to obtain an initial path of the regression reference path.

[0131] The local obstacle avoidance module 530 is configured to perform a local A-star search for obstacles on the initial path based on the initial path and the local occupancy grid map to obtain an initial collision-free path for local obstacle avoidance.

[0132] The path optimization module 540 is configured to optimize the initial collision-free path based on the B-spline curve, and use the optimized collision-free path as the local path of the AGV.

[0133] In addition, an embodiment of the present application also provides an automated logistics equipment, which includes at least one processor and a memory communicatively connected to the at least one processor; wherein the memory stores instructions executable by the at least one processor, and the instructions are executed by the at least one processor so that the at least one processor can execute the above-mentioned AGV local path planning method 1000.

[0134] The embodiment of the present application further provides a computer-readable storage medium storing a computer program, and when the computer program is executed by a processor, the local path planning method 1000 of the AGV described above is implemented.

[0135] Fig.15 A block diagram of an automated logistics device 3000 that can be used to implement an example of an embodiment of the present application is shown. The automated logistics device 3000 is intended to represent various forms of digital computers. The automated logistics device 3000 can also represent various forms of mobile devices that can run computing programs. The components shown herein, their connections and relationships, and their functions are merely examples and are not intended to limit the implementation of the present application described and / or required herein.

[0136] like Fig.15 As shown, the automated logistics equipment 3000 includes a computing unit 610, which can perform various appropriate actions and processes according to a computer program stored in a read-only memory (ROM) 620 or a computer program loaded from a storage unit 680 to a random access memory (RAM) 630. In the RAM 630, various programs and data required for the operation of the automated logistics equipment 3000 can also be stored. The computing unit 610, the ROM 620, and the RAM 630 are connected to each other via a bus 640. An input / output (I / O) interface 650 is also connected to the bus 640.

[0137] Multiple components in the automated logistics equipment 3000 are connected to the I / O interface 650, including: an input unit 660, such as a touch screen, etc.; an output unit 670, such as various types of displays, speakers, etc.; a storage unit 680, such as a disk, etc.; and a communication unit 690, such as a network card, a modem, a wireless communication transceiver, etc. The communication unit 690 allows the automated logistics equipment 3000 to exchange information / data with other devices through a computer network such as the Internet and / or various telecommunication networks.

[0138] The computing unit 610 may be a variety of general and / or special processing components with processing and computing capabilities. Some examples of the computing unit 610 include, but are not limited to, a central processing unit (CPU), a graphics processing unit (GPU), various dedicated artificial intelligence (AI) computing chips, various computing units running machine learning model algorithms, digital signal processors (DSPs), and any appropriate processors, controllers, microcontrollers, etc. The computing unit 610 performs the various methods and processes described above, such as the local path planning method 1000 for AGVs. For example, in some embodiments, the local path planning method 1000 for AGVs may be implemented as a computer software program, which is tangibly contained in a machine-readable medium, such as a storage unit 680. In some embodiments, part or all of the computer program may be loaded and / or installed on the automated logistics equipment 3000 via the ROM 620 and / or the communication unit 690. When the computer program is loaded into the RAM 630 and executed by the computing unit 610, one or more steps of the local path planning method 1000 for AGVs described above may be executed. Alternatively, in other embodiments, the computing unit 610 may be configured to execute the local path planning method 1000 for the AGV in any other appropriate manner (eg, by means of firmware).

[0139] Various implementations of the systems and techniques described above herein can be implemented in digital electronic circuit systems, integrated circuit systems, field programmable gate arrays (FPGAs), application specific integrated circuits (ASICs), application specific standard products (ASSPs), systems on chips (SOCs), load programmable logic devices (CPLDs), computer hardware, firmware, software, and / or combinations thereof. These various implementations can include: being implemented in one or more computer programs that can be executed and / or interpreted on a programmable system including at least one programmable processor, which can be a special purpose or general purpose programmable processor that can receive data and instructions from a storage system, at least one input device, and at least one output device, and transmit data and instructions to the storage system, the at least one input device, and the at least one output device.

[0140] The program code for implementing the method of the present application can be written in any combination of one or more programming languages. These program codes can be provided to a processor or controller of a general-purpose computer, a special-purpose computer or other programmable data processing device, so that the program code, when executed by the processor or controller, implements the functions / operations specified in the flow diagram and / or block diagram. The program code can be executed entirely on the machine, partially on the machine, partially on the machine and partially on a remote machine as a stand-alone software package, or entirely on a remote machine or server.

[0141] In the context of the present application, a machine-readable medium may be a tangible medium that may contain or store a program for use by or in conjunction with an instruction execution system, device, or equipment. A machine-readable medium may be a machine-readable signal medium or a machine-readable storage medium. A machine-readable medium may include, but is not limited to, an electronic, magnetic, optical, electromagnetic, infrared, or semiconductor system, device, or equipment, or any suitable combination of the foregoing. A more specific example of a machine-readable storage medium may include an electrical connection based on one or more lines, a portable computer disk, a hard disk, a random access memory (RAM), a read-only memory (ROM), an erasable programmable read-only memory (EPROM or flash memory), an optical fiber, a portable compact disk read-only memory (CD-ROM), an optical storage device, a magnetic storage device, or any suitable combination of the foregoing.

[0142] It should be understood that the various forms of processes shown above can be used to reorder, add or delete steps. For example, the steps recorded in this disclosure can be executed in parallel, sequentially or in different orders, as long as the desired results of the technical solution disclosed in this application can be achieved, and this document is not limited here.

[0143] The above specific implementations do not constitute a limitation on the protection scope of this application. It should be understood by those skilled in the art that various modifications, combinations, sub-combinations and substitutions can be made according to design requirements and other factors. Any modifications, equivalent substitutions and improvements made within the spirit and principles of this application should be included in the protection scope of this application.

Claims

1. A local path planning method for AGV, characterized in that: include: Acquire point cloud data of the area around the AGV and the posture information of the AGV to construct a local occupancy grid map, and map the global path of the AGV and its road width constraint to the local occupancy grid map; Performing spline curve fitting on a reference path of the global path in the local occupancy grid map based on the posture information to obtain an initial path for regressing the reference path; Performing a local A-star search for obstacles on the initial path based on the initial path and the local occupancy grid map to obtain an initial collision-free path for local obstacle avoidance; as well as The initial collision-free path is optimized based on a B-spline curve, and the optimized collision-free path is used as a local path of the AGV.

2. The method according to claim 1, wherein: Acquiring point cloud data of the area around the AGV and the posture information of the AGV to construct a local occupancy grid map, and mapping the global path of the AGV and its road width constraint to the local occupancy grid map, including: Obtaining point cloud data of the area around the AGV and the position information of the AGV for point cloud mapping to construct a local point cloud map; Determine the reference path and the road width boundary curve corresponding to the global path of the AGV and the road width constraint thereof based on the local point cloud map; and The local point cloud map is rasterized to generate the local occupancy grid map, and the reference path and its road width boundary curve are mapped into the local occupancy grid map.

3. The method according to claim 2, wherein: The local occupancy grid map includes a dilated local occupancy grid map.

4. The method according to claim 3, wherein: Performing spline curve fitting on a reference path of the global path in the local occupancy grid map based on the posture information to obtain an initial path for regressing the reference path includes: Determine a starting point, an end point, and a time of passing through the spline curve on the reference path based on the posture information; Determining parameters of the spline curve based on the starting point, the end point and the time of passing through the spline curve, and generating a corresponding spline curve; and The generated spline curve is combined with the reference path to obtain an initial path for regressing the reference path.

5. The method according to claim 3, wherein: Based on the initial path and the local occupancy grid map, a local A-star search is performed for obstacles on the initial path to obtain an initial collision-free path for local obstacle avoidance, including: Determining a start point and an end point of a local A-star search on the initial path based on the expanded local occupancy grid map; Performing a local A-star search on unoccupied and travel-allowed grids in the expanded local occupied grid map based on the starting point and the end point of the local A-star search to obtain a local obstacle avoidance path; and The local obstacle avoidance path is combined with the initial path to obtain the initial collision-free path for local obstacle avoidance.

6. The method according to claim 3, wherein: The local occupancy grid map further comprises a non-expanded local occupancy grid map; The method further comprises: A non-dilated local Euclidean signed distance field map is constructed based on the non-dilated local occupancy grid map.

7. The method according to claim 6, wherein: The initial collision-free path is optimized based on the B-spline curve, and the optimized collision-free path is used as the local path of the AGV, including: Performing quasi-uniform B-spline curve fitting on the initial collision-free path to obtain an initial B-spline curve and its control points; constructing a target cost function based on the non-expanded local Euclidean signed distance field map and the control points of the initial B-spline curve; and The control points of the initial B-spline curve are optimized based on the target cost function to obtain an optimized B-spline curve as the local path of the AGV.

8. A local path planning device for AGV, characterized in that: include: A map construction module is configured to obtain point cloud data of the area around the AGV and the posture information of the AGV to construct a local occupancy grid map, and map the global path of the AGV and its road width constraint to the local occupancy grid map; A path planning module is configured to perform spline curve fitting on a reference path of the global path in the local occupancy grid map based on the posture information to obtain an initial path for regressing the reference path; A local obstacle avoidance module is configured to perform a local A-star search for obstacles on the initial path based on the initial path and the local occupancy grid map to obtain an initial collision-free path for local obstacle avoidance; as well as The path optimization module is configured to optimize the initial collision-free path based on the B-spline curve, and use the optimized collision-free path as the local path of the AGV.

9. An automated logistics equipment, characterized in that: include: at least one processor; as well as, A memory communicatively connected to the at least one processor; wherein the memory stores instructions executable by the at least one processor, the instructions being executed by the at least one processor so that the at least one processor can execute the local path planning method for the AGV described in any one of claims 1 to 7.

10. A computer-readable storage medium storing a computer program, characterized in that: When the computer program is executed by a processor, the local path planning method for an AGV as described in any one of claims 1 to 7 is implemented.

Images