Environment self-adaptive AGV (Automatic Guided Vehicle) path navigation method

Abstract

The invention discloses an environment self-adaptive AGV path navigation method, which comprises the following steps: an AGV scans and establishes a map, and specific coordinates of obstacles in the map are determined; selecting working points inevitably passed by the AGV during working in the map, and determining a connection relationship between the working points; determining a path corresponding to each connection relationship; when the AGV receives the current moving task, all routes which can be formed through the known connection relation between the current working point and the target working point are searched, and the route with the shortest total route length is selected as the final route to execute the moving task. According to the method, only a small number of working points and the connection relations between the working points need to be marked, and on the basis, the algorithm can automatically determine the path corresponding to each connection relation, so that the workload of field implementation personnel is reduced, the working efficiency is improved, and meanwhile, the method has the advantages of being small in calculation amount, reasonable and effective in planning and the like.

Description

Technical Field

[0001] The invention belongs to the technical field of AGV navigation, and in particular relates to an AGV path navigation method. Background Art

[0002] AGV (Automated Guided Vehicle) navigation is one of the key technologies in the field of logistics automation. Its main function is to enable AGV to accurately and efficiently travel on a designated route to complete the task of handling goods. AGV navigation systems usually rely on sensors to obtain environmental information and process this information through algorithms to achieve autonomous navigation.

[0003] The AGV navigation system is mainly composed of the following parts: LiDAR, controller, driver and actuator. As the core sensor, LiDAR is responsible for collecting information about the surrounding environment and providing accurate data support for AGV navigation. The controller is responsible for processing the data collected by LiDAR and formulating driving strategies. The driver receives instructions from the controller and drives the AGV's motor to run. The actuator includes the motor, wheels and intermediate transmission mechanism, which is responsible for realizing the specific actions of the AGV, such as turning, acceleration and deceleration, etc.

[0004] The traditional AGV navigation method is to establish the relationship between points and paths after SLAM (simultaneous localization and mapping) laser mapping. Before navigation, points are first established in the map. The points here include the working points that the AGV must pass through, as well as a large number of points in order to achieve path navigation. Then the path relationship between the points is edited, and finally the AGV navigates based on the established path relationship. The defect of this method is that each path relationship of each point must be manually edited. For scenes with hundreds of points, thousands of paths may need to be edited, and the workload of deployment and maintenance is extremely large. In front of some key points or in more complex scenes, a large number of auxiliary points need to be manually established for transition, which further increases the workload. Summary of the invention

[0005] The present invention proposes an environment-adaptive AGV path navigation method, the purpose of which is to solve the problem of heavy workload in setting path relationships between points.

[0006] The technical solution of the present invention is as follows:

[0007] An environment-adaptive AGV path navigation method, comprising the following steps:

[0008] Step 1: AGV scans and creates a map to identify the specific coordinates of obstacles in the map;

[0009] Step 2: Select the work points that the AGV must pass through when working in the map, and determine the connection relationship between the work points;

[0010] Step 3: Determine the path corresponding to each connection relationship;

[0011] Step 4: When the AGV receives the current moving task, it searches for all routes between the current work point and the target work point that can be formed by known connection relationships. According to the path established in step 3, the path length corresponding to each connection relationship in each route is obtained, thereby obtaining the total path length of each route. The route with the shortest total path length is selected as the final route to execute the moving task.

[0012] As a further improvement of the environment-adaptive AGV path navigation method: in step 2, the default orientation of each working point is also determined; when the AGV reaches a certain working point during operation, it will automatically rotate to the default orientation of the point.

[0013] As a further improvement of the environment-adaptive AGV path navigation method:

[0014] Assuming that the working points at both ends of a connection relationship are working point A and working point B, the specific method for determining the path segment is:

[0015] Step 3-1, if there is no obstacle between working point A and working point B, the straight line segment between working point A and working point B is used as the path of the connection relationship; otherwise, execute step 3-2;

[0016] Step 3-2, construct a curve connecting working point A and working point B and passing through the control point by selecting the side control point, and use the curve as the path of the connection relationship; if a curve that meets the requirements cannot be constructed, execute step 3-3;

[0017] Step 3-3: Establish a path from work point A to work point B by gradually searching for intermediate points.

[0018] As a further improvement of the environment-adaptive AGV path navigation method, in step 3-2, the curve is constructed as follows:

[0019] Step 3-2-1, select the initial position of control point C;

[0020] Step 3-2-2: Construct a second-order Bezier curve from working point A to working point B and passing through control point C. The formula of the curve is:

[0021] x(t)=(1-t) 2 x A +2t(1-t)x C +t 2 x B ;

[0022] y(t)=(1-t) 2 yA +2t(1-t)y C +t 2 y B ;

[0023] Among them, t is the time parameter normalized to [0,1] within the path, x A ,y A are the X-axis coordinates and Y-axis coordinates of the working point A, respectively. B ,y B are the X-axis coordinates and Y-axis coordinates of the working point B, respectively. C ,y C are the X-axis coordinate and Y-axis coordinate of the control point C respectively;

[0024] Step 3-2-3, calculate the evaluation function F of the currently constructed curve;

[0025] Step 3-2-4, if the update iteration number of the control point C is less than or equal to the preset value, and the difference between the current evaluation function and the previous evaluation function is less than the preset threshold, then execute step 3-2-6; if the update iteration number of the control point C is less than or equal to the preset value, and the difference between the current evaluation function and the previous evaluation function is greater than or equal to the preset threshold, then jump to step 3-2-5; if the update iteration number of the control point C is greater than the preset value, then end step 3-2 and execute step 3-3;

[0026] Step 3-2-5, update the position of the control point C based on the gradient of the current evaluation function F, and then jump to step 3-2-2;

[0027] Step 3-2-6: Use the currently obtained curve as the path corresponding to the current connection relationship.

[0028] As a further improvement of the environment-adaptive AGV path navigation method, the method of updating the position of the control point C in step 3-2-5 is:

[0029] Assume that the X-axis coordinate and Y-axis coordinate of the current control point C are and Then the X-axis coordinate of the updated control point C is and the Y-axis coordinate They are:

[0030]

[0031] Where η is the learning rate.

[0032] As a further improvement of the environment-adaptive AGV path navigation method, the calculation method of the evaluation function F is:

[0033]

[0034] Among them, α, β and γ are preset weight coefficients, L is the path length of the currently constructed curve, S is the minimum distance between the curve and the obstacle, and P is the smoothness of the curve.

[0035] As a further improvement of the environment-adaptive AGV path navigation method, in the evaluation function F:

[0036] The path length L is calculated as:

[0037]

[0038] in,

[0039] The minimum distance value S is calculated as follows:

[0040] S=min t∈[0，1] Distance(t);

[0041] Wherein, Distance(t) is the distance between the point on the curve corresponding to the normalized time parameter t and the obstacle;

[0042] The smoothness P is calculated as:

[0043]

[0044] As a further improvement of the environment-adaptive AGV path navigation method, the specific process of step 3-3 is as follows:

[0045] Step 3-3-1, initialize the extension distance step;

[0046] Step 3-3-2, take the coordinates of working point A as the current coordinates (x now ,y now ), the default orientation set for work point A when building the map is used as the current orientation θ;

[0047] Step 3-3-3, determine an intermediate point D according to the current orientation θ and the extension distance step, and its coordinates are:

[0048] x D =x now +step·cos(θ);

[0049] y D =y now +step·sin(θ);

[0050] Step 3-3-4, determine whether the currently obtained middle point D meets the obstacle-free judgment condition, if it does, execute step 3-3-5, otherwise execute step 3-3-6;

[0051] The obstacle-free judgment condition is: there is no obstacle to move from the current coordinate position to the middle point D, and the distance between the middle point D and the nearest obstacle is greater than the safety threshold S threshold ; The safety threshold refers to the minimum safe distance between the AGV and obstacles during movement;

[0052] Step 3-3-5, record the middle point D as one of the path points; if there is no obstacle between the current middle point D and the working point B, connect the working points A, B and all the recorded path points with a straight line to form the path to be determined, and end step 3-3; otherwise, the coordinates of the middle point D are used as the current coordinates (x now ,y now ), take the direction from the previous position to the middle point D as the current orientation θ, and then jump to step 3-3-2 to find the next middle point;

[0053] Step 3-3-6, update the extension distance step = step-△s, △s is the step size; if the current extension distance step is greater than 0, jump to step 3-3-3, and find the next middle point along the original direction with the new extension distance; if the current extension distance step is less than or equal to 0, restore the extension distance step to the initial value, then change the current direction θ, and then jump to step 3-3-3 to find the next middle point according to the new direction.

[0054] As a further improvement of the environment-adaptive AGV path navigation method, the initial value of the extension distance step is set to the safety threshold S threshold 3 times of the step length, △s=S threshold / 2.

[0055] As a further improvement of the environment-adaptive AGV path navigation method: the connection relationship includes a unidirectional connection and a bidirectional connection.

[0056] Compared with the prior art, the present invention has the following positive effects:

[0057] 1. The present invention only needs to mark a small number of working points and the connection relationships between the working points. On this basis, the algorithm can automatically establish the path corresponding to each connection relationship, thereby reducing the workload of on-site implementation personnel and improving work efficiency.

[0058] 2. In the traditional navigation method, AGV needs to determine the points to be passed from a large number of points before determining the moving route, which requires a large amount of calculation. However, the present invention only needs to determine the route based on a small number of working points, which reduces the amount of calculation during navigation and provides more timely response.

[0059] 3. The present invention fully considers two different situations: barrier-free and barrier-existing. At the same time, for the situation with obstacles, two path determination methods, Bezier curve fitting and iterative search for waypoints, are further proposed. The former comprehensively considers the path length, safety distance and smoothness, and can obtain a more ideal path, while the latter ensures that the target location can be reached, ultimately achieving effective and reasonable path planning. BRIEF DESCRIPTION OF THE DRAWINGS

[0060] Figure 1 A schematic diagram of the connection relationship between working points. DETAILED DESCRIPTION

[0061] The technical solution of the present invention is described in detail below in conjunction with the accompanying drawings. Obviously, the described embodiments are only a part of the embodiments of the present invention, rather than all the embodiments.

[0062] An environment-adaptive AGV path navigation method, comprising the following steps:

[0063] Step 1: AGV scans and creates a map to identify the specific coordinates of obstacles in the map.

[0064] Step 2: Select the work points that the AGV must pass through when working in the map, and determine the connection relationship between the work points.

[0065] Determine the default orientation of each work point. When the AGV reaches a work point, it will automatically rotate to the default orientation of that point.

[0066] The connection relationship includes a unidirectional connection and a bidirectional connection.

[0067] like Figure 1 In this embodiment, a total of 6 working points and 7 connection relationships are established, and all of the 7 connection relationships are bidirectional connections.

[0068] Step 3: Determine the path corresponding to each connection relationship.

[0069] Assuming that the working points at both ends of a connection relationship are working point A and working point B, the specific method for determining the path segment is:

[0070] Step 3-1: If there is no obstacle between work point A and work point B, the straight line segment between work point A and work point B is used as the path of the connection relationship; otherwise, execute step 3-2.

[0071] Step 3-2: construct a curve connecting work point A and work point B and passing through the control point by selecting the side control point, and use the curve as the path of the connection relationship; if a curve that meets the requirements cannot be constructed, execute step 3-3.

[0072] The curve is constructed as follows:

[0073] Step 3-2-1. Select the initial position of control point C.

[0074] Step 3-2-2: Construct a second-order Bezier curve from working point A to working point B and passing through control point C. The formula of the curve is:

[0075] x(t)=(1-t) 2 x A +2t(1-t)x C +t 2 x B ;

[0076] y(t)=(1-t) 2 y A +2t(1-t)y C +t 2 y B ;

[0077] Among them, t is the time parameter normalized to [0, 1] within the path, x A ,y A are the X-axis coordinates and Y-axis coordinates of the working point A, respectively. B ,y B are the X-axis coordinates and Y-axis coordinates of the working point B, respectively. C ,y C are the X-axis and Y-axis coordinates of the control point C respectively.

[0078] Step 3-2-3, calculate the evaluation function F of the currently constructed curve:

[0079]

[0080] Among them, α, β and γ are preset weight coefficients, L is the path length of the currently constructed curve, S is the minimum distance between the curve and the obstacle, and P is the smoothness of the curve.

[0081] Specifically, the path length L is calculated as:

[0082]

[0083] in,

[0084] The minimum distance value S is calculated as follows:

[0085] S=min t∈[0，1] Distance(t);

[0086] Wherein, Distance(t) is the distance between the point on the curve corresponding to the normalized time parameter t and the obstacle.

[0087] The smoothness P is calculated as:

[0088]

[0089] Step 3-2-4: If the number of update iterations of the control point C is less than or equal to the preset value, and the difference between the current evaluation function and the previous evaluation function is less than the preset threshold, execute step 3-2-6; if the number of update iterations of the control point C is less than or equal to the preset value, and the difference between the current evaluation function and the previous evaluation function is greater than or equal to the preset threshold, jump to step 3-2-5; if the number of update iterations of the control point C is greater than the preset value, end step 3-2 and execute step 3-3.

[0090] Step 3-2-5: Update the position of control point C based on the gradient of the current evaluation function F, and then jump to step 3-2-2.

[0091] The way to update the position of control point C is:

[0092] Assume that the X-axis coordinate and Y-axis coordinate of the current control point C are and Then the X-axis coordinate of the updated control point C is and the Y-axis coordinate They are:

[0093]

[0094] Among them, η is the learning rate, which determines the step size of each update.

[0095] Step 3-2-6: Use the currently obtained curve as the path corresponding to the current connection relationship.

[0096] Step 3-3: Establish a path from work point A to work point B by gradually searching for intermediate points.

[0097] The specific process is:

[0098] Step 3-3-1, initialize the extension distance step.

[0099] In this embodiment, the initial value of the extension distance step is set to the safety threshold S thresholdThe safety threshold refers to the minimum safe distance between the AGV and obstacles during movement.

[0100] Step 3-3-2, take the coordinates of working point A as the current coordinates (x now ,y now ), and the default orientation set for work point A when building the map is used as the current orientation θ.

[0101] Step 3-3-3, determine an intermediate point D according to the current orientation θ and the extension distance step, and its coordinates are:

[0102] x D =x now +step·cos(θ);

[0103] y D =y now +step·sin(θ).

[0104] Step 3-3-4: Determine whether the currently obtained middle point D meets the obstacle-free judgment condition. If it does, execute step 3-3-5; otherwise, execute step 3-3-6.

[0105] The obstacle-free judgment condition is: there is no obstacle to move from the current coordinate position to the middle point D, and the distance between the middle point D and the nearest obstacle is greater than the safety threshold S threshold .

[0106] Step 3-3-5, record the middle point D as one of the path points; if there is no obstacle between the current middle point D and the working point B, connect the working points A, B and all the recorded path points with a straight line to form the path to be determined, and end step 3-3; otherwise, the coordinates of the middle point D are used as the current coordinates (x now ,y now ), take the direction from the previous position to the middle point D as the current orientation θ, and then jump to step 3-3-2 to find the next middle point.

[0107] Step 3-3-6, update the extension distance step = step-Δs, Δs is the step length, in this embodiment Δs = S threshold / 2; if the current extension distance step is greater than 0, jump to step 3-3-3 and search for the next middle point along the original direction with the new extension distance; if the current extension distance step is less than or equal to 0, restore the extension distance step to the initial value, then change the current direction θ, and then jump to step 3-3-3 to search for the next middle point according to the new direction.

[0108] When changing the current orientation θ, the new orientation can be determined in steps of 5° or 1°.

[0109] It should be noted that for a certain connection relationship, assuming that the working points at both ends are working point A and working point B respectively, no matter the AGV moves from A to B or from B to A, the path is the same, and only the movement direction is opposite.

[0110] Step 4: When the AGV receives the current moving task, it searches for all routes between the current work point and the target work point that can be formed by known connection relationships. According to the path established in step 3, the path length corresponding to each connection relationship in each route is obtained, thereby obtaining the total path length of each route. The route with the shortest total path length is selected as the final route to execute the moving task.

[0111] like Figure 1 , assuming that the current AGV is at work point 1 and the target position is work point 4, there are 4 optional routes searched: 1-2-4, 1-3-4, 1-2-3-4, 1-3-2-4. Since the paths between 1 and 2, 1 and 3, 2 and 3, 2 and 4, and 3 and 4 have been determined in step 3, the total path length of each of the 4 routes can be quickly calculated, and finally the route with the shortest total path length is used for movement.

[0112] It should be noted that it is obvious to those skilled in the art that the present invention is not limited to the details of the above exemplary embodiments, and that the present invention can be implemented in other specific forms without departing from the spirit or essential features of the present invention. The scope of the present invention is defined by the claims rather than the above description.

Claims

1. An environment-adaptive AGV path navigation method, characterized in that: include: Step 1: AGV scans and creates a map to identify the specific coordinates of obstacles in the map; Step 2: Select the work points that the AGV must pass through when working in the map, and determine the connection relationship between the work points; Step 3: Determine the path corresponding to each connection relationship; Step 4: When the AGV receives the current moving task, it searches for all routes between the current work point and the target work point that can be formed by known connection relationships. According to the path established in step 3, the path length corresponding to each connection relationship in each route is obtained, thereby obtaining the total path length of each route. The route with the shortest total path length is selected as the final route to execute the moving task.

2. The environment-adaptive AGV path navigation method according to claim 1, characterized in that: In step 2, the default orientation of each working point must also be determined; when the AGV reaches a certain working point during operation, it will automatically rotate to the default orientation of that point.

3. The environment-adaptive AGV path navigation method according to claim 1, characterized in that: Assuming that the working points at both ends of a connection relationship are working point A and working point B, the specific method for determining the path segment is: Step 3-1, if there is no obstacle between working point A and working point B, the straight line segment between working point A and working point B is used as the path of the connection relationship; otherwise, execute step 3-2; Step 3-2, construct a curve connecting working point A and working point B and passing through the control point by selecting the side control point, and use the curve as the path of the connection relationship; if a curve that meets the requirements cannot be constructed, execute step 3-3; Step 3-3: Establish a path from work point A to work point B by gradually searching for intermediate points.

4. The environment-adaptive AGV path navigation method according to claim 3, characterized in that: In step 3-2, the curve is constructed as follows: Step 3-2-1, select the initial position of control point C; Step 3-2-2: Construct a second-order Bezier curve from working point A to working point B and passing through control point C. The formula of the curve is: x(t)=(1-t) 2 x A +2t(1-t)x C +t 2 x B ; y(t)=(1-t) 2 y A +2t(1-t)y C +t 2 y B ; Among them, t is the time parameter normalized to [0,1] within the path, x A ,y A are the X-axis coordinates and Y-axis coordinates of the working point A, respectively. B ,y B are the X-axis coordinates and Y-axis coordinates of the working point B, respectively. C ,y C are the X-axis coordinate and Y-axis coordinate of the control point C respectively; Step 3-2-3, calculate the evaluation function F of the currently constructed curve; Step 3-2-4, if the update iteration number of the control point C is less than or equal to the preset value, and the difference between the current evaluation function and the previous evaluation function is less than the preset threshold, then execute step 3-2-6; if the update iteration number of the control point C is less than or equal to the preset value, and the difference between the current evaluation function and the previous evaluation function is greater than or equal to the preset threshold, then jump to step 3-2-5; if the update iteration number of the control point C is greater than the preset value, then end step 3-2 and execute step 3-3; Step 3-2-5, update the position of the control point C based on the gradient of the current evaluation function F, and then jump to step 3-2-2; Step 3-2-6: Use the currently obtained curve as the path corresponding to the current connection relationship.

5. The environment-adaptive AGV path navigation method according to claim 4, characterized in that: The method for updating the position of control point C in step 3-2-5 is: Assume that the X-axis coordinate and Y-axis coordinate of the current control point C are and Then the X-axis coordinate of the updated control point C is and the Y-axis coordinate They are: Where η is the learning rate.

6. The environment-adaptive AGV path navigation method according to claim 4, characterized in that: The calculation method of the evaluation function F is: Among them, α, β and γ are preset weight coefficients, L is the path length of the currently constructed curve, S is the minimum distance between the curve and the obstacle, and P is the smoothness of the curve.

7. The environment-adaptive AGV path navigation method according to claim 6, characterized in that: In the evaluation function F: The path length L is calculated as: in, The minimum distance value S is calculated as follows: S=min t∈[0,1] Distance(t); Wherein, Distance(t) is the distance between the point on the curve corresponding to the normalized time parameter t and the obstacle; The smoothness P is calculated as:

8. The environment-adaptive AGV path navigation method according to claim 3, characterized in that: The specific process of step 3-3 is: Step 3-3-1, initialize the extension distance step; Step 3-3-2, take the coordinates of working point A as the current coordinates (x now ,y now ), the default orientation set for work point A when building the map is used as the current orientation θ; Step 3-3-3, determine an intermediate point D according to the current orientation θ and the extension distance step, and its coordinates are: x D =x now +step·cos(θ); and D =and now +step·sin(θ); Step 3-3-4, determine whether the currently obtained middle point D meets the obstacle-free judgment condition, if it does, execute step 3-3-5, otherwise execute step 3-3-6; The obstacle-free judgment condition is: there is no obstacle to move from the current coordinate position to the middle point D, and the distance between the middle point D and the nearest obstacle is greater than the safety threshold S threshold ; The safety threshold refers to the minimum safe distance between the AGV and obstacles during movement; Step 3-3-5, record the middle point D as one of the path points; if there is no obstacle between the current middle point D and the working point B, connect the working points A, B and all the recorded path points with a straight line to form the path to be determined, and end step 3-3; otherwise, the coordinates of the middle point D are used as the current coordinates (x now ,y now ), take the direction from the previous position to the middle point D as the current orientation θ, and then jump to step 3-3-2 to find the next middle point; Step 3-3-6, update the extension distance step = step-Δs, Δs is the step size; if the current extension distance step is greater than 0, jump to step 3-3-3, and find the next middle point along the original direction with the new extension distance; if the current extension distance step is less than or equal to 0, restore the extension distance step to the initial value, then change the current direction θ, and then jump to step 3-3-3 to find the next middle point according to the new direction.

9. The environment-adaptive AGV path navigation method according to claim 8, characterized in that: The initial value of the extension distance step is set to the safety threshold S threshold 3 times, the step size is Δs=S threshold / 2.

10. The environment-adaptive AGV path navigation method according to any one of claims 1 to 9, characterized in that: The connection relationship includes a unidirectional connection and a bidirectional connection.

Images