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

Abstract

The present invention discloses a method, system, and electronic device for unmanned aerial vehicle (UAV) path planning and tracking control, relating to the field of path planning technology. The method comprises determining the UAV position at the i×Nth first discrete point, the UAV state and control variable at the kth first discrete point, performing a primary optimization to obtain the primary optimized UAV state at each first discrete point, thereby determining an initial path; determining the UAV state and control variable at the mth second discrete point, and the discrete time interval of the ith trajectory segment; performing a secondary optimization to obtain the shortest time path and interpolating to obtain a parameterized path; determining the UAV position, UAV state, and control variable at the ath third discrete point based on the initial first discrete distance and the initial path interval; performing a tertiary optimization to obtain the trajectory tracking UAV state and trajectory tracking control variable at each third discrete point, thereby controlling the flight of the UAV. The present invention improves path planning efficiency while ensuring that the planned path is time-efficient.

Description

Technical Field

[0001] The present invention relates to the field of path planning technology, and in particular to a method, system and electronic equipment for unmanned aerial vehicle (UAV) path planning and tracking control. Background Art

[0002] Drones are now widely used for reconnaissance, cargo delivery, cinematography, search and rescue, and entertainment activities such as drone racing. One of the most prominent drones is the quadcopter. Due to its simplicity and versatility, quadcopters can perform a wide range of maneuvers, from smooth flight to complex stunts. This makes quadcopters one of the most flexible and maneuverable aerial robots. In many drone applications, time-minimizing path planning and tracking control are crucial. For example, in search and rescue missions, time-minimizing path planning allows drones to reach and search disaster areas more quickly, providing assistance faster and buying valuable time for rescue efforts. In reconnaissance and detection missions, time-minimizing path planning ensures drones complete their missions in a short time, reducing the risk of detection. In the drone delivery industry, time-minimizing path planning that meets waypoint constraints allows drones to deliver goods to their destinations in the shortest possible time, thereby improving efficiency and customer satisfaction. Furthermore, in the entertainment sector, drone racing competitions require participants to control their drones around a prescribed track and strive to complete the course in the shortest possible 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.

[0003] 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 UAV's motion potential 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 to represent 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 the quadcopter UAV. Although the dynamic model of the UAV and the time optimality of the trajectory are taken into account, 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 several hours. When the landmark point position changes, the path cannot be replanned in real time, and the safety of the flight cannot be guaranteed. Given that generating the time-optimal path for full quadrotor dynamics is computationally expensive, taking minutes or even hours, Angel Romero et al. (Time-Optimal Online Replanning for AgileQuadrotor Flight, arXiv preprint arXiv:2203.09839, 2022) introduced a sampling-based method to efficiently generate the time-optimal path for a point mass model. This method then uses a model-predictive trajectory control method to track this path. While this method can quickly solve the time-optimal path for the mass model, it replaces the UAV dynamics model with the mass model in this process, sacrificing some optimality.

[0004] Therefore, existing methods for UAV path planning and tracking control cannot improve the efficiency of path planning while ensuring that the planned path is completed in the shortest time. Summary of the Invention

[0005] 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 improves the efficiency of path planning while ensuring that the time of the planned path is the shortest.

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

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

[0008] Determine N wp The locations of the landmarks and the starting point are used to determine N wp Track segments; N wp >1;

[0009] N first discrete points are determined on each of the trajectory segments, thereby determining N×N wp The first discrete points; N>1;

[0010] Initializing the drone state and control variables to obtain an initial state and initial control variables; the state includes position, velocity, and attitude quaternions; the control variables include thrust and angular velocity;

[0011] 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 ;

[0012] Based on the first constraint, the UAV states and control variables of all first discrete points are optimized to obtain an optimized UAV state and an optimized control variable for each first discrete point. The first constraint 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 position of the i-th landmark point. 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 variable of the k-1th first discrete point, and a preset time interval. The minimum control constraint is a function of the control variable of the k-1th first discrete point.

[0013] The initial path of the UAV is determined based on the optimized UAV state of all discrete points; the initial path includes N wp trajectory segments;

[0014] N second discrete points are determined on each of the trajectory segments of the initial path, thereby determining N×N wp Second discrete point;

[0015] 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;

[0016] Based on the second constraint, 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 drone's shortest-time path; the second constraint 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 being 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 being 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;

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

[0018] Determine N on the parameterized path T A third discrete point;

[0019] 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,…,N T The ath path interval is the difference between the a+1th discrete distance and the ath discrete distance;

[0020] Determining, based on the initial first discrete distance and the initial path interval, a drone position at the ath third discrete point, a drone state at the ath third discrete point, and a control variable at the ath third discrete point;

[0021] Based on a third constraint, the path interval, the first discrete distance, the UAV states of all third discrete points, and the control variables of all third discrete points are optimized to obtain the trajectory tracking UAV state and trajectory tracking control variable at each third discrete point. The third constraint is a condition that simultaneously satisfies the distance constraint and the third UAV dynamics constraint, wherein the distance constraint is a function of the 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. The third UAV dynamics constraint is a function of the UAV state at the ath third discrete point, the UAV state at the a-1th third discrete point, the control variable at the a-1th third discrete point, and the preset time interval.

[0022] The UAV flight is controlled based on the trajectory tracking control quantities of all third discrete points.

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

[0024]

[0025] U lb ≤U k ≤U ub ;

[0026] Among them, X k is the UAV state of the kth first discrete point, U k is the control quantity of the kth first discrete point, i is the sequence number of the trajectory segment, P iN is the position of the UAV at the first discrete point i×N, P w,i is the position of the i-th landmark point, X k-1 is the UAV state of the k-1th first discrete point, U k-1 is the control quantity of the k-1th first discrete point, f(·) is the relationship function, dt is the preset time interval, R is a real number vector, U lb is the lower bound of the UAV control quantity, U ub is the upper bound of the UAV control quantity.

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

[0028]

[0029]

[0030] P' iN -P w,i =0;

[0031] in, is the drone state of the m×i second discrete point, is the control quantity of the m×i second discrete point, dt i is the i-th discrete time interval, X mi-1 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 second discrete point i×N, P w,i is the position of the i-th landmark point.

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

[0033]

[0034]

[0035] X a =X a-1 +f(X a-1 ,U a-1 )dt;

[0036] U lb ≤U a ≤U ub ;

[0037] Among them, X a is the state of the UAV at the ath third discrete point, U a is the control quantity of the ath third discrete point, l1 is the first discrete distance, c a is the position of the ath third discrete point on the parameterized path, P a is the position of the drone at the ath third discrete point, dl j is the difference between the j+1th discrete path and the jth discrete path, j is the sequence number of the discrete path, X a-1 is the drone state at the a-1th third discrete point, U a-1 is the control quantity of the a-1th third discrete point, dt is the preset time interval, f(·) is the relationship function, U lb is the lower bound of the UAV control quantity, U ub is the upper bound of the UAV control quantity.

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

[0039] path(l)=(P a+1 -P a )(la)+P a ,l∈((a,a+1);

[0040] Among them, path(·) is the parameterized path, P a+1 is the position of the drone at the a+1th third discrete point, P a 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.

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

[0042] The module for determining the location of landmarks and starting points is used to determine N wp The locations of the landmarks and the starting point are used to determine N wp Track segments; N wp >1;

[0043] The first discrete point determination module is used to determine N first discrete points on each of the trajectory segments, thereby determining N×N wp The first discrete points; N>1;

[0044] 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;

[0045] A first state and control quantity module is 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 quantity of the kth first discrete point based on the initial state and the initial control quantity; i = 1, 2, ..., N wp ; k=1,2,…,N×N wp ;

[0046] a first optimization module, configured to optimize the UAV states and control variables of all first discrete points based on a first constraint condition, thereby obtaining a first-time optimized UAV state and a first-time optimized control variable for each first discrete point; the first constraint condition being a condition that simultaneously satisfies a first landmark point position constraint, a first UAV dynamics constraint, and a minimum control constraint, wherein the first landmark point position constraint is a function of the UAV position of the (i×N)th first discrete point and the position of the (i)th landmark point; the first UAV dynamics constraint is a function of the UAV state of the (k)th first discrete point, the UAV state of the (k-1)th first discrete point, the control variable of the (k-1)th first discrete point, and a preset time interval; and the minimum control constraint is a function of the control variable of the (k-1)th first discrete point;

[0047] The initial path determination module is used to determine the initial path of the UAV based on the optimized UAV state of all discrete points; the initial path includes N wp trajectory segments;

[0048] The second discrete point determination module is configured to determine N second discrete points on each of the trajectory segments of the initial path, thereby determining N×N wp Second discrete point;

[0049] A second state and control amount determination module is configured to determine the state of the UAV at the mth second discrete point, the control amount at 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 UAV state of all first discrete points, and the once-optimized control amounts of all first discrete points; m = 1, 2, ..., N;

[0050] a second optimization module, configured to optimize the drone states of all second discrete points, the control variables of all second discrete points, and the discrete time intervals of all trajectory segments of the initial path based on the second constraint condition, to obtain the shortest-time drone states of each second discrete point, thereby determining the drone's shortest-time path; 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, wherein the second landmark point position constraint is a function of the drone position of the i×Nth second discrete point and the position of the i-th landmark point; 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 variable of the m×i-1th second discrete point, and the i-th discrete time interval; and the shortest-time constraint is a function of the second landmark point position constraint and the second drone dynamics constraint;

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

[0052] The third discrete determination module is used to determine N on the parameterized path. T A third discrete point;

[0053] 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, ..., N T The ath path interval is the difference between the a+1th discrete distance and the ath discrete distance;

[0054] A third state and control amount determination module is configured 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;

[0055] a third optimization module, configured to optimize the path interval, the first discrete distance, the UAV states of all third discrete points, and the control variables of all third discrete points based on a third constraint condition, to obtain the trajectory tracking UAV state and trajectory tracking control variable at each third discrete point; the third constraint condition being a condition that simultaneously satisfies both the distance constraint and a third UAV dynamics constraint, the distance constraint being a function of the 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 being a function of the UAV state at the ath third discrete point, the UAV state at the a-1th third discrete point, the control variable at the a-1th third discrete point, and the preset time interval;

[0056] The control module is used to control the flight of the UAV based on the trajectory tracking control quantities of all third discrete points.

[0057] An electronic device, comprising:

[0058] one or more processors;

[0059] a storage device having one or more programs stored thereon;

[0060] 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.

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

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

[0063] The present invention discloses a method, system, and electronic device for unmanned aerial vehicle (UAV) path planning and tracking control. The method first determines an initial path based on a first landmark point position constraint, a first UAV dynamics constraint, and a minimum control constraint. Then, the UAV's shortest path in time is determined based on a second landmark point position constraint, a second UAV dynamics constraint, and a minimum time constraint. Compared with existing path planning methods, this method, which optimizes the initial path once and then optimizes the shortest path in time twice, not only implements the planning of the shortest path in time but also improves the efficiency of path planning. Furthermore, based on the shortest path in time, optimization is performed based on the distance constraint and the third UAV dynamics constraint, further enabling control of the UAV's path tracking and improving the UAV's flight capability. BRIEF DESCRIPTION OF THE DRAWINGS

[0064] In order to more clearly illustrate the embodiments of the present invention or the technical solutions in the prior art, the following briefly introduces the drawings required for use in the embodiments. 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 any creative work.

[0065] Figure 1 A schematic flow chart of a method for drone path planning and tracking control provided in Example 1 of the present invention;

[0066] Figure 2 A flowchart of a method for planning and tracking the shortest path for a UAV when the location of a landmark point changes;

[0067] Figure 3 This is a diagram showing the shortest path planning result in a specific embodiment;

[0068] Figure 4 It is a trajectory tracking effect diagram of a specific embodiment. DETAILED DESCRIPTION

[0069] The following will clearly and completely describe the technical solutions in the embodiments of the present invention in conjunction with the accompanying drawings. 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 making creative efforts are within the scope of protection of the present invention.

[0070] 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 of the planned path is the shortest.

[0071] 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.

[0072] Example 1

[0073] Figure 1 This is a flow chart of the method for drone path planning and tracking control provided in Example 1 of the present invention. Figure 1 As shown, the method for drone path planning and tracking control in this embodiment includes:

[0074] Step 101: Determine N×N wp The first discrete point.

[0075] Step 101 specifically includes:

[0076] Step 1011: Determine N wp The locations of the landmarks and the starting point are used to determine N wp Track segments; N wp >1.

[0077] Step 1012: Determine N first discrete points on each trajectory segment, thereby determining N×N wp The first discrete points; N>1.

[0078] In fact, the real-time state X(t) of the UAV is obtained by using the UAV’s onboard sensors and the external motion capture system. 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 reciprocal 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 dynamic 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 position of the landmark point that the UAV needs to pass through {P w,i |i=1,2,...,N wp}, N wp is the number of landmark points. According to the number of landmark points, the trajectory is divided into N wp There are N trajectory segments, each of which is discretized into N points. Therefore, the number of discrete points in the entire trajectory is N·N wp .

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

[0080] Step 102 specifically includes:

[0081] Step 1021: Initialize the drone state and control variables to obtain the initial state and initial control variables; the state includes position, velocity, and attitude quaternion; the control variables include thrust and angular velocity.

[0082] Step 1022: Based on the initial state and 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 .

[0083] 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 the first optimized UAV state and the first optimized control quantity of each first discrete point; the first constraint condition is the condition of simultaneously satisfying 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 position of the i-th landmark point, 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 the preset time interval, and the minimum control constraint is a function of the control quantity of the k-1th first discrete point.

[0084] Step 1024: Determine the initial path of the UAV based on the optimized UAV state of all discrete points; the initial path includes N wp track segments.

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

[0086] Step 103 specifically includes:

[0087] Step 1031: Determine N second discrete points on each trajectory segment of the initial path, thereby determining N×N wp The second discrete point.

[0088] Step 1032: Based on the initial state, the initial control amount, the preset time interval, the first optimized drone state of all first discrete points, and the first optimized control amount of all first discrete points, 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; m = 1, 2, ..., N.

[0089] Step 1033: Based on the second constraint, the UAV states of all second discrete points, the control variables 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 states of each second discrete point, thereby determining the UAV's shortest-time path; the second constraint is to simultaneously satisfy 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 position of the i-th landmark point. 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 variable 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.

[0090] Step 104: Perform three optimizations based on the shortest path in time to determine the trajectory tracking control value of the UAV, thereby controlling the 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 disturbances in the real environment and modeling errors in the UAV's dynamics model, after obtaining the shortest path in time, the UAV needs to be controlled in real time to track the trajectory. To ensure tracking accuracy and maximize the performance of the UAV, the trajectory tracking error and trajectory tracking distance are optimized simultaneously. 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 in the above steps is subjected to polynomial interpolation to obtain the path path(l) with l as the parameter variable. The trajectory tracking error is defined as the distance between the UAV's position and certain points on the parameterized path. Since path(l) advances as l increases, maximizing the trajectory tracking distance is equivalent to maximizing l at the end of the optimization range.

[0094] Step 1042: Determine N on the parameterized path T A third discrete point.

[0095] 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,…,N T ; The ath path interval is the difference between the a+1th discrete distance and the ath discrete distance.

[0096] Step 1044: Based on the initial first discrete distance and the initial path interval, determine the drone position at the a-th third discrete point, the drone state at the a-th third discrete point, and the control variable at the a-th third discrete point.

[0097] Step 1045: Based on the third constraint, the path interval, the first discrete distance, the UAV states of all third discrete points, and the control quantities of all third discrete points are optimized to obtain the trajectory tracking UAV state and trajectory tracking control quantity at each third discrete point; the third constraint is a condition that simultaneously satisfies the distance constraint and the third UAV dynamics constraint. The distance constraint is a function of the 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; 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 quantity of the a-1th third discrete point, and a preset time interval.

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

[0099] Further, such as Figure 2 As shown, in actual use, the locations of waypoints may change. When a change is detected, steps 101 through 104 are re-executed to replan the global path. This process is executed at a fixed frequency, and after each execution, the control variables at the first moment of the optimal control sequence are sent to the drone for execution, thus achieving real-time control of the drone.

[0100] As a specific implementation, the expression for optimization based on the first constraint condition is:

[0101]

[0102] U lb ≤U k ≤U ub .

[0103] Among them, X k is the UAV state of the kth first discrete point, U k is the control quantity of the kth first discrete point, i is the sequence number of the trajectory segment, P iN is the position of the UAV at the first discrete point i×N, P w,i is the position of the i-th landmark point, X k-1 is the UAV state of the k-1th first discrete point, U k-1 is the control quantity of the k-1th first discrete point, f(·) is the relationship function, dt is the preset time interval, R is a real number vector, U lb is the lower bound of the UAV control quantity, U ub is the upper bound of the UAV control quantity.

[0104] Specifically. In the actual optimization process, it is also necessary to initialize the control quantity of the first discrete point, using the formula X0=X init Initialize the control quantity of the first discrete point, where X0 is the control quantity of the first discrete point after initialization, X init Initialize the value for the first discrete point.

[0105] As a specific implementation, the expression for optimization based on the second constraint condition is:

[0106]

[0107]

[0108] P' iN -Pw,i =0.

[0109] in, is the drone state of the m×i 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 second discrete point i×N, P w,i is the position of the i-th landmark point.

[0110] Specifically. In the actual optimization process, it is also necessary to initialize the control quantity of the second discrete point, using the formula X'0=X init Initialize the control quantity of the second discrete point, where X'0 is the control quantity of the second discrete point after initialization, X init Initialize the value of the second discrete point (equal to the initialization value of the first discrete point).

[0111] As a specific implementation, the expression for optimization based on the third constraint condition is:

[0112]

[0113]

[0114] X a =X a-1 +f(X a-1 ,U a-1 )dt.

[0115] U lb ≤U a ≤U ub .

[0116] Among them, X a is the state of the UAV at the ath third discrete point, U a is the control quantity of the third discrete point a, l1 is the first discrete distance, c a is the position of the ath third discrete point on the parameterized path, P a is the position of the drone at the ath third discrete point, dl j is the difference between the j+1th discrete path and the jth discrete path, j is the sequence number of the discrete path, X a-1 is the drone state at the a-1th third discrete point, U a-1is the control quantity of the a-1th third discrete point, dt is the preset time interval, f(·) is the relationship function, U lb is the lower bound of the UAV control quantity, U ub is the upper bound of the UAV control quantity.

[0117] Specifically. In the actual optimization process, it is also necessary to initialize the control quantity of the second discrete point, using the formula X"0=X init Initialize the control amount of the third discrete point, where X"0 is the control amount of the third discrete point after initialization, X init Initialize the value of the third discrete point (equal to the initialization value of the first discrete point).

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

[0119] path(l)=(P a+1 -P a )(la)+P a ,l∈((a,a+1).

[0120] Among them, path(·) is the parameterized path, P a+1 is the position of the drone at the a+1th third discrete point, P a 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.

[0121] The above method is described below with specific embodiments:

[0122] S1, define the real-time status of the drone as X=[P T V T q T ] T , where P∈R 3 、V∈R 3 ,q∈R 4 are the position, velocity and attitude quaternions of the UAV respectively; the control quantity of the UAV is defined as U = [f ω], where f∈R is the thrust of the UAV, ω∈R 3 is the angular velocity of the UAV; given the landmark position that the UAV needs to pass through {P w,i |i=1,2,...,N wp}, N wp is the number of landmarks. In this embodiment, N wp =7, the locations of the landmarks are P w,1 =[14.8715,-0.7906,-2.6] T 、P w,2 =[37.0,-12,-0.5] T 、Pw,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 landmarks, the trajectory is divided into N wp =7 trajectory segments, each of which is discretized into N = 20 points, so the number of discrete points in the entire trajectory is N·N wp =140.

[0123] S2, first calculates the initial path by solving the following optimization problem:

[0124]

[0125] U lb ≤U k ≤U ub .

[0126] X0=X init .

[0127] 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 drone is X init =[0,0,0,0,0,0,1,0,0,0] T , UAV dynamics equation Has the following form:

[0128] S3, the discrete time interval of each trajectory is used as the optimization variable {dt i |i=1,2,...,7}, directly use the total time of the trajectory as the optimization target, and use the initial path solved in S2 as the initial value, that is, solve the following optimization problem to obtain the shortest path in time:

[0129]

[0130]

[0131] P' iN -P w,i=0.

[0132] X'0=X init .

[0133] S4, linearly interpolate the shortest time path obtained in S3 to obtain the path path(l)=(P a+1 -P a )(la)+P a ,l∈((a,a+1). Trajectory tracking control is to solve the following optimization problem:

[0134]

[0135]

[0136] X a =X a-1 +f(X a-1 ,U a-1 )dt.

[0137] U lb ≤U a ≤U ub .

[0138] X"0=X init .

[0139] Among them, set N T =5. Solving this optimization problem, we can get the optimal trajectory tracking sequence {X a |a=1,2,...,5} and the optimal control sequence {U a |a=1,2,...,5}.

[0140] S5, when a change in the landmark position is detected, re-executes S3 to replan the global path. S4 is executed at a fixed frequency of 50Hz. After each execution, the first control variable U1 of the optimal control sequence is sent to the UAV for execution, thus achieving real-time control.

[0141] like Figure 3-Figure 4 As shown, Figure 3 : This is a diagram showing the shortest path planning result in a specific embodiment. The three coordinate axes in the diagram represent the (x, y, z) positions of the drone in three-dimensional space, with units in meters. The star points represent the landmarks that the drone needs to pass through. The dotted line is the initial path of the drone obtained in S2, and the solid line is the shortest path obtained in S3. Figure 4This is a trajectory tracking result diagram for a specific example. The horizontal and vertical axes represent the x and y coordinates of the drone, respectively, in meters. The solid line is a segment of the shortest path obtained by solving S3, and the star point trajectory is the trajectory tracking result in S4. This example was solved on a standard laptop computer. The solution time for S2 was 1.2 seconds, for S3 was 0.4 seconds, and for S4 was 0.02 seconds.

[0142] Example 2

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

[0144] The module for determining the location of landmarks and starting points is used to determine N wp The locations of the landmarks and the starting point are used to determine N wp Track segments; N wp >1.

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

[0146] 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.

[0147] The first state and control quantity 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 quantity of the kth first discrete point based on the initial state and initial control quantity; i = 1, 2, ..., N wp ; k=1,2,…,N×N wp .

[0148] The first optimization module is used to optimize the UAV states and control quantities of all first discrete points based on the first constraint condition to obtain the first optimized UAV state and the first optimized control quantity of each first discrete point; the first constraint condition is the 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 position of the i-th landmark point. 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 the preset time interval. The minimum control constraint is a function of the control quantity of the k-1th first discrete point.

[0149] The initial path determination module is used to determine the initial path of the UAV based on the optimized UAV state of all discrete points; the initial path includes N wp track segments.

[0150] 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 The second discrete point.

[0151] A second state and control quantity determination module is used to determine the drone state of the mth second discrete point, the control quantity 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 quantity, the preset time interval, the once-optimized drone state of all first discrete points, and the once-optimized control quantities of all first discrete points; m = 1, 2, …, N.

[0152] The 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 the condition of simultaneously satisfying 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.

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

[0154] The third discrete determination module is used to determine N on the parameterized path. T A third discrete point.

[0155] 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,…,N T ; The ath path interval is the difference between the a+1th discrete distance and the ath discrete distance.

[0156] 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.

[0157] 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 quantity of all third discrete points based on the third constraint condition, and obtain the trajectory tracking UAV state and trajectory tracking control quantity at each third discrete point; the third constraint condition is a condition that simultaneously satisfies the distance constraint and the third UAV dynamic constraint. The distance constraint is a function of the 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; the third UAV dynamic constraint is a function of the UAV state at the ath third discrete point, the UAV state at the a-1th third discrete point, the control quantity at the a-1th third discrete point and a preset time interval.

[0158] The control module is used to control the flight of the UAV based on the trajectory tracking control quantities of all third discrete points.

[0159] Example 3

[0160] An electronic device, comprising:

[0161] One or more processors.

[0162] A storage device having one or more programs stored thereon.

[0163] 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.

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

[0165] The various embodiments in this specification are described in a progressive manner, with each embodiment focusing on the differences from other embodiments. Reference can be made to the common and similar parts between the various embodiments. For the systems disclosed in the embodiments, since they correspond to the methods disclosed in the embodiments, the description is relatively simple, and the relevant parts can be referred to the method description.

[0166] This document uses specific examples to illustrate the principles and implementation methods of the present invention. The above examples are only intended to help understand the method and core concept of the present invention. At the same time, those skilled in the art will find that the specific implementation methods and application scopes may vary based on the concept of the present invention. In summary, the contents of this specification should not be construed as limiting the present invention.

Claims

1. A method for UAV path planning and tracking control, characterized in that: The method comprises: Determine N wp The locations of the landmarks and the starting point are used to determine N wp Track segments; N wp >1; N first discrete points are determined on each of the trajectory segments, thereby determining N×N wp The first discrete points; N>1; Initializing the drone state and control variables to obtain an initial state and initial control variables; the state includes position, velocity, and attitude quaternions; the control variables include 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, the UAV states and control variables of all first discrete points are optimized to obtain an optimized UAV state and an optimized control variable for each first discrete point. The first constraint 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 position of the i-th landmark point. 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 variable of the k-1th first discrete point, and a preset time interval. The minimum control constraint is a function of the control variable of the k-1th first discrete point. The initial path of the UAV is determined based on the optimized UAV state of all discrete points; the initial path includes N wp trajectory segments; N second discrete points are determined on each of the trajectory segments of the initial path, thereby determining N×N wp Second discrete point; 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; Based on the second constraint, 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 drone's shortest-time path; the second constraint 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 being 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 being 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; Determine N on the parameterized path T A third discrete point; 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,…,N T The ath path interval is the difference between the a+1th discrete distance and the ath discrete distance; Determining, based on the initial first discrete distance and the initial path interval, a drone position at the ath third discrete point, a drone state at the ath third discrete point, and a control variable at the ath third discrete point; Based on a third constraint, the path interval, the first discrete distance, the UAV states of all third discrete points, and the control variables of all third discrete points are optimized to obtain the trajectory tracking UAV state and trajectory tracking control variable at each third discrete point. The third constraint is a condition that simultaneously satisfies the distance constraint and the third UAV dynamics constraint, wherein the distance constraint is a function of the 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. The third UAV dynamics constraint is a function of the UAV state at the ath third discrete point, the UAV state at the a-1th third discrete point, the control variable at the a-1th third discrete point, and the preset time interval. The UAV flight is controlled based on the trajectory tracking control quantities of all third discrete points.

2. The method for UAV path planning and tracking control according to claim 1, characterized in that: The expression optimized based on the first constraint condition is: IN lb ≤U k ≤U ub ; Among them, X k is the UAV state of the kth first discrete point, U k is the control quantity of the kth first discrete point, i is the sequence number of the trajectory segment, P iN is the position of the UAV at the first discrete point i×N, P w,i is the position of the i-th landmark point, X k-1 is the UAV state of the k-1th first discrete point, U k-1 is the control quantity of the k-1th first discrete point, f(·) is the relationship function, dt is the preset time interval, R is a real number vector, U lb is the lower bound of the UAV control quantity, U ub is the upper bound of the UAV control quantity.

3. The method for UAV path planning and tracking control according to claim 1, characterized in that: The expression optimized based on the second constraint is: P‘ iN -P w,i =0; in, is the drone state of the m×i 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 second discrete point i×N, P w,i is the position of the i-th landmark point.

4. The method for UAV 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; IN lb ≤U a ≤U ub ; Among them, X a is the state of the UAV at the ath third discrete point, U a is the control quantity of the ath third discrete point, l1 is the first discrete distance, c a is the position of the ath third discrete point on the parameterized path, P a is the position of the drone at the ath third discrete point, dl j is the difference between the j+1th discrete path and the jth discrete path, j is the sequence number of the discrete path, X a-1 is the drone state at the a-1th third discrete point, U a-1 is the control quantity of the a-1th third discrete point, dt is the preset time interval, f(·) is the relationship function, U lb is the lower bound of the UAV control quantity, U ub is the upper bound of the UAV control quantity.

5. The method for UAV path planning and tracking control according to claim 1, characterized in that: The expression of the parameterized path is: path(l)=(P a+1 -P a )(l-a)+P a ,l∈(a,a+1); Among them, path(·) is the parameterized path, P a+1 is the position of the drone at the a+1th third discrete point, P a 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 UAV path planning and tracking control, characterized in that: The system comprises: The module for determining the location of landmarks and starting points is used to determine N wp The locations of the landmarks and the starting point are used to determine N wp Track segments; N wp >1; The first discrete point determination module is used to determine N first discrete points on each of the trajectory segments, thereby determining N×N wp The 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 quantity module is 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 quantity of the kth first discrete point based on the initial state and the initial control quantity; i = 1, 2, ..., N wp ; k=1,2,…,N×N wp ; a first optimization module, configured to optimize the UAV states and control variables of all first discrete points based on a first constraint condition, thereby obtaining a first-time optimized UAV state and a first-time optimized control variable for each first discrete point; the first constraint condition being a condition that simultaneously satisfies a first landmark point position constraint, a first UAV dynamics constraint, and a minimum control constraint, wherein the first landmark point position constraint is a function of the UAV position of the (i×N)th first discrete point and the position of the (i)th landmark point; the first UAV dynamics constraint is a function of the UAV state of the (k)th first discrete point, the UAV state of the (k-1)th first discrete point, the control variable of the (k-1)th first discrete point, and a preset time interval; and the minimum control constraint is a function of the control variable of the (k-1)th first discrete point; The initial path determination module is used to determine the initial path of the UAV based on the optimized UAV state of all discrete points; the initial path includes N wp trajectory segments; The second discrete point determination module is configured to determine N second discrete points on each of the trajectory segments of the initial path, thereby determining N×N wp Second discrete point; A second state and control amount determination module is configured to determine the state of the UAV at the mth second discrete point, the control amount at 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 UAV state of all first discrete points, and the once-optimized control amounts of all first discrete points; m = 1, 2, ..., N; a second optimization module, configured to optimize the drone states of all second discrete points, the control variables of all second discrete points, and the discrete time intervals of all trajectory segments of the initial path based on the second constraint condition, to obtain the shortest-time drone states of each second discrete point, thereby determining the drone's shortest-time path; 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, wherein the second landmark point position constraint is a function of the drone position of the i×Nth second discrete point and the position of the i-th landmark point; 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 variable of the m×i-1th second discrete point, and the i-th discrete time interval; and the shortest-time constraint is a function of the second landmark point position constraint and the second drone dynamics constraint; An interpolation module, configured to perform polynomial interpolation on the shortest time path to obtain a parameterized path; The third discrete determination module is used to determine N on the parameterized path. T A third discrete point; 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, ..., N T 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 is configured 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, configured to optimize the path interval, the first discrete distance, the UAV states of all third discrete points, and the control variables of all third discrete points based on a third constraint condition, to obtain the trajectory tracking UAV state and trajectory tracking control variable at each third discrete point; the third constraint condition being a condition that simultaneously satisfies both the distance constraint and a third UAV dynamics constraint, the distance constraint being a function of the 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 being a function of the UAV state at the ath third discrete point, the UAV state at the a-1th third discrete point, the control variable at the a-1th third discrete point, and the preset time interval; The control module is used to control the flight of the UAV based on the trajectory tracking control quantities of all third discrete points.

7. An electronic device, characterized in that: include: 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