Unmanned aerial vehicle flight path planning method and system for unknown multi-target area

Abstract

The invention discloses a multi-target area-oriented unmanned aerial vehicle flight path planning method and system. The method comprises the following steps of: selecting a current global optimal cruise sequence; selecting a target area for heuristic guidance according to the real-time unmanned aerial vehicle node positions of the global optimal cruise sequence, and finding out a path which is optimal in control input and time cost as an initial path; constructing a trajectory probabilistic reasoning model, and executing an unmanned aerial vehicle trajectory optimization algorithm based on probabilistic reasoning; carrying out track refinement on the obtained initial path by using the algorithm; and solving the probabilistic reasoning model of the trajectory to obtain the state xi on the maximum posterior trajectory. Compared with the prior art, the unmanned aerial vehicle flight path planning method for the multi-target area, which is realized by the invention, is combined with a front-end search algorithm and a trajectory optimization algorithm to carry out real-time re-planning so as to ensure that the path of the unmanned aerial vehicle is safe, smooth, dynamic and feasible.

Description

Technical Field

[0001] The present invention belongs to the research field of unmanned aerial vehicle path planning, and in particular relates to a trajectory planning strategy for unmanned aerial vehicles in unknown and complex environments. Background Art

[0002] Due to their small size and high flexibility, drones are widely used in military reconnaissance, civilian package delivery, and aerial photography (Journal: Control and Decision; Authors: Du Yun, Jia Huimin, Shao Shikai, Hao Jing; Publication Year: 2021; Article Title: UAV Route Planning for Multi-Target Reconnaissance Missions; Pages: 1191-1198). With the development of automation technology, computer science, and sensor technology, people expect drones to be able to fly autonomously in unknown and complex environments. In this regard, the research on path planning algorithms is crucial, as it is one of the key technologies to achieve autonomous flight of drones in complex environments.

[0003] In the field of UAV path planning, traditional path planning algorithms and intelligent bionic path planning algorithms have been widely studied. Traditional algorithms include graph search-based A* and D* algorithms, as well as sampling-based RPM and RRT algorithms (Journal: Control and Decision; Author: Zhao Chang, Liu Yungang, Chen Lin, et al.; Publication Year: 2022; Article Title: Current Status and Prospects of Multi-UAV Path Planning for Metaheuristic Algorithms; Pages: 1102-1115). Intelligent bionic algorithms mainly include particle swarm algorithms, genetic algorithms, and ant colony algorithms (Journal: Journal of Mechanical Engineering; Author: Ma Xiaolu, Mei Hong; Publication Year: 2021; Article Title: Global Path Planning of Mobile Robots Based on Improved Potential Field Ant Colony Algorithm; Pages: 19-27). However, these algorithms usually perform path planning for known and relatively simple environments, and most of them are simulated and verified on a two-dimensional plane, and most studies ignore the high-order dynamic constraints of UAVs. Therefore, these algorithms have great limitations when performing path planning in unknown and complex environments.

[0004] The optimization-based path planning algorithm expresses the trajectory in the form of a polynomial or spline function, and constructs mathematical optimization problems such as quadratic programming and nonlinearity according to constraints such as dynamics and obstacles, and obtains the optimal trajectory by solving the optimization problem. The literature (Journal: IEEE Transactions on Robotics; Author: Ding, Wenchao and Gao, Wenliang and Wang, Kaixuan and et al; Publication Year: 2019; Article Title: An Efficient B-Spline-Based Kinodynamic Replanning Framework for Quadrotors; Pages: 1287-1306) adopts a hierarchical framework to cope with the path planning challenges in unknown environments. The front end searches for the initial path through a B-spline-based dynamic search algorithm, and the back end places the control points of the B-spline in the optimal position by constructing a convex quadratic optimization problem to generate a safe and dynamically feasible trajectory. The literature (Journal: IEEE Transactions on Robotics; Author: Zhou, Boyu and Pan, Jie and Gao, Feiand et al; Publication Year: 2021; Article Title: RAPTOR: Robust and Perception-Aware Trajectory Replanning for Quadrotor Fast Flight; Pages: 1992-2009) introduces risk awareness to optimize the trajectory, effectively increasing the visibility of unknown obstacles and the safe reaction distance. Through active planning of yaw motion, this method can explore unknown environments, collect information that is beneficial to flight, and establish an active and safe trajectory planning strategy. The literature (Journal: IEEE Robotics and Automation Letters; Author: Zhou, Xin and Wang, Zhepei and Ye, Hongkai and et al; Publication Year: 2021; Article Title: EGO-Planner: An ESDF-Free Gradient-Based Local Planner for Quadrotors; Pages: 478-485) improves the time efficiency of trajectory planning by designing collision penalty terms and avoiding the use of Euclidean Signed Distance Field (ESDF) for optimization.Although the optimization-based path planning algorithm can generate smooth and safe trajectories and can be deployed on resource-constrained quadrotor drones to conduct flight experiments in unknown complex environments, different trajectory representation methods will affect the quality of the generated trajectory, and representing the trajectory as a high-order curve will increase the complexity of the calculation.

[0005] In recent years, researchers have begun to apply Gaussian Process (GP) samples to represent continuous-time trajectories, and then optimize motion trajectories based on gradients (Conference: 2016 IEEE International Conference on Robotics and Automation (ICRA); Author: Mukadam, Mustafa and Yan, Xinyan and Boots, Byron; Publication Year: 2016; Article Title: Gaussian Process Motion Planning; Pages: 9-15). On this basis, the literature (Conference: Robotics: Science and Systems; Author: Jing Dong AND Mustafa Mukadam AND Frank Dellaert AND et al; Publication Year: 2016; Article Title: Motion Planning as Probabilistic Inference using Gaussian Processes and Factor Graphs; Pages: 1-9) regards motion planning problems as probabilistic inference, and explicitly expresses probabilistic inference problems using factor graphs, and then performs inference through numerical optimization. This type of optimization algorithm can directly sample from GP, reducing the computational complexity of optimization. However, this type of optimization algorithm is currently mainly used in robotic arms. Applying it to the field of drone path planning requires more comprehensive modeling and more efficient solution methods. In solving probabilistic reasoning problems, the Belief Propagation (BP) algorithm (Journal: SIAM Journal on Scientific Computing; Author: Fanaskov, Vladimir; Publication Year: 2022; Article Title: Gaussian Belief Propagation Solvers for Nonsymmetric Systems of Linear Equations; Pages: 77-102) can effectively converge to the accurate marginal probability in the reasoning model by avoiding repeated complex calculations, which provides a new research direction for drone path planning.

[0006] Based on existing research, current path planning algorithms can support drones to plan trajectories from the starting point to the end point in unknown environments. However, in the actual execution of tasks, drones often need to cross multiple designated target areas, which poses new challenges to path planning algorithms.

[0007] The DFS algorithm is a widely used method for traversing or searching a tree or graph structure. The algorithm starts from the starting point, explores deeply along a path until it reaches the end point, then returns to the nearest branch node and explores another unexplored path until all paths are explored.

[0008] The traditional A* algorithm performs node expansion and search based on position information, which may result in the generated path being dynamically unfeasible. Summary of the invention

[0009] In order to overcome the shortcomings of the prior art, the present invention proposes a UAV flight path planning method and system for unknown multi-target areas. Based on the global optimal cruising order obtained by the Depth First Search (DFS) algorithm, a strategy of real-time replanning of the UAV flight path traversing multi-target areas in unknown complex environments is implemented by combining the front-end path search algorithm and the trajectory optimization algorithm.

[0010] In order to achieve the above-mentioned object of the invention, the present invention proposes the following technical solutions:

[0011] The present invention proposes a UAV flight path planning method for an unknown multi-target area, comprising:

[0012] Step 1: Based on the DFS algorithm, N cruise orders of drones in multi-target areas are searched and obtained, and the path cost P of each path in the cruise order is calculated. The cruise order with the smallest total path cost value is selected as the current global optimal cruise order, and the global optimal cruise order is represented as the corresponding drone node position set P = {P s , P1, ..., P n , P g},in and Represent the locations of the starting point, end point and target area respectively. Represents three-dimensional space;

[0013] Step 2: Execute the front-end path search algorithm that crosses multiple target areas, select the target area for heuristic guidance based on the instant drone node position of the global optimal cruise order, and find the optimal path in terms of control input and time cost as the initial path; the specific processing is as follows:

[0014] According to the current drone node, the motion primitives are expanded and used as extension nodes. These extension nodes are the child nodes of the current drone node, and the current drone node is the parent node of the extension node.

[0015] Get the optimal control about the k-axis and the optimal system state The expression is as follows:

[0016]

[0017] Among them, k∈{x,y,z}, x, y, z represent the x, y, z axes in three-dimensional space, p kc and v kc Respectively represent the position and speed of the current expansion node on the k-axis, p kg and v kg Indicates the position and expected speed of the target area or end point on the k-axis. β k represents the intermediate variable, t represents the time;

[0018] Will Summarize the optimal control amount u applied within the UAV flight time T * (t), optimal control quantity u * (t) Make the UAV reach the target point from the current node within time T;

[0019] Calculate the path cost c from the current expansion node to the target point * (T), the expression is as follows:

[0020]

[0021] Find the path cost c * (T) The smallest constant T*, the drone takes T* time to fly to the target point and the cost of this flight process is the smallest, that is, T* satisfies

[0022] Set T = T * , taking the minimum path cost as the heuristic trajectory cost h c , the expression is as follows:

[0023] h c =c * (T * )

[0024] According to the accumulation of all the expansion costs from the starting node to the current node, the expansion cost to child node n is obtained. c The actual path cost g c , the expression is as follows:

[0025]

[0026] Among them, u c represents the control input constant, I represents the number of motion primitives generated from the starting point to the current expansion node, and u ci represents the control input constant for generating the i-th motion primitive, u ci ∈U, ρ represents the time penalty constant;

[0027] According to the heuristic cost h c The target area for heuristic guidance is selected, the UAV performs motion primitive expansion based on the current position, and then calculates the total cost f of each motion primitive c , f c =g c +h c , select the total cost f c The smallest node is expanded until the target point, the selected node is returned, the position data in the node is selected to form a position sequence, and the position sequence {p1, p2…, pn} is used as the initial path;

[0028] Step 3: construct a trajectory probability reasoning model and execute a UAV trajectory optimization algorithm based on probability reasoning; use the algorithm to refine the trajectory of the initial path obtained in step 2; the trajectory probability reasoning model specifically includes:

[0029] Use the initial path guidance likelihood probability distribution function to guide the initial trajectory optimization; design the initial path guidance likelihood probability distribution function P(e PG |x), the expression is as follows:

[0030]

[0031] Among them, p init represents the initial path sequence, p represents the position vector of the trajectory state, ∑ PG represents the covariance matrix;

[0032] Using the obstacle avoidance likelihood probability distribution function P(e obs |x) to keep the trajectory at a safe distance from the obstacle; design the obstacle avoidance likelihood probability distribution function P(e obs |x) is expressed as follows:

[0033]

[0034] Where d represents the preset safety distance between the trajectory and the obstacle, d obs represents the distance between the drone and the obstacle, f obs (x) represents the obstacle avoidance function, p represents the position vector, Σ obs represents the obstacle covariance matrix;

[0035] Using the velocity likelihood probability distribution function P(e v |x) and the acceleration likelihood probability distribution function P(e a |x) makes the speed close to the expected value and the acceleration tends to zero. The design speed likelihood probability distribution function P(e v |x) and the acceleration likelihood probability distribution function P(e a |x), the expression is as follows:

[0036]

[0037] Where v and a represent the velocity and acceleration vectors respectively, v des represents the desired velocity, ||v|| represents the scalar velocity, and f v (x) represents the velocity constraint function, f a (x) represents the acceleration constraint function;

[0038] Step 4: Solve the trajectory probability inference model to obtain the state x on the maximum posterior trajectory i , specifically including the following processing;

[0039] The precision matrix and information vector of the trajectory likelihood probability distribution on the support state are obtained, and the expressions are as follows:

[0040]

[0041] in, represents the covariance matrix, represents the Jacobian transposed matrix, represents the Jacobian matrix of h(x) at the initial position x=x0, h(x) represents the cost function of a given trajectory x, and z represents the expected value of h(x);

[0042] The prior probability distribution of trajectories and the likelihood probability distribution of trajectory interpolation states are binary probability distributions;

[0043] The precision matrix and information vector of the prior probability distribution on the support state are defined as follows

[0044]

[0045] Among them, h(x i ,x i+1 ) represents the i-th state x on a given trajectory i and the i+1th state x i+1 The cost function of represents the prior distribution Jacobian matrix;

[0046] The precision matrix and information vector of the likelihood probability distribution on the interpolation state are obtained as follows:

[0047]

[0048] in, represents the Jacobian matrix representing the likelihood probability distribution of the trajectory interpolation state;

[0049] Get the unary factor on the support state The precision matrix and information vector of are expressed as:

[0050]

[0051] in, represents the total precision matrix of the unary factors, represents the path-guided precision matrix of the univariate factor, represents the speed accuracy matrix of the unary factor, represents the acceleration precision matrix of the unary factor, represents the obstacle avoidance accuracy matrix of the unary factor, represents the total information vector of the unary factor;

[0052] Get the binary factors on the interpolation state The precision matrix and information vector of

[0053]

[0054] in, represents the total precision matrix of the binary factors, represents the path-guided precision matrix for binary factors, represents the speed-accuracy matrix of binary factors, represents the acceleration precision matrix of the binary factors, represents the obstacle avoidance accuracy matrix of binary factors, Represents the prior distribution precision matrix; represents the total information vector of the binary factors (interpolation states);

[0055] Get the state x on the maximum a posteriori trajectory i The posterior probability distribution of is expressed as follows:

[0056]

[0057] Taking the derivative of the probability distribution and setting it equal to zero, the expression is as follows:

[0058]

[0059] Solving this equation yields the state x on the maximum a posteriori trajectory i The expression is:

[0060]

[0061] Among them, Λ i represents the precision matrix, η i Represents an information vector, which contains obstacle avoidance, velocity, acceleration, position and prior information.

[0062] The present invention proposes a UAV flight path planning system for multi-target areas, including a global optimal cruise sequence search module, a front-end path search module, and a UAV trajectory optimization module based on probability reasoning, wherein:

[0063] The global optimal cruise sequence search module searches for N cruise sequences of drones based on the DFS algorithm, calculates the path cost P of each path on the cruise sequence, takes the cruise sequence with the smallest path cost as the global optimal cruise sequence, and represents the global optimal cruise sequence as the corresponding drone node position set P = {P s , P1, ..., P n , P g},in and Represent the locations of the starting point, end point and target area respectively. Represents three-dimensional space;

[0064] The front-end path search module executes a front-end path search algorithm that crosses multiple target areas, selects a target area for heuristic guidance based on the real-time position of the UAV through the front-end path search algorithm, and finds the optimal initial path in terms of control input and time cost;

[0065] The probability-based reasoning UAV trajectory optimization module constructs a trajectory probability reasoning model, executes the probability-based reasoning UAV trajectory optimization algorithm, and uses the algorithm to refine the trajectory of the initial path obtained by the front-end path search module to solve the trajectory probability reasoning model, and obtain the state x on the maximum a posteriori trajectory. i .

[0066] Compared with the prior art, the characteristics and beneficial effects of the present invention are as follows:

[0067] 1) A front-end path search algorithm suitable for crossing multiple target areas is designed. Different from the conventional end-to-end search method, this algorithm uses heuristic guidance to search for the initial path, generates a safe, low-cost and dynamic initial path, and changes the target area for heuristic guidance according to the real-time position of the UAV, achieving higher search efficiency.

[0068] 2) A trajectory optimization algorithm based on probabilistic reasoning is designed. The trajectory is modeled as a GP, thereby converting the trajectory optimization problem into a probabilistic reasoning problem and solving it through factor graphs and BP algorithms. When constructing the probabilistic reasoning model, the initial path, obstacle gradients, and high-order kinematic constraints are fully considered to generate a safe and smooth trajectory;

[0069] 3) A trajectory planning algorithm framework for crossing multiple target areas in unknown complex environments is designed. The framework is based on the DFS algorithm and introduces a designed cost function to determine the optimal cruising sequence;

[0070] 4) Combine the front-end search algorithm with the trajectory optimization algorithm to perform real-time replanning to ensure that the UAV’s path is safe, smooth, and dynamically feasible. BRIEF DESCRIPTION OF THE DRAWINGS

[0071] Figure 1 It is an overall flow chart of the UAV flight path planning method for multiple target areas of the present invention;

[0072] Figure 2 It is a module diagram of the UAV flight path planning system for multiple target areas of the present invention;

[0073] Figure 3 : is a probability model factor graph constructed by the present invention, (a) a probability model factor graph, (b) a simplified version of the probability model factor graph;

[0074] Figure 4 It is a diagram of the UAV experimental platform constructed in a specific embodiment;

[0075] Figure 5 It is the speed curve of the flight experiment;

[0076] Figure 6 It is the acceleration curve of the flight experiment;

[0077] Figure 7 This is the flight experiment trajectory diagram. DETAILED DESCRIPTION

[0078] The present invention first uses a depth-first search (DFS) algorithm to determine the global optimal cruising order through target areas, and then combines a front-end path search algorithm that crosses multiple target areas and a back-end optimization algorithm based on probabilistic reasoning to perform real-time replanning to generate a continuous-time trajectory that is safe, smooth and meets the flight requirements of the UAV.

[0079] The strategy first determines the global optimal cruise sequence through the target areas, and then combines the front-end path search algorithm that crosses multiple target areas with the back-end optimization algorithm based on probabilistic reasoning to perform real-time replanning to generate a continuous-time trajectory that is safe, smooth, and meets the UAV flight requirements.

[0080] The technical solution of the present invention is described in detail below with reference to the accompanying drawings.

[0081] Embodiment 1:

[0082] like Figure 1 As shown, the overall process of the UAV flight path planning method for unknown multi-target areas of the present invention is as follows:

[0083] Step 1: Based on the DFS algorithm, N cruise orders of UAVs in multi-target areas are searched and obtained, and the path cost P of the cruise order is calculated. The cruise order h with the smallest path cost is taken as the global optimal cruise order. Its mathematical expression is the position set P, which is described as follows:

[0084] The cruise sequence refers to the cruise path of the drone from the starting point through n target areas in sequence to the end point. The expression of the cruise sequence N is as follows:

[0085] N=n! (1)

[0086] Calculate the path cost D for each cruise sequence i , the calculation formula is as follows:

[0087]

[0088] in, and They represent the positions of the jth target region and the adjacent target region j+1 in the i-th order respectively;

[0089] The cruise sequence with the smallest path cost value is selected as the current global optimal cruise sequence; the optimal cruise sequence is represented by the corresponding node position set P = {P s , P1, ..., P n , P g},in and Respectively represent the locations of the starting point, end point, and target area;

[0090] Step 2: Execute the front-end path search algorithm across multiple target areas, select the target area for heuristic guidance based on the real-time position of the UAV, and find the optimal path in terms of control input and time cost; specifically, the following processing is included:

[0091] The second-order dynamics model is used to generate motion primitives, and the expression is as follows:

[0092]

[0093] Where x(t) = [p(t) T v(t) T ] T represents the second-order dynamic state of the UAV system at time t, that is, the path to be searched, u(t) = [a x (t) a y (t) a z (t)] T It represents the control input matrix of the UAV at time t, i.e. the acceleration matrix, and They represent the position and speed of the drone at time t, A and B are the state transfer matrix and control input matrix, T represents transposition, a x (t), a y (t), a z (t) represents the acceleration of the drone on the x, y, and z axes at time t, respectively, where each control input component (acceleration component) u i The value of (t) is limited to [-u max u max ]

[0094] The range is set to ensure the feasibility of generating motion primitives. Motion primitives refer to the states reached by the drone in a limited time by the control input.

[0095] In formula (3), the solution expression of the second-order dynamic state equation of the UAV system is as follows:

[0096]

[0097] By giving the initial state x(0) and the control input matrix u(t), the state of the drone at any time t can be accurately derived. Discretize the control input quantity to obtain the control input set For a certain duration τ F Applying these control inputs, we get (2r+1) 3 motion primitives, x(t) represents the path that the front-end search can find;

[0098] The heuristic function has a key impact on the search efficiency and result quality in the front-end path search algorithm of this step. The basic form of the heuristic function can be expressed as

[0099] f c =g c +h c (5)

[0100] Among them, f c represents the total cost, gc represents the actual cost to the current node, h c Represents the estimated heuristic cost from the current node to the goal.

[0101] According to the current drone node, the motion primitives are expanded and used as extension nodes. These extension nodes are the child nodes of the current drone node, and the current drone node is the parent node of the extension nodes.

[0102] Calculate the parent node n parent At time τ F Internal application control input constant u c Expand to child node n c The path cost c(T) is expressed as follows:

[0103]

[0104] The time τ F and the control input constant u c Substitute into (6) to obtain the path cost E from the parent node to the child node c , the expression is as follows:

[0105] E c =(||u c || 2 +ρ)τ F (7)

[0106] According to the accumulation of all the expansion costs from the starting node to the current node, the expansion cost to child node n is obtained. c The actual path cost g c , the expression is as follows:

[0107]

[0108] Among them, u c represents a control input constant belonging to the set U, u c ∈U, I represents the number of motion primitives generated from the starting point to the current expansion node, u ci represents the control input constant for generating the i-th motion primitive, u ci ∈U, ρ represents the time penalty constant;

[0109] Find the optimal path in terms of control input and time. The specific steps are as follows:

[0110] Given the state equation, initial state and target state of the UAV system, the optimal control u can be obtained by using the Pontryagin minimum principle. * (t) and the optimal system state s * The specific expression of (t) is as follows:

[0111]

[0112] Among them, k∈{x,y,z}, x, y, z represent the x, y, z axes in three-dimensional space, p kc and v kc Respectively represent the position and speed of the current expansion node on the k-axis, p kg and v kg Indicates the position and expected speed of the target area or end point on the k-axis. β k represents the intermediate variable, t represents the time;

[0113] Use the control quantity u within time T * (t) can make the UAV reach the target point from the current node. According to equations (6) and (9), the optimal control quantity u is applied within the UAV flight time T. * (t), get the path cost c from the current expansion node to the target point * (T), the expression is as follows:

[0114]

[0115] Find the path cost c in equation (10) * (T) The smallest constant T*, the drone takes T* time to fly to the target point and the cost of this flight process is the smallest, that is, T* satisfies

[0116] T = T * , using the minimum path cost as the heuristic trajectory cost h c , the expression is as follows:

[0117] hc=c * (T * ) (11)

[0118] Heuristic cost h c The target area for heuristic guidance is used to generate the initial path of the current UAV cruise; here, the target area for the current heuristic guidance of the UAV is determined based on the real-time position of the UAV. The UAV expands the motion primitives according to the current position, and then calculates the total cost f of each motion primitive c , f c =g c +h c , select the total cost f c

[0119] The smallest node is expanded until the target point, and the selected nodes are returned. Each node contains the position and speed. This step only takes the position data in the node to form a position sequence, and the position sequence is used as the initial path {p1, p2..., pn});

[0120] The front-end path search algorithm of this step draws on the A* search process and further expands the search space to a two-dimensional space containing position and speed, ensuring the dynamic feasibility of the path. c , find an initial path x that saves control cost and avoids collision while meeting the UAV dynamic constraints 初始 *(t)={p1, p2, ..., p n};

[0121] Step 3: Build a trajectory probability inference model and execute the UAV trajectory optimization algorithm based on probability inference; use the algorithm to refine the initial path obtained in step 2 to find a continuous-time trajectory x that is away from obstacles, satisfies high-order dynamic constraints, is smooth and safe * (t);

[0122] The dynamic model of the drone is transformed into a third-order dynamic model, and the expression is as follows:

[0123]

[0124] Where x′(t)=[p′ T (t), v′ T (t), a′ T (t)] T represents the state of the drone at time t, and (' is added) respectively represent the position, velocity and acceleration of the UAV, u'(t) = [u x (t)u y (t)u z (t)] T represents the control input of the drone;

[0125] The goal of refining the trajectory of this step is the continuous time trajectory x * (t) is transformed into the problem of finding the maximum a posteriori trajectory, which is expressed as follows:

[0126]

[0127] Among them, P(x|e) represents the posterior probability distribution of trajectory X when a given event e occurs, and e represents events such as moving away from obstacles and reaching the desired speed.

[0128] Based on Bayes' theorem, the expression of the posterior probability distribution is as follows:

[0129]

[0130] Among them, P(x) represents the prior probability distribution of a given trajectory x, and P(e i |x) represents the event e under a given trajectory x i The likelihood distribution of occurrence, x * (t) optimizing the trajectory to be obtained for the backend;

[0131] Based on the third-order linear steady-state system model of the UAV, this step models the continuous-time trajectory x(t) as a sample of the vector-valued GP, i.e., x(t)~GP(μ(t), K(t, t′)), and obtains the prior distribution P(x) that encourages trajectory smoothness and zero control input, which is expressed as follows:

[0132]

[0133] Formula (15) can be further simplified to

[0134]

[0135] in, represents two adjacent states x on a continuous-time trajectory sample modeled as a vector-valued GP i and x i+1 The constraints between and Represents the constraints of the start and end states of the trajectory, x i and x i+1 represents the i-th and i+1-th states selected in the trajectory x(t), i∈(0,...,N);

[0136] According to the Markovian nature of the trajectory and the characteristics of sparse GP, time τ∈(t i , t i+1 ) The corresponding trajectory interpolation state x(τ) is expressed as:

[0137] x(τ)=Λ(τ)x i +Ψ(τ)x i+1 (17)

[0138] Among them, the expressions of intermediate variables Λ(τ) and Ψ(τ) are as follows:

[0139]

[0140] Among them, Φ represents the state transfer matrix, Q represents the power spectrum density matrix;

[0141] Design the initial path to guide the likelihood probability distribution function P(e PG |x), the expression is as follows:

[0142]

[0143] Among them, p init represents the initial path sequence, p represents the position vector of the trajectory state, ∑ PG represents the covariance matrix; the initial path likelihood probability distribution function encourages the trajectory to approach the initial path, thereby improving the optimization efficiency; although the initial path may approach an obstacle, it will not collide with it;

[0144] The initial path guide likelihood probability distribution function is used to guide the initial optimization of the trajectory;

[0145] Design the obstacle avoidance likelihood probability distribution function P(e obs |x) is expressed as follows:

[0146]

[0147] Where d represents the preset safety distance between the trajectory and the obstacle, d obs =||pp obs ||, p obs represents the location of the obstacle, f obs (x) represents the obstacle avoidance function, p represents the position vector, Σ obs represents the obstacle covariance matrix;

[0148] Using the obstacle avoidance likelihood probability distribution function P(e obs |x) Keep the track at a safe distance from obstacles;

[0149] At the same time, the design speed likelihood probability distribution function P(e v |x) and the acceleration likelihood probability distribution function P(e a |x), the expression is as follows:

[0150]

[0151] Where v and a represent the velocity and acceleration vectors respectively, v des represents the desired velocity, ||v|| represents the scalar velocity, and f v (x) represents the velocity constraint function, f a (x) represents the acceleration constraint function;

[0152] Using the velocity likelihood probability distribution function P(e v |x) and the acceleration likelihood probability distribution function P(e a |x) makes the velocity close to the expected value and the acceleration tends to zero, ensuring the dynamic constraints and smoothness of the trajectory.

[0153] like Figure 3(a) shows the factor graph of the probability model constructed by the present invention; x0, x1, x2, x3, x4, etc. represent the states on the trajectory, that is, the states obtained by substituting a fixed length of time t into the trajectory x(t), each state including position, velocity and acceleration. Among them, x0 and x4 at the two ends become support states, and the rest x1, x2, x3 in the middle are interpolation nodes or interpolation states. The likelihood probability distribution and the prior probability distribution naturally correspond to the likelihood factor nodes and the prior factor nodes in the factor graph, while the trajectory states serve as variable nodes. The factor graph clearly presents the maximum a posteriori problem, where f p and Represent the prior factors between the initial and final state nodes and the two supporting state nodes, respectively, and represents the likelihood factor of the supporting state node, represents the likelihood factor of the interpolated state between adjacent support state nodes. represents the likelihood factor of the support state, Represents the likelihood factor for interpolating states between adjacent support states.

[0154] here and The definition of

[0155]

[0156]

[0157] like Figure 3 (b) shows a simplified factor graph of the probability model constructed by the present invention. In order to facilitate the subsequent solution of the probability model, the factor graph is further simplified, and the factor nodes in the graph are only represented by unary factors. and binary factors Represents. Unary factor Indicates that the factor here is only related to one state; binary factor The factor here is related to the states of the two nodes before and after.

[0158] Unary Factor The expression is as follows:

[0159]

[0160] Binary Factor The expression is as follows:

[0161]

[0162] Step 4: Solve the probability inference model of the trajectory and obtain the state x on the maximum a posteriori trajectory i ; The specific process is as follows:

[0163] The mathematical general form of a probability distribution is as follows:

[0164]

[0165] Among them, Λ represents the accuracy matrix, η represents the information vector, and x represents the given trajectory;

[0166] For the likelihood probability distribution and the prior probability distribution, we first define a general form as follows:

[0167]

[0168] Where h(x) represents the function of a given trajectory x (x is just a variable), z represents the expected value of h(x), and E(x)

[0169] Indicates intermediate parameters;

[0170] The first-order Taylor expansion of h(x) at x=x0 is as follows:

[0171] h(x)≈h(x0)+J0(x-x0) (28)

[0172] in, represents the Jacobian matrix of h(x) at x=x0;

[0173] Substituting formula (28) into formula (27), we get the standardized form of E(x) as follows:

[0174] E(x)=x T Λx-2η T x (29)

[0175] The precision matrix and information vector of the trajectory likelihood probability distribution on the supporting state node (monad state) are obtained, and the expression is as follows:

[0176]

[0177] in, represents the covariance matrix, represents the Jacobian transposed matrix;

[0178] The trajectory prior probability distribution on the intermediate state node (binary state) and the likelihood probability distribution of the trajectory interpolation state are binary probability distributions;

[0179] Substituting the variables, the precision matrix and information vector of the prior probability distribution are defined as follows

[0180]

[0181] The precision matrix and information vector of the likelihood probability distribution of the interpolation state are obtained as follows:

[0182]

[0183] According to formula (24) to formula (25), we get the factor and The precision matrix and information vector of The precision matrix and information vector of

[0184]

[0185] in, represents the total precision matrix of the univariate factor (support state), represents the path-guided precision matrix of the univariate factor, represents the speed accuracy matrix of the unary factor, represents the acceleration precision matrix of the unary factor, represents the obstacle avoidance accuracy matrix of the unary factor; η here is the same as above changed into information vector, represents the total information vector of the unary factor (support state);

[0186] Binary factors on interpolation states The precision matrix and information vector of

[0187]

[0188] in, represents the total precision matrix of the binary factors (interpolation states), represents the path-guided precision matrix for binary factors, represents the speed-accuracy matrix of binary factors, represents the acceleration precision matrix of the binary factors, represents the obstacle avoidance accuracy matrix of binary factors, represents the prior distribution precision matrix; η here is to change the above into information vector, represents the total information vector of the binary factors (interpolation states);

[0189] According to formula (26), the state x on the maximum a posteriori trajectory is obtained i The posterior probability distribution of is expressed as follows:

[0190]

[0191] Taking the derivative of the probability distribution and setting it equal to zero, the expression is as follows:

[0192]

[0193] Solving this equation yields the state x on the maximum a posteriori trajectory i The expression is

[0194]

[0195] Among them, Λ i represents the precision matrix, η i Represents an information vector, which contains obstacle avoidance, velocity, acceleration, position and prior information.

[0196] Embodiment 2:

[0197] like Figure 2 As shown, the UAV flight path planning system for multiple target areas of the present invention includes a global optimal cruise sequence planning module, a front-end path search module, and a UAV trajectory optimization module based on probability reasoning.

[0198] In this step, the BP algorithm is used to pass messages between the factor nodes and variable nodes in the factor graph to accurately determine the precision matrix Λ i and the information vector η i ; The specific process is as follows:

[0199] Using the BP algorithm, information is transferred from factor nodes to variable nodes, from variable nodes to factor nodes, and information is updated on variable nodes, where the transferred information is Λ and η;

[0200] The information transmission from factor node to variable node includes two steps: first, the factor node receives the information of variable nodes except the target variable node, and then marginalizes the information of other variable nodes; i Connected Factor Nodes Only perform the first step;

[0201] From the factor node Passed to variable node x i Information Expressed as

[0202]

[0203] For binary factor nodes First collect the variables from the neighboring node x i+1 The information of the relay is obtained by r and the information vector η r , then by marginalizing x i+1 To get the value passed to x i The message is expressed as follows:

[0204]

[0205] Among them, when the factor node Collected from x i+1 After obtaining the information of r and the information vector η r , the expression is as follows:

[0206]

[0207] Variable node x i When passing a message to a target factor node, the information of all adjacent factor nodes except the target factor node is first summarized, and then the information is passed to the target factor node. i Factor Node The message passed is defined as

[0208]

[0209] Through the message propagation in the first two steps, the variable node summarizes the information of all adjacent factor nodes to update the precision matrix Λ i and the information vector η i

[0210]

[0211] After several rounds of information dissemination and updating, the information Λ on the variable node can be used to i and η i Calculate the state x on the maximum a posteriori trajectory i , and its marginal posterior distribution.

[0212] like Figure 4 As shown, it is a diagram of a drone experimental platform constructed according to a specific embodiment of the present invention. The drone experimental platform consists of three core components: a sensor device consisting of a camera (Intel RealSense D435i), a drone intelligent computing module (Jetson OrinNX) and a drone flight control module (Pixracer). In order to verify the effectiveness of the algorithm proposed in the present invention, the present invention conducted a flight experiment using the algorithm in an indoor environment. The D435i binocular camera outputs a depth map to assist in three-dimensional environment modeling; the Jetson OrinNX computing board is responsible for performing computing tasks including environmental modeling, global planning, real-time replanning and trajectory tracking control; the Pixracer flight control system is responsible for solving the control instructions from the control module and driving the motor to achieve drone trajectory tracking. The positioning information of the drone is provided by a motion capture system installed indoors, with an update frequency of about 150Hz.

[0213] The horizontal size of the experimental site is 4m×5m. Various obstacles, including straight poles and square plastic mats, are set up in the site and arranged in a random manner. At the same time, the starting point, end point and target area are marked on the ground. The flight mission of the drone is to start from the designated starting point, pass through the target area in the order of small path cost, bypass obstacles, and finally arrive at the end point safely. The horizontal coordinates of the starting point and the end point are (-0.17m, 2.24m) and (1.40m, -2.30m). Considering the size of the experimental site, two circular target areas are set in this experiment, and their center points are located at (1.58m, 0.10m) and (-0.94m, -0.94m) respectively. The radius of each target area is set to 0.5m. The height of the starting point, the midpoint of the target area and the end point is uniformly set to 0.8m, and the expected speed of the drone is set to 0.8m / s.

[0214] like Figure 5 and Figure 6 The dynamic curves of velocity and acceleration during the flight are shown in Table 1. The key data in the flight experiment are summarized. Figure 7 As shown in the figure, it depicts the complete trajectory of the drone in avoiding obstacles, crossing the preset must-go area, and successfully reaching the target destination. In addition, the figure also shows the map modeling, trajectory planning and actual flight of the drone at three key moments during the flight.

[0215] Table 1

[0216] distance(m) time(s) <![CDATA[v ave (m / s)]]> <![CDATA[v max (m / s)]]> <![CDATA[a ave (m / s 2 )]]> <![CDATA[a max (m / s 2 )]]> 8.57 11.99 0.695 0.933 0.596 1.351

[0217] It can be seen from the speed curve that the flight speed of the drone is generally stable at the expected speed of 0.8m / s. The two obvious speed changes during the period reflect the deceleration and yaw operation of the drone when approaching the target area. The acceleration change curve shows that the drone has experienced several significant acceleration and deceleration, which is caused by takeoff and deceleration yaw, while the rest of the time it maintains a low acceleration.

[0218] The experimental data showed that the average speed of the drone reached 0.695m / s, close to the preset expected speed, indicating that the speed can be effectively adjusted during the flight mission. At the same time, the drone showed good acceleration planning, with an average acceleration of 0.596m / s 2 , the maximum acceleration is 1.351m / s 2 , ensuring the stability and safety of the flight. Overall, the algorithm proposed in this invention can effectively guide the UAV to cross multiple target areas in an unknown environment to reach the destination, and plan a safe and smooth flight trajectory in real time.

[0219] In summary, the effectiveness of the algorithm framework is verified by simulation and real-time flight experiments. The algorithm framework proposed in the present invention can generate safe and smooth trajectories in real time and guide the drone to cross multiple target areas and reach the destination safely.

[0220] 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, improvements, etc. made within the spirit and principle of the present invention should be included in the protection scope of the present invention.

Claims

1. A method for planning a UAV flight path in an unknown multi-target area, characterized in that: include: Step 1: Based on the DFS algorithm, N cruise orders of drones in multi-target areas are searched and obtained, and the path cost P of each path in the cruise order is calculated. The cruise order with the smallest total path cost value is selected as the current global optimal cruise order, and the global optimal cruise order is represented as the corresponding drone node position set P = {P s , P1, ..., P n , P g },in and Represent the locations of the starting point, end point and target area respectively. Represents three-dimensional space; Step 2: Execute the front-end path search algorithm that crosses multiple target areas, select the target area for heuristic guidance based on the instant drone node position of the global optimal cruise order, and find the optimal path in terms of control input and time cost as the initial path; the specific processing is as follows: According to the current drone node, the motion primitives are expanded and used as extension nodes. These extension nodes are the child nodes of the current drone node, and the current drone node is the parent node of the extension node. Get the optimal control about the k-axis and the optimal system state The expression is as follows: Among them, k∈{x,y,z}, x, y, z represent the x, y, z axes in three-dimensional space, p kc and v kc Respectively represent the position and speed of the current expansion node on the k-axis, p kg and v kg Indicates the position and expected speed of the target area or end point on the k axis, α k , β k represents the intermediate variable, t represents the time; Will Summarize the optimal control amount u applied within the UAV flight time T * (t), optimal control quantity u * (t) Make the UAV reach the target point from the current node within time T; Calculate the path cost c from the current expansion node to the target point * (T), the expression is as follows: Find the path cost c * (T) The smallest constant T*, the drone takes T* time to fly to the target point and the cost of this flight process is the smallest, that is, T* satisfies Set T = T * , taking the minimum path cost as the heuristic trajectory cost h c , the expression is as follows: h c =c * (T * ) According to the accumulation of all the expansion costs from the starting node to the current node, the expansion cost to child node n is obtained. c The actual path cost g c , the expression is as follows: Among them, u c represents the control input constant, I represents the number of motion primitives generated from the starting point to the current expansion node, and u ci represents the control input constant for generating the i-th motion primitive, u ci ∈U, ρ represents the time penalty constant; According to the heuristic cost h c The target area for heuristic guidance is selected, the UAV performs motion primitive expansion based on the current position, and then calculates the total cost f of each motion primitive c , f c =g c +h c , select the total cost f c The smallest node is expanded until the target point, the selected node is returned, the position data in the node is selected to form a position sequence, and the position sequence {p1, p2…, pn} is used as the initial path; Step 3: construct a trajectory probability reasoning model and execute a UAV trajectory optimization algorithm based on probability reasoning; use the algorithm to refine the trajectory of the initial path obtained in step 2; the trajectory probability reasoning model specifically includes: Use the initial path guidance likelihood probability distribution function to guide the initial trajectory optimization; design the initial path guidance likelihood probability distribution function P(e PG |x), the expression is as follows: Among them, p init represents the initial path sequence, p represents the position vector of the trajectory state, ∑ PG represents the covariance matrix; Using the obstacle avoidance likelihood probability distribution function P(e obs |x) to keep the trajectory at a safe distance from the obstacle; design the obstacle avoidance likelihood probability distribution function P(e obs |x) is expressed as follows: Where d represents the preset safety distance between the trajectory and the obstacle, d obs represents the distance between the drone and the obstacle, f obs (x) represents the obstacle avoidance function, p represents the position vector, Σ obs represents the obstacle covariance matrix; Using the velocity likelihood probability distribution function P(e v |x) and the acceleration likelihood probability distribution function P(e a |x) makes the speed close to the expected value and the acceleration tends to zero. The design speed likelihood probability distribution function P(e v |x) and the acceleration likelihood probability distribution function P(e a |x), the expression is as follows: Where v and a represent the velocity and acceleration vectors respectively, v des represents the desired velocity, ||v|| represents the scalar velocity, and f v (x) represents the velocity constraint function, f a (x) represents the acceleration constraint function; Step 4: Solve the trajectory probability inference model to obtain the state x on the maximum posterior trajectory i , specifically including the following processing; The precision matrix and information vector of the trajectory likelihood probability distribution on the support state are obtained, and the expressions are as follows: in, represents the covariance matrix, represents the Jacobian transposed matrix, represents the Jacobian matrix of h(x) at the initial position x=x0, h(x) represents the cost function of a given trajectory x, and z represents the expected value of h(x); The prior probability distribution of trajectories and the likelihood probability distribution of trajectory interpolation states are binary probability distributions; The precision matrix and information vector of the prior probability distribution on the support state are defined as follows Among them, h(x i ,x i+1 ) represents the i-th state x on a given trajectory i and the i+1th state x i+1 The cost function of represents the prior distribution Jacobian matrix; The precision matrix and information vector of the likelihood probability distribution on the interpolation state are obtained as follows: in, represents the Jacobian matrix representing the likelihood probability distribution of the trajectory interpolation state; Get the unary factor on the support state The precision matrix and information vector of are expressed as: in, represents the total precision matrix of the unary factors, represents the path-guided precision matrix of the univariate factor, represents the speed-accuracy matrix of the unary factors, represents the acceleration precision matrix of the unary factor, represents the obstacle avoidance accuracy matrix of the unary factor, represents the total information vector of the unary factor; Get the binary factors on the interpolation state The precision matrix and information vector of in, represents the total precision matrix of the binary factors, represents the path-guided precision matrix for binary factors, represents the speed-accuracy matrix of binary factors, represents the acceleration precision matrix of the binary factors, represents the obstacle avoidance accuracy matrix of binary factors, Represents the prior distribution precision matrix; represents the total information vector of the binary factors (interpolation states); Get the state x on the maximum a posteriori trajectory i The posterior probability distribution of is expressed as follows: Taking the derivative of the probability distribution and setting it equal to zero, the expression is as follows: Solving this equation yields the state x on the maximum a posteriori trajectory i The expression is: Among them, Λ i represents the precision matrix, η i Represents an information vector, which contains obstacle avoidance, velocity, acceleration, position and prior information.

2. The method for planning a UAV flight path for an unknown multi-target area according to claim 1, characterized in that: The step 2 further includes the following processing: Calculate the parent node n parent At time τ F Internal application control input constant u c Expand to child node n c The trajectory cost c(T) is expressed as follows: Based on formula (6), we get u c represents a control constant belonging to the set U, u c ∈U; The time τ F and u c Substitute into (6), and we get the trajectory cost E from the parent node to the child node: c , the expression is as follows: E c =(||u c || 2 +r)t F According to the accumulation of all the expansion costs from the starting node to the current node, the expansion cost to child node n is obtained. c The actual trajectory cost g c , the expression is as follows: Where I represents the number of motion primitives generated from the starting point to the current expansion node, and u ci ∈U represents the control input constant for generating the i-th motion primitive, and ρ represents the temporal penalty constant.

3. The method for planning a UAV flight path for an unknown multi-target area according to claim 1, characterized in that: in: Unary Factor The expression is as follows: Binary Factor The expression is as follows:

4. A UAV flight path planning system for multiple target areas that implements the UAV flight path planning method for multiple target areas as described in any one of claims 1 to 3, characterized in that: It includes a global optimal cruise sequence search module, a front-end path search module, and a UAV trajectory optimization module based on probability reasoning, among which: The global optimal cruise sequence search module searches for N cruise sequences of drones based on the DFS algorithm, calculates the path cost P of each path on the cruise sequence, takes the cruise sequence with the smallest path cost as the global optimal cruise sequence, and represents the global optimal cruise sequence as the corresponding drone node position set P = {P s , P1, ..., P n , P g },in and Represent the locations of the starting point, end point and target area respectively. Represents three-dimensional space; The front-end path search module executes a front-end path search algorithm that crosses multiple target areas, selects a target area for heuristic guidance based on the real-time position of the UAV through the front-end path search algorithm, and finds the optimal initial path in terms of control input and time cost; The probability-based reasoning UAV trajectory optimization module constructs a trajectory probability reasoning model, executes the probability-based reasoning UAV trajectory optimization algorithm, and uses the algorithm to refine the trajectory of the initial path obtained by the front-end path search module to solve the trajectory probability reasoning model, and obtain the state x on the maximum a posteriori trajectory. i .

Images