Path planning method based on improved particle swarm optimization and storage medium

Abstract

The invention discloses a path planning method based on an improved particle swarm algorithm and a storage medium, and the method comprises the steps: generating a grid map of a target region, and enabling grids in the grid map to comprise passable grids and impassable grids; according to the grid map and preset starting and ending points, performing global path planning through a discrete binary particle swarm algorithm to generate a global path; and in the process that the target moving object moves according to the global path, if an obstacle appears on the global path, performing local path planning through a Q learning algorithm, generating a local path to avoid the obstacle, and returning to the global path after the obstacle avoidance is completed. The method can improve the calculation efficiency of path planning, and can be applied to an environment with moving obstacles.

Description

Technical Field

[0001] The present invention relates to the technical field of path planning, and in particular to a path planning method and a storage medium based on an improved particle swarm algorithm. Background Art

[0002] Automated Guided Vehicle (AGV) has the advantages of high flexibility and high degree of automation, and its demand in manufacturing, warehousing and logistics, ports and terminals is growing. As a key part of AGV planning and scheduling, path planning technology enables AGVs to complete tasks faster, plays an important role in saving transportation costs, improving user satisfaction, and increasing corporate profits, and has attracted the attention of many scientific research institutions, enterprises, and universities.

[0003] Path planning is a key technology in the field of robotics, which aims to find the optimal collision-free path from the starting point to the end point. Depending on the available information, path planning can be divided into global path planning and local path planning. The former plans the robot's movement route under the condition of the global environment being known, while the latter focuses more on considering the local information of the robot's current position, so that the robot has good obstacle avoidance capabilities. Therefore, in practical applications, the two are often used in combination to obtain the best path.

[0004] Traditional path planning algorithms include geometric search algorithms, sampling-based path planning algorithms, etc. Dijkstra algorithm, Although geometric search algorithms such as the algorithm can guarantee the shortest path, they may have problems such as high computational cost, low efficiency, and difficulty in adapting to dynamic environments in actual applications due to the influence of the environmental range. Sampling-based path planning algorithms, such as rapid random number search (RRT), Algorithms, PRM algorithms, etc., can generate feasible paths between the starting point and the end point at a faster speed, but they may not be the optimal path.

[0005] In addition, intelligent algorithms are also applied to path planning. Among them, genetic algorithms have good convergence and global search capabilities, but poor local search capabilities and are prone to falling into local optimality. Particle swarm algorithms have a simple structure and are easy to implement, but are also prone to falling into local optimality. Ant colony algorithms are prone to falling into local optimality when facing a more complex state space, and real-time performance is difficult to guarantee. Summary of the invention

[0006] The technical problem to be solved by the present invention is to provide a path planning method and a storage medium based on an improved particle swarm algorithm, which can improve the calculation efficiency of path planning and can be applied to an environment with moving obstacles.

[0007] In order to solve the above technical problems, the technical solution adopted by the present invention is: a path planning method based on an improved particle swarm algorithm, comprising: Generate a grid map of the target area, wherein the grids in the grid map include passable grids and inpassable grids; According to the grid map and the preset starting point and end point, a global path is planned by using a discrete binary particle swarm algorithm to generate a global path; When the target moving object moves according to the global path, if an obstacle appears on the global path, a local path planning is performed through the Q learning algorithm to generate a local path to avoid the obstacle, and the global path is returned after the obstacle avoidance is completed.

[0008] The present invention also provides a computer-readable storage medium on which a computer program is stored. When the program is executed by a processor, the method described above is implemented.

[0009] The beneficial effects of the present invention are: global path planning is performed through the BPSO algorithm (discrete binary particle swarm algorithm), and local path planning is performed through the Q-learning algorithm (Q learning algorithm) when encountering obstacles during movement, which can help the robot find the optimal path from the starting point to the end point in a complex environment, and can be applied to environments with moving obstacles while ensuring high computational efficiency. BRIEF DESCRIPTION OF THE DRAWINGS

[0010] Figure 1 This is a flow chart of a path planning method based on an improved particle swarm algorithm according to the first embodiment of the present invention; Figure 2 This is a global path schematic diagram of the first embodiment of the present invention; Figure 3 This is a schematic diagram of obstacle avoidance according to the first embodiment of the present invention. DETAILED DESCRIPTION

[0011] In order to explain the technical content, achieved objectives and effects of the present invention in detail, the following is an explanation in combination with the implementation modes and the accompanying drawings.

[0012] Please refer to Figure 1 , a path planning method based on improved particle swarm algorithm, comprising: Generate a grid map of the target area, wherein the grids in the grid map include passable grids and inpassable grids; According to the grid map and the preset starting point and end point, a global path is planned by using a discrete binary particle swarm algorithm to generate a global path; When the target moving object moves according to the global path, if an obstacle appears on the global path, a local path planning is performed through the Q learning algorithm to generate a local path to avoid the obstacle, and the global path is returned after the obstacle avoidance is completed.

[0013] From the above description, it can be seen that the beneficial effects of the present invention are: it can be applied to an environment with moving obstacles and can improve the calculation efficiency of path planning.

[0014] Furthermore, generating a grid map of the target area includes: Rasterizing the map of the target area to obtain a raster map; If there is an obstacle in a grid in the grid map, the grid is regarded as an inaccessible grid, otherwise the grid is regarded as a accessible grid.

[0015] As can be seen from the above description, since there may be movable obstacles, the state of the grid is changeable.

[0016] Furthermore, the discrete binary particle swarm algorithm includes: Initialize the particle swarm, preset the size of the particle swarm and the initial position and initial velocity of each particle in the particle swarm; Update the position and velocity of each particle separately; According to the preset objective function, the fitness value of each particle is calculated, and the individual optimal value of each particle and the optimal value of the group are determined; It is determined whether a preset first termination condition is met, and if so, the optimal solution is output; if not, the steps of respectively updating the position and velocity of each particle are continued.

[0017] From the above description, it can be seen that the BPSO algorithm is used for global path planning to generate the optimal path in the target area.

[0018] Furthermore, the particle velocity update formula is:

[0019] in, represents the velocity of the i-th particle in the t+1th iteration, represents the position of the i-th particle in the t-th iteration, represents the individual optimal value of the ith particle after t iterations, represents the optimal value of the group after t iterations, ω represents the inertia weight, c 1 represents the individual learning factor, c 2 represents the group learning factor, r 1 、r 2represents a random number in the interval [0,1], θ represents the preset angle, and Δt represents the time interval between the tth iteration and the t+1th iteration.

[0020] From the above description, we can see that by adding the term θΔt to the velocity update formula, we can add a random amount to the particle velocity update and expand the search and movement range of the particles, thereby enhancing the search ability of the group and improving the convergence ability.

[0021] Furthermore, when the target moving object moves according to the global path, if an obstacle appears on the global path, a local path planning is performed through a Q learning algorithm to generate a local path to avoid the obstacle, and the global path is returned after the obstacle avoidance is completed, including: The target moving object detects and obtains environmental information while moving according to the global path, and determines whether there is an obstacle within a preset minimum distance based on the environmental information; If so, determine whether a collision will occur within a preset time based on the current positions and motion parameters of the target moving object and the obstacle, where the motion parameters include the direction and speed of motion; If a collision occurs, a path point is selected in the global path as the first local target point, and a local path planning is performed through a Q learning algorithm to generate a first local path to avoid obstacles; When obstacle avoidance is completed, the path point in the global path that is closest to the current position is obtained as the second local target point, and local path planning is performed through the Q learning algorithm to generate a second local path to return to the global path.

[0022] Furthermore, the Q learning algorithm includes: Initialize the Q value, where the Q value is Q(s,a), s represents the state, and a represents the action corresponding to the state; According to the preset Q value update formula and reward function, the Q value is updated until the preset second termination condition is met.

[0023] From the above description, it can be seen that the Q-learning algorithm is used for local path planning to avoid moving obstacles.

[0024] The present invention also provides a computer-readable storage medium on which a computer program is stored. When the program is executed by a processor, the method described above is implemented.

[0025] Embodiment 1 Please refer to Figure 1-3, Embodiment 1 of the present invention is: an AGV path planning method based on an improved particle swarm algorithm, which can be applied to an environment with moving obstacles. In this embodiment, the AGV is taken as the target moving object and the working environment of the AGV is taken as the target area for illustration.

[0026] like Figure 1 As shown, the following steps are included: S1: Generate a grid map of the target area.

[0027] Specifically, a grid map of the AGV working environment can be generated based on the grid method using laser radar, IMU (Inertial Measurement Unit), GPS sensor, etc.

[0028] In this embodiment, the grid is divided into two states: 0 and 1, where 0 represents free space, which is a passable area for AGVs; 1 represents an obstacle, which is an inaccessible area. When there is an obstacle in the grid, it is divided into an inaccessible area.

[0029] Since there may be moving obstacles in the target area, the state of the grid is variable. For example, when a moving obstacle moves to a grid with a state of 0 at time t, the state of the grid is updated to 1; when the moving obstacle moves out of the grid at time t+a and there are no other obstacles in the grid, the state of the grid is updated to 0.

[0030] S2: According to the grid map and the preset starting point and end point, a global path planning is performed by using a discrete binary particle swarm algorithm to generate a global path.

[0031] In this embodiment, it is assumed that the starting point is S (x start ,y start ), the end point is T(x target ,y target ), the path P is composed of the path point set {p 1 ,p 2 ,……,p n}, path point p i (p i ∈P) at time t is expressed as ( , ); represents an obstacle, where (x oi ,y oi ) indicates obstacle O i The coordinates of addo oi Record obstacles i Additional information such as the size, speed, direction, etc. of the obstacle.

[0032] The discrete binary particle swarm algorithm mainly includes the following steps: S201: Initialize the particle swarm, set the size of the particle swarm and the initial position and initial velocity of each particle in the particle swarm.

[0033] In this embodiment, the size of the particle group nPop can be 150, and the initial position S (x start ,y start ), the initial velocity can be obtained by random initialization.

[0034] S202: Update the position and velocity of each particle.

[0035] In this embodiment, the particle speed update formula is:

[0036] in, represents the velocity of the i-th particle in the t+1th iteration, represents the position of the i-th particle in the t-th iteration, represents the individual optimal value of the ith particle after t iterations, represents the optimal value of the group after t iterations (the initialization value is used in the first iteration), ω represents the inertia weight; c 1 represents the individual learning factor, c 2 represents the group learning factor. In this embodiment, the individual learning factor c 1 =1.5, group learning factor c 2 =1.5; r 1 、r 2 represents a random number in the interval [0,1], θ represents a preset angle. In this embodiment, the angle θ is randomly generated during initialization, θ=2π(rand-0.5), rand represents a randomly generated angle; Δt represents the time interval between the tth iteration and the t+1th iteration.

[0037] By adding the term θΔt to the velocity update formula, the random amount is added to the particle velocity update, and the search and movement range of the particles are expanded, thereby enhancing the search ability of the group and improving the convergence ability.

[0038] The particle position update formula is:

[0039] in, represents the position of the i-th particle in the t+1th iteration.

[0040] Furthermore, if the velocity or position is outside the search space, the velocity or position of the particle will be clamped to the nearest boundary value.

[0041] S203: According to the preset objective function, the fitness value of each particle is calculated, and the individual optimal value and the group optimal value of each particle are determined. The optimal value includes the optimal fitness value and the optimal position. The objective function is:

[0042]

[0043]

[0044]

[0045]

[0046]

[0047] Among them, (x k ,y k ) represents the coordinates of the particle at the kth iteration; ω 1 ,ω 2 represents the weight factor, ω 1 +ω 2 =1, in this embodiment, ω 1 =ω 2 =0.5; R represents a real number; Represents the path point at time t and The distance between them, n represents the number of path points in the global path, indicates the penalty for colliding with an obstacle, represents the distance between the particle and the center of the obstacle, represents the radius of the obstacle, and j represents the number of obstacles.

[0048] After the fitness value is calculated according to the objective function, if the currently calculated fitness value is less than the individual optimal value pbest, the individual optimal value pbest is updated; if the individual optimal value pbest is less than the group optimal value gbest, the group optimal value gbest is updated.

[0049] S204: Update the inertia weight and the number of iterations.

[0050] In this embodiment, the inertia weight ω is updated according to a preset inertia weight reduction coefficient in each iteration, and in the next iteration, the speed of each particle is updated according to the latest inertia weight.

[0051] In an optional embodiment, the initial inertia weight ω=1, and the inertia weight reduction coefficient wdamp=0.98.

[0052] S205: Determine whether the current state meets the iteration termination condition. If so, output the optimal solution, otherwise execute step S202. The iteration termination condition may be that the optimal value reaches a preset convergence condition (such as the global optimal value is not updated for 10 consecutive iterations), or the number of iterations reaches a preset number threshold (such as 150 times).

[0053] After several iterations, the particle swarm converges to a path, namely the global path. For example, assuming that the size of the grid map is 622×768, the starting point of the AGV is (200, 540) and the end point is (50, 100), the global path is as follows: Figure 2 shown.

[0054] S3: When the target moving object moves according to the global path, if an obstacle appears on the global path, a local path planning is performed through the Q learning algorithm to generate a local path to avoid the obstacle, and then return to the global path after the obstacle avoidance is completed.

[0055] In this embodiment, the AGV can detect environmental information within a certain range during movement, so as to determine whether there are obstacles within the preset minimum distance. Obstacles include dynamic obstacles, such as other AGVs, that is, in this embodiment, other AGVs are regarded as dynamic obstacles with known positions, movement directions, and speeds. Since the position, movement direction, and speed of dynamic obstacles are known, combined with timestamp information, the collision time can be calculated.

[0056] Specifically, the AGV moves toward the end point according to the global path. During the movement, it determines whether there are obstacles within the preset minimum distance. If not, it continues to move according to the global path. If there are obstacles, it determines whether a collision will occur within the preset time (such as the next moment). If no collision occurs, it continues to move according to the global path. If a collision occurs, it learns the corresponding environmental state through the Q-learning algorithm to select the corresponding obstacle avoidance action, and avoids the obstacle by executing the obstacle avoidance action (such as forward, backward, left, and right). After the obstacle avoidance is completed, it returns to the global path to continue moving. If the AGV is deadlocked, it will remain stationary.

[0057] When it is detected that a collision will occur at the next moment, a path point is selected in the global path as the first local target point, and the local path planning is performed through the Q-learning algorithm to generate the first local path to avoid obstacles.

[0058] In a preferred embodiment, the global path point after the current path point is selected as the first local target point. After the global path is determined, the path points that can be reached at each time without encountering obstacles can be calculated based on the speed of the target moving object. Therefore, assuming that the current time is time t, when a collision is detected at time t+1, the path point corresponding to time t+a can be selected as the first local target point, where a is a preset value. In this embodiment, a=10.

[0059] For example, when the AGV moves to a path point on the global path at time t (the path point has not yet reached the global target point) and detects a dynamic obstacle (the position, movement direction, and speed are known), it determines whether a collision will occur at time t+1 based on the movement state of the dynamic obstacle. If a collision will occur, the grid map of the global path point range corresponding to the AGV from time t to time t+10 is obtained and scaled into a 10×10 grid map. Then, the global path point corresponding to time t+10 is used as the first local target point, and obstacle avoidance is performed using the Q-learning algorithm.

[0060] When obstacle avoidance is completed, the path point closest to the current position (i.e. the position after obstacle avoidance) in the global path is found as the second local target point, and the AGV moves to the second local target point to return to the global path. After returning to the global path, it continues to move along the global path to the end point.

[0061] Among them, the Q-learning algorithm is a reinforcement learning method that aims to learn a value function Q(s,a), where s represents the state and a represents the action taken in the state. The Q value is maximized by selecting from all possible actions a in state s.

[0062] In this embodiment, the Q value update formula is:

[0063] Among them, Q(s t ,a t ) represents the state s at time t t Take action a t Q value, α represents the learning rate, r(s t ,a t ) means taking action a t The reward obtained after is calculated by the reward function R(s, a); γ represents the discount factor; Indicates that in the next state s t+1 Take the maximum Q value among all possible actions. In this embodiment, α=0.1, γ=0.9.

[0064] When the Q-learning algorithm is used for path planning, Q(s,a) is first randomly initialized, and then the Q value is iteratively updated by exploring the environment and using the Q value update formula and reward function. As the number of iterations increases, the AGV will have a better understanding of the environment, and the Q value will tend to converge or reach the set number of iterations. In this embodiment, the number of iterations is 5000.

[0065] After the Q value converges, AGV can choose the best action to take in each state to find the optimal path. The reward function is:

[0066] Among them, R collision Indicates the next state s k+1 The penalty for collision with an obstacle, R goal Indicates reaching the local target point T'(x target ,y target ) reward, R moving Indicates a mobile bonus.

[0067] Assume that at time t, the AGV moves along the global path to the path point (x 1 ,y 1 ) and detected (x 2 ,y 2 ), the next state can be judged. Assume that the AGV and the dynamic obstacle will be at (x 0 ,y 0 ), the Q-learning algorithm is used to select obstacle avoidance actions, including forward, backward, left, and right.

[0068] Assuming that the AGV and the dynamic obstacle are moving towards each other, several situations may occur at this time: (1) The obstacle avoidance action performed by the AGV at time t+1 is forward, and the obstacle continues to move and collides.

[0069] (2) The obstacle avoidance action performed by the AGV at time t+1 is backward. The obstacle continues to move, and it is determined that there will still be a collision at the next moment, so the obstacle avoidance action is selected to avoid the obstacle.

[0070] (3) The obstacle avoidance action performed by the AGV at time t+1 is to move left. The obstacle continues to move. It determines whether there are any obstacles at the current position that may cause a collision. If so, it continues to avoid the obstacle. If not, it returns to the global path.

[0071] (4) The obstacle avoidance action performed by the AGV at time t+1 is to the right. The obstacle continues to move, and it determines whether there are any obstacles at the current position that may cause a collision. If so, it continues to avoid the obstacle. If not, it returns to the global path.

[0072] (5) The AGV is stuck and waits in place. The obstacle continues to move. It determines whether there are any obstacles at the current position that may cause a collision. If so, it continues to avoid obstacles. If not, it returns to the global path.

[0073] For example, in Figure 2 Based on this, the AGV detected a position of (124, 515) and a speed of v x =3, v y = -1, and it is determined that a collision will occur at the next moment, then avoid the obstacle, such as Figure 3 shown.

[0074] This embodiment uses the BPSO algorithm for global path planning to generate the optimal path in the target area, and uses the Q-learning algorithm for local path planning to avoid moving obstacles. By combining the advantages of the swarm intelligence algorithm and the reinforcement learning algorithm, the safety and accuracy of the planned path can be improved.

[0075] Embodiment 2 This embodiment is a computer-readable storage medium corresponding to the above embodiment, on which a computer program is stored. When the program is executed by the processor, each step of a path planning method based on an improved particle swarm algorithm in the above embodiment is implemented, and the same technical effect can be achieved, which will not be repeated here.

[0076] In summary, the present invention provides a path planning method and storage medium based on an improved particle swarm algorithm, which performs global path planning through the BPSO algorithm and performs local path planning through the Q-learning algorithm when encountering obstacles during movement. It can help the robot find the optimal path from the starting point to the end point in a complex environment, and can be applied to environments with moving obstacles while ensuring high computational efficiency.

[0077] The above descriptions are merely embodiments of the present invention and are not intended to limit the patent scope of the present invention. Any equivalent transformations made using the contents of the present invention's specification and drawings, or directly or indirectly applied in related technical fields, are also included in the patent protection scope of the present invention.

Claims

1. A path planning method based on an improved particle swarm algorithm, characterized in that: include: Generate a grid map of the target area, wherein the grids in the grid map include passable grids and inpassable grids; According to the grid map and the preset starting point and end point, a global path planning is performed by using a discrete binary particle swarm algorithm to generate a global path; When the target moving object moves according to the global path, if an obstacle appears on the global path, a local path planning is performed through the Q learning algorithm to generate a local path to avoid the obstacle, and the global path is returned after the obstacle avoidance is completed.

2. The path planning method according to claim 1, characterized in that: The generating a grid map of the target area includes: Rasterizing the map of the target area to obtain a raster map; If there is an obstacle in a grid in the grid map, the grid is regarded as an inaccessible grid, otherwise the grid is regarded as a accessible grid.

3. The path planning method according to claim 1, characterized in that: The discrete binary particle swarm algorithm includes: Initialize the particle swarm, preset the size of the particle swarm and the initial position and initial velocity of each particle in the particle swarm; Update the position and velocity of each particle separately; According to the preset objective function, the fitness value of each particle is calculated, and the individual optimal value of each particle and the optimal value of the group are determined; It is determined whether a preset first termination condition is met, and if so, the optimal solution is output; if not, the steps of respectively updating the position and velocity of each particle are continued.

4. The path planning method according to claim 3, characterized in that: The objective function is: Among them, (x k ,y k ) represents the coordinates of the particle at the kth iteration; ω1, ω2 represent weight factors, ω‍1+ω2=1; R represents a real number; Represents the path point at time t and The distance between indicates the penalty for colliding with an obstacle, represents the distance between the particle and the center of the obstacle, represents the radius of the obstacle, and j represents the number of obstacles.

5. The path planning method according to claim 3, characterized in that: The particle velocity update formula is: in, represents the velocity of the i-th particle in the t+1th iteration, represents the position of the i-th particle in the t-th iteration, represents the individual optimal value of the ith particle after t iterations, represents the optimal value of the group after t iterations, ω represents the inertia weight, c1 represents the individual learning factor, c2 represents the group learning factor, r1 and r2 represent random numbers in the interval [0,1], θ represents the preset angle, and Δt represents the time interval between the tth iteration and the t+1th iteration.

6. The path planning method according to claim 1, characterized in that: In the process of the target moving object moving according to the global path, if an obstacle appears on the global path, a local path planning is performed through a Q learning algorithm to generate a local path to avoid the obstacle, and the global path is returned after the obstacle avoidance is completed, including: The target moving object detects and obtains environmental information while moving according to the global path, and determines whether there is an obstacle within a preset minimum distance based on the environmental information; If so, determine whether a collision will occur within a preset time based on the current positions and motion parameters of the target moving object and the obstacle, where the motion parameters include the direction and speed of motion; If a collision occurs, a path point is selected in the global path as the first local target point, and a local path planning is performed through a Q learning algorithm to generate a first local path to avoid obstacles; When obstacle avoidance is completed, the path point in the global path that is closest to the current position is obtained as the second local target point, and local path planning is performed through the Q learning algorithm to generate a second local path to return to the global path.

7. The path planning method according to claim 1, characterized in that: The Q-learning algorithm includes: Initialize the Q value, where the Q value is Q(s,a), s represents the state, and a represents the action corresponding to the state; According to the preset Q value update formula and reward function, the Q value is updated until the preset second termination condition is met.

8. The path planning method according to claim 7, characterized in that: The Q value update formula is: Among them, Q(s t ,a t ) represents the state s at time t t Take action a t Q value, α represents the learning rate, r(s t ,a t ) means taking action a t The reward obtained after , γ represents the discount factor; Indicates that in the next state s t+1 The maximum Q value among the corresponding actions is taken.

9. The path planning method according to claim 7, characterized in that: The reward function is: Among them, R collision Indicates the next state s k+1 The penalty for collision with an obstacle, R goal Indicates reaching the local target point T'(x target ,y target ) reward, R moving Indicates a mobile bonus.

10. A computer-readable storage medium having a computer program stored thereon, characterized in that: When the program is executed by a processor, the method according to any one of claims 1 to 9 is implemented.

Images