CN110319837A - A path planning method for indoor complex conditions of a service robot - Google Patents

Abstract

The invention provides a method for planning complex situation paths in a service robot room, which comprises the following steps: determining coordinate values of a starting point and a target point based on a grid map represented by a two-dimensional array, creating and initializing an OPEN list and a CLOSE list, and placing the starting point in the OPEN list; detecting an OPEN list, judging whether the path is searched successfully or not, and if not, carrying out the next step; alternative paths are searched by adopting a jumping point expansion rule, so that the expansion of invalid alternative points is reduced, and the path searching speed is accelerated; detecting connectivity among the path points, and removing useless path points; and adding information of steering judgment and required rotation angle for each path point based on the vector cross product and the vector dot product. The invention simultaneously detects the connectivity among the interval path points to remove redundant points in the path points, and reduces the stay of the robot at unnecessary path points to shorten the total path length. And the vector dot product and the vector cross product are used for carrying out steering judgment and corner calculation at the path point, so that the pose of the robot can be automatically adjusted at the path point.

Description

A path planning method for indoor complex conditions of a service robot

technical field

The invention belongs to the technical field of path planning for service robots, and in particular relates to a path planning method for indoor complex conditions of a service robot.

Background technique

In recent years, with the rapid development of robotics technology and further changes in social needs, in the field of robotics, research on related technologies has gradually extended from large-scale industrial robots to indoor service robots. The premise of the robot's autonomous navigation and movement in the indoor environment is to complete the path planning according to the target points and algorithms, and then perform motion walking according to the planned path points. Efficient and reliable path planning algorithm is the key technology for autonomous movement of indoor robots, and it is also an important basis for robots to complete service tasks. At present, in the global path planning algorithm, most algorithms use the A* algorithm or an improved algorithm based on A*. In a large-scale scene map, the algorithm is limited by a fixed neighbor expansion strategy, which makes the pathfinding algorithm consume a large amount of memory space. The calculation time is long and the calculation performance is high. As a result, they cannot meet the requirements of practicality and real-time performance when applied to indoor path planning of service robots. For example, the Swamps algorithm proposed by Pochter uses a preprocessing method to decompose the grid map into a series of adjacent areas, and then identifies the areas that do not need to be explored in the map to reduce the expansion of useless alternative path points and speed up the search. Although the successor path point expansion strategy has increased the road strength planning speed, the performance of the algorithm operation speed is limited. The TDHs algorithm proposed by Sturtevant improves the speed of the algorithm by improving the accuracy of the heuristic function of the guided search. The algorithm usually pre-calculates and saves the distance between some key nodes in the area. While they are fast and yield optimal solutions, they often consume too much memory.

In summary, the existing global path planning algorithms have the following disadvantages:

1. Limited by the inherent neighbor expansion strategy, a large number of invalid alternative path points have been expanded, which reduces the performance of the algorithm;

2. In a large-scale map environment, it is necessary to store a large amount of information on alternative waypoints when screening subsequent waypoints, which requires a certain storage performance;

3. There are many redundant path points in the planned path;

4. In the real application scenario of the robot, the path planning algorithm only gives the key path points, so that the robot cannot autonomously adjust the pose at the path points.

Contents of the invention

The purpose of the present invention is to provide a path planning method for indoor complex conditions of a service robot. At the same time, it detects the connectivity between path points at intervals, removes redundant points in path points, and reduces the robot's stay at unnecessary path points to shorten the total time. the path length. And use the vector dot product and vector cross product to judge the steering and calculate the corner at the way point, so that the robot can adjust the pose autonomously at the way point.

The present invention provides a path planning method for indoor complex conditions of a service robot, comprising the following steps:

Step 1. Determine the coordinate values of the starting point and the target point based on the grid map represented by a two-dimensional array, create and initialize the OPEN list and the CLOSE list, and place the starting point in the OPEN list; among them, the OPEN list is used to store the expanded For the unexamined waypoints, the CLOSE list is used to record the waypoints that have been visited;

Step 2, detect the OPEN list, and judge whether the path search is successful, if not, go to step 3;

Step 3, use jump point expansion rules to find alternative paths to reduce the expansion of invalid alternative points and speed up path search;

Step 4, detecting the connectivity between waypoints and removing useless waypoints;

Step 5, based on the vector cross product and the vector dot product, add steering judgment and required rotation angle information to each way point.

Further, said step 2 includes:

Check whether the OPEN list is empty, if the OPEN list is empty, it means that the path search failed and no feasible path was found; if the OPEN list is not empty, select the path point with the smallest evaluation function value from the list, and then select the path point from the OPEN Take it out of the list and put it in the CLOSE list;

Check whether the path point corresponds to the coordinate value of the target point, if yes, it means that the path search is successful, exit, if the coordinate value does not correspond, go to step 3.

Further, the evaluation function is expressed as:

f(n)=g(n)+h(n);

Among them, f(n) refers to the cost estimate from the initial path point through the intermediate path point n to the target point, g(n) is the actual cost value from the initial point to the intermediate path point n in the state space, h(n ) is the estimated cost of the best path from the intermediate waypoint to the goal point.

Further, said step 3 includes:

Set the expansion rule of the jump point; the expansion rule is: if y has the minimum value k, so that And one of the following conditions is met, then the path point y is a jump point from the path point x in the d direction: condition ①, the path point y is the target point; condition ②, when expanding the jump point along the straight line, the path point y contains Forced neighbors; condition ③, when expanding the jump point along the diagonal direction, there is a jump point z obtained by expanding along the straight line or diagonal direction through the y path point, that is, the jump point z passes through the y path point and satisfies the condition ② ;

In the process of expansion, according to the expansion rules of jump points, all the alternative path points corresponding to the eight directions of the path point are expanded. Assuming that the equipment selection point is represented by m, the evaluation function value of each alternative point m is calculated, and the selection The path point with the smallest evaluation function value among the candidate points is the next sub-target point, and then repeat steps 2 and 3 until the found path point is the target point, and the initial path P _n is obtained, otherwise the algorithm search fails.

Further, said step 4 includes:

Starting from the starting point, smooth the obtained initial path point set P _n , and judge the direct connection between the previous path point P _i-1 and the rear path point P _i+1 of the current path point P _i one by one Whether there is an obstacle, if there is no obstacle, delete the redundant waypoint P _i , if there is an obstacle, continue to judge the next waypoint until all the waypoints are traversed.

Further, the method for judging whether there is an obstacle in the case of direct connection between two path points described in step 4 includes:

Input the coordinate values of two path points and the slope value k of the line between the two points, and process in three cases:

One is the case where the slope of the line between two points does not exist, that is, the line is perpendicular to the X axis, and the relative positions of the two path points are on the same vertical line;

The second case is when the slope of the straight line is zero, that is, the straight line is perpendicular to the Y axis, and the relative positions of the two path points are on the same horizontal line;

The third case is when the slope exists and is not zero, and the relative position of the two path points is in the direction of the diagonal line, first judge whether the slope is greater than 1, determine the starting direction of the check, and then use the function f or inverse of the straight line The function f ^-1 calculates another corresponding coordinate value, and then checks each point step by step to complete the judgment operation of whether there is an obstacle.

Further, the method for steering judgment described in step 5 includes:

Suppose a=(a _x , a _y , a _z ), b=(b _x , b _y , b _z ), then the determinant expression of the vector product is as follows:

The coordinate expression is:

a×b＝(a _y b _z -a _z b _z )i+(a _z b _x -a _x b _z )j+(a _x b _y -a _y b _x )k;

Judgment rules:

When a×b>0, it is counterclockwise, that is, the vector b is on the left side of the vector a;

When a×b<0, it is clockwise, that is, the vector b is on the right side of the vector a;

When a×b=0, the direction does not change, that is, the vector b is in the same direction as the vector a.

Further, the calculation method of the rotation angle described in step 5 includes:

Based on the vector angle calculation, the rotation angle is obtained. The calculation formula of the vector angle is:

where a and b are two vectors of the vector product.

Compared with prior art, the beneficial effects of the present invention are:

1) In the present invention, the alternative path expansion method adopts the method of jumping point search strategy to replace the traditional neighbor expansion mode, which effectively reduces the expansion of invalid alternative path points, reduces the algorithm's requirements for storage performance, and makes the algorithm more sensitive to OPEN The number of operations on the list and the CLOSE list is greatly reduced, and finally achieves the purpose of improving the pathfinding speed of the algorithm.

2) The present invention optimizes the initial path obtained by the algorithm, and removes redundant path points by using the method of detecting connectivity between path points at intervals, effectively shortening the total path length, and smoothing the path to a certain extent , which also greatly reduces the cumulative turning angle of the robot, reducing the time for the robot to rotate at the waypoint.

3) The present invention uses the vector cross product and vector dot product to add information such as steering judgment and rotation angle to the path points in the obtained path for the algorithm, which is conducive to the robot's autonomous adjustment of pose and action at the path point, and satisfies the real-time control of robot motion. sex.

Description of drawings

FIG. 1 is a flow chart of the present invention for detecting connectivity between waypoints.

Detailed ways

The present invention will be described in detail below in conjunction with the implementations shown in the drawings, but it should be noted that these implementations are not limitations of the present invention, and those of ordinary skill in the art based on the functions, methods, or structural changes made by these implementations Equivalent transformations or substitutions all fall within the protection scope of the present invention.

This embodiment provides a path planning method for indoor complex conditions of a service robot, which is mainly divided into the following steps:

1. Algorithm initialization;

2. Detect the OPEN list;

3. Use the jump point search strategy to expand the alternative path points;

4. Detect connectivity between waypoints;

5. Add corner judgment and rotation angle information to the path point.

The algorithm initialization process in step 1 is as follows:

Since the map applicable to this algorithm is a grid map, the initialization of the algorithm first needs to set a grid map that can be represented by a two-dimensional array, and "1" and "0" in the map represent "impassable" in the map, respectively. " and "passable", then determine the two inputs of the algorithm, that is, the coordinate values of the starting point S and the target point E, and finally create and initialize the OPEN list and the CLOSE list (the two lists can be completed using queue design), where The OPEN list is used to store the expanded but unexplored waypoints, and the CLOSE list is used to record the visited waypoints. Finally, place the starting point S in the OPEN list.

The process of detecting the OPEN list in step 2 is as follows:

The main purpose of detecting the OPEN list is to detect whether the OPEN list is empty. If the OPEN list is empty, it means that the path search failed and no feasible path was found; if the OPEN list is not empty, select the evaluation function f(n) with the largest value from the list. Small path point n, then take path point n out of the OPEN list, and then place it in the CLOSE list. Then check whether the path point n corresponds to the coordinate value of the target point E. If so, it means that the path search is successful, and exit the algorithm. If the coordinate value does not correspond, proceed to the next step. Where the valuation function f(n) is expressed as:

f(n)=g(n)+h(n);

Here f(n) refers to the cost estimate from the initial path point through the intermediate path point n to the target point, g(n) is the actual cost value from the initial point to the intermediate path point n in the state space, h(n ) is the estimated cost of the best path from the intermediate waypoint to the goal point. The selection of h(n) evaluation function value generally includes Manhattan distance, Euclidean distance and Chebyshev distance, etc., and the present invention mainly uses Euclidean distance D.

In Step 3, the process of expanding candidate points by adopting jump-point search strategy is as follows:

The jump point search strategy is mainly to use jump point expansion rules to find alternative paths, the purpose of which is to reduce the expansion of invalid alternative points and speed up the path search. The expansion rule of the jump point is as follows: if y has the smallest value k, so that And if one of the following conditions is met, the path point y is a jump point from the path point x in the d direction: ① path point y is the target point; ② when the jump point is expanded along the straight line, the y path point contains forced neighbors; ③When the jump point is expanded along the diagonal direction, there will be a jump point z expanded along the straight line or diagonal direction through the y path point, that is, the jump point z passes through the y path point and satisfies the condition ②. In the process of expansion, the algorithm expands all the candidate route points corresponding to the eight directions of the route point n according to the expansion rules of the jump point. Assuming that the device selection point is represented by m, the evaluation function value of each candidate point m is calculated. , select the path point with the smallest value of the evaluation function among the candidate points as the next sub-target point, and then repeat steps 2 and 3 until the found path point is the target point E, then the algorithm obtains the initial path P _n . Otherwise the algorithm search fails.

The method for detecting the connectivity between path points in step 4 is as follows:

Aiming at the existence of redundant waypoints in the path, the algorithm adopts the method of detecting the connectivity between waypoints to remove useless waypoints. Starting from the starting point S, smooth the initial path point set _Pn obtained according to the algorithm, and judge the distance between the two path points of the current path point P _i , the former path point P _i-1 and the latter path point P _{i+1 one} by one Whether there is an obstacle in the case of direct connection, if there is no obstacle, delete the redundant waypoint P _i , if there is an obstacle, continue to judge the next waypoint until all the waypoints are traversed. The specific process is shown in Figure 1.

In order to judge whether there is an obstacle between the waypoints, apply Algorithm 1, it is required to input the coordinate values of the two waypoints and the slope value k of the line between the two points, and it is divided into three cases, one is between two points The case where the slope of the line does not exist, that is, the line is perpendicular to the X axis, and the relative positions of the two path points are on the same vertical line; the second case is the case where the slope of the line is zero, that is, the line is perpendicular to the Y axis, and the two paths The relative positions of the points are on the same horizontal line; the third case is when the slope exists and is not zero, and the relative position of the two path points is in the diagonal direction, it is necessary to first determine whether the slope is greater than 1, because whether the slope is greater than 1 determines Then check the starting basis direction, and then calculate another corresponding coordinate value according to the function f or the inverse function f ^-1 of the straight line, and then check each point step by step to complete the judgment operation whether there is an obstacle.

The method of steering judgment and corner calculation in step 5 is as follows:

The judgment result of the direction of the waypoint is one of turning left, turning right, or maintaining the original direction. If the robot keeps turning left, it can be regarded as the robot rotating in reverse time. If the robot keeps turning right, it can be regarded as the robot When doing clockwise rotation, that is, the judgment of turning left and right can be transformed into the problem of judging the direction of clockwise rotation and counterclockwise rotation at the inflection point. Here, this paper uses the vector product of vectors to judge whether the relationship between two vectors is clockwise or counterclockwise.

Suppose a=(a _x , a _y , a _z ), b=(b _x , b _y , b _z ), then the determinant expression of the vector product is as follows:

The coordinate expression is:

a×b＝(a _y b _z -a _z b _z )i+(a _z b _x -a _x b _z )j+(a _x b _y -a _y b _x )k;

Judgment rules:

When a×b>0, it is counterclockwise, that is, the vector b is on the left side of the vector a;

When a×b<0, it is clockwise, that is, the vector b is on the right side of the vector a;

When a×b=0, the direction does not change, that is, the vector b is in the same direction as the vector a.

After obtaining the rotation direction at a certain path point, it is necessary to calculate the rotation angle of the robot (0° to 180°), and the calculation problem of the rotation angle can also be transformed into the problem of vector angle calculation. Taking the two vectors a and b for calculating the vector product as an example, the formula for calculating the angle between the vectors is:

The present invention has the following technical effects.

1) In the present invention, the alternative path expansion method adopts the method of jumping point search strategy to replace the traditional neighbor expansion mode, which effectively reduces the expansion of invalid alternative path points, reduces the algorithm's requirements for storage performance, and makes the algorithm more sensitive to OPEN The number of operations on the list and the CLOSE list is greatly reduced, and finally achieves the purpose of improving the pathfinding speed of the algorithm.

2) The present invention optimizes the initial path obtained by the algorithm, and removes redundant path points by using the method of detecting connectivity between path points at intervals, effectively shortening the total path length, and smoothing the path to a certain extent , which also greatly reduces the cumulative turning angle of the robot, reducing the time for the robot to rotate at the waypoint.

3) The present invention uses the vector cross product and vector dot product to add information such as steering judgment and rotation angle to the path points in the obtained path for the algorithm, which is conducive to the robot's autonomous adjustment of pose and action at the path point, and satisfies the real-time control of robot motion. sex.

It will be apparent to those skilled in the art that the invention is not limited to the details of the above-described exemplary embodiments, but that the invention can be embodied in other specific forms without departing from the spirit or essential characteristics of the invention. Accordingly, the embodiments should be regarded in all points of view as exemplary and not restrictive, the scope of the invention being defined by the appended claims rather than the foregoing description, and it is therefore intended that the scope of the invention be defined by the appended claims rather than by the foregoing description. All changes within the meaning and range of equivalents of the elements are embraced in the present invention.

Claims (8)

1. A path planning method for indoor complex conditions of a service robot, characterized in that, comprising the steps of: Step 1. Determine the coordinate values of the starting point and the target point based on the grid map represented by a two-dimensional array, create and initialize the OPEN list and the CLOSE list, and place the starting point in the OPEN list; among them, the OPEN list is used to store the expanded For the unexamined waypoints, the CLOSE list is used to record the waypoints that have been visited; Step 2, detect the OPEN list, and judge whether the path search is successful, if not, go to step 3; Step 3, use jump point expansion rules to find alternative paths to reduce the expansion of invalid alternative points and speed up path search; Step 4, detecting the connectivity between waypoints and removing useless waypoints; Step 5, based on the vector cross product and the vector dot product, add steering judgment and required rotation angle information to each way point. 2. The indoor complex path planning method for service robots according to claim 1, wherein the step 2 comprises: Check whether the OPEN list is empty, if the OPEN list is empty, it means that the path search failed and no feasible path was found; if the OPEN list is not empty, select the path point with the smallest evaluation function value from the list, and then select the path point from the OPEN Take it out of the list and put it in the CLOSE list; Check whether the path point corresponds to the coordinate value of the target point, if yes, it means that the path search is successful, exit, if the coordinate value does not correspond, go to step 3. 3. The indoor complex path planning method for service robots according to claim 2, wherein the evaluation function is expressed as: f(n)=g(n)+h(n); Among them, f(n) refers to the cost estimate from the initial path point through the intermediate path point n to the target point, g(n) is the actual cost value from the initial point to the intermediate path point n in the state space, h(n ) is the estimated cost of the best path from the intermediate waypoint to the goal point. 4. The indoor complex path planning method for service robots according to claim 3, wherein the step 3 comprises: Set the expansion rule of the jump point; the expansion rule is: if y has the minimum value k, so that And one of the following conditions is met, then the path point y is a jump point from the path point x in the d direction: condition ①, the path point y is the target point; condition ②, when expanding the jump point along the straight line, the path point y contains Forced neighbors; condition ③, when expanding the jump point along the diagonal direction, there is a jump point z obtained by expanding along the straight line or diagonal direction through the y path point, that is, the jump point z passes through the y path point and satisfies the condition ② ; In the process of expansion, according to the expansion rules of jump points, all the alternative path points corresponding to the eight directions of the path point are expanded. Assuming that the equipment selection point is represented by m, the evaluation function value of each alternative point m is calculated, and the selection The path point with the smallest evaluation function value among the candidate points is the next sub-target point, and then repeat steps 2 and 3 until the found path point is the target point, and the initial path P _n is obtained, otherwise the algorithm search fails. 5. The indoor path planning method for service robots in complex conditions according to claim 4, wherein said step 4 comprises: Starting from the starting point, smooth the obtained initial path point set P _n , and judge the direct connection between the previous path point P _i-1 and the rear path point P _i+1 of the current path point P _i one by one Whether there is an obstacle, if there is no obstacle, delete the redundant waypoint P _i , if there is an obstacle, continue to judge the next waypoint until all the waypoints are traversed. 6. The indoor complex path planning method for service robots according to claim 5, wherein the method for judging whether there is an obstacle in the case of direct connection between two path points described in step 4 comprises: Input the coordinate values of two path points and the slope value k of the line between the two points, and process in three cases: One is the case where the slope of the line between two points does not exist, that is, the line is perpendicular to the X axis, and the relative positions of the two path points are on the same vertical line; The second case is when the slope of the straight line is zero, that is, the straight line is perpendicular to the Y axis, and the relative positions of the two path points are on the same horizontal line; The third case is when the slope exists and is not zero, and the relative position of the two path points is in the direction of the diagonal line, first judge whether the slope is greater than 1, determine the starting direction of the check, and then use the function f or inverse of the straight line The function f ^-1 calculates another corresponding coordinate value, and then checks each point step by step to complete the judgment operation of whether there is an obstacle. 7. The indoor complex path planning method for service robots according to claim 6, characterized in that the method of steering judgment in step 5 comprises: Suppose a=(a _x , a _y , a _z ), b=(b _x , b _y , b _z ), then the determinant expression of the vector product is as follows: The coordinate expression is: a×b＝(a _y b _z -a _z b _z )i+(a _z b _x -a _x b _z )j+(a _x b _y -a _y b _x )k; Judgment rules: When a×b>0, it is counterclockwise, that is, the vector b is on the left side of the vector a; When a×b<0, it is clockwise, that is, the vector b is on the right side of the vector a; When a×b=0, the direction does not change, that is, the vector b is in the same direction as the vector a. 8. The indoor complex path planning method for service robots according to claim 7, wherein the method for calculating the rotation angle in step 5 comprises: Based on the vector angle calculation, the rotation angle is obtained. The calculation formula of the vector angle is: where a and b are two vectors of the vector product.