A path planning and evaluation method based on active loop perception strategy

Abstract

This invention discloses a path planning and evaluation method based on an active loop perception strategy. The method designs a cost function for path planning and determines the optimal waypoint for the next step based on the cost function. Based on the waypoints, a key waypoint selection strategy is designed, and based on the key waypoints, an active loop perception strategy is designed. Based on the planned path, a dynamic obstacle avoidance method based on fuzzy reasoning is designed to optimize the planned path. Based on the optimized path, ORB-SLAM3 is used for positioning, outputting the UAV positioning result, and evaluating the positioning result. This method solves the problem of increased errors in real-time positioning and map construction during autonomous exploration of micro-UAVs due to the lack of loops. It optimizes the planned trajectory, improves the positioning accuracy and stability of the micro-UAV during autonomous exploration, and is suitable for engineering applications.

Description

Technical Field

[0001] The present invention belongs to the technical field of autonomous navigation path planning, and relates to a path planning and evaluation method based on an active loopback perception strategy. Background Art

[0002] Micro air vehicles (MAVs) are intelligent vehicles that integrate MEMS, microelectronics, computing, and intelligent control technologies. They are characterized by low cost, compact size, ease of operation, and high flexibility. They are capable of performing various missions, such as reconnaissance, surveillance, and search and rescue, in complex flight environments, including low-altitude, indoor, marine, and urban environments. In recent years, localization and mapping (SLAM) has become a hot topic in the field of mobile robotics, attracting significant attention. Currently, many classic SLAM frameworks and excellent algorithms are used for autonomous navigation and localization. SLAM primarily focuses on localization and mapping, rather than trajectory planning. When SLAM and trajectory planning are considered together, the problem becomes even more complex and challenging; this is known as active SLAM. In active SLAM, a robot, such as an MAV or unmanned ground vehicle (UGV), needs to solve a decision-making problem to plan a collision-free trajectory to improve position estimation accuracy, as well as perform other tasks such as exploring unknown environments and planning coverage. This problem involves both SLAM and path planning. Localization accuracy must be considered during the planning process. Therefore, it is considered one of the most challenging problems in mobile robotics. Therefore, in the process of autonomous detection, planning a trajectory that is conducive to positioning is of high necessity and engineering application value. Summary of the Invention

[0003] Purpose of the invention: The present invention provides a path planning method that is conducive to improving positioning performance. It can realize path planning based on active loop perception when performing tasks, and evaluate the flight positioning results, so as to meet the needs of high-precision real-time positioning during path planning and flight.

[0004] Technical solution: The present invention provides a path planning and evaluation method based on an active loop perception strategy, comprising the following steps:

[0005] (1) Using the RRT algorithm to randomly generate candidate waypoints, designing the cost function for path planning, determining the optimal waypoint and the current waypoint based on the cost function, and generating the path;

[0006] (2) Based on the determined waypoints, design a key waypoint selection strategy, and based on the key waypoints, design an active loop perception strategy;

[0007] (3) Based on the paths planned in steps (1) and (2), a dynamic obstacle avoidance method based on fuzzy reasoning is designed to optimize the planned paths;

[0008] (4) Based on the optimized path in step (3), ORB-SLAM3 is used to perform positioning solution, output the UAV positioning result, and evaluate the positioning result.

[0009] Furthermore, the implementation process of step (1) is as follows:

[0010] S represents the state of the drone, ε represents the path, and the path from time k-1 to time k is represented by Indicates that the exploration area W is represented by the occupancy grid map, and the map grid visual information that can be explored under the drone state S is represented by Vis(S). In the exploration space, the random tree is incrementally generated according to the current state S using the RRT algorithm. The random tree contains N T There are n nodes, and the nodes are connected by paths ε. The information gain of the node is represented by Gain(n), which represents the sum of the information about the unexplored part that can be obtained within the field of view of the node position. For the node n at time k, k , the node information gain is expressed as follows:

[0011]

[0012] Where λ is the adjustment factor, ω1 and ω2 are weight factors, Loop(S k ) indicates that k The distance between existing waypoints in the neighborhood, S k Represents the state of the UAV at time k; the node with the highest information gain is the best waypoint.

[0013] Furthermore, the implementation process of step (2) is as follows:

[0014] The selection strategy of key waypoints is as follows: First, the starting point of the planned path is taken as the first key waypoint. Second, for the best waypoint determined, the waypoint whose heading changes by more than 45 degrees compared with the previous key waypoint or has 5 new non-key waypoints with the previous key waypoint will be determined as a key waypoint.

[0015] Active loop perception strategy: When the distance between the UAV and the surrounding key waypoints is less than the pre-set threshold d th , the system will actively loop back and plan the drone to fly to the key waypoint; the current position of the drone and the position of the key waypoint are expressed by the following formula:

[0016]

[0017] Where d represents the distance, pcurrent Indicates the current location, Indicates the position of the i-th key landmark point; in order to take into account the efficiency of path planning; in a planning time window T w After that or when the number of iterations of a certain plan exceeds the set maximum value and a suitable optimal waypoint is still not found, active loop detection is performed.

[0018] Furthermore, the implementation process of step (3) is as follows:

[0019] Fuzzy inference rules are used for dynamic obstacle avoidance. When an obstacle is detected during flight, the drone's heading is determined through fuzzy inference to avoid the obstacle. The fuzzy inference system has three inputs and one output, which are defined as follows:

[0020] Input 1: The distance between the drone and the obstacle in the flight direction;

[0021] Input 2: The distance between the drone and the obstacle 45 degrees to the left of the flight direction;

[0022] Input 3: The distance between the drone and the obstacle 45 degrees to the right of the flight direction;

[0023] Output: The angle at which the drone turns;

[0024] The fuzzification process of input and output variables is as follows:

[0025] In the input variables, if the distance to the obstacle is very far, it is described as "large" in the fuzzy language; if the distance to the obstacle is very close, it is described as "small"; if the distance to the obstacle is neither far nor close, it is described as "medium"; in the output variables, in order to facilitate fuzzy language description, the size of the turn is defined according to the left turn angle; similar to the description of the input variables, if a left turn is required and the left turn angle is small, it is described as "positive small", if the left turn angle is large, it is described as "positive large"; if a straight line is required, it is described as "zero"; if a right turn is required and the right turn angle is small, it is described as "negative small", if the left turn angle is large, it is described as "negative large"; triangular and trapezoidal membership functions are used as membership functions for quantitatively describing language variables;

[0026] After fuzzifying the input and output variables, reasoning needs to be performed based on fuzzy rules. The fuzzy rules formulated are as follows:

[0027] 1) If the distance between the drone and the obstacle in the direction of flight is "small", the distance between the drone and the obstacle 45 degrees to the left of the flight direction is "small", and the distance between the drone and the obstacle 45 degrees to the right of the flight direction is "small", then the drone's turning angle is "positive";

[0028] 2) If the distance between the drone and the obstacle in the direction of flight is "small", the distance between the drone and the obstacle 45 degrees to the left of the flight direction is "small", and the distance between the drone and the obstacle 45 degrees to the right of the flight direction is "medium", then the steering angle of the drone is "negative large";

[0029] 3) If the distance between the drone and the obstacle in the flight direction is "small", the distance between the drone and the obstacle 45 degrees to the left of the flight direction is "small", and the distance between the drone and the obstacle 45 degrees to the right of the flight direction is "large", then the steering angle of the drone is "negative large";

[0030] 4) If the distance between the drone and the obstacle in the direction of flight is "small", the distance between the drone and the obstacle 45 degrees to the left of the direction of flight is "medium", and the distance between the drone and the obstacle 45 degrees to the right of the direction of flight is "small", then the turning angle of the drone is "positive";

[0031] 5) If the distance between the drone and the obstacle in the direction of flight is "small", the distance between the drone and the obstacle 45 degrees to the left of the direction of flight is "medium", and the distance between the drone and the obstacle 45 degrees to the right of the direction of flight is "medium", then the turning angle of the drone is "positive";

[0032] 6) If the distance between the drone and the obstacle in the direction of flight is "small", the distance between the drone and the obstacle 45 degrees to the left of the direction of flight is "medium", and the distance between the drone and the obstacle 45 degrees to the right of the direction of flight is "large", then the steering angle of the drone is "negative large";

[0033] 7) If the distance between the drone and the obstacle in the direction of flight is "small", the distance between the drone and the obstacle 45 degrees to the left of the flight direction is "large", and the distance between the drone and the obstacle 45 degrees to the right of the flight direction is "small", then the turning angle of the drone is "positive and large";

[0034] 8) If the distance between the drone and the obstacle in the direction of flight is "small", the distance between the drone and the obstacle 45 degrees to the left of the direction of flight is "large", and the distance between the drone and the obstacle 45 degrees to the right of the direction of flight is "medium", then the turning angle of the drone is "positive large";

[0035] 9) If the distance between the drone and the obstacle in the direction of flight is "small", the distance between the drone and the obstacle 45 degrees to the left of the direction of flight is "large", and the distance between the drone and the obstacle 45 degrees to the right of the direction of flight is "large", then the turning angle of the drone is "positive large";

[0036] 10) If the distance between the drone and the obstacle in the direction of flight is "medium", the distance between the drone and the obstacle 45 degrees to the left of the direction of flight is "small", and the distance between the drone and the obstacle 45 degrees to the right of the direction of flight is "small", then the turning angle of the drone is "positive";

[0037] 11) If the distance between the drone and the obstacle in the direction of flight is "medium", the distance between the drone and the obstacle 45 degrees to the left of the direction of flight is "small", and the distance between the drone and the obstacle 45 degrees to the right of the direction of flight is "medium", then the steering angle of the drone is "negative large";

[0038] 12) If the distance between the drone and the obstacle in the direction of flight is "medium", the distance between the drone and the obstacle 45 degrees to the left of the direction of flight is "small", and the distance between the drone and the obstacle 45 degrees to the right of the direction of flight is "large", then the steering angle of the drone is "negative small";

[0039] 13) If the distance between the drone and the obstacle in the direction of flight is "medium", the distance between the drone and the obstacle 45 degrees to the left of the direction of flight is "medium", and the distance between the drone and the obstacle 45 degrees to the right of the direction of flight is "small", then the turning angle of the drone is "positive";

[0040] 14) If the distance between the drone and any obstacle in the direction of flight is "medium", the distance between the drone and any obstacle 45 degrees to the left of the direction of flight is "medium", and the distance between the drone and any obstacle 45 degrees to the right of the direction of flight is "medium", then the turning angle is "positive";

[0041] 15) If the distance between the drone and the obstacle in the direction of flight is "medium", the distance between the drone and the obstacle 45 degrees to the left of the direction of flight is "medium", and the distance between the drone and the obstacle 45 degrees to the right of the direction of flight is "large", then the steering angle of the drone is "negative small";

[0042] 16) If the distance between the drone and the obstacle in the direction of flight is "medium", the distance between the drone and the obstacle 45 degrees to the left of the direction of flight is "large", and the distance between the drone and the obstacle 45 degrees to the right of the direction of flight is "small", then the turning angle of the drone is "positive small";

[0043] 17) If the distance between the drone and the obstacle in the direction of flight is "medium", the distance between the drone and the obstacle 45 degrees to the left of the direction of flight is "large", and the distance between the drone and the obstacle 45 degrees to the right of the direction of flight is "medium", then the steering angle of the drone is "positive small";

[0044] 18) If the distance between the drone and the obstacle in the direction of flight is "medium", the distance between the drone and the obstacle 45 degrees to the left of the direction of flight is "large", and the distance between the drone and the obstacle 45 degrees to the right of the direction of flight is "large", then the turning angle of the drone is "positive small";

[0045] 19) If the distance between the drone and the obstacle in the direction of flight is "large", the distance between the drone and the obstacle 45 degrees to the left of the direction of flight is "small", and the distance between the drone and the obstacle 45 degrees to the right of the direction of flight is "small", then the angle of the drone's turn is "positive large";

[0046] 20) If the distance between the drone and the obstacle in the direction of flight is "large", the distance between the drone and the obstacle 45 degrees to the left of the direction of flight is "small", and the distance between the drone and the obstacle 45 degrees to the right of the direction of flight is "medium", then the steering angle of the drone is "negative large";

[0047] 21) If the distance between the drone and the obstacle in the flight direction is "large", the distance between the drone and the obstacle 45 degrees to the left of the flight direction is "small", and the distance between the drone and the obstacle 45 degrees to the right of the flight direction is "large", then the steering angle of the drone is "negative small";

[0048] 22) If the distance between the drone and the obstacle in the direction of flight is "large", the distance between the drone and the obstacle 45 degrees to the left of the direction of flight is "medium", and the distance between the drone and the obstacle 45 degrees to the right of the direction of flight is "small", then the angle of the drone's turn is "positive large";

[0049] 23) If the distance between the drone and the obstacle in the direction of flight is "large", the distance between the drone and the obstacle 45 degrees to the left of the direction of flight is "medium", and the distance between the drone and the obstacle 45 degrees to the right of the direction of flight is "medium", then the angle of the drone's turn is "positive large";

[0050] 24) If the distance between the drone and the obstacle in the direction of flight is "large", the distance between the drone and the obstacle 45 degrees to the left of the direction of flight is "medium", and the distance between the drone and the obstacle 45 degrees to the right of the direction of flight is "large", then the steering angle of the drone is "negative small";

[0051] 25) If the distance between the drone and the obstacle in the flight direction is "large", the distance between the drone and the obstacle 45 degrees to the left of the flight direction is "large", and the distance between the drone and the obstacle 45 degrees to the right of the flight direction is "small", then the steering angle of the drone is "positive small";

[0052] 26) If the distance between the drone and the obstacle in the direction of flight is "large", the distance between the drone and the obstacle 45 degrees to the left of the flight direction is "large", and the distance between the drone and the obstacle 45 degrees to the right of the flight direction is "medium", then the steering angle of the drone is "positive small";

[0053] 27) If the distance between the drone and the obstacle in the flight direction is "large", the distance between the drone and the obstacle 45 degrees to the left of the flight direction is "large", and the distance between the drone and the obstacle 45 degrees to the right of the flight direction is "large", then the steering angle of the drone is "zero".

[0054] Furthermore, the implementation process of step (4) is as follows:

[0055] Based on the path generated in step 4 and the images captured by the binocular camera, the ORB-SLAM3 algorithm is used for real-time positioning and map construction. The positioning results are compared with the generated trajectory truth value to evaluate the impact of the path planning method based on the active loop perception strategy on positioning accuracy.

[0056] Beneficial Effects: Compared with existing technologies, this invention improves the loop probability of micro-UAVs during autonomous exploration through path planning based on active loop perception, thereby improving the accuracy of simultaneous localization and mapping (SLAM). Furthermore, obstacle avoidance decisions made through fuzzy reasoning can effectively avoid dynamic obstacles in unknown environments, ensuring the safe flight of micro-UAVs during mission execution. BRIEF DESCRIPTION OF THE DRAWINGS

[0057] Figure 1 is a flow chart of the present invention;

[0058] Figure 2 This is the active loop detection flow chart;

[0059] Figure 3 It is a dynamic obstacle avoidance flow chart based on fuzzy reasoning;

[0060] Figure 4 is the input 1 membership function curve;

[0061] Figure 5 is the membership function curve of input 2 and input 3;

[0062] Figure 6 is the output membership function curve. DETAILED DESCRIPTION

[0063] The present invention will be further described in detail below with reference to the accompanying drawings.

[0064] This paper proposes a path planning and evaluation method based on active loop perception strategy, such as Figure 1As shown, the specific steps include:

[0065] Step 1: Use the RRT algorithm to randomly generate candidate waypoints, design the cost function for path planning, determine the optimal waypoint and the current waypoint based on the cost function, and generate the path.

[0066] S represents the state of the drone, ε represents the path, and the path from time k-1 to time k is represented by The exploration area W is represented by the occupancy grid map, and the visual information of the map grid that can be explored under the drone state S (obstacle, no obstacle, unexplored) is represented by Vis(S). In the exploration space, the RRT algorithm is used to incrementally generate random trees according to the current state S. The random tree contains N T There are n nodes, and the nodes are connected by paths ε. The information gain of a node is represented by Gain(n), which represents the sum of the information about the unexplored part that can be obtained within the field of view of the node position. For node n at time k k , the node information gain is expressed as follows:

[0067]

[0068] Where λ is the adjustment factor, ω1 and ω2 are weight factors, Loop(S k ) indicates that k The distance between existing waypoints in the neighborhood, S k represents the state of the UAV at time k. The node with the highest information gain will be determined as the optimal waypoint.

[0069] Step 2: Based on the determined waypoints, design a key waypoint selection strategy, and based on the key waypoints, design an active loop perception strategy, such as Figure 2 shown.

[0070] The strategy for selecting key waypoints is as follows: First, the starting point of the planned path is used as the first key waypoint. Second, for the best waypoint, if the heading of the current waypoint changes by more than 45 degrees compared with the previous key waypoint or there are 5 new non-key waypoints with the previous key waypoint, it will be determined as a key waypoint.

[0071] Active loop perception strategy. When the distance between the drone and the surrounding key waypoints is less than a certain threshold d th , the system will actively loop back and plan the drone to fly to the key waypoint. The current position of the drone and the position of the key waypoint are expressed by the following formula:

[0072]

[0073] Among them, d represents the distance, p current Indicates the current location, In order to take into account the efficiency of path planning, active loop detection will be performed when one of the following conditions is met: ① In a planning time window T w After the time has passed; ② When the number of iterations of a certain planning exceeds the set maximum value and still no suitable optimal waypoint is found. The above strategy selects key waypoints and sets the conditions for active loopback, thereby avoiding loopback detection at each waypoint and improving planning efficiency.

[0074] Step 3: Based on the paths planned in steps 1 and 2, a dynamic obstacle avoidance method based on fuzzy reasoning is designed to optimize the planned path.

[0075] like Figure 3 As shown in the figure, in a dynamic obstacle avoidance method based on fuzzy reasoning, fuzzy reasoning is combined with traditional RRT to effectively generate a path to the next optimal viewpoint waypoint while dynamically avoiding obstacles. Specifically, in the first step, the RRT algorithm is executed for global path planning. Then, in the second step, key waypoints are selected for active loop perception. This step uses fuzzy reasoning rules to optimize the dynamic obstacle avoidance trajectory based on the trajectory generated in the first two steps. When an obstacle is detected during flight, fuzzy reasoning is used to determine the drone's heading to circumvent the obstacle and follow the planned path. Finally, an optimized path consisting of several optimal viewpoint waypoints is generated. The generated optimized path is used for autonomous exploration by the micro-UAV.

[0076] The designed fuzzy inference system has three inputs and one output, which are defined as follows:

[0077] Input 1: The distance between the drone and the obstacle in the flight direction;

[0078] Input 2: The distance between the drone and the obstacle 45 degrees to the left of the flight direction;

[0079] Input 3: The distance between the drone and the obstacle 45 degrees to the right of the flight direction;

[0080] Output: The angle the drone turns to.

[0081] The fuzzification process of input and output variables is as follows:

[0082] In the input variables, if the distance to the obstacle is very far, it is described as "large" in fuzzy language. If the distance to the obstacle is very close, it is described as "small". If the distance to the obstacle is neither far nor close, it is described as "medium". In the output variables, in order to facilitate fuzzy language description, the size of the turn is defined according to the angle of the left turn. Similar to the description of the input variables, if you need to turn left and the left turn angle is small, use "positive small" to describe it. If the left turn angle is large, use "positive large" to describe it. If you need to go straight, use "zero" to describe it. If you need to turn right and the right turn angle is small, use "negative small" to describe it. If the left turn angle is large, use "negative large" to describe it. Triangular and trapezoidal membership functions are used as membership functions for quantitatively describing language variables, such as Figures 4 to 6 As shown in the following figure. In the input membership function curve, the x-axis represents distance in meters, and the y-axis represents membership, with a value between 0 and 1. In the output membership function curve, the x-axis represents angle in degrees, and the y-axis represents membership, with a value range the same as the input membership function.

[0083] After fuzzifying the input and output variables, reasoning needs to be performed based on fuzzy rules. The fuzzy rules are shown in Table 1:

[0084] (1) If the distance between the drone and the obstacle in the direction of flight is "small", the distance between the drone and the obstacle 45 degrees to the left of the flight direction is "small", and the distance between the drone and the obstacle 45 degrees to the right of the flight direction is "small", then the turning angle of the drone is "positive";

[0085] (2) If the distance between the drone and the obstacle in the flight direction is "small", the distance between the drone and the obstacle 45 degrees to the left of the flight direction is "small", and the distance between the drone and the obstacle 45 degrees to the right of the flight direction is "medium", then the angle of the drone's turn is "negative large";

[0086] (3) If the distance between the drone and the obstacle in the flight direction is "small", the distance between the drone and the obstacle 45 degrees to the left of the flight direction is "small", and the distance between the drone and the obstacle 45 degrees to the right of the flight direction is "large", then the steering angle of the drone is "negative large";

[0087] (4) If the distance between the drone and the obstacle in the direction of flight is "small", the distance between the drone and the obstacle 45 degrees to the left of the direction of flight is "medium", and the distance between the drone and the obstacle 45 degrees to the right of the direction of flight is "small", then the angle of the drone's turn is "positive";

[0088] (5) If the distance between the drone and the obstacle in the direction of flight is "small", the distance between the drone and the obstacle 45 degrees to the left of the direction of flight is "medium", and the distance between the drone and the obstacle 45 degrees to the right of the direction of flight is "medium", then the angle of the drone's turn is "positive";

[0089] (6) If the distance between the drone and the obstacle in the direction of flight is "small", the distance between the drone and the obstacle 45 degrees to the left of the direction of flight is "medium", and the distance between the drone and the obstacle 45 degrees to the right of the direction of flight is "large", then the angle of the drone's turn is "negative large";

[0090] (7) If the distance between the drone and the obstacle in the direction of flight is "small", the distance between the drone and the obstacle 45 degrees to the left of the direction of flight is "large", and the distance between the drone and the obstacle 45 degrees to the right of the direction of flight is "small", then the angle of the drone's turn is "positive and large";

[0091] (8) If the distance between the drone and the obstacle in the direction of flight is "small", the distance between the drone and the obstacle 45 degrees to the left of the direction of flight is "large", and the distance between the drone and the obstacle 45 degrees to the right of the direction of flight is "medium", then the angle of the drone's turn is "positive large";

[0092] (9) If the distance between the drone and the obstacle in the direction of flight is "small", the distance between the drone and the obstacle 45 degrees to the left of the direction of flight is "large", and the distance between the drone and the obstacle 45 degrees to the right of the direction of flight is "large", then the angle of the drone's turn is "positive large";

[0093] (10) If the distance between the drone and the obstacle in the direction of flight is "medium", the distance between the drone and the obstacle 45 degrees to the left of the direction of flight is "small", and the distance between the drone and the obstacle 45 degrees to the right of the direction of flight is "small", then the angle of the drone's turn is "positive";

[0094] (11) If the distance between the drone and the obstacle in the direction of flight is "medium", the distance between the drone and the obstacle 45 degrees to the left of the direction of flight is "small", and the distance between the drone and the obstacle 45 degrees to the right of the direction of flight is "medium", then the angle of the drone's turn is "negative large";

[0095] (12) If the distance between the drone and the obstacle in the direction of flight is "medium", the distance between the drone and the obstacle 45 degrees to the left of the direction of flight is "small", and the distance between the drone and the obstacle 45 degrees to the right of the direction of flight is "large", then the angle of the drone's turn is "negative small";

[0096] (13) If the distance between the drone and the obstacle in the direction of flight is "medium", the distance between the drone and the obstacle 45 degrees to the left of the direction of flight is "medium", and the distance between the drone and the obstacle 45 degrees to the right of the direction of flight is "small", then the angle of the drone's turn is "positive";

[0097] (14) If the distance between the drone and the obstacle in the direction of flight is "medium", the distance between the drone and the obstacle 45 degrees to the left of the direction of flight is "medium", and the distance between the drone and the obstacle 45 degrees to the right of the direction of flight is "medium", then the angle of the drone's turn is "positive";

[0098] (15) If the distance between the drone and the obstacle in the direction of flight is "medium", the distance between the drone and the obstacle 45 degrees to the left of the direction of flight is "medium", and the distance between the drone and the obstacle 45 degrees to the right of the direction of flight is "large", then the angle of the drone's turn is "negative small";

[0099] (16) If the distance between the drone and the obstacle in the direction of flight is "medium", the distance between the drone and the obstacle 45 degrees to the left of the direction of flight is "large", and the distance between the drone and the obstacle 45 degrees to the right of the direction of flight is "small", then the angle of the drone's turn is "positive small";

[0100] (17) If the distance between the drone and the obstacle in the direction of flight is "medium", the distance between the drone and the obstacle 45 degrees to the left of the direction of flight is "large", and the distance between the drone and the obstacle 45 degrees to the right of the direction of flight is "medium", then the angle of the drone's turn is "positive small";

[0101] (18) If the distance between the drone and the obstacle in the direction of flight is "medium", the distance between the drone and the obstacle 45 degrees to the left of the direction of flight is "large", and the distance between the drone and the obstacle 45 degrees to the right of the direction of flight is "large", then the angle of the drone's turn is "positive small";

[0102] (19) If the distance between the drone and the obstacle in the direction of flight is "large", the distance between the drone and the obstacle 45 degrees to the left of the direction of flight is "small", and the distance between the drone and the obstacle 45 degrees to the right of the direction of flight is "small", then the angle of the drone's turn is "positively large";

[0103] (20) If the distance between the drone and the obstacle in the direction of flight is "large", the distance between the drone and the obstacle 45 degrees to the left of the direction of flight is "small", and the distance between the drone and the obstacle 45 degrees to the right of the direction of flight is "medium", then the angle of the drone's turn is "negative large";

[0104] (21) If the distance between the drone and the obstacle in the flight direction is "large", the distance between the drone and the obstacle 45 degrees to the left of the flight direction is "small", and the distance between the drone and the obstacle 45 degrees to the right of the flight direction is "large", then the angle of the drone's steering is "negative small";

[0105] (22) If the distance between the drone and the obstacle in the direction of flight is "large", the distance between the drone and the obstacle 45 degrees to the left of the direction of flight is "medium", and the distance between the drone and the obstacle 45 degrees to the right of the direction of flight is "small", then the angle of the drone's turn is "positive large";

[0106] (23) If the distance between the drone and the obstacle in the direction of flight is “large”, the distance between the drone and the obstacle 45 degrees to the left of the direction of flight is “medium”, and the distance between the drone and the obstacle 45 degrees to the right of the direction of flight is “medium”, then the angle of the drone’s turn is “positive large”;

[0107] (24) If the distance between the drone and the obstacle in the direction of flight is "large", the distance between the drone and the obstacle 45 degrees to the left of the direction of flight is "medium", and the distance between the drone and the obstacle 45 degrees to the right of the direction of flight is "large", then the angle of the drone's turn is "negative small";

[0108] (25) If the distance between the drone and the obstacle in the flight direction is "large", the distance between the drone and the obstacle 45 degrees to the left of the flight direction is "large", and the distance between the drone and the obstacle 45 degrees to the right of the flight direction is "small", then the angle at which the drone turns is "positively small";

[0109] (26) If the distance between the drone and the obstacle in the flight direction is "large", the distance between the drone and the obstacle 45 degrees to the left of the flight direction is "large", and the distance between the drone and the obstacle 45 degrees to the right of the flight direction is "medium", then the angle at which the drone turns is "positive small";

[0110] (27) If the distance between the drone and the obstacle in the flight direction is "large", the distance between the drone and the obstacle 45 degrees to the left of the flight direction is "large", and the distance between the drone and the obstacle 45 degrees to the right of the flight direction is "large", then the angle of the drone's turn is "zero".

[0111] Table 1 Fuzzy rules for dynamic obstacle avoidance

[0112]

[0113]

[0114] Step 4: Based on the path optimized in step 3, use ORB-SLAM3 to perform positioning solution, output the UAV positioning result, and evaluate the positioning result.

[0115] Based on the generated path and images captured by the binocular camera, the ORB-SLAM3 algorithm is used for real-time positioning and map construction. The positioning results are compared with the true value of the generated trajectory to evaluate the impact of the path planning method based on the active loop perception strategy on positioning accuracy.

[0116] The above are merely preferred embodiments of the present invention. The scope of protection of the present invention is not limited to the above embodiments. All technical solutions based on the principles of the present invention are within the scope of protection of the present invention. It should be noted that for those skilled in the art, various improvements and modifications that do not depart from the principles of the present invention should be considered within the scope of protection of the present invention.

Claims

1. A path planning and evaluation method based on active loop perception strategy, characterized in that: The following steps are involved: (1) Using the RRT algorithm to randomly generate candidate waypoints, designing the cost function for path planning, determining the optimal waypoint and the current waypoint based on the cost function, and generating the path; (2) Based on the determined waypoints, design a key waypoint selection strategy, and based on the key waypoints, design an active loop perception strategy; (3) Based on the paths planned in steps (1) and (2), a dynamic obstacle avoidance method based on fuzzy reasoning is designed to optimize the planned paths; (4) Based on the optimized path in step (3), ORB-SLAM3 is used to perform positioning calculation, output the UAV positioning result, and evaluate the positioning result; The implementation process of step (2) is as follows: The selection strategy of key waypoints is as follows: First, the starting point of the planned path is taken as the first key waypoint. Second, for the best waypoint determined, the waypoint whose heading changes by more than 45 degrees compared with the previous key waypoint or has 5 new non-key waypoints with the previous key waypoint will be determined as a key waypoint. Active loop perception strategy: When the distance between the UAV and the surrounding key waypoints is less than the pre-set threshold d th , the system will actively loop back and plan the drone to fly to the key waypoint; the current position of the drone and the position of the key waypoint are expressed by the following formula: Where d represents the distance, p current Indicates the current location, Indicates the position of the i-th key landmark; in order to take into account the efficiency of path planning; In a planning time window T w After that or when the number of iterations of a certain plan exceeds the set maximum value and a suitable optimal waypoint is still not found, active loop detection is performed.

2. The path planning and evaluation method based on active loop perception strategy according to claim 1 is characterized in that: The implementation process of step (1) is as follows: S represents the state of the drone, ε represents the path, and the path from time k-1 to time k is represented by Indicates that the exploration area W is represented by the occupancy grid map, and the map grid visual information that can be explored under the drone state S is represented by Vis(S). In the exploration space, the random tree is incrementally generated according to the current state S using the RRT algorithm. The random tree contains N T There are n nodes, and the nodes are connected by paths ε. The information gain of the node is represented by Gain(n), which represents the sum of the information about the unexplored part that can be obtained within the field of view of the node position. For the node n at time k, k , the node information gain is expressed as follows: Where λ is the adjustment factor, ω1 and ω2 are weight factors, Loop(S k ) indicates that k The distance between existing waypoints in the neighborhood, S k Represents the state of the UAV at time k; the node with the highest information gain is the best waypoint.

3. The path planning and evaluation method based on active loop perception strategy according to claim 1, characterized in that: The implementation process of step (3) is as follows: Fuzzy inference rules are used for dynamic obstacle avoidance. When an obstacle is detected during flight, the drone's heading is determined through fuzzy inference to avoid the obstacle. The fuzzy inference system has three inputs and one output, which are defined as follows: Input 1: The distance between the drone and the obstacle in the flight direction; Input 2: The distance between the drone and the obstacle 45 degrees to the left of the flight direction; Input 3: The distance between the drone and the obstacle 45 degrees to the right of the flight direction; Output: The angle at which the drone turns; The fuzzification process of input and output variables is as follows: In the input variables, if the distance to the obstacle is very far, it is described as "large" in fuzzy language; if the distance to the obstacle is very close, it is described as "small"; if the distance to the obstacle is neither far nor close, it is described as "medium"; in the output variables, in order to facilitate fuzzy language description, the magnitude of the turn is defined according to the left turn angle; similar to the description of the input variables, if a left turn is required and the left turn angle is small, it is described as "positive small", if the left turn angle is large, it is described as "positive large"; if a straight line is required, it is described as "zero"; if a right turn is required and the right turn angle is small, it is described as "negative small", if the left turn angle is large, it is described as "negative large"; triangular and trapezoidal membership functions are used as membership functions for quantitatively describing linguistic variables; After fuzzifying the input and output variables, reasoning needs to be performed based on fuzzy rules. The fuzzy rules formulated are as follows: 1) If the distance between the drone and the obstacle in the flight direction is "small", the distance between the drone and the obstacle 45 degrees to the left of the flight direction is "small", and the distance between the drone and the obstacle 45 degrees to the right of the flight direction is "small", then the drone's turning angle is "positive"; 2) If the distance between the drone and the obstacle in the direction of flight is "small", the distance between the drone and the obstacle 45 degrees to the left of the flight direction is "small", and the distance between the drone and the obstacle 45 degrees to the right of the flight direction is "medium", then the steering angle of the drone is "negative large"; 3) If the distance between the drone and the obstacle in the flight direction is "small", the distance between the drone and the obstacle 45 degrees to the left of the flight direction is "small", and the distance between the drone and the obstacle 45 degrees to the right of the flight direction is "large", then the steering angle of the drone is "negatively large"; 4) If the distance between the drone and the obstacle in the direction of flight is "small", the distance between the drone and the obstacle 45 degrees to the left of the direction of flight is "medium", and the distance between the drone and the obstacle 45 degrees to the right of the direction of flight is "small", then the turning angle of the drone is "positive"; 5) If the distance between the drone and the obstacle in the direction of flight is "small", the distance between the drone and the obstacle 45 degrees to the left of the direction of flight is "medium", and the distance between the drone and the obstacle 45 degrees to the right of the direction of flight is "medium", then the turning angle of the drone is "positive"; 6) If the distance between the drone and the obstacle in the direction of flight is "small", the distance between the drone and the obstacle 45 degrees to the left of the direction of flight is "medium", and the distance between the drone and the obstacle 45 degrees to the right of the direction of flight is "large", then the steering angle of the drone is "negative large"; 7) If the distance between the drone and the obstacle in the flight direction is "small", the distance between the drone and the obstacle 45 degrees to the left of the flight direction is "large", and the distance between the drone and the obstacle 45 degrees to the right of the flight direction is "small", then the drone's turning angle is "positive and large"; 8) If the distance between the drone and the obstacle in the direction of flight is "small", the distance between the drone and the obstacle 45 degrees to the left of the direction of flight is "large", and the distance between the drone and the obstacle 45 degrees to the right of the direction of flight is "medium", then the turning angle of the drone is "positive large"; 9) If the distance between the drone and the obstacle in the flight direction is "small", the distance between the drone and the obstacle 45 degrees to the left of the flight direction is "large", and the distance between the drone and the obstacle 45 degrees to the right of the flight direction is "large", then the turning angle of the drone is "positive large"; 10) If the distance between the drone and the obstacle in the direction of flight is "medium", the distance between the drone and the obstacle 45 degrees to the left of the direction of flight is "small", and the distance between the drone and the obstacle 45 degrees to the right of the direction of flight is "small", then the turning angle of the drone is "positive large"; 11) If the distance between the drone and the obstacle in the direction of flight is "medium", the distance between the drone and the obstacle 45 degrees to the left of the direction of flight is "small", and the distance between the drone and the obstacle 45 degrees to the right of the direction of flight is "medium", then the steering angle of the drone is "negative large"; 12) If the distance between the drone and the obstacle in the direction of flight is "medium", the distance between the drone and the obstacle 45 degrees to the left of the direction of flight is "small", and the distance between the drone and the obstacle 45 degrees to the right of the direction of flight is "large", then the steering angle of the drone is "negative small"; 13) If the distance between the drone and the obstacle in the direction of flight is "medium", the distance between the drone and the obstacle 45 degrees to the left of the direction of flight is "medium", and the distance between the drone and the obstacle 45 degrees to the right of the direction of flight is "small", then the turning angle of the drone is "positive and large"; 14) If the distance between the drone and the obstacle in the direction of flight is "medium", the distance between the drone and the obstacle 45 degrees to the left of the direction of flight is "medium", and the distance between the drone and the obstacle 45 degrees to the right of the direction of flight is "medium", then the turning angle of the drone is "positive"; 15) If the distance between the drone and the obstacle in the direction of flight is "medium", the distance between the drone and the obstacle 45 degrees to the left of the direction of flight is "medium", and the distance between the drone and the obstacle 45 degrees to the right of the direction of flight is "large", then the steering angle of the drone is "negative small"; 16) If the distance between the drone and the obstacle in the direction of flight is "medium", the distance between the drone and the obstacle 45 degrees to the left of the direction of flight is "large", and the distance between the drone and the obstacle 45 degrees to the right of the direction of flight is "small", then the turning angle of the drone is "positive small"; 17) If the distance between the drone and the obstacle in the direction of flight is "medium", the distance between the drone and the obstacle 45 degrees to the left of the direction of flight is "large", and the distance between the drone and the obstacle 45 degrees to the right of the direction of flight is "medium", then the steering angle of the drone is "positive small"; 18) If the distance between the drone and the obstacle in the direction of flight is "medium", the distance between the drone and the obstacle 45 degrees to the left of the direction of flight is "large", and the distance between the drone and the obstacle 45 degrees to the right of the direction of flight is "large", then the turning angle of the drone is "positive small"; 19) If the distance between the drone and the obstacle in the direction of flight is "large", the distance between the drone and the obstacle 45 degrees to the left of the flight direction is "small", and the distance between the drone and the obstacle 45 degrees to the right of the flight direction is "small", then the drone's turning angle is "positive large"; 20) If the distance between the drone and the obstacle in the direction of flight is "Large", the distance between the drone and the obstacle 45 degrees to the left of the direction of flight is "Small", and the distance between the drone and the obstacle 45 degrees to the right of the direction of flight is "Medium", then the steering angle of the drone is "Negative Large"; 21) If the distance between the drone and the obstacle in the flight direction is "large", the distance between the drone and the obstacle 45 degrees to the left of the flight direction is "small", and the distance between the drone and the obstacle 45 degrees to the right of the flight direction is "large", then the steering angle of the drone is "negative small"; 22) If the distance between the drone and the obstacle in the direction of flight is "Large", the distance between the drone and the obstacle 45 degrees to the left of the direction of flight is "Medium", and the distance between the drone and the obstacle 45 degrees to the right of the direction of flight is "Small", then the drone's turning angle is "Positive Large"; 23) If the distance between the drone and the obstacle in the direction of flight is "Large", the distance between the drone and the obstacle 45 degrees to the left of the direction of flight is "Medium", and the distance between the drone and the obstacle 45 degrees to the right of the direction of flight is "Medium", then the drone's turning angle is "Positive Large"; 24) If the distance between the drone and the obstacle in the direction of flight is "Large", the distance between the drone and the obstacle 45 degrees to the left of the direction of flight is "Medium", and the distance between the drone and the obstacle 45 degrees to the right of the direction of flight is "Large", then the steering angle of the drone is "Negative Small"; 25) If the distance between the drone and the obstacle in the flight direction is "large", the distance between the drone and the obstacle 45 degrees to the left of the flight direction is "large", and the distance between the drone and the obstacle 45 degrees to the right of the flight direction is "small", then the steering angle of the drone is "positive small"; 26) If the distance between the drone and the obstacle in the flight direction is "Large", the distance between the drone and the obstacle 45 degrees to the left of the flight direction is "Large", and the distance between the drone and the obstacle 45 degrees to the right of the flight direction is "Medium", then the steering angle of the drone is "Positive Small"; 27) If the distance between the drone and the obstacle in the flight direction is "large", the distance between the drone and the obstacle 45 degrees to the left of the flight direction is "large", and the distance between the drone and the obstacle 45 degrees to the right of the flight direction is "large", then the steering angle of the drone is "zero".

4. The path planning and evaluation method based on active loop perception strategy according to claim 1, characterized in that: The implementation process of step (4) is as follows: Based on the generated path and images captured by the binocular camera, the ORB-SLAM3 algorithm is used for real-time positioning and map construction. The positioning results are compared with the true value of the generated trajectory to evaluate the impact of the path planning method based on the active loop perception strategy on positioning accuracy.

Images