Adaptive wind estimation, trajectory generation, and flight control for aerial systems using motion data

Abstract

Adaptive wind estimation, trajectory generation, and flight control for aerial systems using motion data is provided. The adaptive wind estimation approach may be implemented using onboard computing power, may rapidly converge to true values, may be computationally inexpensive, and may not require any specific hardware or specific vehicle maneuvers for the convergence. There may be no prior knowledge of the wind field, using the motion of the aircraft itself rather than wind sensors. The algorithm may include three blocks. An identification/estimation block may identify aerodynamic drag coefficients in still-air flight and estimate the wind components in moving and variable air flight. A navigation block may generate feasible trajectories, taking into account the estimated wind field. A control block may generate motor/engine thrust commands necessary to track the generated trajectories while compensating for the wind disturbance.

Claims

1. A computer program embodied on a non-transitory computer-readable medium, the program configured to cause at least one processor to: perform adaptive wind estimation by identifying aerodynamic drag coefficients in still-air flight of an aircraft and estimating wind components in moving and variable air based on the identified aerodynamic drag coefficients; generate a feasible trajectory based on the estimated wind components; generate motor and thrust commands based on said feasible trajectory, wherein said motor and thrust commands compensate for wind disturbances; and control the aircraft based on the generated motor and thrust commands.

2. The computer program of claim 1, wherein no information from one or more wind sensors is used.

3. The computer program of claim 1, wherein the adaptive wind estimation comprises: estimating a translational drag coefficient using an inertial velocity and orientation angle measurements of the aircraft; and estimating a rotational drag coefficient using a prediction of angular rate measurements.

4. The computer program of claim 1, wherein the wind components comprise wind velocities and accelerations.

5. The computer program of claim 1, wherein the feasible trajectory generation comprises a jerk minimization approach that is modified to directly take an estimate of aerodynamic drag into account.

6. The computer program of claim 1, wherein the feasible trajectory generation comprises: computing a minimum possible time-to-go for all segments of the trajectory, the trajectory comprising a plurality of waypoints and the segments defined as lines between each pair of waypoints.

7. The computer program of claim 6, wherein the feasible trajectory generation further comprises: generating a segment trajectory on an interval between two adjacent waypoints according to an optimal control problem; and computing a mass-normalized thrust vector required to traverse the segment trajectory component-wise.

8. The computer program of claim 7, wherein the feasible trajectory generation further comprises: checking thrust feasibility conditions; when the thrust feasibility conditions are satisfied: computing and checking rate feasibility, and when the rate is feasible: marking the segment trajectory as a feasible trajectory between the waypoints, and advancing to the next segment of the trajectory, and taking a final state of the current segment trajectory as an initial state for a next segment.

9. The computer program of claim 8, wherein the process of checking thrust feasibility and rate feasibility conditions is repeated until all segments of the trajectory are checked.

10. The computer program of claim 8, wherein when the thrust feasibility conditions are not satisfied or the rate is not feasible, time is advanced and the segment trajectory generation, thrust vector computation, and feasibility checks are repeated.

11. The computer program of claim 1, wherein the generating of the motor and thrust commands comprises: determining center of gravity (CG) control of the aircraft; and determining attitude control of the aircraft.

12. A computer-implemented method, comprising: performing adaptive wind estimation, by a computing system, by identifying aerodynamic drag coefficients in still-air flight of an aircraft and estimating wind components in moving and variable air based on the identified aerodynamic drag coefficients, the estimated wind components comprising wind velocities and accelerations; generating a feasible trajectory, by the computing system, based on the estimated wind components; generating motor and thrust commands, by the computing system, based on the generated feasible trajectory by determining center of gravity (CG) control of the aircraft and determining attitude control of the aircraft, wherein the motor and thrust commands compensate for wind disturbances; and controlling the aircraft based on the generated motor and thrust commands, by the computing system, wherein no information from one or more wind sensors is available to the computing system.

13. The computer-implemented method of claim 12, wherein the adaptive wind estimation comprises: estimating a translational drag coefficient, by the computing system, using an inertial velocity and orientation angle measurements of the aircraft; and estimating a rotational drag coefficient, by the computing system, using a prediction of angular rate measurements.

14. The computer-implemented method of claim 12, wherein the feasible trajectory generation comprises: computing, by the computing system, a minimum possible time-to-go for all segments of the trajectory, the trajectory comprising a plurality of waypoints and the segments defined as lines between each pair of waypoints.

15. The computer-implemented method of claim 12, wherein the feasible trajectory generation further comprises: generating, by the computing system, a segment trajectory on an interval between two adjacent waypoints according to an optimal control problem; and computing, by the computing system, a mass-normalized thrust vector required to traverse the segment trajectory component-wise.

16. The computer-implemented method of claim 15, wherein the feasible trajectory generation further comprises: checking thrust feasibility conditions, by the computing system; when the thrust feasibility conditions are satisfied: computing and checking rate feasibility, by the computing system, and when the rate is feasible: marking the segment trajectory as a feasible trajectory between the waypoints, by the computing system, and advancing to the next segment of the trajectory, by the computing system, and taking a final state of the current segment trajectory as an initial state for a next segment.

17. The computer-implemented method of claim 16, wherein the process of checking thrust feasibility and rate feasibility conditions is repeated until all segments of the trajectory are checked.

18. A computer-implemented method, comprising: performing adaptive wind estimation for an aircraft, by a computing system; generating a feasible trajectory, by the computing system, that takes the estimated wind components into account; generating motor and thrust commands, by the computing system, based on the generated feasible trajectory; and controlling the aircraft based on the generated motor and thrust commands, by the computing system, wherein the performing of the adaptive wind estimation comprises identifying aerodynamic drag coefficients in still-air flight of the aircraft and estimating wind components in moving and variable air based on the identified aerodynamic drag coefficients, the estimated wind components comprising wind velocities and accelerations.

19. The computer-implemented method of claim 18, wherein no information from one or more wind sensors is available to the computing system.

Description

BRIEF DESCRIPTION OF THE DRAWINGS

(1) In order that the advantages of certain embodiments of the invention will be readily understood, a more particular description of the invention briefly described above will be rendered by reference to specific embodiments that are illustrated in the appended drawings. While it should be understood that these drawings depict only typical embodiments of the invention and are not therefore to be considered to be limiting of its scope, the invention will be described and explained with additional specificity and detail through the use of the accompanying drawings, in which:

(2) FIG. 1 is a perspective view illustrating a sUAS, according to an embodiment of the present invention.

(3) FIG. 2A is a flowchart illustrating a process for performing adaptive wind estimation and flight control for aerial systems using motion data, according to an embodiment of the present invention.

(4) FIG. 2B is a flowchart illustrating a process for performing adaptive estimation, according to an embodiment of the present invention.

(5) FIG. 2C is a flowchart illustrating a process for generating a trajectory based on the adaptive estimation of FIG. 2B, according to an embodiment of the present invention.

(6) FIG. 2D is a flowchart illustrating a process for controlling an aircraft to track to the trajectory generated in FIG. 2C, according to an embodiment of the present invention.

(7) FIG. 3 is a graph illustrating a generated 3D trajectory and corresponding waypoints, according to an embodiment of the present invention.

(8) FIG. 4 is a block diagram illustrating a computing system configured to perform adaptive wind estimation, trajectory generation, and flight control for aerial systems using motion data, according to an embodiment of the present invention.

DETAILED DESCRIPTION OF THE EMBODIMENTS

(9) Some embodiments of the present invention pertain to adaptive wind estimation, trajectory generation, and flight control for aerial systems using motion data. The estimation approach of some embodiments is implemented using onboard computing power, rapidly converges to true values, is computationally inexpensive, and does not require any specific hardware or specific vehicle maneuvers for the convergence. Some embodiments are particularly effective for sUASs, which are more affected by wind disturbances in complex wind fields, such as those of urban environments. Furthermore, rotorcraft create large amounts of local airflow, which makes wind measurements more difficult or even impossible with the typically less power hardware of sUASs as compared to military drones, for example. There is no prior knowledge of the wind field in some embodiments, using the motion of the aircraft itself rather than wind sensors, which requires less computing power, complexity, and cost than conventional approaches. It should be noted that the approaches of some embodiments may be applied to rotorcraft, fixed wing aircraft, manned aircraft, unmanned aircraft, small aircraft, large aircraft, or any combination thereof without deviating from the scope of the invention.

(10) Certain embodiments have three components: (1) an identification/estimation block; (2) a navigation block; and (3) a control block. The identification/estimation block identifies aerodynamic drag coefficients in still air (e.g., indoor) flight and estimates the wind components in moving and variable air (e.g., outdoor) flight. The navigation block generates feasible trajectories taking into account the estimated wind field. The control block generates motor/engine thrust (e.g., spin rate for rotorcraft) commands necessary to track the generated trajectories while compensating for the wind disturbance. These blocks may not use information from wind sensors in some embodiments. Indeed, wind sensors may not be present in some aircraft.

(11) Such embodiments enable safe navigation in windy environments while continuously estimating the wind field along the trajectory, which may be broadcast for air traffic management in some embodiments. It should be noted that while some embodiments are applicable to sUASs and other rotorcraft, embodiments may be applied to any aircraft regardless of size, design, propulsion type, and control type (e.g., whether manned or unmanned) without deviating from the scope of the invention. Characteristics of some embodiments include, but are not limited to, fast and reliable estimation of the wind and related aerodynamic drag components without requiring expensive measurement units and significant computational power, generating feasible trajectories in real-time that take into account the estimated wind field, and designing controllers capable of compensating for the wind effects in tracking of the designed trajectories.

(12) FIG. 1 is a perspective view illustrating a sUAS 100, according to an embodiment of the present invention. While a sUAS is shown here, certain embodiments may be used for any type and/or size of aircraft without deviating from the scope of the invention. In this embodiment, sUAS 100 is a rotorcraft that includes four rotors 110 that are spaced equally about a body 120. Each rotor 110 includes a propeller 112, a motor 114 that drives propeller 112, and a shaft 116 that connects motor 114 to body 120. Motor 114, and thus propeller 112, can rotate about shaft 116. For instance, by rotating two opposite rotors 110 by a same amount, lateral movement is possible. Other orientations would cause at least some degree of rotation. In some embodiments, only two opposing rotors are rotatable.

(13) A computing system 130, such as computing system 400 of FIG. 4, controls operation of sUAS 100. sUAS 100 also includes sensors 140, which provide data to computing system 130 that can be used for flight control. Sensors 140 may include, but are not limited to, IMUs, rate gyros, accelerometers, spin rate sensors, and/or any other suitable sensor without deviating from the scope of the invention.

(14) I. Rotorcraft Dynamic Model

(15) An example dynamic model for a sUAS rotorcraft is discussed below.

(16) A. Equations of Motion

(17) The dynamics of the center of mass of a multi-rotor vehicle, such as sUAS 100 of FIG. 1, in the East-North-Up Earth (inertial) frame (F.sub.E) are given by:
{dot over (r)}(t)=v(t)  (1)
m{dot over (v)}(t)=R.sub.B/E(t)e.sub.3.sup.Bf.sub.T(t)+f.sub.D(t)+mg  (2)

(18) where r(t)=[x(t) y(t) z(t)].sup.T is the position of the center of mass in F.sub.E, v(t)=[v.sub.x(t)v.sub.y(t)v.sub.z(t)].sup.T is the inertial velocity, m is the mass, f.sub.T(t) is the total thrust generated by the rotors, R.sub.B/E(t) is the rotation matrix from the body frame F.sub.B (Forward-Left-Up) to F.sub.E, e.sub.3.sup.B=[0 0 1].sup.T is the third unit vector of F.sub.B, f.sub.D(t) is the aerodynamic drag force, and g=[0 0 −g].sup.T is the acceleration of gravity.

(19) The rotational dynamics of the vehicle about the center of mass are given in the frame F.sub.B by:
{dot over (R)}.sub.B/E(t)=R.sub.B/E(t)ω.sup.x(t)  (3)
J{dot over (ω)}(t)=−ω(t)×Jω(t)+J.sub.mω.sub.m(t)ω(t)+τ(t)+τ.sub.D(t)  (4)

(20) where ω(t)=[p(t) q(t) r(t)].sup.T is the angular rate of F.sub.B with respect to the inertial frame F.sub.E expressed in F.sub.B, J=diag(J.sub.1,J.sub.2,J.sub.3) is the inertia matrix of the vehicle (the body frame is aligned with the principal axes of inertia), J.sub.m is the rotor inertia about the axis of rotation (assuming this is identical for all rotors), ω(t)=[−q(t) p(t) 0].sup.T,

(21) ${\omega }_m\left(t\right)=\underset{i=1}{\overset{n}{.Math.}}{\left(-1\right)}^i{\Omega }_i\left(t\right),{\Omega }_i\left(t\right)$
is the angular rate of the i.sup.th rotor about its axis of rotation, τ(t) is the torque generated by the rotors, and τ.sub.D(t) is the aerodynamic rotational drag torque.

(22) It is assumed that all motors generate thrust in the positive z-direction in F.sub.B frame e.sub.3.sup.B, and

(23) $f_T\left(t\right)=\underset{i=1}{\overset{n}{.Math.}}{f}_i\left(t\right),$
where f.sub.i(t) is the thrust generated by the i.sup.th rotor at time t.

(24) B. Aerodynamic Drag

(25) The quadratic model

(26) $D=-\frac{1}{2}\rho v_a^{2}S{C}_D$
for the translational drag (i.e., drag force) is used herein, where ρ is the air density, v.sub.a is the speed of the body relative to the air, S is the cross sectional area, and C.sub.D is the drag coefficient. It can reasonably be assumed that the air density is constant at the altitude corresponding to an urban environment. However, S and C.sub.D depend on the body configuration and orientation with respect to air speed. In other words, S and C.sub.D are constants in the body frame. Therefore, the drag force can be modeled in the body frame as f.sub.D.sup.B=[−v.sub.a.sub.x.sup.B|v.sub.a.sub.x.sup.B|c.sub.D.sub.x −v.sub.a.sub.y.sup.B|v.sub.a.sub.y.sup.B|c.sub.D.sub.y −v.sub.a.sub.z.sup.B|v.sub.a.sub.z.sup.B|c.sub.D.sub.z].sup.T, where superscript B indicates body frame quantities, v.sub.a.sub.x.sup.B, v.sub.a.sub.y.sup.B, v.sub.a.sub.z.sup.B are the components of the body relative to the air velocity in the body frame, and the coefficients c.sub.D.sub.i=½ρS.sub.iC.sub.D.sub.i are constant for each axis i=x, y, z. The drag force f.sub.D.sup.B should be translated in the inertial frame in order to apply to the translational dynamics. In other words, f.sub.D=R.sub.B/Ef.sub.D.sup.B, which can be written in vector-matrix form as:

(27) $\begin{array}{cc}{f}_D=R_{B/E}\left[\begin{array}{c}-v_{a_x}^B.Math.v_{a_x}^B.Math.c_{D_X}\\ -v_{a_y}^B.Math.v_{a_y}^B.Math.c_{D_y}\\ -v_{a_z}^B.Math.v_{a_z}^B.Math.c_{D_z}\end{array}\right]=-R_{B/E}\Phi \left({\nu }_a^B\left|{\nu }_a^B\right|\right)c_D=-R_{B/E}\Phi \left(c_D\right){\nu }_a^B.Math.{\nu }_a^B.Math.& \left(5\right)\end{array}$

(28) where the notation Φ(b)=diag (b.sub.x, b.sub.y, b.sub.z) has been introduced for a vector b.

(29) A similar model is adopted herein for rotational drag (i.e., drag torque) using the body angular velocity ω.sub.a with respect to the air. In the body frame, the rotational drag can be expressed as τ.sub.D.sup.B=[−ω.sub.a.sub.x.sup.B|ω.sub.a.sub.x.sup.B|c.sub.τ.sub.x −ω.sub.a.sub.y.sup.B|ω.sub.a.sub.y.sup.B|c.sub.τ.sub.y −ω.sub.a.sub.z.sup.B|ω.sub.a.sub.z.sup.B|c.sub.τ.sub.z].sup.T, where ω.sub.a.sub.x.sup.B, ω.sub.a.sub.y.sup.B, ω.sub.a.sub.z.sup.B are the components of the body relative to the air angular velocity and c.sub.τ.sub.x, c.sub.τ.sub.y, c.sub.τ.sub.z are rotational drag coefficients, which are constant in the body frame. In this case, there is no need to express the rotational drag in the inertial frame. The following expressions for the translational and rotational drag are used herein:
f.sub.D=−R.sub.B/EΦ(v.sub.a.sup.B|v.sub.a.sup.B|)c.sub.D=−R.sub.B/EΦ(c.sub.D)v.sub.a.sup.B|v.sub.a.sup.B|  (6)
τ.sub.D=−Φ(ω.sub.a.sup.B|ω.sub.a.sup.B|)c.sub.τ=−Φ(c.sub.τ)ω.sub.a.sup.B|ω.sub.a.sup.B|  (7)

(30) where c.sub.D and c.sub.τ are constant vectors of translational and rotational drag coefficients, respectively.

(31) Adaptive Estimation, Trajectory Generation, and Flight Control

(32) FIG. 2A is a flowchart illustrating a process 200 for performing adaptive wind estimation and flight control for aerial systems using motion data, according to an embodiment of the present invention. The process includes performing adaptive estimation at 210 and generating a trajectory at 220 based on the adaptive estimation. The process also includes controlling an aircraft to track to the generated trajectory at 230. In some embodiments, steps 210, 220, and 230 may include part or all of the steps shown in FIGS. 2B, 2C, and 2D, respectively.

(33) II. Adaptive Estimation

(34) FIG. 2B is a flowchart illustrating a process 210 for performing adaptive estimation, according to an embodiment of the present invention.

(35) A. Drag Estimation

(36) First, the translational drag coefficient c.sub.D is estimated at 211 when the airspeed and the inertial velocity are equal, i.e., when the rotorcraft flies in still air (such as indoors). To this end, the inertial velocity and orientation angle measurements of the rotorcraft are used, which are available from the sensors of the rotorcraft. In addition, it is assumed here that the total thrust generated by the rotors is available from the rotor models and spin rate measurements. By representing the translational dynamics in the form:
m{dot over (v)}(t)=f.sub.T(t)R.sub.B/E(t)e.sub.3.sup.B+mg−R.sub.B/E(t)Φ(v.sub.a.sup.B(t)|v.sub.a.sup.B(t)|)c.sub.D  (8)

(37) the following prediction model results:
m {circumflex over ({dot over (v)})}(t)=f.sub.T(t)R.sub.B/E(t)e.sub.3.sup.B+mg−R.sub.B/E(t)Φ(v.sub.a.sup.B(t)|v.sub.a.sup.B(t)|)ĉ.sub.D(t)+λ.sub.l{tilde over (v)}(t)  (9)

(38) where {circumflex over (v)}(t) is the velocity prediction, ĉ.sub.D(t) is the translational drag coefficient estimate, λ.sub.l is the error feedback gain, and {tilde over (v)}(t)=v(t)−{circumflex over (v)}(t) is the prediction error. The adaptive law for ĉ.sub.D(t) is derived from the Lyapunov stability analysis for the prediction error dynamics:
m {circumflex over ({dot over (v)})}(t)=−λ.sub.l{tilde over (v)}(t)−R.sub.B/E(t)Φ(v.sub.a.sup.B(t)|v.sub.a.sup.B(t)|){tilde over (c)}.sub.D(t)  (10)

(39) where {tilde over (c)}.sub.D(t)=c.sub.D−ĉ.sub.D(t) is the estimation error. The Lyapunov function is chosen as:

(40) $\begin{array}{cc}L\left(t\right)=\frac{m}{2}{\tilde{\nu }}^T\left(t\right)\tilde{\nu }\left(t\right)+\frac{1}{2{\gamma }_l}{\tilde{c}}_D^T\left(t\right){\tilde{c}}_D\left(t\right)& \left(11\right)\end{array}$

(41) where γ.sub.l>0 is the adaptation rate. It is straightforward that:

(42) $\begin{array}{cc}\stackrel{.}{L}\left(t\right)=-{\lambda }_l{\tilde{\nu }}^T\left(t\right)\tilde{\nu }\left(t\right)+{\tilde{c}}_D^T\left(t\right)\left[-\Phi \left({\nu }_a^B\left(t\right).Math.{\nu }_a^B\left(t\right).Math.\right)R_{E/B}\left(t\right)\tilde{\nu }\left(t\right)+\frac{1}{{\gamma }_l}{\stackrel{.}{\tilde{c}}}_D\left(t\right)\right]& \left(12\right)\end{array}$

(43) Therefore, defining the adaptive law as:
{circumflex over (ċ)}.sub.D(t)=−γ.sub.lΦ(v.sub.a.sup.B(t)|v.sub.a.sup.B(t)|)R.sub.E/B(t){tilde over (v)}(t)  (13)

(44) renders {dot over (L)}(t) negative semidefinite, implying that {tilde over (v)}(t) and {tilde over (c)}.sub.D(t) are globally bounded. In addition, application of Barbalat's lemma ensures that {tilde over (v)}(t).fwdarw.0 as t.fwdarw.∞ when v(t) is bounded. Since Eq. (10) is a linear time invariant (LTI) system with the input R.sub.B/E(t)Φ(v.sub.a.sup.B(t)|v.sub.a.sup.B(t)|)c.sub.D(t), it follows that R.sub.B/E(t)Φ(v.sub.a.sup.B(t)|v.sub.a.sup.B(t)|){tilde over (c)}.sub.D(t).fwdarw.0 as t.fwdarw.∞ if Φ(v.sub.a.sup.B(t)|v.sub.a.sup.B(t)|) is nonsingular (i.e., if the airspeed components v.sub.x.sup.B(t), v.sub.y.sup.B(t), v.sub.z.sup.B(t) are nonzero). In fact, in this case, the convergence is exponential.

(45) Next, the rotational drag coefficient is estimated at 212 using the prediction of the angular rate dynamics:
J{circumflex over ({dot over (ω)})}(t)=−ω(t)×Jω(t)+J.sub.mω.sub.m(t)ω(t)+τ(t)−Φ(ω.sub.a.sup.B(t)|ω.sub.a.sup.B(t)|)ĉ.sub.τ(t)+λ.sub.r{circumflex over (ω)}(t)  (14)

(46) where {circumflex over (ω)}(t) is the prediction of the vehicle's angular rate, ĉ.sub.τ(t) is the estimation of the rotational drag coefficient, λ.sub.r>0 is the error feedback gain, and {tilde over (ω)}(t) is the prediction error. The adaptive law for the estimate ĉ.sub.τ(t) is given by:
{circumflex over (ċ)}.sub.τ(t)=−γ.sub.rΦ(ω.sub.a.sup.B(t)|ω.sub.a.sup.B(t)|){tilde over (ω)}(t)  (15)

(47) which results in the error system:
J{tilde over ({dot over (ω)})}(t)=−λ.sub.r{tilde over (ω)}(t)−Φ(ω.sub.a.sup.B(t)|ω.sub.a.sup.B(t)|){tilde over (c)}.sub.τ(t)  (16)
{tilde over (ċ)}(t)=γ.sub.rΦ(ω.sub.a.sup.B(t)|ω.sub.a.sup.B(t)|){tilde over (ω)}(t)  (17)

(48) As in the previous case, it can be shown that the error system in Eq. (16) is globally stable, and {tilde over (ω)}(t).fwdarw.0 as t.fwdarw.∞ when ω(t) is bounded. Additionally, if all of the components of ω(t) are nonzero, {tilde over (c)}.sub.τ(t).fwdarw.0 exponentially as t.fwdarw.∞. It should be noted that in the case of rotational drag, partial convergence is possible. In other words, if all components are not nonzero, the rotational drag coefficients corresponding to the nonzero components exponentially converge to their true values since Eqs. (16) and (17) are decoupled.

(49) B. Wind Estimation

(50) Once the translational and rotational drag coefficients have been estimated, wind velocities and accelerations are estimated at 213. To this end, the dynamics equations can be written as:

(51) $\begin{array}{cc}\stackrel{.}{\nu }\left(t\right)=\frac{f_T\left(t\right)}{m}{R}_{B/E}\left(t\right)e_3^B+g-\frac{1}{m}{R}_{B/E}\left(t\right)\Phi \left(c_D\right){\nu }_a^B\left(t\right).Math.{\nu }_a^B\left(t\right).Math.& \left(18\right)\\ \stackrel{.}{\omega }\left(t\right)=-J^{-1}\omega \left(t\right)\times J\omega \left(t\right)+J_m{\omega }_m\left(t\right)J^{-1}\stackrel{\_}{\omega }\left(t\right)+J^{-1}\tau \left(t\right)-J^{-1}\Phi \left(c_{\tau }\right){\omega }_a^B\left(t\right).Math.{\omega }_a^B\left(t\right).Math.& \left(19\right)\end{array}$

(52) where the airspeed v.sub.a.sup.B(t)=v.sup.B(t)−w.sup.B(t) also includes the wind velocity w.sup.B(t) expressed in the body frame, and the relative angular rate ω.sub.a.sup.B(t)=ω(t)−ω.sub.c.sup.B(t) includes the air mass circulation rate (i.e., vorticity) ω.sub.c.sup.B(t) expressed in the body frame. The terms −m.sup.−1R.sub.B/E(t)Φ(c.sub.D)v.sub.a.sup.B(t)|v.sub.a.sup.B(t)| and −J.sup.−1Φ(c.sub.τ)ω.sub.a.sup.B(t)|ω.sub.a.sup.B(t)| are treated as external disturbance signals s.sub.v(t) and s.sub.ω(t), respectively, so Eqs. (18) and (19) take the form:

(53) $\begin{array}{cc}\stackrel{.}{\nu }\left(t\right)=\frac{1}{m}{f}_T\left(t\right)R_{B/E}\left(t\right)e_3^B+g+s_v\left(t\right)& \left(20\right)\\ \stackrel{.}{\omega }\left(t\right)=-J^{-1}\omega \left(t\right)\times J\omega \left(t\right)+J_m{\omega }_m\left(t\right)J^{-1}\stackrel{\_}{\omega }\left(t\right)+J^{-1}\tau \left(t\right)+s_{\omega }\left(t\right)& \left(21\right)\end{array}$

(54) Similar to the drag estimation case, the prediction model and adaptive law for the translational dynamics are introduced as:

(55) $\begin{array}{cc}\stackrel{.}{\hat{\nu }}\left(t\right)=\frac{1}{m}{f}_T\left(t\right)R_{B/E}\left(t\right)e_3^B+g+{\hat{s}}_v\left(t\right)+{\lambda }_v\tilde{\nu }\left(t\right)& \left(22\right)\\ {\stackrel{.}{\hat{s}}}_v\left(t\right)={\gamma }_v\tilde{v}\left(t\right)& \left(23\right)\end{array}$

(56) where λ.sub.v>0 and γ.sub.v>0 are design parameters, {tilde over (v)}(t)=v(t)−{circumflex over (v)}(t) is the inertial velocity prediction error, and ŝ.sub.v(t) is the disturbance estimate. For the rotational dynamics:
{circumflex over ({dot over (ω)})}(t)=−J.sup.−1ω(t)×Jω(t)+J.sub.mω.sub.m(t)J.sup.−1ω(t)+J.sup.−1τ(t)+ŝ.sub.ω(t)+λ.sub.ω{tilde over (ω)}(t)  (24)
{circumflex over ({dot over (s)})}.sub.ω(t)=γ.sub.ω{tilde over (ω)}(t)  (25)

(57) where Δ.sub.ω>0 and γ.sub.ω>0 are design parameters, {tilde over (ω)}(t)=ω(t)−{circumflex over (ω)}(t) is the angular rate prediction error, and ŝ.sub.ω(t) is the disturbance estimate. Denoting the estimation errors as {tilde over (s)}.sub.v(t)=s.sub.v(t)−ŝ.sub.v(t) and {tilde over (s)}.sub.ω(t)=s.sub.ω(t)−ŝ.sub.ω(t), the error system can be derived as:
{tilde over ({dot over (v)})}(t)=λ.sub.v{tilde over (v)}(t)+{tilde over (s)}.sub.v(t)  (26)
{tilde over ({dot over (s)})}.sub.v(t)=−γ.sub.v{tilde over (v)}(t)+{dot over (s)}.sub.v(t)  (27)

(58) for the translational dynamics and
{tilde over ({dot over (ω)})}(t)=λ.sub.ω{tilde over (ω)}(t)+{tilde over (s)}.sub.ω(t)  (28)
{tilde over ({dot over (s)})}.sub.ω(t)=−γ.sub.ω{tilde over (ω)}(t)+{dot over (s)}.sub.ω(t)  (29)

(59) for the rotational dynamics.

(60) The error system of Eqs. (26)-(29) is a stable LTI system with inputs {dot over (s)}.sub.v(t) and {dot over (s)}.sub.ω(t). Thus, these equations have bounded solutions if the signals {dot over (s)}.sub.v(t) and {dot over (s)}.sub.ω(t) are essentially bounded (i.e., bounded everywhere except for on the sets that measure zero). Assuming that the flight control system provides a bounded and continuous inertial velocity and angular rate (or at least provides these sufficiently frequently), the approach of some embodiments can produce valid estimates of any wind field, even if the wind components abruptly change during a significant number of measurement instances over time.

(61) To derive the upper bounds on the components of the estimation errors, it should be noted that Eqs. (26) and (27) and Eqs. (28) and (29) are decoupled. Accordingly, the following generic system may be introduced:
{dot over (x)}.sub.1(t)=−λx.sub.1(t)+x.sub.2(t)  (30)
{dot over (x)}.sub.2(t)=−γx.sub.1(t)+f(t)  (31)

(62) where x.sub.1(t) represents any component of the linear velocity prediction errors {tilde over (v)}(t) or angular velocity prediction errors {tilde over (ω)}(t), and x.sub.2(t) and f(t) represent the corresponding components of {tilde over (s)}.sub.v(t) or {tilde over (s)}.sub.ω(t), and {dot over (s)}.sub.v(t) or {dot over (s)}.sub.ω(t), respectively.

(63) It should be noted that if f(t)=0, then x.sub.1(t) and x.sub.2(t) exponentially converge to zero from all initial conditions, which means that translational and rotational drag estimates exponentially converge to true values on any interval where the corresponding linear or angular drag components are constant.

(64) Next, ignoring the exponentially decaying effects of the initial errors, the rate of decay for which is given by the design parameters k.sub.v and k.sub.ω, the solution of EQs. (30) and (31) can be represented in the operator form as:

(65) 0$\begin{array}{cc}{x}_1\left(s\right)=\frac{1}{s^2+\lambda s+\gamma }f\left(s\right),x_2\left(s\right)=\frac{s+\lambda }{s^2+\lambda s+\gamma }f\left(s\right)& \left(32\right)\end{array}$

(66) Since

(67) $\begin{array}{cc}{.Math.\frac{1}{s^2+\lambda s+\gamma }.Math.}_{H_{\infty }}=\frac{1}{\gamma },{.Math.\frac{s+\lambda }{s^2+\lambda s+\gamma }.Math.}_{H_{\infty }}=\frac{\lambda }{\gamma }& \left(33\right)\end{array}$

(68) it can be concluded that

(69) $\begin{array}{cc}.Math.x_1\left(t\right).Math.\le \frac{1}{\gamma }\mathrm{ess}\sup .Math.f_{\left[0,t\right]}.Math.,.Math.x_1\left(t\right).Math.\le \frac{\lambda }{\gamma }\mathrm{ess}\sup .Math.f_{\left[0,t\right]}.Math.& \left(34\right)\end{array}$

(70) where ess sup|f.sub.[0,t]| denotes the essential supremum of |f(t)| on the [0, t] interval. It follows that the drag estimation errors {tilde over (s)}.sub.v(t) or {tilde over (s)}.sub.ω(t) can be decreased as desired by the proper choice of design parameters γ and λ.

(71) The translational and rotational drag estimates ŝ.sub.v(t) and ŝ.sub.ω(t) will be used for the trajectory generation and control design purposes. Their respective rates of change are generated according to the corresponding adaptive laws.

(72) The wind linear and angular change velocities can be computed from the equations:

(73) $\begin{array}{cc}{\hat{s}}_v\left(t\right)=-\frac{1}{m}{R}_{\frac{B}{E}}\left(t\right)\Phi \left(c_D\right){\nu }_a^B\left(t\right).Math.{\nu }_a^B\left(t\right).Math.& \left(35\right)\\ {\hat{s}}_{\omega }\left(t\right)=-J^{-1}\Phi \left(c_{\tau }\right){\omega }_a^B\left(t\right).Math.{\omega }_a^B\left(t\right).Math.& \left(36\right)\end{array}$

(74) Since ŝ.sub.v(t) is in the inertial frame, it is first translated to the body frame ŝ.sub.v.sup.B(t)=R.sub.E/B(t)ŝ.sub.v(t). Once this is done, it should be noted that the components of s.sub.v.sup.B(t) have signs opposite to those of the corresponding components of v.sub.a.sup.B(t) in the body frame. Therefore, the following equations can be written:
c.sub.Dx|v.sub.ax.sup.B(t)|.sup.2=m|ŝ.sub.vx.sup.B(t)|  (37)
c.sub.Dy|v.sub.ay.sup.B(t)|.sup.2=m|ŝ.sub.vy.sup.B(t)|  (38)
c.sub.Dz|v.sub.az.sup.B(t)|.sup.2=m|ŝ.sub.vz.sup.B(t)|  (39)

(75) Solving for the wind components in the body frame yields:

(76) $\begin{array}{cc}{\hat{w}}_x^B\left(t\right)=v_x^B\left(t\right)-\mathrm{sign}\left({\hat{s}}_{vx}^B\left(t\right)\right)\sqrt{\frac{m}{c_{Dx}}.Math.{\hat{s}}_{vx}^B\left(t\right).Math.}& \left(40\right)\\ {\hat{w}}_y^B\left(t\right)=v_y^B\left(t\right)-\mathrm{sign}\left({\hat{s}}_{vy}^B\left(t\right)\right)\sqrt{\frac{m}{c_{Dy}}.Math.{\hat{s}}_{vy}^B\left(t\right).Math.}& \left(41\right)\\ {\hat{w}}_z^B\left(t\right)=v_z^B\left(t\right)-\mathrm{sign}\left({\hat{s}}_{vz}^B\left(t\right)\right)\sqrt{\frac{m}{c_{Dz}}.Math.{\hat{s}}_{vz}^B\left(t\right).Math.}& \left(42\right)\end{array}$

(77) The wind angular velocity components are found in a similar manner:

(78) $\begin{array}{cc}{\hat{\omega }}_{cx}^B\left(t\right)=p\left(t\right)-\mathrm{sign}\left({\hat{s}}_{\omega x}^B\left(t\right)\right)\sqrt{\frac{J_1}{c_{\tau x}}.Math.{\hat{s}}_{\omega x}^B\left(t\right).Math.}& \left(43\right)\\ {\hat{\omega }}_{cy}^B\left(t\right)=q\left(t\right)-\mathrm{sign}\left({\hat{s}}_{\omega y}^B\left(t\right)\right)\sqrt{\frac{J_2}{c_{\tau y}}y.Math.{\hat{s}}_{\omega y}^B\left(t\right).Math.}& \left(44\right)\\ {\hat{\omega }}_{cz}^B\left(t\right)=r\left(t\right)-\mathrm{sign}\left({\hat{s}}_{\omega z}^B\left(t\right)\right)\sqrt{\frac{J_3}{c_{\tau z}}.Math.{\hat{s}}_{\omega z}^B\left(t\right).Math.}& \left(45\right)\end{array}$

(79) Next, the wind linear and angular accelerations are computed using {circumflex over ({dot over (s)})}.sub.v(t) and {circumflex over ({dot over (s)})}.sub.ω(t) from the prediction models of Eqs. (22)-(25). Differentiating Eqs. (35) and (36) with respect to time:

(80) $\begin{array}{cc}{\stackrel{.}{\hat{s}}}_v\left(t\right)=-\frac{1}{m}{R}_{B/E}\left(t\right)\omega ⨯\left(t\right)\Phi \left(c_D\right){\nu }_a^B\left(t\right).Math.{\nu }_a^B\left(t\right).Math.-\frac{2}{m}{R}_{B/E}\left(t\right)\Phi \left(c_D\right){\stackrel{.}{\nu }}_a^B\left(t\right).Math.{\stackrel{.}{\nu }}_a^B\left(t\right).Math.& \left(46\right)\\ {\stackrel{.}{\hat{s}}}_{\omega }\left(t\right)=-2J^{-1}\Phi \left(c_{\tau }\right){\stackrel{.}{\omega }}_a^B\left(t\right).Math.{\omega }_a^B\left(t\right).Math.& \left(47\right)\end{array}$

(81) Taking into account

(82) $\begin{array}{cc}\begin{array}{c}{\stackrel{.}{\nu }}_a^B\left(t\right)={\stackrel{.}{\nu }}^B\left(t\right)-{\stackrel{.}{w}}^B\left(t\right)\mathrm{and}{\stackrel{.}{\omega }}_a^B\left(t\right)={\stackrel{.}{\omega }}^B\left(t\right)-{\stackrel{.}{\omega }}_c^B\left(t\right),\\ {\stackrel{.}{\hat{\omega }}}^B\left(t\right)=\frac{\begin{array}{c}{\stackrel{.}{\hat{s}}}_v\left(t\right)+\frac{1}{m}{R}_{B/E}\left(t\right)\omega ⨯\left(t\right)\Phi \left(c_D\right){\nu }_a^B\left(t\right).Math.{\nu }_a^B\left(t\right).Math.+\\ \frac{2}{m}{R}_{B/E}\left(t\right)\Phi \left(c_D\right){\stackrel{.}{\nu }}^B\left(t\right).Math.{\nu }_a^B\left(t\right).Math.\end{array}}{\frac{2}{m}{R}_{B/E}\left(t\right)\Phi \left(c_D\right).Math.{\nu }_a^B\left(t\right).Math.}\end{array}& \left(48\right)\\ {\stackrel{.}{\hat{\omega }}}_c^B\left(t\right)=\frac{{\stackrel{.}{\hat{s}}}_{\omega }\left(t\right)+2J^{-1}\Phi \left(c_{\tau }\right){\stackrel{.}{\omega }}^B\left(t\right).Math.{\omega }_a^B\left(t\right).Math.}{2J^{-1}\Phi \left(c_{\tau }\right)\left|{\omega }_a^B\left(t\right)\right|}& \left(49\right)\end{array}$

(83) where division is understood to be component-wise. It should be noted that the wind acceleration estimates involve the inertial linear and angular accelerations of the rotorcraft expressed in the body frame.

(84) III. Trajectory Generation

(85) FIG. 2C is a flowchart illustrating a process 220 for generating a trajectory based on the adaptive estimation, according to an embodiment of the present invention. In some embodiments, generating a trajectory through waypoints may take less than one millisecond. However, the processing time depends on the computational power of the given rotorcraft. The trajectory may be re-planned periodically (e.g., every five seconds). Each re-planning may involve executing process 220 of FIG. 2C again.

(86) The trajectory generation algorithm takes into account the atmospheric effects in multi-copter dynamics using the estimates ŵ(t), {circumflex over (ω)}.sub.c(t), ŝ.sub.v(t), ŝ.sub.ω(t), and their derivatives from step 210 of the process described above. For this purpose, the following simplified equation of motion is considered:
{dot over (v)}(t)=f(t)R.sub.B/E(t)e.sub.3.sup.B+g+ŝ.sub.v(t)  (50)

(87) where the rotation matrix R.sub.B/E (t) evolves according to:
{dot over (R)}.sub.B/E(t)=R.sub.B/E(t)ω.sup.x(t)  (51)

(88) and the mass-normalized total thrust

(89) $\stackrel{\_}{f}\left(t\right)=\frac{f_T\left(t\right)}{m}$
and the angular rate ω(t) are viewed as control inputs. The justification of this simplification is that the controller designed for the angular rate dynamics
{dot over (ω)}(t)=−J.sup.−1ω(t)×Jω(t)+J.sub.mω.sub.m(t)J.sup.−1ω(t)+J.sup.−1τ(t)+ŝ.sub.ω(t)  (52)

(90) can provide fast and accurate tracking of the angular rate commands in the presence of rotational drag with or without wind. The design of the controller in some embodiments is presented in the next section.

(91) To generate trajectories, the jerk minimization approach of Eqs. (26) and (27) is modified so that the estimate ŝ.sub.v(t) of the aerodynamic drag can be directly taken into account. For the given trajectory r(t)=[x(t) y(t) z(t)].sup.T, the mass-normalized thrust vector T(t)=f(t)R.sub.B/E(t)e.sub.3.sup.B can be expressed as:
T(t)=(t)−g−ŝ.sub.v(t)  (53)

(92) which implies that
∥T(t)∥=∥(t)−g−ŝ.sub.v(t)∥=f(t)  (54)

(93) In other words, the mass-normalized total thrust magnitude to traverse the given trajectory is defined by Eq. (54). The orientation of the thrust vector is defined by roll and pitch angles, the rate of change of which is related to the jerk (i.e., the third derivative of the position) of the given trajectory through Eqs. (50) and (51). Differentiating Eqs. (50) and (54) yields:
(t)={dot over (f)}(t)R.sub.B/E(t)e.sub.3.sup.B+f(t)R.sub.B/E(t)ω.sup.x(t)e.sub.3.sup.B+{circumflex over ({dot over (s)})}.sub.v(t)  (55)
{dot over (f)}(t)=(e.sub.3.sup.B).sup.TR.sub.B/E(t) (t)−{circumflex over ({dot over (s)})}.sub.v(t))  (56)

(94) Solving for angular rates results in:

(95) $\begin{array}{cc}\left[\begin{array}{c}{\omega }_y\left(t\right)\\ -{\omega }_x\left(t\right)\\ 0\end{array}\right]=\frac{1}{\overline{f}\left(t\right)}\left[\begin{array}{ccc}1& 0& 0\\ 0& 1& 0\\ 0& 0& 0\end{array}\right]R_{B/E}\left(t\right)\left(\stackrel{\mathrm{.Math.}}{r}\left(t\right)-{\stackrel{.}{\hat{s}}}_v\left(t\right)\right)& \left(57\right)\end{array}$

(96) which implies that:

(97) 0$\begin{array}{cc}\sqrt{{\omega }_x^2\left(t\right)+{\omega }_y^2\left(t\right)}\le \frac{1}{\stackrel{\_}{f}\left(t\right)}.Math.\stackrel{\mathrm{.Math.}}{r}\left(t\right)-{\hat{s}}_v\left(t\right).Math.& \left(58\right)\end{array}$

(98) Eq. (57) defines the angular rates ω.sub.x and ω.sub.y required to traverse the given trajectory. A given trajectory can be traversed using the control inputs f, ω.sub.x, and ω.sub.y. In other words, only these three control inputs are needed to generate a three-dimensional (3D) trajectory in some embodiments. Then, ω.sub.z can be used to control the onboard sensor direction (i.e., the rotation of the rotorcraft around the thrust vector) for surveillance, mapping, package delivery, etc. herein, it is assumed that ω.sub.z=0.

(99) The total thrust generated by the motors satisfies the physical constraint:
0≤f.sub.min≤f(t)≤f.sub.max  (59)

(100) and the angular rate input is bounded due to sensor limitations as:
−ω.sub.max≤ω.sub.a.sup.B(t)≤ω.sub.max  (60)

(101) which directly takes into account the estimate of wind vorticity {circumflex over (ω)}.sub.c.sup.B(t).

(102) Following the steps of Eqs. (26) and (27), single axis motion primitives are generated using a third order system:
.sub.j(t)=u.sub.j(t)  (61)

(103) with performance index

(104) $\begin{array}{cc}J={\int }_0^{t_f}{u}_j^2\left(\tau \right)d\tau & \left(62\right)\end{array}$

(105) initial conditions s.sub.j(0),{dot over (s)}.sub.j(0),{umlaut over (s)}.sub.j(0) and final conditions s.sub.j(t.sub.f),.sub.j(t.sub.f),{umlaut over (s)}.sub.j(t.sub.f) for each j=x, y, z, where t.sub.f is the time to traverse. The resulting closed form s.sub.j is a 5.sup.th order polynomial in time, the coefficients of which depend on final time t.sub.f and final conditions. Therefore, to fully define s(t)=[s.sub.x(t) s.sub.y(t) s.sub.z(t)].sup.T, the initial state s(0),{dot over (s)}(0),{umlaut over (s)}(j) needs to be selected, which is assumed herein to be coincident with the current state r(t), v(t), a(t) of the rotorcraft (assuming all measurements are available), the final time t.sub.f, and the final state s(t.sub.f),(t.sub.f),{umlaut over (s)}(t.sub.f) represent the final position, velocity, and acceleration. Velocity and acceleration at the final time may be free or specified without deviating from the scope of the invention.

(106) The following algorithm is used for trajectory generation in some embodiments. Let a series of waypoints p.sub.j, j=1, . . . , N.sub.p be provided by any desired planner as a matter of design choice without deviating from the scope of the invention, and let p.sub.0=r(t), where r(t) is the position of the vehicle at a current time t. An example of waypoints along a trajectory is shown in graph 300 of FIG. 3 with arbitrary coordinates. As can be seen, each pair of adjacent waypoints is connected by a respective line segment.

(107) For each line segment of the trajectory, the minimum possible time-to-go is computed at 221 by dividing the distance between the waypoints by the maximum possible velocity V.sub.max. If V.sub.max is not available from the specifications of the vehicle, the following equation may be solved:
f.sub.maxe.sub.3.sup.B+max R.sub.E/B(t)g=Φ(c.sub.D)(v.sup.B(t)−ŵ.sup.B(t))|v.sup.B(t)−ŵ.sup.B(t)|  (63)

(108) for v.sup.B(t) and set V.sub.max=∥v.sup.B(t)∥, using the available wind velocity estimate ŵ.sup.B(t). The resulting time-to-go t.sub.go may be used as a first iteration for the final time t.sub.f.sub.1=t.sub.go.

(109) A trajectory for a segment s(η) is generated at 222 on the interval t≤η≤t.sub.f.sub.1 according to the optimal control problem in Eqs. (61) and (62). The mass-normalized thrust vector T(η) required to traverse the trajectory s(η) component-wise is then computed at 223 using the equations:
T.sub.x(η)=.sub.x(η)−ŝ.sub.vx*(t)  (64)
T.sub.y(η)=.sub.y(η)−ŝ.sub.vy*(t)  (65)
T.sub.z(η)=.sub.z(η)−ŝ.sub.vz*(t)+g  (66)

(110) where ŝ.sub.v*(t) is the worst case prediction of the maximum drag force, since the estimates of the drag force ŝ(t) and the wind velocity ŵ(t) are available only at current time t. This prediction can be computed as s.sub.v*(t)=ŝ.sub.v(t)+η{circumflex over ({dot over (s)})}.sub.v(t), assuming a constraint {circumflex over ({dot over (s)})}.sub.v(t) on the interval t≤η≤t.sub.f.sub.1. The total mass-normalized thrust is computed as

(111) $\overline{f}\left(\eta \right)=\sqrt{T_x^2\left(\eta \right)+T_y^2\left(\eta \right)+T_z^2\left(\eta \right)}$
and the minimum and maximum values f.sub.min and f.sub.max are numerically are numerically computed on the interval t≤η≤t.sub.f.sub.1.

(112) The thrust feasibility conditions of Eq. (59) is then checked for f.sub.max and f.sub.min at 224. If the conditions are satisfied, then the following equation is computed:

(113) $\begin{array}{cc}\chi \left(t,t_{f_1}\right)={\max }_{t\le \eta \le t_{f_1}}\sqrt{{\left[{\stackrel{⃛}{s}}_x\left(\eta \right)-{\stackrel{.}{\hat{s}}}_{vx}\left(t\right)\right]}^2+{\left[{\stackrel{⃛}{s}}_y\left(\eta \right)-{\stackrel{.}{\hat{s}}}_{vy}\left(t\right)\right]}^2+{\left[{\stackrel{⃛}{s}}_z\left(\eta \right)-{\stackrel{.}{\hat{s}}}_{vz}\left(t\right)\right]}^2}& \left(67\right)\end{array}$

(114) assuming that {circumflex over ({dot over (s)})}.sub.v(t) is constant on the interval t≤η≤t.sub.f.sub.1, and the rate feasibility is checked at 225. If the rate is feasible, the trajectory s(η), t≤η≤t.sub.f.sub.1 is marked as a feasible trajectory between waypoints p.sub.0 and p.sub.1 at 226, and the algorithm is advanced to the next waypoint at 227 taking s(t.sub.f.sub.1), {dot over (s)}(t.sub.f.sub.1), {umlaut over (s)}(t.sub.f.sub.1) as the initial state of the next segment of the trajectory.

(115) However, if any of the above checks fail, the algorithm sets t.sub.f=t.sub.f+iΔt at 228 for some time step Δt and i=1, . . . , N. The algorithm then returns to step 222. This iteration is continued until a preset reasonable number N is reached. If no feasible trajectory is found, then the algorithm may exit with no feasible trajectory generated for the corresponding waypoint. The algorithm may then wait to receive a new set of waypoints from the planner. In the meantime, the rotorcraft can execute the generated segment of the trajectory if one exists, or hover or land if not. Landing may also be selected as an option if insufficient battery power or fuel remains.

(116) When the algorithm of FIG. 2C exits with a feasible trajectory, it is a sub-minimum time trajectory. This means that for each segment of the trajectory, the time to traverse is within a Δt margin of a true minimum time trajectory.

(117) With respect to the final state s(t.sub.f.sub.1), {dot over (s)}(t.sub.f.sub.1), {umlaut over (s)}(t.sub.f.sub.1) selection in the optimal control problem formulation in Eqs. (61) and (62), the first state (position) s.sub.j(t.sub.f.sub.1), i=1, . . . , N in each direction j=x, y, z is set to a corresponding waypoint p.sub.i=[x.sub.i y.sub.i z.sub.i].sup.T provided by the planner. The third (acceleration) state ŝ(t.sub.f.sub.1) is set to zero for all waypoints. The second (velocity) state, {dot over (s)}(t.sub.f.sub.1) is selected as follows. The algorithm sets

(118) $\stackrel{.}{s}\left(t_{f_{N_p}}\right)=0$
and

(119) $\stackrel{\mathrm{.Math.}}{s}\left(t_{f_{N_p}}\right)=0$
at the final waypoint p.sub.N.sub.p (arriving at rest). For all other waypoints, set {dot over (s)}.sub.x(t.sub.f.sub.i)=0 if |x.sub.i+1−x.sub.i|≤ε.sub.x and leave {dot over (s)}.sub.x(t.sub.f.sub.i) free (unspecified) otherwise. Here, ε>0 is a design parameter that the designer can choose according to the scale of the distance to be traveled in the x direction. For the remaining two directions (i.e., y and z), the waypoint velocity is set in a similar manner. This setup enables the rotorcraft to travel along the straight (or approximately straight) trajectories with maximal speed without slowing down at waypoints and engaging in excessive cross-track maneuvers.

(120) IV. Trajectory Tracking Controller

(121) In this section, a controller is described for the rotorcraft to track the 3D trajectory r.sub.com(t)=[x.sub.com(t) y.sub.com(t) z.sub.com(t)].sup.T and the heading angle (ψ.sub.com(t)) commands generated in the previous section. FIG. 2D is a flowchart illustrating a process 230 for controlling an aircraft to track to the trajectory generated in FIG. 2C, according to an embodiment of the present invention.

(122) A. Center of Gravity (CG) Control

(123) The motion of the CG of the rotorcraft is controlled at 231 by the thrust vector f(t)R.sub.B/E(t)e.sub.3.sup.B or by the magnitude of total thrust f(t) and the orientation angles ϕ(t) and θ(t) according to the force equation in Eq. (50), which are designed from the perspective of tracking the trajectory command r.sub.com(t) or the velocity command v.sub.com(t), depending on the preference of the designer. When the objective is to track r.sub.com(t), the corresponding v.sub.com(t) can be generated using a backstepping approach as:
v.sub.com(t)=c.sub.1[r.sub.com(t)−r(t)]+{dot over (r)}.sub.com(t)  (68)

(124) where c.sub.1>0 is a design parameter, otherwise:
v.sub.com(t)={dot over (r)}.sub.com(t)  (69)

(125) The control law is defined according to the equation:
f(t)R.sub.B/E(t)e.sub.3.sup.B=−ŝ.sub.v(t)−ŝ.sub.v(t)−g+c.sub.2[v.sub.com(t)−v(t)]+{dot over (v)}.sub.com(t)  (70)

(126) where c.sub.2>0 is a design parameter and {dot over (v)}.sub.com(t) is computed respectively from Eq. (68) or Eq. (69). Substituting the control law in Eq. (50) results in the exponentially stable error dynamics:
ė.sub.v(t)=−c.sub.2e.sub.v(t)  (71)

(127) for the tracking error e.sub.v(t)=v(t)−v.sub.com(t).

(128) The requisite total thrust and orientation angle may be obtained from Eq. (70) assuming that −π/2<ϕ, θ<π/2 (in other words, assuming that there are no flip-over maneuvers). This assumption ensures that the functions cos ϕ and cos θ are nonzero, and that sin ϕ and sin θ are one-to-one invertible. It follows from Eq. (70) written component-wise that:
f(t)[cos ϕ(t)sin θ(t)cos ψ(t)+sin ϕ(t)sin ψ(t)]=−ŝ.sub.vx(t)−c.sub.1ė.sub.x(t)−c.sub.2e.sub.x(t)+{umlaut over (x)}.sub.ref(t)≙k.sub.x(t)  (72)
f(t)[cos ϕ(t)sin θ(t)cos ψ(t)+sin ϕ(t)sin ψ(t)]=−ŝ.sub.vy(t)−c.sub.1ė.sub.y(t)−c.sub.2e.sub.y(t)+ÿ.sub.ref(t)≙k.sub.y(t)  (73)
f(t)cos ϕ(t)cos θ(t)=g−ŝ.sub.vz(t)−c.sub.1ė.sub.z(t)−c.sub.2e.sub.z(t)+.sub.ref(t)≙k.sub.z(t)  (74)

(129) The total thrust can be obtained from Eq. (74) as:

(130) $\begin{array}{cc}{f}_T\left(t\right)=m\frac{{\kappa }_z\left(t\right)}{\cos \varphi \left(t\right)\cos \theta \left(t\right)}& \left(75\right)\end{array}$

(131) which basically controls the altitude or vertical speed of the rotorcraft. Next, multiplying Eq. (72) by cos ψ(t), multiplying Eq. (73) by sin ψ(t), and then adding and subtracting yields:

(132) $\begin{array}{cc}{\varphi }_{com}\left(t\right)={\sin }^{-1}\left(\frac{{\kappa }_x\left(t\right)\sin \psi \left(t\right)-{\kappa }_y\left(t\right)\cos \psi \left(t\right)}{f_T\left(t\right)}\right)& \left(76\right)\\ {\theta }_{com}\left(t\right)={\sin }^{-1}\left(\frac{{\kappa }_x\left(t\right)\cos \psi \left(t\right)-{\kappa }_y\left(t\right)\sin \psi \left(t\right)}{f_T\left(t\right)\cos \varphi \left(t\right)}\right)& \left(77\right)\end{array}$

(133) Changing ϕ and θ changes the thrust direction, which enables motion of the CG.

(134) B. Attitude Control

(135) The attitude control is determined at 232. The control torque for the rotational dynamics can now be derived such that the Euler angle E(t) tracks the reference signal E.sub.ref(t) generated through the dynamics:
Ė.sub.ref(t)=−c.sub.E[E.sub.ref(t)−E.sub.com(t)]  (78)

(136) where c.sub.E>0 is a design constant and E.sub.com(t)=[ϕ.sub.com(t) θ.sub.com(t) ψ.sub.com(t)].sup.T is the combination of roll and pitch angles commands obtained from the perspective of the position tracking and yaw angle command provided by the trajectory generation algorithm. Using time scale separation and dynamic inversion techniques, an expression for the desired angular rates can be derived as follows:
ω.sub.com(t)=H.sup.−1(t)[−c.sub.ωe.sub.E(t)+Ė.sub.ref(t)]  (79)

(137) where e.sub.E(t)=E(t)−E.sub.ref(t) is the attitude angles tracking error, c.sub.ω>0 is the control gain, and H.sup.−1(t) is the inverse matrix of H(t) given by:

(138) $\begin{array}{cc}{H}^{-1}\left(t\right)=\left[\begin{array}{ccc}1& 0& -\sin \theta \left(t\right)\\ 0& \cos \varphi \left(t\right)& \sin \varphi \left(t\right)\cos \theta \left(t\right)\\ 0& -\sin \varphi \left(t\right)& \cos \varphi \left(t\right)\cos \theta \left(t\right)\end{array}\right]& \left(80\right)\end{array}$

(139) The control torque can then be derived using Eq. (52) as follows:
τ(t)=ω(t)×Jω(t)−J.sub.r3Ω(t)ω(t)−ŝ.sub.ω(t)+J[−c.sub.ωe.sub.ω(t)+{dot over (ω)}.sub.ref(t)]  (81)

(140) where c.sub.ω>0 is the control gain and e.sub.ω(t)=ω(t)−ω.sub.ref(t) is the angular rate tracking error, which satisfies the exponentially stable dynamics:
ė.sub.ω(t)=−c.sub.ωe.sub.ω(t)  (82)

(141) and the signal ω.sub.ref(t) is generated through the reference dynamics:
{dot over (ω)}.sub.ref(t)=−c.sub.ω[ω.sub.ref(t)−ω.sub.com(t)]  (83)

(142) The individual motor inputs can be obtained by solving the control allocation equation:

(143) $\begin{array}{cc}\left[\begin{array}{c}{f}_T\\ {\tau }_1\\ {\tau }_2\\ {\tau }_3\end{array}\right]=\left[\begin{array}{cccc}1& 1& \text{...}& 1\\ b_{11}& b_{12}& \text{...}& b_{1n}\\ b_{21}& b_{22}& \text{...}& b_{2n}\\ b_{31}& b_{32}& \text{...}& b_{3n}\end{array}\right]\left[\begin{array}{c}{f}_1\\ f_2\\ \mathrm{.Math.}\\ f_n\end{array}\right]& \left(84\right)\end{array}$

(144) where the coefficients b.sub.ij, i=1, 2, j=1, . . . , n are derived from the geometry of the rotorcraft, and b.sub.3j=(−1).sup.jd, j=1, . . . , n, where d is the ratio between the drag and the thrust coefficients of the propeller blade for f.sub.i, i=1, . . . , n. Control allocation in general is specific to the design of the aircraft. Once the CG control and attitude control are determined, the rotorcraft is then controlled based on these determinations at 233. In some embodiments, the CG and attitude control determinations may occur in parallel.

(145) FIG. 4 is a block diagram illustrating a computing system 400 configured to perform adaptive wind estimation, trajectory generation, and flight control for aerial systems using motion data, according to an embodiment of the present invention. In some embodiments, system 400 may be, or may be integrated with, the flight control system of an aircraft, such as a rotorcraft. System 400 includes a bus 405 or other communication mechanism for communicating information, and processor(s) 410 coupled to bus 405 for processing information. Processor(s) 410 may be any type of general or specific purpose processor, including a central processing unit (CPU) or application specific integrated circuit (ASIC). Processor(s) 410 may also have multiple processing cores, and at least some of the cores may be configured for specific functions. System 400 further includes a memory 415 for storing information and instructions to be executed by processor(s) 410. Memory 415 can be comprised of any combination of random access memory (RAM), read only memory (ROM), flash memory, cache, static storage such as a magnetic or optical disk, or any other types of non-transitory computer-readable media or combinations thereof. Additionally, system 400 includes a communication device 420, such as a transceiver, to wirelessly provide access to a communications network.

(146) Non-transitory computer-readable media may be any available media that can be accessed by processor(s) 410 and may include volatile media, non-volatile media, removable media, and/or non-removable media. The media may store computer-readable instructions, data structures, program modules, and/or other data.

(147) Processor(s) 410 are further coupled via bus 405 to flight control systems 425 that facilitate physical control of the aircraft (e.g., rotors, ailerons, flaps, air brakes, etc.). As computing system 400 computes new wing, trajectory, and flight control information and solutions, flight control systems 425 may be controlled accordingly.

(148) Memory 415 stores software modules that provide functionality when executed by processor(s) 410. The modules include an operating system 430 for system 400. The modules further include an adaptive wind estimation and flight control module 435 that is configured to perform the various adaptive wind estimation, trajectory generation, and flight control processes disclosed herein. System 400 may include one or more additional functional modules 440 that include additional functionality.

(149) One skilled in the art will appreciate that a “system” could be any suitable avionics system for an aircraft, or a remote computing system, such as a personal computer, a server, a console, a personal digital assistant (PDA), a cell phone, a tablet computing device, or any other suitable computing device, or combination of devices, that is configured to control the aircraft remotely. Presenting the above-described functions as being performed by a “system” is not intended to limit the scope of the present invention in any way, but is intended to provide one example of many embodiments of the present invention. Indeed, methods, systems and apparatuses disclosed herein may be implemented in localized and distributed forms consistent with computing technology, including cloud computing systems.

(150) It should be noted that some of the system features described in this specification have been presented as modules, in order to more particularly emphasize their implementation independence. For example, a module may be implemented as a hardware circuit comprising custom very large-scale integration (VLSI) circuits or gate arrays, off-the-shelf semiconductors such as logic chips, transistors, or other discrete components. A module may also be implemented in programmable hardware devices such as field programmable gate arrays, programmable array logic, programmable logic devices, graphics processing units, or the like.

(151) A module may also be at least partially implemented in software for execution by various types of processors. An identified unit of executable code may, for instance, comprise one or more physical or logical blocks of computer instructions that may, for instance, be organized as an object, procedure, or function. Nevertheless, the executables of an identified module need not be physically located together, but may comprise disparate instructions stored in different locations which, when joined logically together, comprise the module and achieve the stated purpose for the module. Further, modules may be stored on a computer-readable medium, which may be, for instance, a hard disk drive, flash device, RAM, tape, or any other such medium used to store data.

(152) Indeed, a module of executable code could be a single instruction, or many instructions, and may even be distributed over several different code segments, among different programs, and across several memory devices. Similarly, operational data may be identified and illustrated herein within modules, and may be embodied in any suitable form and organized within any suitable type of data structure. The operational data may be collected as a single data set, or may be distributed over different locations including over different storage devices, and may exist, at least partially, merely as electronic signals on a system or network.

(153) The process steps performed in FIGS. 2A-D may be performed by a computer program, encoding instructions for processor(s) to perform at least the processes described in FIGS. 2A-D, in accordance with embodiments of the present invention. The computer program may be embodied on a non-transitory computer-readable medium. The computer-readable medium may be, but is not limited to, a hard disk drive, a flash device, RAM, a tape, or any other such medium used to store data. The computer program may include encoded instructions for controlling the processor(s) to implement the processes described in FIGS. 2A-D, which may also be stored on the computer-readable medium.

(154) The computer program can be implemented in hardware, software, or a hybrid implementation. The computer program can be composed of modules that are in operative communication with one another, and which are designed to pass information or instructions to display. The computer program can be configured to operate on a general-purpose computer, or an ASIC.

(155) Some embodiments provide a unified adaptive wind estimation, trajectory generation, and flight control approach for aircraft, such as multi-rotor drones, to facilitate effective navigation in environments with potentially rapidly changing wind conditions, such as urban environments. The adaptive algorithms of some embodiments provide capabilities for fast and reliable estimation of the aerodynamic drag coefficients of the aircraft in zero wind conditions, as well as the wind components along the flight trajectory. The navigation algorithms of some embodiments generate sub-minimum time and minimum jerk trajectories between given waypoints of the trajectory (i.e., along segments), taking into account the estimated wind.

(156) The algorithms of some embodiments are fast enough to enable real-time adaptive wind estimation, trajectory generation, and flight control, relying on the analytic solutions of the single axis optimal control problem, the feasibility of which is checked with respect to the dynamics of the aircraft. The dynamics include real-time estimations of aerodynamic drag. The control algorithms of some embodiments are designed to track the generated trajectories as long as the aircraft retains controllability. The algorithms of some embodiments are computationally effective and can readily be implemented using the relatively smaller amounts onboard computing power available to sUASs as compared to larger aircraft.

(157) It will be readily understood that the components of various embodiments of the present invention, as generally described and illustrated in the figures herein, may be arranged and designed in a wide variety of different configurations. Thus, the detailed description of the embodiments of the present invention, as represented in the attached figures, is not intended to limit the scope of the invention as claimed, but is merely representative of selected embodiments of the invention.

(158) The features, structures, or characteristics of the invention described throughout this specification may be combined in any suitable manner in one or more embodiments. For example, reference throughout this specification to “certain embodiments,” “some embodiments,” or similar language means that a particular feature, structure, or characteristic described in connection with the embodiment is included in at least one embodiment of the present invention. Thus, appearances of the phrases “in certain embodiments,” “in some embodiment,” “in other embodiments,” or similar language throughout this specification do not necessarily all refer to the same group of embodiments and the described features, structures, or characteristics may be combined in any suitable manner in one or more embodiments.

(159) It should be noted that reference throughout this specification to features, advantages, or similar language does not imply that all of the features and advantages that may be realized with the present invention should be or are in any single embodiment of the invention. Rather, language referring to the features and advantages is understood to mean that a specific feature, advantage, or characteristic described in connection with an embodiment is included in at least one embodiment of the present invention. Thus, discussion of the features and advantages, and similar language, throughout this specification may, but do not necessarily, refer to the same embodiment.

(160) Furthermore, the described features, advantages, and characteristics of the invention may be combined in any suitable manner in one or more embodiments. One skilled in the relevant art will recognize that the invention can be practiced without one or more of the specific features or advantages of a particular embodiment. In other instances, additional features and advantages may be recognized in certain embodiments that may not be present in all embodiments of the invention.

(161) One having ordinary skill in the art will readily understand that the invention as discussed above may be practiced with steps in a different order, and/or with hardware elements in configurations which are different than those which are disclosed. Therefore, although the invention has been described based upon these preferred embodiments, it would be apparent to those of skill in the art that certain modifications, variations, and alternative constructions would be apparent, while remaining within the spirit and scope of the invention. In order to determine the metes and bounds of the invention, therefore, reference should be made to the appended claims.