Grey wolf optimization algorithm and reeds-shepp curve-based search path optimization method for jump point

Abstract

The present application relates to a kind of based on grey wolf optimization algorithm and Reeds-Shepp curve's jump point search path optimization method, belong to mobile robot path planning field.The method includes: S1: the path obtained according to jump point search is generated for mobile robot at each path node except start and end point the predicted attitude of mobile robot when reaching the node;S2: according to the judgment standard of the characteristics of improved Reeds-Shepp curve and whether the current node can be merged, merge node;S3: based on the predicted attitude, use grey wolf optimization algorithm to regenerate the new attitude of mobile robot when passing through each node;In the case where node position is not changed, so that mobile robot passes through in shortest time;S4: based on path coordinate, use grey wolf optimization algorithm to generate new position coordinate in the neighborhood of the position coordinate of each node, and synthesis Reeds-Shepp path, guarantee that mobile robot can pass through in shortest time.

Description

Technical Field

[0001] This invention belongs to the field of mobile robot path planning, and relates to a method for optimizing the search path for jump points of mobile robots based on the Grey Wolf Optimization Algorithm and Reeds-Shepp curve. Background Technology

[0002] Robots are currently the most widely used intelligent mechanical devices in various industries. Among them, wheeled mobile robots are increasingly being used in daily production and life due to their unique properties. Therefore, the control requirements for wheeled mobile robots are becoming more diverse, and how to design stable and excellent path planning and motion control methods is becoming a research hotspot.

[0003] Path planning for mobile robots refers to the process by which a robot calculates a path based on known information that allows it to reach its destination from its starting point while avoiding obstacles and meeting various performance requirements. To shorten the time to reach the target point and reduce the path length, mobile robots must find an optimal path planning algorithm during navigation. Commonly used path planning algorithms include artificial potential field methods, fuzzy logic algorithms, Dijkstra's algorithm, and A* algorithm. In recent years, scholars both domestically and internationally have also proposed many intelligent algorithms, such as genetic algorithms, particle swarm optimization, and ant colony optimization.

[0004] In the geometric grid method, the A* algorithm, as a heuristic algorithm that uses an evaluation function to traverse the non-obstacle neighbors of the current node to find the shortest path, has advantages such as simplicity, ease of operation, and high accuracy. However, the A* algorithm suffers from excessively long runtime due to its search and calculation of a large number of useless neighbor nodes during the shortest path search process. Furthermore, the path calculated by the A* algorithm is actually the shortest path among discrete cells in the given map model, which differs significantly from the actual optimal path.

[0005] In response to the shortcomings of the A* algorithm, Daniel Harabor et al. proposed the jump point search algorithm in 2011. While retaining the original A* algorithm framework, the algorithm further optimized the operation of finding subsequent nodes. However, the path planning result is not the actual optimal motion trajectory.

[0006] Therefore, there is an urgent need for an improved jump point search algorithm to optimize the path of mobile robots. Summary of the Invention

[0007] In view of this, the purpose of this invention is to provide a method for optimizing the search path of jump points for mobile robots based on the gray wolf optimization algorithm and the Reeds-Shepp curve, so as to solve the problem of how to combine path planning and motion control and ensure that the mobile robot can pass through in the shortest time.

[0008] To achieve the above objectives, the present invention provides the following technical solution:

[0009] A method for optimizing the jump point search path of a mobile robot based on the gray wolf optimization algorithm and Reeds-Shepp curves includes the following steps:

[0010] S1: Based on the path obtained by the jump point search, generate the expected posture of the mobile robot when it arrives at each path node except for the origin and destination (the posture of the origin and destination is the input quantity);

[0011] S2: Merge redundant nodes based on the characteristics of the improved Reeds-Shepp curve, that is, merge nodes based on the characteristics of the improved Reeds-Shepp curve and the criteria for determining whether the current node can be merged.

[0012] S3: Optimize the predicted pose of the path nodes: Based on the predicted pose, use the Grey Wolf Optimization Algorithm to regenerate the new pose of the mobile robot when passing through each node; enable the mobile robot to pass through in the shortest time without changing the node position;

[0013] S4: Optimize the position coordinates of each node: Based on the path coordinates obtained from path planning, use the Grey Wolf optimization algorithm to generate new position coordinates in the neighborhood of each node's position coordinates, and synthesize the Reeds-Shepp path to ensure that the mobile robot can pass through in the shortest time.

[0014] Furthermore, in step S1, the expected pose of the mobile robot when it arrives at a path node is the directional pose from the parent node of each path node to that path node.

[0015] Furthermore, in step S2, due to the characteristics of the Reeds-Shepp curve, some nodes obtained by path planning can be merged by connecting parent and child nodes through the Reeds-Shepp curve without crossing obstacles. Therefore, it is necessary to determine whether to merge nodes according to the following criteria.

[0016] The criteria for judgment are as follows: there exists a Reeds-Shepp curve solution for the pose of the current node's parent and child nodes; if there are no obstacles in the triangular region formed by the current node and its parent and child nodes, the current node can be merged; if the current node and its parent and child nodes are on the same straight line, the current node cannot be merged.

[0017] Furthermore, in step S2, the traditional Reeds-Shepp curve feature is the shortest time curve based on a simple mobile robot model that adds a description of the mobile robot's direction of motion, and does not consider collision-free paths with obstacles; theoretically, there are 46 possible shortest curves.

[0018] Since traditional Reeds-Shepp curve trajectory generation does not consider the presence of obstacles in the path, this invention employs an improved path obstacle detection method based on the Reeds-Shepp curve to generate improved Reeds-Shepp curve features. This detection method combines the Bressenham circle and Bressenham line algorithms, and detects whether the grid cells along the Reeds-Shepp curve trajectory contain obstacles based on known obstacle location data; if the trajectory does not pass through an obstacle, the return value is 1; otherwise, if the trajectory passes through an obstacle, the return value is positive infinity.

[0019] Furthermore, in step S3, the predicted pose of the path nodes is optimized, specifically including: using the Grey Wolf optimization algorithm, firstly, generating and initializing a population based on the path obtained by the jump point search algorithm, with each individual in the population representing the pose of each path node; then, performing an optimization process of the Grey Wolf optimization algorithm once according to the first fitness function to obtain the optimal individual; finally, merging the path and the optimal individual yields a set of pose sequences that describe the positions traversed by the mobile robot and the pose at those positions.

[0020] Furthermore, in step S3, the expression for calculating the first fitness function is:

[0021]

[0022] Where, {θ1,θ2,...,θ n} represents the individual population, indicating the pose of each path node; n is the number of path nodes; and the coefficient s is the return value of the path obstacle detection method improved based on the Reeds-Shepp curve. i =(x i ,y i ,θ i ), (x i ,y i Let θ be the coordinate of path node i. i ∈{θ1,θ2,...,θ n}, V RS This is the value function for the Reeds-Shepp curve.

[0023] Furthermore, in step S4, the position coordinates of each node are optimized, specifically including: based on the path obtained by the jump point search algorithm, the nodes other than the start and end points are processed in the sequential order as follows:

[0024] S41: Starting from the first node of the sequence other than the starting point, generate and initialize the population based on the neighborhood of the current node, and the individual population members are the poses of the current node;

[0025] S42: Based on the second fitness function, perform the optimization process of the Gray Wolf Optimization Algorithm once to obtain the optimal individual of the current node;

[0026] S43: Replace the original pose parameters of the current node with the best individual of the current node. If the current node is not the last node of the sequence other than the endpoint, then execute steps S41 to S42 for the next node.

[0027] S44: Output the pose sequence and merge the poses of the start and end points, and connect them with Reeds-Shepp curves to obtain the shortest time curve describing the journey from the start point to the end point.

[0028] Furthermore, in step S42, the expression for calculating the second fitness function is:

[0029]

[0030] Where, q k Let q be the pose of an individual in the population at path node k. end V represents the pose of the path's endpoint. RS This is the value function for the Reeds-Shepp curve.

[0031] Furthermore, in step S3 or S4, the optimization process of the gray wolf optimization algorithm includes: (1) initially creating a random gray wolf population; (2) calculating the fitness of individual gray wolves according to the designed fitness function, and saving the top 3 individuals a, b, and c with the highest fitness; (3) updating the gray wolf positions and convergence factors. sum coefficient vector (4) Calculate the fitness of all gray wolves and update individuals a, b, and c; (5) Repeat steps (3) to (4) before reaching the maximum iteration; (6) Output the individual with the highest fitness.

[0032] The beneficial effects of this invention are as follows: This invention solves the problem of how to combine path planning and motion control. It designs a new fitness function and establishes a criterion for node merging based on improved Reeds-Shepp curve characteristics, ensuring that the mobile robot can pass through in the shortest time. This invention combines the Grey Wolf optimization algorithm with Reeds-Shepp curve characteristics, improving the solution accuracy and accelerating the convergence speed of the algorithm. Therefore, this invention has greater advantages in motion control compared to traditional methods.

[0033] Other advantages, objectives, and features of the invention will be set forth in part in the description which follows, and in part will be apparent to those skilled in the art from the following examination, or may be learned from practice of the invention. The objectives and other advantages of the invention can be realized and obtained through the following description. Attached Figure Description

[0034] To make the objectives, technical solutions, and advantages of the present invention clearer, the preferred embodiments of the present invention will be described in detail below with reference to the accompanying drawings, wherein:

[0035] Figure 1 Flowchart for node pose optimization;

[0036] Figure 2 Optimize the flowchart for node position coordinates;

[0037] Figure 3 This is a schematic diagram illustrating the effect of node pose optimization. Detailed Implementation

[0038] The following specific examples illustrate the implementation of the present invention. Those skilled in the art can easily understand other advantages and effects of the present invention from the content disclosed in this specification. The present invention can also be implemented or applied through other different specific embodiments, and various details in this specification can be modified or changed based on different viewpoints and applications without departing from the spirit of the present invention. It should be noted that the illustrations provided in the following embodiments are only schematic representations of the basic concept of the present invention. Unless otherwise specified, the following embodiments and features can be combined with each other.

[0039] Please see Figures 1-3 This invention provides a method for optimizing the jump point search path of a mobile robot based on the gray wolf optimization algorithm and the Reeds-Shepp curve, specifically including the following steps:

[0040] S1: Based on the input start and end point information, perform jump point search to obtain a list of path nodes; then, based on the path obtained from the jump point search, generate the expected posture of the mobile robot when it arrives at each path node other than the start and end point. The expected posture is the directional posture from the parent node of each path node to the path node.

[0041] S2, merge some redundant nodes according to the characteristics of the improved Reeds-Shepp curve, that is, merge nodes according to the characteristics of the improved Reeds-Shepp curve and the judgment criteria for whether the current node can be merged.

[0042] The criteria for judgment are as follows: there exists a Reeds-Shepp curve solution for the pose of the current node's parent and child nodes; if there are no obstacles in the triangular region formed by the current node and its parent and child nodes, the current node can be merged; if the current node and its parent and child nodes are on the same straight line, the current node cannot be merged.

[0043] The traditional Reeds-Shepp curve is the shortest time curve based on a simple mobile robot model that adds a description of the mobile robot's direction of motion, without considering collision-free paths with obstacles; theoretically, there are 46 possible shortest curves.

[0044] Since traditional Reeds-Shepp curve trajectory generation does not consider the presence of obstacles in the path, this invention employs an improved path obstacle detection method based on the Reeds-Shepp curve to generate improved Reeds-Shepp curve features. This detection method combines the Bressenham circle and Bressenham line algorithms, and detects whether the grid cells along the Reeds-Shepp curve trajectory contain obstacles based on known obstacle location data; if the trajectory does not pass through an obstacle, the return value is 1; otherwise, if the trajectory passes through an obstacle, the return value is positive infinity.

[0045] Specifically, the simple mobile robot model upon which the Reeds-Shepp curve is based is:

[0046] A mobile robot can be considered a rigid body during operation; therefore, its C-space is C=R. 2 ×S 1 Let q = (x, y, θ) represent the pose of the mobile robot. The robot's frame has its origin at the center of the rear axle, with the x-axis pointing towards the robot's main axis. Let s represent the robot's (symmetric) velocity. Let φ represent the steering angle. The distance between the front and rear axles is denoted by L. If the steering angle is fixed at φ, the mobile robot travels in a circular motion with a radius of ρ. Note that ρ can be determined by the intersection of the two axes (the angle between these axes is |φ|).

[0047] Using the notation above, the motion of a mobile robot can be represented by a set of equations, in the form of:

[0048]

[0049]

[0050]

[0051] Within a small time interval Δt, the mobile robot must move approximately in the direction the rear wheels are pointing. When Δt approaches zero, we have dy / dx = tanθ. Because And tanθ = sinθ / cosθ, this constraint can be expressed as:

[0052]

[0053] if and This satisfies the constraint. Furthermore, any scalar multiple of this solution is also a solution; the scaling factor directly corresponds to the robot's velocity *s*. Therefore, the first two scalar components of the equation are... and

[0054]

[0055] Let w represent the distance traveled by the mobile robot (the integral of its velocity). Let ρ represent the radius of the circle traversed by the center of the rear axle when the steering angle is fixed. Then we have dw = ρdθ, ρ = L / tanφ, and dθ can be expressed as...

[0056]

[0057] Divide both sides by dt and import. get

[0058]

[0059] At this point, s and φ are specified as action variables and determined by u. s and u φ Instead, the equations for the simple mobile robot model can be expressed as follows:

[0060]

[0061]

[0062]

[0063] Add a description of the mobile robot's direction of motion:

[0064] Since the Reeds-Shepp curve assumes that the mobile robot model moves at a constant speed, the model can be simplified to...

[0065]

[0066]

[0067]

[0068] The simplified model cannot clearly indicate whether the mobile robot is moving forward or backward. Therefore, an action variable u1 is added to represent the direction, and the model changes to...

[0069]

[0070]

[0071]

[0072] Where u1∈{-1,1} and u2∈[-tanφ] max ,tanφ max The first action variable, u1, represents either moving forward (u1 = 1) or backward (u1 = -1). For simplicity, we again assume u2 ∈ [-1, 1]. This result applies to φ. max ∈(0,π / 2).

[0073] S3: As Figure 1 As shown, the pose of the path nodes is optimized. Based on the predicted pose, an intelligent optimization algorithm (Grey Wolf Optimization Algorithm) is used to regenerate the new pose of the mobile robot when passing through each node. This allows the mobile robot to pass through in the shortest time without changing the node positions. Specifically, the Grey Wolf Optimization Algorithm is used for optimization.

[0074] S31: Generate and initialize the population based on the path obtained by the jump point search algorithm. The individual population members are the poses of each path node.

[0075] S32: Based on the first fitness function, perform the optimization process of the Gray Wolf Optimization Algorithm once to obtain the optimal individual.

[0076] The expression for calculating the first fitness function is:

[0077]

[0078] Where, {θ1,θ2,...,θ n} represents the individual population, indicating the pose of each path node; n is the number of path nodes; and the coefficient s is the return value of the path obstacle detection method improved based on the Reeds-Shepp curve. i =(x i ,y i ,θ i ), (x i ,y i Let θ be the coordinate of path node i. i ∈{θ1,θ2,...,θ n}, V RS This is the value function for the Reeds-Shepp curve.

[0079] S33: Merging the path and the optimal individual yields a set of pose sequences, describing the positions traversed by the mobile robot and its pose at each position. The results are then plotted; see [link to plotting example]. Figure 3 The thinner lines represent the original path for jump point search, while the thicker lines represent the trajectory after attitude optimization.

[0080] S4: As Figure 2As shown, the position coordinates of each node are optimized. Based on the path coordinates obtained from path planning, an intelligent optimization algorithm (Grey Wolf Optimization Algorithm) is used to generate new position coordinates in the neighborhood of each node's position coordinates and synthesize a Reeds-Shepp path to ensure that the mobile robot can pass through in the shortest time. According to the path obtained from the jump point search algorithm, the nodes other than the start and end points are processed in the following order:

[0081] S41: Starting from the first node of the sequence other than the starting point, generate and initialize the population based on the neighborhood of the current node, and the individual population members are the poses of the current node.

[0082] S42: Based on the second fitness function, perform the optimization process of the Gray Wolf Optimization Algorithm once to obtain the optimal individual of the current node.

[0083] The expression for calculating the second fitness function is:

[0084]

[0085] Where, q k Let q be the pose of an individual in the population at path node k. end V represents the pose of the path's endpoint. RS This is the value function for the Reeds-Shepp curve.

[0086] S43: Replace the original pose parameters of the current node with the best individual of the current node. If the current node is not the last node of the sequence other than the endpoint, then execute steps S41 to S42 for the next node.

[0087] S44: Output the pose sequence and merge the poses of the start and end points, and connect them with Reeds-Shepp curves to obtain the shortest time curve describing the journey from the start point to the end point.

[0088] Finally, it should be noted that the above embodiments are only used to illustrate the technical solutions of the present invention and are not intended to limit it. Although the present invention has been described in detail with reference to preferred embodiments, those skilled in the art should understand that modifications or equivalent substitutions can be made to the technical solutions of the present invention without departing from the spirit and scope of the present invention, and all such modifications or substitutions should be covered within the scope of the claims of the present invention.

Claims

1. A jump point search path optimization method based on the Grey Wolf Optimization Algorithm and Reeds-Shepp curve, characterized in that, The method specifically includes the following steps: S1: Based on the path obtained from the jump point search, the expected posture of the mobile robot when it arrives at each path node other than the origin and destination is generated. S2: Merge nodes based on the improved Reeds-Shepp curve characteristics and the criteria for determining whether the current node can be merged; The criteria for determining whether a node can be merged are as follows: the poses of the current node's parent and child nodes have a solution to the Reeds-Shepp curve; if there are no obstacles within the triangular region formed by the current node and its parent and child nodes, the current node can be merged; if the current node and its parent and child nodes are on the same straight line, the current node cannot be merged. An improved Reeds-Shepp curve-based path obstacle detection method is adopted to generate improved Reeds-Shepp curve features. This detection method combines the Bressenham circle and Bressenham line algorithms. Based on the known obstacle location data, it detects whether the grid cells along the Reeds-Shepp curve trajectory contain obstacles. If the trajectory does not pass through an obstacle, the return value is 1; otherwise, if the trajectory passes through an obstacle, the return value is positive infinity. S3: Optimize the predicted pose of the path nodes: Based on the predicted pose, use the Grey Wolf Optimization Algorithm to regenerate the new pose of the mobile robot when passing through each node; enable the mobile robot to pass through in the shortest time without changing the node position; S4: Optimize the position coordinates of each node: Based on the path coordinates obtained from path planning, use the Grey Wolf optimization algorithm to generate new position coordinates in the neighborhood of each node's position coordinates, and synthesize the Reeds-Shepp path to ensure that the mobile robot can pass through in the shortest time. The position coordinates of each node are optimized, specifically by processing the nodes other than the start and end points in sequence according to the path obtained by the jump point search algorithm: S41: Starting from the first node of the sequence other than the starting point, generate and initialize the population based on the neighborhood of the current node, and the individual population members are the poses of the current node; S42: Based on the second fitness function, perform the optimization process of the Gray Wolf Optimization Algorithm once to obtain the optimal individual of the current node; S43: Replace the original pose parameters of the current node with the best individual of the current node. If the current node is not the last node of the sequence other than the endpoint, then execute steps S41~S42 for the next node. S44: Output the pose sequence and merge the poses of the start and end points, and connect them with Reeds-Shepp curves to obtain the shortest time curve describing the journey from the start point to the end point.

2. The jump point search path optimization method according to claim 1, characterized in that, In step S1, the expected pose of the mobile robot when it arrives at a path node is the directional pose of the parent node of each path node pointing to that path node.

3. The jump point search path optimization method according to claim 1, characterized in that, In step S3, the predicted poses of the path nodes are optimized, specifically including: using the Grey Wolf optimization algorithm, firstly, generating and initializing a population based on the path obtained by the jump point search algorithm, with each individual in the population representing the pose of each path node; then, performing the Grey Wolf optimization algorithm once according to the first fitness function to obtain the optimal individual; finally, merging the path and the optimal individual to obtain a set of pose sequences, describing the positions traversed by the mobile robot and the pose at those positions.

4. The jump point search path optimization method according to claim 3, characterized in that, In step S3, the expression for calculating the first fitness function is: in, For individuals in the population, each represents the pose of a path node. The coefficient represents the number of path nodes. This is the return value of the improved path obstacle detection method based on Reeds-Shepp curves; , Path node coordinates , This is the value function for the Reeds-Shepp curve.

5. The jump point search path optimization method according to claim 1, characterized in that, In step S42, the expression for calculating the second fitness function is: in, For individuals in the population at path nodes Position, Indicates the pose of the path's endpoint. This is the value function for the Reeds-Shepp curve.

6. The jump point search path optimization method according to claim 1 or 3, characterized in that, The optimization process of the gray wolf optimization algorithm includes: (1) initially creating a random gray wolf population; (2) calculating the fitness of individual gray wolves according to the designed fitness function, and saving the top 3 individuals a, b, and c with the highest fitness; (3) updating the gray wolf positions and convergence factors. , and coefficient vector , (4) Calculate the fitness of all gray wolves and update individuals a, b, and c; (5) Repeat steps (3) to (4) before reaching the maximum iteration; (6) Output the individual with the highest fitness.

Images