Path planning method and system for unmanned aerial vehicle formation
By using distance field maps and improved A* algorithm in drone formation path planning, combining two-way search and bresenham algorithm to generate global flight path belts, the safety and efficiency problems of drone formation path planning are solved, and the safety and efficiency problems of drone formation path planning are achieved. The efficient and safe flight of drone formations in obstacle areas.
Patent Information
- Application Number
- CN202411212001.2
- Authority / Receiving Office
- CN · China
- Patent Type
- Applications(China)
- Current Assignee / Owner
- Filing Date
- 2024-08-30
- Publication Date
- 2024-12-06
AI Technical Summary
There is a lack of an overall path planning method suitable for drone formations in the prior art, and it is difficult to effectively plan the motion path of drone formations through obstacle areas.
By obtaining the environment raster map, adding distance information of the distance between obstacle areas to form a distance field map, and using the improved A* algorithm for initial path planning, combining two-way search and bresenham algorithm for path optimization and reconnection, generating a global flight path belt , ensure the safety and optimality of the path.
It realizes safe and efficient flight of drone formations in obstacle areas, provides controllability and scalability of path planning, and improves the safety and efficiency of task execution of formations.
Smart Images

Figure CN119088058A_ABST
Abstract
Description
Technical Field
[0001] The present invention belongs to the technical field related to unmanned aerial vehicles, and more specifically, relates to a path planning method and system for an unmanned aerial vehicle formation. Background Art
[0002] Compared with a single drone, drone formations show obvious advantages in mission execution. First, formation flying can greatly improve the efficiency of mission execution and the scope of operation coverage, especially in applications such as large-scale monitoring, search and rescue, and agricultural spraying. Secondly, through division of labor and cooperation, the formation can perform multiple subtasks at the same time, improving the ability to handle the complexity of tasks. In addition, formation flying enhances the redundancy of the system. Even if individual drones fail, other members can quickly fill in and maintain the continuity and stability of the mission. Finally, through collaborative operations, the formation can achieve more complex operations and strategies, such as environmental adaptability and dynamic response capabilities, which are often difficult to achieve in a single drone. Overall, drone formations not only expand the functional limitations of a single machine, but also greatly improve the quality of mission execution and the robustness of the system.
[0003] However, in the prior art, there are more path trajectory planning for a single UAV, and fewer path planning solutions for the entire UAV formation, and there is a lack of path planning methods suitable for the entire UAV formation. Summary of the invention
[0004] In response to the above defects or improvement needs of the prior art, the present invention provides a path planning method and system for drone formations, which are used to solve the problems in the prior art that there are few path planning schemes for the entire drone formation and lack of a path planning method suitable for the entire drone formation. Through this method and system, the motion planning information of the drone formation passing through the obstacle area can be quickly obtained, and it can be used as a control variable to control the drone formation.
[0005] To achieve the above object, according to one aspect of the present invention, a path planning method for a UAV formation is provided, comprising:
[0006] S1, obtaining an environmental grid map, wherein any grid of the environmental grid map has coordinate information and attribute information, and the attribute information is a feasible area or an obstacle area;
[0007] S2, adding distance information of the distance between any grid of the environmental grid map and the nearest obstacle area to form a distance field map;
[0008] S3, taking the distance information on the distance field map as a consideration to improve the A* algorithm, using the improved A* algorithm to perform initial path planning, and obtaining an initial global flight path;
[0009] S4, based on the initial global flight path, the global flight path belt of the UAV formation is obtained by expanding to both sides to complete the global path planning.
[0010] According to the path planning method for UAV formation provided by the present invention, the heuristic function of the improved A* algorithm in S3 is:
[0011]
[0012] Where h represents the heuristic function, coord represents the coordinates of the current search point, goalcoord represents the coordinates of the target point, (1) and (2) represent the x-coordinate and y-coordinate respectively, abs represents the absolute value returned, distance_field represents the distance information on the distance field map, and k and m represent the weight correction coefficients of the distance information;
[0013] And / or, using the improved A* algorithm to perform initial path planning in S3 specifically includes: using the improved A* algorithm in combination with a bidirectional search algorithm to perform initial path planning.
[0014] According to the path planning method for UAV formation provided by the present invention, S4 specifically includes:
[0015] S41, performing secondary path planning based on the search area when the initial global flight path is generated to obtain an optimized global flight path;
[0016] S42, acquiring a global flight path band of the UAV formation by expanding the optimized global flight path to both sides;
[0017] Among them, S41 specifically includes:
[0018] S411, using the grid points in the search area when the initial global flight path is generated as search points;
[0019] S412, along the initial global flight path, obtaining the number of search points within a preset range around any path point on the initial global flight path;
[0020] S413, determining a problematic path point on the initial global flight path according to the number of search points within a preset range around any path point;
[0021] S414, removing problematic path points on the initial global flight path, and obtaining a path segment to be reconnected;
[0022] S415, using the Bresenham algorithm to perform line of sight detection and reconnection on the path segment to be reconnected, to obtain the optimized global flight path.
[0023] According to the path planning method for UAV formation provided by the present invention, S413 specifically includes:
[0024] Get the ratio of the number of search points in a preset range around any path point to the area of the preset range;
[0025] When the proportion is greater than or equal to a preset ratio threshold, the corresponding path point is determined to be a problem path point.
[0026] According to the path planning method for UAV formation provided by the present invention, the proportion K is specifically:
[0027]
[0028] Among them, n F is the number of search points within the preset range around the path point, r s The preset range radius is set in advance.
[0029] According to the path planning method for UAV formation provided by the present invention, obtaining the global flight path band of the UAV formation by expanding to both sides in S4 specifically includes:
[0030] In the search area when the initial global flight path is generated, the distance information of the grid points around the path point is retrieved, and the grid points with distance information greater than a preset distance threshold are incorporated into the path belt to form a global flight path belt.
[0031] According to the path planning method for UAV formation provided by the present invention, after S4, the method further includes:
[0032] S5, performing filtering and smoothing processing on the optimized global flight path to obtain a global smooth path;
[0033] S6, performing trajectory planning on the global smooth path, and obtaining speed information of the UAV formation during its flight along the global flight path.
[0034] According to the path planning method for UAV formation provided by the present invention, S6 specifically includes:
[0035] S61, performing sampling processing on all path points on the global smooth path to obtain a sampling point set;
[0036] S62, allocating the preset total time between any two adjacent sampling points according to the interval between any two adjacent sampling points and the preset total time of the UAV formation flight, and obtaining the allocated time between any two adjacent sampling points;
[0037] S63, performing trajectory planning for the path between any two adjacent sampling points according to the allocated time between any two adjacent sampling points, and obtaining speed information of the UAV formation between any two adjacent sampling points.
[0038] According to the path planning method for UAV formation provided by the present invention, S61 specifically includes:
[0039] The global smooth path is sampled in sections according to the distance information of the path points on the global smooth path, and the sampling ratio of the path segment with a shorter distance in the distance information is larger.
[0040] According to another aspect of the present invention, a path planning system for a UAV formation is provided, comprising:
[0041] A first map module is used to obtain an environmental grid map, wherein any grid of the environmental grid map has coordinate information and attribute information, and the attribute information is a feasible area or an obstacle area;
[0042] A second map module is used to add distance information of the distance between any grid of the environmental grid map and the nearest obstacle area to form a distance field map;
[0043] An initial path planning module is used to improve the A* algorithm by taking the distance information on the distance field map as a consideration, and perform initial path planning using the improved A* algorithm to obtain an initial global flight path;
[0044] The path optimization expansion module is used to obtain the global flight path belt of the UAV formation by expanding to both sides based on the initial global flight path to complete the global path planning.
[0045] In general, compared with the prior art, the above technical solutions conceived by the present invention provide a path planning method and system for drone formations:
[0046] 1. It is proposed to add the distance information of the distance between the nearest obstacle area to the environmental grid map to form a distance field map, improve the A* algorithm based on the distance information, and generate the initial global flight path based on the improved A* algorithm, so that the distance between the generated path and the obstacle area can be controlled, which is conducive to maintaining a safe distance from the obstacle area, and can be better suitable for the flight of UAV formations, which is conducive to ensuring safety; further generating path belts can provide a basis for the scaling control of the UAV formation, so that the UAV formation has scaling space. This scheme can be applied to the path planning of the UAV formation and has high safety;
[0047] 2. The effective path segments of the initial path planning are retained through secondary path planning, and the problematic path segments of the initial path planning are re-determined, which is conducive to further shortening the total path length, ensuring the security and optimality of the global path, while also ensuring a high search efficiency;
[0048] 3. The local trajectory optimization method is coupled with the global path planning method. On the basis of obtaining the optimized global flight path, the path points are sampled, and the motion information of the UAV formation between the path points is planned. The control input of the acceleration level of the UAV formation can be obtained, which is conducive to the smooth and stable flight of the UAV formation.
[0049] 4. Compared with the traditional single UAV path planning and trajectory optimization scheme, it is a planning scheme that is more suitable for the overall UAV formation. It not only ensures the safety and optimality of the planning results, but also ensures the efficiency of the solution through the improvement of the algorithm, and the results can be obtained in a short time. BRIEF DESCRIPTION OF THE DRAWINGS
[0050] Figure 1 It is a flow chart of a path planning method for UAV formation provided by the present invention;
[0051] Figure 2 It is a heat map illustration of the distance field map provided by the present invention;
[0052] Figure 3 It is a schematic diagram of the initial global path planning result provided by the present invention;
[0053] Figure 4 The secondary global path planning provided by the present invention adopts a map diagram;
[0054] Figure 5 It is a secondary global path feature extraction result graph provided by the present invention;
[0055] Figure 6 is a path reconnection flow chart provided by the present invention;
[0056] Figure 7 It is a schematic diagram of the reconnection result of the secondary global path planning provided by the present invention;
[0057] Figure 8 It is a schematic diagram of the path band expansion result provided by the present invention;
[0058] Fig. 9 It is a schematic diagram of the SG filtering result provided by the present invention;
[0059] Fig.10 It is a schematic diagram of the local trajectory optimization result provided by the present invention;
[0060] Fig.11It is a principle architecture diagram of the method provided by the present invention. DETAILED DESCRIPTION
[0061] In order to make the purpose, technical solutions and advantages of the present invention more clearly understood, the present invention is further described in detail below in conjunction with the accompanying drawings and embodiments. It should be understood that the specific embodiments described herein are only used to explain the present invention and are not intended to limit the present invention. In addition, the technical features involved in the various embodiments of the present invention described below can be combined with each other as long as they do not conflict with each other.
[0062] See also Figure 1 , this embodiment 1 provides a path planning method for a UAV formation, the method comprising:
[0063] S1, obtaining an environmental grid map, wherein any grid of the environmental grid map has coordinate information and attribute information, and the attribute information is a feasible area or an obstacle area;
[0064] S2, adding distance information of the distance between any grid of the environmental grid map and the nearest obstacle area to form a distance field map;
[0065] S3, taking the distance information on the distance field map as a consideration to improve the A* algorithm, using the improved A* algorithm to perform initial path planning, and obtaining an initial global flight path;
[0066] S4, based on the initial global flight path, the global flight path belt of the UAV formation is obtained by expanding to both sides to complete the global path planning.
[0067] In S1, an environmental map can be obtained first. The environmental map can be obtained by collecting global images through drones, or by pre-investigation, or by obtaining existing maps, etc. The specific acquisition method is not limited. Then, the obstacle danger area is marked in the environmental map, such as Figure 2 As shown, the dangerous area is not regular, but unstructured, and can be represented by polygons; for example Figure 2 This is a dangerous area map generated in MATLAB using random polygons. The polygons represent dangerous areas. The environmental map that actually marks the dangerous obstacle area is as follows Figure 2 After the construction of the basic environment map is completed, the global path planning process begins. First, the basic environment map is rasterized, and the coordinates are represented by a two-dimensional array, with 0 representing the feasible area and 1 representing the obstacle area, forming an environmental grid map. The environmental grid map is represented in matrix form.
[0068] Based on the environment grid map, a distance field map is constructed, and the nearest obstacle distance information is added to each coordinate in the grid map to construct a grid map with a distance field, which is used as a map for subsequent path planning algorithms. The nearest obstacle distance information is the straight-line distance between the grid point and the nearest obstacle area.
[0069] Then an improved A* search algorithm is proposed. The A* algorithm is a heuristic search algorithm used to find the shortest path from the starting point to the end point. It combines the comprehensiveness of the Dijkstra algorithm and the efficiency of the greedy algorithm. It determines the search priority by evaluating the cost of each node (the actual cost from the starting point to the node plus the estimated cost from the node to the target). This allows the A* algorithm to effectively reduce the search space and improve the search efficiency while ensuring that the optimal path is found. This algorithm is widely used in path planning, NPC movement decisions in graphic games, robot navigation and other fields. The purpose of using the A* algorithm is to obtain the optimal route. The algorithm samples the Euclidean distance and can diagonally expand adjacent nodes. By considering the distance field information in the heuristic function of the algorithm, the purpose of maintaining a safe distance from obstacles can be achieved. Therefore, by performing initial path planning through the improved A* algorithm, a feasible global path, that is, the initial global flight path, can be obtained.
[0070] Furthermore, based on the initial global flight path, the path is expanded into a trajectory belt in the grid map through expansion processing to obtain the feasible movement area of the formation. The UAV formation can be scaled to ensure that the UAV formation maintains a certain distance from the edge of the trajectory belt, thereby achieving overall obstacle avoidance for the UAV formation.
[0071] Furthermore, the distance information on the distance field map is taken into consideration to improve the A* algorithm. Specifically, the distance information is added when the A* algorithm performs path planning and searches to determine path points; points far from the obstacle area are searched first. Specifically, the improved A* algorithm considers the distance field information of the grid map and designs a new heuristic function. The heuristic function of the improved A* algorithm in S3 is:
[0072]
[0073] Wherein, h represents the heuristic function, coord represents the coordinates of the current search point, goalcoord represents the coordinates of the target point, (1) and (2) represent the x-coordinate and y-coordinate respectively, abs represents the absolute value returned, distance_field represents the distance information on the distance field map, specifically all the distance information on the distance field map, k and m represent the weight correction coefficients of the distance information; the preferred range of k is 0.5 to 3, and the preferred range of m is 2 to 3.5. Figure 3The middle blue area is the search area formed by searching according to the distance field. Points far from the obstacle area will be searched first, and the planned path is the optimal path within the search area. By adding the weighted distance information consideration item to the A* algorithm, the obtained path may not be the global shortest path, but it is the path with the optimal distance between the path points on the path and the obstacle area.
[0074] The use of the improved A* algorithm for initial path planning in S3 specifically includes: using the improved A* algorithm and combining it with a bidirectional search algorithm for initial path planning. The sampled improved A* algorithm is a bidirectional search algorithm, that is, searching from both the starting point and the end point at the same time, and bidirectional search can significantly improve the search efficiency of the algorithm.
[0075] Furthermore, S4 specifically includes:
[0076] S41, performing secondary path planning based on the search area when the initial global flight path is generated to obtain an optimized global flight path; that is, the secondary search map is constructed based on the search results of the first improved A* algorithm, taking the searched area as the feasible domain and the unsearched area as the obstacle area.
[0077] S42, obtaining a global flight path band of the UAV formation by expanding the optimized global flight path to both sides.
[0078] Among them, S41 specifically includes:
[0079] S411, using the grid points in the search area when the initial global flight path is generated as search points;
[0080] S412, along the initial global flight path, obtaining the number of search points within a preset range around any path point on the initial global flight path;
[0081] S413, determining a problematic path point on the initial global flight path according to the number of search points within a preset range around any path point;
[0082] S414, removing problematic path points on the initial global flight path, and obtaining a path segment to be reconnected;
[0083] S415, using the Bresenham algorithm to perform line of sight detection and reconnection on the path segment to be reconnected, and obtain the optimized global flight path. That is, firstly determine the line of sight endpoints of the path segment to be reconnected under line of sight detection through line of sight detection and reconnection, and then realize the connection between the line of sight endpoints on the grid map through the Bresenham algorithm.
[0084] S413 specifically includes:
[0085] Get the ratio of the number of search points in a preset range around any path point to the area of the preset range;
[0086] When the ratio is greater than or equal to the preset ratio threshold, the corresponding path point is determined to be a problem path point. By setting the ratio threshold, feature extraction is performed to extract the path segment that needs to be reconnected. The path segment that needs to be reconnected is deleted from the planned path, and the breakpoints are reconnected using the Bresenham algorithm.
[0087] Specifically, the proportion K is:
[0088]
[0089] Among them, n F is the number of search points within the preset range around the path point, r s The preset range radius is set in advance. The optional range of the preset ratio threshold is 0.4-0.6.
[0090] in,
[0091] where k r The preferred range is 6 to 8, n o The number of grids representing the obstacle area in the environmental grid map, n m Represents the total number of grids in the environment grid map.
[0092] like Figure 3 As shown in the figure, after completing the first search, i.e. the initial path planning, a safe path can be obtained. However, although the A* algorithm itself can obtain the optimal route, due to the discrete path points, in fact, there are many deficiencies in the planning results. Since the A* algorithm uses a heuristic function, it will give priority to searching for path points with a small end cost. Although this method will search for the optimal route under normal circumstances, due to the discrete path points searched by A*, in actual situations, the search results are not optimal. As shown in the figure, the route planned by the A* algorithm will first extend to the end point, and will choose to bypass the obstacle when it is close to the obstacle. In fact, the better route should be extended in the direction away from the obstacle from the beginning.
[0093] Therefore, it is necessary to perform secondary path planning based on the existing planning results. Since the secondary planning is based on the search map of the first planning, it is more efficient and takes less time. Figure 4 As shown, first obtain the closed set after the first planning as the map of the secondary planning, where the points searched in the initial planning are regarded as feasible regions and the unsearched points are regarded as obstacles.
[0094] After the secondary planning map is built, the secondary path planning is started. The purpose of secondary planning is to extract the valid part of the primary planning path, remove the problematic path, and reconnect the trajectory on this basis.
[0095] Because the A* algorithm will only extend the search for adjacent nodes after encountering obstacles, the problem routes that need to be reconnected are all in areas with more search points. Therefore, the reconnection path range can be determined by retrieving the search point ratio within a certain range around the path point. Specifically, a preset search point ratio threshold is set, and the path points that jump from the threshold are retrieved, which can quickly divide the problem route segments. The center point of the grid in the environmental grid map is the grid point, and the grid point on the path is the path point. The division result is as follows: Figure 5 As shown, the red area is the path segment that needs to be reconnected.
[0096] After completing the problem route division, the trajectory needs to be reconnected, such as Figure 6 As shown, the reconnection method is to start from the starting point of each problem path and perform line of sight detection to the end of the problem path. If the line of sight detection is blocked, the target point of the line of sight detection is recursively pushed from the end point to the starting point until the line of sight detection is not blocked. The current target point is recorded and used as the starting point. The line of sight detection continues from the end point, and so on until the end point becomes the starting point of the line of sight detection.
[0097] In order to avoid the loss of effective information, it is necessary to connect the key nodes of line of sight detection, that is, the two endpoints, to ensure the uniformity of the distribution of discrete points. The connection method uses the Bresenham algorithm. The path after reconnection is as follows Figure 7 As shown. The Bresenham algorithm is an algorithm widely used in computer graphics, mainly used to efficiently draw lines, circles or other simple shapes in a pixel grid. The algorithm was invented by Jack Bresenham in 1962. Its core advantage is that it only uses integer operations to determine which pixels are closest to the path of an ideal straight line. This approach reduces computational complexity and increases execution speed, especially suitable for systems with limited hardware. The Bresenham algorithm iterates from one pixel to the next step by step, deciding which pixel should be selected at each step to best represent the line or shape being drawn.
[0098] Furthermore, the purpose of path planning is to plan a safe path for the drone formation from the starting point to the end point. Since the drone formation plays the role of guiding ground vehicles, the drone formation as a whole should also maintain a safe distance from obstacles, so it is necessary to plan the path belt to provide the scaling information of the drone formation. Specifically, S4 obtains the global flight path belt of the drone formation by expanding to both sides, which specifically includes:
[0099] In the search area when the initial global flight path is generated, the distance information of the grid points around the path point is retrieved, and the grid points with distance information greater than a preset distance threshold are incorporated into the path belt to form a global flight path belt.
[0100] Specifically, first complete the planned path extraction, and in this embodiment, extract the optimized global flight path. Further, based on the path, generate the edge of the path band, and set the path band open set and the path band closed set at the beginning. In the grid map, each grid point has obstacle distance information. This distance is the distance from the current point to the nearest obstacle, and a preset obstacle distance threshold is set. The initial path band closed set is zero, and the path points on the optimized global flight path are stored in the open set. The coordinates in the open set are traversed in the loop, the adjacent nodes of the coordinates are retrieved, and the currently retrieved coordinates are stored in the closed set. If the distance between the adjacent node and the obstacle is greater than the preset distance threshold and the node is not in the two sets, the adjacent node is stored in the open set. This cycle is repeated, and the maximum number of cycles can be set. Finally, a strip area is expanded. The area is centered on the path point, and the distances between the internal grid points and the nearest obstacles are greater than the preset distance threshold. The final result is as follows. Figure 8 As shown in the figure, by cyclically expanding the path points, the feasible domain is included in the path point set to form a wider trajectory belt as the feasible area of the UAV formation.
[0101] Therefore, after the path strip is extracted, the global path planning process is completed. Next, based on the path, the path needs to be optimized to complete the trajectory planning including time information. Further trajectory planning with motion information is required, which will become the input for subsequent formation control.
[0102] Since the A* algorithm can only extend path points in eight directions, the planned path needs to be smoothed. Path smoothing uses the SG filter, which works based on local polynomial regression. For each data point, select the data points in the window around it and fit a polynomial to these points. Then use the value of this polynomial at the current point to replace the value of the original data point. In this way, each data point is smoothed, but the overall data structure such as local extreme values and inflection points is retained, such as Fig. 9 shown.
[0103] That is, after S4, it also includes:
[0104] S5, filtering and smoothing the optimized global flight path to obtain a global smooth path; the initial global flight path, the optimized global flight path and the global smooth path in the above embodiment are the moving paths of the geometric center of the UAV formation.
[0105] S6, performing trajectory planning on the global smooth path, obtaining speed information of the UAV formation during the flight along the global flight path, thereby completing the final path planning. The speed information is the speed information of the geometric center of the UAV formation along the path.
[0106] Furthermore, S6 specifically includes:
[0107] S61, performing sampling processing on all path points on the global smooth path to obtain a sampling point set;
[0108] S62, allocating the preset total time between any two adjacent sampling points according to the interval between any two adjacent sampling points and the preset total time of the UAV formation flight, and obtaining the allocated time between any two adjacent sampling points;
[0109] S63, according to the allocated time between any two adjacent sampling points, trajectory planning is performed on the path between any two adjacent sampling points, and speed information of the UAV formation between any two adjacent sampling points is obtained. For example, the minimum jerk algorithm can be used to perform trajectory planning between any two adjacent sampling points.
[0110] Furthermore, S61 specifically includes:
[0111] The global smooth path is sampled in sections according to the distance information of the path points on the global smooth path, and the sampling ratio of the path segment with a shorter distance in the distance information is larger.
[0112] In some specific embodiments, trajectory planning is implemented through a time allocation scheme. When planning the trajectory, it is necessary to pre-set the time step between the path points, and then fit the points through polynomials to obtain the motion trajectory between the points.
[0113] The scheme of this embodiment is as follows: after obtaining the set of path points for secondary planning and filtering, since there are many path points, it is necessary to sample the path points to reduce the computational burden of trajectory planning. In this process, the distance field information of the path points is considered, and weighted sampling is performed. For path segments far from obstacles, the sampling number is low, and for path segments close to obstacles, the sampling number is high. After sampling is completed, a set of sampling points is obtained, and time is allocated according to the straight-line distance between each sampling point. When the distance between sampling points is large, the allocation time is high, and when the distance between sampling points is small, the allocation time is low.
[0114] Sampling ratio calculation formula:
[0115]
[0116] Among them, S(d) is the sampling ratio, d refers to the obstacle distance field information, that is, the distance information between the nearest obstacle area, and the distance information represents the ratio of the straight line spacing between grid points to the grid side length.
[0117] Time allocation calculation formula:
[0118]
[0119] Among them, t i -t i-1 is the allocation time of the i-th path segment, t 0 =0, T is the preset total time, d i is the distance between two points in the i-th path segment, d total is the sum of all straight-line distances between segments.
[0120] The sampling and time allocation scheme can improve the computational efficiency while ensuring that the fitted trajectory maintains a safe distance from obstacles and reduces information loss. The subsequent trajectory planning calculation process, including constraint formulation, is similar to the conventional scheme. For discrete path points, the minimum jerk algorithm is used for trajectory planning to plan the real-time speed of the geometric center of the drone formation, so that the drone formation can track the trajectory movement with smooth speed changes to ensure the stability of the formation. The MinimumJerk algorithm is an optimization method used in robotics and biomechanics, mainly used to plan smooth and natural motion trajectories. This algorithm is based on minimizing the rate of change of acceleration (jerk) during motion to generate the smoothest possible motion path. By minimizing jerk, the algorithm can mimic the natural movement of humans and animals, so that it can move efficiently from the starting point to the end point without producing abrupt or unnatural movements. This method is particularly valuable in ensuring the smooth and precise movement of robotic arms or other automated equipment, and also helps to reduce mechanical wear and energy consumption.
[0121] For the path points in space, a time-dependent polynomial is needed to describe the changes in its x and y coordinates. By adjusting the coefficients of the polynomial, the current non-smooth path can be fitted. However, due to the large number of path points, it is unrealistic to fit with a polynomial trajectory, which will result in too high polynomial coefficients, not only time-consuming and expensive to solve, but also the fitting effect will not be very good. In contrast, a discrete path point can be fitted with multiple polynomial trajectories, but there are some differences from fitting a single trajectory. In most cases, it is difficult to give the robot's speed, acceleration, Jerk and other information when it passes through the intermediate track point. It can be considered that the speed, acceleration and other information of the intermediate point is a state that can change freely. For this kind of intermediate state uncertainty problem, it is necessary to construct an optimization problem so that when the intermediate state obtains a certain value, the formulated cost function reaches the minimum value. On the one hand, it is not necessary to design the state in which the robot passes through the intermediate point, and on the other hand, an optimal trajectory can be obtained by optimizing the cost function. Ultimately, the trajectory is required to satisfy a series of constraints, such as: the position, speed or acceleration of the starting and ending points, the connection between adjacent trajectories is expected to be smooth (position continuity, speed continuity, etc.), the trajectory passes through certain path points, etc.
[0122] First, we need to divide the trajectory into k segments according to the path points, calculate the distance of each segment, divide the time T by the distance (uniform time allocation), obtain the time series, and plan the trajectory separately for the x and y dimensions.
[0123] There are usually countless trajectories that meet the constraints, but in practical problems, a specific trajectory is often required, so it is necessary to construct an optimal function to find the "optimal" specific trajectory among the feasible trajectories.
[0124] Therefore, in the end, the trajectory planning process can be expressed as a constrained optimization problem, that is, when the minimum jerk algorithm is used to plan the trajectory between any two adjacent sampling points, the optimization objective function of the algorithm is as follows:
[0125]
[0126] Where f(p) represents the trajectory fitting polynomial. The optimization problem here is to solve the polynomial min f(p) with the smallest fourth-order derivative. Min f(p) is the optimization function. p represents the vector of coefficients of each order of the polynomial, t represents time, and A eq p=b eq represents an equality constraint, A eq p≤b eq Represents an inequality constraint. The purpose of adding constraints is to make the transition between different segments of polynomial trajectories smooth. eq , b eqThat is, the constraints of the optimization problem. Specifically, when the minimum jerk algorithm is used to plan the trajectory between any two adjacent sampling points in this embodiment, the constraints include: setting the position, speed, and acceleration of a specific point to a specific value, respectively forming equation constraints, for example, setting the starting position and the end position to be on the path points respectively, setting the speed and acceleration values at the intermediate moments; and the equation constraints continuously formed by the position, speed, and acceleration between adjacent path segments.
[0127] Specifically, the optimization function is:
[0128]
[0129] In order to ensure that the value of the optimization function is greater than or equal to 0, it is squared. In the optimization function, T represents the preset total time, k represents the total number of path segments for trajectory optimization, n represents the order of the polynomial, and t i Represents the allocated time period of the current trajectory. Since the fourth-order derivative of the polynomial is obtained, the coefficients of the first four terms are 0. The part of the optimization function other than the polynomial coefficients is written as the matrix Q i Specifically, Q i for:
[0130]
[0131] Where r represents the matrix Q i The number of rows of the matrix Q i The optimization results are as follows Fig.10 shown.
[0132] The second embodiment provides a path planning system for a UAV formation, which is used to implement any of the above-mentioned path planning methods for a UAV formation. The system can be understood by referring to the above-mentioned method. The system includes:
[0133] A first map module is used to obtain an environmental grid map, wherein any grid of the environmental grid map has coordinate information and attribute information, and the attribute information is a feasible area or an obstacle area;
[0134] A second map module is used to add distance information of the distance between any grid of the environmental grid map and the nearest obstacle area to form a distance field map;
[0135] An initial path planning module is used to improve the A* algorithm by taking the distance information on the distance field map as a consideration, and perform initial path planning using the improved A* algorithm to obtain an initial global flight path;
[0136] The path optimization expansion module is used to obtain the global flight path belt of the UAV formation by expanding to both sides based on the initial global flight path to complete the global path planning.
[0137] refer to Fig.11 In view of the current situation that the research on trajectory planning algorithms for UAV formations is insufficient, this embodiment proposes a global path planning algorithm for the entire UAV formation and a local trajectory optimization algorithm for the movement of the UAV formation. The method includes rasterizing the global image information collected by the UAV to construct an obstacle map; further generating a distance field of obstacles based on the map information; based on the distance field map, using an improved bidirectional A* search algorithm to obtain a global feasible path, and based on the feasible path and the area information that the A* algorithm has searched, a second path search is performed, and after completing feature extraction, the Bresenham algorithm is used to reconnect the path based on feature points to obtain a better global feasible path; the secondary search path is expanded to obtain a safe path belt for the UAV formation; after completing the global path planning, the final path is extracted, and filtering and sampling are performed in sequence to obtain the key path points of the path, and the sampled path points are optimized locally using the improved minimum jerk algorithm to obtain the motion information of the UAV formation when tracking the global path, so as to provide an acceleration-level control input for the motion control of the UAV formation.
[0138] This embodiment provides a complete solution for the overall motion planning of the UAV formation by adopting a global path planning and local trajectory optimization method for the UAV formation. It enables the UAV formation to quickly perform global optimal path planning according to the existing map during mission execution, as well as local trajectory optimization during the tracking process. The research on the overall path planning and trajectory optimization of the UAV formation is of great significance and can further improve the mission execution safety and operating efficiency of the UAV formation.
[0139] This embodiment can significantly improve the safety, efficiency, adaptability and ability to handle complex tasks of drone formations. This method effectively solves the conflict problem in multi-machine formations through global coordination and real-time local adjustment, enhances the ability to respond to environmental changes, and enables drones to perform diversified tasks more efficiently in vast or complex environments, thereby giving full play to the advantages of formation collaboration.
[0140] It will be easily understood by those skilled in the art that the above description is only a preferred embodiment of the present invention and is not intended to limit the present invention. Any modifications, equivalent substitutions and improvements made within the spirit and principles of the present invention should be included in the protection scope of the present invention.
Claims
1. A path planning method for UAV formation, It is characterized in that include: S1, obtaining an environmental grid map, wherein any grid of the environmental grid map has coordinate information and attribute information, and the attribute information is a feasible area or an obstacle area; S2, adding distance information of the distance between any grid of the environmental grid map and the nearest obstacle area to form a distance field map; S3, taking the distance information on the distance field map as a consideration to improve the A* algorithm, using the improved A* algorithm to perform initial path planning, and obtaining an initial global flight path; S4, based on the initial global flight path, the global flight path belt of the UAV formation is obtained by expanding to both sides to complete the global path planning.
2. The path planning method for UAV formation according to claim 1, It is characterized in that The heuristic function of the improved A* algorithm in S3 is: Where h represents the heuristic function, coord represents the coordinates of the current search point, goalcoord represents the coordinates of the target point, (1) and (2) represent the x-coordinate and y-coordinate respectively, abs represents the absolute value returned, distance_field represents the distance information on the distance field map, and k and m represent the weight correction coefficients of the distance information; And / or, using the improved A* algorithm to perform initial path planning in S3 specifically includes: using the improved A* algorithm in combination with a bidirectional search algorithm to perform initial path planning.
3. The path planning method for UAV formation according to claim 1, It is characterized in that S4 specifically includes: S41, performing secondary path planning based on the search area when the initial global flight path is generated to obtain an optimized global flight path; S42, acquiring a global flight path band of the UAV formation by expanding the optimized global flight path to both sides; Among them, S41 specifically includes: S411, using the grid points in the search area when the initial global flight path is generated as search points; S412, along the initial global flight path, obtaining the number of search points within a preset range around any path point on the initial global flight path; S413, determining a problematic path point on the initial global flight path according to the number of search points within a preset range around any path point; S414, removing problematic path points on the initial global flight path, and obtaining a path segment to be reconnected; S415, using the Bresenham algorithm to perform line of sight detection and reconnection on the path segment to be reconnected, to obtain the optimized global flight path.
4. The path planning method for UAV formation according to claim 3, It is characterized in that S413 specifically includes: Get the ratio of the number of search points in a preset range around any path point to the area of the preset range; When the proportion is greater than or equal to a preset ratio threshold, the corresponding path point is determined to be a problem path point.
5. The path planning method for UAV formation according to claim 4, It is characterized in that The proportion K is specifically: Among them, n F is the number of search points within the preset range around the path point, r s The preset range radius is set in advance.
6. The path planning method for UAV formation according to claim 1, It is characterized in that In S4, the global flight path band of the UAV formation is obtained by expanding to both sides, specifically including: In the search area when the initial global flight path is generated, the distance information of the grid points around the path point is retrieved, and the grid points with distance information greater than a preset distance threshold are incorporated into the path belt to form a global flight path belt.
7. The path planning method for UAV formation according to claim 3, It is characterized in that S4 and later also include: S5, performing filtering and smoothing processing on the optimized global flight path to obtain a global smooth path; S6, performing trajectory planning on the global smooth path, and obtaining speed information of the UAV formation during the flight along the global flight path.
8. The path planning method for UAV formation according to claim 7, It is characterized in that S6 specifically includes: S61, performing sampling processing on all path points on the global smooth path to obtain a sampling point set; S62, allocating the preset total time between any two adjacent sampling points according to the interval between any two adjacent sampling points and the preset total time of the UAV formation flight, and obtaining the allocated time between any two adjacent sampling points; S63, performing trajectory planning for the path between any two adjacent sampling points according to the allocated time between any two adjacent sampling points, and obtaining speed information of the UAV formation between any two adjacent sampling points.
9. The path planning method for UAV formation according to claim 8, It is characterized in that S61 specifically includes: The global smooth path is sampled in sections according to the distance information of the path points on the global smooth path, and the sampling ratio of the path segment with a shorter distance in the distance information is larger.
10. A path planning system for UAV formations. It is characterized in that include: A first map module is used to obtain an environmental grid map, wherein any grid of the environmental grid map has coordinate information and attribute information, and the attribute information is a feasible area or an obstacle area; A second map module is used to add distance information of the distance between any grid of the environmental grid map and the nearest obstacle area to form a distance field map; An initial path planning module is used to improve the A* algorithm by taking the distance information on the distance field map as a consideration, and perform initial path planning using the improved A* algorithm to obtain an initial global flight path; The path optimization expansion module is used to obtain the global flight path belt of the UAV formation by expanding to both sides based on the initial global flight path to complete the global path planning.
Citation Information
Cited By
New energy power station management system based on user feedback
CN119853286A
Unmanned aerial vehicle route smoothing method and system facing grid path
CN121612316A
