Extremal Control and Modified Explicit Guidance for Autonomous Unmanned Aerial Vehicles

Kawamura, Evan; Azimov, Dilmurat

doi:10.1115/1.4054951

Abstract

This study aims to develop and integrate guidance and control functions for applications such as rendezvous and docking, trajectory planning, entry descent and landing, executing maneuvers, and minimum fuel consumption. The utility of integrated nonlinear optimal control and explicit guidance functions replaces linear proportional-integral-derivative (PID) control laws. This approach leverages unmanned aerial vehicle (UAV) flight autonomy, thereby paving the way for creating an autonomous control technology with real-time target-relative guidance and re-targeting capabilities. A 360 deg roll maneuver combines extremal control and modified explicit guidance in which “explicit” means the acceleration commands are functions of time. The roll maneuver accurately reaches the desired position and velocity vectors through the proposed integration. Satisfying the first-order necessary optimality conditions demonstrates that the roll maneuver has extremal trajectories. To the best of the authors’ knowledge, this is the first time analyzing and testing the Weierstrass condition and the first- and second-order conditions of optimality for UAVs. Second-order conditions show that the 360 deg roll maneuver with explicit rotational attitude guidance does not have an optimal trajectory but yields an extremal trajectory.

1 Introduction

This study aims to develop a framework for integrating targeting, guidance, navigation, and control (TGNC) functions for applications. Integrating control and guidance is a first step towards completing the integration of TGNC functions. In particular, many research studies have focused on developing control technology of unmanned aerial vehicles (UAVs), where only a small number of works deal with or address questions related to optimality. One example includes using the Hamilton–Jacobi–Bellman equation based on a Dubin’s vehicle model [1]. Another study used the Hamiltonian formalism using an ad hoc intuitive approach to break the optimal trajectory into seven sections, simplifying the optimal trajectory and control [2]. Another work showed that Legendre polynomials allow for the determination of the optimal trajectories and controls. However, the analysis does not involve the Hamiltonian and the Euler–Lagrangian equations to determine extremals or satisfy the necessary and sufficient conditions of optimality [3]. Another approach utilized the Hamilton–Jacobi–Isaacs equation with a state-feedback Taylor series expansion determining optimal control [4]. Another work used the Pontryagin minimum principle and the generalized vector explicit guidance law to find the optimal control to guide a missile to intercept a target in the air and an air-to-surface munition [5]. It has been shown that many nonlinear models of flight vehicles, including quadcopters, can be linearized and consequently used linear controllers such as proportional-integral-derivative (PID) controllers [6].

Another group of studies deals with guidance problems. In particular, Lee et al. designed a nonlinear guidance and control system based on a sliding-mode control scheme for UAVs to land automatically on carriers out at sea [7]. Park et al. demonstrated that their nonlinear guidance logic guides UAVs very accurately on curvilinear trajectories. Their guidance logic succeeded in three cases: following a straight line, following a non-straight line with a perturbation from a straight line, and following a circular path [8]. Closed-form and analytical solutions of UAV non-steady flight in a vertical plane have complete and explicit solutions, which lead to targeting and guidance solutions [9]. Another study revealed that computing instantaneous screw motion invariants could allow for the computation of guidance commands [10]. George Cherry derived a general explicit, optimizing guidance law for rocket-propelled spacecraft for translational motion. According to explicit guidance laws, the steering commands can directly be expressed in terms of the current and desired values of the position and velocity vectors’ components [11].

Despite the successful implementation of PID controllers, a significant drawback of these controllers is that tuning is subjective and catered to a particular system and its parameters. Another disadvantage of linearizing nonlinear systems is that large perturbations can decrease accuracy, especially when the solution is not near equilibrium. Linearizing nonlinear systems simplifies the problem but can lead to errors, so developing nonlinear controllers yields higher accuracy. Integrating guidance with control provides commands to stay on a desired or reference trajectory by steering a vehicle to the desired position and velocity. Guidance laws are explicit only when derived as direct solutions to the equations of motion. Sometimes explicit guidance laws cannot be exact, so the explicit equations become more accurate as the gap closes between current and desired boundary conditions [11].

Analysis of existing literature shows that no other works with explicit guidance for attitude motion in the context of optimal control for rotational motion exist. However, there were studies that utilized explicit guidance for translational motion with extremal control [12,13]. Modifying Cherry’s original explicit guidance with attitude guidance formulates UAV explicit guidance, in particular, for the quadcopter roll maneuver. This paper uses the nontrivial variable motor thrust case presented in Refs. [12,13] but with a slightly modified Pontraygin function and for rotational motion instead of translational motion. The extremal control solutions are such that they satisfy the dynamical model equations, constraints on the state and controls, and terminal conditions, but do not necessarily provide minimum to the functional [14]. Admissible control solutions are those that satisfy only the dynamical model equations and constraints on the state and controls. Thus, the field of the corresponding admissible solutions contains and is larger than the field of the extremal solutions.

Reference [15] was first presented and published at the 29th AAS/AIAA Space Flight Mechanics Meeting, Ka’anapali, HI, and the main scope of this paper extends the previous work by:

incorporating the transversality condition for the roll maneuver
satisfying the Jacobi condition
satisfying the first-order necessary conditions of optimality to demonstrate extremality
considering the second-order conditions of optimality such as the Legendre–Clebsch condition
considering the second differential of the extended functional
considering the Weierstrass condition

2 Quadcopter Dynamical Model

This section follows the quadcopter dynamical model in Refs. [12,13,15,16] and neglects complexities such as rotational drag forces, propeller deformation caused by high velocities or flexible propeller material, and strong winds.

2.1 State Vector.

Let the general state vector for a quadcopter be s ∈ ℝⁿ where n = 12. The state variables have four groups, each with three components: position vector in meters (m), velocity vector in meters per second (m/s), quaternions, and angular velocity in radians per second (rad/s):

\begin{aligned} s = [E N U v_{E} v_{N} v_{U} q_{1} q_{2} q_{3} ω_{x} ω_{y} ω_{z}]^{T} \\ p = [E N U]^{T}, v = [v_{E} v_{N} v_{U}]^{T} \\ q = [q_{1} q_{2} q_{3}]^{T}, Ω = [ω_{x} ω_{y} ω_{z}]^{T} \end{aligned}

(1)

where p denotes the position in the right-handed East North Up (ENU) coordinate system, v denotes the translational velocity, q denotes the quaternions using the vector first formalism, and Ω denotes the angular velocity in the body frame. The computations of the quaternions must satisfy the quaternion identity [17]:

q_{1}^{2} + q_{2}^{2} + q_{3}^{2} + q_{4}^{2} = 1

(2)

2.2 Coordinate Systems.

Figure 1 shows the inertial North, East, Up frame axes denoted by N, E, U and the body frame axes denoted by X, Y, Z. The fixed inertial frame on the ground has gravity pointing in the negative U-direction. The body frame is fixed at the center of mass of the quadcopter such that the X and Y axes point in the direction of the arms with the assumption that the frame is symmetrical, and the Z-axis points up in the same direction as the motor axes.

Fig. 1

View large Download slide

Body and inertial frames of a quadcopter

2.3 Forces.

The three main forces are weight, thrust, and drag. The weight force is defined as W = Mg such that M is the mass and g is the acceleration due to gravity. The total thrust in the body frame’s Z axis is defined as

\sum_{i = 1}^{4} (T_{m i})

such that [16]

T_{b} = k {[\begin{matrix} 0 & 0 & \sum_{i = 1}^{4} ω_{i}^{2} \end{matrix}]}^{T}

(3)

where k is the motor thrust coefficient in kg m, and

ω_{i}^{2}

is the motor spin rate of the ith motor in (rad/s)². This thrust model is also seen in another quadcopter model [18]. A friction drag vector for quadcopters can be defined as [16]

F_{D} = {[\begin{matrix} - k_{d x} v_{E} & - k_{d y} v_{N} & - k_{d z} v_{U} \end{matrix}]}^{T}

(4)

where k_dx, k_dy, and k_dz are constant drag coefficients to be determined or estimated. Considering separate coefficients on each axis is an extension of Gibiansky’s formulation for drag. Overall, this drag model resembles a friction drag model or viscous damper [19]. This simplified drag model is used for simulation purposes by acting in the inertial system. A more accurate model may include components given in the body frame with velocity squared instead of linear velocity [20].

2.4 Torques.

Summation of torques gives the typical equation:

τ = I \dot{ω} + ω \times (I ω)

(5)

where the moment of inertia matrix,

I

⁠, is assumed to be diagonal such that the body axes align with the principal axes:

I = [\begin{matrix} I_{x x} & 0 & 0 \\ 0 & I_{y y} & 0 \\ 0 & 0 & I_{z z} \end{matrix}]

(6)

Expanding Eq. with the inertia matrix components gives

τ = [\begin{matrix} τ_{x} \\ τ_{y} \\ τ_{z} \end{matrix}] = [\begin{matrix} I_{x x} α_{x} - I_{y y} ω_{y} ω_{z} + I_{z z} ω_{y} ω_{z} \\ I_{y y} α_{y} + I_{x x} ω_{x} ω_{z} - I_{z z} ω_{x} ω_{z} \\ I_{z z} α_{z} - I_{x x} ω_{x} ω_{y} + I_{y y} ω_{x} ω_{y} \end{matrix}]

(7)

where

α = \dot{ω}

is the angular acceleration. A simple torque model about the z-axis assumes constant velocity and negligible angular acceleration:

τ_{z} = (- 1)^{i + 1} b_{τ} ω_{i}^{2}

(8)

with (−1)ⁱ⁺¹ positive for clockwise propellers and negative for counterclockwise propellers. Thus, the total torque about the z axis from all four propellers of a quadcopter is

\begin{matrix} τ_{ψ} = b_{τ} (ω_{1}^{2} - ω_{2}^{2} + ω_{3}^{2} - ω_{4}^{2}) \end{matrix}

where

b_{τ}

is the torque drag coefficient. Thus, the torques in the body frame about the x, y, z axes from the motors are defined as

τ_{b} = [\begin{matrix} ℓ k (ω_{2}^{2} - ω_{4}^{2}) \\ ℓ k (ω_{1}^{2} - ω_{3}^{2}) \\ b_{τ} (ω_{1}^{2} - ω_{2}^{2} + ω_{3}^{2} - ω_{4}^{2}) \end{matrix}]

(9)

where ℓ is the distance from the center of mass of the quadcopter to the center of a propeller, and

b_{τ}

is the torque drag coefficient. The roll axis uses the even numbered motors, and the pitch axis uses the odd numbered motors. Gibiansky’s torque model resembles another quadcopter torque model [18]. The torque model neglects rotational drag forces since the quadcopter rotates at low speeds.

2.5 Dynamical Model Equations.

The velocity and acceleration vectors of the translational motion are

\dot{p} = v = {[\begin{matrix} v_{E} & v_{N} & v_{U} \end{matrix}]}^{T}

(10)

\dot{v} = {[\begin{matrix} 0 & 0 & - g \end{matrix}]}^{T} + \frac{1}{M} R T_{b} + \frac{1}{M} F_{D}

(11)

where the rotation matrix,

R

⁠, uses the (3-2-1) sequence to rotate the body frame to the inertial frame [17]:

R = [\begin{matrix} q_{4}^{2} + q_{1}^{2} - q_{2}^{2} - q_{3}^{2} & 2 (q_{1} q_{2} - q_{4} q_{3}) & 2 (q_{1} q_{3} + q_{4} q_{2}) \\ 2 (q_{1} q_{2} + q_{4} q_{3}) & q_{4}^{2} - q_{1}^{2} + q_{2}^{2} - q_{3}^{2} & 2 (q_{2} q_{3} - q_{4} q_{1}) \\ 2 (q_{1} q_{3} - q_{4} q_{2}) & 2 (q_{2} q_{3} + q_{4} q_{1}) & q_{4}^{2} - q_{1}^{2} - q_{2}^{2} + q_{3}^{2} \end{matrix}]

(12)

Expanding the thrust vector in the body frame from Eq. and the drag force

F_{D}

from Eq. turns Eq. into

\dot{v} = [\begin{matrix} 2 Γ (q_{4} q_{2} + q_{1} q_{3}) - \frac{k_{d x} v_{E}}{M} \\ - 2 Γ (q_{4} q_{1} - q_{2} q_{3}) - \frac{k_{d y} v_{N}}{M} \\ - g + Γ (q_{4}^{2} - q_{1}^{2} - q_{2}^{2} + q_{3}^{2}) - \frac{k_{d z} v_{U}}{M} \end{matrix}]

(13)

where Γ is

Γ = \frac{k}{M} (\sum_{i = 1}^{4} ω_{i}^{2})

(14)

Let Θ denote the Euler angles (ϕ, θ, ψ) in rad, and the time derivatives of the Euler angles are related to the angular velocity vector [16]:

\dot{Θ} = [\begin{matrix} 1 & 0 & - \sin θ \\ 0 & \cos ϕ & \cos θ \sin ϕ \\ 0 & - \sin ϕ & \cos θ \cos ϕ \end{matrix}] Ω

(15)

The Euler angles and quaternions are related by the (3-2-1) direction cosine matrix sequence and the quaternion identity from Eq. :

\begin{aligned} q_{1} = s (\frac{ϕ}{2}) c (\frac{θ}{2}) c (\frac{ψ}{2}) - c (\frac{ϕ}{2}) s (\frac{θ}{2}) s (\frac{ψ}{2}) \\ q_{2} = c (\frac{ϕ}{2}) s (\frac{θ}{2}) c (\frac{ψ}{2}) + s (\frac{ϕ}{2}) c (\frac{θ}{2}) s (\frac{ψ}{2}) \\ q_{3} = c (\frac{ϕ}{2}) c (\frac{θ}{2}) s (\frac{ψ}{2}) - s (\frac{ϕ}{2}) s (\frac{θ}{2}) c (\frac{ψ}{2}) \\ q_{4} = \pm \sqrt{1 - q_{1}^{2} - q_{2}^{2} - q_{3}^{2}} \end{aligned}

(16)

where s denotes sin, c denotes cos, and the quaternion identity solves q₄ [17]. Alternatively, one can use the (3-2-1) sequence given the time derivatives of the quaternions as

\begin{aligned} {\dot{q}}_{1} = \frac{1}{2} (q_{4} ω_{x} + q_{2} ω_{z} - q_{3} ω_{y}) \\ {\dot{q}}_{2} = \frac{1}{2} (q_{4} ω_{y} - q_{1} ω_{z} + q_{3} ω_{x}) \\ {\dot{q}}_{3} = \frac{1}{2} (q_{4} ω_{z} + q_{1} ω_{y} - q_{2} ω_{x}) \end{aligned}

(17)

and the scalar part of the quaternions, q₄, can be found using the quaternion identity from Eq. after integrating [17]. Solving Eq. for the angular acceleration yields:

α = \dot{Ω} = [\begin{matrix} τ_{x} / I_{x x} \\ τ_{y} / I_{y y} \\ τ_{z} / I_{z z} \end{matrix}] = [\begin{matrix} \frac{I_{y y} - I_{z z}}{I_{x x}} ω_{y} ω_{z} \\ \frac{I_{z z} - I_{x x}}{I_{y y}} ω_{x} ω_{z} \\ \frac{I_{x x} - I_{y y}}{I_{z z}} ω_{x} ω_{y} \end{matrix}]

(18)

3 Optimal Control Problem

This paper applies optimal control theory to the quadcopter 360 deg roll maneuver with state and control constraints. This maneuver demonstrates the utility of extremal control with the modified explicit guidance for rotational motion in which the original explicit guidance formulation was for only translational motion [11].

3.1 Control Vector.

Let the control vector u ∈ ℝ⁴ have the motor angular rates such that $u = [\begin{matrix} u_{1} & u_{2} & u_{3} & u_{4} \end{matrix}]$ ⁠, where u_i = ω_i and i = 1, 2, 3, 4 based on thrust from Eq. (3) [15].

3.2 State Constraint Equations.

This subsection modifies an earlier study [15] for describing the initial and final state constraint equations. The previous study includes an intermediate state constraint, which is neglected in this paper such that this paper only considers initial and final constraints, each with four functions. The first function contains position constraints in m, the second function has constraints on the velocity in m/s, the third function places constraints on the Euler angles in rad, and the fourth function constrains the angular velocities in rad/s. The third function constrains the Euler angles instead of the quaternions because the Euler angles have a clearer visual representation than the quaternions. The quadcopter must adhere to the Federal Aviation Administration (FAA) regulations by never exceeding 400 ft [21] and the initial constraints begin when the quadcopter is hovering after taking off. There are four initial constraint equations, E, denoted by 0:

\begin{aligned} E_{1} (E, N, U) = (E_{0}, N_{0}, U_{0}), E_{2} (v_{E}, v_{N}, v_{U}) = (v_{E_{0}}, v_{N_{0}}, v_{U_{0}}) \\ E_{3} (ϕ, θ, ψ) = (ϕ_{0}, θ_{0}, ψ_{0}), E_{4} (ω_{x}, ω_{y}, ω_{z}) = (ω_{x_{0}}, ω_{y_{0}}, ω_{z_{0}}) \end{aligned}

(19)

Once the quadcopter reaches some prescribed altitude below 400 ft, it performs a 360 deg roll maneuver. The attitude at the intermediate and final states will be identical because the quadcopter resets back to neutral attitude. The final state constraints,

\bar{F}

⁠, are denoted by f:

\begin{aligned} F_{1} (E, N, U) = (E_{f}, N_{f}, U_{f}), F_{2} (v_{E}, v_{N}, v_{U}) = (v_{E_{f}}, v_{N_{f}}, v_{U_{f}}) \\ F_{3} (ϕ, θ) = (ϕ_{f}, θ_{f}), F_{4} (ω_{x}, ω_{y}, ω_{z}) = (ω_{x_{f}}, ω_{y_{f}}, ω_{z_{f}}) \end{aligned}

(20)

Note that the yaw angle (heading), ψ, is not constrained because the quadcopter can perform the roll maneuver at any arbitrary heading. However, ψ plays a role when computing quaternions, as shown in Eq. (16).

The angular rates of the motors are constrained by their maximum and minimum angular rates throughout the flight, i.e., after takeoff and before landing at the final destination:

ω_{i, min}^{2} \leq ω_{i}^{2} \leq ω_{i, max}^{2}, i = 1, 2, 3, 4

(21)

The auxiliary control variables (slack variables), η_i, are defined such that [22]

η = (η_{1}, η_{2}, η_{3}, η_{4}) \in R^{d} for d = 4

(22)

Using these slack variables converts the inequality constraint in Eq. into the equality constraints:

Φ_{i} = (ω_{i, max}^{2} - ω_{i}^{2}) (ω_{i}^{2} - ω_{i, min}^{2}) - η_{i}^{2} = 0

(23)

where i = 1, 2, 3, 4. The control vector

\tilde{u} \in R^{8}

can be augmented to include the slack variables, η_i:

\tilde{u} = [ω_{1} ω_{2} ω_{3} ω_{4} η_{1} η_{2} η_{3} η_{4}]

(24)

The controls are admissible if they are defined and piece-wise continuous on some time interval [t₀, t_f] and satisfy Eqs. (10), (11), and (23) [22].

3.3 Functional.

The general functional is defined as [22]

Ψ = J (s_{0}, s_{f}, t_{0}, t_{f}) + \int g (s, u, t) d t

(25)

where 0 and f denote initial and final instances of time, s₀ and s_f have r₁ + 1 and r₂ + 1 to n components, r₁ and r₂ are the numbers of the initial and final state constraint equations, respectively, and g(s, u, t) is the Lagrangian. It is required to find state and control functions that satisfy Eqs. , –. The extended functional, K, is given by

\begin{aligned} K (s_{0}, t_{0}, s_{f}, t_{f}, μ, ν, γ, η) & = G (s_{0}, t_{0}, s_{f}, t_{f}, ε, μ, ν) \\ + \int_{t_{0}}^{t_{f}} [H (s, u, t, λ, η, γ) - λ^{T} \dot{s}] d t \end{aligned}

(26)

such that H(s, u, t, λ, η, γ) is the Pontryagin function and G(s₀, t₀, s_f, t_f, ɛ, μ, ν) contains the non-integral terminal and boundary constraints:

\begin{aligned} H (s, u, t, λ, η, γ) = λ^{T} f (s, u, t) + γ Φ (s, u, η) + g (s, u, t) \\ G (s_{0}, t_{0}, s_{f}, t_{f}, ε, μ, ν) = ε J (s_{0}, t_{0}, s_{f}, t_{f}) + μ^{T} E (s_{0}, t_{0}) + ν^{T} F (s_{f}, t_{f}) \end{aligned}

(27)

with μ, ν, γ considered as vectors of multipliers to be determined and ɛ is an arbitrary small parameter.

3.4 First Differential of Extended Functional.

Satisfying the first-order necessary conditions of optimality means dK = 0, and the general (extended) differential of the functional K is derived in Ref. [22]:

\begin{aligned} d K & = (G_{s_{0}} + λ_{0}^{T})^{T} d s_{0} + (G_{s_{f}} + λ_{f}^{T}) d s_{f} \\ + (G_{t_{0}} - H_{0}) d t_{0} + (G_{t_{f}} - H_{f}) d t_{f} \\ + \int_{t_{0}}^{t_{f}} [(H_{s} + {\dot{λ}}^{T}) δ s + {[H_{λ} - {\dot{s}}^{T}]}^{T} δ λ + H_{u}^{T} δ u + H_{η}^{T} δ η] d t \end{aligned}

(28)

such that 0 and f denote initial and final, respectively. The G terms are defined as

\begin{aligned} G_{t_{0}} = ε \frac{\partial J}{\partial t_{0}} + μ^{T} \frac{\partial \bar{E}}{\partial t_{0}}, G_{t_{f}} = ε \frac{\partial J}{\partial t_{f}} + ν^{T} \frac{\partial \bar{F}}{\partial t_{f}} \\ G_{s_{0}} = ε \frac{\partial J}{\partial s_{0}} + μ^{T} \frac{\partial \bar{E}}{\partial s_{0}}, G_{s_{f}} = ε \frac{\partial J}{\partial s_{f}} + ν^{T} \frac{\partial \bar{F}}{\partial s_{f}} \end{aligned}

(29)

where μ and ν are Lagrange multipliers, ɛ ∈ ℝ,

\bar{E}

contains the initial state constraint equations from Eq. , and

\bar{F}

contains the final state constraint equations from Eq. :

\bar{E} = [E_{1} E_{2} E_{3} E_{4}]^{T}, \bar{F} = [F_{1} F_{2} F_{3} F_{4}]^{T}

(30)

A similar representation exists from the intermediate state equations to the final state constraint equations. All the terms outside of the integral in Eq. (28) are zero by the transversality conditions, where the first two terms in the integral are also zero from the canonical equations in Eq. (37). The last two terms in the integral are also zero by the local optimality condition in Eq. (37). Overall, when all the terms in Eq. (28) are zero, the necessary conditions of optimality are satisfied.

3.5 Second Differential of Extended Functional.

The second differential of the functional is defined as

d^{2} K = Ω_{0} + Ω_{f} + \int_{t_{0}}^{t_{f}} (s^{T} H_{s s} s + 2 s^{T} H_{s u} u + u^{T} H_{u u} u) d t

(31)

The Ω′ terms are defined as

\begin{aligned} {Ω^{'}}_{0} = Ω_{t_{0}} + {\bar{E}}_{s_{0}} {\dot{s}}_{0} \\ {Ω^{'}}_{f} = Ω_{t_{f}} + {\bar{F}}_{s_{f}} {\dot{s}}_{f} \end{aligned}

(32)

The terms for

Ω_{t_{0}}

and

Ω_{t_{f}}

are assumed to be Ω₀ and Ω₁ on p. 39 of Ref. [22]

\begin{aligned} Ω_{t_{0}} = [Ω (s, u, μ, t_{0}, γ)]_{0} = {[G_{t_{0}} - (g (s, u, t) - γ^{T} Φ + G_{s_{0}}^{T} f)]}_{0} \\ Ω_{t_{f}} = [Ω (s, u, ν, t_{f}, γ)]_{f} = {[G_{t_{f}} - (g (s, u, t) + γ^{T} Φ + G_{s_{f}}^{T} f)]}_{f} \end{aligned}

(33)

where g(s, u, t) is the Lagrangian, Φ is defined in Eq. and is the intermediate constraint variables set equal to zero, γ is the vector of Lagrange multipliers for the intermediate constraints, and

G_{t_{0}}

and

G_{t_{f}}

are the non-integral boundary conditions.

The first two terms of Eq. are defined as

\begin{aligned} Ω_{0} = Ω (s_{0}, t_{0}) = [δ s_{0}^{T} d t_{0}] [\begin{matrix} G_{s_{0} s_{0}} & Ω_{s_{0}}^{T} \\ Ω_{s_{0}} & {Ω^{'}}_{0} \end{matrix}] [\begin{matrix} δ s_{0} \\ d t_{0} \end{matrix}] \\ Ω_{f} = Ω (s_{f}, t_{f}) = [δ s_{f}^{T} d t_{f}] [\begin{matrix} G_{s_{f} s_{f}} & Ω_{s_{f}}^{T} \\ Ω_{s_{f}} & {Ω^{'}}_{1} \end{matrix}] [\begin{matrix} δ s_{f} \\ d t_{f} \end{matrix}] \end{aligned}

(34)

The second partial derivatives of

G_{s_{0} s_{0}}

and

G_{s_{f} s_{f}}

are defined by partial differentiation:

\begin{aligned} G_{s_{0} s_{0}} = ε \frac{\partial^{2} J}{\partial s_{0} \partial s_{0}} + μ^{T} \frac{\partial^{2} \bar{E}}{\partial s_{0} \partial s_{0}}, G_{t_{0} s_{0}} = ε \frac{\partial^{2} J}{\partial t_{0} \partial s_{0}} + μ^{T} \frac{\partial^{2} \bar{E}}{\partial t_{0} \partial s_{0}} \\ G_{s_{f} s_{f}} = ε \frac{\partial^{2} J}{\partial s_{f} \partial s_{f}} + ν^{T} \frac{\partial^{2} \bar{F}}{\partial s_{f} \partial s_{f}}, G_{t_{f} s_{f}} = ε \frac{\partial^{2} J}{\partial t_{f} \partial s_{f}} + ν^{T} \frac{\partial^{2} \bar{F}}{\partial t_{f} \partial s_{f}} \end{aligned}

(35)

Thus, taking the first differential of the extended functional for the roll maneuver and satisfying dK = 0 demonstrates extremality (first derivative test). Then, taking the second differential of the extended functional and satisfying d²K > 0 yields a minimum (second derivative test). Overall, if d²K > 0 for the extended functional of the roll maneuver, then the roll maneuver is optimal. However, if $d^{2} K ≱ 0$ (no minimum) but dK = 0 (stationary point), then the roll maneuver is only extremal.

4 Hamiltonian Formalism

The authors use the indirect method and canonical equations to analytically solve a system of ordinary differential equations, which avoids numerical methods for solving partial differential equations such as the Hamilton–Jacobi–Bellmann equation. The functional based on the Mayer problem with a terminal cost for finite time is minimized (Lawden [23]): min J = min (t_f − t₀).

4.1 Pontryagin Function.

Forming the Pontryagin function for the Mayer problem with time optimization using the motor constraints from Eq. with η_i as the slack variables and its associated Lagrange multipliers, γ_i, yields:

\begin{aligned} H (t, s, \bar{u}, λ, ν, η) & = λ_{1} v_{E} + λ_{2} v_{N} + λ_{3} v_{U} + λ_{4} [2 Γ (q_{4} q_{2} + q_{1} q_{3}) - \frac{k_{d x} v_{E}}{M}] \\ + λ_{5} [- 2 Γ (q_{4} q_{1} - q_{2} q_{3}) - \frac{k_{d y} v_{N}}{M}] + λ_{6} [- g + Γ (q_{4}^{2} - q_{1}^{2} - q_{2}^{2} + q_{3}^{2}) - \frac{k_{d z} v_{U}}{M}] \\ + \frac{λ_{7}}{2} (- ω_{x} q_{1} - ω_{y} q_{2} - ω_{z} q_{3}) + \frac{λ_{8}}{2} (- ω_{x} q_{4} + ω_{z} q_{2} - ω_{y} q_{3}) + \frac{λ_{9}}{2} (- ω_{y} q_{4} - ω_{z} q_{1} - ω_{x} q_{3}) \\ + λ_{10} (\frac{ℓ k}{I_{x x}} (ω_{2}^{2} - ω_{4}^{2}) - \frac{I_{y y} - I_{z z}}{I_{x x}} ω_{y} ω_{z}) + λ_{11} (\frac{ℓ k}{I_{y y}} (ω_{1}^{2} - ω_{3}^{2}) - \frac{I_{z z} - I_{x x}}{I_{y y}} ω_{x} ω_{z}) \\ + λ_{12} (\frac{b_{τ}}{I_{z z}} (ω_{1}^{2} - ω_{2}^{2} + ω_{3}^{2} - ω_{4}^{2}) - \frac{I_{x x} - I_{y y}}{I_{z z}} ω_{x} ω_{y}) + γ^{T} Φ \end{aligned}

(36)

where γ_i and Φ_i are the Lagrange multipliers and motor constraint equations, respectively. Adding γ^TΦ does not change the overall expression because Φ_i = 0 as defined in Eq. . In other words, adding zero does not change it, which resembles the additive identity.

4.2 Canonical Equations and Local Optimality Conditions.

The canonical equations and the local optimality condition must be satisfied to find extremals of the problem [22]

{[\frac{\partial H}{\partial s}]}^{T} = - \dot{λ}, {[\frac{\partial H}{\partial λ}]}^{T} = \dot{s}, {[\frac{\partial H}{\partial \tilde{u}}]}^{T} = 0

(37)

where s is the state vector and λ is the vector of Lagrange multipliers. Equation comes from equating each of the terms inside the integral of Eq. to zero [22]. The auxiliary control variables (η_i) can be determined using the local optimality condition:

\begin{aligned} {[\frac{\partial H}{\partial η_{i}}]}^{T} = 0 or - 2 ν_{i} η_{i} = 0, i = 1, 2, 3, 4 {\begin{matrix} η_{i} \neq 0 if ν_{i} = 0 \\ η_{i} = 0 if ν_{i} \neq 0 \end{matrix} \end{aligned}

(38)

where η_i depends on ν_i for i = 1,…, 4. Therefore, there are two main cases from Eq. : one case has variable motor thrust when η_i ≠ 0, ν_i = 0, and the other case has constant motor thrust when η_i = 0, ν_i ≠ 0 [22]. The other cases involve combinations of minimum, intermediate, and maximum motor spin rates. This paper utilizes the first case with variable (intermediate) motor thrust for the

360 \deg

roll maneuver [13].

4.3 Analytical Solutions for the Lagrange Multipliers—Nontrivial Motor Thrust.

This subsection summarizes the analytical solutions of the nontrivial variable motor thrust case’s Lagrange multipliers based on previous work and presents the main results here for the reader’s convenience [12,13,15]:

\begin{aligned} λ_{1}, λ_{2}, λ_{3} \in R \\ λ_{4} (t) = \frac{λ_{1}}{A_{x}} + \frac{\exp (A_{x} t)}{A_{x}} (λ_{4} (t_{0}) A_{x} - λ_{1}) \\ λ_{5} (t) = \frac{λ_{2}}{A_{y}} + \frac{\exp (A_{y} t)}{A_{y}} (λ_{5} (t_{0}) A_{y} - λ_{2}) \\ λ_{6} (t) = \frac{λ_{3}}{A_{z}} + \frac{\exp (A_{z} t)}{A_{z}} (λ_{6} (t_{0}) A_{z} - λ_{3}) \end{aligned}

(39)

in which this change of variables makes the notation more compact:

A_{x} = \frac{k_{d x}}{M}, A_{y} = \frac{k_{d y}}{M}, A_{z} = \frac{k_{d z}}{M}

(40)

4.4 Weierstrass Condition.

Satisfying the Weierstrass condition for all points, including corner points, on the trajectory satisfies one of the necessary conditions for optimality of variational problems [24]:

δ H (s, J_{s}^{o}, u^{1}, u^{o}, t) = H (s, J_{s}^{o}, u^{1}, t) - H (s, J_{s}^{o}, u^{o}, t) \geq 0, \forall t, u^{1} \neq u^{o}

(41)

such that the superscript, o, in u^o indicates optimal control. An alternate way to express the Weierstrass condition is given by [22]

δ H (s, u, \tilde{u}, t, λ, η, γ) = H (s, u, t, λ, η, γ) - H (s, \tilde{u}, t, λ, η, γ) \geq 0

(42)

4.5 Legendre–Clebsch Condition.

The Legendre–Clebsch condition determines whether the control is extremal or optimal. If H_uu contains all zeros, singular control and extremality exist instead of optimality. The necessary condition of Legendre–Clebsch for a weak minimum is [22]

H_{u u} \geq 0

(43)

Taking second partial derivatives of H from Eq. with respect to the control variables yields

H_{u u} = [\begin{matrix} \frac{\partial^{2} H}{\partial ω_{1}^{2}} & \frac{\partial^{2} H}{\partial ω_{1} \partial ω_{2}} & \frac{\partial^{2} H}{\partial ω_{1} \partial ω_{3}} & \frac{\partial^{2} H}{\partial ω_{1} \partial ω_{4}} \\ \frac{\partial^{2} H}{\partial ω_{2} \partial ω_{1}} & \frac{\partial^{2} H}{\partial ω_{2}^{2}} & \frac{\partial^{2} H}{\partial ω_{2} \partial ω_{3}} & \frac{\partial^{2} H}{\partial ω_{2} \partial ω_{4}} \\ \frac{\partial^{2} H}{\partial ω_{3} \partial ω_{1}} & \frac{\partial^{2} H}{\partial ω_{3} \partial ω_{2}} & \frac{\partial^{2} H}{\partial ω_{3}^{2}} & \frac{\partial^{2} H}{\partial ω_{3} \partial ω_{4}} \\ \frac{\partial^{2} H}{\partial ω_{4} \partial ω_{1}} & \frac{\partial^{2} H}{\partial ω_{4} \partial ω_{2}} & \frac{\partial^{2} H}{\partial ω_{4} \partial ω_{3}} & \frac{\partial^{2} H}{\partial ω_{4}^{2}} \end{matrix}]

(44)

The strengthened version of Legendre–Clebsch condition is positive definite and is necessary for determining optimal control. If d²K > 0, then the second-order conditions must be satisfied, i.e., all the second partial derivative submatrices $(H_{u u}, H_{s u}, H_{u s}, H_{s s})$ must be positive definite [22].

4.6 Jacobi Condition (Conjugate Points).

Showing the absence of conjugate points satisfies the Jacobi condition. If

\bar{R}

⁠, the solution of the accessory Riccati equation, is finite over (t₀, t_f), then δs ≠ 0. For arbitrary variations δλ [22]:

δ s = {\bar{R}}^{- 1} δ λ \forall δ λ, δ s \neq 0 (or δ λ \neq 0)

(45)

If analytical solutions exist for the canonical equations and local optimality condition in Eq. , it is possible to find conjugate points without the condition,

H_{u u}

⁠, which appears in the accessory Riccati equation. Thus, it is desired to find analytical solutions for the canonical equations and local optimality condition such that the presence of conjugate points on an extremal can be computed like a Jacobian:

δ s = S {[\frac{\partial s}{\partial C}]}^{T}, δ λ = V {[\frac{\partial s}{\partial C}]}^{T}

(46)

in which

S

and

V

are matrices of constants and C is the vector of the integration constants.

It is important to note that if the canonical equations, local optimality equation, Legendre–Clebsch condition, Weierstrass condition, and Jacobi condition are satisfied, then these necessary conditions lead to optimality for variational problems. If strict inequalities signs exist, then the necessary conditions are strengthened into sufficient conditions from weak minimum to strong minimum [24]. Therefore, satisfying these conditions and equations for the $360 \deg$ roll maneuver demonstrates optimality. However, if the Legendre–Clebsch condition or Weierstrass condition are not satisfied for the roll maneuver, then the roll maneuver is extremal instead of optimal.

5 Explicit Guidance

Cherry’s explicit guidance (E Guidance) was derived for rocket-propelled spacecraft and begins with the rendezvous two-point boundary-value problem, and the goal is to find a thrust acceleration vector from a vehicle’s current position and velocity to a specified position and velocity at some terminal time [11]. His E Guidance formulation would become the fundamentals for Apollo lunar descent-guidance [25]. Even though E Guidance was originally for rockets, this paper demonstrates its utility for UAVs through translational and rotational guidance. In this work, modifying Cherry’s original E Guidance for translational acceleration results in attitude guidance for UAVs.

5.1 Explicit Guidance Computations.

The first step in E Guidance sets the boundary conditions of the (1) initial position and velocity at an initial time, t₀ and (2) the desired position and velocity at some terminal time, T. The time-to-go to reach the desired position and velocity is the difference between the terminal and initial time: T_go = T − t₀. Second, once the boundary conditions and terminal time are set, one must choose linearly independent functions, p_i(t), p_j(t), for i = 1, 3, 5 and j = 2, 4, 6 for three dimensional E Guidance. Cherry’s translational thrust acceleration in the x-direction is

α_{T x} (t) = c_{1} p_{1} (t) + c_{2} p_{2} (t) - g_{x} (t)

(47)

where c₁, c₂ ∈ ℝ and p₁(t), p₂(t) are linearly independent functions. Numerous trials of exponential, trigonometric, and polynomial functions demonstrated that linearly independent polynomials performed best because they satisfied the boundary conditions unlike the combinations of exponential and trigonometric functions. Having linearly independent polynomials yields an invertible

F

matrix with full rank, which is required to determine the

E

matrix and then the coefficients, c_k, k = 1,…, 6. Third, integrating the linearly independent functions yields

F = [f_{i j}]

and

E = [e_{i j}]

as the inverse of the

F

matrix:

\begin{aligned} e_{11} = f_{22} / Δ, f_{11} = \int_{t_{0}}^{T} p_{1} (t) d t \\ e_{12} = - f_{12} / Δ, f_{12} = \int_{t_{0}}^{T} p_{2} (t) d t \\ e_{21} = f_{21} / Δ, f_{21} = \int_{t_{0}}^{T} [\int_{t_{0}}^{t} p_{1} (s) d s] d t \\ e_{22} = f_{11} / Δ, f_{22} = \int_{t_{0}}^{T} [\int_{t_{0}}^{t} p_{2} (s) d s] d t \\ Δ = det (F) = f_{11} f_{22} - f_{12} f_{21} \end{aligned}

(48)

For the x-direction, the coefficients are determined by the

E

matrix components and the boundary conditions:

[\begin{matrix} c_{1} \\ c_{2} \end{matrix}] = [\begin{matrix} e_{11} & e_{12} \\ e_{21} & e_{22} \end{matrix}] [\begin{matrix} {\dot{x}}_{D} - {\dot{x}}_{0} \\ x_{D} - x_{0} - {\dot{x}}_{0} T_{g o} \end{matrix}]

(49)

where “0” denotes initial, and “D” denotes desired. Finally, once the coefficients are known, the thrust acceleration profile is known. Integrating the acceleration yields the guided velocity profile, and integrating again generates the guided position profile.

For angular acceleration, the cross product between angular velocity and angular momentum replaces the gravity vector, i.e., ω × Iω. Choosing p_1,3,5 = (T − t)² and p_2,4,6 = (T − t)³, where T is the terminal time, and expanding the cross product yields

\begin{aligned} α_{ϕ} (t) = c_{1} (T - t)^{2} + c_{2} (T - t)^{3} - \frac{I_{y y} - I_{z z}}{I_{x x}} ω_{y} ω_{z} \\ α_{θ} (t) = c_{3} (T - t)^{2} + c_{4} (T - t)^{3} - \frac{I_{z z} - I_{x x}}{I_{y y}} ω_{x} ω_{z} \\ α_{ψ} (t) = c_{5} (T - t)^{2} + c_{6} (T - t)^{3} - \frac{I_{x x} - I_{y y}}{I_{z z}} ω_{x} ω_{y} \end{aligned}

(50)

Choosing higher order polynomials yielded larger roll torques but did not satisfy the boundary conditions. The initial Euler angles (rad) and Euler angle velocities (rad/s) from the gyroscope measurements are (last six components of Eq. ):

\begin{aligned} ϕ_{0} = - 0.0972, θ_{0} = - 0.02877, ψ_{0} = 5.188 \\ {\dot{ϕ}}_{0} = - 0.0524, {\dot{θ}}_{0} = - 0.2793, {\dot{ψ}}_{0} = 0.0175 \end{aligned}

(51)

The desired Euler angles and Euler angular velocities are

ϕ_{D} = - 2 π, {\dot{ϕ}}_{D} = 0, θ_{D} = 0, {\dot{θ}}_{D} = 0, ψ_{D} = 0, {\dot{ψ}}_{D} = 0

(52)

Angular acceleration commands will be determined for 1.626 s, which is how long it took for the Flite Test 270 Chase Quad to perform one complete

360 \deg

roll maneuver. Therefore, the terminal time T = 1.626 s where the initial time is t₀ = 0 and the time-to-go is T_go = T − t₀ = 1.626 s. Then, c₁ and c₂ are determined by

[\begin{matrix} c_{1} \\ c_{2} \end{matrix}] = [\begin{matrix} e_{11} & e_{12} \\ e_{21} & e_{22} \end{matrix}] [\begin{matrix} {\dot{ϕ}}_{D} - {\dot{ϕ}}_{0} \\ ϕ_{D} - ϕ_{0} - {\dot{ϕ}}_{0} T_{g o} \end{matrix}]

(53)

and this equation for the coefficients of the rotational guidance simply comes from replacing x with ϕ from Eq. . The c_i can be computed using Eqs. , –:

\begin{aligned} c_{1} = 53.7857, c_{2} = - 44.0747, c_{3} = 0.7819 \\ c_{4} = - 0.8010, c_{5} = 0.0489, c_{6} = - 0.0501 \end{aligned}

(54)

Note that c₁ and c₂ are the largest because they are the coefficients for ϕ. Therefore, they play the biggest role in the roll maneuver. On the other hand, θ, ψ do not have a significant impact, so c₃, c₄, c₅, and c₆ have small values. The angular acceleration commands can be computed using these six coefficients, which lead to the torque commands and motor spin rate commands. Finally, integrating these angular accelerations twice yield the Euler angles.

5.2 Discussion of Quadcopter Guidance Problem.

The quadcopter problem considered in this paper with nonlinear dynamics, explicit guidance, and extremal control differs from other approaches. One work showed only translational guidance for quadcopters but not attitude guidance [12]. Translational guidance with a gradient vector field demonstrates autonomous path following and circular obstacle avoidance [26]. Integrating a closing velocity controller with a pursuit guidance law yields accurate landing for quadcopters compared to typical vertical descent methods [27]. Another work combines feedback control and guidance to send quadcopters to desired waypoints with a desired attitude trajectory [28]. This paper focuses on both translational and rotational guidance problems via the takeoff maneuver and roll maneuver by integrating extremal control and explicit guidance.

6 Simulation Results

The simulated roll maneuver uses physical parameters from the FliteTest 270 Chase Quad flight test and the nontrivial variable (intermediate) motor thrust case with explicit guidance.

6.1 Simulation Setup.

Simulating a FliteTest 270 Chase Quad demonstrates the $360 \deg$ roll maneuver with these physical parameters: ℓ = 0.136 m (arm length), $k = 3 \cdot 10^{- 6} kg m, b_{τ} = 1 \cdot 10^{- 7} kg m^{2}$ ⁠, $k_{d x} = k_{d y} = k_{d z} = 0.25 kg / s$ ⁠, and M = 1 kg. For the $360 \deg$ roll maneuver, the position and velocity constraints are approximately constant during the short maneuver, and the initial constraints obtained from the Blackbox telemetry data are (recall Eq. (51) was for initial state constraints)

\begin{aligned} E_{1} (E, N, U) \approx (0, 0, 5), E_{2} (v_{E}, v_{N}, v_{U}) \approx (0, 0, 0) \\ E_{3} (ϕ, θ, ψ) = (- 0.09719; - 0.02877; 5.188) \\ E_{4} (ω_{x}, ω_{y}, ω_{z}) = (- 0.0524, 0.2793, 0.0175) \end{aligned}

(55)

The final constraints from the Blackbox data are (recall the desired terminal boundary conditions from Eq. )

\begin{aligned} F_{1} (E, N, U) \approx (0, 0, 5), F_{2} (v_{E}, v_{N}, v_{U}) \approx (0, 0, 0) \\ F_{3} (ϕ, θ) = (- 0.0004743, 0.3756) \\ F_{4} (ω_{x}, ω_{y}, ω_{z}) = (- 1.100, 0.2967, 0.05236) \end{aligned}

(56)

Figure 2 shows the E Guidance Euler angles compared with the Euler angles from the Blackbox flight data. The original Blackbox Euler angle data has the roll angle switch from −180 deg to 180 deg because Blackbox constrains the Euler angles from −π to π. To help see this clearer, roll values beyond −π decrease by 2π, which allows the roll angle profile to transition smoothly and eliminates a vertical spike at the transition from −180 deg to 180 deg. The roll angle in E Guidance is smoother and less steep than the flight data roll angle. Even though it is harder to determine in the E Guidance roll angle, both seem to have inflection points at around 0.5 s.

Fig. 2

View large Download slide

Roll maneuver: Euler angles comparison with roll, pitch, and yaw as ϕ, θ, ψ_, respectively

The motor spin rates in Fig. 3 mainly depend on the torques and quaternions, which come from the angular acceleration and Euler angles from E Guidance:

\begin{aligned} ω_{1}^{2} = \frac{M g}{4 k (q_{4}^{2} - q_{1}^{2} - q_{2}^{2} + q_{3}^{2})} + \frac{τ_{ψ}}{4 b} + \frac{τ_{ϕ}}{2 ℓ k} \\ ω_{2}^{2} = \frac{M g}{4 k (q_{4}^{2} - q_{1}^{2} - q_{2}^{2} + q_{3}^{2})} - \frac{τ_{ψ}}{4 b} + \frac{τ_{θ}}{2 ℓ k} \\ ω_{3}^{2} = \frac{M g}{4 k (q_{4}^{2} - q_{1}^{2} - q_{2}^{2} + q_{3}^{2})} + \frac{τ_{ψ}}{4 b} - \frac{τ_{ϕ}}{2 ℓ k} \\ ω_{4}^{2} = \frac{M g}{4 k (q_{4}^{2} - q_{1}^{2} - q_{2}^{2} + q_{3}^{2})} - \frac{τ_{ψ}}{4 b} - \frac{τ_{θ}}{2 ℓ k} \end{aligned}

(57)

Fig. 3

View large Download slide

Roll maneuver: motor spin rates

For the E Guidance motor spin rates, there are small initial torques that generate a smooth roll maneuver. Then, the motor spin rates switch to negative thrust to decelerate and then switch to stabilize to neutral attitude. This switch occurs from the roll maneuver quaternions, which leads to a sinusoidal response with respect to time. For the manual PID flight data motor spin rates, the pilot makes numerous throttle fluctuations to generate the roll maneuver. The PID controller stabilizes the quadcopter’s attitude, and the setpoints are the rotation rates in all three body axes to match the stick input rates commanded by the pilot [29]. Even though the simulated guided roll maneuver is smoother, the pilot has faster reflexes, which causes bigger spikes. However, larger initial motor spin rates force him to aggressively maintain stabilization after performing the $360 \deg$ roll and cause the smaller spikes and fluctuations from 0.6 s to the end of the maneuver. Consequently, the manual PID control flight data have several fluctuations due to manual attitude stabilization, which contributes to the spikes. However, the E Guidance motor spin rates resemble bang-bang arcs, are very smooth, and reach neutral attitude at around 1 s.

6.2 Integration of State Equations.

This subsection integrates the right-hand side of the dynamical model equations in Eqs. (10), (11), (17), and (18). Figure 4 shows the integration of the state equations.

Fig. 4

View large Download slide

Integration of state equations: (a) Position, (b) Velocity, (c) Quaternions, and (d) Angular velocity

The rotational equations of motion of the angular acceleration and time derivatives of the quaternions contain several trigonometric functions due to the (3-2-1) direction cosine matrix and explicit guidance rotational commands. Curve fitting them into higher order polynomials with respect to time works well due to the smooth rotational explicit guidance commands. Having polynomials as functions of time significantly simplifies the integration since integrating trigonometric functions with polynomials is difficult to integrate. Utilizing the Runge–Kutta 8(7) pair with a seventh-order continuous extension solves the ordinary differential equations for the position and velocity state equations [30]. The relative tolerance was set to 0.001, and the absolute tolerance was set to 1 · 10⁻⁶.

6.3 Jacobi Condition (Conjugate Points).

Having closed-form analytical solutions for the state equations and Lagrange multipliers allows one to determine the presence of conjugate points. Since the roll maneuver utilizes the nontrivial intermediate motor thrust case, the last six Lagrange multipliers vanish. Thus, there are 18 integration constants since there are 12 states and six Lagrange multipliers in the intermediate motor thrust case. Let

V

and

S

be the identity matrices, i.e.,

S = I_{12 \times 12}, V = I_{6 \times 6}

⁠, to ensure that

S

and

V

do not yield any undesired zeros after multiplying with [∂s/∂C]^T and [∂λ/∂C]^T, respectively. Note that having undesired zeros in the product of the matrices may potentially yield conjugate points, i.e., δs or δλ may inadvertently have all zeros. The initial values from Eq. at the start of the roll maneuver are the integration constants used in the partial differentiation of the states and costates. Thus, the vector of integration constants, C, is defined as

\begin{aligned} C & = [C_{1} C_{2} \dots C_{17} C_{18}]^{T} \\ = {[\begin{matrix} E_{0} & N_{0} & U_{0} & v_{E, 0} & v_{N, 0} & v_{U, 0} & q_{1, 0} & q_{2, 0} & q_{3, 0} \\ ω_{x, 0} & ω_{y, 0} & ω_{z, 0} & λ_{1} & λ_{2} & λ_{3} & λ_{4, 0} & λ_{5, 0} & λ_{6, 0} \end{matrix}]}^{T} \end{aligned}

(58)

Using the analytical solutions for the states and costates and taking partial derivatives with respect to the integration constants leads to simple and quick computations. None of the state equations depend on λ and do not depend on other initial state and costate variables. The closed-form solutions for the Lagrange multipliers show that the primer vector depends on the first three Lagrange multipliers and its own initial integration constants, i.e., λ_4,0, λ_5,0, λ_6,0. Consolidating all this together yields

\begin{aligned} {[\frac{\partial s}{\partial C}]}^{T} = [I_{12 \times 12} 0_{12 \times 6}], {[\frac{\partial λ}{\partial C}]}^{T} = [I_{6 \times 12} W] W = [\begin{matrix} I_{3 \times 3} & 0_{3 \times 3} \\ I_{3 \times 3} & 0_{3 \times 3} \end{matrix}] \end{aligned}

(59)

Since there are non-zero elements in [∂s/∂C]^T and [∂λ/∂C]^T, δs and δλ will also have non-zero elements. Therefore, conjugate points do not exist, which satisfies the Jacobi condition.

6.4 Transversality Conditions.

Only ψ is free, and the roll maneuver utilizes only the intermediate thrust case, so λ₇ = λ₈ = λ₉ = 0. Since J depends on t₀ and t_f,

\bar{F}

and

\bar{G}

do not depend on t₀ and t_f, and the terms in Eq. reduce to

G_{t_{0}} = - ε, G_{t_{f}} = ε, G_{s_{0}} = μ^{T}, G_{s_{f}} = ν^{T}

(60)

The vectors, μ^T and ν^T, contain zeros because the roll maneuver utilizes the intermediate thrust case due to λ₇ = λ₈ = λ₉ = 0.

6.5 Satisfying the First-Order Necessary Conditions of Optimality.

Using the transversality conditions from Eq. , zero velocity at the end of the roll maneuver, and returning to neutral attitude solves the first four terms in Eq. as

\begin{aligned} G_{s_{0}} = - λ_{0}^{T} = - [λ_{7} (t_{0}) λ_{8} (t_{0}) λ_{9} (t_{0})]^{T} = - [0 0 0]^{T} \\ G_{s_{f}} = λ_{f}^{T} = - [λ_{7} (t_{f}) λ_{8} (t_{f}) λ_{9} (t_{f})]^{T} = - [0 0 0]^{T} \\ G_{t_{0}} = H_{0} \to ε = 0, G_{t_{f}} = - H_{f} = 0 \end{aligned}

(61)

The last four terms of Eq. (28) are zero by the canonical equations and local optimality conditions. Thus, dJ = 0, so the roll maneuver yields an extremal trajectory.

6.6 Legendre–Clebsch Condition.

Computing the second partial derivatives in

H_{u u}

in Eq. yields zeros on the off diagonal elements:

\begin{aligned} \frac{\partial^{2} H}{\partial ω_{1}^{2}} = L + \frac{2 λ_{12} ℓ k}{I_{y y}} + \frac{2 λ_{12} b_{τ}}{I_{z z}} \\ \frac{\partial^{2} H}{\partial ω_{2}^{2}} = L + \frac{2 λ_{10} ℓ k}{I_{x x}} - \frac{2 λ_{12} b_{τ}}{I_{z z}} \\ \frac{\partial^{2} H}{\partial ω_{3}^{2}} = L - \frac{2 λ_{11} ℓ k}{I_{y y}} + \frac{2 λ_{12} b_{τ}}{I_{z z}} \\ \frac{\partial^{2} H}{\partial ω_{4}^{2}} = L - \frac{2 λ_{10} ℓ k}{I_{x x}} - \frac{2 λ_{12} b_{τ}}{I_{z z}} \end{aligned}

(62)

with L defined in Refs. [12,13] as

\begin{aligned} L & = \frac{4 k λ_{4}}{M} (q_{0} q_{2} + q_{1} q_{3}) + \frac{4 k λ_{5}}{M} (q_{2} q_{3} - q_{0} q_{1}) \\ + \frac{2 k λ_{6}}{M} (q_{0}^{2} - q_{1}^{2} - q_{2}^{2} + q_{3}^{2}) \end{aligned}

(63)

Since the roll maneuver utilizes the variable motor thrust case, λ₁₀ = λ₁₁ = λ₁₂ = 0. Also, solving for one of the Lagrange multipliers associated with the primer vector in terms of the other two ensures that L = 0 to satisfy the local optimality condition from Eq. (37) [13]. Consequently, $H_{u u}$ contains all zeros, so this satisfies the necessary Legendre–Clebsch condition but is not sufficient because $H_{u u}$ is non-negative instead of positive definite. Having $H_{u u}$ with all zeros confirms the mathematical theory of second-order conditions for intermediate thrust arcs [14].

6.7 Second-Order Conditions of Optimality.

The previous subsection shows that the Legendre–Clebsch condition satisfies only the necessary condition. Therefore, it does not matter if any of

H_{s s}, H_{u s}, H_{s u}

are positive definite because all of the submatrices must be positive definite to have a strong minimum. Even though the roll maneuver has an extremal and not an optimal trajectory, this subsection discusses the other second partial derivative submatrices for completeness. There are numerous zeros in the

H_{s s}

matrix with non-zero elements for partials with respect to quaternions and angular velocity. Since the roll maneuver utilizes the intermediate thrust case, this narrows it down further to only six non-zero elements without λ₇,…, λ₁₂:

\begin{aligned} \frac{\partial^{2} H}{\partial q_{1}^{2}} = - 2 λ_{6} Γ, \frac{\partial^{2} H}{\partial q_{1} \partial q_{3}} = 2 λ_{4} Γ, \frac{\partial^{2} H}{\partial q_{2}^{2}} = - 2 λ_{6} Γ \\ \frac{\partial^{2} H}{\partial q_{2} \partial q_{3}} = 2 λ_{5} Γ, \frac{\partial^{2} H}{\partial q_{3}^{2}} = 2 λ_{6} Γ, \frac{\partial^{2} H}{\partial q_{3} \partial q_{1}} = 2 λ_{4} Γ \end{aligned}

(64)

There are also numerous zeros in

H_{u s}

and

H_{s u}

with only 12 non-zero elements:

\begin{aligned} \frac{\partial^{2} H}{\partial ω_{1} \partial q_{1}} = \frac{\partial^{2} H}{\partial ω_{2} \partial q_{1}} = \frac{\partial^{2} H}{\partial ω_{3} \partial q_{1}} = \frac{\partial^{2} H}{\partial ω_{4} \partial q_{1}} = \frac{4 λ_{4} k q_{3}}{M} - \frac{4 λ_{5} k q_{4}}{M} - \frac{4 λ_{6} k q_{1}}{M} \\ \frac{\partial^{2} H}{\partial ω_{1} \partial q_{2}} = \frac{\partial^{2} H}{\partial ω_{2} \partial q_{2}} = \frac{\partial^{2} H}{\partial ω_{3} \partial q_{2}} = \frac{\partial^{2} H}{\partial ω_{4} \partial q_{2}} = \frac{4 λ_{4} k q_{4}}{M} + \frac{4 λ_{5} k q_{3}}{M} - \frac{4 λ_{6} k q_{2}}{M} \\ \frac{\partial^{2} H}{\partial ω_{1} \partial q_{3}} = \frac{\partial^{2} H}{\partial ω_{2} \partial q_{3}} = \frac{\partial^{2} H}{\partial ω_{3} \partial q_{3}} = \frac{\partial^{2} H}{\partial ω_{4} \partial q_{3}} = \frac{4 λ_{4} k q_{q}}{M} + \frac{4 λ_{5} k q_{2}}{M} - \frac{4 λ_{6} k q_{3}}{M} \end{aligned}

(65)

6.8 Weierstrass Condition.

To the best of the authors’ knowledge, this is the first time that the Weierstrass condition has been considered for UAVs. Selecting a small control perturbation, δu = 0.01(rad/s), yields an admissible control in the neighborhood of the extremal control, i.e., adding the small control perturbation, δu, to the extremal control, δu, generates the admissible control, $\tilde{u}$ ⁠. Playing with larger values for δu generates the same overall shape of the profile but with taller spikes. Figure 5 shows that δH (unitless) is not semi-positive definite. Consequently, the Weierstrass condition is not satisfied. However, this checks out because the second-order conditions of optimality are not satisfied, i.e., $d^{2} K \geq 0$ ⁠.

Fig. 5

View large Download slide

Weierstrass condition test

Figure 6 compares the Pontryagin function based on admissible control, $\tilde{u}$ ⁠, and extremal control, u^o. The overall trend is that the Pontryagin function with extremal control decreases monotonically, while the Pontryagin function with admissible control follows the same trend but with several fluctuations. Note that both demonstrate negative values throughout the entire roll maneuver. From a thermodynamics perspective, negative values indicate that work is done by the system [31]. The quadcopter consumes battery life to perform the roll maneuver such that energy has left the system, which yields negative values for the Pontryagin function.

Fig. 6

View large Download slide

Comparison of the Pontryagin function with admissible control and extremal control (top figure is zoomed in)

This guidance and control solution leverages autonomous UAV applications for real-time targeting and re-targeting capabilities. The proposed integrated control and E Guidance solutions allow us to develop algorithms that execute onboard independently of human controllers without losing the accuracy of maneuver trajectories and performance time, thereby leveraging autonomy.

7 Conclusion

This study has demonstrated the implementation of the integrated extremal control and explicit guidance of a quadcopter for a $360 \deg$ roll maneuver through novel explicit guidance commands for rotational motion to satisfy the terminal boundary conditions of −2π and 0 rad/s for the desired roll angle and roll angular velocity, respectively. The smooth, integrated extremal control and E Guidance performs well against the manual $360 \deg$ roll maneuver with PID control. This paper has applied the optimal control formalism with explicit guidance for UAVs to generate a field of extremals for quadcopter takeoff and roll maneuvers. Through UAV optimal control analysis by considering the Weierstrass condition, the Jacobi condition, and the second-order conditions of optimality, the roll maneuver is extremal and/or admissible but not optimal. Future work may include conducting flight tests to obtain experimental results to verify the simulation results. Ultimately, the proposed extremal control and explicit guidance integration can leverage the autonomous capabilities of UAVs and be adjusted and extended to spacecraft and other airspace flight vehicles.

Acknowledgment

The research presented in this paper has been supported, in part, by the NASA-funded EPSCoR—Autonomous Control Technologies Unmanned Aerial Systems (ACTUAS) project.

Conflict of Interest

There are no conflicts of interest.

Data Availability Statement

The datasets generated and supporting the findings of this article are obtainable from the corresponding author upon reasonable request.

References

1.

Munishkin

,

A. A.

,

Milutinović

,

D.

, and

Casbeer

,

D. W.

,

2016

, “

Stochastic Optimal Control Navigation With the Avoidance of Unsafe Configurations

,”

The 2016 International Conference on Unmanned Aircraft Systems

,

Arlington, VA

,

June 7–10

, IEEE, pp.

211

–

218

.

Google Scholar

Crossref

2.

Beul

,

M.

, and

Behnke

,

S.

,

2016

, “

Analytical Time-Optimal Trajectory Generation and Control for Multirotors

,”

The 2016 International Conference on Unmanned Aircraft Systems

,

Arlington, VA

,

June 7–10

, IEEE, pp.

87

–

96

.

Google Scholar

Crossref

3.

Blouin

,

C.

,

Lanteigne

,

E.

, and

Gueaieb

,

W.

,

2017

, “

Optimal Control for the Trajectory Planning of Micro Airships

,”

The 2017 International Conference on Unmanned Aircraft Systems

,

Miami, FL

,

June 13–16

, IEEE, pp.

885

–

892

.

Google Scholar

Crossref

4.

Garcia

,

G. A.

,

Kashmiri

,

S.

, and

Shukla

,

D.

,

2017

, “

Nonlinear Control Based on h-Infinity Theory for Autonomous Aerial Vehicle

,”

The 2017 International Conference on Unmanned Aircraft Systems

,

Miami, FL

,

June 13–16

, IEEE, pp.

336

–

345

.

Google Scholar

Crossref

5.

Ohlmeyer

,

E. J.

, and

Phillips

,

C. A.

,

2006

, “

Generalized Vector Explicit Guidance

,”

J. Guid. Control Dyn.

,

29

(

2

), pp.

261

–

268

.

Google Scholar

Crossref

6.

Kuantama

,

E.

,

Vesselenyi

,

T.

,

Dzitac

,

S.

, and

Tarca

,

R.

,

2017

, “

PID and Fuzzy-PID Control Model for Quadcopter Attitude With Disturbance Parameter

,”

Int. J. Comput. Commun. Control

,

12

(

4

), pp.

519

–

532

.

Google Scholar

Crossref

7.

Lee

,

S.

,

Lee

,

J.

,

Lee

,

S.

,

Choi

,

H.

,

Kim

,

Y.

,

Kim

,

S.

, and

Suk

,

J.

,

2018

, “

Sliding Mode Guidance and Control for UAV Carrier Landing

,”

IEEE Trans. Aerosp. Electron. Syst.

,

55

(

2

), pp.

951

–

966

.

Google Scholar

Crossref

8.

Park

,

S.

,

Deyst

,

J.

, and

How

,

J.

,

2004

, “

A New Nonlinear Guidance Logic for Trajectory Tracking

,”

AIAA Guidance, Navigation, and Control Conference and Exhibit

,

Providence, RI

,

Aug. 16–19

, p.

4900

.

Google Scholar

Crossref

9.

Azimov

,

D.

, and

Allen

,

J.

,

2018

, “

Analytical Model and Control Solutions for Unmanned Aerial Vehicle Maneuvers in a Vertical Plane

,”

J. Intell. Rob. Syst.

,

91

(

3–4

), pp.

725

–

733

.

Google Scholar

Crossref

10.

Azimov

,

D.

, and

Kawamura

,

E.

,

2018

, “

Real-Time Guidance, Navigation and Control Framework for Fixed-Wing Aircraft Maneuvers in a Vertical Plane

,”

2018 International Conference on Unmanned Aircraft Systems (ICUAS)

,

Dallas, TX

,

June 12–15

, IEEE, pp.

621

–

630

.

Google Scholar

Crossref

11.

Cherry

,

G.

,

1964

, “

A General, Explicit, Optimizing Guidance Law for Rocket-Propelled Spaceflight

,”

Astrodynamics Guidance and Control Conference

,

Los Angeles, CA

,

Aug. 24–26

, p.

638

.

Google Scholar

Crossref

12.

Kawamura

,

E.

, and

Azimov

,

D.

,

2019

, “

Integrated Optimal Control and Explicit Guidance for Quadcopters

,”

2019 International Conference on Unmanned Aircraft Systems (ICUAS)

,

Atlanta, GA

,

June 11–14

, IEEE, pp.

513

–

522

.

Google Scholar

Crossref

13.

Kawamura

,

E.

, and

Azimov

,

D.

,

2020

, “

Integrated Extremal Control and Explicit Guidance for Quadcopters

,”

J. Intell. Rob. Syst.

,

100

(

3

), pp.

1583

–

1613

.

14.

Kelley

,

H. J.

,

Kopp

,

R. E.

, and

Moyer

,

H.

,

1967

, “Singular Extremals,”

Topics in Optimization

,

G.

Leitmann

, ed.,

Academic Press, Inc.

,

London

, pp.

63

–

101

.

Google Scholar

Crossref

15.

Kawamura

,

E.

, and

Azimov

,

D.

,

2019

, “

Integrated Targeting, Guidance, Navigation, and Control for Unmanned Aerial Vehicles

,”

29th AAS/AIAA Space Flight Mechanics Meeting

,

Ka’anapali, HI

,

Jan. 13–17

.

16.

Gibiansky

,

A.

,

2012

, “

Quadcopter Dynamics, Simulation, and Control

,”

Github

, pp.

1

–

24

. https://github.com/gibiansky/experiments/blob/master/quadcopter/project.pdf

17.

Schaub

,

H.

, and

Junkins

,

J. L.

,

2018

,

Analytical Mechanics of Space Systems

, 4th ed.,

AIAA

,

Reston, VA

.

18.

Luukkonen

,

T.

,

2011

, “

Modelling and Control of Quadcopter

,” Independent Research Project in Applied Mathematics, Espoo, 22, p.

22

.

19.

Palm

,

W. J.

,

2010

,

System Dynamics

, Vol. 2,

McGraw-Hill

,

New York

.

20.

Çengel

,

Y. C.

, and

Cimbala

,

J.

,

2014

,

Fluid Mechanics: Fundamentals and Applications

, 3th ed.,

McGraw-Hill Higher Education

,

Boston, MA

.

21.

Federal Aviation Administration Press Office

,

2020

, “

Fact Sheet—Small Unmanned Aircraft Regulations (Part 107)

,”

Federal Aviation Administration

. https://www.faa.gov/newsroom/small-unmanned-aircraft-systems-uas-regulations-part-107?newsId=22615

22.

Azimov

,

D. M.

,

2017

,

Analytical Solutions for Extremal Space Trajectories

,

Butterworth-Heinemann

,

Cambridge, MA

, pp.

1

–

317

.

23.

Lawden

,

D. F.

,

1963

,

Optimal Trajectories for Space Navigation

, Vol. 3,

Butterworths

,

London

, p.

126

.

24.

Bryson

,

A. E.

, and

Ho

,

Y.-C.

,

2018

,

Applied Optimal Control: Optimization, Estimation, and Control

,

Routledge

,

Boca Raton, FL

.

Google Scholar

Crossref

25.

Klumpp

,

A.

,

1974

, “

Apollo Lunar-Descent Guidance

,”

Automatica

,

10

(

2

), pp.

133

–

146

.

26.

Wilhelm

,

J. P.

, and

Clem

,

G.

,

2019

, “

Vector Field UAV Guidance for Path Following and Obstacle Avoidance With Minimal Deviation

,”

J. Guid. Control Dyn.

,

42

(

8

), pp.

1848

–

1856

.

Google Scholar

Crossref

27.

Gautam

,

A.

,

Sujit

,

P.

, and

Saripalli

,

S.

,

2017

, “

Autonomous Quadrotor Landing Using Vision and Pursuit Guidance

,”

IFACPapersOnLine

,

50

(

1

), pp.

10501

–

10506

.

28.

Viswanathan

,

S. P.

,

Sanyal

,

A. K.

, and

Izadi

,

M.

,

2017

, “

Integrated Guidance and Nonlinear Feedback Control of Underactuated Unmanned Aerial Vehicles in SE(3)

,”

AIAA Guidance, Navigation, and Control Conference

,

Grapevine, TX

,

Jan. 9–13

, p.

1044

.

Google Scholar

Crossref

29.

Clifton

,

D.

,

2015

, “

PID Tuning. Github—Martinbudden/Betaflight/Docs/PID Tuning.md

,”

Github

. https://github.com/martinbudden/betaflight/blob/master/docs/PID

30.

Verner

,

J. H.

,

2010

, “

Numerically Optimal Runge–Kutta Pairs With Interpolants

,”

Numer. Algorithms

,

53

(

2

), pp.

383

–

396

.

Google Scholar

Crossref

31.

Moran

,

M. J.

,

Shapiro

,

H. N.

,

Boettner

,

D. D.

, and

Bailey

,

M. B.

,

2014

,

Fundamentals of Engineering Thermodynamics

, 8th ed.,

John Wiley & Sons

,

Hoboken, NJ

.

Extremal Control and Modified Explicit Guidance for Autonomous Unmanned Aerial Vehicles

Abstract

1 Introduction

2 Quadcopter Dynamical Model

2.1 State Vector.

2.2 Coordinate Systems.

2.3 Forces.

2.4 Torques.

2.5 Dynamical Model Equations.

3 Optimal Control Problem

3.1 Control Vector.

3.2 State Constraint Equations.

3.3 Functional.

3.4 First Differential of Extended Functional.

3.5 Second Differential of Extended Functional.

4 Hamiltonian Formalism

4.1 Pontryagin Function.

4.2 Canonical Equations and Local Optimality Conditions.

4.3 Analytical Solutions for the Lagrange Multipliers—Nontrivial Motor Thrust.

4.4 Weierstrass Condition.

4.5 Legendre–Clebsch Condition.

4.6 Jacobi Condition (Conjugate Points).

5 Explicit Guidance

5.1 Explicit Guidance Computations.

5.2 Discussion of Quadcopter Guidance Problem.

6 Simulation Results

6.1 Simulation Setup.

6.2 Integration of State Equations.

6.3 Jacobi Condition (Conjugate Points).

6.4 Transversality Conditions.

6.5 Satisfying the First-Order Necessary Conditions of Optimality.

6.6 Legendre–Clebsch Condition.

6.7 Second-Order Conditions of Optimality.

6.8 Weierstrass Condition.

7 Conclusion

Acknowledgment

Conflict of Interest

Data Availability Statement

References

Get Email Alerts

Cited By

Related Articles

Related Proceedings Papers

Related Chapters

ASME Journals

ASME Conference Proceedings

ASME eBooks

Resources

Opportunities

This Feature Is Available To Subscribers Only