CN115981374A - Method, system and electronic device for UAV path planning and tracking control - Google Patents

Abstract

The invention discloses a method, system and electronic equipment for UAV path planning and tracking control, and relates to the technical field of path planning. The UAV state and control amount at discrete points; perform an optimization to obtain an optimized UAV state at each first discrete point to determine the initial path; determine the UAV state, control amount and The discrete time interval of the i-th trajectory segment; perform secondary optimization to obtain the shortest time path and interpolate to obtain a parameterized path; based on the initial first discrete distance and initial path interval, determine the position of the UAV at the ath third discrete point , unmanned aerial vehicle state and control quantity; carry out three optimizations, obtain the trajectory tracking unmanned aerial vehicle state and the trajectory tracking control quantity at each third discrete point; thereby control the flight of the unmanned aerial vehicle. The present invention improves the efficiency of path planning and at the same time ensures that the planned path time is the shortest.

Description

Method, system and electronic device for unmanned aerial vehicle path planning and tracking control

Technical Field

The present invention relates to the technical field of path planning, and in particular to a method, system and electronic equipment for unmanned aerial vehicle path planning and tracking control.

Background Art

Drones are now widely used for reconnaissance, cargo transportation, film photography, search and rescue, and entertainment activities such as drone racing. One of the most prominent drones is the quadcopter, which can perform various maneuvers from smooth flight to complex stunts due to its simplicity and versatility. This makes the quadcopter one of the most flexible and maneuverable aerial robots. In many application scenarios of drones, time-shortest path planning and tracking control are of great significance. For example, when drones perform search and rescue missions, time-shortest path planning allows drones to reach and search disaster areas faster, thereby providing help faster and buying precious time for rescue; when performing reconnaissance and detection missions, time-shortest path planning can ensure that drones complete reconnaissance and detection missions in a short time, thereby reducing the risk of being discovered; in the drone delivery industry, time-shortest path planning that meets the constraints of landmark points allows drones to deliver goods to the destination in the shortest time, thereby improving efficiency and customer satisfaction. In addition, in terms of entertainment, drone racing requires contestants to control drones to race on a specified track and try their best to complete the course in the shortest time. The purpose of drone racing competitions is usually to demonstrate the performance of drones, attract spectators' interest, and promote the development of drone technology.

The time-optimal path planning and tracking of UAVs requires planning the UAV flight path with the shortest time and controlling the UAV to accurately track the path. The method of using polynomial trajectories for trajectory planning cannot fully utilize the motion potential of UAVs due to the inherent smoothness of polynomial trajectories. The method of using numerical optimization for trajectory planning requires allocating landmark points as constraints within a specific discrete time, making it impossible to generate a true time-optimal path. In order to generate a true time-optimal path, Philipp Foehn et al. introduced a complementary constraint formula representing the trajectory flight process in the literature (Time-optimal planning for quadrotor waypoint flight, Science Robotics, 6(56):eabh1221, 2021) to solve the time allocation problem and fully utilize the motion potential of quadrotor UAVs. Although the dynamic model of the UAV and the time optimality of the trajectory are considered, the proposed complementary constraint method increases the difficulty of solving the optimal solution. When the number of landmark point constraints is large, the time-optimal path solution takes more than ten minutes or even hours. When the landmark point position changes, the path cannot be replanned in real time, and the safety of flight cannot be guaranteed. Considering that the generation of the time-optimal path for full quadrotor dynamics is computationally very expensive, taking about several minutes or even hours, Angel Romero et al. introduced a sampling-based method in the literature (Time-Optimal Online Replanning for AgileQuadrotor Flight, arXiv preprint arXiv:2203.09839, 2022.) to effectively generate the time-optimal path of the point mass model, and then use the model prediction trajectory control method to track the path. Although the time-optimal path of the mass model can be solved quickly, the drone dynamics model is replaced by the mass model in the process, sacrificing a certain degree of optimality.

Therefore, the existing methods for UAV path planning and tracking control cannot ensure that the planned path is completed in the shortest time while improving the efficiency of path planning.

Summary of the invention

The purpose of the present invention is to provide a method, system and electronic equipment for unmanned aerial vehicle path planning and tracking control, which can improve the efficiency of path planning while ensuring that the time for planning the path is the shortest.

To achieve the above object, the present invention provides the following solutions:

A method for unmanned aerial vehicle path planning and tracking control, the method comprising:

Determine the locations of N _wp waypoints and the starting point, thereby determining N _wp trajectory segments; N _wp >1;

N first discrete points are determined on each of the trajectory segments, thereby determining N×N _wp first discrete points; N>1;

Initialize the state and control quantity of the drone to obtain the initial state and initial control quantity; the state includes position, velocity and attitude quaternion; the control quantity includes thrust and angular velocity;

Based on the initial state and the initial control amount, determine the drone position of the i×Nth first discrete point, the drone state of the kth first discrete point, and the control amount of the kth first discrete point; i=1,2,…,N _wp ; k=1,2,…,N×N _wp ;

Based on the first constraint condition, the drone states of all first discrete points and the control quantities of all first discrete points are optimized to obtain an optimized drone state and an optimized control quantity of each first discrete point; the first constraint condition is a condition that simultaneously satisfies the first landmark point position constraint, the first drone dynamics constraint and the minimum control constraint, the first landmark point position constraint is a function of the drone position of the i×Nth first discrete point and the i-th landmark point position, the first drone dynamics constraint is a function of the drone state of the kth first discrete point, the drone state of the k-1th first discrete point, the control quantity of the k-1th first discrete point and a preset time interval, and the minimum control constraint is a function of the control quantity of the k-1th first discrete point;

Determine an initial path of the UAV based on an optimized UAV state of all discrete points; the initial path includes N _wp trajectory segments;

Determine N second discrete points on each of the trajectory segments of the initial path, thereby determining N×N _wp second discrete points;

Based on the initial state, the initial control amount, the preset time interval, the once-optimized UAV state of all first discrete points, and the once-optimized control amount of all first discrete points, determine the UAV state of the mth second discrete point, the control amount of the mth second discrete point, and the discrete time interval of the i-th trajectory segment of the initial path; m=1, 2, ..., N;

Based on the second constraint condition, the drone states of all second discrete points, the control quantities of all second discrete points and the discrete time intervals of all trajectory segments of the initial path are optimized to obtain the shortest time drone states of each second discrete point, thereby determining the shortest time path of the drone; the second constraint condition is a condition that simultaneously satisfies the second landmark point position constraint, the second drone dynamics constraint and the shortest time constraint, the second landmark point position constraint is a function of the drone position of the i×Nth second discrete point and the i-th landmark point position, the second drone dynamics constraint is a function of the drone state of the m×ith second discrete point, the drone state of the m×i-1th second discrete point, the control quantity of the m×i-1th second discrete point and the i-th discrete time interval; the shortest time constraint is a function of the second landmark point position constraint and the second drone dynamics constraint;

Performing polynomial interpolation on the shortest time path to obtain a parameterized path;

Determining _NT third discrete points on the parameterized path;

Initialize the first discrete distance to obtain the initial first discrete distance; initialize the path interval to obtain the initial path interval; the ath discrete distance is the distance from the starting point to the ath third discrete point; a=1, 2, ..., _NT ; the ath path interval is the difference between the a+1th discrete distance and the ath discrete distance;

Based on the initial first discrete distance and the initial path interval, determine the drone position of the a-th third discrete point, the drone state of the a-th third discrete point, and the control amount of the a-th third discrete point;

Based on the third constraint condition, the path interval, the first discrete distance, the drone states of all third discrete points and the control quantities of all third discrete points are optimized to obtain the trajectory tracking drone states and trajectory tracking control quantities at each third discrete point; the third constraint condition is a condition that satisfies the distance constraint and the third drone dynamics constraint at the same time, and the distance constraint is a function of the position of the ath third discrete point on the parameterized path, the drone position of the ath third discrete point, the path interval and the first discrete distance; the third drone dynamics constraint is a function of the drone state of the ath third discrete point, the drone state of the a-1th third discrete point, the control quantity of the a-1th third discrete point and the preset time interval;

The UAV flight is controlled based on the trajectory tracking control quantity of all the third discrete points.

Optionally, the expression optimized based on the first constraint condition is:

_{Ulb≤Uk≤Uub ; ​}

Where _Xk is the UAV state of the kth first discrete point, _Uk is the control amount of the kth first discrete point, i is the sequence number of the trajectory segment, P _iN is the UAV position of the i×Nth first discrete point, P _w,i is the position of the i-th landmark point, Xk _-1 is the UAV state of the k-1th first discrete point, Uk _-1 is the control amount of the k-1th first discrete point, f(·) is the relationship function, dt is the preset time interval, R is a real number vector, _Ulb is the lower bound of the UAV control amount, and _Uub is the upper bound of the UAV control amount.

Optionally, the expression optimized based on the second constraint condition is:

_P'iN -Pw _,i = 0;

为第m×i个第二离散点的无人机状态，

为第m×i个第二离散点的控制量，dt_i为第i个所述离散时间间隔，X_mi-1为第m×i-1个第二离散点的无人机状态、第m×i-1个第二离散点的控制量和第i个所述离散时间间隔的函数，

为第m×i-1个第二离散点的控制量，f(·)为关系函数，P‘_iN为第i×N个第二离散点的无人机位置，P_w,i为第i个路标点位置。in,

is the drone state of the m×ith second discrete point,

is the control value of the m×i second discrete point, _dti is the i-th discrete time interval, _Xmi-1 is the function of the UAV state of the m×i-1 second discrete point, the control value of the m×i-1 second discrete point and the i-th discrete time interval,

is the control quantity of the m×i-1th second discrete point, f(·) is the relationship function, _P'iN is the position of the UAV at the i×Nth second discrete point, and _Pw,i is the position of the i-th landmark point.

Optionally, the expression optimized based on the third constraint condition is:

X _a =X _a-1 +f(X _a-1 ,U _a-1 )dt;

_{Ulb≤Ua≤Uub ; ​}

Wherein, _Xa is the state of the UAV at the a-th third discrete point, _Ua is the control amount of the a-th third discrete point, _l1 is the first discrete distance, _ca is the position of the a-th third discrete point on the parameterized path, _Pa is the position of the UAV at the a-th third discrete point, _dlj is the difference between the j+1th discrete distance and the j-th discrete distance, j is the sequence number of the discrete distance, Xa _-1 is the state of the UAV at the a-1th third discrete point, Ua _-1 is the control amount of the a-1th third discrete point, dt is the preset time interval, f(·) is the relationship function, _Ulb is the lower bound of the UAV control amount, and _Uub is the upper bound of the UAV control amount.

Optionally, it is characterized in that the expression of the parameterized path is:

path(l)=(P _a+1 -P _a )(la)+P _a ,l∈((a,a+1);

Among them, path(·) is the parameterized path, Pa ₊₁ is the UAV position of the a+1th third discrete point, _Pa is the UAV position of the ath third discrete point, l is the difference parameter, and a is the sequence number of the third discrete point.

A system for unmanned aerial vehicle path planning and tracking control, the system comprising:

The landmark position and starting point determination module is used to determine N _wp landmark positions and starting points, thereby determining N _wp trajectory segments; N _wp >1;

A first discrete point determination module is used to determine N first discrete points on each of the trajectory segments, thereby determining N×N _wp first discrete points; N>1;

The first initialization module is used to initialize the state and control quantity of the drone to obtain the initial state and initial control quantity; the state includes position, velocity and attitude quaternion; the control quantity includes thrust and angular velocity;

A first state and control amount module, used to determine the drone position of the i×Nth first discrete point, the drone state of the kth first discrete point and the control amount of the kth first discrete point based on the initial state and the initial control amount; i=1,2,…,N _wp ; k=1,2,…,N×N _wp ;

A first optimization module is used to optimize the drone states of all first discrete points and the control quantities of all first discrete points based on a first constraint condition, so as to obtain a first optimized drone state and a first optimized control quantity of each first discrete point; the first constraint condition is a condition that satisfies the first landmark point position constraint, the first drone dynamics constraint and the minimum control constraint at the same time, the first landmark point position constraint is a function of the drone position of the i×Nth first discrete point and the i-th landmark point position, the first drone dynamics constraint is a function of the drone state of the kth first discrete point, the drone state of the k-1th first discrete point, the control quantity of the k-1th first discrete point and a preset time interval, and the minimum control constraint is a function of the control quantity of the k-1th first discrete point;

An initial path determination module is used to determine the initial path of the UAV according to the optimized UAV state of all discrete points; the initial path includes N _wp trajectory segments;

A second discrete point determination module, configured to determine N second discrete points on each of the trajectory segments of the initial path, thereby determining N×N _wp second discrete points;

A second state and control amount determination module is used to determine the drone state of the mth second discrete point, the control amount of the mth second discrete point and the discrete time interval of the i-th trajectory segment of the initial path based on the initial state, the initial control amount, the preset time interval, the once-optimized drone state of all first discrete points, and the once-optimized control amount of all first discrete points; m=1, 2, ..., N;

A second optimization module is used to optimize the drone states of all second discrete points, the control quantities of all second discrete points and the discrete time intervals of all trajectory segments of the initial path based on the second constraint condition, so as to obtain the shortest time drone states of each second discrete point, thereby determining the shortest time path of the drone; the second constraint condition is a condition that satisfies the second landmark point position constraint, the second drone dynamics constraint and the shortest time constraint at the same time, the second landmark point position constraint is a function of the drone position of the i×Nth second discrete point and the i-th landmark point position, the second drone dynamics constraint is a function of the drone state of the m×ith second discrete point, the drone state of the m×i-1th second discrete point, the control quantity of the m×i-1th second discrete point and the i-th discrete time interval; the shortest time constraint is a function of the second landmark point position constraint and the second drone dynamics constraint;

An interpolation module, used for performing polynomial interpolation on the shortest time path to obtain a parameterized path;

A third discrete determination module, configured to determine _NT third discrete points on the parameterized path;

The second initialization module is used to initialize the first discrete distance to obtain the initial first discrete distance; initialize the path interval to obtain the initial path interval; the ath discrete distance is the distance from the starting point to the ath third discrete point; a=1, 2, ..., _NT ; the ath path interval is the difference between the a+1th discrete distance and the ath discrete distance;

A third state and control amount determination module, used to determine the drone position of the ath third discrete point, the drone state of the ath third discrete point, and the control amount of the ath third discrete point based on the initial first discrete distance and the initial path interval;

A third optimization module is used to optimize the path interval, the first discrete distance, the drone states of all third discrete points and the control quantities of all third discrete points based on a third constraint condition, so as to obtain the trajectory tracking drone state and trajectory tracking control quantity at each third discrete point; the third constraint condition is a condition that satisfies both the distance constraint and the third drone dynamics constraint, and the distance constraint is a function of the position of the ath third discrete point on the parameterized path, the drone position of the ath third discrete point, the path interval and the first discrete distance; the third drone dynamics constraint is a function of the drone state of the ath third discrete point, the drone state of the a-1th third discrete point, the control quantity of the a-1th third discrete point and the preset time interval;

The control module is used for controlling the flight of the UAV based on the trajectory tracking control quantity of all the third discrete points.

An electronic device, comprising:

one or more processors;

a storage device having one or more programs stored thereon;

When the one or more programs are executed by the one or more processors, the one or more processors implement the method for drone path planning and tracking control as described above.

The storage device is a readable storage medium.

According to the specific embodiments provided by the present invention, the present invention discloses the following technical effects:

The present invention discloses a method, system and electronic device for unmanned aerial vehicle path planning and tracking control. First, an initial path is determined based on a first landmark point position constraint, a first unmanned aerial vehicle dynamics constraint and a minimum control constraint. Then, the shortest time path of the unmanned aerial vehicle is determined based on a second landmark point position constraint, a second unmanned aerial vehicle dynamics constraint and a shortest time constraint. Compared with the existing path planning method, the method of first optimizing the initial path and then optimizing the shortest time path twice not only realizes the planning of the shortest time path, but also improves the efficiency of path planning. On the basis of the shortest time path, optimization is performed based on the distance constraint and the third unmanned aerial vehicle dynamics constraint, and the control of the unmanned aerial vehicle path tracking is further realized, thereby improving the flight capability of the unmanned aerial vehicle.

BRIEF DESCRIPTION OF THE DRAWINGS

In order to more clearly illustrate the embodiments of the present invention or the technical solutions in the prior art, the drawings required for use in the embodiments will be briefly introduced below. Obviously, the drawings described below are only some embodiments of the present invention. For ordinary technicians in this field, other drawings can be obtained based on these drawings without paying creative labor.

FIG1 is a schematic flow chart of a method for drone path planning and tracking control provided in Example 1 of the present invention;

FIG2 is a flow chart of a method for planning and tracking the shortest path for a UAV when the position of a landmark point changes;

FIG3 is a diagram showing the shortest path planning result in a specific embodiment;

FIG. 4 is a trajectory tracking effect diagram of a specific embodiment.

DETAILED DESCRIPTION

The following will be combined with the drawings in the embodiments of the present invention to clearly and completely describe the technical solutions in the embodiments of the present invention. Obviously, the described embodiments are only part of the embodiments of the present invention, not all of the embodiments. Based on the embodiments of the present invention, all other embodiments obtained by ordinary technicians in this field without creative work are within the scope of protection of the present invention.

The purpose of the present invention is to provide a method, system and electronic equipment for unmanned aerial vehicle path planning and tracking control, aiming to improve the efficiency of path planning while ensuring that the time for planning the path is the shortest.

In order to make the above-mentioned objects, features and advantages of the present invention more obvious and easy to understand, the present invention is further described in detail below with reference to the accompanying drawings and specific embodiments.

Example 1

FIG1 is a flow chart of a method for drone path planning and tracking control provided by Embodiment 1 of the present invention. As shown in FIG1 , the method for drone path planning and tracking control in this embodiment includes:

Step 101: Determine N×N _wp first discrete points.

Step 101 specifically includes:

Step 1011: Determine N _wp waypoint positions and a starting point, thereby determining N _wp trajectory segments; N _wp >1.

Step 1012: Determine N first discrete points on each trajectory segment, thereby determining N×N _wp first discrete points; N>1.

动力学方程还可以表示为

其中，

表示对位置p求导后得到速度v，

表示姿态四元数q的导数是关于姿态四元数q和角速度ω的函数，其中，Λ(·)为四元数乘子，

表示速度v的倒数是关于重力加速度g、无人机的质量m、姿态四元数q和时间T的函数，其中，R(q)为以姿态四元数q为参数的旋转矩阵,

q＝[q_w q_x q_y q_z]。为了实现实际的计算，将无人机连续时间动力学方程进行离散化处理，得到X_k+1＝X_k+f(X_k,U_k)dt_k，其中dt_k为第k时刻的离散时间间隔，X_k和U_k分别为无人机第k时刻的状态与控输入(即控制量)；给定无人机需要经过的路标点位置{P_w,i|i＝1,2,...,N_wp}，N_wp为路标点数量。根据路标点的数量，将轨迹划分为N_wp个轨迹段，每个轨迹段离散为N个点，因此整个轨迹离散点数量为N·N_wp。In fact, the real-time state X(t) of the UAV is obtained by using the UAV onboard sensors and the external motion capture system, and the control quantity of the UAV is U(t); the dynamic equation of the UAV is

The kinetic equation can also be expressed as

in,

It means that the velocity v is obtained by taking the derivative of the position p,

The derivative of the attitude quaternion q is a function of the attitude quaternion q and the angular velocity ω, where Λ(·) is the quaternion multiplier,

The inverse of the velocity v is a function of the gravitational acceleration g, the mass m of the drone, the attitude quaternion q, and the time T, where R(q) is the rotation matrix with the attitude quaternion q as a parameter.

q＝[q _w q _x q _y q _z ]. In order to realize the actual calculation, the UAV continuous time dynamics equation is discretized to obtain X _k+1 ＝X _k +f(X _k ,U _k )dt _k , where dt _k is the discrete time interval at the kth moment, X _k and U _k are the state and control input (i.e., control quantity) of the UAV at the kth moment respectively; given the landmark positions that the UAV needs to pass through {P _w,i |i＝1,2,...,N _wp }, N _wp is the number of landmarks. According to the number of landmarks, the trajectory is divided into N _wp trajectory segments, each of which is discretized into N points, so the number of discrete points of the entire trajectory is N·N _wp .

Step 102: Perform an optimization to determine the initial path of the drone.

Step 102 specifically includes:

Step 1021: Initialize the drone state and control quantity to obtain the initial state and initial control quantity; the state includes position, velocity and attitude quaternion; the control quantity includes thrust and angular velocity.

Step 1022: Based on the initial state and the initial control amount, determine the drone position of the i×Nth first discrete point, the drone state of the kth first discrete point, and the control amount of the kth first discrete point; i=1,2,…,N _wp ; k=1,2,…,N×N _wp .

Step 1023: Based on the first constraint condition, the UAV states of all first discrete points and the control quantities of all first discrete points are optimized to obtain an optimized UAV state and an optimized control quantity of each first discrete point; the first constraint condition is a condition that simultaneously satisfies the first landmark point position constraint, the first UAV dynamics constraint and the minimum control constraint, the first landmark point position constraint is a function of the UAV position of the i×Nth first discrete point and the i-th landmark point position, the first UAV dynamics constraint is a function of the UAV state of the kth first discrete point, the UAV state of the k-1th first discrete point, the control quantity of the k-1th first discrete point and a preset time interval, and the minimum control constraint is a function of the control quantity of the k-1th first discrete point.

Step 1024: Determine the initial path of the UAV based on the optimized UAV states of all discrete points; the initial path includes N _wp trajectory segments.

Step 103: Perform secondary optimization based on the initial path to determine the shortest path for the UAV.

Step 103 specifically includes:

Step 1031: Determine N second discrete points on each trajectory segment of the initial path, thereby determining N×N _wp second discrete points.

Step 1032: Based on the initial state, the initial control amount, the preset time interval, the once-optimized UAV state of all first discrete points, and the once-optimized control amount of all first discrete points, determine the UAV state of the mth second discrete point, the control amount of the mth second discrete point, and the discrete time interval of the i-th trajectory segment of the initial path; m=1, 2,…, N.

Step 1033: Based on the second constraint condition, the UAV state of all second discrete points, the control amount of all second discrete points and the discrete time intervals of all trajectory segments of the initial path are optimized to obtain the shortest time UAV state of each second discrete point, thereby determining the UAV's shortest time path; the second constraint condition is a condition that simultaneously satisfies the second landmark point position constraint, the second UAV dynamics constraint and the shortest time constraint, the second landmark point position constraint is a function of the UAV position of the i×Nth second discrete point and the i-th landmark point position, the second UAV dynamics constraint is a function of the UAV state of the m×ith second discrete point, the UAV state of the m×i-1th second discrete point, the control amount of the m×i-1th second discrete point and the i-th discrete time interval; the shortest time constraint is a function of the second landmark point position constraint and the second UAV dynamics constraint.

Step 104: Perform three optimizations based on the shortest path in time to determine the trajectory tracking control amount of the UAV, thereby controlling the flight of the UAV.

Step 104 specifically includes:

Step 1041: Perform polynomial interpolation on the shortest time path to obtain a parameterized path.

Specifically, due to the disturbances in the actual environment and the modeling errors in the UAV dynamics model, after obtaining the shortest path in time, it is necessary to control the UAV in real time to track the trajectory. In order to ensure the tracking accuracy and maximize the performance of the UAV, the trajectory tracking error and trajectory tracking distance are optimized at the same time, that is, within a fixed optimization time range, the trajectory tracking distance error is minimized and the trajectory tracking distance is maximized. The shortest path in time obtained by the above steps is polynomially interpolated to obtain the path path(l) with l as the parameter variable. The trajectory tracking error is defined as the distance between the UAV position and certain points on the parameterized path. Since path(l) moves forward as l increases, maximizing the trajectory tracking distance is equivalent to maximizing l at the end of the optimization range.

Step 1042: Determine _NT third discrete points on the parameterized path.

Step 1043: Initialize the first discrete distance to obtain the initial first discrete distance; initialize the path interval to obtain the initial path interval; the ath discrete distance is the distance from the starting point to the ath third discrete point; a=1, 2, ..., _NT ; the ath path interval is the difference between the a+1th discrete distance and the ath discrete distance.

Step 1044: Based on the initial first discrete distance and the initial path interval, determine the drone position of the a-th third discrete point, the drone state of the a-th third discrete point, and the control amount of the a-th third discrete point.

Step 1045: Based on the third constraint condition, the path interval, the first discrete distance, the UAV state of all third discrete points and the control amount of all third discrete points are optimized to obtain the trajectory tracking UAV state and trajectory tracking control amount at each third discrete point; the third constraint condition is a condition that satisfies the distance constraint and the third UAV dynamics constraint at the same time, and the distance constraint is a function of the position of the ath third discrete point on the parameterized path, the UAV position of the ath third discrete point, the path interval, and the first discrete distance; the third UAV dynamics constraint is a function of the UAV state of the ath third discrete point, the UAV state of the a-1th third discrete point, the control amount of the a-1th third discrete point and a preset time interval.

Step 1046: Control the flight of the UAV based on the trajectory tracking control quantities of all third discrete points.

Furthermore, as shown in FIG2 , in actual use, the position of the landmark point may change. When the position of the landmark point changes, steps 101 to 104 are re-executed to re-plan the global path. The execution is performed at a fixed frequency, and after each execution, the control amount at the first moment of the optimal control sequence obtained is sent to the UAV for execution, thereby achieving real-time control of the UAV.

As a specific implementation, the expression optimized based on the first constraint condition is:

U _lb ≤U _k ≤U _ub .

Where _Xk is the UAV state of the kth first discrete point, _Uk is the control amount of the kth first discrete point, i is the sequence number of the trajectory segment, P _iN is the UAV position of the i×Nth first discrete point, P _w,i is the position of the i-th landmark point, Xk _-1 is the UAV state of the k-1th first discrete point, Uk _-1 is the control amount of the k-1th first discrete point, f(·) is the relationship function, dt is the preset time interval, R is a real number vector, _Ulb is the lower bound of the UAV control amount, and _Uub is the upper bound of the UAV control amount.

Specifically, in the actual optimization process, the control amount of the first discrete point needs to be initialized, and the control amount of the first discrete point is initialized using the formula X ₀ =X _init , where X ₀ is the control amount of the first discrete point after initialization, and _Xinit is the initialization value of the first discrete point.

As a specific implementation, the expression optimized based on the second constraint condition is:

_P'iN -Pw _,i =0.

为第m×i个第二离散点的无人机状态，

为第m×i个第二离散点的控制量，dt_i为第i个离散时间间隔，

为第m×i-1个第二离散点的无人机状态、第m×i-1个第二离散点的控制量和第i个离散时间间隔的函数，

为第m×i-1个第二离散点的控制量，f(·)为关系函数，P‘_iN为第i×N个第二离散点的无人机位置，P_w,i为第i个路标点位置。in,

is the drone state of the m×ith second discrete point,

is the control quantity of the m×i second discrete point, dt _i is the i-th discrete time interval,

is a function of the UAV state at the m×i-1th second discrete point, the control quantity at the m×i-1th second discrete point, and the i-th discrete time interval,

is the control quantity of the m×i-1th second discrete point, f(·) is the relationship function, P' _iN is the position of the UAV at the i×Nth second discrete point, and P _w,i is the position of the i-th landmark point.

Specifically, in the actual optimization process, the control amount of the second discrete point needs to be initialized, and the control amount of the second discrete point is initialized using the formula _X'0 = _Xinit , where _X'0 is the control amount of the second discrete point after initialization, and _Xinit is the initialization value of the second discrete point (equal to the initialization value of the first discrete point).

As a specific implementation, the expression optimized based on the third constraint condition is:

X _a =X _a-1 +f(X _a-1 ,U _a-1 )dt.

U _lb ≤U _a ≤U _ub .

Wherein, _Xa is the state of the UAV at the a-th third discrete point, _Ua is the control amount of the a-th third discrete point, _l1 is the first discrete path, c _a is the position of the a-th third discrete point on the parameterized path, P _a is the position of the UAV at the a-th third discrete point, dl _j is the difference between the j+1th discrete path and the j-th discrete path, j is the sequence number of the discrete path, Xa _-1 is the state of the UAV at the a-1th third discrete point, Ua _-1 is the control amount of the a-1th third discrete point, dt is the preset time interval, f(·) is the relationship function, _Ulb is the lower bound of the UAV control amount, and _Uub is the upper bound of the UAV control amount.

Specifically. In the actual optimization process, it is also necessary to initialize the control amount of the second discrete point, and use the formula X" ₀ = _Xinit to initialize the control amount of the third discrete point, where X" ₀ is the control amount of the third discrete point after initialization, and _Xinit is the initialization value of the third discrete point (equal to the initialization value of the first discrete point).

As a specific implementation, it is characterized in that the expression of the parameterized path is:

path(l)=(P _a+1 -P _a )(la)+P _a ,l∈((a,a+1).

Among them, path(·) is the parameterized path, Pa ₊₁ is the UAV position of the a+1th third discrete point, _Pa is the UAV position of the ath third discrete point, l is the difference parameter, and a is the sequence number of the third discrete point.

The above method is described below with specific embodiments:

S1. Define the real-time state of the UAV as X = [P ^T V ^T q ^T ] ^T , where P∈R ³ , V∈R ³ , q∈R ⁴ are the position, velocity and attitude quaternion of the UAV respectively; define the control quantity of the UAV as U = [f ω], where f∈R is the thrust of the UAV and ω∈R ³ is the angular velocity of the UAV; given the positions of the landmark points that the UAV needs to pass through {P _w,i |i＝1,2,...,N _wp }, N _wp is the number of landmark points. In this embodiment, N _wp = 7, and the positions of the landmark points are P _w,1 = [14.8715, -0.7906, -2.6] ^T , P _w,2 = [37.0, -12, -0.5] ^T , P _w,3 = [68, -13, 0] ^T , P _w,4 = [95, -8, 0.5] ^T , P _w,5 =[114.5,-36,0] ^T , P _w,6 =[121.5,-67.7,-3.5] ^T , P _w,7 =[121.5,-96,-7] ^T . According to the number of landmark points, the trajectory is divided into N _wp =7 trajectory segments, each of which is discretized into N =20 points. Therefore, the number of discrete points in the entire trajectory is N·N _wp =140.

S2, first calculate the initial path by solving the following optimization problem:

U _lb ≤U _k ≤U _ub .

X ₀ =X _init .

具有如下形式：

In this embodiment, the discrete time interval dt = 0.1, the lower bound of the UAV control quantity U _lb = [-20, -3, -3, -2] ^T , the upper bound of the UAV control quantity U _ub = [0, 3, 3, 2] ^T , the initial state of the UAV _Xinit = [0, 0, 0, 0, 0, 1, 0, 0, 0] ^T , and the UAV dynamics equation

Has the following form:

S3, taking the discrete time interval of each trajectory as the optimization variable {dt _i |i＝1,2,...,7}, directly taking the total time of the trajectory as the optimization target, and taking the initial path solved in S2 as the initial value, that is, solving the following optimization problem, thus obtaining the shortest path in time:

_P'iN -Pw _,i =0.

X' ₀ =X _init .

S4, linearly interpolate the shortest time path obtained in S3 to obtain the path path(l)＝(Pa ₊₁ - _Pa )(la)+ _Pa ,l∈((a,a+1)) with l as the parameter variable. Trajectory tracking control is to solve the following optimization problem:

X _a =X _a-1 +f(X _a-1 ,U _a-1 )dt.

U _lb ≤U _a ≤U _ub .

X” ₀ =X _init .

Wherein, _NT is set to 5. By solving the optimization problem, the optimal trajectory tracking sequence {Xa| _a ＝1,2,...,5} and the optimal control sequence { _Ua |a＝1,2,...,5} can be obtained.

S5, when the position of the landmark point changes, re-execute S3 to re-plan the global path. S4 is executed at a fixed frequency of 50Hz. After each execution, the first moment control quantity _U1 of the optimal control sequence obtained is sent to the UAV for execution, thereby achieving real-time control.

As shown in Figures 3-4, Figure 3 is a diagram of the shortest path planning result in a specific embodiment, in which the three coordinate axes represent the (x, y, z) position of the drone in three-dimensional space, in meters, and the star points represent the landmarks that the drone needs to pass through. The dotted line trajectory is the initial path of the drone obtained by solving in S2, and the solid line trajectory is the shortest path in time obtained by solving in S3; Figure 4 is a trajectory tracking effect diagram of a specific embodiment, in which the horizontal and vertical coordinates represent the x and y coordinates of the drone, in meters, respectively, the solid line trajectory is a section of the shortest path in time obtained by solving in S3, and the star point trajectory is the trajectory tracking result in S4. This embodiment is solved on an ordinary laptop computer, in which the solution time of S2 is 1.2 seconds, the solution time of S3 is 0.4 seconds, and the solution time of S4 is 0.02 seconds.

Example 2

The system for drone path planning and tracking control in this embodiment includes:

The landmark position and starting point determination module is used to determine N _wp landmark positions and starting points, thereby determining N _wp trajectory segments; N _wp >1.

The first discrete point determination module is used to determine N first discrete points on each trajectory segment, thereby determining N×N _wp first discrete points; N>1.

The first initialization module is used to initialize the drone state and control quantity to obtain the initial state and initial control quantity; the state includes position, velocity and attitude quaternion; the control quantity includes: thrust and angular velocity.

The first state and control amount module is used to determine the UAV position of the i×Nth first discrete point, the UAV state of the kth first discrete point and the control amount of the kth first discrete point based on the initial state and the initial control amount; i＝1,2,…,N _wp ; k＝1,2,…,N×N _wp .

The first optimization module is used to optimize the UAV states of all first discrete points and the control quantities of all first discrete points based on the first constraint condition to obtain an optimized UAV state and an optimized control quantity of each first discrete point; the first constraint condition is a condition that simultaneously satisfies the first landmark point position constraint, the first UAV dynamics constraint and the minimum control constraint, the first landmark point position constraint is a function of the UAV position of the i×Nth first discrete point and the i-th landmark point position, the first UAV dynamics constraint is a function of the UAV state of the kth first discrete point, the UAV state of the k-1th first discrete point, the control quantity of the k-1th first discrete point and a preset time interval, and the minimum control constraint is a function of the control quantity of the k-1th first discrete point.

The initial path determination module is used to determine the initial path of the UAV according to the optimized UAV state of all discrete points; the initial path includes N _wp trajectory segments.

The second discrete point determination module is used to determine N second discrete points on each trajectory segment of the initial path, thereby determining N×N _wp second discrete points.

The second state and control amount determination module is used to determine the drone state of the mth second discrete point, the control amount of the mth second discrete point and the discrete time interval of the i-th trajectory segment of the initial path based on the initial state, the initial control amount, the preset time interval, the once-optimized drone state of all first discrete points, and the once-optimized control amount of all first discrete points; m=1,2,…,N.

The second optimization module is used to optimize the UAV state of all second discrete points, the control amount of all second discrete points and the discrete time intervals of all trajectory segments of the initial path based on the second constraint condition, so as to obtain the shortest time UAV state of each second discrete point, thereby determining the shortest time path of the UAV; the second constraint condition is the condition that simultaneously satisfies the second landmark point position constraint, the second UAV dynamics constraint and the shortest time constraint, the second landmark point position constraint is a function of the UAV position of the i×Nth second discrete point and the i-th landmark point position, the second UAV dynamics constraint is a function of the UAV state of the m×ith second discrete point, the UAV state of the m×i-1th second discrete point, the control amount of the m×i-1th second discrete point and the i-th discrete time interval; the shortest time constraint is a function of the second landmark point position constraint and the second UAV dynamics constraint.

The interpolation module is used to perform polynomial interpolation on the shortest time path to obtain a parameterized path.

The third discrete determination module is used to determine _NT third discrete points on the parameterized path.

The second initialization module is used to initialize the first discrete distance to obtain the initial first discrete distance; initialize the path interval to obtain the initial path interval; the ath discrete distance is the distance from the starting point to the ath third discrete point; a=1, 2, ..., _NT ; the ath path interval is the difference between the a+1th discrete distance and the ath discrete distance.

The third state and control amount determination module is used to determine the drone position of the ath third discrete point, the drone state of the ath third discrete point and the control amount of the ath third discrete point based on the initial first discrete distance and the initial path interval.

The third optimization module is used to optimize the path interval, the first discrete distance, the UAV state of all third discrete points and the control amount of all third discrete points based on the third constraint condition, so as to obtain the trajectory tracking UAV state and trajectory tracking control amount at each third discrete point; the third constraint condition is a condition that satisfies the distance constraint and the third UAV dynamics constraint at the same time, and the distance constraint is a function of the position of the ath third discrete point on the parameterized path, the UAV position of the ath third discrete point, the path interval, and the first discrete distance; the third UAV dynamics constraint is a function of the UAV state of the ath third discrete point, the UAV state of the a-1th third discrete point, the control amount of the a-1th third discrete point and a preset time interval.

The control module is used for controlling the flight of the UAV based on the trajectory tracking control quantity of all the third discrete points.

Example 3

An electronic device, comprising:

One or more processors.

A storage device having one or more programs stored thereon.

When one or more programs are executed by one or more processors, the one or more processors implement the method for drone path planning and tracking control as in Example 1.

The storage device is a readable storage medium.

In this specification, each embodiment is described in a progressive manner, and each embodiment focuses on the differences from other embodiments. The same or similar parts between the embodiments can be referred to each other. For the system disclosed in the embodiment, since it corresponds to the method disclosed in the embodiment, the description is relatively simple, and the relevant parts can be referred to the method part.

This article uses specific examples to illustrate the principles and implementation methods of the present invention. The above examples are only used to help understand the method and core ideas of the present invention. At the same time, for those skilled in the art, according to the ideas of the present invention, there will be changes in the specific implementation methods and application scope. In summary, the content of this specification should not be understood as limiting the present invention.

Claims (8)

1. A method for unmanned aerial vehicle path planning and tracking control, characterized in that the method comprises: Determine the locations of N _wp waypoints and the starting point, thereby determining N _wp trajectory segments; N _wp >1; N first discrete points are determined on each of the trajectory segments, thereby determining N×N _wp first discrete points; N>1; Initialize the state and control quantity of the drone to obtain the initial state and initial control quantity; the state includes position, velocity and attitude quaternion; the control quantity includes thrust and angular velocity; Based on the initial state and the initial control amount, determine the drone position of the i×Nth first discrete point, the drone state of the kth first discrete point, and the control amount of the kth first discrete point; i=1,2,…,N _wp ; k=1,2,…,N×N _wp ; Based on the first constraint condition, the drone states of all first discrete points and the control quantities of all first discrete points are optimized to obtain an optimized drone state and an optimized control quantity of each first discrete point; the first constraint condition is a condition that simultaneously satisfies the first landmark point position constraint, the first drone dynamics constraint and the minimum control constraint, the first landmark point position constraint is a function of the drone position of the i×Nth first discrete point and the i-th landmark point position, the first drone dynamics constraint is a function of the drone state of the kth first discrete point, the drone state of the k-1th first discrete point, the control quantity of the k-1th first discrete point and a preset time interval, and the minimum control constraint is a function of the control quantity of the k-1th first discrete point; Determine an initial path of the UAV based on an optimized UAV state of all discrete points; the initial path includes N _wp trajectory segments; Determine N second discrete points on each of the trajectory segments of the initial path, thereby determining N×N _wp second discrete points; Based on the initial state, the initial control amount, the preset time interval, the once-optimized UAV state of all first discrete points, and the once-optimized control amount of all first discrete points, determine the UAV state of the mth second discrete point, the control amount of the mth second discrete point, and the discrete time interval of the i-th trajectory segment of the initial path; m=1, 2, ..., N; Based on the second constraint condition, the drone states of all second discrete points, the control quantities of all second discrete points and the discrete time intervals of all trajectory segments of the initial path are optimized to obtain the shortest time drone states of each second discrete point, thereby determining the shortest time path of the drone; the second constraint condition is a condition that simultaneously satisfies the second landmark point position constraint, the second drone dynamics constraint and the shortest time constraint, the second landmark point position constraint is a function of the drone position of the i×Nth second discrete point and the i-th landmark point position, the second drone dynamics constraint is a function of the drone state of the m×ith second discrete point, the drone state of the m×i-1th second discrete point, the control quantity of the m×i-1th second discrete point and the i-th discrete time interval; the shortest time constraint is a function of the second landmark point position constraint and the second drone dynamics constraint; Performing polynomial interpolation on the shortest time path to obtain a parameterized path; Determining _NT third discrete points on the parameterized path; Initialize the first discrete distance to obtain the initial first discrete distance; initialize the path interval to obtain the initial path interval; the ath discrete distance is the distance from the starting point to the ath third discrete point; a=1, 2, ..., _NT ; the ath path interval is the difference between the a+1th discrete distance and the ath discrete distance; Based on the initial first discrete distance and the initial path interval, determine the drone position of the a-th third discrete point, the drone state of the a-th third discrete point, and the control amount of the a-th third discrete point; Based on the third constraint condition, the path interval, the first discrete distance, the drone states of all third discrete points and the control quantities of all third discrete points are optimized to obtain the trajectory tracking drone states and trajectory tracking control quantities at each third discrete point; the third constraint condition is a condition that satisfies both the distance constraint and the third drone dynamics constraint, and the distance constraint is a function of the position of the ath third discrete point on the parameterized path, the drone position of the ath third discrete point, the path interval and the first discrete distance; the third drone dynamics constraint is a function of the drone state of the ath third discrete point, the drone state of the a-1th third discrete point, the control quantity of the a-1th third discrete point and the preset time interval; The UAV flight is controlled based on the trajectory tracking control quantity of all the third discrete points. 2. The method for unmanned aerial vehicle path planning and tracking control according to claim 1, characterized in that the expression optimized based on the first constraint condition is:

_{Ulb≤Uk≤Uub ; ​} Where _Xk is the UAV state of the kth first discrete point, _Uk is the control amount of the kth first discrete point, i is the sequence number of the trajectory segment, P _iN is the UAV position of the i×Nth first discrete point, P _w,i is the position of the i-th landmark point, Xk _-1 is the UAV state of the k-1th first discrete point, Uk _-1 is the control amount of the k-1th first discrete point, f(·) is the relationship function, dt is the preset time interval, R is a real number vector, _Ulb is the lower bound of the UAV control amount, and _Uub is the upper bound of the UAV control amount. 3. The method for unmanned aerial vehicle path planning and tracking control according to claim 1, characterized in that the expression optimized based on the second constraint condition is:

_P'iN -Pw _,i = 0; in,

is the drone state of the m×ith second discrete point,

is the control quantity of the m×i second discrete point, dt _i is the i-th discrete time interval,

is a function of the UAV state at the m×i-1th second discrete point, the control amount at the m×i-1th second discrete point, and the i-th discrete time interval,

is the control quantity of the m×i-1th second discrete point, f(·) is the relationship function, _P'iN is the position of the UAV at the i×Nth second discrete point, and _Pw,i is the position of the i-th landmark point. 4. The method for unmanned aerial vehicle path planning and tracking control according to claim 1, characterized in that the expression optimized based on the third constraint condition is:

X _a =X _a-1 +f(X _a-1 ,U _a-1 )dt; _{Ulb≤Ua≤Uub ; ​} Wherein, _Xa is the state of the UAV at the a-th third discrete point, _Ua is the control amount of the a-th third discrete point, _l1 is the first discrete distance, _ca is the position of the a-th third discrete point on the parameterized path, _Pa is the position of the UAV at the a-th third discrete point, _dlj is the difference between the j+1th discrete distance and the j-th discrete distance, j is the sequence number of the discrete distance, Xa _-1 is the state of the UAV at the a-1th third discrete point, Ua _-1 is the control amount of the a-1th third discrete point, dt is the preset time interval, f(·) is the relationship function, _Ulb is the lower bound of the UAV control amount, and _Uub is the upper bound of the UAV control amount. 5. The method for unmanned aerial vehicle path planning and tracking control according to claim 1, wherein the expression of the parameterized path is: path(l)=(P _a+1 -P _a )(la)+P _a ,l∈((a,a+1); Among them, path(·) is the parameterized path, Pa ₊₁ is the UAV position of the a+1th third discrete point, _Pa is the UAV position of the ath third discrete point, l is the difference parameter, and a is the sequence number of the third discrete point. 6. A system for unmanned aerial vehicle path planning and tracking control, characterized in that the system comprises: The landmark position and starting point determination module is used to determine N _wp landmark positions and starting points, thereby determining N _wp trajectory segments; N _wp >1; A first discrete point determination module is used to determine N first discrete points on each of the trajectory segments, thereby determining N×N _wp first discrete points; N>1; The first initialization module is used to initialize the state and control quantity of the drone to obtain the initial state and initial control quantity; the state includes position, velocity and attitude quaternion; the control quantity includes thrust and angular velocity; A first state and control amount module, used to determine the drone position of the i×Nth first discrete point, the drone state of the kth first discrete point and the control amount of the kth first discrete point based on the initial state and the initial control amount; i=1,2,…,N _wp ; k=1,2,…,N×N _wp ; A first optimization module is used to optimize the drone states of all first discrete points and the control quantities of all first discrete points based on a first constraint condition, so as to obtain a first optimized drone state and a first optimized control quantity of each first discrete point; the first constraint condition is a condition that satisfies the first landmark point position constraint, the first drone dynamics constraint and the minimum control constraint at the same time, the first landmark point position constraint is a function of the drone position of the i×Nth first discrete point and the i-th landmark point position, the first drone dynamics constraint is a function of the drone state of the kth first discrete point, the drone state of the k-1th first discrete point, the control quantity of the k-1th first discrete point and a preset time interval, and the minimum control constraint is a function of the control quantity of the k-1th first discrete point; An initial path determination module is used to determine the initial path of the UAV according to the optimized UAV state of all discrete points; the initial path includes N _wp trajectory segments; A second discrete point determination module, configured to determine N second discrete points on each of the trajectory segments of the initial path, thereby determining N×N _wp second discrete points; A second state and control amount determination module is used to determine the drone state of the mth second discrete point, the control amount of the mth second discrete point and the discrete time interval of the i-th trajectory segment of the initial path based on the initial state, the initial control amount, the preset time interval, the once-optimized drone state of all first discrete points, and the once-optimized control amount of all first discrete points; m=1, 2, ..., N; A second optimization module is used to optimize the drone states of all second discrete points, the control quantities of all second discrete points and the discrete time intervals of all trajectory segments of the initial path based on the second constraint condition, so as to obtain the shortest time drone states of each second discrete point, thereby determining the shortest time path of the drone; the second constraint condition is a condition that satisfies the second landmark point position constraint, the second drone dynamics constraint and the shortest time constraint at the same time, the second landmark point position constraint is a function of the drone position of the i×Nth second discrete point and the i-th landmark point position, the second drone dynamics constraint is a function of the drone state of the m×ith second discrete point, the drone state of the m×i-1th second discrete point, the control quantity of the m×i-1th second discrete point and the i-th discrete time interval; the shortest time constraint is a function of the second landmark point position constraint and the second drone dynamics constraint; An interpolation module, used for performing polynomial interpolation on the shortest time path to obtain a parameterized path; A third discrete determination module, configured to determine _NT third discrete points on the parameterized path; The second initialization module is used to initialize the first discrete distance to obtain the initial first discrete distance; initialize the path interval to obtain the initial path interval; the ath discrete distance is the distance from the starting point to the ath third discrete point; a=1, 2, ..., _NT ; the ath path interval is the difference between the a+1th discrete distance and the ath discrete distance; A third state and control amount determination module, used to determine the drone position of the ath third discrete point, the drone state of the ath third discrete point, and the control amount of the ath third discrete point based on the initial first discrete distance and the initial path interval; A third optimization module is used to optimize the path interval, the first discrete distance, the drone states of all third discrete points and the control quantities of all third discrete points based on a third constraint condition, so as to obtain the trajectory tracking drone state and trajectory tracking control quantity at each third discrete point; the third constraint condition is a condition that satisfies both the distance constraint and the third drone dynamics constraint, and the distance constraint is a function of the position of the ath third discrete point on the parameterized path, the drone position of the ath third discrete point, the path interval and the first discrete distance; the third drone dynamics constraint is a function of the drone state of the ath third discrete point, the drone state of the a-1th third discrete point, the control quantity of the a-1th third discrete point and the preset time interval; The control module is used for controlling the flight of the UAV based on the trajectory tracking control quantity of all the third discrete points. 7. An electronic device, comprising: one or more processors; a storage device having one or more programs stored thereon; When the one or more programs are executed by the one or more processors, the one or more processors implement the method for drone path planning and tracking control as described in any one of claims 1 to 5. 8 . The electronic device according to claim 7 , wherein the storage device is a readable storage medium.

Images