Method and system for unmanned aerial vehicle (UAV) path planning and tracking control and electronic device

Abstract

Unmanned aerial vehicle (UAV) path planning and tracking control is performed by determining a UAV position at an (i×N)th first discrete point, and a UAV status and a control quantity that are at a kt

Description

TECHNICAL FIELD

[0001] The present disclosure relates to the field of path planning technologies, and in particular, to a method and system for unmanned aerial vehicle (UAV) path planning and tracking control and an electronic device. BACKGROUND

[0002] UAVs are now widely used for reconnaissance, freight transport, cinematography, search and rescue, and recreational activities, for example, UAV racing. One of the most prominent UAVs is a quadrotor aircraft. Because of simplicity and versatility of the quadrotor aircraft, the quadrotor aircraft can implement various maneuvers from smooth flight to complex stunts. This makes the quadrotor aircraft one of the most agile and maneuverable aerial robots. In many application scenarios of UAVs, shortest-time path planning and tracking control are of great significance. For example, when a UAV executes a search and rescue mission, shortest-time path planning can allow the UAV to reach and search a disaster area faster, providing help faster and buying valuable time for rescue. When a UAV executes a reconnaissance and detection mission, shortest-time path planning can ensure that the UAV can complete the reconnaissance and detection mission in a short time, reducing a risk of being spotted. In a UAV delivery industry, shortest-time path planning satisfying a landmark constraint can allow a UAV to successively deliver goods to destinations in a shortest time, improving efficiency and customer satisfaction. In addition, in terms of recreation, in a UAV racing competition, participants are required to control UAVs to race on a prescribed track and try their best to finish the full distance in a shortest time. A purpose of UAV racing competitions is usually to demonstrate performance of UAVs, attract interests of spectators, and promote the development of UAV technologies.

[0003] UAV shortest-time path planning and tracking requires planning a shortest-time UAV flight path and controlling a UAV to precisely track the path. In a method of using a polynomial trajectory for trajectory planning, motion potential of the UAV cannot be fully exploited due to inherent smoothness of the polynomial trajectory. A method of using numerical optimization for trajectory planning requires distributing, in a specific discrete time, a landmark as a constraint, making it impossible to generate a real shortest-time path. To generate the real shortest-time path, Philipp Foehn and others introduced a complementary constraint formula representing a trajectory flight process in the literature (Time-optimal planning for quadrotor waypoint flight, Science Robotics,6(56):eabhl221,2021), which resolves a time allocation problem, and also gives full play to motion potential of a quadrotor UAV. Although a UAV dynamics model and time optimality of a trajectory are considered, a proposed complementary constraint method increases difficulty of solving an optimal solution. When a quantity of landmark constraints is large, it takes more than ten minutes or even several hours to solve a shortest-time path. When a landmark position changes, a path cannot be replanned in real time, which cannot ensure flight safety. Generation of a shortest-time path for full quadrotor dynamics is computationally time-consuming, which takes minutes or even hours. Considering that, Angel Romero and others introduced, in the literature (Time-Optimal Online Replanning for Agile Quadrotor Flight, arXiv preprint arXiv:2203.09839, 2022.), a sampling-based approach to efficiently generate a time-optimal path of a point mass model, which is then tracked by using a method for controlling a model predictive trajectory. Although a shortest-time path of the mass model can be quickly solved, the UAV dynamics model is replaced by the mass model in a process, sacrificing some optimality.

[0004] In conclusion, the existing methods for UAV path planning and tracking control cannot ensure a shortest time for a planned path while improving path planning efficiency. SUMMARY

[0005] The present disclosure aims to provide a method and system for UAV path planning and tracking control and an electronic device, which ensures a shortest time for a planned path while improving path planning efficiency.

[0006] To achieve the above objective, the present disclosure provides the following technical solutions.

[0007] A method for unmanned aerial vehicle (UAV) path planning and tracking control is provided, which includes:

[0008] determining Nwp landmark positions and a starting point, to determine Nwp track segments, where Nwp>l;

[0009] determining N first discrete points on each of the track segments, to determine N*Nwp first discrete points, where N>1;

[0010] initializing a UAV status and a control quantity to obtain an initial status and an initial control quantity, where the status includes a position, a velocity, and an attitude quaternion, and the control quantity includes thrust and an angular velocity;

[0011] determining a UAV position at an (UN)* first discrete point, a UAV status at a kth first discrete point, and a control quantity at the kth first discrete point based on the initial status and the initial control quantity, where i=l, 2, ..., Nwp, and k=l, 2, ..., N><NwP;

[0012] optimizing UAV statuses at all first discrete points and control quantities at all the first discrete points based on a first constraint condition, to obtain a primarily-optimized UAV status and a primarily-optimized control quantity that are at each first discrete point, where the first constraint condition is a condition that satisfies all of a first landmark position constraint, a first UAV dynamics constraint, and a minimum control constraint, the first landmark position constraint is a function about the UAV position at the (i*N)th first discrete point and an 1th landmark position, the first UAV dynamics constraint is a function about the UAV status at the klh first discrete point, a UAV status at a (Ud first discrete point, a control quantity at the (kU)th first discrete point, and a preset time interval, and the minimum control constraint is a function about the control quantity at the (k-l)111 first discrete point;

[0013] determining an initial path of a UAV according to primarily-optimized UAV statuses at all discrete points, where the initial path includes the Nwp track segments;

[0014] determining N second discrete points on each of the track segments of the initial path, to determine NxNwp second discrete points;

[0015] determining a UAV status at an m* second discrete point, a control quantity at the mth second discrete point, and a discrete time interval of an ith track segment of the initial path based on the initial status, the initial control quantity, the preset time interval, the primarily-optimized UAV statuses at all the first discrete points, and the primarily-optimized control quantities at all the first discrete points, where m=l, 2, ..., N;

[0016] optimizing UAV statuses at all second discrete points, control quantities at all the second discrete points, and discrete time intervals of all track segments of the initial path based on a second constraint condition to obtain a shortest-time UAV status at each second discrete point, to determine a shortest-time path of the UAV, where the second constraint condition is a condition that satisfies all of a second landmark position constraint, a second UAV dynamics constraint, and a shortest time constraint, the second landmark position constraint is a function about a UAV position at an (iN)lh second discrete point and an ith landmark position, the second UAV dynamics constraint is a function about a UAV status at an (m*i)th second discrete point, a UAV status at an (m*i-l)th second discrete point, a control quantity at the (mxi-l)th second discrete point, and an ith discrete time interval, and the shortest time constraint is a function about the second landmark position constraint and the second UAV dynamics constraint;

[0017] performing polynomial interpolation on the shortest-time path to obtain a parameterized path;

[0018] determining Nt third discrete points on the parameterized path;

[0019] initializing a first discrete distance to obtain an initial first discrete distance, and initializing a path interval to obtain an initial path interval, where an ath discrete distance is a distance from the starting point to an a* third discrete point, a=l, 2, ..., Nt, and an a"1 path interval is a difference between an (a+1)"' discrete distance and the ath discrete distance;

[0020] determining a UAV position at the ath third discrete point, a UAV status at the ath third discrete point, and a control quantity at the ath third discrete point based on the initial first discrete distance and the initial path interval;

[0021] optimizing the path interval, the first discrete distance, UAV statuses at all third discrete points, and control quantities at all the third discrete points based on a third constraint condition, to obtain a UAV status and a control quantity that are at each third discrete point, where the third constraint condition is a condition that satisfies both a distance constraint and a third UAV dynamics constraint, the distance constraint is a function about a position of the a,h third discrete point on the parameterized path, the UAV position at the a111 third discrete point, the path interval, and the first discrete distance, and the third UAV dynamics constraint is a function about the UAV status at the ath third discrete point, a UAV status at an (a-l)111 third discrete point, a control quantity at the (a-l)th third discrete point, and the preset time interval; and

[0022] controlling flight of the UAV based on the trajectory tracking control quantities at all the third discrete points.

[0023] Optionally, expressions for optimizing based on the first constraint condition are: N-N N-N ny _ + j wy . 1 wp

[0024] * «=i *=i and

[0025] U’b , where

[0026] ^k is the UAV status at the kth first discrete point, ''k is the control quantity at the k* first discrete point, z is a sequence number of a track segment, ?iN is the UAV position at the (ixN)th first discrete point, is the 1th landmark position, is the UAV status at the (k-U first discrete point, is the control quantity at the (kU first discrete point, is a relationship function, is the preset time interval, is a real number vector, ^lb is a lower bound of a UAV control quantity, and is an upper bound of the UAV control quantity.

[0027] Optionally, expressions for optimizing based on the second constraint condition are: min V N -dt X U dt- !

[0028] ” — 1 =i V = X , + f(X ,,U ,)Jr ,

[0029] J v 1 ’,and p / > _ A

[0030] - w ' , where

[0031] is the UAV status at the (mxi)th second discrete point, is a control dt X quantity at the (mxi)th second discrete point, ! is the ith discrete time interval, “■ 1 is the UAV status at the (mxi-l)th second discrete point, is the control quantity at the mrHilh second discrete point, is a relationship function, is the UAV p position at the (ixN)th second discrete point, and H' is the ith landmark position.

[0032] Optionally, expressions for optimizing based on the third constraint condition are: mm V

[0033] ca = path dl.

[0034]

[0035] and

[0036]

[0037] ub, where is the UAV status at the ath third discrete point, a is the control quantity at the ath third discrete point, is the first discrete distance, Ca is the position of the ath p th third discrete point on the parameterized path, a is the UAV position at the a third dl discrete point, 7 is a difference between a (j+1 )lh discrete distance and a jth discrete distance, j is a sequence number of a discrete distance, 1 is the UAV status at the (a-l)th third discrete point, 1 is the control quantity at the (a-1 )th third discrete point, is the preset time interval, is a relationship function, ^,b is a lower bound of U a UAV control quantity, and * is an upper bound of the UAV control quantity.

[0038] Optionally, an expression of the parameterized path is:

[0039] + +

[0040] PatK*}is the parameterized path, is a UAV position at an (a+l)111 third p discrete point, a is the UAV position at the a” third discrete point, ' is a difference parameter, and a is a sequence number of a third discrete point.

[0041] A system for UAV path planning and tracking control is provided, which includes:

[0042] a landmark position and starting point determining module, configured to determine Nwp landmark positions and a starting point, to determine Nwp track segments, where Nwp>l;

[0043] a first discrete point determining module, configured to determine N first discrete points on each of the track segments, to determine NxNwp first discrete points, where N> 1;

[0044] a first initialization module, configured to initialize a UAV status and a control quantity to obtain an initial status and an initial control quantity, where the status includes a position, a velocity, and an attitude quaternion, and the control quantity includes thrust and an angular velocity;

[0045] a first status and control quantity determining module, configured to determine a UAV position at an (i ■ N)th first discrete point, a UAV status at a kth first discrete point, and a control quantity at the kth first discrete point based on the initial status and the initial control quantity, where i=l, 2, ..., Nwp, and k=l, 2, ..., NxNwp;

[0046] a first optimization module, configured to optimize UAV statuses at all first discrete points and control quantities at all the first discrete points based on a first constraint condition, to obtain a primarily-optimized UAV status and a primarily-optimized control quantity that are at each first discrete point, where the first constraint condition is a condition that satisfies all of a first landmark position constraint, a first UAV dynamics constraint, and a minimum control constraint, the first landmark position constraint is a function about the UAV position at the (i*N)th first discrete point and an i* landmark position, the first UAV dynamics constraint is a function about the UAV status at the klh first discrete point, a UAV status at a (H)th first discrete point, a control quantity at the (IvU first discrete point, and a preset time interval, and the minimum control constraint is a function about the control quantity at the (k-l)111 first discrete point;

[0047] an initial path determining module, configured to determine an initial path of a UAV according to primarily-optimized UAV statuses at all discrete points, where the initial path includes the Nwp track segments;

[0048] a second discrete point determining module, configured to determine N second discrete points on each of the track segments of the initial path, to determine NNwp second discrete points;

[0049] a second status and control quantity determining module, configured to determine a UAV status at an mA second discrete point, a control quantity at the mth second discrete point, and a discrete time interval of an ith track segment of the initial path based on the initial status, the initial control quantity, the preset time interval, the primarily-optimized UAV statuses at all the first discrete points, and the primarily-optimized control quantities at all the first discrete points, where m=l, 2, ..., N;

[0050] a second optimization module, configured to optimize UAV statuses at all second discrete points, control quantities at all the second discrete points, and discrete time intervals of all track segments of the initial path based on a second constraint condition to obtain a shortest-time UAV status at each second discrete point, to determine a shortest-time path of the UAV, where the second constraint condition is a condition that satisfies all of a second landmark position constraint, a second UAV dynamics constraint, and a shortest time constraint, the second landmark position constraint is a function about a UAV position at an (i><N)th second discrete point and an ith landmark position, the second UAV dynamics constraint is a function about a UAV status at an (mxi)th second discrete point, a UAV status at an (mxi-1 )th second discrete point, a control quantity at the (mxi-l)* second discrete point, and an i111 discrete time interval, and the shortest time constraint is a function about the second landmark position constraint and the second UAV dynamics constraint;

[0051] an interpolation module, configured to perform polynomial interpolation on the shortest-time path to obtain a parameterized path;

[0052] a third discrete point determining module, configured to determine Nt third discrete points on the parameterized path;

[0053] a second initialization module, configured to initialize a first discrete distance to obtain an initial first discrete distance, and initialize a path interval to obtain an initial path interval, where an ath discrete distance is a distance from the starting point to an ath third discrete point, a=l, 2, ..., Nt, and an ath path interval is a difference between an (a+l)th discrete distance and the ath discrete distance;

[0054] a third status and control quantity determining module, configured to determine a UAV position at the ath third discrete point, a UAV status at the a* third discrete point, and a control quantity at the ath third discrete point based on the initial first discrete distance and the initial path interval;

[0055] a third optimization module, configured to optimize the path interval, the first discrete distance, UAV statuses at all third discrete points, and control quantities at all the third discrete points based on a third constraint condition, to obtain a UAV status and a control quantity that are at each third discrete point, where the third constraint condition is a condition that satisfies both a distance constraint and a third UAV dynamics constraint, the distance constraint is a function about a position of the ath third discrete point on the parameterized path, the UAV position at the ath third discrete point, the path interval, and the first discrete distance, and the third UAV dynamics constraint is a function about the UAV status at the a111 third discrete point, a UAV status at an (a-l)th third discrete point, a control quantity at the (a-1)111 third discrete point, and the preset time interval; and

[0056] a control module, configured to control flight of the UAV based on the trajectory tracking control quantities at all the third discrete points.

[0057] An electronic device is provided, which includes:

[0058] one or more processors; and

[0059] a storage apparatus, configured to store one or more programs, where

[0060] when the one or more programs are executed by the one or more processors, the one or more processors are enabled to implement the above method for UAV path planning and tracking control.

[0061] The storage apparatus is a readable storage medium.

[0062] According to the embodiments of the present disclosure, the present disclosure has the following technical effects:

[0063] The present disclosure discloses a method and system for UAV path planning and tracking control and an electronic device. First, an initial path is determined based on a first landmark position constraint, a first UAV dynamics constraint, and a minimum control constraint. Then, a shortest-time path of a UAV is determined based on a second landmark position constraint, a second UAV dynamics constraint, and a shortest time constraint. In comparison with an existing path planning method, the initial path is first obtained through primary optimization, and the shortest-time path is then determined through secondary optimization. This not only implements shortest-time path planning, but also improves path planning efficiency. On a basis of the shortest-time path, optimization is performed based on a distance constraint and a third UAV dynamics constraint. This further implements UAV path tracking control and improves a flight capability of the UAV. BRIEF DESCRIPTION OF THE DRAWINGS

[0064] To describe the technical solutions in embodiments of the present disclosure or in the conventional technology more clearly, the accompanying drawings required in the embodiments are briefly described below. Apparently, the accompanying drawings in the following description show merely some embodiments of the present disclosure, and other drawings can be derived from these accompanying drawings by those of ordinary skill in the art without creative efforts.

[0065] FIG. 1 is a schematic flowchart of a method for UAV path planning and tracking control according to Embodiment 1 of the present disclosure;

[0066] FIG. 2 is a schematic flowchart of a method for UAV shortest-time path planning and tracking when a landmark position changes;

[0067] FIG. 3 is a diagram of a result of shortest-time path planning in a particular embodiment; and

[0068] FIG. 4 is a diagram of a trajectory tracking effect in a particular embodiment. DETAILED DESCRIPTION OF THE EMBODIMENTS

[0069] The technical solutions of the embodiments of the present disclosure are clearly and completely described below with reference to the accompanying drawings. Apparently, the described embodiments are merely a part rather than all of the embodiments of the present disclosure. All other embodiments obtained by those of ordinary skill in the art based on the embodiments of the present disclosure without creative efforts shall fall within the protection scope of the present disclosure.

[0070] The present disclosure aims to provide a method and system for UAV path planning and tracking control and an electronic device, to ensure a shortest time for a planned path while improving path planning efficiency.

[0071] To make the above objectives, features, and advantages of the present disclosure clearer and more comprehensible, the present disclosure will be further described in detail below with reference to the accompanying drawings and specific implementations.

[0072] Embodiment 1

[0073] FIG 1 is a schematic flowchart of a method for UAV path planning and tracking control according to Embodiment 1 of the present disclosure. As shown in FIG. 1, the method for UAV path planning and tracking control in this embodiment includes:

[0074] Step 101: Determine \ NW|, first discrete points.

[0075] Step 101 specifically includes:

[0076] Step 1011: Determine Nwp landmark positions and a starting point, to determine Nwp track segments, where Nwp>l.

[0077] Step 1012: Determine N first discrete points on each track segment, to determine the N*Nwp first discrete points, where N>1.

[0078] Actually, a real-time status ' ' of a UAV is obtained by using an in-vehicle sensor of the UAV and an external motion capture system, and a control quantity of the UAV is a dynamics equation of the UAV is ^(0-4(^(0^(0) The p = v v = g +—R(q)T dynamics equation may also be expressed as m pv indicates that a velocity v is obtained after derivation is performed on a position p. L - indicates that a derivative of an attitude quaternion q is a function about the attitude quaternion q and an angular velocity ® , where ^(0 js a quaternion multiplier. v = gd—R(q)T m indicates that a derivative of the velocity v is a function about an acceleration of gravity g, a mass m of the UAV, the attitude quaternion q, and time T, where ^(^) is a rotation matrix with the attitude quaternion q as a parameter, R{q) 1-2^2^ 2^-2^¾ 2^+2^ 27, / / . +2^1: 2qyqz ~2qwqx 2qzq2q.q, 2qyqz+2qvqx \ 2q- 2qy and« = k % U To implement an actual calculation, a continuous-time dynamics equation of the UAV is discretized to obtain + / (^^4)^, where ^k is a discrete time interval X u at a kth moment, and k and k respectively are a UAV status and a control input (that is, a control quantity) that are at the kth moment; Landmark positions 1 w’’ ’ ’ that the UAV needs to pass through are given, where is a quantity of landmarks. According to the quantity of landmarks, a trajectory is divided N Ar into • track segments, and each track segment is discretized into A points. In this N-N case, a quantity of discrete points on the entire trajectory is wp .

[0079] Step 102: Perform primary optimization to determine an initial path of the UAV.

[0080] Step 102 specifically includes:

[0081] Step 1021: Initialize a UAV status and a control quantity to obtain an initial status and an initial control quantity, where the status includes a position, a velocity, and an attitude quaternion, and the control quantity includes thrust and an angular velocity.

[0082] Step 1022: Determine a UAV position at an (ixN)*11 first discrete point, a UAV status at a kth first discrete point, and a control quantity at the kth first discrete point based on the initial status and the initial control quantity, where i—1, 2, ..., Nwp, and k=l, 2, ...,NxNwp.

[0083] Step 1023: Optimize UAV statuses at all first discrete points and control quantities at all the first discrete points based on a first constraint condition, to obtain a primarily-optimized UAV status and a primarily-optimized control quantity that are at each first discrete point, where the first constraint condition is a condition that satisfies all of a first landmark position constraint, a first UAV dynamics constraint, and a minimum control constraint, the first landmark position constraint is a function about the UAV position at the (ixN)th first discrete point and an ith landmark position, the first UAV dynamics constraint is a function about the UAV status at the k* first discrete point, a UAV status at a (k-l)* first discrete point, a control quantity at the (k-1 )th first discrete point, and a preset time interval, and the minimum control constraint is a function about the control quantity at the (k-1 )th first discrete point.

[0084] Step 1024: Determine the initial path of the UAV according to primarily-optimized UAV statuses at all discrete points, where the initial path includes the Nwp track segments.

[0085] Step 103: Perform secondary optimization based on the initial path to determine a shortest-time path of the UAV.

[0086] Step 1 03 specifically includes:

[0087] Step 1031: Determine N second discrete points on each track segment of the initial path, to determine NxNwp second discrete points.

[0088] Step 1032: Determine a UAV status at an mlh second discrete point, a control quantity at the mth second discrete point, and a discrete time interval of an i* track segment of the initial path based on the initial status, the initial control quantity, the preset time interval, the primarily-optimized UAV statuses at all the first discrete points, and the primarily-optimized control quantities at all the first discrete points, where m=l, 2, ..., N.

[0089] Step 1033: Optimize UAV statuses at all second discrete points, control quantities at all the second discrete points, and discrete time intervals of all track segments of the initial path based on a second constraint condition to obtain a shortest-time UAV status at each second discrete point, to determine a shortest-time path of the UAV, where the second constraint condition is a condition that satisfies all of a second landmark position constraint, a second UAV dynamics constraint, and a shortest time constraint, the second landmark position constraint is a function about a UAV position at an (kN)4 second discrete point and an ith landmark position, the second UAV dynamics constraint is a function about a UAV status at an (mxi)th second discrete point, a UAV status at an (m i l)'11 second discrete point, a control quantity at the (m / rU second discrete point, and an ilh discrete time interval, and the shortest time constraint is a function about the second landmark position constraint and the second UAV dynamics constraint.

[0090] Step 104: Perform tertiary optimization based on the shortest-time path to determine a control quantity of the UAV, to control flight of the UAV.

[0091] Step 104 specifically includes:

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

[0093] Specifically, due to a disturbance in an actual environment and a modeling error in a UAV dynamics model, after the shortest-time path is obtained, it is necessary to control the UAV in real time to track the trajectory. To ensure tracking precision and maximize performance of the UAV, while optimizing a trajectory tracking error and a trajectory tracking distance, that is, minimizing the trajectory tracking error and maximizing the trajectory tracking distance within a specified optimization time range, polynomial interpolation is performed on the shortest-time path obtained in the above steps to obtain a path Pa^^ with a parameter variable . The trajectory tracking error is defined as distances between a UAV position and some points on the parameterized path. Because Pa^^ moves forward with an increase of a maximum trajectory tracking distance is equivalent to a maximum at an end point of an optimized range.

[0094] Step 1042: Determine Nt third discrete points on the parameterized path.

[0095] Step 1043: Initialize a first discrete distance to obtain an initial first discrete distance, and initialize a path interval to obtain an initial path interval, where an a* discrete distance is a distance from the starting point to an ath third discrete point, a=l, 2, ..., Nt, and an ath path interval is a difference between an (a+l)th discrete distance and the ath discrete distance.

[0096] Step 1044: Determine a UAV position at the aA third discrete point, a UAV status at the ath third discrete point, and a control quantity at the a* third discrete point based on the initial first discrete distance and the initial path interval.

[0097] Step 1045: Optimize the path interval, the first discrete distance, UAV statuses at all third discrete points, and control quantities at all the third discrete points based on a third constraint condition, to obtain a UAV status and a control quantity that are at each third discrete point, where the third constraint condition is a condition that satisfies both a distance constraint and a third UAV dynamics constraint, the distance constraint is a function about a position of the ath third discrete point on the parameterized path, the UAV position at the ath third discrete point, the path interval, and the first discrete distance, and the third UAV dynamics constraint is a function about the UAV status at the ath third discrete point, a UAV status at an (a^l)A third discrete point, a control quantity at the (aT third discrete point, and the preset time interval.

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

[0099] Further, as shown in FIG. 2, a landmark position may change in actual use. When a change in the landmark position is detected, steps 101 to steps 104 are re-executed to re-plan a global path. In addition, execution is performed at a fixed frequency. After each execution, a control quantity that is at a first moment and that is of an optimal control sequence obtained through solving is sent to the UAV for execution, thereby achieving real-time control of the UAV.

[0100] As a specific implementation, expressions for optimizing based on the first constraint condition are: Z k»* Z IKI

[0101] ‘ ‘ '=1 , and

[0102] U‘b ~ Uk ~ Uub, where X u

[0103] k is the UAV status at the k"' first discrete point, k is the control quantity P at the kth first discrete point, 1 is a sequence number of a track segment, ,N is the UAV P X position at the (i*N)th first discrete point, is the 1th landmark position, k~x is the UAV status at the (k-l)th first discrete point, is the control quantity at the (k-l)th first discrete point, is a relationship function, is the preset time interval, R is U U a real number vector, lb is a lower bound of a UAV control quantity, and 1 / 6 is an upper bound of the UAV control quantity.

[0104] Specifically, in an actual optimization process, it is also necessary to initialize the control quantity at the first discrete point. A formula - ^mt is used to initialize X the control quantity at the first discrete point, where 0 is an initialized control quantity at the first discrete point, and is an initialization value at the first discrete point.

[0105] As a specific implementation, expressions for optimizing based on the second constraint condition are: min V N • dt. Y T j 1

[0106] / =1 X = X , + f(X ..U Adt. ,

[0107] m‘ ‘ ,and

[0108] P -P,, =0, where

[0109] is the UAV status at the (mxi)th second discrete point, is a control dt X quantity at the (mxi)lh second discrete point, 1 is the ith discrete time interval, "K is the UAV status at the (m *i— I )111 second discrete point, “■ 1 is the control quantity at the (m i d)111 second discrete point, is a relationship function, P' is the UAV position at the (iN)'11 second discrete point, and M' is the ith landmark position.

[0110] Specifically, in an actual optimization process, it is also necessary to initialize X = X the control quantity at the second discrete point. A formula » init is used to initialize the control quantity at the second discrete point, where « is an initialized X control quantity at the second discrete point, and iM is an initialization value at the second discrete point (which is equal to the initialization value at the first discrete point). [OlH] As a specific implementation, expressions for optimizing based on the third constraint condition are: min

[0112] ( ca = path dl.

[0113] <=i ),

[0114] + Z )dl, and

[0115] Uib^Ua^1ub , where

[0116] X- is the UAV status at the ath third discrete point, is the control quantity at the ath third discrete point, ' is the first discrete distance, C“ is the position of the ath P third discrete point on the parameterized path, a is the UAV position at the a'1 third dl discrete point, 7 is a difference between a (j+i)th discrete distance and a j,h discrete distance, j is a sequence number of a discrete distance, X“ ] is the UAV status at the (a-l)th third discrete point, 1 is the control quantity at the (a-1 )th third discrete point, is the preset time interval, is a relationship function, is a lower bound of U a UAV control quantity, and ’* is an upper bound of the UAV control quantity.

[0117] Specifically, in an actual optimization process, it is also necessary to initialize the control quantity at the third discrete point. A formula ^init is used to initialize x" the control quantity at the third discrete point, where « is an initialized control quantity at the third discrete point, and is an initialization value at the third discrete point (which is equal to the initialization value at the first discrete point).

[0118] As a specific implementation, an expression of the parameterized path is: miin. path(l) = (PA-P }(l-a) + P , I e ((a,a + l) ,

[0119] \ / \ au ><>- \ \ where

[0120] Path^ is the parameterized path, is a UAV position at an (a+l)th third discrete point, ?a is the UAV position at the ath third discrete point, is a difference parameter, and a is a sequence number of a third discrete point.

[0121] The following describes the foregoing method by using particular embodiments: A = Fp7 VT aT T

[0122] SI: Define a real-time status of the UAV as L -I , where P ^R3, P an(j q e A reSpecpveiy are the position, the velocity, and the attitude quaternion of the UAV; define the control quantity of the UAV as , where ■' is the thrust of the UAV, and ^^R is the angular velocity of the UAV; give ip i / = i,2,...,n | the landmark positions 1 ’ wp 7 that the UAV needs to pass through, where vp is the quantity of landmarks, and in this embodiment, and the landmark P , = [14.8715,-0.7906,-2.6]r P „ = [37.0,-12, -0.5f positions respectively are L J , M- L J , |-< -13. Of / >„4 =[95,-8, 0.5f / >,„=[114.5,-36, of / >.„=[1215, -67.7, -3.5f and ”7 L ’ ’J . According to the quantity of landmarks, a trajectory is divided into wp track segments, and each track segment is discretized into A = 20 points. In this case, a quantity of discrete points on the entire . N-N =140 trajectory is wp

[0123] S2: First calculate an initial path by solving the following optimization problems: ^wp ^'^wp Q ^Wp S||^V — ^,|2 + IIML

[0124] / =i fc=i ' k=i

[0125] U^<Uk<Uub

[0126] Xo = XMt

[0127] In this embodiment, the discrete time interval ° -1, the lower bound of the _r_20_j _2 _21^ UAV control quantity ® ’ ’ ’ -I , the upper bound of the UAV control . t / 6 =[0,3,3,2f . . .. , + TTAW X =(0,0,0,0,0,0,1,0,0,of quantity ub L J , the initial status of the UAV L J , and the UAV dynamics equation fopowjng form; p = v • 1 A / \F0 2 co

[0128] S3 : Use a discrete time interval of each track segment as an optimization i dt I / 1 2 7 < variable * '1 == - = / , directly use a total time of the trajectory as an optimization target, and use the initial path solved in S2 as an initial value, that is, the following optimization problems are solved to obtain the shortest-time path: min V N -dt

[0129]

[0130] xm. = xm,-\^ f<xm^um,-ddh

[0131]

[0132] x'<, =

[0133] S4: Perform linear interpolation on the shortest-time path obtained in S3 to obtain a path Pat^^ a) + ^a , / g ((a,a + l) a parameter variable ; and solve the following optimization problems for trajectory tracking control:

[0134] mm X<MXdlj a-1 = path / , + dl

[0135] I j=i

[0136] Xa = Xa^ + f(X_, Ua^ )dt

[0137] Ulb<Ua<Uub

[0138] X — ^init

[0139] Nt =:is set. By solving the optimization problems, an optimal trajectory . • {X \ a =1,2,...,5} , ., tracking sequence ’ and an optimal control sequence {Ua |« =1,2,...,5} can be obtained.

[0140] S5 : When a change in the landmark position is detected, re-execute S3 to replan a global path. S4 is executed at a fixed frequency of 50 Hz. After each execution, the control quantity that is at the first moment and that is of the optimal control sequence obtained through solving is sent to the UAV for execution, thereby achieving real-time control.

[0141] FIG. 3 is a diagram of a result of shortest-time path planning according to a particular embodiment. In the figure, three coordinate axes respectively represent (x, y, z) positions of the UAV in three-dimensional space, a unit is "meter", star points represent landmarks that the UAV needs to pass through, a dotted trajectory is the initial path of the UAV that is obtained through solving in S2, and a solid trajectory is the shortest-time path obtained through solving in S3. FIG. 4 is a diagram of a trajectory tracking effect according to a particular implementation. In the figure, horizontal and vertical coordinates respectively represent x and y coordinates of the UAV, a unit is "meter", a solid trajectory is a section of the shortest-time path obtained through solving in S3, and a star point trajectory is a trajectory tracking result in S4. In this embodiment, the solution is performed on an ordinary notebook computer, a solution time of S2 is 1.2 seconds, a solution time of S3 is 0.4 seconds, and a solution time of S4 is 0.02 seconds.

[0142] Embodiment 2

[0143] A system for UAV path planning and tracking control in this embodiment includes:

[0144] a landmark position and starting point determining module, configured to determine Nwp landmark positions and a starting point, to determine Nwp track segments, where Nwp>l;

[0145] a first discrete point determining module, configured to determine N first discrete points on each track segment, to determine N*Nwp first discrete points, where N>1;

[0146] a first initialization module, configured to initialize a UAV status and a control quantity to obtain an initial status and an initial control quantity, where the status includes a position, a velocity, and an attitude quaternion, and the control quantity includes thrust and an angular velocity;

[0147] a first status and control quantity determining module, configured to determine a UAV position at an (i*N)th first discrete point, a UAV status at a kth first discrete point, and a control quantity at the kth first discrete point based on the initial status and the initial control quantity, where i=l, 2, ..., Nwp, and k=l, 2, ..., N*Nwp;

[0148] a first optimization module, configured to optimize UAV statuses at all first discrete points and control quantities at all the first discrete points based on a first constraint condition, to obtain a primarily-optimized UAV status and a primarily-optimized control quantity that are at each first discrete point, where the first constraint condition is a condition that satisfies all of a first landmark position constraint, a first UAV dynamics constraint, and a minimum control constraint, the first landmark position constraint is a function about the UAV position at the (i*N)th first discrete point and an ith landmark position, the first UAV dynamics constraint is a function about the UAV status at the kth first discrete point, a UAV status at a (k-l)th first discrete point, a control quantity at the (k-l)111 first discrete point, and a preset time interval, and the minimum control constraint is a function about the control quantity at the (k-l)*11 first discrete point;

[0149] an initial path determining module, configured to determine an initial path of a UAV according to primarily-optimized UAV statuses at all discrete points, where the initial path includes the Nwp track segments;

[0150] a second discrete point determining module, configured to determine N second discrete points on each track segment of the initial path, to determine N><Nwp second discrete points;

[0151] a second status and control quantity determining module, configured to determine a UAV status at an m111 second discrete point, a control quantity at the mth second discrete point, and a discrete time interval of an i'h track segment of the initial path based on the initial status, the initial control quantity, the preset time interval, the primarily-optimized UAV statuses at all the first discrete points, and the primarily-optimized control quantities at all the first discrete points, where m=l, 2, ..., N;

[0152] a second optimization module, configured to optimize UAV statuses at all second discrete points, control quantities at all the second discrete points, and discrete time intervals of all track segments of the initial path based on a second constraint condition to obtain a shortest-time UAV status at each second discrete point, to determine a shortest-time path of the UAV, where the second constraint condition is a condition that satisfies all of a second landmark position constraint, a second UAV dynamics constraint, and a shortest time constraint, the second landmark position constraint is a function about a UAV position at an (i N)"1 second discrete point and an ilh landmark position, the second UAV dynamics constraint is a function about a UAV status at an (mxi)th second discrete point, a UAV status at an (mxi-l)th second discrete point, a control quantity at the (mxi-l)111 second discrete point, and an 1th discrete time interval, and the shortest time constraint is a function about the second landmark position constraint and the second UAV dynamics constraint;

[0153] an interpolation module, configured to perform polynomial interpolation on the shortest-time path to obtain a parameterized path;

[0154] a third discrete point determining module, configured to determine Nt third discrete points on the parameterized path;

[0155] a second initialization module, configured to initialize a first discrete distance to obtain an initial first discrete distance, and initialize a path interval to obtain an initial path interval, where an ath discrete distance is a distance from the starting point to an ath third discrete point, a=l, 2, ..., Nt, and an ath path interval is a difference between an (a+l)111 discrete distance and the ath discrete distance;

[0156] a third status and control quantity determining module, configured to determine a UAV position at the ath third discrete point, a UAV status at the a* third discrete point, and a control quantity at the ath third discrete point based on the initial first discrete distance and the initial path interval;

[0157] a third optimization module, configured to optimize the path interval, the first discrete distance, UAV statuses at all third discrete points, and control quantities at all the third discrete points based on a third constraint condition, to obtain a UAV status and a control quantity that are at each third discrete point, where the third constraint condition is a condition that satisfies both a distance constraint and a third UAV dynamics constraint, the distance constraint is a function about a position of the a111 third discrete point on the parameterized path, the UAV position at the ath third discrete point, the path interval, and the first discrete distance, and the third UAV dynamics constraint is a function about the UAV status at the ath third discrete point, a UAV status at an (a^ l)th third discrete point, a control quantity at the (a-l)1h third discrete point, and the preset time interval; and

[0158] a control module, configured to control flight of the UAV based on the trajectory tracking control quantities at all the third discrete points.

[0159] Embodiment 3

[0160] An electronic device includes:

[0161] one or more processors; and

[0162] a storage apparatus, configured to store one or more programs, where

[0163] when the one or more programs are executed by the one or more processors, the one or more processors are enabled to implement the method for UAV path planning and tracking control according to Embodiment 1.

[0164] The storage apparatus is a readable storage medium.

[0165] Each embodiment of the present specification is described in a progressive manner, each embodiment focuses on the difference from other embodiments, and the same and similar parts between the embodiments may refer to each other. Since the system disclosed in an embodiment corresponds to the method disclosed in another embodiment, the description is relatively simple, and reference can be made to the method description. Specific examples are used herein to explain the principles and implementations of the present disclosure. The foregoing description of the embodiments is merely intended to help understand the method of the present disclosure and its core ideas; besides, various modifications may be made by those of ordinary skill in the art to specific implementations and the scope of application in accordance with the ideas of the present disclosure. In conclusion, the content of the present specification shall not be construed as limitations to the present disclosure.

Claims

1. A method for unmanned aerial vehicle (UAV) path planning and tracking control, comprising:determining Nwp landmark positions and a starting point, to determine Nwp track segments, wherein Nwp>l;determining N first discrete points on each of the track segments, to determine NxNwp first discrete points, wherein N> 1;initializing a UAV status and a control quantity to obtain an initial status and an initial control quantity, wherein the status comprises a position, a velocity, and an attitude quaternion, and the control quantity comprises thrust and an angular velocity;determining a UAV position at an (i><N)th first discrete point, a UAV status at a kth first discrete point, and a control quantity at the kth first discrete point based on the initial status and the initial control quantity, wherein i=l, 2, ..., Nwp, and k=l, 2, ..., NxNwP;optimizing UAV statuses at all first discrete points and control quantities at all the first discrete points based on a first constraint condition, to obtain a primarily-optimized UAV status and a primarily-optimized control quantity that are at each first discrete point, wherein the first constraint condition is a condition that satisfies all of a first landmark position constraint, a first UAV dynamics constraint, and a minimum control constraint, the first landmark position constraint is a function about the UAV position at the (ixN)th first discrete point and an ith landmark position, the first UAV dynamics constraint is a function about the UAV status at the kth first discrete point, a UAV status at a (k-l)th first discrete point, a control quantity at the (k-l),h first discrete point, and a preset time interval, and the minimum control constraint is a function about the control quantity at the (k l)"' first discrete point;determining an initial path of a UAV according to primarily-optimized UAV statuses at all discrete points, wherein the initial path comprises the Nwp track segments;determining N second discrete points on each of the track segments of the initial path, to determine NxNwp second discrete points;determining a UAV status at an m* second discrete point, a control quantity at the mth second discrete point, and a discrete time interval of an i* track segment of theinitial path based on the initial status, the initial control quantity, the preset time interval, the primarily-optimized UAV statuses at all the first discrete points, and the primarily-optimized control quantities at all the first discrete points, wherein m=l, 2, ...,N;optimizing UAV statuses at all second discrete points, control quantities at all the second discrete points, and discrete time intervals of all track segments of the initial path based on a second constraint condition to obtain a shortest-time UAV status at each second discrete point, to determine a shortest-time path of the UAV, wherein the second constraint condition is a condition that satisfies all of a second landmark position constraint, a second UAV dynamics constraint, and a shortest time constraint, the second landmark position constraint is a function about a UAV position at an (i*N)th second discrete point and an ith landmark position, the second UAV dynamics constraint is a function about a UAV status at an (mxi)th second discrete point, a UAV status at an (m / p^ second discrete point, a control quantity at the (mxUl)th second discrete point, and an ith discrete time interval, and the shortest time constraint is a function about the second landmark position constraint and the second UAV dynamics constraint;performing polynomial interpolation on the shortest-time path to obtain a parameterized path;determining Nt third discrete points on the parameterized path;initializing a first discrete distance to obtain an initial first discrete distance, and initializing a path interval to obtain an initial path interval, wherein an a1h discrete distance is a distance from the starting point to an ath third discrete point, a=l, 2, ..., Nt, and an ath path interval is a difference between an (a+l),h discrete distance and the ath discrete distance;determining a UAV position at the ath third discrete point, a UAV status at the a* third discrete point, and a control quantity at the ath third discrete point based on the initial first discrete distance and the initial path interval;optimizing the path interval, the first discrete distance, UAV statuses at all third discrete points, and control quantities at all the third discrete points based on a third constraint condition, to obtain a UAV status and a control quantity that are at each third discrete point, wherein the third constraint condition is a condition that satisfies both adistance constraint and a third UAV dynamics constraint, the distance constraint is a function about a position of the ath third discrete point on the parameterized path, the UAV position at the a* third discrete point, the path interval, and the first discrete distance, and the third UAV dynamics constraint is a function about the UAV status at the ath third discrete point, a UAV status at an (a-1)111 third discrete point, a control quantity at the (a-l)th third discrete point, and the preset time interval; andcontrolling flight of the UAV based on the trajectory tracking control quantities at all the third discrete points.

2. The method for UAV path planning and tracking control according to claim 1, wherein expressions for optimizing based on the first constraint condition are:N-Nwp N-Nwp™"Zh--p.4.+ Z Z h-,L* >=1 t=1 ^=1 and77 <77 <77lb k +\ whereinX uk is the UAV status at the kth first discrete point, k is the control quantity at the kth first discrete point, z is a sequence number of a track segment, ?iN is the UAV P Xposition at the (i*N)th first discrete point, "'is the ith landmark position, is theUAV status at the (k -1 ),h first discrete point, "is the control quantity at the (k-^first discrete point, is a relationship function, is the preset time interval, is a real number vector, ^ib js a iower bound of a UAV control quantity, and is an upper bound of the UAV control quantity.

3. The method for UAV path planning and tracking control according to claim 1, wherein expressions for optimizing based on the second constraint condition are:min V N -dtX., = Xn,PiN0 u • , whereinm‘ is the UAV status at the (mi)111 second discrete point, is a control quantity at the (m><i)th second discrete point, ’ is the ith discrete time interval, m is the uUAV status at the (m / bU second discrete point, is the control quantity at the (mxi-l)th second discrete point, J ' is a relationship function, * is the UAV pposition at the (ixN)th second discrete point, and is the i* landmark position.

4. The method for UAV path planning and tracking control according to claim 1, wherein expressions for optimizing based on the third constraint condition are:x=xa.+f(xa^u.)dt ,a a—1 « / v a—a—i ’ and^ib - a - ^ub, whereinis the UAV status at the ath third discrete point, is the control quantity atthe a” third discrete point, 1 is the first discrete distance, a is the position of the a”Pthird discrete point on the parameterized path, a is the UAV position at the a11 third discrete point, is a difference between a (j+l)th discrete distance and a j* discretedistance, j is a sequence number of a discrete distance, 1 is the UAV status at the(ad)111 third discrete point, ; ' is the control quantity at the (ad)'11 third discrete point, &is the preset time interval, is a relationship function, ^lb is a lower bound of a UAV control quantity, and is an upper bound of the UAV control quantity.

5. The method for UAV path planning and tracking control according to claim 1, wherein an expression of the parameterized path is:path(l) = (Pa+] -Pa)(l - a)+ Pa , I e ( («, a +1), whereinpath(*) js parameterizeci path, is a UAV position at an (a+l)th third discrete point, ?a is the UAV position at the a* third discrete point, ' is a difference parameter, and a is a sequence number of a third discrete point.

6. A system for UAV path planning and tracking control, comprising:a landmark position and starting point determining module, configured to determine Nwp landmark positions and a starting point, to determine Nwp track segments, wherein Nwp>l;a first discrete point determining module, configured to determine N first discrete points on each of the track segments, to determine N*Nwp first discrete points, wherein N>1;a first initialization module, configured to initialize a UAV status and a control quantity to obtain an initial status and an initial control quantity, wherein the status comprises a position, a velocity, and an attitude quaternion, and the control quantity comprises thrust and an angular velocity;a first status and control quantity determining module, configured to determine a UAV position at an (i*N)th first discrete point, a UAV status at a kth first discrete point, and a control quantity at the kth first discrete point based on the initial status and the initial control quantity, wherein i=l, 2, ..., Nwp, and k=l, 2, ..., N*Nwp;a first optimization module, configured to optimize UAV statuses at all first discrete points and control quantities at all the first discrete points based on a first constraint condition, to obtain a primarily-optimized UAV status and a primarily-optimized control quantity that are at each first discrete point, wherein the first constraint condition is a condition that satisfies all of a first landmark position constraint, a first UAV dynamics constraint, and a minimum control constraint, the first landmark position constraint is a function about the UAV position at the (i*N)th first discrete point and an ith landmark position, the first UAV dynamics constraint is a function about the UAV status at the kth first discrete point, a UAV status at a (k-l)111 first discrete point, a control quantity at the (kl first discrete point, and a preset time interval, and the minimum control constraint is a function about the control quantity at the (k-l)th first discrete point;an initial path determining module, configured to determine an initial path of a UAV according to primarily-optimized UAV statuses at all discrete points, wherein the initial path comprises the Nwp track segments;a second discrete point determining module, configured to determine N second discrete points on each of the track segments of the initial path, to determine N*Nwp second discrete points;a second status and control quantity determining module, configured to determine a UAV status at an mth second discrete point, a control quantity at the mth second discrete point, and a discrete time interval of an ith track segment of the initial path based on the initial status, the initial control quantity, the preset time interval, the primarily-optimized UAV statuses at all the first discrete points, and the primarily-optimized control quantities at all the first discrete points, wherein m=l, 2, ..., N;a second optimization module, configured to optimize UAV statuses at all second discrete points, control quantities at all the second discrete points, and discrete time intervals of all track segments of the initial path based on a second constraint condition to obtain a shortest-time UAV status at each second discrete point, to determine a shortest-time path of the UAV, wherein the second constraint condition is a condition that satisfies all of a second landmark position constraint, a second UAV dynamics constraint, and a shortest time constraint, the second landmark position constraint is a function about a UAV position at an (UN)th second discrete point and an ilh landmark position, the second UAV dynamics constraint is a function about a UAV status at an (mxi)th second discrete point, a UAV status at an (m i-1)'11 second discrete point, a control quantity at the (mxi-l)th second discrete point, and an ith discrete time interval, and the shortest time constraint is a function about the second landmark position constraint and the second UAV dynamics constraint;an interpolation module, configured to perform polynomial interpolation on the shortest-time path to obtain a parameterized path;a third discrete point determining module, configured to determine Nt third discrete points on the parameterized path;a second initialization module, configured to initialize a first discrete distance to obtain an initial first discrete distance, and initialize a path interval to obtain an initial path interval, wherein an ath discrete distance is a distance from the starting point to anath third discrete point, a=l, 2, ..., Nt, and an ath path interval is a difference between an (a+l)111 discrete distance and the ath discrete distance;a third status and control quantity determining module, configured to determine a UAV position at the a* third discrete point, a UAV status at the a* third discrete point, and a control quantity at the a1h third discrete point based on the initial first discrete distance and the initial path interval;a third optimization module, configured to optimize the path interval, the first discrete distance, UAV statuses at all third discrete points, and control quantities at all the third discrete points based on a third constraint condition, to obtain a UAV status and a control quantity that are at each third discrete point, wherein the third constraint condition is a condition that satisfies both a distance constraint and a third UAV dynamics constraint, the distance constraint is a function about a position of the ath third discrete point on the parameterized path, the UAV position at the ath third discrete point, the path interval, and the first discrete distance, and the third UAV dynamics constraint is a function about the UAV status at the ath third discrete point, a UAV status at an (a^ l)th third discrete point, a control quantity at the (a—I)111 third discrete point, and the preset time interval; anda control module, configured to control flight of the UAV based on the trajectory tracking control quantities at all the third discrete points.

7. An electronic device, comprising:one or more processors; anda storage apparatus, configured to store one or more programs, whereinwhen the one or more programs are executed by the one or more processors, the one or more processors are enabled to implement the method for UAV path planning and tracking control according to any one of claims 1 to 5.

8. The electronic device according to claim 7, wherein the storage apparatus is a readable storage medium.

Images