A search and pursuit unmanned aerial vehicle path planning control method and system for open waters

Abstract

The application discloses a kind of open water area-oriented hunting unmanned plane path planning control method and system, it is related to hunting unmanned plane global path planning field;The scheme is first obtained by camera surveying and mapping device the scene boundary of hunting area;The real-time position of hunting unmanned plane is obtained by position sensor;According to the initial position of hunting unmanned plane and the position boundary of the hunting area to be searched, a global coordinate system is established;According to the position of hunting unmanned plane, global coordinate system, target hunting position, path planning is carried out;Control hunting unmanned plane moves along the path planned well.The scheme can reduce the work intensity of artificial hunting, obtain reference trajectory through path planning, thereby improve work efficiency.

Description

Technical Field

[0001] This invention relates to the field of search and capture drone control and path planning technology, and in particular to an autonomous drone path planning and control scheme suitable for searching targets in open water environments such as lakes. Background Technology

[0002] With advancements in control technology and the development of the low-altitude economy, unmanned aerial vehicles (UAVs) are playing an increasingly important role in practical applications. One key application scenario for UAVs is searching open waters (such as the Yangtze River in China). However, due to the windy and rainy conditions in open waters, search and capture UAVs are susceptible to external interference during flight missions, posing a challenge to successfully completing these missions. 1. Environmental characteristics: Open water lacks significant visual features, GPS signals may be unstable due to water surface reflection and multipath effect, and there are also characteristics such as open environment and significant wind interference.

[0003] 2. Path planning complexity: The search and capture mission requires a comprehensive coverage path planning (CPP) of a vast area, rather than simple point-to-point movement.

[0004] 3. Dynamic uncertainty: It is necessary to avoid obstacles that suddenly appear on the lake surface (such as boats and birds).

[0005] 4. Communication reliability: When operating on a long-distance lake surface, control signals and image transmission signals are susceptible to interference, which can cause them to attenuate or be interrupted.

[0006] Therefore, existing single-path planning algorithms (such as A*, RRT) or simple waypoint flight modes are insufficient to simultaneously address the issues of coverage, real-time obstacle avoidance, and interference resistance. Thus, developing a control scheme for search and capture drones designed for open waters is particularly important.

[0007] Existing search and capture drones need to design optimized planning paths based on their takeoff position and the location of the core search area. Commonly used path planning methods include the RRT algorithm and its improved version.

[0008] While the RRT algorithm has advantages such as not requiring system modeling, not requiring geometric division of the search area, high search space coverage, wide search range, and the ability to explore unknown areas as much as possible, it also suffers from excessive computational cost. Furthermore, due to the randomness of the sampling points in the RRT algorithm and related algorithms, and their poor stability, it is difficult to plan an optimal path. Summary of the Invention

[0009] To address the problems existing in path planning of existing search and capture drones operating in open water, the present invention aims to provide a path planning and control scheme for search and capture drones in open water, which can effectively plan search and capture paths for target areas in open water (such as the Yangtze River) and avoid dynamic obstacles, thus ensuring the operational efficiency and reliability of automatic search and capture operations of search and capture drones.

[0010] To achieve the above objectives, the present invention provides a path planning and control method for a search and capture drone oriented towards open water areas, the control method comprising the following steps: Step 1: Obtain the scene map of the area to be searched, construct the scene area, and form the corresponding boundary of the area to be searched based on the scene area, as well as determine the core search area within the scene area; Step 2: Obtain the real-time location of the search drone; Step 3: Establish a global coordinate system based on the initial position of the search drone determined in step (2) and the position boundary of the search area determined in step (1); Step 4: Based on the initial position of the search and capture drone, the global coordinate system, and the core search and capture area, perform path planning to form a global planning path covering the entire search and capture area; Step 5: Control the search drone to travel along the global planning path planned in step (4). When a target object is detected ahead of the travel path, perform local path planning for the target object, generate a local optimized path, and continue to move forward based on the local optimized path. Step 6: After completing the target object search operation based on the locally optimized path, control the search drone to return to the globally planned path.

[0011] Furthermore, the global planning path planned in step (4) takes the initial position of the search and capture drone as the starting point and the core search and capture area position within the field area as the ending point.

[0012] Furthermore, the search and capture drone can take off from any initial position and reach the destination in the core area.

[0013] Furthermore, when multiple target objects are detected ahead of the search and capture drone while it is moving along the globally planned path, a local path is first planned for the closest target object to generate a locally optimized path, and the drone continues to move forward based on the locally optimized path. After the search and capture operation for the closest target object is completed based on the locally optimized path, the search and capture operation for the next target object is executed according to the distance of the next target object or the globally planned path.

[0014] Furthermore, the search and capture drone can construct a geometric model based on the shape and size of the target object, the camera location, and the state of the camera coverage area.

[0015] Furthermore, in step 6, after the search and capture drone completes the local target search and capture task, the target point to which the search and capture drone needs to travel is first provided by dynamic look-ahead points to clarify the flight direction of the search and capture drone in the next step; then, the path is provided by B-spline to return to the global path, and at the same time, B-spline curves are used to control the search and capture drone to smoothly reach the target.

[0016] Furthermore, in the entire execution process, the control method starts the control based on step (2) every time the search and capture drone deviates from the planned trajectory.

[0017] Furthermore, after completing global or local path planning, the control method simultaneously introduces a position feedback-based active disturbance rejection control mode to control the motion attitude of the search and capture UAV during its journey along the planned path.

[0018] Furthermore, the active disturbance rejection control mode compensates for the displacement difference caused by external or internal disturbances during the movement of the search and capture drone along the path by using the position difference between the trajectory corresponding to the planned path and the actual position of the search and capture drone itself.

[0019] To achieve the above objectives, the present invention also provides a path planning and control system for a search and capture drone oriented towards open waters. The control system includes a processor, a memory, and a program stored in the memory and capable of running on the processor. The program is loaded by the processor and executes the steps of the above method.

[0020] The path planning and control scheme for search and capture drones provided in this invention has the following specific technical effects: 1) High search efficiency: The global path planning method significantly improves the probability of discovering high-value search areas while ensuring full coverage. The combination of global and local path planning enables the search drone to find the optimal path and provides a certain degree of real-time avoidance of dynamic obstacles; 2) The active disturbance rejection control algorithm based on position error feedback can effectively resist the complex external disturbances on the Yangtze River surface, effectively improving the robustness of the search and capture drone, so as to ensure that the search and capture drone can accurately travel on the planned path. 3) High search and capture quality: The row spacing is determined based on the effective swath width of the search and capture drone's sensors. After arriving at the search and capture area, the drone's flight altitude, speed, and hovering photography time are adjusted according to the coreness of the search area, thereby improving the search and capture quality. Attached Figure Description

[0021] The present invention will be further described below with reference to the accompanying drawings and specific embodiments.

[0022] Figure 1This is a flowchart of the path planning and control for the search and capture drone in this invention; Figure 2 This is an example diagram of obstacle distribution under the grid method involved in the embodiments of the present invention; Figure 3 This is an example diagram of global path planning involved in the embodiments of the present invention; Figure 4 This is an example diagram of local path correction involved in the embodiments of the present invention; Figure 5 This is a flowchart illustrating how the resultant force direction calculated by the artificial potential field (APF) is incorporated into the evaluation function of the dynamic window method (DWA) in the example of this invention. Detailed Implementation

[0023] To make the technical means, creative features, objectives and effects of this invention easier to understand, the invention will be further described below with reference to specific illustrations.

[0024] To address the challenges faced by search and capture drones in open water operations, this invention provides a path planning and control scheme for search and capture drones in open water. This control scheme, in conjunction with the operation of the search and capture drone, can effectively plan paths to target areas on the Yangtze River and avoid dynamic obstacles, ensuring the operational efficiency and reliability of the search and capture drone's automated operations.

[0025] Combination Figure 1 As shown, the path planning and control scheme for search and capture drones in open waters provided by this invention mainly consists of the following steps: Step 1: Obtain the scene map of the area to be searched, construct the scene area, and form the corresponding boundary of the area to be searched based on the scene area, as well as determine the core search area within the scene area; Step 2: Obtain the real-time location of the search drone; Step 3: Establish a global coordinate system based on the initial position of the search drone determined in step (2) and the position boundary of the search area determined in step (1); Step 4: Based on the initial position of the search and capture drone, the global coordinate system, and the core search and capture area, perform path planning to form a global planning path covering the entire search and capture area; Step 5: Control the search drone to travel along the global planning path planned in step (4). When a target object is detected ahead of the travel path, perform local path planning for the target object, generate a local optimized path, and continue to move forward based on the local optimized path. Step 6: After completing the target object search operation based on the locally optimized path, control the search drone to return to the globally planned path.

[0026] The following provides a further explanation of the specific implementation schemes for each step in the above scheme.

[0027] In step 1 of this scheme, the scene map of the area to be searched can be obtained by the camera measurement device on the search drone.

[0028] Furthermore, this step incorporates the boundary constraints of the outermost target object of the defined site area into the scene graph.

[0029] Specifically, this plan defines the search area in open water areas using three layers of boundaries based on the distribution of the search targets: an outer boundary, a middle boundary, and an inner boundary. The inner boundary is defined according to the location of key targets to facilitate priority searching and rapid response. The middle boundary is defined based on the possible drifting locations of key targets to facilitate UAVs entering the actual search range. The outer boundary, defined according to operational specifications, is a physical hard boundary constraint that serves as the physical boundary upon which the UAV must begin searching after entering the outer boundary.

[0030] In this solution, step 2, when obtaining the real-time location of the search and capture drone, is preferably achieved through the position sensor configured on the search and capture drone.

[0031] In step 3 of this solution, by establishing a global coordinate system, the "position of the search and capture drone" and the "position of the target" can be correlated in search and capture missions in wide waters, thereby achieving precise search and positioning.

[0032] Specifically, this solution uses the Northeast-Eastern Sky (ENU) Cartesian coordinate system, with the takeoff point of the search drone as its origin, as the core working coordinate system. All spatial information (drone position, area boundary, target coordinates) is uniformly mapped to this coordinate system to facilitate subsequent geometric calculations and path planning. Details are as follows: (1) Takeoff point calibration and zeroing. The takeoff point of the search and capture UAV is used as the origin of the global coordinate system. A Northeast Sky (ENU) rectangular coordinate system with the takeoff point as the origin is established. All subsequent flight waypoints and target positions will be represented as three-dimensional coordinate increments relative to this origin. (2) Transform the geographic boundary into a planar polygon. Calculate the latitude and longitude of each boundary vertex. Converted to planar coordinates using Gauss-Kruger projection Subtract the takeoff point coordinates. This yields the coordinates relative to the ENU origin. All transformed vertices This forms a planar polygon, which is the "mission area" where the search and capture drone needs to perform search and coverage operations.

[0033] In step 4 of this scheme, a grid-based method is preferred for path planning to form a globally planned path covering the entire search area. The formed globally planned path takes the initial position of the search drone as the starting point and the core search area within the field area as the ending point.

[0034] As further explanation, the search and capture drone can take off from any initial position and reach the endpoint of the core search and capture area without any restrictions on the initial position. The global planning path here is only related to the initial position of the search and capture drone, while ensuring that the search and capture drone can take off from any position without being restricted by the spatial starting point.

[0035] In step 5 of this scheme, when performing local path planning for the target object, the Dynamic Window Method (DWA) based on Artificial Potential Field (APF) is used. The Dynamic Window Algorithm based on Artificial Potential Field used here actively approaches and takes further pictures when the target object is detected in the water area, rather than treating the target object in the water area as an obstacle to avoid. The obstacles that need to be avoided are obstacles in the air above the water (such as boats and birds).

[0036] Furthermore, in the process of controlling the search and capture drone to travel along the globally planned path, if multiple target objects are detected ahead of the travel path, the solution first performs local path planning for the closest target object, generates a locally optimized path, and continues to move forward based on the locally optimized path; after completing the search and capture operation task for the closest target object based on the locally optimized path, the search and capture operation task for the next target object is executed according to the distance of the next target object or the globally planned path.

[0037] As a further explanation, when the search and capture drone detects multiple targets ahead of its path, this solution first ranks all detected targets in real time based on a multi-target priority evaluation model. This multi-target priority evaluation model comprehensively calculates the value score of each target detection: ; in: Represents the model evaluation coefficients. Indicates proximity. Indicates the target size. Indicates the speed of motion. Indicates task relevance.

[0038] The optimal processing sequence is determined based on the value score of each target detection. For the highest priority nearest target, path planning is performed using a dynamic window local path planning algorithm. Generate a velocity space sample (v, ω) at the current position, and evaluate the multi-objective cost function of the local trajectory generated for each velocity pair: ; in, Indicates the degree of orientation toward the target. Indicates the distance to the obstacle. Indicates path smoothness. Indicates energy consumption. These represent the coefficient weights for target orientation, obstacle distance, path smoothness, and energy consumption, respectively.

[0039] Based on this, the optimal trajectory is selected using the multi-objective cost function as an indicator. When performing the search, after completing the current target, the system automatically switches to the next priority target according to the pre-calculated optimal sequence. After the detection of multiple targets is completed, the system returns to the global path trajectory.

[0040] The entire process is managed by a finite state machine (detection → evaluation → planning → tracking → capture → switching). After each target is processed, the global task graph is updated, realizing the dynamic and coherent execution of multi-target search and capture.

[0041] Furthermore, this solution completes local path planning based on the most recently captured target object, and constructs a geometric model based on the shape and size of the captured target object, the location of the camera, and the state of the camera coverage area.

[0042] Specifically, to avoid the problems of "flying too high and not being able to see clearly" or "flying too low and scanning too slowly," this solution first inputs the "shape and size of the target object" through the ground station—for example, a person who has fallen into the water (approximately 0.5m × 0.5m), an oil slick (aspect ratio > 10:1), etc.; secondly, the search drone system establishes an object-image ratio triangle based on the focal length and pixel size of the onboard camera, and calculates the maximum flight altitude corresponding to the minimum imaging size that ensures the target can be reliably identified; finally, the search drone, based on the current flight altitude... Camera horizontal field of view Vertical field of view The actual coverage rectangle of the camera projection on the water surface is calculated based on the following calculation model formula: ; ; in: This indicates the coverage width in the vertical flight direction.

[0043] Based on this, The search area is rasterized using the grid as the basic unit. For irregular boundaries such as shorelines and islands, the grid is refined to 1 / 2 or 1 / 4 cells; for open, homogeneous waters, the grids are merged. Parallel routes are planned, and the route spacing is set to... × Overlap coefficient (usually taken as 0.7~0.8) to ensure that the flight path geometry of the search and capture drone strictly matches the field of view geometry of the camera, eliminating invalid flights.

[0044] Furthermore, after the search and capture drone reaches the search area, the drone's flight altitude, speed, and hovering time for taking photos are adjusted according to the core nature of the search area, thereby improving the quality of the search.

[0045] Specifically, after the onboard camera on the search drone identifies a target on the water surface, it extracts the bounding rectangle of the target's outline to obtain the pixel coordinates of the target's center point in the image coordinate system. The pixel deviation is then converted into adjustments for the gimbal's pitch and yaw angles to ensure the target remains centered in the image (camera position optimization). If the target is drifting (such as a person falling into the water and moving with the current), the search drone switches from "coverage search" mode to "tracking and locking" mode, and its flight path changes from straight-line scanning to curve tracking.

[0046] As further explanation, this solution can be implemented through the following method: To ensure the quality of the search, the core information must first be provided. Definition: ; in, It is based on the probability of the target existing in the grid after visual recognition. This is the time elapsed since the area was last searched. This is a heat map showing the density of ships and historical overboard incidents in the nearby waters. Weighting coefficients, satisfying .

[0047] To ensure the resolution of target recognition, the minimum resolution required for target recognition is set here. By reverse calculation, the maximum allowable height can be determined as follows: ; Simultaneously, according to the optical imaging formula, the ground sampling distance... With flight altitude Proportional: ; Based on the scope of core sensitivity, further define the execution actions and altitude control of the search and capture drones:

[0048] in, Indicates the reference height, which is set according to the sensor parameters; This represents the sensitivity coefficient (a value of 0.5 to 1.0 is recommended).

[0049] Because the core area requires a high detection rate, the speed must be reduced to ensure image clarity and overlap. Based on the range of core density, the execution actions and speed control of the search and capture drone are further described below:

[0050] Since hovering is the most effective but also the most time-consuming method to improve search quality, this solution allocates hovering time to the most critical grid cells. Hovering control is achieved through the following hovering time calculation formula: ; in, Indicates the baseline hovering duration. This indicates the coreness of the point. This indicates an environmental correction factor (increased by 30% when wind speed > 8 m / s to ensure gimbal stability).

[0051] Furthermore, in step 6 of this plan, the search and capture operation task of the search and capture drone mainly includes collecting and transmitting video streams of the target object.

[0052] Furthermore, after completing global or local path planning, this scheme simultaneously introduces a position feedback-based active disturbance rejection control mode to control the motion attitude of the search and capture drone during its journey along the planned path. By designing an active disturbance rejection control mode based on position feedback, the search and capture drone that deviates from the planned trajectory can return to the planned path.

[0053] The active disturbance rejection control mode here compensates for the displacement difference caused by external and / or internal disturbances received by the search and capture drone during its movement along the path by using the position difference between the trajectory corresponding to the planned path and the actual position of the search and capture drone.

[0054] As further explanation, the internal disturbances received by the search and capture drone are mainly caused by the inaccuracy of the drone model, which affects the flight control of the drone. The active disturbance rejection control mode based on position feedback can compensate for the internal disturbances in the model when the drone model is inaccurate, greatly reducing the internal disturbance error caused by model problems. It can be applied to different types of search and capture drones, expanding the applicability of the algorithm and reducing the requirements for model inaccuracies caused by different load devices on the search and capture drone.

[0055] As further explanation, the external disturbances received by the search and rescue drone are mainly disturbances brought about by the external objective environment, such as wind speed, rain and other normal natural weather. In this regard, the active disturbance rejection control mode based on position feedback can compensate for the external disturbances of the drone, so that the search and rescue drone has a certain degree of robustness, that is, it can maintain accurate path tracking and local path planning under external interference, thereby completing the search and rescue mission.

[0056] As a further explanation, the active disturbance rejection control mode introduced in this solution also includes a corresponding transition link, which reduces overshoot and ensures the stability of the search and capture drone during the search process. This provides support for the image quality captured by the camera on the Yangtze River and provides an important basis and guarantee for assessing the quality of the work.

[0057] Furthermore, in step 6 of this scheme, after the search and capture drone completes the local target search and capture task, the target point to which the search and capture drone needs to travel is first provided by dynamic look-ahead points to clarify the flight direction of the search and capture drone in the next step; then, the path is provided by B-spline to return to the global path, and at the same time, B-spline curves are used to control the search and capture drone to smoothly reach the target.

[0058] Specifically, in this solution, the search and capture drone calculates dynamic look-ahead points through a state perception module to determine the target location to be traveled. Then, it uses constraint optimization to generate a smooth trajectory to achieve target tracking. The entire process integrates real-time monitoring and fault recovery strategies to ensure that the search and capture drone can smoothly, safely, and efficiently return to the global path.

[0059] As further explained, since the dynamic look-ahead point is not a point at a fixed distance, but rather the optimal regression target point adaptively calculated based on the current state of the search and capture drone and environmental characteristics, and then the kinematically optimal access position is selected, it is necessary to analyze the speed and path curvature of the search and capture drone in real time.

[0060] The implementation scheme is explained in detail below: For ease of explanation, the following definitions are provided first: Represents a global path point sequence. Indicates the current location of the search drone. This indicates the local endpoint of the search and capture drone's path.

[0061] At the same time, determine a smooth trajectory It must meet the following requirements: ; This ensures that the search drone can smoothly transition to the global path after completing the search on a local path.

[0062] Based on this, this scheme ensures that the search and capture drone can return to the globally planned path using the shortest smooth local trajectory by constructing the following constraint equations: ; ; The turning radius of the search and capture drone is as follows: ; in: Indicates the current speed. Represents gravitational acceleration. This indicates the maximum roll angle.

[0063] As further explanation, in the entire execution process of controlling the search and capture drone, this scheme starts the control process based on step (2) every time the search and capture drone deviates from the planned trajectory.

[0064] The resulting path planning and control scheme for search and capture drones in open waters can reduce the workload of manual search and capture operations when used in conjunction with the drones. By obtaining reference trajectories through path planning, it can improve operational efficiency.

[0065] The following specific examples illustrate the application of this solution.

[0066] Here, we take the example of using search and capture drones to automatically search and capture a certain river surface.

[0067] As the executor of the entire search and capture mission, the search and capture drone is equipped with corresponding sensors to acquire relevant data about its surroundings or itself, in order to assist in the corresponding path planning and control.

[0068] For example, the sensors here include camera measurement devices used to acquire scene images of the target area to be searched and the distance between the search drone and the target object.

[0069] The specific configuration of the camera measuring device is not limited here; it can be determined according to actual needs, which is well known to those skilled in the art.

[0070] The sensors here also include position sensors, used to obtain the real-time location of the search drone.

[0071] The specific configuration of the position sensor is not limited here; it can be determined according to actual needs, which is a well-known technology to those skilled in the art.

[0072] Based on this, a corresponding path planning and control scheme for search and capture drones is developed to assist them in completing automatic flight and search operations.

[0073] To address the need for automated operation of search and capture drones, a core area and its endpoint coordinates are first delineated on the river surface; secondly, the area where the target may drift is inferred based on the water flow speed and direction.

[0074] After delineating the core area, the search drone plans a global path based on its takeoff position and the endpoint coordinates of the core area. It moves at full speed before reaching the core area, and after entering the core area, it adjusts its flight speed and hovering time for taking pictures, thereby improving the quality of the search.

[0075] Given the unique working environment of search and capture drones, the path planning in this example specifically includes a global path planning scheme that plans at least one movement path from the starting point to the destination for the search and capture drone based on the acquired global environmental information, as well as a local path planning scheme for the search and capture drone to explore the current local environment.

[0076] Accordingly, the path planning and control scheme for the search and capture UAV in this example mainly consists of three parts: global path planning, local path planning, and active disturbance rejection control based on position feedback.

[0077] See Figure 3 The entire plan first involves constructing a global path that adapts to the entire search and capture area, based on the endpoint coordinates of the core area and the global path planning of the mission objective.

[0078] This example demonstrates how to perform global path planning using a grid-based approach, such as... Figure 2 As shown, the grid method can divide the area to be searched into grid cells of fixed size using grids of fixed size. The position of each grid cell can be determined by its row and column. The search area can be simplified into a two-dimensional array, where each item of the array can represent a grid cell.

[0079] In the search area of ​​this example, if a grid cell is covered, it indicates the presence of an obstacle within that cell. This grid cell can be marked as 1 and assigned a probability value P(x,y) based on the size of the covered area. If a grid cell is completely uncovered, it indicates the absence of an obstacle within that cell. This grid cell can be marked as 0. Figure 2 As shown, white squares and blue squares represent areas with no obstacles and areas with obstacles, respectively. The probability value P(x,y) depends on the last position of the target. For high-probability areas, dense scanning is performed by reducing the line spacing; for low-probability areas, sparse scanning is performed by appropriately increasing the line spacing, improving efficiency and saving time and power.

[0080] This example, based on global path planning, further performs local path planning to enable further search and capture of targets on the water surface captured on the global path.

[0081] In this example, local path planning is preferably implemented using the Dynamic Window Method (DWA) based on Artificial Potential Field (APF). It is important to note that the Dynamic Window Algorithm based on Artificial Potential Field used here actively approaches and takes further pictures when a target is detected on the water surface, rather than treating the target on the water surface as an obstacle to avoid. The obstacles to avoid are those above the water surface, such as boats and birds.

[0082] See Figure 4 When a search drone identifies a target on the water surface, it needs to use that target as a provisional endpoint for local path planning, and then adjust the drone's direction and speed to conduct a detailed search and photograph of the target.

[0083] Specifically, the dynamic window algorithm based on artificial potential field is implemented as follows in this example: The search and capture drone travels along a global path. When it detects a surface target, it initiates a local path planning mode. Through a designed function, the surface target exerts an attractive force on the drone, drawing it towards the target and causing it to translate. Specifically, this can be expressed by the following formula: ; in, This is the current location of the search drone. It is the location of the target point. This is the gravitational gain coefficient. The gravitational force points towards the target, and its magnitude is proportional to the distance. The magnitude and direction of the gravitational force are determined by the difference between the current position of the search drone and the position of the target on the water surface.

[0084] Considering the potential obstacles above the river, this example incorporates a repulsive force function to achieve obstacle avoidance. This function ensures that obstacles exert a repulsive force on the search drone, preventing it from colliding with them. It's important to note that this repulsive force is only effective within a certain range of the obstacle's influence; that is, when the distance exceeds a safe distance. At that time, the repulsive force is zero. The direction of the repulsive force is from the obstacle to the drone, and its magnitude is inversely proportional to the distance (the closer the distance, the greater the repulsive force).

[0085] in, ; Therefore, the net force acting on the search drone can be expressed by the following formula: ; The next direction of the search drone's movement will be the direction of this combined force.

[0086] In this process, only the direction of the search drone's movement in the local path planning needs to be considered; its speed is achieved using a dynamic window method. After adjusting the drone's direction in the local path planning, its speed and hovering shooting performance need to be evaluated. Based on the actual situation of the search drone, this example further constructs and introduces the following evaluation function: ; in, These represent the azimuth weighting coefficient, the distance weighting coefficient between the search and capture drone and the obstacle weighting coefficient, and the speed weighting coefficient of the search and capture drone, respectively. These three weighting coefficients need to be determined by debugging based on the actual scenario.

[0087] This represents the azimuth score for the search and capture drone, as shown below: ; in, This indicates the direction angle of the current predicted trajectory endpoint. This represents the gravitational force calculated using the artificial potential field method. The direction angle.

[0088] The closer the two directions are, the higher the score. This forced dynamic window method (DWA) prioritizes trajectories that align with the direction indicated by the artificial potential field (APF) when selecting velocities.

[0089] This represents the obstacle distance score (Dist), which is expressed as follows:

[0090] If any point on the trajectory is less than the safety threshold from an obstacle, the trajectory score is immediately 0 (a veto). Otherwise, the greater the distance, the higher the score.

[0091] This represents the speed score, and its format is as follows: ; Search and capture drones are encouraged to fly at higher speeds, provided it is safe to do so, in order to improve efficiency.

[0092] Based on the above algorithm, the specific process of local path planning for the search and capture drone in this example is as follows: See Figure 3 and Figure 4 The search drone travels along the globally planned path. When it reaches the target area, the airborne lidar detects multiple target objects ahead of the travel path. It then performs local path planning on the closest target object to obtain a locally optimized path, reduces the line spacing, and performs dense scanning.

[0093] Specifically, the search drone activates Artificial Potential Field (APF) mode to calculate the distance to the nearest target and the distance between the search drone and obstacles, and then substitutes these values ​​into the formula. In order to obtain the direction of the search and capture drone in the local path planning, the search and capture drone will move along the resultant force acting on it. Proceed in that direction.

[0094] For ease of description of the APF's working mode, let's assume the target point is T1. T1 generates a strong gravitational field that attracts the search drone towards it. If an obstacle (not within a safe distance) O1 exists, it generates a repulsive force away from O1. Therefore, the search drone will simultaneously experience an attractive force towards T1 and a repulsive force away from O1.

[0095] After determining the approximate direction of the search drone in local path planning, the next step is to determine its speed. At this stage, the Dynamic Window (DWA) method is specifically applied. The DWA algorithm samples in the velocity space, generating a large number of samples. Yes, and simulate the corresponding trajectory. An evaluation function needs to be established. This function selects the speed command for the optimal group.

[0096] The following example illustrates the planning of local paths.

[0097] Combination Figure 4 and Figure 5 As the search drone travels along its globally planned path, it detects a target object and an obstacle over the water. The drone is currently positioned between the two. Assuming the target object is to the left and the obstacle to the right, the target object will exert an attractive force on the drone, pointing to the left and forward. Once the drone reaches a safe distance from the obstacle, it will exert a strong repulsive force on the obstacle, pointing to the left. The resultant force is calculated using the APF algorithm. The direction, namely left front, allows for both approaching the target and bypassing obstacles. The next step is to use the DWA algorithm to determine the speed of the search drone. Since the search drone is moving left front, its direction... and High consistency, which means The score is very high; at the same time, the trajectory maintains a safe distance from obstacles. High score; fast speed. The score is high. Therefore, this locally planned path has the highest total score. The search drone selects the speed command "accelerate to the left and forward". In the next cycle, the APF algorithm will recalculate the direction based on the new relative position, and the DWA algorithm will continue to execute until the threat is eliminated. This cycle repeats until the target in the target area is found, and the search mission is completed.

[0098] This example, based on global path planning and local path planning, further introduces a position feedback-based active disturbance rejection control scheme to control the motion attitude of the search and capture drone during its journey along the planned path, enabling the search and capture drone to move accurately along the planned trajectory.

[0099] Specifically, after the global path planning scheme or the local path planning scheme plans the corresponding path for the search and capture drone, this active disturbance rejection control scheme will drive the search and capture drone to travel along the planned path. At the same time, it will compensate for external disturbances (such as waves and wind on the river) encountered by the search and capture drone during its travel along the path, so as to ensure that the search and capture drone can move accurately along the planned trajectory.

[0100] The active disturbance rejection control scheme presented in this example is based on the difference between the trajectory planned by the search and capture drone and its own position, which forms the basis of the control law.

[0101] After the search and capture drone completes local path planning and returns to global path planning, an active disturbance rejection control algorithm is needed to ensure that the search and capture drone travels along the planned trajectory.

[0102] Due to the complex river environment and external disturbances (such as wind and waves), as well as inherent inaccuracies in the model (such as changes in load), deviations in the actual output speed may occur. These external disturbances can be considered as external forces, the sources of which include, but are not limited to, the displacement difference caused by wind and waves on the river surface. In this example, this displacement difference caused by wind and waves is considered as an external force acting on the search and capture drone, causing it to displace relative to the planned trajectory. Furthermore, in this example, the disturbance caused by the inaccuracies in the search and capture drone model is called an internal disturbance, and the internal and external disturbances are referred to as the total disturbance.

[0103] Therefore, in this example, an active disturbance rejection control algorithm is constructed and introduced to compensate for the displacement difference caused by the total disturbance on the search and capture drone. The specific scheme is as follows: This example will search for the location of the drone. As the input to the active disturbance rejection controller, the output variable after passing through the tracking differentiator is... This process is a transitional stage in active disturbance rejection control. The configuration of the tracking differentiator is not limited here and can be determined according to actual needs.

[0104] The interference received by the entire UAV is estimated by combining the extended state observer, and then the estimated value is introduced into the active disturbance rejection controller. Interference error correction is performed to compensate for the total interference experienced by the search and capture drone. For ease of description, let... We can obtain the following formula: ; in, For the total disturbance, This is the first derivative of the total disturbance. Since the total disturbance is not known, it needs to be estimated by using information about the state (such as position and speed) of the search drone.

[0105] Although the position and speed of the search and capture drone are known, it is necessary to estimate the total disturbance. Therefore, it is also necessary to estimate the position and speed information of the search and capture drone. The difference between the estimated position value (not the value of the reference trajectory) and the actual position value is used to construct an extended state observer, calculate the total disturbance, and compensate for the total disturbance so that the search and capture drone can operate in an ideal environment. The output speed and angle information will no longer be disturbed, thus ensuring that the search and capture drone can walk along the global trajectory.

[0106] Accordingly, the extended state observer designed in this example scheme is as follows: ; in, These represent the designed extended state observer pairs. The estimate, This indicates the observed positional error of the search and capture drone. The parameters need to be designed by the user. The specific parameters depend on the designed active interference rejection controller to drive the search and capture drone to move accurately along the global planned trajectory. The key feature is the performance of the designed active interference rejection controller.

[0107] ; Through the constructed extended state observer, we can obtain: ; Therefore, the active disturbance rejection controller is: ; in, ; Once the self-interference rejection controller is acquired, the search and capture drone can still accurately travel along the globally planned trajectory even under disturbance conditions.

[0108] The path planning and control scheme for search and capture drones in open water presented in this example can be implemented as a corresponding software program, forming a path planning and control system for the search and capture drones. When running, this software program will execute the aforementioned path planning and control method for the search and capture drones and store it in a suitable storage medium for the processor to retrieve and execute.

[0109] Furthermore, this example also provides a computer-readable storage medium storing a program that, when executed by a processor, implements the steps of the above-described path planning and control method for the search and capture drone.

[0110] This example also provides a processor for running a program, wherein the program executes the steps of the above-described path planning and control method for search and capture drones.

[0111] This example also provides a terminal device, which includes a processor, a memory, and a program stored in the memory and executable on the processor. The program code is loaded and executed by the processor to implement the steps of the above-described path planning and control method for the search and capture drone.

[0112] This example also provides a computer program product that, when executed on a data processing device, is suitable for performing the steps of the above-described search and capture drone path planning and control method.

[0113] The foregoing has shown and described the basic principles, main features, and advantages of the present invention. Those skilled in the art should understand that the present invention is not limited to the above embodiments. The embodiments and descriptions in the specification are merely illustrative of the principles of the invention. Various changes and modifications can be made to the invention without departing from its spirit and scope, and all such changes and modifications fall within the scope of the present invention as claimed. The scope of protection of this invention is defined by the appended claims and their equivalents.

Claims

1. A path planning and control method for a search and capture drone in open water, characterized in that, The control method includes the following steps: Step 1: Obtain the scene map of the area to be searched, construct the scene area, and form the corresponding boundary of the area to be searched based on the scene area, as well as determine the core search area within the scene area; Step 2: Obtain the real-time location of the search drone; Step 3: Establish a global coordinate system based on the initial position of the search drone determined in step (2) and the position boundary of the search area determined in step (1); Step 4: Based on the initial position of the search and capture drone, the global coordinate system, and the core search and capture area, perform path planning to form a global planning path covering the entire search and capture area; Step 5: Control the search drone to travel along the global planning path planned in step (4). When a target object is detected ahead of the travel path, perform local path planning for the target object, generate a local optimized path, and continue to move forward based on the local optimized path. Step 6: After completing the target object search operation based on the locally optimized path, control the search drone to return to the globally planned path.

2. The path planning and control method for search and capture drones in open water as described in claim 1, characterized in that, The global planning path planned in step (4) starts from the initial position of the search and capture drone and ends at the core search and capture area within the field area.

3. The path planning and control method for search and capture drones in open water as described in claim 1, characterized in that, The search and capture drone can take off from any initial position and reach the destination in the core area.

4. The path planning and control method for search and capture drones in open water as described in claim 1, characterized in that, When controlling the search and capture drone to travel along the globally planned path, if multiple target objects are detected ahead of the travel path, a local path is first planned for the closest target object to generate a locally optimized path, and the drone continues to move forward based on the locally optimized path. After completing the search and capture operation for the closest target object based on the locally optimized path, the search and capture operation for the next target object is executed according to the distance of the next target object or the globally planned path.

5. The path planning and control method for search and capture drones in open water as described in claim 1, characterized in that, The search and capture drone can construct a geometric model based on the shape and size of the target object, the camera location, and the camera coverage area.

6. The path planning and control method for search and capture drones in open water as described in claim 1, characterized in that, In step 6, after the search and capture drone completes the local target search and capture task, the target point to which the search and capture drone needs to travel is first provided by the dynamic look-ahead point to determine the flight direction of the search and capture drone in the next step; then the path is provided by the B-spline to return to the global path, and the B-spline curve is used to control the search and capture drone to smoothly reach the target.

7. The path planning and control method for search and capture drones in open water as described in claim 1, characterized in that, The control method, throughout the entire execution process, initiates control based on step (2) each time the search and capture drone deviates from the planned trajectory.

8. The path planning and control method for search and capture drones in open water as described in claim 1, characterized in that, After completing global or local path planning, the control method simultaneously introduces a position feedback-based active disturbance rejection control mode to control the motion attitude of the search and capture drone during its journey along the planned path.

9. The path planning and control method for search and capture drones in open water as described in claim 1, characterized in that, The active disturbance rejection control mode compensates for the displacement difference caused by external and / or internal disturbances during the movement of the search and capture drone along the path by using the position difference between the trajectory corresponding to the planned path and the actual position of the search and capture drone.

10. A path planning and control system for a search and capture drone operating in open water, the control system comprising a processor, a memory, and a program stored in the memory and capable of running on the processor, characterized in that, The program is loaded by the processor and executes the steps of the search and capture drone path planning and control method as described in any one of claims 1-9.

Images