Improved bidirectional theta* mobile robot path planning method for complex terrain

Abstract

The invention provides an improved bidirectional theta * mobile robot path planning method for a complex terrain. The method comprises the following steps: modeling environment information obtained from a sensing system into a trafficability grid map for a mobile robot; when the trafficability grid map is in a static environment, performing global path planning through an improved bidirectional theta * algorithm; and when the trafficability grid map changes, the trafficability grid map is in a dynamic environment, an incremental algorithm is added to judge edge cost change, and a trajectory updating map of the mobile robot in the dynamic environment is obtained through quadratic programming. On the basis of a traditional theta * algorithm, node transfer cost, topographic features and track smoothness are comprehensively considered, adaptive weight parameters are added, the problems that path corner turning is large and local optimum exists are effectively solved, the robustness and efficiency of path planning are improved, and the real-time effectiveness of the path and the moving safety of the robot are guaranteed.

Description

Technical Field

[0001] The invention relates to the technical field of path planning, and in particular to a mobile robot path planning method for improving bidirectional theta* in complex terrain. Background Art

[0002] With the widespread application of mobile robots in various industries, the complexity and uncertainty of their working environment are increasing. This puts more stringent requirements on traditional path planning algorithms. Therefore, it is crucial to develop an algorithm that can generate safe and efficient paths in real time in complex environments.

[0003] The current mainstream global path planning algorithms are generally planned in a static environment, such as the commonly used A* algorithm, Dijkstra algorithm, RRT sampling algorithm, ant colony algorithm, etc. However, when facing large-scale grid maps, the node search and path finding of traditional path planning will take up a lot of time and be inefficient. When facing environmental changes, the algorithm can only be re-called, and real-time paths cannot be obtained. In addition, the complexity of the environment makes it impossible for ordinary heuristic functions to accurately estimate the actual cost of the path, and it is easy to fall into local optimality, resulting in the inability to find the shortest path. Summary of the invention

[0004] The purpose of the present invention is to overcome the limitations of the above-mentioned prior art and provide a mobile robot path planning method for complex terrain with improved bidirectional theta*, including: modeling the environmental information obtained from the perception system as a passability grid map for the mobile robot; when the passability grid map is in a static environment, global path planning is performed by using an improved bidirectional theta* algorithm; when the passability grid map changes, the passability grid map can be in a dynamic environment, and the edge cost change is judged by adding an incremental algorithm, and the trajectory update map of the mobile robot in the dynamic environment is obtained by quadratic programming.

[0005] In one implementation, a passability grid map is modeled through the following steps: environmental information obtained from a perception system is modeled as a preliminarily generated grid map, wherein grids in the grid map include two states: occupied and idle; the preliminarily generated grid map is secondary processed according to the height of obstacles in the grids in the occupied state to obtain a passability grid map for the mobile robot.

[0006] In another implementation, when the accessibility grid map is in a static environment, the global path planning is performed by using an improved bidirectional theta* algorithm based on node transfer costs, terrain features, and trajectory smoothness, and adding adaptive weight parameters, where:

[0007] S31. Define p1. p1 represents the transfer cost between two nodes, which is calculated by Euclidean distance. The larger p1 is, the higher the transfer cost between two nodes is:

[0008]

[0009] Among them, (x c ,y c ,z c ) represents the coordinate value of the current node in the base coordinates, (x s ,y s ,z s ) represents the coordinate value of the starting point in the base coordinates;

[0010] S32. Define p2 to represent terrain features. The larger the p2, the greater the unevenness of the road surface, and the more difficult the transfer between two nodes. Convert the target mesh node and all meshes between them into corresponding triangular meshes, and calculate the normal vector of the triangular mesh.

[0011]

[0012]

[0013] Among them, n c and n i Represents the normal vector of the triangular mesh between the current node and the starting point, and k represents the number of meshes;

[0014] S33. Define p3 to represent the smoothness of the trajectory. The larger the p3, the more turning points the path has:

[0015]

[0016] Among them, P m-1 P m , P m P m+1 They represent path vectors respectively.

[0017] S34, based on the three constraint parameters of node transfer cost, terrain characteristics and trajectory smoothness obtained in step S33, the cost function is defined as follows based on the bidirectional theta* algorithm:

[0018] f1(c)=g1(c)+h1(c) (5)

[0019] f2(c)=g2(c)+h2(c) (6)

[0020] Where c represents the current node, g1(c) represents the actual cost from the path start point to the current node, g2(c) represents the actual cost from the path end point to the current node, h1(c) represents the estimated cost from the current node to the path end point, and h2(c) represents the estimated cost from the current node to the path start point.

[0021] Wherein, g(c) is defined by the three constraint parameters obtained in step S33:

[0022]

[0023] S35. Define the rhs function:

[0024]

[0025] Among them, c′ is the parent node of c, d(c′, c) is the cost value of the node between c′ and c, rhs(c) is the smallest node g(c) value among all child nodes of c′. In a static environment, rhs(c) = g(c), that is, only the node with the smallest cost is assigned the rhs(c) value.

[0026] In another implementation, the method further includes measuring h(c) using a weighted Euclidean distance, that is:

[0027]

[0028] w=1+e -(H+obs) (10)

[0029]

[0030] Among them, w is the adaptive weight coefficient of the heuristic function h(c), (x m ,y m ,z m ) represents the position coordinates of the middle node, (x g ,y g ,z g ) represents the position coordinates of the target node, and obs represents the number of obstacles near the current node.

[0031] In another implementation, when the accessibility grid map changes, an incremental algorithm is added to determine the edge cost change, and a trajectory update map of the mobile robot in a dynamic environment is obtained through quadratic programming, specifically including: when the accessibility grid map changes, the impact on the grid node is divided into three states: rhs(c)=g(c), rhs(c)<g(c) and rhs(c)>g(c); if the path node is in the first state rhs(c)=g(c), it means that the node is not affected; if the path node is in the second state rhs(c)=g(c), it means that the node is not affected. If the state rhs(c)<g(c), it means that the cost of reaching the child node c through the parent node c′ is lower, rhs(c) is assigned to g(c) and the path is updated; if the path node is in the third state rhs(c)>g(c), it means that the parent node becomes an obstacle, the state of the parent node and all its affected child nodes is updated, and then the current node where the mobile robot is currently located is taken as the new starting point, and searches are conducted from both ends to the middle until the preset termination condition is met, and a new shortest path is found to obtain the trajectory update map of the mobile robot in a dynamic environment.

[0032] Compared with the prior art, the present invention has the following beneficial effects:

[0033] (1) Based on theta*, the algorithm of the present invention improves its cost function, comprehensively considers the node transfer cost, terrain characteristics, and trajectory smoothness, effectively solves the problems of large path corners and the existence of local optimal points, and adds adaptive weight parameters to adjust the weight coefficient of the heuristic function according to the distance from the target point, thereby improving the efficiency of path search.

[0034] (2) The present invention adopts incremental bidirectional theta* to achieve real-time update of the path in a dynamic environment, solves the problem of being unable to quickly plan the shortest path under complex terrain, and reduces the burden on the system. BRIEF DESCRIPTION OF THE DRAWINGS

[0035] Figure 1 This is a flowchart of the steps of a mobile robot path planning method for complex terrain with improved bidirectional theta* according to an embodiment of the present invention.

[0036] Figure 2 For Figure 1 The corresponding overall step flow chart of the mobile robot path planning method for complex terrain with improved bidirectional theta*. DETAILED DESCRIPTION

[0037] The following will be combined with the drawings in the embodiments of the present invention to clearly and completely describe the technical solutions in the embodiments of the present invention. Obviously, the described embodiments are only part of the embodiments of the present invention, not all of the embodiments. Based on the embodiments of the present invention, all other embodiments obtained by ordinary technicians in this field without creative work are within the scope of protection of the present invention.

[0038] See also Figure 1 , 2 , a mobile robot path planning method for complex terrain with improved bidirectional theta* according to an embodiment of the present invention comprises:

[0039] 1. Model the environmental information obtained from the perception system as a passability grid map for the mobile robot;

[0040] Optionally, the modeling of the accessibility grid map is performed through the following steps: the environmental information obtained from the perception system is modeled as a preliminary generated grid map, wherein the grids in the grid map include two states: occupied and idle; the preliminary generated grid map is secondary processed according to the height of obstacles in the grids in the occupied state to obtain a accessibility grid map for the mobile robot.

[0041] It should be understood that when establishing a grid map, the grids in the grid map mainly have two states: occupied and idle. However, for a mobile robot, due to its geometric configuration, some occupied grids are "passable". The initially generated grid map can be processed secondary according to the obstacle height to obtain a passability grid map for the mobile robot.

[0042] 2. When the accessibility grid map is in a static environment, global path planning is performed using the improved bidirectional theta* algorithm;

[0043] The node transfer cost, terrain characteristics, and trajectory smoothness are comprehensively considered, and adaptive weight parameters are added.

[0044] That is, when the passability grid map is in a static environment, the global path planning is performed based on the node transfer cost, terrain characteristics and trajectory smoothness through the improved bidirectional theta* algorithm, and the adaptive weight parameters are added, where:

[0045] S31. Define p1. p1 represents the transfer cost between two nodes, which is calculated by Euclidean distance. The larger p1 is, the higher the transfer cost between two nodes is:

[0046]

[0047] Among them, (x c ,y c ,z c) represents the coordinate value of the current node in the base coordinates, (x s ,y s ,z s ) represents the coordinate value of the starting point in the base coordinates;

[0048] S32. Define p2 to represent the terrain characteristics, i.e., the environmental characteristics of the terrain. The larger the p2, the greater the unevenness of the road surface, and the more difficult the transfer between two nodes. Convert the target mesh node and all meshes between them into corresponding triangular meshes, and calculate the normal vector of the triangular mesh.

[0049]

[0050]

[0051] Among them, n c and n i Represents the normal vector of the triangular mesh between the current node and the starting point, and k represents the number of meshes;

[0052] S33. Define p3 to represent the trajectory smoothness, i.e. the path smoothness. The larger p3 is, the more turning points the path has:

[0053]

[0054] Among them, P m-1 P m , P m P m+1 They represent path vectors respectively.

[0055] S34, based on the three constraint parameters of node transfer cost, terrain characteristics and trajectory smoothness obtained in step S33, the cost function is defined as follows based on the bidirectional theta* algorithm:

[0056] f1(c)=g1(c)+h1(c) (5)

[0057] f2(c)=g2(c)+h2(c) (6)

[0058] Where c represents the current node, g1(c) represents the actual cost from the path start point to the current node, g2(c) represents the actual cost from the path end point to the current node, h1(c) represents the estimated cost from the current node to the path end point, and h2(c) represents the estimated cost from the current node to the path start point.

[0059] Wherein, g(c) is defined by the three constraint parameters obtained in step S33:

[0060]

[0061] S35. Define the rhs function:

[0062]

[0063] Among them, c′ is the parent node of c, d(c′, c) is the cost value between c′ and c, rhs(c) is the smallest node g(c) value among all child nodes of c. In a static environment, rhs(c) = g(c), that is, only the node with the smallest cost is assigned the rhs(c) value.

[0064] The method further includes: S36, in step S34, h(c) is measured using a weighted Euclidean distance, that is:

[0065]

[0066] w=1+e -(H+obs) (10)

[0067]

[0068] Among them, w is the adaptive weight coefficient of the heuristic function h(c), (x m ,y m ,z m ) represents the position coordinates of the middle (current c) node, (x g ,y g ,z g ) represents the position coordinates of the target node, and obs represents the number of obstacles near the current node.

[0069] When the passability grid map is more complex, the weight coefficient can be adjusted adaptively to improve the search efficiency and find the optimal path. The value range of w is (1, 2]. When the current node is close to the starting point, H is close to 0 and close to 2, and the search speed will be faster; when the current node is close to the target point, H increases and w approaches 1. When there are many obstacles around the node, the obs value becomes larger and w decreases, which will increase the algorithm search space and avoid falling into the local optimum.

[0070] 3. When the passability grid map changes, the passability grid map can be in a dynamic environment. By adding an incremental algorithm to determine the edge cost change, and through quadratic programming, the trajectory update map of the mobile robot in the dynamic environment is obtained:

[0071] When the accessibility grid map changes, the impact on the grid nodes is divided into three states: rhs(c) = g(c), rhs(c) < g(c) and rhs(c) > g(c);

[0072] If the path node is in the first state rhs(c) = g(c), it means that the node is not affected;

[0073] If the path node is in the second state rhs(c)<g(c), it means that the cost of reaching the child node c through the parent node c′ is lower, assign rhs(c) to g(c) and update the path;

[0074] If the path node is in the third state rhs(c)>g(c), it means that the parent node becomes an obstacle. The state of the parent node and all its affected child nodes is updated. Then, the current node where the mobile robot is currently located is used as the new starting point, and searches are conducted from both ends to the middle until the preset termination condition is met. A new shortest path is found to obtain the trajectory update map of the mobile robot in a dynamic environment.

[0075] In summary, compared with the prior art, the beneficial effects of the present invention are:

[0076] (1) Based on theta*, the algorithm of the present invention improves its cost function, comprehensively considers the node transfer cost, terrain characteristics, and trajectory smoothness, effectively solves the problems of large path corners and the existence of local optimal points, and adds adaptive weight parameters to adjust the weight coefficient of the heuristic function according to the distance from the target point, thereby improving the efficiency of path search.

[0077] (2) The present invention adopts incremental bidirectional theta* to achieve real-time update of the path in a dynamic environment, solves the problem of being unable to quickly plan the shortest path under complex terrain, and reduces the burden on the system.

[0078] It should be noted that the present invention may be a method, a system, an apparatus and / or a computer program product. The computer program product may include a computer-readable storage medium carrying computer-readable program instructions for causing a processor to implement various aspects of the present invention.

[0079] Computer readable storage medium can be a tangible device that can hold and store instructions used by an instruction execution device. Computer readable storage medium can be, for example, but not limited to, an electrical storage device, a magnetic storage device, an optical storage device, an electromagnetic storage device, a semiconductor storage device, or any suitable combination thereof. More specific examples (non-exhaustive list) of computer readable storage medium include: a portable computer disk, a hard disk, a random access memory (RAM), a read-only memory (ROM), an erasable programmable read-only memory (EPROM or flash memory), a static random access memory (SRAM), a portable compact disk read-only memory (CD-ROM), a digital versatile disk (DVD), a memory stick, a floppy disk, a mechanical encoding device, for example, a punch card or a convex structure in a groove on which instructions are stored, and any suitable combination thereof. The computer readable storage medium used here is not interpreted as a transient signal itself, such as a radio wave or other freely propagating electromagnetic wave, an electromagnetic wave propagated by a waveguide or other transmission medium (for example, a light pulse by an optical fiber cable), or an electrical signal transmitted by a wire.

[0080] The computer-readable program instructions described herein can be downloaded from a computer-readable storage medium to each computing / processing device, or downloaded to an external computer or external storage device via a network, such as the Internet, a local area network, a wide area network, and / or a wireless network. The network can include copper transmission cables, optical fiber transmissions, wireless transmissions, routers, firewalls, switches, gateway computers, and / or edge servers. The network adapter card or network interface in each computing / processing device receives the computer-readable program instructions from the network and forwards the computer-readable program instructions for storage in the computer-readable storage medium in each computing / processing device.

[0081] The computer program instructions for performing the operation of the present invention may be assembly instructions, instruction set architecture (ISA) instructions, machine instructions, machine-dependent instructions, microcode, firmware instructions, state setting data, or source code or object code written in any combination of one or more programming languages, including object-oriented programming languages, such as Smalltalk, C++, Python, etc., and conventional procedural programming languages, such as "C" language or similar programming languages. Computer-readable program instructions may be executed entirely on a user's computer, partially on a user's computer, as an independent software package, partially on a user's computer, partially on a remote computer, or entirely on a remote computer or server. In the case of a remote computer, the remote computer may be connected to the user's computer via any type of network, including a local area network (LAN) or a wide area network (WAN), or may be connected to an external computer (e.g., using an Internet service provider to connect via the Internet). In some embodiments, an electronic circuit, such as a programmable logic circuit, a field programmable gate array (FPGA), or a programmable logic array (PLA), may be personalized by utilizing the state information of the computer-readable program instructions, and the electronic circuit may execute the computer-readable program instructions, thereby realizing various aspects of the present invention.

[0082] Various aspects of the present invention are described herein with reference to the flow charts and / or block diagrams of the methods, devices (systems) and computer program products according to embodiments of the present invention. It should be understood that each box of the flow chart and / or block diagram and the combination of each box in the flow chart and / or block diagram can be implemented by computer-readable program instructions.

[0083] These computer-readable program instructions can be provided to a processor of a general-purpose computer, a special-purpose computer, or other programmable data processing device, thereby producing a machine, so that when these instructions are executed by the processor of the computer or other programmable data processing device, a device that implements the functions / actions specified in one or more boxes in the flowchart and / or block diagram is generated. These computer-readable program instructions can also be stored in a computer-readable storage medium, and these instructions cause the computer, programmable data processing device, and / or other equipment to work in a specific manner, so that the computer-readable medium storing the instructions includes a manufactured product, which includes instructions for implementing various aspects of the functions / actions specified in one or more boxes in the flowchart and / or block diagram.

[0084] Computer-readable program instructions may also be loaded onto a computer, other programmable data processing apparatus, or other device so that a series of operating steps are performed on the computer, other programmable data processing apparatus, or other device to produce a computer-implemented process, thereby causing the instructions executed on the computer, other programmable data processing apparatus, or other device to implement the functions / actions specified in one or more boxes in the flowchart and / or block diagram.

[0085] The flowcharts and block diagrams in the accompanying drawings show the possible architecture, functions and operations of the systems, methods and computer program products according to multiple embodiments of the present invention. In this regard, each box in the flowchart or block diagram can represent a module, a program segment or a part of an instruction, and the module, a program segment or a part of an instruction contains one or more executable instructions for realizing the specified logical function. In some alternative implementations, the functions marked in the box can also occur in a different order from the order marked in the accompanying drawings. For example, two consecutive boxes can actually be executed substantially in parallel, and they can sometimes be executed in the opposite order, depending on the functions involved. It should also be noted that each box in the block diagram and / or the flowchart, and the combination of the boxes in the block diagram and / or the flowchart can be implemented by a dedicated hardware-based system that performs the specified function or action, or can be implemented by a combination of dedicated hardware and computer instructions. It is well known to those skilled in the art that it is equivalent to implement it by hardware, implement it by software, and implement it by combining software and hardware.

[0086] Embodiments of the present invention have been described above, and the above description is exemplary, not exhaustive, and is not limited to the disclosed embodiments. Many modifications and variations will be apparent to those of ordinary skill in the art without departing from the scope and spirit of the described embodiments. The selection of terms used herein is intended to best explain the principles of the embodiments, practical applications, or technical improvements in the marketplace, or to enable other persons of ordinary skill in the art to understand the embodiments disclosed herein. The scope of the present invention is defined by the appended claims.

Claims

1. A mobile robot path planning method based on improved bidirectional theta* for complex terrain, characterized in that: Including: Modeling the environmental information obtained from the perception system as a traversability grid map for the mobile robot; When the traversability grid map is in a static environment, performing global path planning through an improved bidirectional theta* algorithm; When the traversability grid map changes, i.e., when the traversability grid map is in a dynamic environment, judging the change of the edge cost by adding an incremental algorithm, and obtaining the trajectory of the mobile robot in the dynamic environment through quadratic programming to update the map.

2. The method according to claim 1, characterized in that Modeling the traversability grid map through the following steps: Modeling the environmental information obtained from the perception system as a preliminarily generated grid map, where the grids in the grid map include two states: occupied and free; Performing secondary processing on the preliminarily generated grid map according to the obstacle height in the occupied grids to obtain a traversability grid map for the mobile robot.

3. The method according to claim 1, characterized in that When the traversability grid map is in a static environment, performing global path planning through an improved bidirectional theta* algorithm based on the node transfer cost, terrain features, and trajectory smoothness, and adding an adaptive weight parameter, where: S31. Define p1, p1 represents the transfer cost between two nodes, calculated by the Euclidean distance. The larger p1 is, the higher the transfer cost between the two nodes: Among them, (x c ,y c ,z c ) represents the coordinate value of the current node in the base coordinates, (x s ,y s ,z s ) represents the coordinate value of the starting point in the base coordinates; S32. Define p2 to represent terrain features. The larger the p2, the greater the unevenness of the road surface, and the more difficult the transfer between two nodes. Convert the target mesh node and all meshes between them into corresponding triangular meshes, and calculate the normal vector of the triangular mesh. Among them, n c and n i Represents the normal vector of the triangular mesh between the current node and the starting point, and k represents the number of meshes; S33. Define p3 to represent the trajectory smoothness. The larger p3 is, the more turning points the path has: Among them, P m-1 P m , P m P m+1 They represent path vectors respectively. S34. According to the three constraint parameters of the node transfer cost, terrain features, and trajectory smoothness obtained in step S33, based on the bidirectional theta* algorithm, define the cost function as follows: f1(c) = g1(c) + h1(c) (5) f2(c) = g2(c) + h2(c) (6) Where c represents the current node, g1(c) represents the actual cost from the path start point to the current node, g2(c) represents the actual cost from the path end point to the current node, h1(c) represents the estimated cost from the current node to the path end point, and h2(c) represents the estimated cost from the current node to the path start point; Where g(c) is defined by the three constraint parameters obtained in step S33: S35. Define the rhs function: Where c′ is the parent node of c, d(c′, c) is the cost value between c′ and c, and rhs(c) is the minimum node g(c) value among all the child nodes of c′. In a static environment, rhs(c) = g(c), that is, only the node with the minimum cost is assigned the value of rhs(c).

4. The method according to claim 3, characterized in that The method further includes that h(c) is measured by a weighted Euclidean distance, that is: w=1+e -(H+obs) (10) Among them, w is the adaptive weight coefficient of the heuristic function h(c), (x m ,y m , z m ) represents the position coordinates of the middle node, (x g ,y g , z g ) represents the position coordinates of the target node, and obs represents the number of obstacles near the current node.

5. The method according to claim 1, characterized in that: When the traversability grid map changes, judging the change of the edge cost by adding an incremental algorithm, and obtaining the trajectory of the mobile robot in the dynamic environment through quadratic programming to update the map, specifically including: When the traversability grid map changes, the influence on the grid nodes is divided into three states: rhs(c) = g(c), rhs(c) < g(c), and rhs(c) > g(c); If the path node is in the first state rhs(c) = g(c), it means that the node is not affected; If the path node is in the second state where rhs(c) < g(c), it means that the cost of reaching the child node c through the parent node c' is lower. Assign rhs(c) to g(c) and update the path. If the path node is in the third state where rhs(c) > g(c), it means that the parent node has become an obstacle. Update the states of this parent node and all its affected child nodes. Then, with the current node where the mobile robot is located as the new starting point, search from both ends towards the middle until the preset termination condition is met, find the new shortest path, and obtain the updated map of the trajectory of the mobile robot in the dynamic environment.

Images