Path planning method, device and computer storage medium for mobile robot

Abstract

The present invention discloses a path planning method, device and computer storage medium for a mobile robot. The path planning method for a mobile robot of the present invention adopts a two-way sampling strategy to generate four search trees, and uses the four search trees to search simultaneously, thereby shortening the path planning time and improving the path planning efficiency; when the search tree expands the node, the repulsive field generated by the obstacle is used to constrain the generation of the sampling point, so that the generation of the sampling point has directionality, so that the sampling point is far away from the obstacle, and the obstacle is expanded at the same time, so as to avoid the generated path from being close to the obstacle; in addition, the gradient descent optimization strategy is used to smoothly optimize the generated path, so that the path is more in line with the dynamic model of the mobile robot, thereby making the robot move more smoothly.

Description

Technical Field

[0001] The present invention relates to the field of robot technology, and in particular to a path planning method, device and computer storage medium for a mobile robot. Background Art

[0002] Since mobile robots have the characteristics of autonomous operation and flexible mobility, they have broad prospects for development and utilization in important fields such as national defense science and technology, life services, production and construction. As mobile robots continue to be popularized and applied, the path planning algorithm of mobile robots has become one of the key technologies of mobile robots.

[0003] As the application of mobile robots becomes more and more extensive, traditional single path planning algorithms such as A* and RRT* have technical problems such as poor results in finding the optimal path, slow path search speed, and uneven paths, and are difficult to apply to complex and changing environments. In addition, although the path planning algorithm based on intelligent algorithms has improved the optimization effect, it is less efficient and is very easy to fall into local optimality. Summary of the invention

[0004] The technical problem to be solved by the present invention is to provide a path planning method, device and computer storage medium for a mobile robot, so as to solve the technical problems of slow path search speed and uneven path in traditional path planning methods and improve the search efficiency.

[0005] In order to solve the above technical problems, the present invention adopts the following technical solutions:

[0006] A path planning method for a mobile robot comprises the following steps: obtaining an initial position, a target position, an intermediate position between the initial position and the target position, and obstacle information of the mobile robot; constructing two search trees T with the initial position and the intermediate position as root nodes respectively. 1 , T 3 Search in opposite directions, and construct two search trees T with the target position and the middle position as root nodes respectively. 2 , T 4 Search in opposite directions; expand new nodes for the four search trees based on the preset node expansion strategy; repeat the previous step until the search tree T 1 With the search tree T 3 Connected and search tree T 2 With the search tree T 4 The paths from the initial position to the target position are connected to each other, and the path obtained by the previous step planning is smoothed by the gradient descent optimization strategy to obtain the best motion path suitable for the mobile robot.

[0007] A path planning device for a mobile robot comprises a data acquisition module, a search tree construction module, a search tree expansion module, a path generation module and a path smoothing module. The data acquisition module is used to acquire the initial position, target position, intermediate position between the initial position and the target position and obstacle information of the mobile robot; the search tree construction module is used to construct two search trees T with the initial position and the intermediate position as root nodes respectively. 1 , T 3 , and used to construct two search trees T with the target position and the middle position as root nodes respectively 2 , T 4 , where the search tree T 1 , T 3 Search in opposite directions, search tree T 2 , T 4 Search in opposite directions; the search tree expansion module is used to expand new nodes for the four search trees based on the preset node expansion strategy; the path generation module is used when the search tree T 1 With the search tree T 3 Connected and search tree T 2 With the search tree T 4 When connected, a path from the initial position to the target position is generated; the path smoothing module is used to smooth the path generated by the path generation module through a gradient descent optimization strategy to obtain the optimal motion path suitable for the mobile robot.

[0008] A computer storage medium stores program instructions therein, and when the program instructions are executed by a processor, the path planning method for a mobile robot is implemented.

[0009] The beneficial technical effect of the present invention is that the above-mentioned path planning method for a mobile robot adopts a two-way sampling strategy to generate four search trees, and uses the four search trees to search simultaneously, which shortens the path planning time and improves the path planning efficiency; in addition, a gradient descent optimization strategy is used to smoothly optimize the generated path, so that the path is more in line with the power model of the mobile robot, thereby making the robot movement smoother. BRIEF DESCRIPTION OF THE DRAWINGS

[0010] Figure 1 A schematic diagram of a flow chart of a path planning method for a mobile robot according to the present invention;

[0011] Figure 2 A schematic flow chart of the steps of expanding new nodes for four search trees based on a preset node expansion strategy of the present invention;

[0012] Figure 3 It is a structural schematic diagram of a path planning device for a mobile robot of the present invention;

[0013] Figure 4 It is a structural schematic diagram of the search tree expansion module of the present invention. DETAILED DESCRIPTION

[0014] In order to enable those skilled in the art to more clearly understand the objectives, technical solutions and advantages of the present invention, the present invention is further described below in conjunction with the accompanying drawings and embodiments.

[0015] like Figure 1 As shown, in one embodiment of the present invention, a path planning method for a mobile robot includes the following steps:

[0016] S110, obtaining the initial position, target position, intermediate position between the initial position and the target position, and obstacle information of the mobile robot.

[0017] S120, construct two search trees T with the initial position and the middle position as root nodes respectively 1 、T 3 Search in opposite directions, and construct two search trees T with the target position and the middle position as root nodes respectively. 2 、T 4 Search in opposite directions.

[0018] S130, expanding new nodes for the four search trees based on a preset node expansion strategy.

[0019] S140, determine whether to search tree T 1 With the search tree T 3 Connected and search tree T 2 With the search tree T 4 If yes, go to step S150, if no, go back to step S130.

[0020] S150: Generate a path from the initial position to the target position.

[0021] S160, smoothing the path planned in step S150 through a gradient descent optimization strategy to obtain an optimal motion path suitable for the mobile robot.

[0022] The step S110 includes: collecting working environment information of the mobile robot through the laser radar sensor of the mobile robot; establishing a grid map; determining the initial position X of the mobile robot on the grid map based on the working environment information of the mobile robot start 、Target position X goal , middle position X mid and obstacle information.

[0023] Among them, the middle position X mid At the initial position X start and the target position X goalBetween, and the middle position X mid To the initial position X start The distance and the middle position X mid To the initial position X start The distances are equal.

[0024] If the initial position X start and the target position X goal If there is no obstacle at the midpoint of the line connecting start and the target position X goal The midpoint of the line connecting the mid ; If the initial position X start and the target position X goal If there is an obstacle at the midpoint of the line connecting start and the target position X goal The midpoint of the line connecting the initial position X start and the target position X goal The middle position X is obtained by translating the connecting line in the vertical direction by a certain distance. mid . In the initial translation position X start and the target position X goal When the midpoint of the line is drawn along the initial position X start and the target position X goal The line connecting the two points is perpendicular to the initial position X. start and the target position X goal Move to either side of the line until you leave the obstacle.

[0025] In some preferred embodiments, the initial position X start and the target position X goal The midpoint of the line is towards the initial position X start and the target position X goal The first side of the line connecting the two sides is translated to leave the obstacle, which requires a unit translation distance, and the initial position X start and the target position X goal The midpoint of the line is towards the initial position X start and the target position X goal The second side of the line connecting the two sides of ... start and the target position X goal The midpoint of the line connecting the initial position X start and the target position X goal The line connecting the two points is perpendicular to the initial position X. start and the target position X goal The second side of the connecting line is translated by b units and becomes the middle position X mid ; If a<b, then the initial position Xstart and the target position X goal The midpoint of the line connecting the initial position X start and the target position X goal The line connecting the two points is perpendicular to the initial position X. start and the target position X goal The first side of the connecting line is translated a unit distance as the middle position X mid .

[0026] The step S120 includes: initializing four search trees T 1 、T 2 、T 3 、T 4 , and assume that the initial path information list L is empty, where the initial position X start 、Target position X goal As two search trees T 1 、T 2 The root node, the middle position X mid At the same time, as two search trees T 3 、T 4 The root node of .

[0027] When the search tree T 1 From the initial position X start To the middle position X mid When searching, the search tree T 2 Also from the target position X goal To the middle position X mid Search; at the same time, search tree T 3 From the middle position X mid To the initial position X start Search, search tree T 4 From the middle position X mid Towards the target position X goal Search, four search trees are searched simultaneously, which shortens the path planning time and improves the path planning efficiency.

[0028] like Figure 2 As shown, the step S130 further includes the following steps:

[0029] S131, performing expansion processing on the obstacle, and recording the obstacle area and the obstacle-free area on the map based on the expanded obstacle;

[0030] S132: Apply a repulsive force field on the obstacle to constrain the random sampling point, and generate a random sampling point X in the obstacle-free area. rand ;

[0031] S133, searching for the nearest node X that is closest to the random sampling point within the area of ​​the search tree nearest;

[0032] S134, determine the random sampling point X rand and the nearest node X nearest Whether the line connecting the two contacts the obstacle area, if not, executing step S135, if yes, returning to step S132;

[0033] S135, at random sampling point X rand and the nearest node X nearest Select a node on the line as the new node X new Insert into the search tree;

[0034] S136: Reconnect the new node X based on the preset reconnection strategy new and new node X new All nodes X in the neighborhood i Reconnect.

[0035] In step S131, the obstacle is expanded by setting the expansion coefficient K, and the position of the expanded obstacle on the grid map is marked as the obstacle area, and the position without obstacles on the grid map is marked as the obstacle-free area. The expansion coefficient K is the Euclidean distance between the edge of the expanded obstacle and the edge of the original obstacle, and the K value can be adjusted according to the size of the mobile robot. This step expands the obstacle, and the search tree expands new nodes in the environment after the obstacle is enlarged, avoiding the generated path from being close to the obstacle.

[0036] In step S132, a repulsive field is applied to the obstacle to constrain the random sampling points, thereby generating random sampling points X of each of the four search trees in the obstacle-free area. rand The obstacle is a random sampling point X rand The repulsion function can be expressed as:

[0037]

[0038] Where m represents the repulsion gain coefficient of the obstacle, x i represents the coordinates of the random sampling points, x o represents the coordinates of the obstacle, ||x i -x o || represents the Euclidean distance from the random sampling point to the obstacle, ρ 0 is the distance threshold, which indicates the maximum distance of influence of the repulsive force generated by the obstacle. This step uses the repulsive field generated by the obstacle to constrain the generation of sampling points, making the generation of sampling points directional, so that the sampling points are far away from the obstacles, and generating random sampling points X in the obstacle-free area. rand .

[0039] Since in step S130, the four search trees simultaneously expand their respective new nodes. Therefore, in step S133, in the search tree T 1 、T 2 、T 3 、T 4 Find the node X that is closest to the corresponding random sampling point within the area nearest In other words, when each search tree expands a new node, it generates a corresponding random sampling point X in the barrier-free area. rand , each search tree has a region with the same random sampling point X rand The closest node X nearest .

[0040] In step S136, the following method is used to insert a new node X into the search tree: new and new node X new All nodes X in the neighborhood i Reconnect.

[0041] Traverse the new node X new All nodes X in the neighborhood i , let the node X with the smallest sum of cost values i The node X with the minimum cost value min , the sum of the cost values ​​is expressed as C||X new -X i ||+C||X i -X base ||, where ||X new -X i || represents node X i To the new node X new The Euclidean distance of ||X i -X base || represents node X i Euclidean distance to the root of the search tree. X base Represents the coordinates of the root node of the search tree. For example, in the search tree T 1 In, X base =X start ; In the search tree T 2 In, X base =X goal ; In the search tree T 3 、T 4 In, X base =X mid .

[0042] Determine the minimum cost value node X min With the new node X new Whether the connection line collides with the obstacle area, if not, the new node X newDisconnect from its original parent node and create node X with the minimum cost value min As new node X new Reconnect the parent node, if so, keep the new node X new The original connection relationship.

[0043] For the new node X new The node X with the smallest cost value is included in the neighborhood. min All nodes outside X i , if C||X new -X base ||+C||X i -X new ||<C||X i -X base ||, and node X i With the new node X new If the line connecting the nodes does not collide with the obstacle area, then the node X i Disconnect from its original parent node and make the new node X new Become Node X i The new parent node is reconnected, and node X i The connections of the child nodes of are recursively changed, where ||X new -X base || represents a new node X new Euclidean distance to the root of the search tree, ||X i -X new || represents node X i To the new node X new Euclidean distance; if C||X new -X base ||+C||X i -X new ||≥C||X i -X base ||, or node X i With the new node X new The line of the node collides with the obstacle area, keeping the node X i The original connection relationship.

[0044] In step S140, when the search tree T 1 Expand to search tree T 3 When a node of 3 Expand to search tree T 1 A node of , then the search tree T 1 , and the search tree T 3 connected; when the search tree T 2 Expand to search tree T 4 When a node of 4Expand to search tree T 2 When a node of 2 With the search tree T 4 connected; when the search tree T 1 , and the search tree T 3 Connected and search tree T 2 With the search tree T 4 If connected, execute step S150, if not, return to step S130.

[0045] The step S160 further includes the steps of:

[0046] The path planned in step S40 is regarded as a curve, represented by function S'(n) and the derivative of function S(n) is obtained.

[0047] S(n)=ω 0 x 0 +ω 1 x 1 +ω 2 x 2 +...+ω n x n (1)

[0048]

[0049] Among them, x n To process the eigenvalues ​​in the data, ω n is the weight coefficient, h ω (x) is the expression after the derivative of function S(n);

[0050] The mean square error is used to represent the error between the function S(n) and the derivative of the function S(n).

[0051]

[0052] Among them, G(ω) is the mean square loss difference, which includes all sample data, and m represents the number of samples;

[0053] Establish the differential iteration formula,

[0054]

[0055] Among them, ω represents the weight value, l represents the width of the value range of ω, and β represents the differential step size;

[0056] The differential iterative formula (4) is solved, and the loss is minimized each time through iteration, so that the objective function |S'(n)-S(n)| obtains the minimum value, thereby achieving the effect of smooth optimization of the path.

[0057] The path planning method for a mobile robot of the present invention utilizes four search trees to search simultaneously, thereby shortening the path planning time and improving the path planning efficiency; when the search tree expands the node, the repulsive field generated by the obstacle is utilized to constrain the generation of sampling points, so that the generation of the sampling points has directionality, so that the sampling points are far away from the obstacle, and the obstacle is expanded at the same time, so as to avoid the generated path being close to the obstacle; and the gradient descent optimization strategy is adopted to perform smooth optimization processing on the generated path, so as to obtain the best motion path suitable for the mobile robot.

[0058] like Figure 3 As shown, based on Figure 1 , Figure 2 The path planning method for a mobile robot is shown. The present invention provides a path planning device for a mobile robot, which includes a data acquisition module 110, a search tree construction module 120, a search tree expansion module 130, a path generation module 140 and a path smoothing module 150.

[0059] The data acquisition module 110 is used to acquire the initial position, target position, intermediate position between the initial position and the target position, and obstacle information of the mobile robot, that is, to perform Figure 1 Step S110 in the path planning method for a mobile robot in the illustrated embodiment.

[0060] The search tree construction module 120 is used to construct two search trees T with the initial position and the middle position as root nodes respectively. 1 , T 3 , and used to construct two search trees T with the target position and the middle position as root nodes respectively 2 , T 4 , where the search tree T 1 , T 3 Search in opposite directions, search tree T 2 , T 4 Reverse search, that is, used to perform Figure 1 Step S120 in the path planning method for a mobile robot in the illustrated embodiment.

[0061] The search tree expansion module 130 is used to expand new nodes for the four search trees based on a preset node expansion strategy, that is, to execute Figure 1 Step S130 in the path planning method for a mobile robot in the illustrated embodiment.

[0062] The path generation module 140 is used to search the tree T 1 With the search tree T 3 Connected and search tree T 2 With the search tree T 4When connected, a path from the initial position to the target position is generated, which is used to execute Figure 1 Steps S140 and S150 in the path planning method for a mobile robot in the illustrated embodiment.

[0063] The path smoothing module 150 is used to smooth the path generated by the path generation module through a gradient descent optimization strategy to obtain an optimal motion path suitable for the mobile robot, that is, to perform Figure 1 Step S160 in the path planning method for a mobile robot in the illustrated embodiment.

[0064] like Figure 4 As shown, the search tree expansion module 130 includes an obstacle expansion unit 131 , a sampling point generation unit 132 , a nearest node search unit 133 , a new node insertion unit 134 and a node reconnection unit 135 .

[0065] The obstacle expansion unit 131 is used to expand the obstacle and mark the obstacle area and the obstacle-free area on the map based on the expanded obstacle, that is, to perform Figure 2 Step S131 in the path planning method for a mobile robot in the illustrated embodiment.

[0066] The sampling point generating unit 132 is used to impose a repulsive force field on the obstacle to constrain the random sampling point and generate a random sampling point X in the obstacle-free area. rand , which is used to execute Figure 2 Step S132 in the path planning method for a mobile robot in the illustrated embodiment.

[0067] The nearest node search unit 133 is used to find the nearest node X that is closest to the random sampling point within the area of ​​the search tree. nearest , which is used to execute Figure 2 Step S133 in the path planning method for a mobile robot in the illustrated embodiment.

[0068] The new node insertion unit 134 is used to randomly sample the point X rand and the nearest node X nearest When the line connecting the two points has no contact with the obstacle area, at the random sampling point X rand and the nearest node X nearest Select a node on the line as the new node X new Insert into the search tree; that is, to perform Figure 2 Steps S134 and S135 in the path planning method for a mobile robot in the illustrated embodiment.

[0069] The node reconnection unit 135 is used to connect the new node X based on the preset reconnection strategy. new and new node Xnew All nodes X in the neighborhood i Reconnect, that is, to execute Figure 2 Step S136 in the path planning method for a mobile robot in the illustrated embodiment.

[0070] The present invention also provides a computer storage medium, wherein the computer storage medium stores program instructions, and when the program instructions are executed by a processor, Figure 1 or Figure 2 A path planning method for a mobile robot in the illustrated embodiment.

[0071] The above description is only a preferred embodiment of the present invention, and does not limit the present invention in any form. Those skilled in the art can make various equivalent changes and improvements based on the above embodiments, and all equivalent changes or modifications made within the scope of the claims should fall within the protection scope of the present invention.

Claims

1. A path planning method for a mobile robot, characterized in that: The path planning method for a mobile robot comprises the following steps: S10, obtaining an initial position, a target position, an intermediate position between the initial position and the target position, and obstacle information of the mobile robot; S20, construct two search trees T1 and T3 with the initial position and the middle position as root nodes respectively, and construct two search trees T2 and T4 with the target position and the middle position as root nodes respectively, to search in the opposite direction; S30, expanding new nodes for the four search trees based on a preset node expansion strategy; S40, repeating step S30 until the search tree T1 is connected to the search tree T3 and the search tree T2 is connected to the search tree T4, generating a path from the initial position to the target position; S50, smoothing the path planned in step S40 by using a gradient descent optimization strategy to obtain an optimal motion path suitable for the mobile robot.

2. The path planning method for a mobile robot according to claim 1, characterized in that: If there is no obstacle at the midpoint of the line connecting the initial position and the target position, the midpoint of the line connecting the initial position and the target position is taken as the intermediate position; if there is an obstacle at the midpoint of the line connecting the initial position and the target position, the midpoint of the line connecting the initial position and the target position is translated by several unit distances in a direction perpendicular to the line connecting the initial position and the target position and taken as the intermediate position.

3. The path planning method for a mobile robot according to claim 1, characterized in that: The step S30 further includes the steps of: S31, performing expansion processing on the obstacle, and marking the obstacle area and the obstacle-free area on the map based on the expanded obstacle; S32, apply a repulsive field on the obstacle to constrain the random sampling point, and generate a random sampling point X in the obstacle-free area rand ; S33, find the nearest node X that is closest to the random sampling point within the area of ​​the search tree nearest ; S34, determine the random sampling point X rand and the nearest node X nearest Does the line connecting the two contact the obstacle area? If not, then at the random sampling point X rand and the nearest node X nearest Select a node on the line as the new node X new Insert the search tree, if yes, return to step S32; S35, based on the preset reconnection strategy to the new node X new and new node X new All nodes X in the neighborhood i Reconnect.

4. The path planning method for a mobile robot according to claim 3, characterized in that: The step S35 further includes the steps of: Traverse the new node X new All nodes X in the neighborhood i , let the node X with the smallest sum of cost values i The node X with the minimum cost value min , the sum of the cost values ​​is expressed as C||X new -X i ||+C||X i -X base ||, where X base Represents the coordinates of the search tree root node, ||X new -X i || represents node X i To the new node X new The Euclidean distance of ||X i -X base || represents node X i Euclidean distance to the root of the search tree; Determine the minimum cost value node X min With the new node X new Whether the connection line collides with the obstacle area, if not, the new node X new Disconnect from its original parent node and create node X with the minimum cost value min As new node X new Reconnect the parent node, if so, keep the new node X new The original connection relationship; For the new node X new The node X with the smallest cost value is included in the neighborhood. min All nodes outside X i , if C||X new -X base ||+C||X i -X new ||<C||X i -X base ||, and node X i With the new node X new If the line connecting the nodes does not collide with the obstacle area, then the node X i Disconnect from its original parent node and make the new node X new Become Node X i The new parent node is reconnected, and node X i The connections of the child nodes of are recursively changed, where ||X new -X base || represents a new node X new Euclidean distance to the root of the search tree, ||X i -X new || represents node X i To the new node X new Euclidean distance; if C||X new -X base ||+C||X i -X new ||≥C||X i -X base ||, or node X i With the new node X new The line of the node collides with the obstacle area, keeping the node X i The original connection relationship.

5. The path planning method for a mobile robot according to claim 3, characterized in that: In step S32, the obstacle is rand The repulsion function can be expressed as: Where m represents the repulsion gain coefficient of the obstacle, x i Represents the coordinates of the random sampling points, x o represents the coordinates of the obstacle, ||x i -x o || represents the Euclidean distance from the random sampling point to the obstacle, ρ0 is the distance threshold, and represents the maximum influence distance of the repulsive force produced by the obstacle.

6. The path planning method for a mobile robot according to claim 3, characterized in that: In step S31, the obstacle is expanded by setting an expansion coefficient K, where the expansion coefficient K is the Euclidean distance between the expanded obstacle edge and the original obstacle edge.

7. The path planning method for a mobile robot according to claim 1, characterized in that: In step S40, when search tree T1 is expanded to a node of search tree T3, or search tree T3 is expanded to a node of search tree T1, search tree T1 is connected to search tree T3; when search tree T2 is expanded to a node of search tree T4, or search tree T4 is expanded to a node of search tree T2, search tree T2 is connected to search tree T4.

8. The path planning method for a mobile robot according to claim 1, characterized in that: The step S50 further includes the steps of: The path planned in step S40 is regarded as a curve, represented by function S'(n) and the derivative of function S(n) is obtained. S(n)=ω0x0+ω1x1+ω2x2+...+ω n x n (1) Among them, x n To process the eigenvalues ​​in the data, ω n is the weight coefficient, h ω (x) is the expression after the derivative of function S(n); The mean square error is used to represent the error between the function S(n) and the derivative of the function S(n). Among them, G(ω) is the mean square loss difference, which includes all sample data, and m represents the number of samples; Establish the differential iteration formula, Among them, ω represents the weight value, l represents the width of the value range of ω, and β represents the differential step size; The differential iterative formula (4) is solved, and the loss is minimized each time through iteration, so that the objective function |S'(n)-S(n)| reaches the minimum value.

9. A path planning device for a mobile robot, characterized in that: The path planning device for the mobile robot comprises: A data acquisition module is used to acquire the initial position, target position, intermediate position between the initial position and the target position, and obstacle information of the mobile robot; A search tree construction module, used to construct two search trees T1 and T3 with the initial position and the middle position as root nodes respectively, and used to construct two search trees T2 and T4 with the target position and the middle position as root nodes respectively, wherein the search trees T1 and T3 search in opposite directions, and the search trees T2 and T4 search in opposite directions; Search tree expansion module, used to expand new nodes for the four search trees based on the preset node expansion strategy; A path generation module, for generating a path from an initial position to a target position when the search tree T1 is connected to the search tree T3 and the search tree T2 is connected to the search tree T4; The path smoothing module is used to smooth the path generated by the path generation module through the gradient descent optimization strategy to obtain the best motion path suitable for the mobile robot.

10. A computer storage medium, characterized in that: The computer storage medium stores program instructions, and when the program instructions are executed by the processor, the path planning method for a mobile robot according to any one of claims 1 to 8 is implemented.

Images