Path planning method and device based on complex environment

Abstract

The invention relates to a path planning method and device based on a complex environment, and the method comprises the steps: obtaining the map information of a surrounding complex environment, calculating the Euclidean geometric distance between a starting node and a target node according to an attenuation factor, and obtaining the Euclidean geometric distance between the starting node and the target node according to the attenuation factor, and performing node expansion on the Euclidean geometric distance and the initial step length between the starting node and the target node according to an attenuation factor to obtain a new node, and further controlling the new node between the starting node and the target node through the attenuation factor. The route is prevented from being converged too early and excessively concentrated near the target node and cannot reach the target node; after the new node is determined each time, the initial step length is adjusted to obtain the adjusted step length, and the Euclidean geometric distance between the new node and the target node in the random tree is judged through the adjusted step length, so that generation of redundant nodes in a complex environment is avoided, and the efficiency and quality of path planning are improved.

Description

Technical Field

[0001] The present invention relates to the technical field of path planning, and particularly to a path planning method and device based on a complex environment. Background Art

[0002] Path planning technology is one of the important research directions in the field of robotics. Its core goal is to generate an optimal path from a starting point to a target point for a robot or an autonomous driving system. In practical applications, such algorithms need to comprehensively consider the length, smoothness, and obstacle avoidance ability of the path to meet the navigation requirements in a dynamically complex environment. The classical Rapidly-exploring Random Tree (RRT) algorithm has become an important method in the field of global path planning due to its randomness and asymptotic optimality. However, with the increase in the complexity of application scenarios, the classical RRT algorithm has exposed many defects, such as long paths, redundant nodes, unguided tree expansion, and low planning efficiency in complex environments. These problems significantly restrict its performance in high-precision and high-efficiency tasks.

[0003] In recent years, in view of the deficiencies of the classical RRT algorithm, researchers have proposed various improvement strategies, such as introducing a target bias mechanism to improve the directivity, dynamically adjusting the step size based on the environment to optimize the exploration efficiency, reducing the generation of redundant nodes through node density detection, and using path post-processing techniques such as node direct connection and curve smoothing to optimize the generated path. These methods have alleviated the defects of the classical algorithm to varying degrees, but in multi-obstacle dense areas and high-dynamic environments, the efficiency and quality of path planning still do not meet the actual application requirements, especially in scenarios with extremely high requirements for path optimization such as autonomous driving and industrial robot path navigation.

[0004] Therefore, there is an urgent need to propose a path planning method and device based on a complex environment to solve the technical problem that in multi-obstacle dense areas and high-dynamic environments in the prior art, the efficiency and quality of the path planning method still do not meet the actual application requirements. Summary of the Invention

[0005] In view of this, it is necessary to provide a path planning method and device based on a complex environment to solve the technical problem that in multi-obstacle dense areas and high-dynamic environments in the prior art, the efficiency and quality of the path planning method still do not meet the actual application requirements.

[0006] To solve the above problems, in a first aspect, the present invention provides a path planning method based on a complex environment, including: Obtain the map information of the surrounding complex environment, and initialize the random tree and parameters; the random tree includes a starting node and a target node; the parameters include an initial step size, an initial radius, and a decay factor; Calculate the Euclidean geometric distance between the starting node and the target node according to the attenuation factor, determine the target bias probability, and expand the node according to the target bias probability and the initial step size to obtain a new node; When the node density within the local area circle formed with the new node as the center and the initial radius as the radius is not greater than the density threshold, add the new node to the random tree and adjust the initial step size to obtain an adjusted step size; When the Euclidean geometric distance between the new node and the target node in the random tree is less than the adjusted step size, obtain the planned path.

[0007] In a possible implementation manner, the expanding the node according to the target bias probability and the initial step size to obtain a new node includes: Judge the target bias probability to determine the next node; Determine the distance node closest to the next node in the random tree; Expand from the distance node towards the next node according to the initial step size to generate a new node.

[0008] In a possible implementation manner, the judging the target bias probability to determine the next node includes: Generate a random number and judge whether the target bias probability is greater than the random number; If so, determine the target node as the next node; If not, randomly generate the next node in the configuration space.

[0009] In a possible implementation manner, before adding the new node to the random tree when the node density within the local area circle formed with the new node as the center and the initial radius as the radius is not greater than the density threshold, it further includes: Judge whether the path from the distance node to the new node collides with an obstacle; If so, remove the new node, return to the parent node of the new node, and re-obtain a new node; If not, judge the Euclidean geometric distances between the new node, the target node, and the parent node.

[0010] In a possible implementation manner, the judging the Euclidean geometric distances between the new node, the target node, and the parent node includes: Judge whether the Euclidean geometric distance between the new node and the target node is greater than the Euclidean geometric distance between the parent node and the target node; If so, remove the new node, return the parent node of the new node, and re-obtain a new node. If not, determine whether the node density within the local area circle formed with the new node as the center and the initial radius as the radius is greater than the density threshold.

[0011] In a possible implementation manner, the adjusting the initial step length to obtain an adjusted step length includes: Determine the area of the local area circle of the local area circle and the occupied area occupied by the obstacle in the local area circle; Adjust the initial step length according to the ratio of the local area circle area to the occupied area to obtain an adjusted step length.

[0012] In a possible implementation manner, after adjusting the initial step length to obtain an adjusted step length, it further includes: When the Euclidean geometric distance between the new node and the target node in the random tree is not less than the adjusted step length, calculate and expand the node according to the attenuation factor and the adjusted step length for the Euclidean geometric distance between the new node and the target node to obtain a next new node, and judge the node density of the next new node.

[0013] In a possible implementation manner, after obtaining the planned path, it further includes: Arrange all the nodes on the planned path in sequence to obtain a node set; Connect the start node in the node set to the subsequent other nodes one by one in a straight line, obtain a first node where the last straight line does not intersect with an obstacle, and store the start node and the first node in an optimization set; Connect the first node in the node set to the subsequent other nodes one by one in a straight line, and store a second node where the last straight line does not intersect with an obstacle in the optimization set; When all the nodes in the node set are optimized, connect the nodes in the optimization set in sequence starting from the start node to obtain an optimized path.

[0014] In a possible implementation manner, after obtaining the optimized path, it further includes: Determine the other nodes in the optimization set except the start node and the target node as intermediate nodes; Expand the intermediate nodes along both sides of the optimized path according to a preset fixed step length to obtain two new child nodes for each intermediate node; Use a second-order Bezier curve to smooth the connection line between each intermediate node and the corresponding two new sub-nodes, obtaining the current second-order Bezier curve; When all intermediate nodes on the optimized path are processed, connect all the second-order Bezier curves with the starting node and the target node in sequence to obtain the final smooth path.

[0015] In a second aspect, the present invention also provides a path planning device based on a complex environment, including: An information acquisition module for acquiring map information of the surrounding complex environment and initializing a random tree and parameters; the random tree includes a starting node and a target node; the parameters include an initial step size, an initial radius, and an attenuation factor; A node expansion module for calculating the Euclidean geometric distance between the starting node and the target node according to the attenuation factor, determining a target bias probability, and expanding nodes according to the target bias probability and the initial step size to obtain new nodes; A node judgment module for adding the new node to the random tree and adjusting the initial step size to obtain an adjusted step size when the node density within the local area circle formed with the new node as the center and the initial radius as the radius is not greater than the density threshold; A path generation module for obtaining a planned path when the Euclidean geometric distance between the new node and the target node in the random tree is less than the adjusted step size.

[0016] The beneficial effects of the present invention are: acquiring map information of the surrounding complex environment and initializing a random tree and parameters, so that the Euclidean geometric distance between the starting node and the target node can be calculated according to the attenuation factor, obtaining the Euclidean geometric distance between the starting node and the target node according to the attenuation factor, so that nodes can be expanded according to the Euclidean geometric distance between the starting node and the target node and the initial step size according to the attenuation factor to obtain new nodes, and further, the new nodes between the starting node and the target node can be controlled by the attenuation factor to avoid premature convergence of the route and over-concentration near the target node and being unable to reach the target node; it is also possible to adjust the initial step size after each new node is determined to obtain an adjusted step size, and the Euclidean geometric distance between the new node and the target node in the random tree can be judged by the adjusted step size, thereby avoiding the generation of redundant nodes in the complex environment and improving the efficiency and quality of path planning. Description of the Drawings

[0017] Figure 1 It is a schematic flowchart of an embodiment of the path planning method based on a complex environment provided by the present invention; Figure 2 It is a schematic structural diagram of an embodiment of the third node judgment provided by the present invention; Figure 3 For an embodiment of step S102 in the present invention Figure 1 flow schematic diagram; Figure 4 For an embodiment of step S301 in the present invention Figure 3 flow schematic diagram; Figure 5 Schematic structural diagram of an embodiment of local node density detection provided by the present invention; Figure 6 Schematic structural diagram of an embodiment of the dynamic step size of obstacle density provided by the present invention; Figure 7 Schematic structural diagram of an embodiment of the optimized path of the node direct connection strategy provided by the present invention; Figure 8 Schematic structural diagram of an embodiment of the second-order Bezier curve smoothing path provided by the present invention; Figure 9 Schematic structural diagram of an embodiment of the simple environment algorithm simulation comparison diagram provided by the present invention; Figure 10 Schematic structural diagram of an embodiment of the complex environment algorithm simulation comparison diagram provided by the present invention; Figure 11 Schematic structural diagram of an embodiment of the path planning device based on complex environment provided by the present invention. Detailed implementation manners

[0018] The following will specifically describe the preferred embodiments of the present invention with reference to the accompanying drawings. The accompanying drawings form a part of this application and are used together with the embodiments of the present invention to explain the principle of the present invention, rather than to limit the scope of the present invention.

[0019] RRT (Rapidly-exploring Random Tree) is an algorithm widely used in robot path planning, especially suitable for solving complex path planning problems in high-dimensional spaces. It is a sampling-based probabilistically complete algorithm that constructs an expanding tree in the configuration space through random sampling and finally finds a feasible path connecting the starting point to the target point.

[0020] Local Node Density Detection Strategy: In sampling-based path planning algorithms (such as RRT, PRM, etc.), the local node density detection strategy is an optimization method used to dynamically adjust the sampling and node expansion methods to improve the efficiency and quality of path planning. The core idea of this strategy is to detect the distribution density of nodes in the current tree (or graph), reduce redundant sampling in dense areas, and strengthen exploration in sparse areas, thereby balancing the exploration and optimization processes.

[0021] As Figure 1 shown, a specific embodiment of the present invention discloses a path planning method based on a complex environment, including: S101. Obtain the map information of the surrounding complex environment, and initialize the random tree and parameters; the random tree includes a starting node and a target node; the parameters include an initial step size, an initial radius, and a decay factor.

[0022] The embodiments of the present invention can be applied to a path planning system, and the path planning system can be applied to the field of robots or the field of autonomous driving, and can generate an optimal path from a starting point to a target point. Among them, the path planning can be based on a software system running on a terminal device. The terminal device can be a server, a tablet computer, an augmented reality (AR) / virtual reality (VR) device, a laptop computer, an ultra-mobile personal computer (UMPC), a netbook, a personal digital assistant (PDA), a mobile phone, and other terminal devices. The specific type of the terminal device in the embodiments of the present application is not limited in any way.

[0023] In the classic RRT path planning algorithm, the expansion of the tree is usually random. Although it can ensure global reachability, it fails to effectively guide the search towards the target point. In the existing goal bias mechanism of the improved RRT algorithm, a random number is generated in each iteration. If the number is less than a preset goal bias probability (such as 10%), the target point is selected as the expansion direction; otherwise, a point is randomly sampled from the configuration space as the expansion direction. However, this mechanism will frequently guide the algorithm to expand towards the target point, which may cause the algorithm to converge prematurely to a locally optimal path and ignore a globally better path. The enhanced goal bias strategy proposed in the embodiments of the present invention optimizes this process by introducing a goal bias asymptotic decay mechanism. Introduce a decay factor , so as to control the goal bias. The embodiments of the present invention can obtain the map information of the surrounding complex environment through radar or other means, or can obtain the map information from the stored historical data. The specific acquisition method can be set according to the actual situation, and the embodiments of the present invention do not limit this here. In order to perform more accurate processing, the random tree and parameters can be initialized. The random tree can include a starting node and a target node. The starting node can be the current position of the robot when starting path planning, and the target position can be the position that the robot needs to reach, and this position can be set according to other requirements. The parameters can include an initial step size and a decay factor, and the initial step size and the initial step size can be set according to the actual situation.

[0024] S102. Calculate the Euclidean geometric distance between the starting node and the target node according to the attenuation factor, determine the target bias probability, and expand the node according to the target bias probability and the initial step size to obtain a new node.

[0025] Among them, in path planning (such as algorithms like RRT, Dijkstra, etc.) and robot navigation, the Euclidean Distance is the most commonly used distance metric for calculating the straight-line distance between two points in space. After adding the attenuation factor, the Euclidean geometric distance between the starting node and the target node can be calculated through the attenuation factor to obtain the target bias probability. The specific calculation of the starting node to the target node The Euclidean geometric distance between is calculated as shown in formula (1): (1) After obtaining the Euclidean geometric distance it is possible to calculate according to the attenuation factor the Euclidean geometric distance to obtain the target bias probability, which is calculated as shown in formula (2): (2) In the formula, represents the target bias probability of the Euclidean geometric distance ; e represents the natural constant; represents the attenuation factor, which is recommended to be set to 0.05 - 0.2, and the specific value needs to be adjusted according to the environmental complexity and task requirements.

[0026] The target bias probability can be dynamically adjusted according to the distance of the node (if is the Euclidean geometric distance between the initial node and the target node, then represents the initial node, if is the Euclidean geometric distance between the new node and the target node, then represents the new node) to the target node . When the distance is far, the target bias probability is high, encouraging the tree structure to expand towards the target node. When the distance is close, the target bias probability is low, allowing the tree structure to explore the surrounding space more flexibly. To enable the path to explore the surrounding space flexibly, considering the actual requirements of path planning, the exponential growth form usually better meets the needs because it can provide a more flexible weight change, especially in the area where the distance is far, the target bias probability The rapid growth can encourage the search tree to extend towards the target node, avoiding premature convergence of the route and over-concentration near the target node, making it impossible to reach the target node. Thus, node expansion can be carried out according to the target bias probability and the initial step size to determine the new node after the starting node.

[0027] S103. When the node density within the local area circle formed with the new node as the center and the initial radius as the radius is not greater than the density threshold, add the new node to the random tree and adjust the initial step size to obtain the adjusted step size.

[0028] Among them, after obtaining the new node, it is necessary to judge the new node. The third judgment is as Figure 2 shown. Taking the new node ( Figure 2 the current node in ) as the center and the initial radius E as the radius, a local area circle of the new node is obtained. Then, it can be judged whether the node density within this local area circle is greater than the density threshold. If it is, it means the new node is an invalid node, and the new node needs to be deleted, and no new node will be expanded in this direction. Return to the parent node of the new node , and continue to expand the new node until the local node density of this parent node is lower than the density threshold, that is, return to step S102. If not, it means it is not greater than the density threshold, and then the new node can be added to the random tree. The calculation of the node density is shown in formula (3): (3) In the formula, represents the current node (i.e., the new node ); E represents the initial radius, represents the node density, which is the total number of all nodes in the local area circle ; represents the other nodes already generated in the current random tree. When the node is within the local area circle , the value is recorded as 1, otherwise it is 0, and they are accumulated in sequence to obtain ; represents the preset density threshold; if the node density is equal to the density threshold , it is considered that the area is already overcrowded and no new nodes will be added. Otherwise, the node is retained to continue expanding new nodes. To prevent the path from converging to the local optimal solution prematurely, the initial step size can be dynamically adjusted to obtain an adjusted step size. Thus, when detecting the next new node, the detection can be carried out by adjusting the step size, and then the adjusted step size can be dynamically adjusted again by the next new node until the optimal path solution is obtained.

[0029] S104. When the Euclidean geometric distance between the new node in the random tree and the target node is less than the adjusted step size, the planned path is obtained.

[0030] Among them, after adding the new node to the random tree, an attempt can be made to connect the new node in the random tree to the target node, and it is judged whether the Euclidean geometric distance between the new node and the target node is less than the adjusted step size. If so, the nodes between the initial node and the target node can be connected in sequence, so that the planned path can be obtained.

[0031] Compared with the prior art, the present embodiment provides obtaining the map information of the surrounding complex environment, initializing the random tree and parameters, so that the Euclidean geometric distance between the starting node and the target node can be calculated according to the attenuation factor, and the Euclidean geometric distance between the starting node and the target node according to the attenuation factor can be obtained. Thus, node expansion can be carried out according to the Euclidean geometric distance and the initial step size between the starting node and the target node to obtain new nodes. Furthermore, the new nodes between the starting node and the target node can be controlled by the attenuation factor to avoid the route converging prematurely and being overly concentrated near the target node and unable to reach the target node. Also, after each new node is determined, the initial step size can be adjusted to obtain an adjusted step size, and the Euclidean geometric distance between the new node in the random tree and the target node can be judged by adjusting the step size, thereby avoiding the generation of redundant nodes in the complex environment and improving the efficiency and quality of path planning.

[0032] In some embodiments of the present invention, as Figure 3 shown, step S102 includes: S301. Judge the target bias probability to determine the next node.

[0033] S302. Determine the distance node closest to the next node in the random tree.

[0034] S303. Expand from the distance node in the direction of the next node according to the initial step size to generate a new node.

[0035] Among them, the next node can be obtained according to the judgment result of the target bias probability. The random tree can include many nodes obtained through the above process. Thus, after determining the next node, the node with the closest distance to the next node can be determined in the random tree. Furthermore, with the distance node as the origin, expansion can be carried out in the direction of the next node, and the expansion distance is the initial step length to obtain a new node.

[0036] In some embodiments of the present invention, the parameter further includes the maximum target bias probability; as Figure 4 shown, step S301 includes: S401. Generate a random number and determine whether the target bias probability is greater than the random number; S402. If so, determine the target node as the next node; S403. If not, randomly generate the next node in the configuration space.

[0037] Among them, when expanding a new node, it is necessary to check whether the path collides with an obstacle to ensure the safety of the path. Thus, a random number can be generated to judge the target bias probability by the random number, and determine whether the target bias probability is greater than the random number. If so, the target node can be determined as the next node. If not, the next node can be randomly generated in the configuration space.

[0038] In some embodiments of the present invention, before step S103, it further includes: Judge whether the path from the distance node to the new node collides with an obstacle; If so, remove the new node, return to the parent node of the new node, and re-obtain the new node; If not, judge the Euclidean geometric distance among the new node, the target node, and the parent node.

[0039] In a specific embodiment of the present invention, after determining the new node, it is necessary to judge whether the result of the local node density detection of the new node meets a preset condition. The specific judgment process can be divided into three parts. First, judge whether the path from the distance node to the new node collides with an obstacle. If so, it means that the new node is an invalid node, and the new node needs to be deleted, return to the parent node of the new node, and re-determine the new node according to the parent node, that is, return to step S102. If so, the second part of the judgment is required.

[0040] In some embodiments of the present invention, judging the Euclidean geometric distance among the new node, the target node, and the parent node includes: Judge whether the Euclidean geometric distance between the new node and the target node is greater than the Euclidean geometric distance between the parent node and the target node; If so, remove the new node, return the parent node of the new node, and re-obtain the new node; If not, determine whether the node density within the local area circle formed with the new node as the center and the initial radius as the radius is greater than the density threshold.

[0041] In a specific embodiment of the present invention, after determining that the path from the distance node to the new node does not collide with the obstacle, the second part of the determination can be performed, as Figure 5 shown, L is the step size, is a random point, is the starting node, the small black circles are nodes, the large black circles and rectangles are obstacles, and determine the new node to the target node The Euclidean geometric distance between them is greater than the new node 's parent node to the target node The Euclidean geometric distance between them. If so, it means that the new node is an invalid node, and the new node needs to be deleted, return the parent node of the new node , re-determine the new node according to the parent node , that is, return to step S102. If not, the third part of the determination can be performed. The third part of the determination is the determination process of step S103. Among them, the introduction of this mechanism of the second part of the determination can effectively avoid generating unfavorable paths. Especially in the case of a large search space, it can help the algorithm avoid redundant node expansion, make the path planning more directional, more efficient and concise.

[0042] This strategy in the embodiment of the present invention can avoid meaningless expansion in high-density areas, while maintaining the exploration of low-density areas, reducing the generation of redundant nodes, and accelerating the convergence of the path.

[0043] In the traditional RRT algorithm, the expansion of path planning often depends on a fixed step size, which may lead to the generation of redundant nodes, the reduction of efficiency and / or premature convergence to a local optimal solution in a complex environment, especially in an area with dense obstacles. To address these problems, a dynamic step size strategy based on obstacle density is proposed on the basis of the traditional RRT algorithm. In some embodiments of the present invention, step S103 includes: Determine the local area circle area of the local area circle and the occupied area occupied by the obstacles in the local area circle; Adjust the initial step size according to the ratio of the local area circle area to the occupied area to obtain an adjusted step size.

[0044] Among them, in path planning, with the newly explored previous node as the center and the initial radius to form a local area circle , as Figure 6 shown, when Figure 6 in is a new node, a local area circle can be formed with as the radius, and the local area circle area of the local area circle can be calculated, as well as the occupied area occupied by obstacles in the local area circle ( Figure 6 the overlapping area between the local area circle in and the black obstacle). Then, according to the ratio of the local area circle area ρ to the occupied area Figure 6 , the initial step size is adjusted in real time to obtain an adjusted step size. The new node in is a node determined by the adjusted step size (i.e., the dynamic step size) after adjustment. The range of the adjusted step size can also be limited between the minimum step size and the maximum step size to ensure that the step size is neither too small nor too large, enabling a longer step size and faster path exploration in a simple environment, and a shorter step size and more careful exploration in a complex environment, making the search more directional and thus adapting to different search scenarios and difficulties. The local area circle area of the local area circle , the occupied area ρ occupied by obstacles in the local area circle, the ratio and the adjusted step size (4) are calculated as shown in formula (4): and respectively represent the minimum step size and the maximum step size to ensure that the step size is neither too small nor too large; r is the radius of the local area circle, that is, , which affects the scaling of the step size. When the obstacle density increases, the adjusted step size will decrease. When is small, is large and the exploration is faster; when is large, the adjusted step size is small and the exploration is more refined.

[0045] In some embodiments of the present invention, after step S103, it further includes: When the Euclidean geometric distance between a new node in the random tree and the target node is not less than the adjustment step size, calculate and expand the node based on the attenuation factor and the adjustment step size for the Euclidean geometric distance between the new node and the target node to obtain the next new node, and judge the node density of the next new node.

[0046] In a specific embodiment of the present invention, after adding the new node to the random tree, the new node and the target node can be connected, and it is judged whether the Euclidean geometric distance between the new node and the target node is less than the adjustment step size. If not, the new node can be used as the parent node, and the Euclidean geometric distance between the new node and the target node can be calculated and the node expanded through step S102 and subsequent processes to obtain the next new node, so that the node density of the next new node can be judged, and thus a loop can be performed. If so, it can be judged whether the path between the new node and the target node does not collide with the obstacle. If not, the new node can be used as the parent node, and the Euclidean geometric distance between the new node and the target node can be calculated and expanded through step S102 and subsequent processes to obtain the next new node, so that the node density of the next new node can be judged again, and thus a loop can be performed until there is a node with a Euclidean geometric distance less than the adjustment step size from the target node and no collision with the obstacle, and then the initial node, the target node, and the intermediate determined nodes can be connected in sequence to obtain the planned path.

[0047] Since the sampling points of the classical RRT algorithm have a large randomness, the generated path usually has strong tortuosity and irregularity. Such a path often contains redundant nodes and unnecessary detours, increasing the time and space complexity of path planning. In some embodiments of the present invention, after step S104, it further includes: Arrange all the nodes on the planned path in order to obtain a node set; Connect the starting node in the node set to other subsequent nodes one by one in a straight line, obtain the first node where the last straight line does not intersect with the obstacle, and store the starting node and the first node in the optimization set; Connect the first node in the node set to other subsequent nodes one by one in a straight line, and store the second node where the last straight line does not intersect with the obstacle in the optimization set; When all the nodes in the node set are optimized, connect the nodes in the optimization set in sequence starting from the starting node to obtain the optimized path.

[0048] In a specific embodiment of the present invention, after the node path is generated by path backtracking of the classical RRT algorithm, a node direct connection strategy is adopted to remove some unnecessary intermediate nodes, optimize the length and tortuosity of the path, and avoid the problem of long or overly tortuous paths caused by random sampling. The node direct connection path optimization is as Figure 7 shown.Figure 7 The black line in it represents the planned path obtained through path backtracking, the gray line represents the planned path through the node direct connection strategy, the bold part is the obstacle, and the black dots represent each node. Arrange all the nodes in the planned path obtained by backtracking the RRT algorithm in order and store them in the node set where is the starting node of the path , and is the target node of the path . Set the current node in the node set as , and starting from , sequentially try to establish a straight connection with other subsequent nodes in the path . If the connection path intersects with the obstacle, record the first node that the current node can directly connect to and that does not intersect with the obstacle, denoted as , and add the nodes and to the optimization set . Update the current node to , and then continue to try to establish a straight connection with . If a collision is encountered, update the current node to the last non-collision second node , and add the second node to the optimization set . Repeat the above two steps until the last non-collision node to be connected is , then stop the node direct connection strategy. Finally, use the nodes in the optimization set , starting from the starting node , sequentially connect to the termination point to generate an optimized path

[0049] Although the node direct connection strategy can effectively reduce the tortuosity of the path, there are still corner regions in the path, and these corners do not meet the kinematic requirements of the trolley, resulting in a serious impact on the actual execution of the path. In some embodiments of the present invention, after obtaining the optimized path, it further includes: Determine the other nodes in the optimization set except the starting node and the target node as intermediate nodes; Expand the intermediate nodes along both sides of the optimized path according to a preset fixed step length to obtain two new child nodes for each intermediate node; Use the second-order Bezier curve to smooth the connection line between each intermediate node and the corresponding two new child nodes to obtain the current second-order Bezier curve; When all the intermediate nodes on the optimized path are processed, connect all the second-order Bezier curves with the starting node and the target node in sequence to obtain the final smooth path.

[0050] In a specific embodiment of the present invention, the Bezier curve is a mathematical tool for generating a smooth curve through a set of control points and is widely used in path smoothing and interpolation. Therefore, the embodiment of the present invention introduces the Bezier curve to smooth the corner area after optimizing the node direct connection strategy. For most application scenarios, a Bezier curve with too high an order may cause unnecessary fluctuations in the path, thereby reducing the smoothness and feasibility of the overall path, and the computational complexity increases significantly. To further optimize the path, a second-order Bezier curve is selected to smooth the directly connected path. This not only retains the shortest straight-line distance generated by the node direct connection but also avoids the fluctuations in the smoothed path caused by the high-order Bezier curve, making the corner area smooth, and then combining it with the straight-line part to obtain the final smooth and efficient path. The smoothed path of the Bezier curve is as Figure 8 shown, δ is the outward expansion step size, is the threshold range of the target node. The blue line is the optimized path, and the red line is the final smoothed path. After the node direct connection strategy is executed, the remaining nodes are stored in an array in sequence, denoted as , where and are the starting node and the target node of the path, and is the intermediate node. During the path optimization process, first, a control node (where i = 2, 3,..., n - 1) is selected. In the two directions from to and from to , a step size is given, and two new sub-nodes and are extended in these two directions respectively. Then, the Euclidean geometric distance is used to determine which new sub-node is farther from the target point. The farther one is the starting point , and the closer one is the ending point . This is to ensure that the final path can smoothly connect all the smoothed paths and obtain the final planned path. According to the above steps, , and are taken as a group, and the second-order Bezier curve is used to smooth the broken-line area between to and between to to obtain the current second-order Bezier curve. The above steps are continuously repeated, and the intermediate nodes are sequentially selected as control points to form several second-order Bezier curves. Finally, these several second-order Bezier curves are combined with and They are connected in sequence to obtain the final smooth red path, that is, the final smooth path. Among them, the formula of the second-order Bezier curve is shown in formula (5): (5) In the formula, t is a parameter that controls the interpolation position of the new node between and . By this method, the tortuosity of the path can be effectively reduced, making the path smoother and meeting the kinematic constraints of the trolley.

[0051] In a specific embodiment of the present invention, the proposed improved RRT algorithm is simulated and compared with the effect of traditional RRT path planning. A two-dimensional space with obstacles is selected as the test environment to evaluate the performance of the algorithm in a complex environment. The basic parameters of the algorithm are set. The initial step size of the classical RRT algorithm is 20, the maximum step size for node expansion of the improved RRT algorithm is 30, the minimum step size is 15, the target bias decay factor λ is 0.2, the node radius of node density and obstacle density is 10, the density threshold is 5, the control point expansion step size of the Bezier curve is 10. Both algorithms are in a map with a size of , the starting point is , the target node is , the maximum number of iterations of both algorithms is 3000, and the target node threshold range is 20. Algorithm simulation and verification are carried out for different algorithms in different maps. As shown in Figure 9 and Figure 10 , Figure 9 and Figure 10 , the black color represents the obstacles, the red dots represent the starting point, that is, the root node of the tree, the green dots represent the target points, and the blue dots and blue lines represent the new nodes generated during the tree expansion stage And the branch paths. The red line is the tortuous path obtained by backtracking the improved RRT algorithm, the green line is the path obtained by processing with the node direct connection strategy, and the black line is the final path obtained by the enhanced RRT algorithm for smoothing the Bezier curve. Figure (a) is the simulation diagram of the application of the classical RRT algorithm in two different environments. Figure (b) is the path planning diagram of the fusion of the dynamic step size and the asymptotic decay strategy of the target deviation and the local node density detection in two different environments respectively. It can be seen that compared with the classical RRT algorithm, this algorithm significantly reduces the redundant nodes and minimizes the unnecessary exploration to the greatest extent. Figure (c) adds the node direct connection strategy on the basis of the above strategy, further processes and eliminates the redundant nodes, and greatly optimizes the length and curvature of the path. Figure (d) shows that after generating the path using the direct connection strategy, the second-order Bezier curve is used to smooth the corner area, making the path more in line with the kinematic constraints of the vehicle. It can be seen from the generated final path that the improved RRT algorithm performs well in various environments and the path quality has been significantly improved. To evaluate the performance of the algorithm, the embodiments of the present invention conduct 100 simulation experiments in simple and complex environments and take the average value for comparison. The experimental results are shown in Table 1 and Table 2.

[0052] Table 1. Comparison table of algorithm data in simple environment

[0053] Table 2. Comparison table of algorithm data in complex environment

[0054] From the data comparison in Table 1 and Table 2, it can be concluded that the path planning algorithm proposed in this paper shows significant advantages in different environments, especially in terms of path planning time, path length, and the number of random nodes. Compared with the traditional RRT algorithm and the node filtering RRT algorithm, this method improves the path planning speed by about 62% and reduces the number of generated random nodes by about 50% in simple scenarios. In complex scenarios, the planning time increases by about 51% and the number of random nodes decreases by about 57%. In addition, by implementing the node direct connection strategy and using the second-order Bezier curve to smooth the tortuous path, the final path length is significantly shortened, further improving the overall performance of the algorithm.

[0055] In order to better implement the path planning method based on complex environments in the embodiments of the present invention, correspondingly, the embodiments of the present invention also provide a path planning device based on complex environments, as Figure 11 shown. The path planning device 1100 based on complex environments includes: An information acquisition module 1101 is configured to acquire map information of the surrounding complex environment and initialize a random tree and parameters; the random tree includes a start node and a target node; the parameters include an initial step size, an initial radius, and a decay factor; A node expansion module 1102 is configured to calculate the Euclidean geometric distance between the start node and the target node according to the decay factor, determine a target bias probability, and expand the node according to the target bias probability and the initial step size to obtain a new node; A node judgment module 1103 is configured to add the new node to the random tree and adjust the initial step size to obtain an adjusted step size when the node density within the local area circle formed with the new node as the center and the initial radius as the radius is not greater than a density threshold; A path generation module 1104 is configured to obtain a planned path when the Euclidean geometric distance between the new node and the target node in the random tree is less than the adjusted step size.

[0056] The path planning device 1100 based on a complex environment provided in the above embodiment can implement the technical solutions described in the above embodiment of the path planning method based on a complex environment. The specific implementation principles of the above modules or units can be referred to the corresponding content in the above embodiment of the path planning method based on a complex environment, and will not be elaborated here.

[0057] The above has introduced in detail the path planning method and device based on a complex environment provided by the present invention. Specific examples are used in this article to elaborate on the principles and implementation manners of the present invention. The description of the above embodiments is only used to help understand the method and its core idea of the present invention; at the same time, for those skilled in the art, according to the idea of the present invention, there will be changes in the specific implementation manners and application scopes. In summary, the content of this specification should not be construed as a limitation to the present invention.

Claims

1. A path planning method based on a complex environment, characterized in that Including: Obtain the map information of the surrounding complex environment, and initialize the random tree and parameters; the random tree includes a start node and a target node; the parameters include an initial step size, an initial radius, and a decay factor; Calculate the Euclidean geometric distance between the start node and the target node according to the decay factor, determine the target bias probability, and expand the node according to the target bias probability and the initial step size to obtain a new node; When the node density within the local area circle enclosed by taking the new node as the center and the initial radius as the radius is not greater than the density threshold, add the new node to the random tree, and adjust the initial step size to obtain an adjusted step size; When the Euclidean geometric distance between the new node and the target node in the random tree is less than the adjusted step size, obtain the planned path.

2. The path planning method based on a complex environment according to claim 1, wherein The expanding the node according to the target bias probability and the initial step size to obtain a new node includes: Judge the target bias probability to determine the next node; Determine the distance node closest to the next node in the random tree; Expand from the distance node towards the next node according to the initial step size to generate a new node.

3. The path planning method based on a complex environment according to claim 2, wherein The judging the target bias probability to determine the next node includes: Generate a random number, and judge whether the target bias probability is greater than the random number; If so, determine the target node as the next node; If not, randomly generate the next node in the configuration space.

4. The path planning method based on a complex environment according to claim 2, wherein Before adding the new node to the random tree when the node density within the local area circle enclosed by taking the new node as the center and the initial radius as the radius is not greater than the density threshold, it further includes: Judge whether the path from the distance node to the new node collides with an obstacle; If so, remove the new node, return the parent node of the new node, and re-obtain a new node; If not, judge the Euclidean geometric distances among the new node, the target node, and the parent node.

5. The path planning method based on a complex environment according to claim 4, wherein, The judging the Euclidean geometric distances among the new node, the target node, and the parent node includes: Judge whether the Euclidean geometric distance between the new node and the target node is greater than the Euclidean geometric distance between the parent node and the target node; If so, remove the new node, return the parent node of the new node, and re-obtain a new node; If not, judge whether the node density within the local area circle enclosed by taking the new node as the center and the initial radius as the radius is greater than the density threshold.

6. The path planning method based on a complex environment according to claim 5, wherein The adjusting the initial step size to obtain an adjusted step size includes: Determine the area of the local area circle and the occupied area occupied by the obstacle in the local area circle; Adjust the initial step size according to the ratio of the local area circle area to the occupied area to obtain an adjusted step size.

7. The path planning method based on a complex environment according to claim 1, wherein After adjusting the initial step size to obtain an adjusted step size, it further includes: When the Euclidean geometric distance between the new node and the target node in the random tree is not less than the adjustment step size, calculate and expand the node based on the Euclidean geometric distance between the new node and the target node according to the attenuation factor and the adjustment step size to obtain the next new node, and judge the node density of the next new node.

8. The path planning method based on a complex environment according to claim 1, wherein After obtaining the planned path, it further includes: Arrange all the nodes on the planned path in order to obtain a node set; Connect the start node in the node set to each subsequent other node in a straight line, obtain the first node where the last straight line does not intersect with the obstacle, and store the start node and the first node in the optimization set; Connect the first node in the node set to each subsequent other node in a straight line, and store the second node where the last straight line does not intersect with the obstacle in the optimization set; When all the nodes in the node set are optimized, connect the nodes in the optimization set in sequence starting from the start node to obtain an optimized path.

9. The path planning method based on a complex environment according to claim 8, wherein After obtaining the optimized path, it further includes: Determine the other nodes in the optimization set except the start node and the target node as intermediate nodes; Expand each intermediate node along both sides of the optimized path according to a preset fixed step size to obtain two new child nodes for each intermediate node; Use a second-order Bezier curve to smooth the connection line between each intermediate node and the corresponding two new child nodes to obtain the current second-order Bezier curve; When all the intermediate nodes on the optimized path are processed, connect all the second-order Bezier curves to the start node and the target node in sequence to obtain the final smooth path.

10. A path planning device based on a complex environment, characterized in that, It includes: An information acquisition module for acquiring map information of the surrounding complex environment and initializing a random tree and parameters; the random tree includes a start node and a target node; the parameters include an initial step size, an initial radius, and an attenuation factor; A node expansion module for calculating the Euclidean geometric distance between the start node and the target node according to the attenuation factor, determining the target bias probability, and expanding the node according to the target bias probability and the initial step size to obtain a new node; A node judgment module for adding the new node to the random tree and adjusting the initial step size to obtain an adjustment step size when the node density within the local area circle formed with the new node as the center and the initial radius as the radius is not greater than the density threshold; A path generation module for obtaining a planned path when the Euclidean geometric distance between the new node and the target node in the random tree is less than the adjustment step size.

Images