Improved heuristic static path planning method and system applied to mobile robot

Abstract

The application relates to a mobile robot improved heuristic static path planning method and system, and relates to the technical field of robot path planning. The application is proposed to balance the mobility of a mobile robot in all aspects. Technical points: a straight line segment is obtained by connecting a starting point and an ending point, the straight line segment intersects with an obstacle to obtain an intersection point, then a circle is drawn with the intersection point as the center and R as the radius, two intersection points between the circle and the straight line segment are called intersection points, and a boundary point of the obstacle is obtained. A direction number describing the expansion of a current point to the surroundings is introduced in a traditional A-star algorithm. The first intersection point is taken as a starting point, the last intersection point is taken as an ending point, the last intersection point is taken as a starting point, and the last boundary point is taken as an ending point, which are introduced into the improved A-star algorithm, a series of local paths are obtained, and then the local paths are combined into a global path.

Description

Technical Field

[0001] This invention relates to the field of robot path planning technology, and in particular to an improved heuristic static path planning method for mobile robots. Background Technology

[0002] Mobile robots play a vital role in national industrialization and informatization, and have been widely applied in agricultural production, industrial manufacturing, marine development, transportation, aerospace, and national defense. In particular, the gradual realization of autonomous vehicle technology in recent years has further propelled the development of mobile robot technology. Currently, the focus and challenge of mobile robot research lies in how to enable robots to autonomously complete tasks in complex environments. For a mobile robot, planning an effective and feasible path from the starting point to the destination on an existing map, and moving from the starting point to the destination without collisions, is essential for completing navigation tasks. Therefore, research on mobile robot path planning is a research topic of practical significance. Considering the impact of task completion time and power consumption, the path obtained through path planning must comprehensively consider factors such as path length and speed. However, the traditional A-star algorithm's node search strategy has drawbacks such as many inflection points and large turning angles, and the feasible paths it generates are not theoretically optimal.

[0003] The existing technology, document number CN114740870A, discloses a path planning system and method for a home robot based on Internet of Things (IoT) technology. The system includes a main control system, a power module, a LiDAR, a camera module, a sensor module, an RFID sensor module, a handle control module, a display module, a photoelectric encoder, a drive motor module, and a power module. The method includes the following steps: constructing a home environment map; frequently updating the indoor environment model to adjust the map in real time, considering the complex indoor environment; static path planning, where the home robot uses a grid modeling strategy to divide the environment into several grid areas, and optimizes the ant colony algorithm using an extended tree stochastic optimization method to achieve global static path planning for the home robot; and dynamic obstacle avoidance, where the mobile home robot uses a trajectory prediction scheme that integrates a social force model and Kalman filtering to predict the trajectory of moving obstacles. It can achieve reasonable path planning and dynamic obstacle avoidance.

[0004] The prior art, document number CN111426328A, discloses a robot path planning method for static scenes, including the following steps: acquiring surrounding static scene information and generating a grid map; marking the grids on the grid map, including obstacles, no obstacles, start point, and end point; dividing the grids marked as obstacles into a nine-square grid; marking pseudo-obstacles or real obstacles; executing an improved A* algorithm on the marked grid map; reading the parent grids of the grids starting from the end point until the start point is reached, recording the grids passed during the reading process; these grids connecting the start point and the end point constitute the robot's path; and calculating the robot's offset angle during movement by calculating the angle between the direction vectors of adjacent grids along the path. The method disclosed in this invention can improve path planning accuracy while using lower storage resources, and offers fast path planning speed and high accuracy.

[0005] In other words, existing mobile robot path planning does not take into account the advantages of mobile robots in all aspects. Summary of the Invention

[0006] The technical problem to be solved by this invention is:

[0007] To take into account the advantages of mobile robots in all aspects, this paper proposes an improved heuristic static path planning method and system for mobile robots (a static path planning method and system based on heuristics). By designing different map environments, the performance of the path planning algorithm based on heuristics is evaluated, and evaluation indicators are defined. Finally, the algorithm is compared with the traditional A-star algorithm to evaluate the effectiveness and feasibility of the static path planning algorithm based on heuristics.

[0008] The technical solution adopted by the present invention to solve the above-mentioned technical problems is as follows:

[0009] An improved heuristic static path planning method is applied to mobile robots. The implementation process of the method is as follows:

[0010] Step 1: The robot acquires environmental information from the current map, including its own position, its position within the environment, its starting point, its ending point, and obstacle information. Let the starting point be A, and its coordinates be (x, y, y). start ,y start The endpoint is B, and the coordinates are (x...). goal ,y goal Define that there are at least two obstacles between the starting point A and the ending point B;

[0011] Step 2: Use analytic geometry to find the line segment based on the coordinates of the starting and ending points. The equation:

[0012] (yy start (x) goal -xstart ) = (xx start )(y goal -y start (1)

[0013] Step 3: Detect the straight line segment connecting the start and end points. Whether or not obstacles are encountered, if not, then the optimal path between the start and end points is a straight line segment. If you need to pass through an obstacle, first determine the distance between the obstacle and the straight line segment. Intersection C 1i (x obs ,y obs (i = 1, 2, 3...), take the first intersection point C. 11 (x obs ,y obs ), with C 11 (x obs ,y obs Draw a circle with center r and radius r. The equation of the circle is:

[0014] (xx obs ) 2 +(yy obs ) 2 =r 2 (2)

[0015] Calculate the intersection of the circle and the line segment based on the equations of the line and the circle. The intersection of the points yields the point.

[0016] Step 4: Find the first intersection point C 11 (x obs ,y obs Given the edge points of the obstacle, obtain points E1(x1,y1) and E2(x2,y2), and calculate the intersection point C between edge points E1, E2 and the obstacle. 11 (x obs ,y obs Distance:

[0017]

[0018] n takes the value 1 or 2, representing the intersection point C of edge points E1 and E2. 11 (x obs ,y obs The distance;

[0019] If d1 > d2, then keep point E2; otherwise, keep point E1. Assume point E1 is far from intersection point C. 11 (x obs ,y obs Since they are relatively close, keep E1 and discard E2;

[0020] Step 5: Determine the number of intersections between line AB and the obstacle; if the number of intersections is 1, proceed to Step 6; if the number of intersections is greater than 1, proceed to Step 7.

[0021] Step Six: If the number of intersection points is equal to 1, then use point... Starting from point E1 and ending at point E1(x1,y1), the shortest path is found using the A-Star algorithm and designated as local path 1, i.e., point E1(x1,y1). The curve segment leading to point E1(x1,y1);

[0022] Starting point A(x) start ,y start )arrive A straight line connection yields local path 2, from point E1(x1,y1) to the endpoint B(x). goal ,y goal If a straight line is used to connect local paths 3, then local paths 1 to 3 are merged, i.e., A→D1→E1→B, thus obtaining the optimal path.

[0023] Step 7: If the number of intersection points is greater than 1, take point E1(x1,y1) as the new starting point and find the distance from point E1(x1,y1) to point B(x1,y1). goal ,y goal Find the equation of the straight line, and repeat steps two through four to obtain point C. 21 (x obs2 ,y obs2 ), And the corresponding edge points E3(x3,y3), E4(x4,y4) of the obstacles, and then using points Starting point The shortest path to the destination is found using the A-star algorithm and is taken as local path 1.

[0024] Step 8: Based on Step 7, perform another judgment to determine the distance from point E1(x1,y1) to point B(x). goal ,y goal The number of intersection points of the connected straight line segments with obstacles is counted. If the number of intersection points is greater than 1, then proceed to step seven.

[0025] If the number of intersection points is equal to 1, then use the point... Starting from point A and ending at point E3(x3,y3), the local path 2 is obtained using the A-Star algorithm. start ,y start )arrive A straight line connection yields local path 3, from point E3(x3,y3) to the endpoint B(x). goal ,y goal Connecting the lines gives local path 4; merging local paths 1-4 gives the optimal path, which is A→D1→D3→E3→B.

[0026] Furthermore, define at least two long strip-shaped obstacles between the starting point A and the ending point B.

[0027] Furthermore, the A-star algorithm is an improved heuristic A-star algorithm.

[0028] Furthermore, the improved heuristic A-star algorithm is specifically as follows:

[0029] Using the robot's current position (current point) as a reference, the surrounding points are scored using a cost function, and the point with the smallest score is selected as the next point. The next point is the position that the robot will reach in the next moment.

[0030] The cost function of the A-star algorithm is defined as:

[0031] f(n)=h(n)+g(n) (4)

[0032] Where n is the current observation point; f(n) is the sum of the evaluation functions; g(n) is the actual path cost from the starting point to the current observation point; h(n) is the estimated path cost from the current observation point to the target point; h(n) is calculated using the Euclidean distance between the observation point and the target point.

[0033]

[0034] Where (x) n ,y n (x) represents the coordinates of the current observation point; goal ,y goal () are the coordinates of the target point;

[0035] Increasing the number of directions searched around the current point n, when the parameter Connecting_distance is defined as 1, 2, 3 and 4, will generate 8, 16, 32 and 48 directions respectively. That is, when the connection distance Connecting_distance = 1, 2, 3 and 4, the A-star algorithm will expand in 8, 16, 32 and 48 directions.

[0036] An improved heuristic static path planning system for mobile robots is applied. This system has program modules corresponding to the steps of the above-mentioned technical solution, and executes the steps of the improved heuristic static path planning method for mobile robots during runtime.

[0037] A computer-readable storage medium storing a computer program configured to be invoked by a processor of the above-described steps of the improved heuristic static path planning method for mobile robots.

[0038] This invention offers the following beneficial technical effects: It evaluates the performance of a heuristic-based path planning algorithm by designing different map environments and defines evaluation metrics. Finally, it compares the algorithm with the traditional A-star algorithm to assess the effectiveness and feasibility of the heuristic-based static path planning algorithm. In this invention, a static path refers to a path where obstacles remain stationary. The innovation of this invention lies in connecting the starting and ending points to obtain a straight line segment. This line segment intersects the obstacle at a point. Then, a circle is drawn with this intersection point as its center and radius R. The two intersection points between the circle and the line segment are called junction points, and the obstacle boundary points are determined. The traditional A-star algorithm introduces the number of directions describing the expansion of the current point outwards. By substituting the first junction point as the starting point and the last junction point as the ending point, and vice versa, into the improved A-star algorithm, a series of local paths are obtained. These local paths are then combined into a global path. Attached Figure Description

[0039] Figure 1 This is a detailed schematic diagram illustrating the implementation steps of the method of the present invention;

[0040] Figure 2 To use different parameter orientation maps, in the figure: (a) is the orientation map when the parameter Connecting_distance = 1, (b) is the orientation map when the parameter Connecting_distance = 2, (c) is the orientation map when the parameter Connecting_distance = 3, and (d) is the orientation map when the parameter Connecting_distance = 4;

[0041] Figure 3 Comparison of traditional A-star algorithm and improved algorithm for algorithms that do not pass through obstacles;

[0042] Figure 4 The results of the traditional A-star algorithm and the improved algorithm are shown in the figure with different Connecting_distance parameters. In the figure: (a) is the result comparison when the parameter Connecting_distance = 1, (b) is the result comparison when the parameter Connecting_distance = 2, (c) is the result comparison when the parameter Connecting_distance = 3, and (d) is the result comparison when the parameter Connecting_distance = 4.

[0043] Figure 5The graphs show the results of the traditional A-star algorithm and the improved algorithm using different Connecting_distance parameters. In the graphs: (a) is a comparison of the results when the Connecting_distance = 1, (b) is a comparison of the results when the Connecting_distance = 2, (c) is a comparison of the results when the Connecting_distance = 3, and (d) is a comparison of the results when the Connecting_distance = 4. Detailed Implementation

[0044] Combined with appendix Figure 1-5 The implementation process of the improved heuristic static path planning method for mobile robots described in this invention is explained in detail below:

[0045] Step 1: The robot acquires environmental information from the current map, such as its own position, its location within the environment, its starting point, its destination, and obstacle information. Figure 1 The starting point is A, and the coordinates are (x, y). start ,y start The endpoint is B, and the coordinates are (x...). goal ,y goal The black straight lines represent obstacles.

[0046] Step 2: Use analytic geometry to find the line segment based on the coordinates of the starting and ending points. The equation:

[0047] (yy start (x) goal -x start ) = (xx start )(y goal -y start (1)

[0048] Step 3: Detect the straight line segment connecting the start and end points. Whether or not obstacles are encountered, if not, then the optimal path between the start and end points is a straight line segment. And exit the algorithm; if it passes through an obstacle, such as Figure 1 As shown, first determine the obstacles and the straight line segments. Intersection C 1i (x obs ,y obs (i = 1, 2, 3...), take the first intersection point C. 11 (x obs ,y obs ), with C 11 (x obs ,y obs Draw a circle with center r and radius r. The equation of the circle is:

[0049] (xx obs ) 2 +(yy obs ) 2 =r 2 (2)

[0050] Calculate the intersection of the circle and the line segment based on the equations of the line and the circle. The intersection of the points yields the point.

[0051] Step 4: Find the first intersection point C 11 (x obs ,y obs Given the edge points of the obstacle, obtain points E1(x1,y1) and E2(x2,y2), and calculate the intersection point C between edge points E1, E2 and the obstacle. 11 (x obs ,y obs Distance:

[0052]

[0053] If d1 > d2, then keep point E2; otherwise, keep point E1. Figure 1 Since the obvious point E1 is closer to the intersection point, keep E1 and discard E2.

[0054] Step 5: Determine the number of intersections between the line and the obstacle. If the number of intersections is 1, then... Starting from point E1(x1,y1) and ending at point E1(x1,y1), the shortest path is found using the A-Star algorithm and designated as local path 1. Figure 1 Point (a) The blue line pointing to point E1(x1,y1);

[0055] The A-Star algorithm is a typical heuristic algorithm that, given a global (static) map and a starting and ending point, can generate a relatively reasonable path. Its core principle is to use the current point as a baseline, score surrounding points using a cost function, and select the point with the lowest score as the next point.

[0056] The cost function of the A-star algorithm is defined as:

[0057] f(n)=h(n)+g(n) (4)

[0058] Where n is the current observation point; f(n) is the sum of the evaluation functions; g(n) is the actual path cost from the starting point to the current observation point; and h(n) is the estimated path cost from the current observation point to the target point. h(n) is calculated using the Euclidean distance between the observation point and the target point.

[0059]

[0060] Where (x) n ,y n (x) represents the coordinates of the current observation point; goal ,y goal ) are the coordinates of the target point.

[0061] In the standard A-Star algorithm, only 8 surrounding points are searched, which may lead to a suboptimal path. To make the robot path smoother, the number of directions from the current point to the surrounding areas is increased. This invention defines the parameter `Connecting_distance` as 1, 2, 3, and 4, which generates 8, 16, 32, and 48 possible directions respectively. In other words, when `Connecting_distance` = 1, 2, 3, and 4, the algorithm expands to 8, 16, 32, and 48 directions. The effect is shown in the figure below. Figure 2 As shown in Table 1, the mapping relationship between the number of directions and the parameter Connecting_distance is shown in Table 1.

[0062] Table 1 shows the relationship between Connecting_distance and the number of extended directional points.

[0063]

[0064] Starting point A(x) start ,y start )arrive A straight line connection yields local path 2, from point E1(x1,y1) to the endpoint B(x). goal ,y goal If a straight line connection results in local path 3, then the local paths are merged: A→D1→E1→B, as shown below. Figure 1 As shown in (a);

[0065] Step Six: If the number of intersection points is greater than 1, take point E1(x1,y1) as the new starting point and find the distance from point E1(x1,y1) to point B(x1,y1). goal ,y goal Find the equation of the straight line, and repeat steps 2-4 to obtain point C. 21 (x obs2 ,y obs2 ), Then use points Starting point The shortest path to the destination is found using the A-star algorithm and is taken as local path 1.

[0066] Step 7: Repeat steps 5 and 6 to obtain points E3(x3,y3) and E4(x4,y4). Determine the distance from point E1(x1,y1) to point B(x4,y4). goal ,y goal The number of intersections with obstacles: If we consider case 1(b) in the diagram, where the number of intersections is 1, then the number of intersections is... Starting from point A and ending at point E3(x3,y3), the local path 2 is obtained using the A-Star algorithm. start ,y start )arrive A straight line connection yields local path 3, from point E3(x3,y3) to the endpoint B(x). goal ,y goal The direct connection yields local path 4. After the final modification, the merged local paths are: A→D1→D3→E3→B, as shown below. Figure 1 As shown in (b).

[0067] Simulation analysis:

[0068] Mobile robot map environment establishment: A grid map with a length of 140 and a width of 120. When the index value corresponding to a position in the grid map is 1, it is an obstacle. When the index value corresponding to a position in the grid map is 0, it means that there is no obstacle.

[0069] The evaluation platform configuration is as follows: CPU: Intel(R) Core(TM) i5-7200U CPU@2.50GHz 2.70GHz, 8GRAM, Windows 10 64bit, evaluation environment: Matlab R2016a.

[0070] When there are no obstacles between the start and end points, the corresponding simulation results are as follows: Figure 3 As shown in Table 2, the specific data is as follows.

[0071] Table 2 Comparison data without obstacles

[0072]

[0073] For the two obstacle simulation results, with the starting point at (20, 10) and the ending point at (70, 110), the comparison results between the improved algorithm and the traditional algorithm for different orientation patterns are as follows: Figure 4 As shown in Table 3, the specific data is as follows.

[0074] Table 3 Results of the traditional A-star algorithm and the improved algorithm for different connecting distance parameters

[0075]

[0076] For simulation results involving multiple obstacles, with a starting point (20, 10) and an ending point (50, 110), the comparison results between the improved algorithm and the traditional algorithm for different orientation patterns are as follows: Figure 5 As shown in Table 4, the specific data is as follows.

[0077] Table 4 Results of Traditional A-star Algorithm and Improved Algorithm with Different Connecting Distance Parameters

[0078]

[0079] This invention simulates the traditional A-Star algorithm and an improved algorithm in different static environments where a mobile robot is located. The results show that the traditional A-Star algorithm has drawbacks such as excessively large turning angles, uneven path smoothness, and suboptimal path planning. The improved algorithm plans a better path than the traditional A-Star algorithm, and the path becomes smoother and shorter as the orientation expands. This smoothness and shorter path reduces the robot's movement time, thereby reducing power consumption and ensuring the robot has more energy to complete its tasks, thus improving its work efficiency. The experimental results verify the feasibility, effectiveness, and superiority of the improved algorithm, which has practical significance for the research and development of mobile robot path planning problems.

Claims

1. An improved heuristic static path planning method for mobile robots, characterized in that, The implementation process of the method is as follows: Step 1: The robot acquires environmental information from the current map, including its own position, its position within the environment, its starting point, its ending point, and obstacle information. Let the starting point be A, and its coordinates be (x, y, y). start ,y start The endpoint is B, and the coordinates are (x...). goal ,y goal Define that there are at least two obstacles between the starting point A and the ending point B; Step 2: Use analytic geometry to find the line segment based on the coordinates of the starting and ending points. The equation: (yy start )(x goal -x start )=(xx start )(and goal -and start ) (1) Step 3: Detect the straight line segment connecting the start and end points. Whether or not obstacles are encountered, if not, then the optimal path between the start and end points is a straight line segment. If you need to pass through an obstacle, first determine the distance between the obstacle and the straight line segment. Intersection C 1i (x obs ,y obs (i = 1, 2, 3...), take the first intersection point C. 11 (x obs ,y obs ), with C 11 (x obs ,y obs Draw a circle with center r and radius r. The equation of the circle is: (x-x obs ) 2 +(y-y obs ) 2 =r 2 (2) Calculate the intersection of the circle and the line segment based on the equations of the line and the circle. The intersection of the points yields the point. Step 4: Find the first intersection point C 11 (x obs ,y obs Given the edge points of the obstacle, obtain points E1(x1,y1) and E2(x2,y2), and calculate the intersection point C between edge points E1, E2 and the obstacle. 11 (x obs ,y obs Distance: n takes the value 1 or 2, representing the intersection point C of edge points E1 and E2. 11 (x obs ,y obs The distance; If d1 > d2, then keep point E2; otherwise, keep point E1. Assume point E1 is far from intersection point C. 11 (x obs ,y obs Since they are relatively close, keep E1 and discard E2; Step 5: Determine the number of intersections between line AB and the obstacles; If the number of intersections is equal to 1, proceed to step six; if the number of intersections is greater than 1, proceed to step seven. Step Six: If the number of intersection points is equal to 1, then use point... Starting from point E1(x1,y1) and ending at point E1(x1,y1), the shortest path is found using the A-Star algorithm and designated as local path 1, i.e., point E1(x1,y1). The curve segment leading to point E1(x1,y1); Starting point A(x) start ,y start )arrive A straight line connection yields local path 2, from point E1(x1,y1) to the endpoint B(x). goal ,y goal If a straight line is used to connect local paths 3, then local paths 1 to 3 are merged, i.e., A→D1→E1→B, thus obtaining the optimal path. Step 7: If the number of intersection points is greater than 1, take point E1(x1,y1) as the new starting point and find the distance from point E1(x1,y1) to point B(x1,y1). goal ,y goal Find the equation of the straight line, and repeat steps two through four to obtain point C. 21 (x obs2 ,y obs2 ), And the corresponding edge points E3(x3,y3), E4(x4,y4) of the obstacles, and then using points Starting point The shortest path to the destination is found using the A-star algorithm and is taken as local path 1. Step 8: Based on Step 7, perform another judgment to determine the distance from point E1(x1,y1) to point B(x). goal ,y goal The number of intersections of the connected straight segments with obstacles is counted. If the number of intersections is greater than 1, then proceed to step seven. If the number of intersection points is equal to 1, then use the point... Starting from point A and ending at point E3(x3,y3), the local path 2 is obtained using the A-Star algorithm. start ,y start )arrive A straight line connection yields local path 3, from point E3(x3,y3) to the endpoint B(x). goal ,y goal Connecting the lines gives local path 4; merging local paths 1-4 gives the optimal path, which is A→D1→D3→E3→B.

2. The improved heuristic static path planning method for mobile robots according to claim 1, characterized in that, There must be at least two long, narrow obstacles between the starting point A and the ending point B.

3. The improved heuristic static path planning method for mobile robots according to claim 1 or 2, characterized in that, The A-star algorithm is an improved heuristic A-star algorithm.

4. The improved heuristic static path planning method for mobile robots according to claim 3, characterized in that, The improved heuristic A-star algorithm is specifically as follows: Using the robot's current position as a reference, the surrounding points are scored using a cost function, and the point with the smallest score is selected as the next point. The next point is the position that the robot will reach in the next moment. The cost function of the A-star algorithm is defined as: f(n)=h(n)+g(n) (4) Where n is the current observation point; f(n) is the sum of the evaluation functions; g(n) is the actual path cost from the starting point to the current observation point; h(n) is the estimated path cost from the current observation point to the target point; h(n) is calculated using the Euclidean distance between the observation point and the target point. Where (x) n ,y n (x) represents the coordinates of the current observation point; goal ,y goal () are the coordinates of the target point; Increasing the number of directions searched around the current point n, when the parameter Connecting_distance is defined as 1, 2, 3 and 4, will generate 8, 16, 32 and 48 directions respectively. That is, when the connection distance Connecting_distance = 1, 2, 3 and 4, the A-star algorithm will expand in 8, 16, 32 and 48 directions.

5. An improved heuristic static path planning system for mobile robots, characterized by: The system has a program module corresponding to the steps of any one of claims 1-4 above, and executes the steps described above in the improved heuristic static path planning method for mobile robots when it is run.

6. A computer-readable storage medium, characterized in that: The computer-readable storage medium stores a computer program configured to, when invoked by a processor, implement the steps of any one of the claims 1-4 for the improved heuristic static path planning method for mobile robots.

Images