Building robot path planning method and system
By introducing a two-way alternating search strategy into the A* algorithm, the traditional A* algorithm solves the problems of long calculation time, large memory consumption, and failure to consider the actual situation of the robot in path planning of construction robots, achieving more efficient and smoother path planning. path.
Patent Information
- Application Number
- CN202411384280.0
- Authority / Receiving Office
- CN · China
- Patent Type
- Applications(China)
- Current Assignee / Owner
- Filing Date
- 2024-09-30
- Publication Date
- 2025-01-17
AI Technical Summary
In the planning of building robot paths, traditional A* algorithms have problems such as large memory consumption, long calculation time, redundant search nodes, and failure to consider the actual situations such as robot size and motion posture.
A two-way alternating search strategy is introduced to alternately expand each node through forward and reverse searches, and the best node obtained using the opposite search strategy is expanded as its own target node, thereby ensuring that the forward and backward paths meet.
It effectively reduces search time, improves path planning efficiency, overcomes the shortcomings of the traditional A* algorithm, and generates paths that are smoother and more practical.
Smart Images
Figure CN119321765A_ABST
Abstract
Description
Technical Field
[0001] The present invention relates to the field of robot path technology, and in particular to a construction robot path planning method and system. Background Art
[0002] Robots are increasingly deployed in the construction industry, and the participation of robots in construction has become a global trend. In response to this development, some researchers have proposed a robot system equipped with perception sensors and intelligent algorithms to assist construction supervisors in remotely identifying building materials, detecting component installation and defects. The application of construction robots combined with virtual reality technology has significantly improved workers' trust in robots, self-efficacy, and situational awareness of construction sites. Researchers have proposed a hybrid positioning algorithm with adaptive weights, which effectively improves the robust positioning capability of robots in construction scenarios. In response to the initialization problem of robots when deployed in unknown locations, another study proposed an indoor robot initialization system based on the building information model (BIM), which uses object detectors to achieve automatic initialization of robots in unknown locations. The traditional A* algorithm simply takes the geometric path length as the actual cost, consumes a lot of memory, takes a long time to calculate, searches for redundant nodes, and regards the mobile robot as a mass node. It does not consider the size of the robot itself, nor the actual conditions of the mobile robot's movement and posture adjustment. Summary of the invention
[0003] The purpose of the present invention is to provide a construction robot path planning method and system to achieve the purpose of solving the problems raised in the above background technology.
[0004] To achieve the above object, the present invention provides the following technical solution: a construction robot path planning method, comprising the steps of: S1: Build an environment map; S2: Initialize the open list, closed list, and parent list of the forward and backward searches, add the starting point to the open list of the forward search, and then add the target point to the open list of the backward search; S3: Determine whether the open list of the forward search or the reverse search is empty, if not, continue to execute the steps, if yes, it means that after searching all searchable nodes, if no forward and reverse search encounter occurs, the path search fails; S4: Move the node with the smallest total cost F in the forward open list to the forward closed list and set it as the forward current node NF; move the node with the smallest total cost F in the reverse open list to the reverse closed list and set it as the reverse current node NB; S5: Determine whether the forward current node NF and the reverse current node NB are satisfied. If yes, it means that the forward and backward searches have been satisfied, and then output the path nodes according to the forward and backward parent lists. If not, perform forward search; S6: Start forward search: take the backward current point NB as the target, and judge whether the adjacent node Mi (i = 1-8) of NF is an obstacle or already in the closed table. If yes, ignore the adjacent node. If not, update the cost and the parent node of Mi, then update the forward open list and the forward current point NF. S7: Start reverse search: take the current node NF as the target, and the search steps are the same as the forward search; S8: Return to step (3) until the forward and reverse search paths meet.
[0005] Preferably, the heuristic functions for the forward and backward searches in S2 are respectively:
[0006]
[0007]
[0008] .
[0009] Preferably, the is the current node in the forward openlist, is the current node in the backward openlist, is the path from the starting point to Cost, is a heuristic function that estimates the lowest cost path from arrive , The cost from the target node to , is a heuristic function that estimates the cost of the cheapest path arrive .
[0010] Preferably, when the algorithm in S1 starts running, the starting point S and the target point G of the path are used as the initial nodes for forward and reverse searches, respectively. During the forward search, S is used as the starting point and G is used as the target point to obtain the forward optimal node S1, and then a backward search is performed with G and S1 as the starting point and target node, respectively, to obtain a backward optimal node G1, which is used as the target node for the next forward search.
[0011] Preferably, in the step S1, the BAS-A* algorithm alternately expands each node forward and backward, and uses the best node obtained by the reverse search strategy as its target node for expansion, thereby ensuring that the forward and backward paths meet.
[0012] Preferably, in the three-dimensional elevation map, the Euclidean distance function is expressed as
[0013] in, and The target nodes and the current node The elevation value of .
[0014] Preferably, the exponential function in the three-dimensional elevation map weights the heuristic function h(n), and the weighted evaluation function is as follows:
[0015] In the formula , when the current node is far away from the target node, h(n) is large and the weight Large, resulting in rapid encounters between nodes. When the current node and the target node are close to each other, the smaller h(n), the greater the weight. The smaller it is, the closer the estimated cost is to the actual cost.
[0016] A construction robot path planning system, the system comprising: establishing an environment map; Initialize the open list, closed list, and parent list for forward and backward searches, add the starting point to the open list for forward searches, and then add the target point to the open list for backward searches; Determine whether the open list of the forward search or the reverse search is empty, if not, continue to execute the steps, if yes, it means that after searching all searchable nodes, if no forward and reverse search encounter occurs, the path search fails; Move the node with the smallest total cost F in the forward open list to the forward closed list and set it as the forward current node NF; move the node with the smallest total cost F in the reverse open list to the reverse closed list and set it as the reverse current node NB; Determine whether the forward current node NF and the reverse current node NB are satisfied. If so, it means that the forward and backward searches have been satisfied. Then output the path nodes according to the forward and backward parent lists. If not, perform a forward search. Start forward search: take the backward current point NB as the target, and determine whether the adjacent node Mi (i = 1-8) of NF is an obstacle or already in the closed table. If yes, ignore the adjacent node. If not, update the cost and the parent node of Mi, then update the forward open list and the forward current point NF. Start reverse search: take the current node NF as the target, and the search steps are the same as the forward search; Return to step (3) until the forward and reverse search paths meet.
[0017] The present invention provides a construction robot path planning method and system, which has the following beneficial effects: (1) The present invention introduces a bidirectional alternating search strategy into A*. Based on the A* algorithm, the strategy alternately searches for paths from the starting point and the target point until the search paths meet. This strategy can effectively reduce the search time and improve the efficiency of path planning. It solves the problems of the traditional A* algorithm that simply takes the geometric path length as the actual cost, consumes a lot of memory, takes a long calculation time, and searches for redundant nodes. It also regards the mobile robot as a mass node, and does not consider the size of the robot itself, nor the actual situation of the mobile robot's movement and posture adjustment.
[0018] (2) In the present invention, theoretically, the forward search and the reverse search search towards the target point and the starting point respectively and meet at the center of the map. However, in practice, the paths may not intersect at the midpoint due to different obstacle distributions. Therefore, a method of alternately replacing the current best node in the forward and backward openlists is adopted. BRIEF DESCRIPTION OF THE DRAWINGS
[0019] Figure 1 This is a schematic diagram of the principle of the BAS-A* algorithm of the present invention for planning a path in a 100×100 grid map; Figure 2 For the Earth Figure 1 Arrival Figure 3 Schematic diagram of the path; Figure 3 It is a schematic diagram of the Improved A* algorithm of the present invention. DETAILED DESCRIPTION
[0020] In order to have a clearer understanding of the technical features, purposes and effects of the present invention, specific embodiments of the present invention are now described with reference to the accompanying drawings.
[0021] A construction robot path planning method and system provided by the present invention is as follows Figure 1 As shown: A construction robot path planning method, comprising the steps of: S1: Build an environment map; S2: Initialize the open list, closed list, and parent list of the forward and backward searches, add the starting point to the open list of the forward search, and then add the target point to the open list of the backward search; S3: Determine whether the open list of the forward search or the reverse search is empty, if not, continue to execute the steps, if yes, it means that after searching all searchable nodes, if no forward and reverse search encounter occurs, the path search fails; S4: Move the node with the smallest total cost F in the forward open list to the forward closed list and set it as the forward current node NF; move the node with the smallest total cost F in the reverse open list to the reverse closed list and set it as the reverse current node NB; S5: Determine whether the forward current node NF and the reverse current node NB are satisfied. If yes, it means that the forward and backward searches have been satisfied, and then output the path nodes according to the forward and backward parent lists. If not, perform forward search; S6: Start forward search: take the backward current point NB as the target, and judge whether the adjacent node Mi (i = 1-8) of NF is an obstacle or already in the closed table. If yes, ignore the adjacent node. If not, update the cost and the parent node of Mi, then update the forward open list and the forward current point NF. S7: Start reverse search: take the current node NF as the target, and the search steps are the same as the forward search; S8: Return to step (3) until the forward and reverse search paths meet; it is worth noting that the calculation method of the BAS-A* algorithm is not to calculate the path from the current point to the end point, but to plan the path from the current point to the minimum point in the openlist table on the other side, so as to ensure that the paths in the forward and reverse directions will eventually intersect. In addition, the BAS-A* algorithm does not need to ensure forward and reverse "synchronization", allowing one direction to be faster and the other direction to be slower. As for which direction is faster or slower, specific problems should be analyzed. One idea is that the openlists on both sides only give priority to the one with fewer nodes each time, so as to ensure that the final number of searched summary points will be reduced; To verify the effectiveness of the BAS-A* algorithm, multiple simulation experiments were performed in MATLAB 2022b to generate collision-free paths in a 100×100 grid map with three different terrains and 100 obstacles.
[0022] When the algorithm starts running, the starting point S and the target point G of the path are used as the initial nodes for forward and reverse searches, respectively. In the forward search, S is used as the starting point and G is used as the target point to obtain the forward optimal node S1. Then, G and S1 are used as the starting point and target node for backward search, respectively, to obtain a backward optimal node G1, which is used as the target node for the next forward search. Repeat the iteration until they meet, that is, the BAS-A* algorithm alternately expands each node forward and backward, and uses the best node obtained by the opposite search strategy as its own target node to expand, thereby ensuring that the forward and backward paths meet; The heuristic functions for forward and backward search in S2 are:
[0023]
[0024]
[0025] .
[0026] is the current node in the forward openlist, is the current node in the backward openlist, is the path from the starting point to Cost, is a heuristic function that estimates the lowest cost path from arrive , The cost from the target node to , is a heuristic function that estimates the cost of the cheapest path arrive .
[0027] When the algorithm in S1 starts running, the starting point S and the target point G of the path are used as the initial nodes for forward and reverse searches respectively. During the forward search, S is used as the starting point and G is used as the target point to obtain the forward optimal node S1. Then, backward search is performed with G and S1 as the starting point and target node respectively to obtain a backward optimal node G1, which is used as the target node for the next forward search.
[0028] In step S1, the BAS-A* algorithm alternately expands each node forward and backward, and uses the best node obtained by the reverse search strategy as its target node to expand, thereby ensuring that the forward and backward paths meet.
[0029] In a three-dimensional elevation map, the Euclidean distance function is expressed as
[0030] in, and The target nodes and the current node The elevation value of .
[0031] The exponential function in the three-dimensional elevation map weights the heuristic function h(n), and the weighted evaluation function is as follows:
[0032] In the formula , when the current node is far away from the target node, h(n) is large and the weight Large, resulting in rapid encounters between nodes. When the current node and the target node are close to each other, the smaller h(n), the greater the weight. The smaller it is, the closer the estimated cost is to the actual cost.
[0033] A construction robot path planning system, the system comprising: establishing an environment map; Initialize the open list, closed list, and parent list for forward and backward searches, add the starting point to the open list for forward searches, and then add the target point to the open list for backward searches; Determine whether the open list of the forward search or the reverse search is empty, if not, continue to execute the steps, if yes, it means that after searching all searchable nodes, if no forward and reverse search encounter occurs, the path search fails; Move the node with the smallest total cost F in the forward open list to the forward closed list and set it as the forward current node NF; move the node with the smallest total cost F in the reverse open list to the reverse closed list and set it as the reverse current node NB; Determine whether the forward current node NF and the reverse current node NB are satisfied. If so, it means that the forward and backward searches have been satisfied. Then output the path nodes according to the forward and backward parent lists. If not, perform a forward search. Start forward search: take the backward current point NB as the target, and determine whether the adjacent node Mi (i = 1-8) of NF is an obstacle or already in the closed table. If yes, ignore the adjacent node. If not, update the cost and the parent node of Mi, then update the forward open list and the forward current point NF. Start reverse search: take the current node NF as the target, and the search steps are the same as the forward search; Return to step 3 until the forward and reverse search paths meet.
[0034] Experimental verification In the above content, the effectiveness of each improvement is proved. In order to verify the effectiveness of the ImprovedA* algorithm that combines the above improvements, this study designed three different terrains (respectively, concave terrain, convex terrain, and rugged terrain) on a 100×100 elevation grid map, and arranged 50 and 100 obstacles in each terrain, a total of six maps for experimental verification. In order to compare the A* algorithm with the Improved A* algorithm, the algorithm was simulated 50 times on each map, and the average values of the algorithm calculation time, average path length, number of key nodes, climbing angle, and roll angle were obtained.
[0035] From the simulation results in the figure, it can be seen that the paths planned by the Improved A* algorithm in each map avoid areas with large slopes, and at the same time, the path turns are smoother.
[0036] From the experimental results in Table 4, we can see that Figures 1 to 3 Compared with the A* algorithm, the ImprovedA* algorithm reduces the algorithm calculation time by 71.05% to 82.90%, the number of key nodes by 51.94% to 70.53%, the climbing angle by 38.87% to 68.55% and all below 30°, and the roll angle by 71.65% to 85.29% and all below 10°, while the path length increases by 14.6% to 37.84%. At the same time, the path planned by the Improved A* algorithm is relatively smoother. Under the same terrain, compared with the case of 50 obstacles, when there are 100 obstacles, the search time of the A* algorithm will increase by about 4.32% to 7.65%, and the search time of the Improved A* algorithm will increase by about 12.76% to 32.22%. The changes in path length, search nodes, climbing angle and roll angle can be almost ignored.
[0037]
[0038] In order to further compare the performance of the Improved A* algorithm with that of A*, we also conducted hypothesis tests as shown in Tables 8, 9, and 10. The hypothesis test is a large sample population mean hypothesis test, and the null hypothesis definition H0 is: the A* algorithm is statistically superior to the Improved A* algorithm. We choose a significance level of 0.01, rejection region .if Less than , we can reject the null hypothesis H0, which means that the Improved A* algorithm performs better than the A* algorithm statistically.
[0039] In order to avoid areas with large slopes, the Improved A* algorithm will inevitably increase the path length, so there is no need to perform hypothesis testing on the path length between Improved A* and A*.
[0040] From the test results in Tables 5, 6, and 7, it can be concluded that in terms of statistical performance, the Improved A* algorithm is superior to the unimproved A* algorithm in terms of calculation time, number of nodes, climbing angle, and roll angle.
[0041]
[0042]
[0043]
[0044] An Improved A* algorithm is proposed, which introduces a two-way alternating search strategy to improve the efficiency of the search path. By filtering the path nodes, the path duplication problem caused by the two-way search is overcome, and the number of key nodes is reduced, which reduces the difficulty of the search. By introducing climbing angle constraints and roll angle constraints, the search path is limited to the area with a smaller slope. Finally, the Bézier curve is used to fit the path to obtain a smooth path.
[0045] The simulation results in elevation grid maps with different terrains and different numbers of obstacles show that the Improved A* algorithm can significantly improve the speed of path planning, reduce the number of key nodes to be searched, and reduce the climbing angle and roll angle of the path, making the planned path practical and feasible, ensuring the safety of the robot during movement. In addition, the path generated by the Improved A* algorithm is smoother and more conducive to the robot's motion control.
[0046] Although embodiments of the present invention have been shown and described, it will be appreciated by those skilled in the art that various changes, modifications, substitutions and variations may be made to the embodiments without departing from the principles and spirit of the present invention, and that the scope of the present invention is defined by the appended claims and their equivalents.
Claims
1. A construction robot path planning method, characterized in that: The steps include: S1: Build an environment map; S2: Initialize the open list, closed list, and parent list of the forward and backward searches, add the starting point to the open list of the forward search, and then add the target point to the open list of the backward search; S3: Determine whether the open list of the forward search or the reverse search is empty, if not, continue to execute the steps, if yes, it means that after searching all searchable nodes, if no forward and reverse search encounter occurs, the path search fails; S4: Move the node with the smallest total cost F in the forward open list to the forward closed list and set it as the forward current node NF; move the node with the smallest total cost F in the reverse open list to the reverse closed list and set it as the reverse current node NB; S5: Determine whether the forward current node NF and the reverse current node NB are satisfied. If yes, it means that the forward and backward searches have been satisfied, and then output the path nodes according to the forward and backward parent lists. If not, perform forward search; S6: Start forward search: take the backward current point NB as the target, and judge whether the adjacent node Mi (i=1-8) of NF is an obstacle or already in the closed list. If yes, ignore the adjacent node. If not, update the cost and the parent node of Mi, then update the forward open list and the forward current point NF. S7: Start reverse search: take the current node NF as the target, and the search steps are the same as the forward search; S8: Return to step (3) until the forward and reverse search paths meet.
2. A construction robot path planning method according to claim 1, characterized in that: The heuristic functions for the forward and backward searches in S2 are: 。 3. A construction robot path planning method according to claim 2, characterized in that: Said is the current node in the forward openlist, is the current node in the backward openlist, is the path from the starting point to Cost, is a heuristic function that estimates the lowest cost path from arrive , The cost from the target node to , is a heuristic function that estimates the cost of the cheapest path under arrive .
4. A construction robot path planning method according to claim 1, characterized in that: When the algorithm in S1 starts running, the starting point S and the target point G of the path are used as the initial nodes for forward and reverse searches respectively. During the forward search, S is used as the starting point and G is used as the target point to obtain the forward optimal node S1, and then a backward search is performed with G and S1 as the starting point and target node respectively to obtain a backward optimal node G1, which is used as the target node for the next forward search.
5. A construction robot path planning method according to claim 4, characterized in that: In the step S1, the BAS-A* algorithm alternately expands each node forward and backward, and uses the best node obtained by the reverse search strategy as its target node for expansion, thereby ensuring that the forward and backward paths meet.
6. A construction robot path planning method according to claim 5, characterized in that: In the three-dimensional elevation map, the Euclidean distance function is expressed as in, and The target nodes and the current node The elevation value of .
7. A construction robot path planning method according to claim 6, characterized in that: The exponential function in the three-dimensional elevation map weights the heuristic function h(n), and the weighted evaluation function is as follows: In the formula , when the current node is far away from the target node, h(n) is large and the weight Large, resulting in rapid encounters between nodes. When the current node and the target node are close to each other, the smaller h(n), the greater the weight. The smaller it is, the closer the estimated cost is to the actual cost.
8. A construction robot path planning system, characterized in that: The system includes: establishing an environmental map; Initialize the open list, closed list, and parent list for forward and backward searches, add the starting point to the open list for forward searches, and then add the target point to the open list for backward searches; Determine whether the open list of the forward search or the reverse search is empty, if not, continue to execute the steps, if yes, it means that after searching all searchable nodes, if no forward and reverse search encounter occurs, the path search fails; Move the node with the smallest total cost F in the forward open list to the forward closed list and set it as the forward current node NF; move the node with the smallest total cost F in the reverse open list to the reverse closed list and set it as the reverse current node NB; Determine whether the forward current node NF and the reverse current node NB are satisfied. If so, it means that the forward and backward searches have been satisfied. Then output the path nodes according to the forward and backward parent lists. If not, perform a forward search. Start forward search: take the backward current point NB as the target, and determine whether the adjacent node Mi (i = 1-8) of NF is an obstacle or already in the closed table. If yes, ignore the adjacent node. If not, update the cost and the parent node of Mi, then update the forward open list and the forward current point NF. Start reverse search: take the current node NF as the target, and the search steps are the same as the forward search; Return to step (3) until the forward and reverse search paths meet.