An AGV Dynamic Obstacle Avoidance Method Based on Improved Velocity Obstacle

Abstract

An AGV dynamic obstacle avoidance method based on improved speed obstacle aims to achieve obstacle avoidance for dynamic obstacles by AGV while taking into account both efficiency and safety. Its characteristics include the following steps: 1. According to the speed and direction of the dynamic obstacle, use the Kalman filtering algorithm to predict the position of the obstacle at the next moment; 2. Construct a speed obstacle buffer zone based on the predicted position of the obstacle; 3. Perform multi-objective optimization on the two objective functions of efficiency and safety, and select the optimal speed. The present invention predicts the position of the dynamic obstacle based on the Kalman filtering algorithm and constructs a speed obstacle model accordingly. In the actual AGV motion environment, there is a possibility of sudden changes in the speed of dynamic obstacles and unstable turning speeds of AGV cars. The method of the present invention is based on the idea of forward simulation, constructs a speed obstacle buffer zone according to the predicted position of the obstacle, and realizes AGV dynamic obstacle avoidance while taking into account both efficiency and safety.

Description

Technical Field

[0001] The present invention relates to an AGV dynamic obstacle avoidance technology, in particular to an AGV dynamic obstacle avoidance method that takes into account both efficiency and safety, specifically an AGV dynamic obstacle avoidance method based on an improved speed obstacle method. Background Art

[0002] During the movement of an AGV cart, it is often necessary to detect whether it will collide with an obstacle. If the AGV cart fails to successfully identify and avoid the obstacle, resulting in a collision, it may not only cause property losses, but in severe cases, it may even pose a threat to the safety of surrounding operators. In the actual AGV movement environment, there is a possibility of sudden changes in the speed of dynamic obstacles and unstable steering speeds of AGV carts.

[0003] During the process of an AGV avoiding dynamic obstacles, the AGV cart needs to continuously adjust its speed and direction. At this time, due to different road surface factors such as slippery roads, grasslands, and gravel roads, the stability control of the AGV cart may have problems, resulting in obstacle avoidance failure. For example, Figure 6 as shown, for the current relative speed V AB being located within the collision cone, if the AGV selects V C as the obstacle avoidance speed for the next moment, when the AGV steers and switches speeds, if the road surface is slippery, the actual speed may be V real Even if the relative speed is outside the collision cone, it is not the desired obstacle avoidance strategy and the trajectory significantly deviates from the destination; and when encountering difficult-to-drive road surface conditions such as grasslands, as Figure 7 shown, if the AGV selects V C as the obstacle avoidance speed for the next moment, the final actual obstacle avoidance speed is V real and its relative speed is still within the collision cone, in which case obstacle avoidance failure may occur.

[0004] Based on the idea of forward simulation, the present invention combines a collision buffer zone with the speed obstacle method. When the AGV cart overtakes from in front of the obstacle, a collision cone is generated according to the estimated position of the obstacle at the next moment. This method can avoid the obstacle with a small turning angle in the early stage of obstacle avoidance; at the same time, multi-objective optimization is performed on two objective functions of efficiency and safety, and the optimal speed is selected, taking into account both efficiency and safety. Summary of the Invention

[0005] The purpose of the present invention is to use the Kalman filtering algorithm to predict the position of dynamic obstacles in advance, construct a speed obstacle buffer zone according to the predicted position of the obstacles, and achieve AGV dynamic obstacle avoidance while taking into account both efficiency and safety.

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

[0007] An AGV dynamic obstacle avoidance method based on improved velocity obstacles, characterized in that: First, use the Kalman filtering algorithm to estimate the possible position of the obstacle at the next moment. Subsequently, based on the idea of forward simulation, construct a velocity obstacle buffer according to the predicted position of the obstacle. Finally, perform multi-objective optimization on two objective functions of efficiency and safety, and select the optimal velocity. The specific steps are as follows:

[0008] Step 1: Set up three regions centered on the AGV vehicle according to the AGV vehicle's own motion parameters and sensor information: a safety region, an obstacle avoidance region, and an emergency obstacle avoidance region;

[0009] Step 2: When it is detected that the obstacle is located in the obstacle avoidance region, use the Kalman filtering algorithm to update and iterate the position of the obstacle to obtain a more accurate prediction result of the dynamic obstacle position;

[0010] Step 3: The AGV vehicle constructs a velocity obstacle buffer using the estimation result of the Kalman filtering algorithm;

[0011] Step 4: Calculate the obstacle avoidance velocity interval of the AGV vehicle according to the velocity obstacle buffer constructed in Step 3;

[0012] Step 5: Calculate the velocity gap of the AGV vehicle to evaluate the safety of the obstacle avoidance velocity;

[0013] Step 6: Calculate the angle between the obstacle avoidance velocity direction of the AGV vehicle and the boundary velocity direction of the collision cone to evaluate the efficiency of the obstacle avoidance velocity in avoiding obstacles;

[0014] Step 7: Perform dimensionless processing on the two evaluation indexes of safety and efficiency calculated in Step 5 and Step 6. After eliminating the influence of dimensions, use the linear weighted method to perform multi-objective optimization on the two objective functions of safety and efficiency, and select the optimal velocity within the obstacle avoidance velocity interval as the final obstacle avoidance velocity of the AGV vehicle.

[0015] In the above Step 1: Since the prediction result of the Kalman filtering algorithm is not very accurate at the beginning, there is a large error in the estimated value of the Kalman filtering algorithm at the beginning, but this error will gradually decrease after several iterations and finally tend to be stable. Therefore, in this paper, three regions are set within the range that the AGV vehicle can perceive, as Figure 1 shown.

[0016] In the above Step 2: When it is detected that the obstacle is located in the obstacle avoidance region, perform the prediction and update operations of the Kalman filtering algorithm based on the current speed position of the dynamic obstacle. Use the optimal result of the previous moment to predict the prior estimate value, and at the same time use the observation value (the measurement value of the AGV vehicle sensor) to correct the prior estimate value to obtain the optimal estimate value. As shown in formula (2), this formula is the prediction formula, defining Xk is the state of the obstacle at the current moment, X = [x, y, v x , v y , respectively representing the position and velocity of the target on the plane; A represents the state transition matrix, constructed according to the motion state of the target;

[0017]

[0018] Formula (3) is the update formula of the Kalman filtering algorithm, where K k is the Kalman gain, and z k is the observed value.

[0019]

[0020] In step 3: The velocity obstacle buffer CC AB is a collision cone constructed based on the predicted position of the obstacle, the AGV velocity, and the obstacle velocity, and can be represented by formula (4):

[0021]

[0022] In the formula, V AB is the relative velocity, ρ AB is the straight line where the relative velocity V AB is located, is a circular area with the center of the predicted position c b,t+1 of the obstacle as the center and ( where r A , r B are the AGV radius and the obstacle radius, respectively) as the radius.

[0023] In step 4: First, use the velocity obstacle method combined with the dynamic window to screen out the obstacle avoidance velocity interval that the AGV can actually reach in the next time interval according to the actual kinematic model of the AGV car, and make a selection from it. Assume that at the current moment, the range of the speed magnitude that the AGV car can reach is [v min , v max , the heading angle range is

[0024] [ω min , ω max , the minimum speed interval is Δv, and the minimum heading angle interval is Δω. Then the number num of obstacle avoidance speeds is as shown in formula (1)

[0025]

[0026] In step 5: Define the speed gap dist as the perpendicular distance from the center of the circle where the dynamic obstacle is located to the line of the relative speed between the AGV and the obstacle. When it is detected that there is a risk of collision between the AGV and the obstacle, assuming that the motion states of the AGV and the obstacle do not change, at this time, the speed gap dist can be expressed by the following formula (5):

[0027]

[0028] In the formula, C A , C B are respectively the center of the AGV and the center of the obstacle, and θ is the angle between the vector and the relative speed V AB .

[0029] In step 6: Select the boundary speed of the improved velocity obstacle method collision cone as the most ideal obstacle avoidance direction. The smaller the angle avo between the obstacle avoidance speed direction and the boundary speed direction, the better the obstacle avoidance efficiency of the obstacle avoidance speed. Additionally, if two obstacle avoidance speeds are on both sides of the collision cone (the collision cone angle is ), but the included angles are the same. In order to preferentially select the speed outside the collision cone, therefore, if the speed is inside the collision cone, a positive constant κ needs to be subtracted.

[0030] When the speed is outside the collision cone, its expression is as shown in formula (6):

[0031]

[0032] When the speed is inside the collision cone, its expression is as shown in formula (7):

[0033]

[0034] In step 7: Use the method of summation normalization to realize the quantification of indicators. Taking the sum value of the indicators as the reference standard, the obtained data is equivalent to the proportion of the sum, so as to convert the initial value of the indicators into a dimensionless evaluation value. Its calculation formula is as shown in (8):

[0035]

[0036] In the formula, X is the evaluation index to be input. Subsequently, in order to comprehensively consider the influence of the two indicators, the results obtained after summation normalization are subjected to linear weighted operation to generate an obstacle avoidance speed comprehensive evaluation objective function, as shown in formula (9).

[0037] G = α·dist + β·avo (9)

[0038] Among them, α and β are the weights of two evaluation indicators, dist is the speed gap, which is used to evaluate the safety of the obstacle avoidance speed, and avo is the angle between the obstacle avoidance speed direction and the collision cone boundary speed direction, which is used to evaluate the efficiency of the obstacle avoidance speed to escape from the obstacle. Finally, all the obtained comprehensive evaluation indicators G of the obstacle avoidance speed are sorted in descending order according to the numerical values, and the optimal obstacle avoidance speed is selected as the running speed for the next moment.

[0039] The beneficial effects of the present invention are:

[0040] (1) Based on the idea of forward simulation, the present invention combines the collision buffer zone with the speed obstacle method. When the AGV vehicle overtakes from in front of the obstacle, a collision cone is generated according to the estimated position of the obstacle at the next moment. This method can avoid the obstacle with a small turning angle in the early stage of obstacle avoidance, ensuring the stability of the vehicle operation.

[0041] (2) The present invention performs multi-objective optimization on the two objective functions of efficiency and safety, and selects the optimal obstacle avoidance speed. And simulation experiments are carried out. The experimental results show that the algorithm proposed by the present invention selects a path with a relatively small turning arc compared with the adaptive speed obstacle method in the selection of the path, which is beneficial to ensuring the stability of the AGV vehicle operation, and at the same time the obstacle avoidance path length is also relatively small, ensuring the obstacle avoidance efficiency. Description of the Drawings

[0042] Figure 1 is a schematic diagram of the obstacle avoidance area of the present invention.

[0043] Figure 2 is a schematic diagram of the speed obstacle buffer zone of the present invention.

[0044] Figure 3 is a schematic diagram of the speed gap of the present invention.

[0045] Figure 4 is a flowchart of the present invention.

[0046] Figure 5 is a simulation result diagram of the present invention.

[0047] Figure 6 is the current relative speed V of the existing AGV vehicle AB One of the situations where obstacle avoidance failure still occurs when it is located within the collision cone.

[0048] Figure 7 is the current relative speed V of the existing AGV vehicle AB Another situation where obstacle avoidance failure still occurs when it is located within the collision cone.

[0049] Table 1 shows the comparison results of the algorithm in which the present invention simulates the situation of the AGV vehicle slipping during the simulation process and adds a random sliding deviation of 0-50% during the vehicle operation. Detailed implementation manners

[0050] The present invention will be further described in detail below with reference to the accompanying drawings and embodiments.

[0051] As Figures 1-5 shown.

[0052] A dynamic obstacle avoidance method for AGV based on improved speed obstacle. First, the Kalman filtering algorithm is used to estimate the possible position of the obstacle at the next moment. Subsequently, a speed obstacle model is constructed according to the predicted obstacle position. Finally, multi-objective optimization is performed on two objective functions of efficiency and safety, and the optimal speed is selected.

[0053] Figure 4 The flowchart of the dynamic obstacle avoidance implementation strategy of the AGV car of the present invention is as follows. Specifically, it includes the following steps:

[0054] Step 1: Detect whether the dynamic obstacle is in the obstacle avoidance area;

[0055] Since the sensing distance of the sensors of the AGV car is limited, obstacle avoidance can only be performed within the sensing range of the car sensors. In the triple area set by the present invention, the boundary between the obstacle avoidance area and the safety area is the maximum sensing range of the AGV car sensors, as Figure 1 shown. Set the obstacle avoidance start flag S i , initially 0, and set it to 1 when the AGV car detects a dynamic obstacle. Continuously detect the obstacle avoidance start flag S I ;

[0056] if (S i == 0): v

[0057] The AGV car does not detect a dynamic obstacle and travels towards the target along the established path.

[0058] else:

[0059] Jump to step 2 to start local obstacle avoidance.

[0060] Step 2: Use the Kalman filtering algorithm to iteratively predict the obstacle position;

[0061] Define X k as the prior estimated state of the obstacle at the current moment, X = [x, y, v x , v y , respectively representing the position and speed of the target on the plane; A represents the state transition matrix, constructed according to the motion state of the target, K k is the Kalman gain, z k is the observed value, P kThe prior estimation covariance matrix of the state quantity X at time k is predicted based on the dynamic obstacle information obtained by the AGV sensor. The prior estimation state prediction formula for the dynamic obstacle is:

[0062] X k = Ax k-1 + Bu k-1 (1)

[0063] where u k-1 is the control input and B is the control matrix. The prior estimation covariance matrix can be initialized as the identity matrix:

[0064] P k = AP k-1 A T + Q (2)

[0065] Q is the covariance matrix of the process noise.

[0066] The optimal estimation formula for the dynamic obstacle position after each iteration is:

[0067] X^ k = X k + K k (z k - Hx k ) (3)

[0068] where K k is the Kalman gain and z k is the observed value of the car state information.

[0069] Step 3: Construct the velocity obstacle buffer zone of the AGV and the obstacle;

[0070] According to the prediction result of Step 2, as Figure 2 shown, c a represents the position of the AGV car at the current moment, c b,t is the actual position of the obstacle at the current moment, c b,t+1 is the predicted position of the obstacle at the next moment estimated according to the Kalman filtering algorithm, and the velocity obstacle buffer zone CC AB is a collision cone constructed based on the predicted position of the obstacle, the AGV speed, and the obstacle speed, which can be expressed by formula (4):

[0071]

[0072] In the formula, V AB is the relative speed, ρ AB is the straight line where the relative speed V AB is located, is centered on the predicted position c b,t+1 of the obstacle, with ( where r A and r B are the radius of the AGV and the radius of the obstacle respectively) is a circular area with the given radius.

[0073] Step 4: Calculate the speed gap of the AGV, as shown in Figure 3 .

[0074] Define the speed gap dist as the perpendicular distance from the center of the dynamic obstacle to the line where the relative speed of the AGV and the obstacle lies. Calculate the speed gap:

[0075]

[0076] In the formula, C A and C B are the center of the AGV and the center of the obstacle respectively, and θ is the angle between the vector and the relative speed V AB .

[0077] Step 5: Calculate the angle between the obstacle avoidance speed direction of the AGV and the boundary speed direction of the collision cone

[0078] Select the boundary speed of the improved speed obstacle method collision cone as the most ideal obstacle avoidance direction. The smaller the angle avo between the obstacle avoidance speed direction and the boundary speed direction, the better the obstacle avoidance efficiency of the obstacle avoidance speed. Additionally, if two obstacle avoidance speeds are on both sides of the collision cone (the collision cone angle is ) and the angles are the same, in order to preferentially select the speed outside the collision cone, therefore, if the speed is inside the collision cone, a positive constant κ needs to be subtracted.

[0079] When the speed is outside the collision cone, its expression is as shown in formula (6):

[0080]

[0081] When the speed is inside the collision cone, its expression is as shown in formula (7):

[0082]

[0083] Step 6: Perform dimensionless processing on the two evaluation indicators of safety and efficiency calculated in Step 5 and Step 4. After eliminating the dimension influence, the comprehensive evaluation objective function of the obstacle avoidance speed of the AGV is obtained as:

[0084] G = α·dist + β·avo (8)

[0085] Among them, α and β are the weights of two evaluation indicators, dist is the speed gap, which is used to evaluate the safety of the obstacle avoidance speed, and avo is the angle between the obstacle avoidance speed direction and the boundary speed direction of the collision cone, which is used to evaluate the efficiency of the obstacle avoidance speed to escape from the obstacle. The optimal obstacle avoidance speed is selected as the obstacle avoidance strategy of the AGV vehicle.

[0086] The simulation results of this embodiment are as Figure 5 shown, and the performance comparison considering the slipping factor is shown in Table 1.

[0087] Table 1 Performance comparison considering the slipping factor

[0088]

[0089] The parts not involved in the present invention are the same as or can be implemented by the prior art.

Claims

1. An AGV dynamic obstacle avoidance method based on improved velocity obstacles Its characteristics are as follows: First, use Kalman filtering to predict the possible position of the obstacle at the next moment. Subsequently, based on the idea of forward simulation, construct a velocity obstacle buffer according to the predicted position of the obstacle. Finally, perform multi-objective optimization on the two objective functions of efficiency and safety, and select the optimal velocity; specifically, it includes the following steps: Step 1: Set up three regions centered on the AGV vehicle according to the AGV vehicle's own motion parameters and sensor information: a safety region, an obstacle avoidance region, and an emergency obstacle avoidance region; Step 2: When it is detected that the obstacle is in the obstacle avoidance region, use the Kalman filtering algorithm to update and iterate the position of the obstacle to obtain a more accurate prediction result of the dynamic obstacle position; Step 3: The AGV vehicle constructs a velocity obstacle buffer using the estimation result of the Kalman algorithm; Step 4: Calculate the obstacle avoidance speed interval of the AGV according to the speed obstacle buffer constructed in Step 3; First, use the speed obstacle method combined with the dynamic window to screen out the actual obstacle avoidance speed interval that the AGV reaches in the next time interval according to the actual kinematic model of the AGV, and select the optimal speed from it; Assume that at the current moment, the range of the speed magnitude reached by the AGV is [v min , v max , the range of the heading angle is [ω min , ω max , the minimum speed interval is Δv, and the minimum heading angle interval is Δω. Then the number of obstacle avoidance speeds num is shown in formula (1): Step 5: Calculate the velocity gap of the AGV vehicle to evaluate the safety of the obstacle avoidance velocity; Step 6: Calculate the angle between the obstacle avoidance velocity direction of the AGV vehicle and the boundary velocity direction of the collision cone to evaluate the efficiency of the obstacle avoidance velocity in avoiding obstacles; Step 7: Perform dimensionless processing on the two evaluation indicators of safety and efficiency calculated in Step 5 and Step 6. After eliminating the influence of dimensions, use the linear weighted method to multi-objectively optimize the two objective functions of safety and efficiency, and select the optimal velocity within the obstacle avoidance velocity range as the final obstacle avoidance velocity of the AGV vehicle.

2. The method according to claim 1, characterized in that, In Step 1: Since the prediction result of the Kalman filtering algorithm is not very accurate at the beginning, there is a large error in the estimated value of the Kalman filtering algorithm at the beginning, but this error will gradually decrease after several iterations and finally tend to be stable. Therefore, three regions are set within the range that the AGV vehicle can sense: a safety region, an obstacle avoidance region, and an emergency obstacle avoidance region.

3. The method according to claim 1, characterized in that In step 2: When it is detected that an obstacle is located in the obstacle avoidance area, prediction and update operations of the Kalman filtering algorithm are performed based on the current speed and position of the dynamic obstacle. The optimal result of the previous moment is used to predict the prior estimate value. At the same time, the prior estimate value is corrected using the observed value, i.e., the measurement value of the AGV car sensor, to obtain the optimal estimate value, as shown in formula (2). This formula is the prediction formula, and X k represents the state of the obstacle at the current moment, and X = [x, y, v x , v y , which respectively represent the position and speed of the target on the plane; A represents the state transition matrix, which is constructed according to the motion state of the target; Formula (3) is the update formula of the Kalman filter algorithm, where K k is the Kalman gain, and z k is the observed value 4. The method according to claim 1, wherein In the said step 3: the speed obstacle buffer CC AB is a collision cone constructed based on the predicted position of the obstacle, the speed of the AGV, and the speed of the obstacle, and can be expressed by formula (4): where V AB is the relative velocity, ρ AB is the straight line where the relative velocity V AB is located, is a circular area with the center of the predicted position c b,t+1 of the obstacle as the center and as the radius, where r A , r B are the radius of the AGV and the radius of the obstacle respectively.

5. The method according to claim 1, characterized in that, In Step 5: Define the velocity gap dist as the perpendicular distance from the center of the circle where the dynamic obstacle is located to the straight line of the relative velocity between the AGV vehicle and the obstacle; when it is detected that there is a risk of collision between the AGV and the obstacle, assuming that the motion states of the AGV vehicle and the obstacle do not change, at this time, the velocity gap dist can be expressed by the following formula (5): where C A , C B are respectively the centers of the AGV vehicle and the obstacle, and θ is the angle between the vector and the relative velocity V AB .

6. The method according to claim 1, wherein In the said step 6: Select the boundary velocity of the improved speed obstacle method collision cone as the most ideal obstacle avoidance direction. The smaller the angle avo between the obstacle avoidance velocity direction and the boundary velocity direction, the better the obstacle avoidance efficiency of the obstacle avoidance velocity. Additionally, if two obstacle avoidance velocities are on both sides of the collision cone with a collision cone angle of but the included angles are the same. To preferentially select the velocity outside the collision cone, therefore, if the velocity is inside the collision cone, a constant κ needs to be subtracted additionally, where κ is a positive number; when the velocity is outside the collision cone, its expression is as shown in formula (6): When the velocity is within the collision cone, its expression is as shown in formula (7):

7. The method according to claim 1, characterized in that, In Step 7: Use the method of summation normalization to realize the quantification of the index. Using the sum value of the index as the reference standard, the obtained data is equivalent to the proportion of the sum, so as to convert the initial value of the index into a dimensionless evaluation value; Its calculation formula is as shown in (8): In the formula, X is the evaluation index to be input; subsequently, in order to comprehensively consider the influence of the two indexes, perform a linear weighted operation on the result obtained after summation normalization to generate an integrated evaluation objective function for the obstacle avoidance velocity, as shown in formula (9); G = α·dist + β·avo (9) Among them, α and β are the weights of the evaluation indicators, dist is the speed gap, which is used to evaluate the safety of the obstacle avoidance speed, and avo is the angle between the obstacle avoidance speed direction and the boundary speed direction of the collision cone, which is used to evaluate the efficiency of the obstacle avoidance speed to escape from the obstacle; finally, all the obtained comprehensive evaluation indicators G of the obstacle avoidance speed are sorted in descending order according to the numerical values, and the optimal speed is selected as the running speed for the next moment.

Images