A method and system for path planning of a flying patrol robot

Abstract

This invention provides a path planning method and system for a flying patrol robot. The method includes: generating an initial feasible path from a starting point to a target point; generating an extended region: based on the initial feasible path, generating an extended region corresponding to the initial feasible path; generating an incrementally optimized path: performing a heuristic search within the extended region to generate an incrementally optimized path; and iterative steps: repeatedly executing the steps of using the incrementally optimized path as the initial feasible path, generating the extended region, and generating the incrementally optimized path until the incrementally optimized path meets the final optimization path judgment criteria, and then using the incrementally optimized path that meets the final optimization path judgment criteria as the final optimized path. This improves the path search efficiency of the RRT* algorithm in the special indoor environment of the flying patrol robot. The heuristic sampling performed within the extended region improves node utilization and accelerates the convergence speed of the algorithm.

Description

Technical Field

[0001] This invention relates to the field of path planning technology, and specifically to a path planning method and system for a flying patrol robot. Background Technology

[0002] With the development of society, economy, and technology, the widespread application of robotics has brought great convenience to human life. Autonomous mobile robots have reduced the cost of human labor and provided humans with richer and more comprehensive information resources. As a research hotspot in the field of mobile robotics in recent years, flying patrol robots are continuously improving their autonomous capabilities. Path planning, as a typical autonomous capability of mobile robots, has always been a research focus in the robotics field. Among these, the autonomous path planning capability of flying patrol robots in complex environments is a core research area. Currently, research on path planning for flying patrol robots typically focuses on open outdoor environments. In contrast, indoor environments are more confined, have more boundary conditions, and present greater challenges in obstacle avoidance, thus posing a greater challenge to the accurate obstacle avoidance and real-time planning of flying patrol robots. In recent years, China has also begun to introduce or develop indoor flying patrol robot flight experimental systems. Against this backdrop, researching path planning algorithms for indoor flying patrol robots, realizing real-time dynamic path decision-making and guidance functions in complex indoor environments, and conducting experimental verification using existing technology platforms will lay a solid foundation for indoor intelligent autonomous navigation technology for flying patrol robots, possessing significant theoretical research value and application reference value.

[0003] Path planning, in particular, refers to the process by which a flying patrol robot, given a starting point and a target point in a specific environment, uses algorithms, control methods, and optimization techniques to find a safe, dynamically feasible, and optimal path to reach the target point. To date, path planning algorithms have been extensively studied, resulting in a wide variety of approaches, such as artificial potential field methods, intelligent biomimetic algorithms, grid-based search algorithms, and sampling-based algorithms.

[0004] Sampling-based algorithms do not require explicit construction of the configuration space. For example, the well-known Fast Exploratory Random Tree (RRT) algorithm draws edges by iteratively adding random nodes and uses collision detection to verify edge validity. Furthermore, it exhibits superior performance in high-dimensional configuration spaces. RRT also provides probabilistic integrity, ensuring that the probability of finding a feasible path approaches 1 as the number of iterations approaches infinity. However, traditional RRT algorithms have limitations, such as low convergence speed and fluctuating path quality. Many researchers have made improvements to RRT algorithms. For instance, Kuffner et al. proposed the RRT-Connect algorithm by introducing greedy heuristic sampling into the RRT algorithm. Compared to the traditional RRT algorithm, RRT-Connect incrementally constructs two fast exploratory random trees that simultaneously originate from the starting and target states. To improve the search efficiency, each of these trees uses a simple greedy heuristic to move towards its corresponding tree. Some scholars have proposed a rerouting method for RRT connections based on the triangle inequality. This algorithm shortens the planning time and seeks optimization through rerouting. Although RRT-Connect shows good search environment capabilities, similar to the RRT algorithm, the path quality of RRT-Connect is unstable and cannot guarantee optimality. To address the issue that RRT does not consider the cost of feasible solutions, Karaman et al. proposed the RRT* algorithm, which considers path cost. The RRT* algorithm has asymptotic optimality and inherits the probabilistic completeness of the RRT algorithm. It can obtain high-quality feasible paths in a shorter time, but its search efficiency is very low. Among them, the RRT* algorithm described in this application is a sampling-based algorithm, which itself has the aforementioned shortcomings. To further improve the performance of the RRT* algorithm, Nasir et al. proposed the RRT* intelligent algorithm. This algorithm introduces intelligent sampling and path optimization techniques to help approach the optimal or suboptimal solution, and has a faster convergence speed than RRT*. In addition, when a better path is found, the vertices of the optimized path become the source of biased sampling. However, as the algorithm iterates, the number of nodes increases infinitely, which poses a significant challenge to computer memory. To use memory more efficiently, Adiyatov et al. proposed the RRT*-FN algorithm, which limits the maximum number of nodes in the tree and deletes useless nodes that do not help optimize the path when the number of nodes exceeds a preset value. This reduces computer memory usage and speeds up the search for neighboring nodes.Inspired by node constraints, Gammell et al. proposed the Informed RRT* algorithm, which limits the range of node sampling by generating a hyperellipsoidal sampling subset and performs direct sampling within the hyperellipsoidal subset to improve the convergence speed and the quality of the final solution. However, this algorithm has some limitations. In fact, in some special environments, such as narrow indoor corridors and mazes, the hyperellipsoidal sampling area may be too large due to high path costs, even encompassing the entire configuration space. In such cases, the performance of Informed RRT* is similar to that of RRT*. Some researchers have proposed the Quick RRT* algorithm based on the triangle inequality by changing the tree growth strategy. This improves the convergence speed by considering the parent nodes of adjacent nodes when selecting the optimal parent node. However, Quick RRT* is ineffective in special environments such as narrow spaces, requiring further improvement. Therefore, it is necessary to improve upon the existing techniques to address their shortcomings. Summary of the Invention

[0005] The main technical problem solved by this invention is to provide a path planning method and system for a flight patrol robot, so as to improve the path search efficiency of the RRT* algorithm in the special indoor environment of the flight patrol robot.

[0006] According to a first aspect, one embodiment provides a path planning method for a flying patrol robot. The path planning method for the flying patrol robot includes:

[0007] Generate an initial feasible path from the starting point to the target point;

[0008] The steps for generating the extended region are as follows: Based on the initial feasible path, an extended region corresponding to the initial feasible path is generated; wherein the initial feasible path is located inside the extended region;

[0009] The steps for generating an incrementally optimized path are as follows: Perform a heuristic search within the extended region to generate an incrementally optimized path;

[0010] The iterative steps are as follows: the steps of using the progressively optimized path as the initial feasible path, generating the extended region, and generating the progressively optimized path are executed sequentially and repeatedly until the progressively optimized path meets the final optimization path judgment criteria. The progressively optimized path that meets the final optimization path judgment criteria is then used as the final optimization path from the starting point to the target point.

[0011] In one embodiment, generating an extended region corresponding to the initial feasible path based on the initial feasible path includes:

[0012] Obtain the sub-extended region corresponding to the sub-path segment in the initial feasible path;

[0013] Based on the sub-path segments in the initial feasible path, the sub-extended regions corresponding to the sub-path segments in the initial feasible path are spliced ​​together to obtain the extended region corresponding to the initial feasible path.

[0014] In one embodiment, obtaining the sub-extended region corresponding to the sub-path segment in the initial feasible path includes:

[0015] Using the adjacent points of the sub-path segments in the initial feasible path as the vertices of the sub-path segments, a direction vector pointing from the vertices of the sub-path segments to one side of the initial feasible path is determined based on the first sub-path segment vector and the second sub-path segment vector corresponding to the vertices of the sub-path segments; wherein, the initial feasible path is formed by connecting several sub-path segments end to end; the first sub-path segment vector and the second sub-path segment vector are the vectors corresponding to the first sub-path segment and the second sub-path segment with the vertices of the sub-path segments as adjacent points, respectively.

[0016] Based on the actual expansion distance, the vertices of the sub-path segments, and the direction vector, the vertices of the first and second sub-expansion regions corresponding to the vertices of the sub-path segments are determined; wherein, each sub-expansion region corresponds to two vertices of the first sub-expansion region and two vertices of the second sub-expansion region, and both the vertices of the first and second sub-expansion regions are used as vertices of the sub-expansion region; the actual expansion distance is the distance extended from the vertices of the sub-path segments along the direction vector;

[0017] The region enclosed by the two first sub-extension region vertices and the two second sub-extension region vertices corresponding to the sub-extension region is defined as the sub-extension region.

[0018] In one embodiment, the vector corresponding to the vertex of the first sub-extended region corresponding to the vertex of the sub-path segment is... The vector corresponding to the vertices of the second sub-extension region The expressions are as follows:

[0019]

[0020]

[0021] Among them, the This represents the vector corresponding to the vertex of the sub-path segment, where i represents the vertex number of the sub-path segment. The direction vector from the vertex of the sub-path segment to one side of the initial feasible path is represented by D, where D represents the actual dilation distance, and θ represents the direction vector from the vertex of the sub-path segment to one side of the initial feasible path. iIt is half the angle between the first sub-path segment vector and the second sub-path segment vector corresponding to the vertex of the sub-path segment.

[0022] In one embodiment, the expression for the actual expansion distance D is:

[0023] D = k * D base ,

[0024] Wherein, k is the dynamic expansion coefficient, and D base The initial expansion distance is given; the value of k ranges from 0.75 to 1.25.

[0025] In one embodiment, the expression for the dynamic expansion coefficient k is:

[0026]

[0027] Where i represents the current iteration number; init This represents the number of iterations completed when generating the initial feasible path; N represents the preset maximum number of iterations.

[0028] In one embodiment, the initial extension distance D base The expression is:

[0029]

[0030] Wherein, ε represents the initial expansion coefficient, and L and W represent the length and width of the environmental plane corresponding to the initial feasible path, respectively.

[0031] According to a second aspect, one embodiment provides an improved RRT* flight patrol robot path planning system. The flight patrol robot path planning system includes:

[0032] The sensor module is configured to acquire environmental data of the flying patrol robot;

[0033] The flight control module is configured to receive environmental data from the flight patrol robot and calculate the position and attitude of the flight patrol robot based on the environmental data.

[0034] A path planning module is configured to generate a final optimized path from the starting point to the target point using the flight patrol robot path planning method as described in any embodiment herein, based on the position and attitude of the flight patrol robot calculated by the flight control module; wherein the flight control module is capable of controlling the flight patrol robot according to the position and attitude of the flight patrol robot and the final optimized path.

[0035] In one embodiment, the environmental data includes multiple or all of the following: distance and angle data of obstacles around the flying patrol robot obtained by ultrasonic ranging radar and laser ranging radar; speed data of the flying patrol robot obtained by optical flow sensor; angular velocity and angular acceleration data of the flying patrol robot obtained by three-axis gyroscope; acceleration data of the flying patrol robot obtained by three-axis accelerometer; heading data of the flying patrol robot obtained by three-axis magnetometer; and flight altitude data of the flying patrol robot obtained by barometer.

[0036] According to a third aspect, one embodiment provides a computer-readable storage medium. The medium includes a program executable by a processor to implement the flight patrol robot path planning method as described in any of the embodiments herein.

[0037] The beneficial effects of this application are:

[0038] This path planning method for a flight patrol robot includes: generating an initial feasible path from the starting point to the target point; generating an extended region: based on the initial feasible path, generating an extended region corresponding to the initial feasible path; wherein the initial feasible path is located inside the extended region; generating an incrementally optimized path: performing a heuristic search within the extended region to generate an incrementally optimized path; and iterative steps: sequentially and repeatedly executing the steps of using the incrementally optimized path as the initial feasible path, generating the extended region, and generating the incrementally optimized path until the incrementally optimized path meets the final optimization path judgment criteria, and using the incrementally optimized path that meets the final optimization path judgment criteria as the final optimized path from the starting point to the target point, thereby improving the path search efficiency of the RRT* algorithm in the special indoor environment of the flight patrol robot. Attached Figure Description

[0039] Figure 1 This is a flowchart illustrating a path planning method for a flight patrol robot according to one embodiment.

[0040] Figure 2 This is a schematic diagram of an initial feasible path from a starting point to a target point according to one embodiment; wherein, X star X represents the starting point of the initially feasible path. goal The initial feasible path is represented by the target point, and the Obstacle is the starting point X. star To target point X goal Obstacles in the environment;

[0041] Figure 3 This is a schematic diagram illustrating the process of generating an extended region corresponding to the initial feasible path in one embodiment.

[0042] Figure 4 This is a schematic diagram illustrating the process of obtaining the sub-extended region corresponding to the sub-path segment in the initial feasible path, according to one embodiment.

[0043] Figure 5 This is a schematic diagram of the extended region corresponding to the initial feasible path in one embodiment.

[0044] Figure 6 This is a schematic diagram of a path planning system for a flight patrol robot according to one embodiment. Detailed Implementation

[0045] The present invention will now be described in further detail with reference to specific embodiments and accompanying drawings. Similar elements in different embodiments are referred to by associated similar element reference numerals. In the following embodiments, many details are described to facilitate a better understanding of this application. However, those skilled in the art will readily recognize that some features may be omitted in different situations, or may be replaced by other elements, materials, or methods. In some cases, certain operations related to this application are not shown or described in the specification. This is to avoid obscuring the core parts of this application with excessive description. For those skilled in the art, detailed description of these related operations is not necessary; they can fully understand the related operations based on the description in the specification and general technical knowledge in the art.

[0046] Furthermore, the features, operations, or characteristics described in the specification can be combined in any suitable manner to form various embodiments. At the same time, the steps or actions in the method description can be rearranged or adjusted in a manner obvious to those skilled in the art. Therefore, the various orders in the specification and drawings are only for the clear description of a particular embodiment and do not imply a necessary order, unless otherwise stated that a particular order must be followed.

[0047] The serial numbers assigned to components in this document, such as "first" and "second," are used only to distinguish the described objects and have no sequential or technical meaning. The terms "connection" and "linkage" used in this application, unless otherwise specified, include both direct and indirect connections (linkages).

[0048] The technical concept of this application is as follows: First, the environment between the starting point and the target point is quickly explored through the RRT-Connect algorithm to find the initial feasible path from the starting point to the target point. Then, the initial feasible path is expanded into an extended region, and the extended region is used as a heuristic sampling region. After that, the search is iteratively performed in the heuristic sampling region, which also changes as the path is continuously optimized, and finally the optimal or suboptimal path connecting the starting point and the target point is obtained.

[0049] The technical solution of this application will be described in detail below with reference to the embodiments.

[0050] Please refer to Figure 1 This application proposes a path planning method for a flying patrol robot, comprising:

[0051] Step S100: Generate an initial feasible path from the starting point to the target point;

[0052] Step S200 for generating the extended region: Based on the initial feasible path, generate an extended region corresponding to the initial feasible path; wherein the initial feasible path is located inside the extended region;

[0053] Step S300 for generating an incrementally optimized path: Perform a heuristic search within the extended region to generate an incrementally optimized path;

[0054] Iterative step S400: Repeatedly execute the steps of using the progressively optimized path as the initial feasible path, generating the extended region, and generating the progressively optimized path in sequence, until the progressively optimized path meets the final optimization path judgment criteria, and use the progressively optimized path that meets the final optimization path judgment criteria as the final optimization path from the starting point to the target point.

[0055] It should be noted that those skilled in the art can set the above starting point and target point according to the actual application scenario.

[0056] In some embodiments, step S100 above can utilize the existing RRT-Connect algorithm to perform greedy heuristic sampling to generate an initial feasible path from the starting point to the target point. For example, please refer to... Figure 2 The first step is to set the starting point X. star Target point X goal and from starting point X star To target point X goal Obstacles in the indoor environment are all inserted as root nodes in the path planning. Specifically, the starting point X is initialized. star Target point X goal And obstacles, the coordinate information of the starting point and the target point mentioned above are added to the random tree (e.g., Figure 2 Tree1 and Tree2 in the data, and generate a random point X in the path planning space of the flying patrol robot. rand The second step is to construct two fast-exploration random trees alternately in two directions (e.g., ...). Figure 2The algorithm uses Tree1 and Tree2 in the random tree to explore the configuration space, making the RRT-Connect algorithm more targeted and thus reducing the planning time required for searching paths. Specifically, it finds the distance from the random point X on Tree1. rand The point with the smallest Euclidean distance is denoted as X. nearest1 The direction of this point points to X. rand It grows a certain step length in the direction of that point (i.e., Figure 2 The new random point X is obtained by step (in step). new1 If the new random point X new1 If the obstacle constraints in the path planning space are not met, discard the point and regenerate a random point; if the new random point X... new1 If the obstacle constraints in the path planning space are satisfied, the branches and endpoints grown by the RRT-Connect algorithm are injected into the tree. The third step is to introduce a greedy heuristic sampling strategy. Specifically, during the expansion process, the branches and endpoints located at the starting point X are... star and target point X goal The two random trees mentioned above (e.g.) Figure 2 Tree1 and Tree2 in the random tree will attract each other. Then, the distance from the random point X on Tree2 is calculated. new1 The point with the smallest Euclidean distance is denoted as X. nearest2 The direction of this point points to X. new1 A random point X is obtained by growing a certain step length in the direction of that point. new2 If the collision detection passes, then X will be... new2 Inject into the random tree Tree2, and simultaneously start the greedy strategy subroutine to continue moving towards the random point X. new1 It grows in the direction of the tree until it encounters an obstacle or two random trees (such as...). Figure 2 The sampling process stops when the distance between Tree1 and Tree2 in the dataset is less than a threshold; if an obstacle is encountered, the program returns to the main program, swaps the two random trees, and repeats the sampling process. If the distance between the two random trees (e.g., Tree1 and Tree2) is less than a threshold, the process stops; if an obstacle is encountered, the program returns to the main program, swaps the two random trees, and repeats the sampling process. Figure 2 If the distance between Tree1 and Tree2 is less than the connection threshold, the connection point information is returned, the path search ends, and the initial feasible path from the starting point to the target point is generated.

[0057] It should be noted that in some embodiments, a rectangle can be generated based on the starting point and the target point, and this rectangle represents the outdoor or indoor environment. Since the specific settings of the aforementioned environment are prior art, they will not be elaborated upon here.

[0058] A significant drawback of existing RRT* algorithms is the need for a large number of samples to obtain the optimal path. From a computational perspective, this poses a significant challenge to computer memory, especially in high-dimensional spaces where more iterations are required to explore larger areas, greatly reducing the algorithm's efficiency. Improvements to RRT* algorithms can increase convergence speed by creating a hyperellipsoidal sampling region; however, in narrow environments with long indoor corridors, the hyperellipsoidal area cannot be effectively reduced. To further improve the convergence speed of the RRT* algorithm, this application introduces a path expression sampling method and proposes a path planning method for a flying patrol robot. Before finding an initial feasible path, this flying patrol robot path planning method operates in the same way as the RRT-Connect algorithm: step S100 finds an initial feasible path; once an initial feasible path from the starting point to the target point is found, this flying patrol robot path planning method expands the initial feasible path based on the actual expansion distance to obtain an expanded region and performs heuristic sampling within the expanded region; finally, the optimal or suboptimal path connecting the starting point and the target point is obtained as the final optimized path in step S300.

[0059] It should be noted that the key to this flight patrol robot path planning method is how to determine the expansion area, that is, how to expand the initial feasible path based on the actual expansion distance to generate the expansion area. The initial feasible path consists of a set of sequentially connected sub-path segments.

[0060] Please refer to Figure 3 Based on the initial feasible path, an extended region corresponding to the initial feasible path is generated, including:

[0061] Step S210: Obtain the sub-extended region corresponding to the sub-path segment in the initial feasible path;

[0062] Step S220: Based on the sub-path segments in the initial feasible path, the sub-extended regions corresponding to the sub-path segments in the initial feasible path are concatenated to obtain the extended regions corresponding to the initial feasible path.

[0063] In some embodiments, the extended region can be viewed as a set of quadrilateral sub-extended regions pieced together based on an initial feasible path.

[0064] Please refer to Figure 4 Obtain the sub-extended regions corresponding to the sub-path segments in the initial feasible path, including:

[0065] Step S211: Using the adjacent points of the sub-path segments in the initial feasible path as the vertices of the sub-path segments, determine the direction vector pointing from the vertices of the sub-path segments to one side of the initial feasible path based on the first sub-path segment vector and the second sub-path segment vector corresponding to the vertices of the sub-path segments; wherein, the initial feasible path is formed by connecting several sub-path segments end to end; the first sub-path segment vector and the second sub-path segment vector are the vectors corresponding to the first sub-path segment and the second sub-path segment with the vertices of the sub-path segments as adjacent points, respectively.

[0066] Step S212: Based on the actual expansion distance, sub-path segment vertices, and direction vectors, determine the first sub-expansion region vertices and the second sub-expansion region vertices corresponding to the sub-path segment vertices; wherein, each sub-expansion region corresponds to two first sub-expansion region vertices and two second sub-expansion region vertices, and both the first and second sub-expansion region vertices are used as vertices of the sub-expansion region; the actual expansion distance is the distance extended from the sub-path segment vertices along the direction vector;

[0067] Step S213: The region bounded by the two first sub-extension region vertices and the two second sub-extension region vertices corresponding to the sub-extension region is taken as the sub-extension region. The sub-extension region is quadrilateral.

[0068] For example, please refer to Figure 5 The initial feasible path v1v2v3v4 can be formed by three sub-path segments connected end-to-end in sequence (i.e. Figure 3 The path consists of sub-path segments v1v2, v2v3, and v3v4. The adjacent points of sub-path segments v1v2, v2v3, and v3v4 are v2 and v3, respectively. Each sub-path segment has two adjacent points, meaning a sub-path segment has two adjacent points. The sub-expansion region v′1v′2v″2v″1 corresponds to sub-path segment v1v2; the sub-expansion region v′2v′3v″3v″2 corresponds to sub-path segment v2v3; and the sub-expansion region v′3v′4v″4v″3 corresponds to sub-expansion segment v3v4. Therefore, when expanding the initial feasible path (as in step S212) to generate sub-expansion regions, it is necessary to find the position of each vertex of the sub-expansion region (such as the first and second sub-expansion region vertices in step S212).

[0069] It should be noted that in step S211, please refer to... Figure 4 The direction of the direction vector from the vertex of the sub-path segment to one side of the initially feasible path is the first sub-path segment vector. Second sub-path segment The direction pointed to by the angle bisector of the angle formed by the vectors.

[0070] The vector corresponding to the first sub-extended region vertex of the sub-path segment vertex The vector corresponding to the vertices of the second sub-extension region The expressions are as follows:

[0071]

[0072]

[0073] in, This represents the vector corresponding to the vertices of a sub-path segment. Let θ represent the direction vector from the vertex of the sub-path segment to one side of the initially feasible path, D represent the actual dilation distance, and θ represent the direction vector. i It is half the angle between the first sub-path segment vector and the second sub-path segment vector corresponding to the vertex of the sub-path segment (see θ2 in the figure).

[0074] In some embodiments, please refer to Figure 5 In step S212 above, the first sub-expansion region vertex v′1 and the second sub-expansion region vertex v″1 corresponding to the sub-path segment vertex v1 within the sub-expansion region (the first sub-expansion region located in the lower left corner of the graph) can be based on the first sub-expansion region vertex v′1 corresponding to the sub-path segment vertex. i Second sub-extended region vertex v″ i The expression is used to determine this. Similarly, the first sub-expansion region vertex v′2 and the second sub-expansion region vertex v″2, which are located in the first sub-expansion region corresponding to the sub-path segment vertex v2 in the lower left corner of the graph, are also determined based on the above expression for the first sub-expansion region vertex v′ corresponding to the sub-path segment vertex. i Second sub-extended region vertex v″ i The expression is determined.

[0075] In some embodiments, please refer to Figure 5 In step S213 above, the vertices v′1, v″1, v′2, and v″2 of the first sub-extended region can be connected. The region enclosed by these vertices is the first sub-extended region in the lower left corner of the graph. Similarly, other sub-extended regions can be generated based on the above steps. Then, based on the initial feasible path v1v2v3v4, the sub-path segments (i.e.,...) in the initial feasible path v1v2v3v4 are connected to the sub-path segments in the initial feasible path v1v2v3v4. Figure 3The sub-extended regions corresponding to the sub-path segments v1v2, v2v3, and v3v4 in the graph are sequentially concatenated to form the extended region corresponding to the initial feasible path. For example, when the initial feasible path v1v2v3v4 is composed of sub-path segments v1v2, v2v3, and v3v4, the sub-extended regions corresponding to the sub-path segments in the initial feasible path are respectively the first sub-extended region v′1v′2v″2v″1 in the lower left corner of the graph, the second sub-extended region v′2v′3v″3v″2 in the middle of the graph, and the third sub-extended region v′3v′4v″4v″3 in the upper right corner of the graph.

[0076] It should be noted that after obtaining the initial feasible path using step S100, the initial feasible path needs to be expanded based on the actual expansion distance to obtain the expanded area. In this process, the selection of the actual expansion distance significantly affects the performance of the path planning method for this flight patrol robot. To better adapt to different environments (i.e., the spatial environment from the starting point to the target point), this application proposes an adaptive path expansion distance selection method to obtain an appropriate actual expansion distance.

[0077] The expression for the actual expansion distance D is:

[0078] D = k * D base ,

[0079] Where k is the dynamic expansion coefficient, D base The initial expansion distance is denoted by k; the value of k ranges from 0.75 to 1.25.

[0080] The expression for the dynamic expansion coefficient k is:

[0081]

[0082] Where i represents the current iteration number; i init This represents the number of iterations completed when generating the initial feasible path; N represents the preset maximum number of iterations.

[0083] It should be noted that the path planning method of this flight patrol robot is in a loop iteration state until the final optimized path is found, and the current iteration number is the cumulative number of the current iteration from the 1st to the Nth iteration.

[0084] It should be noted that the dynamic expansion coefficient k is set to further improve the convergence speed of the path planning method for this flight patrol robot. In other words, the addition of the dynamic expansion coefficient causes the sampling area of ​​the path planning method to decrease as the method iterates, increasing the search breadth of the early iterations and improving the search efficiency of the later iterations.

[0085] It should be noted that the initial expansion distance D mentioned above... base First, it is determined based on the size of the environment (i.e., the spatial environment between the starting point and the target point).

[0086] Wherein, the initial expansion distance D base The expression is:

[0087]

[0088] Where ε represents the initial expansion coefficient, and L and W represent the length and width of the environment plane corresponding to the initial feasible path, respectively. max{L,W} represents the maximum value of L and W.

[0089] It should be noted that those skilled in the art can modify ε according to the size and complexity of the environment to adjust the breadth and efficiency of the path planning method for this flying patrol robot in the search. For example, in a narrow indoor environment with only one path, a larger ε value should be used to obtain a smaller expansion area, thereby increasing the probability of obtaining effective sampling points within the expansion area and the convergence speed of the algorithm. In some embodiments, ε = 8.

[0090] In some embodiments, in step S200 above, all sub-extended regions corresponding to the initial feasible path can be generated at once, and all sub-extended regions constitute an extended region.

[0091] In some embodiments, step S300 above can employ the RRT-Connect algorithm to perform an iterative search within the extended region, thereby obtaining the final optimized path. For example, in the first step, the RRT-Connect algorithm is used to find the distance to the new random point X on the random tree Tree1, which covers a larger area. rand Find the point with the smallest Euclidean distance, and denote this point as the new X. nearest1 The direction of this point points to the new X. rand A new random point X is obtained by growing a certain step size in the direction of that point. new1 In the first step above, "larger range" refers to the expanded region generated in step S200. The second step involves calculating the distance from the new random point X on the random tree Tree2 located within the larger range. new1 Find the point with the smallest Euclidean distance, and denote this point as X. nearest2 The direction of this point points to the aforementioned new X. new1 A new random point X is obtained by growing a certain step size in the direction of that point. new2Similarly, the "larger range" in the second step above refers to the expanded area generated in step S200. The first and second steps are repeated continuously. In the third step, the path planning points obtained from repeatedly performing the first and second steps are connected. This yields a path planning trajectory for the flying patrol robot from the starting point to the target point. This path planning trajectory is the progressively optimized path. This progressively optimized path can be found by continuously tracing the parent nodes of the aforementioned random points. Finally, the optimal or second-best collision-free path is output as the final optimized path.

[0092] It should be noted that step S300 above can also employ algorithms such as RRT, RRTGoalBiaS, or RRTGoalZoom to perform iterative search within the extended region to obtain the final optimized path. In some embodiments, step S200 above can also generate sub-extended regions corresponding to each sub-path segment in the initial feasible path. Then, step S300 is executed. Afterward, step S100 is repeated to generate a new sub-path segment in the initial feasible path, and steps S200 and S300 are repeated, and so on, until the final optimized path is obtained through iterative search within the extended region composed of multiple sub-extended regions. Finally, the optimal or second-best collision-free path is output as the final optimized path.

[0093] It should be noted that the random trees Tree1 and Tree2 in step S300 are already located inside the extended region generated in step S200.

[0094] It should be noted that in step S300, when the extended region has been determined, performing iterative search within the extended region (e.g., using the RRT-Connect algorithm) to obtain the final optimized path is a prior art in this field, so the specific process of obtaining the final optimized path will not be described in detail.

[0095] In the above iterative step S400, the steps S200 of using the progressively optimized path as the initial feasible path and generating the extended region, and the step S300 of generating the progressively optimized path are repeatedly executed until the progressively optimized path meets the final optimization path judgment criteria. The progressively optimized path that meets the final optimization path judgment criteria is then used as the final optimization path from the starting point to the target point.

[0096] It should be noted that in a map / environment plane filled with obstacles, there must exist an optimal path from the starting point to the target point, i.e., the final optimized path. The result of repeatedly executing steps S200 (using the progressively optimized path as the initial feasible path, generating the extended region), and step S300 (generating the progressively optimized path) is a progressively optimized path that gradually approaches the final optimized path. This continues until the final iteration (i.e., when the progressively optimized path in this iteration meets the criteria for the final optimized path), at which point the final result of the path search can be obtained. This final result is not necessarily the theoretically optimal path, but it is certainly the path with the smallest error from the optimal path under the current algorithm iteration conditions.

[0097] It should be noted that the final optimization path judgment criterion is used to determine whether the asymptotically optimized path obtained in each iteration is the final optimization path. Since existing technologies already have relevant final optimization path judgment criteria (for example, on a grid map, the length of the asymptotically optimized path and the error value compared to the theoretically optimal path can be calculated based on the grid size), step S400 can directly use the existing final optimization path judgment criteria. Therefore, the specific method for using this final optimization path judgment criterion will not be elaborated upon here.

[0098] It should be noted that the path planning method of this flight patrol robot has been verified by simulation. In other words, the simulation results can prove whether the incrementally optimized path obtained by iterating through steps S100, S200 and S300 is the optimal or suboptimal path.

[0099] As can be seen, in some embodiments, compared with the prior art, the path planning method for a flying patrol robot proposed in this application has the following technical effects:

[0100] 1) To address the shortcomings of existing RRT* and its improved algorithms, the present invention proposes a path planning method for a flight patrol robot. First, it utilizes the powerful environmental search capability of RRT-Connect to generate an initial feasible path. Then, it expands the initial feasible path based on the initial feasible path to obtain an expanded area, and performs iterative heuristic sampling within the expanded area.

[0101] 2) The path planning method of this flight patrol robot is combined with the RRT-Connect algorithm, which improves the path search efficiency of the RRT* algorithm in the special indoor environment of the flight patrol robot;

[0102] 3) The heuristic sampling performed by the path planning method of this flight patrol robot in the extended area improves node utilization and speeds up the convergence of the algorithm.

[0103] The above is a description of the path planning method for flight patrol robots. Some embodiments of this application also disclose an improved RRT* path planning system for flight patrol robots. Please refer to... Figure 6 The path planning system for this flying patrol robot includes:

[0104] Sensor module 100 is configured to acquire environmental data of the flying patrol robot;

[0105] The flight control module 200 is configured to receive environmental data from the flight patrol robot and calculate the robot's position and attitude based on that data.

[0106] The path planning module 300 is configured to generate the final optimized path from the starting point to the target point using the path planning method of the flight patrol robot calculated by the flight control module 200 based on the position and attitude of the flight patrol robot according to any one of claims 1 to 7.

[0107] The flight control module 200 can control the flight patrol robot based on its position, attitude, and final optimized path.

[0108] In some embodiments, the sensor module 100 mainly includes ultrasonic ranging radar, laser ranging radar, optical flow sensor, three-axis gyroscope, three-axis accelerometer, three-axis magnetometer, and barometer. The sensor module 100 is primarily used to collect environmental data from the flying patrol robot, and then sends the collected environmental data to the flight control module 200 through information fusion. This allows the flight control module 200 to determine the specific position and attitude of the flying patrol robot indoors based on the environmental data obtained by the sensor module 100, providing prior knowledge for the subsequent path planning algorithm calculation of the indoor flying patrol robot in the path planning module 300. Here, prior knowledge refers to the specific position and attitude of the flying patrol robot indoors.

[0109] In some embodiments, the flight control module 200 receives environmental data collected by the sensor module 100, and calculates the specific position and attitude of the current flight patrol robot indoors or outdoors through data information fusion, and then sends the specific position and attitude of the flight patrol robot to the path planning module 300.

[0110] In some embodiments, the path planning module 300 plans a specific path (i.e., the aforementioned final optimized path) for the flight patrol robot based on the current position and attitude of the flight patrol robot transmitted by the flight control module 200, using the flight patrol robot path planning method of any embodiment herein, and sends the planned final optimized path to the flight control module 200. Then, the flight control module 200 can calculate the required PWM waveform signal for each rotor of the flight patrol robot using the EKF algorithm based on the obtained position and attitude of the flight patrol robot and the aforementioned final optimized path transmitted by the path planning module 300, and transmit the required PWM waveform for each rotor to the electronic speed controller (ESC). The ESC then converts the PWM waveform signal output by the flight control module 200 into the rotational speed of the motor corresponding to each rotor, thereby controlling the flight patrol robot.

[0111] It should be noted that the specific process by which the flight control module 200 controls the flight patrol robot based on its position, attitude, and final optimized path is existing technology and will not be elaborated here.

[0112] In some embodiments, the environmental data includes multiple or all of the following: distance and angle data of obstacles around the flying patrol robot obtained by ultrasonic ranging radar and laser ranging radar; speed data of the flying patrol robot obtained by optical flow sensor; angular velocity and angular acceleration data of the flying patrol robot obtained by three-axis gyroscope; acceleration data of the flying patrol robot obtained by three-axis accelerometer; heading data of the flying patrol robot obtained by three-axis magnetometer; and flight altitude data of the flying patrol robot obtained by barometer.

[0113] It should be noted that those skilled in the art can also edit the environmental data according to actual needs, such as adding new sub-environmental data categories. Since the acquisition of environmental data is prior art, the process of acquiring environmental data will not be elaborated upon here.

[0114] It should be noted that the specific operation process and corresponding technical effects of the flight patrol robot path planning system can be found in the aforementioned discussion of flight patrol robot path planning methods, and will not be repeated here.

[0115] The above is a description of the path planning method for flight patrol robots. Some embodiments of this application also disclose a computer-readable storage medium, characterized in that it includes a program that can be executed by a processor to implement the method of any of the embodiments herein.

[0116] This document describes various exemplary embodiments with reference to them. However, those skilled in the art will recognize that changes and modifications can be made to the exemplary embodiments without departing from the scope of this document. For example, various operational steps and components for performing operational steps can be implemented in different ways depending on the specific application or considering any number of cost functions associated with the operation of the system (e.g., one or more steps can be deleted, modified, or combined with other steps).

[0117] In the above embodiments, implementation can be achieved, in whole or in part, by software, hardware, firmware, or any combination thereof. Furthermore, as those skilled in the art will understand, the principles herein can be reflected in a computer program product on a computer-readable storage medium pre-loaded with computer-readable program code. Any tangible, non-transitory computer-readable storage medium may be used, including magnetic storage devices (hard disks, floppy disks, etc.), optical storage devices (CD-ROMs, DVDs, Blu-ray discs, etc.), flash memory, and / or the like. These computer program instructions can be loaded onto a general-purpose computer, special-purpose computer, or other programmable data processing apparatus to form a machine, such that instructions executing on the computer or other programmable data processing apparatus can generate means for implementing a specified function. These computer program instructions can also be stored in a computer-readable storage medium that can instruct the computer or other programmable data processing apparatus to operate in a particular manner, such that instructions stored in the computer-readable storage medium can form an article of manufacture including means for implementing the specified function. The computer program instructions can also be loaded onto a computer or other programmable data processing apparatus to perform a series of operational steps on the computer or other programmable apparatus to produce a computer-implemented process, such that instructions executing on the computer or other programmable apparatus can provide steps for implementing the specified function.

[0118] While the principles herein have been illustrated in various embodiments, numerous modifications to the structure, arrangement, proportions, elements, materials, and components, particularly suited to specific environmental and operational requirements, may be used without departing from the principles and scope of this disclosure. These modifications and other alterations or alterations will be included within the scope of this document.

[0119] The foregoing specific descriptions have been described with reference to various embodiments. However, those skilled in the art will recognize that various modifications and changes can be made without departing from the scope of this disclosure. Therefore, considerations for this disclosure are to be illustrative rather than restrictive, and all such modifications are to be included within its scope. Similarly, advantages, other advantages, and solutions to problems with respect to various embodiments have been described above. However, benefits, advantages, solutions to problems, and any elements that produce these, or make them more explicit, should not be construed as critical, essential, or necessary. The term “comprising” and any other variations thereof as used herein are non-exclusive inclusion, meaning that a process, method, article, or apparatus that includes a list of elements includes not only those elements but also other elements not expressly listed or not part of the process, method, system, article, or apparatus. Furthermore, the term “coupled” and any other variations thereof as used herein refer to physical connections, electrical connections, magnetic connections, optical connections, communication connections, functional connections, and / or any other connections.

[0120] Those skilled in the art will recognize that many changes can be made to the details of the above embodiments without departing from the basic principles of the invention. Therefore, the scope of the invention should be determined only by the claims.

Claims

1. A path planning method for a flying patrol robot, characterized in that, include: Generate an initial feasible path from the starting point to the target point; The steps for generating the extended region are as follows: Using the adjacent points of sub-path segments in the initial feasible path as sub-path segment vertices, a direction vector pointing from the sub-path segment vertex to one side of the initial feasible path is determined based on the first and second sub-path segment vectors corresponding to the sub-path segment vertices; wherein, the initial feasible path is formed by connecting several sub-path segments end-to-end; the first and second sub-path segment vectors are the vectors corresponding to the first and second sub-path segments with the sub-path segment vertices as adjacent points; based on the actual expansion distance, the sub-path segment vertices, and the direction vectors, the first and second sub-extended region vertices corresponding to the sub-path segment vertices are determined; wherein, each The sub-expansion region corresponds to two first sub-expansion region vertices and two second sub-expansion region vertices, where both first and second sub-expansion region vertices serve as vertices of the sub-expansion region. The actual expansion distance is the distance extended from the sub-path segment vertex along the direction vector. The region enclosed by the two first and two second sub-expansion region vertices corresponding to the sub-expansion region is defined as the sub-expansion region. Based on the sub-path segments in the initial feasible path, the sub-expansion regions corresponding to the sub-path segments in the initial feasible path are concatenated to obtain the expansion region corresponding to the initial feasible path. The initial feasible path is located inside the expansion region. The steps for generating an incrementally optimized path are as follows: Perform a heuristic search within the extended region to generate an incrementally optimized path; The iterative steps are as follows: the steps of using the progressively optimized path as the initial feasible path, generating the extended region, and generating the progressively optimized path are executed sequentially and repeatedly until the progressively optimized path meets the final optimization path judgment criteria. The progressively optimized path that meets the final optimization path judgment criteria is then used as the final optimization path from the starting point to the target point.

2. The path planning method for a flight patrol robot as described in claim 1, characterized in that, The vector corresponding to the first sub-expanded region vertex corresponding to the vertex of the sub-path segment. The vector corresponding to the vertices of the second sub-extension region The expressions are as follows: ， ， Among them, the This represents the vector corresponding to the vertex of the sub-path segment, where i represents the vertex number of the sub-path segment. The direction vector representing the direction from the vertex of the sub-path segment to one side of the initial feasible path is described. Indicates the actual expansion distance, the It is half the angle between the first sub-path segment vector and the second sub-path segment vector corresponding to the vertex of the sub-path segment.

3. The path planning method for a flight patrol robot as described in claim 2, characterized in that, The actual expansion distance The expression is: ， Among them, the For dynamic expansion coefficients, the The initial expansion distance; The value ranges from 0.75 to 1.

25.

4. The path planning method for a flight patrol robot as described in claim 3, characterized in that, The dynamic expansion coefficient The expression is: ， Among them, the Indicates the current iteration number; the This indicates the number of iterations completed when generating the initial feasible path; This indicates the preset maximum number of iterations.

5. The path planning method for a flight patrol robot as described in claim 3, characterized in that, The initial expansion distance The expression is: ， Among them, the Represents the initial expansion coefficient, the and These represent the length and width of the environmental plane corresponding to the initial feasible path, respectively.

6. A path planning system for a flying patrol robot, characterized in that, include: The sensor module is configured to acquire environmental data of the flying patrol robot; The flight control module is configured to receive environmental data from the flight patrol robot and calculate the position and attitude of the flight patrol robot based on the environmental data. The path planning module is configured to receive the position and attitude of the flight patrol robot calculated by the flight control module, and generate a final optimized path from the starting point to the target point using the flight patrol robot path planning method as described in any one of claims 1 to 5. The flight control module can control the flight patrol robot based on its position and attitude, as well as the final optimized path.

7. The flight patrol robot path planning system as described in claim 6, characterized in that, The environmental data includes multiple or all of the following: distance and angle data of obstacles around the flying patrol robot obtained by ultrasonic ranging radar and laser ranging radar; speed data of the flying patrol robot obtained by optical flow sensor; angular velocity and angular acceleration data of the flying patrol robot obtained by three-axis gyroscope; acceleration data of the flying patrol robot obtained by three-axis accelerometer; heading data of the flying patrol robot obtained by three-axis magnetometer; and flight altitude data of the flying patrol robot obtained by barometer.

8. A computer-readable storage medium, characterized in that, Includes a program that can be executed by a processor to implement the method as described in any one of claims 1 to 5.

Images