Keywords

These keywords were added by machine and not by the authors. This process is experimental and the keywords may be updated as the learning algorithm improves.

1 Introduction

In the past decades, the use of fly-by-wire (FBW) became popular as a concept for flight controls. Nowadays, the FBW concept is becoming standard for large transportation aircraft, but research projects of Part-23 general aviation aircraft are also directed towards FBW concepts. In some cases, full FBW is of interest, in some cases the mechanical flight control system is altered by interconnected actuators of (sometimes) lower authority. Especially the emergence of unmanned aerial vehicles (UAV) and electrical flying further led to multiple applications of FBW systems, which incorporate multicopters of small MTOW as well as platforms of several tons.

Both manned and unmanned sensing platforms require waypoint based navigation to complete their missions and as a consequence, trajectory generation and control plays a significant role for these systems. In this paper, the trajectory controller of a fully integrated auto-flight control system is considered. At the institute, a modular control structure was developed in the context of adopting various components of the guidance and control structure to different platforms without the need of redesigning the controllers for the specific applications. Consequently, a range of modules were designed, which range from classical autopilot concepts to trajectory generation and automatic landing as well as to automatic aerodynamic trimming. All of these components may vary for different applications such as UAVs with unstable aircraft dynamics or classical autopilots for manned flight, but the rework can be decreased significantly due to the modular structure.

The trajectory control module is designed as a controller to drive any deviations in the lateral and vertical plane to zero and hence, is a candidate to be utilized by multiple higher-level functions such as waypoint navigation, automatic takeoff and landing, or altitude hold modes. From the controller’s point of view, the deviations and their derivatives are used as control errors whereas other information about the trajectory such as trajectory angles and their rates are further utilized to improve the controller’s performance, especially in cases of dynamical trajectories.

Previous work conducted in the field of trajectory control usually rely on multiple loops to address the problem of trajectory tracking. Commonly, there are two outer loops, which first compute a track / flight path angle command due to deviations and then compute inner loop commands to control the angular error. In this field, work has been conducted ranging from linear controllers to nonlinear methods such as backstepping or nonlinear dynamic inversion. Examples of such methods can be found in [1,2,3,4,5,6,7,8,9,10]. In this paper, a second-order approach for the outer loop is presented, which only consists of one control loop using both the position deviations in the lateral and vertical plane as well as their derivatives as control errors. This reduces the required time scale separation as there are just two second order loops used in the modular structure: trajectory controller and inner loop controller. The trajectory controller’s design is based on the method of nonlinear dynamic inversion (NDI, see e.g. [11, 12]), which inherently leads to utilizing the feedforward terms contained in the dynamical trajectory, along with a PD controller.

In this paper, previous work [13, 14] is extended, specifically the second order error dynamics of the lateral and vertical position deviations, which are the basis for the derivation of the controller, are addressed in detail and the dynamic inversion applied to these error dynamics is derived for the 3D controller. Additionally, the command transformation to the inner loop controller is derived and further information is given regarding the trajectory control and the environment. Ultimately, specific simulation results are presented to show the controller’s lateral tracking performance in comparison to a linear controller and in presence of significant wind as additional test data to the first principal flight test demonstration published in Ref. [14].

The remainder of this paper is arranged as follows: Section 1.1 states the nomenclature used in this paper. The environment of the trajectory controller is presented in Sect. 2, where the trajectory generation (Sect. 2.1), the command transformation (Sect. 2.2), and the inner loop controller (Sect. 2.3) are discussed in more detail. Section 3 deals with the trajectory control design, specifically the discussion of the kinematic equations of motion, the derivation of the nonlinear error dynamics, its dynamic inversion, and the control design. In Sect. 4 simulation results are given to show the controller’s performance in presence of wind and also by comparison to a linear PD controller without feedforward terms. Finally, concluding remarks are given in Sect. 5.

1.1 Nomenclature

In this paper, mainly vectors, which are noted bold, and scalars, noted in italic letters, are used. Furthermore, \(\overrightarrow{\left( \cdot \right) }\) indicates that a vector is an element of the Euclidean space \(\mathbb {R}^3\). In addition, Euclidean vectors considered in this paper are the following:

  • \(\left( \overrightarrow{\mathbf {r}}^{P}\right) _B\) denotes a position vector at the point P given in the body frame.

  • \(\left( \overrightarrow{\mathbf {V}}^{P}_{K}\right) ^{E}\) denotes a kinematic velocity vector at the point P differentiated with respect to the E frame.

  • \(\left( \overrightarrow{\mathbf {\varvec{\omega }}}^{OK}\right) \) denotes an angular rate of the K frame with respect to the O frame.

All vector variables used in this paper can be compounded based on the concept introduced by these three examples.

2 Control Environment

The Institute of Flight System Dynamics owns a Diamond Aircraft Industries DA42 NG aircraft, which is adapted to serve as flying testbed. For this purpose, the aircraft has been modified to be controlled using a fly-by-wire system, which gears into the existing mechanical flight control system utilizing both an electrical and an overload clutch. The testbed is equipped with a safety system, which enables the safety pilot to either open the clutches by pushing a disengage button or by application of a sufficiently large stick force.

Figure 1 (taken from [13, 14] for clarity) shows the overall modular structure of the integrated autoflight system. The modules of the integrated auto-flight system range from the input monitoring and system automation part [15], where the incoming signals of sensors and ground station are evaluated and the overall operating modes are computed based on the result, to the actuator control electronics (ACEs) and the engine control unit (ECU).

Fig. 1
figure 1

Integrated autoflight system

Full size image

The modular concept was chosen to keep certain parts of identical structure throughout different platforms. For this, certain interfaces are maintained throughout various projects. As an example, the control structure of the autopilot persists over several platforms and just requires adaptation of certain parameters such as gains and limits to the dynamics of the aircraft. The inner loop module, however, may differ significantly over different applications as it depends fully on aircraft dynamics and control allocation.

The trajectory controller is a part of the auto-flight-system, which in this context denotes the autopilot functionalities such as heading hold or flight level change mode and the speed controller / auto-thrust. The auto-flight-system also incorporates energy protection and prioritization, limiting the speed and the flight path angle such that no adverse conditions occur. In context of vertical trajectory control or upon application of the altitude hold mode, along with deviations also the permissible flight path angles are forwarded to the trajectory controller, which are then used to additionally limit the trajectory controller’s specific force commands. For more details on the auto-flight-system refer to [16, 17].

The commands of the auto-flight-system and the trajectory controller, which are further dedicated as outer loop, are selected and forwarded to the inner loop controller. While the outer loops provide normalized specific force commands in the kinematic frame with respect to unaccelerated flight, these commands are transformed by a configuration-specific module, which is further depicted in Sect. 2.2, to the inputs required by the corresponding inner loop, which then computes the final commands to the ACEs and ECUs. The inner loop of the DA42 is briefly discussed in Sect. 2.3.

The trajectory controller deals with any deviations as long as they comply with the interface described in Sect. 2.1. In the current implementation for the DA42, trajectories and the corresponding deviations may either be provided by the automatic takeoff and landing module or a trajectory generation module [18, 19], which is based on waypoints. The latter is briefly discussed in Sect. 2.1. Furthermore, the trajectory controller is used to drive the altitude deviation to zero when the altitude hold mode is applied.

2.1 Trajectory Generation

The paper deals with a modular trajectory controller, which is designed to drive the deviations in a trajectory frame to zero. From the controller’s point of view, any source of deviations and additional trajectory information is permitted. In this paper, fly-by trajectories generated by the trajectory generation module are considered. Here, the interface to this module is briefly introduced.

Fig. 2
figure 2

Trajectory frame with deviations and their corresponding time derivatives

Full size image

For this purpose, the trajectory frame T, which is related to the kinematic frame K as commonly used in aerospace applications, is introduced as follows:

  • The x-axis is aligned with and in the direction of the desired velocity at the footpoint on the trajectory.

  • The z-axis points downwards parallel to the projection of the local normal to the WGS84 ellipsoid into a plane perpendicular to the x-axis.

  • The y-axis points parallel to the earth’s surface and yields a right hand system with the previous two axes.

This frame is further illustrated in Fig. 2 (taken from [13, 14]), where the deviations \((\varDelta y)_T\), \((\varDelta z)_T \in \mathbb {R}\) and its time derivatives \((\varDelta \dot{y})_T\), \((\varDelta \dot{z})_T \in \mathbb {R}\) are also displayed.

The trajectory controller is driven by the following inputs:

  • The deviations between aircraft and desired trajectory

$$\begin{aligned} \left( \overrightarrow{\mathbf {r}}^{FR}\right) _T = \begin{pmatrix} \varDelta x\\ \varDelta y \\ \varDelta z \end{pmatrix}_T\triangleq \begin{pmatrix}x^R-x^F \\ y^R-y^F \\ z^R-z^F \end{pmatrix}_T, \end{aligned}$$
(1)

  • the corresponding time derivatives of these deviations

$$\begin{aligned} \left( \overrightarrow{\mathbf {V}}^{FR}_{K}\right) ^{T}_T = \begin{pmatrix} \varDelta \dot{x}\\ \varDelta \dot{y} \\ \varDelta \dot{z} \end{pmatrix}_T^T \triangleq \begin{pmatrix}\dot{x}^R-\dot{x}^F \\ \dot{y}^R-\dot{y}^F \\ \dot{z}^R-\dot{z}^F \end{pmatrix}_T^T, \end{aligned}$$
(2)

  • the desired trajectory angles \(\chi _T\), \(\gamma _T\),

  • the desired trajectory angular rates \(\dot{\chi }_T\), \(\dot{\gamma }_T\),

  • the desired trajectory angular accelerations \(\ddot{\chi }_T\), \(\ddot{\gamma }_T\),

  • the desired kinematic acceleration at the trajectory footpoint \(\dot{V}^F_K\).

Note that \(\overrightarrow{\mathbf {r}}^{FR}, \overrightarrow{\mathbf {V}}^{FR}_K \in \mathbb {R}^3\) and all the other components are real valued.

The current state of the trajectory generation module, previously presented in [18, 19], which generates a trajectory online based on 3+1 given waypoints, employs waypoints of the type fly-by, fly-over, and loiter, where just fly-by trajectories and a loiter are considered in this paper. Note that the trajectory is only generated for three-dimensional trajectories not employing the x-position on the trajectory, i.e. the time component, as the airspeed is controlled independently by the auto-flight system to maintain the aerodynamic integrity of the system.

A fly-by trajectory is generated using the current “from” waypoint, the fly-by waypoint, and the next waypoint after the fly-by to generate a circle passing the fly-by waypoint. Note that the radii of the circle of the fly-by maneuvers and loiters are computed based on the current kinematic velocity, a wind estimation as well as the current and commanded airspeeds. The maximum expected kinematic velocity throughout the turn is used to calculate the radius by aiming for a turn rate of \(5^\circ /s\). In order to smoothly start the curve, the trajectory generation is currently based on clothoids [19,20,21,22] for fly-by maneuvers, which provide a linear devolution of the trajectory angular rate \(\dot{\chi }_T\). As a consequence, however, the angular acceleration \(\ddot{\chi }_T\) employs non-continuous characteristics and hence, the angular accelerations remain unused throughout this paper.

2.2 Outer - Inner Loop Command Interface

As mentioned earlier, the outer loops provide normalized specific force commands in the kinematic frame with respect to unaccelerated flight, i.e. curvature commands given by

$$\begin{aligned} \overrightarrow{\mathbf {u}}_{ol,nrm,K} = \begin{bmatrix} \frac{\varDelta f_{x,cmd,K}}{g}&\frac{f_{y,cmd,K}}{g}&\frac{\varDelta f_{z,cmd,K}}{g} \end{bmatrix}^\mathrm {T}, \end{aligned}$$
(3)

where g represents the acceleration due to gravity. In the following, the curvature commands are first transformed to the north-east-down (NED, O) frame, then converted to absolute normalized specific forces including gravity, and ultimately rotated about the heading \(\varPsi \) as well as the pitch angle \(\varTheta \), yielding

$$\begin{aligned} \overrightarrow{\mathbf {u}}_{ol,nrm,B'} =\mathbf {T}_{B'O} \left( \varPsi ,\varTheta \right) \left( \mathbf {T}_{OK} \left( -\gamma _K,-\chi _K \right) \overrightarrow{\mathbf {u}}_{ol,nrm,K} -\begin{pmatrix} 0 \\ 0 \\ 1 \end{pmatrix}_O\right) , \end{aligned}$$
(4)

where \(\mathbf {T}\in \mathbb {R}^{3\times 3}_i\) depict transformation matrices, \(\gamma _K\) is the kinematic flight path angle, and \(\chi _K\) denotes the kinematic track / course angle. Finally, the command transformation to the body frame can be obtained by

$$\begin{aligned} \overrightarrow{\mathbf {u}}_{ol,nrm,B} = \begin{bmatrix} 1&0&0 \\ 0&\mathrm {cos}\left( \varPhi \right)&\mathrm {sin}\left( \varPhi \right) \\ 0&-\mathrm {sin}\left( \varPhi \right)&\mathrm {cos}\left( \varPhi \right) \end{bmatrix} \overrightarrow{\mathbf {u}}_{ol,nrm,B'}, \end{aligned}$$
(5)

where \(\varPhi \) depicts the roll angle of the aircraft. Now, as no lateral acceleration in the body frame is desired for a coordinated turn, the second row of (5) is utilized to compute

$$\begin{aligned} 0 = \frac{f_{y,cmd,B'}}{g}\mathrm {cos}\left( \varPhi _{cmd}\right) +\frac{f_{z,,B'}}{g}\mathrm {sin}\left( \varPhi _{cmd}\right) , \end{aligned}$$
(6)

ultimately yielding the roll angle command

$$\begin{aligned} \varPhi _{cmd} = \mathrm {atan}\left( -\frac{f_{y,cmd,B'}}{f_{z,cmd,B'}}\right) . \end{aligned}$$
(7)

Taking into account the second and third row of (5) and utilizing \(\varPhi _{curr}\), we have

$$\begin{aligned} f_{y,cmd,B'}= & {} - f_{z,cmd,B'}\mathrm {tan}\left( \varPhi _{curr}\right) \end{aligned}$$
(8)
$$\begin{aligned} \frac{f_{z,cmd,B}}{g}= & {} -\frac{f_{y,cmd,B'}}{g}\mathrm {sin}\left( \varPhi _{curr}\right) +\frac{f_{z,cmd,B'}}{g}\mathrm {cos}\left( \varPhi _{curr}\right) , \end{aligned}$$
(9)

and the normal body load factor command in the longitudinal plane as commanded to the inner loop can be computed as

$$\begin{aligned} \frac{f_{z,cmd,B}}{g}= \frac{f_{z,cmd,B'}}{g\mathrm {cos}\left( \varPhi _{curr}\right) }, \end{aligned}$$
(10)

where \(\varPhi _{curr}\) is currently chosen as the measurement of the roll angle, but can be altered by a small feedforward term based on the roll rate and the computational rate. The command transformation as given by Eqs. (8) and (10) is designed in this form to cope with the different time scales regarding longitudinal and lateral motion. Therefore, the given longitudinal command is computed using the current measurement of the roll angle as it takes more time to built up the angle rather than the longitudinal acceleration to maintain the appropriate curvature in both axes.

2.3 Inner Loop

As mentioned in Sect. 2.2, the inner loop comprises a normal body load factor command system in the longitudinal plane and a roll angle command system in the lateral plane. In case of the longitudinal plane, the normalized normal acceleration in the body frame and the pitch rate q are used for a PI controller. The lateral inner loop comprises feedback of roll and yaw rates, p and r, respectively, along with the normalized body lateral acceleration and the roll angle for a MIMO control architecture.

Since the control design of the inner loop is based on linear approaches such as LQR [23] for the longitudinal plane and eigenstructure assignment [24] for the lateral plane, gain scheduling is required to compose the controller over the whole envelope. In order to increase the validity of the linear controller and incorporate turn compensation, the feedback signals are treated to achieve smaller errors as inputs to the inner loop controller. As a consequence, the feedback signals as seen from the inner loop are given by

$$\begin{aligned} \frac{\delta f_{z,B}}{g}= & {} \frac{f_{z,B}}{g}+\frac{\mathrm {cos}\left( \varTheta \right) }{\mathrm {cos}\left( \varPhi \right) },\end{aligned}$$
(11)
$$\begin{aligned} \delta p= & {} p - \frac{g}{V_K^R}\mathrm {tan}\left( \varPhi \right) ,\end{aligned}$$
(12)
$$\begin{aligned} \delta q= & {} q - \frac{g}{V_K^R}\mathrm {tan}\left( \varPhi \right) \mathrm {cos}\left( \varTheta \right) \mathrm {sin}\left( \varPhi \right) ,\end{aligned}$$
(13)
$$\begin{aligned} \delta r= & {} r - \frac{g}{V_K^R}\mathrm {sin}\left( \varPhi \right) \mathrm {cos}\left( \varTheta \right) , \end{aligned}$$
(14)

where \(V_K^R\) denotes the absolute kinematic velocity of the reference point and \(\mathrm {tan}\left( \mu _K\right) \) is approximated by \(\mathrm {tan}\left( \varPhi \right) \) for turn compensation as the kinematic bank angle \(\mu _K\) is not available for measurement. In order to remain in the same magnitude, also the normal body load factor command is treated by means of

$$\begin{aligned} \frac{\delta f_{z,cmd,B}}{g} = \frac{f_{z,cmd,B}}{g}+\frac{\mathrm {cos}\left( \varTheta \right) }{\mathrm {cos}\left( \varPhi \right) }. \end{aligned}$$
(15)

The current gains are chosen in such a way that the inherent dynamics of the DA42 are not changed significantly. In the lateral plane, the roll dynamics and the natural frequency of the dutch roll are maintained while the yaw damping is increased. Furthermore, the spiral pole is set to −1 for the whole envelope. In the longitudinal plane, the pitch damping is increased and the normal body load factor command system is implemented. The tracking performance is achieved by the feedforward part, while the feedback is designed primarily for disturbance rejection.

3 Trajectory Controller

In this section, the proposed trajectory controller is presented. Section 3.1 gives a short derivation of the kinematic equations of motion for the reference point of a rigid body aircraft. The error dynamics used for the trajectory controller are derived in Sect. 3.2. The nonlinear dynamic inversion controller, which is applied based on these error dynamics, is presented and analyzed in Sect. 3.3.

3.1 Kinematic Equations of Motion

This section deals with the kinematic equations of motion used for the path dynamics of an aircraft. For this purpose, the results known from e.g. [25, 26] are computed based on Newton’s Second Axiom. For the usage addressed in this paper, the reference point R of a rigid body aircraft is considered.

Assumption 1

The aircraft is assumed to be a rigid body. As a consequence, the relative position between points on the aircraft does not change over time, resulting in

$$\begin{aligned} \left( \overrightarrow{\mathbf {V}}^{RP}_{K}\right) ^{B} = 0. \end{aligned}$$
(16)

For computing the equations of motion, consider the position vector \(\overrightarrow{\mathbf {r}}^P \in \mathbb {R}^3\)

$$\begin{aligned} \left( \overrightarrow{\mathbf {r}}^{P}\right) = \left( \overrightarrow{\mathbf {r}}^{R}\right) + \left( \overrightarrow{\mathbf {r}}^{RP}\right) , \end{aligned}$$
(17)

and the corresponding velocities in the inertial frame given by

$$\begin{aligned} \left( \overrightarrow{\mathbf {V}}^{R}_{K}\right) ^{I}= & {} \left( \overrightarrow{\mathbf {V}}^{R}_{K}\right) ^{E} + \left( \overrightarrow{\mathbf {\varvec{\omega }}}^{IE}\right) \times \left( \overrightarrow{\mathbf {r}}^{R}\right) , \quad \end{aligned}$$
(18)
$$\begin{aligned} \left( \overrightarrow{\mathbf {V}}^{RP}_{K}\right) ^{I}= & {} \left( \overrightarrow{\mathbf {\varvec{\omega }}}^{IB}\right) \times \left( \overrightarrow{\mathbf {r}}^{RP}\right) , \end{aligned}$$
(19)

which are valid according to Assumption 1. In addition, the following assumption is made, which is fairly standard [25]:

Assumption 2

The influence of the mass flow onto the impulse of the aircraft can be neglected:

$$\begin{aligned} \dot{m} \left( \overrightarrow{\mathbf {V}}^{P}_{K}\right) ^{E} \approx 0. \end{aligned}$$
(20)

Differentiating the position vector (17) twice and incorporating the velocities (18), (19), and the Assumptions 1 and 2, Newton’s Second Axiom can be given as

$$\begin{aligned} \sum {\left( \overrightarrow{\mathbf {F}}^{R}\right) } =m \left( \dot{\overrightarrow{\mathbf {V}}}^{R}_{K}\right) ^{EO} + m \overrightarrow{\varvec{\mathrm {a}}}_{add}. \end{aligned}$$
(21)

Remark 1

Note that \(\overrightarrow{\varvec{\mathrm {a}}}_{add}\) is a term containing all additional accelerations such as the Coriolis term, the acceleration due to the transport rate, and the acceleration due to difference of reference point R and center of gravity G. When considering a flat, non-rotating earth and choosing the center of gravity as reference point, \(\overrightarrow{\varvec{\mathrm {a}}}_{add} = \overrightarrow{\varvec{\mathrm {0}}}\). For reasons of brevity, \(\overrightarrow{\varvec{\mathrm {a}}}_{add}\) is neglected in the subsequent sections.

Making use of the transformation into the kinematic frame

$$\begin{aligned} \left( \dot{\overrightarrow{\mathbf {V}}}^{R}_{K}\right) ^{EO} = \left( \dot{\overrightarrow{\mathbf {V}}}^{R}_{K}\right) ^{EK} + \left( \overrightarrow{\mathbf {\varvec{\omega }}}^{OK}\right) \times \left( \overrightarrow{\mathbf {V}}^{R}_{K}\right) ^{E}, \end{aligned}$$
(22)

the kinematic equations of motion can be computed as

$$\begin{aligned} \dot{V}^R_K= & {} \varDelta f_{x,K}, \end{aligned}$$
(23)
$$\begin{aligned} \dot{\chi }_K= & {} \frac{f_{y,K}}{V_K^R\mathrm {cos{\gamma _K}}}, \end{aligned}$$
(24)
$$\begin{aligned} \dot{\gamma }_K= & {} \frac{-\varDelta f_{z,K}}{V_K^R}, \end{aligned}$$
(25)

where the specific forces are defined as

$$\begin{aligned} \varDelta f_{x,K}\triangleq & {} \frac{(X_A^R+X_P^R)_K}{m}-g \mathrm {sin{\gamma _K}}, \end{aligned}$$
(26)
$$\begin{aligned} f_{y,K}\triangleq & {} \frac{(Y_A^R+Y_P^R)_K}{m}, \end{aligned}$$
(27)
$$\begin{aligned} \varDelta f_{z,K}\triangleq & {} \frac{(Z_A^R+Z_P^R)_K}{m}+g \mathrm {cos{\gamma _K}}, \end{aligned}$$
(28)

where \(m\in \mathbb {R}\) denotes the mass of the aircraft and \((X_A^R)_K\), \((Y_A^R)_K\), \((Z_A^R)_K \in \mathbb {R}\) as well as \((X_P^R)_K\), \((Y_P^R)_K\), \((Z_P^R)_K \in \mathbb {R}\) are the components of the aerodynamic and propulsion forces at the reference point, respectively, noted in the kinematic frame.

For the further derivation of the trajectory controller, the aircraft dynamics related to the trajectory are given by the kinematic Eqs. (23)–(25). Note that the forces affecting these dynamics require an inner loop controller supplying the respective actuator commands for these loads. In order to achieve this, the commands in the kinematic frame are transformed into roll angle and normal body load factor commands as discussed in Sect. 2.2, which are then utilized by the inner loop controller briefly introduced in Sect. 2.3, whereas the airspeed is controlled by a speed controller as introduced in [16, 17].

3.2 Nonlinear Error Dynamics

In this section, the nonlinear error dynamics between the reference point of the aircraft and a reference trajectory are computed. The relative deviation is defined in (1) and its time derivative is given by Eq. (2). This relative velocity can be further computed as

$$\begin{aligned} \left( \overrightarrow{\mathbf {V}}^{FR}_{K}\right) ^{T}_T = \left( \overrightarrow{\mathbf {V}}^{FR}_{K}\right) ^{E}_T + \left( \overrightarrow{\mathbf {\varvec{\omega }}}^{TO}\right) _T \times \left( \overrightarrow{\mathbf {r}}^{FR}\right) _T, \end{aligned}$$
(29)

where the angular rate is specified as

$$\begin{aligned} \left( \overrightarrow{\mathbf {\varvec{\omega }}}^{TO}\right) _T = \begin{pmatrix} \dot{\chi }_T\mathrm {sin}\gamma _T \\ -\dot{\gamma }_T \\ -\dot{\chi }_T\mathrm {cos}\gamma _T\end{pmatrix}. \end{aligned}$$
(30)

As mentioned earlier, the controller is designed to use the specific forces as inputs to the inner loop. For this purpose, the error dynamics are differentiated once again with respect to time, yielding

$$\begin{aligned} \begin{aligned} \left( \dot{\overrightarrow{\mathbf {V}}}^{FR}_{K}\right) ^{TT}_T = \left( \dot{\overrightarrow{\mathbf {V}}}^{FR}_{K}\right) ^{ET}_T + \left( \dot{\overrightarrow{{\varvec{\omega }}}}^{TO}\right) ^T_T \times \left( \overrightarrow{\mathbf {r}}^{FR}\right) _T + \left( \overrightarrow{\mathbf {\varvec{\omega }}}^{TO}\right) _T \times \left( \overrightarrow{\mathbf {V}}^{FR}_{K}\right) ^{T}_T, \end{aligned}\nonumber \\ \end{aligned}$$
(31)

which can be further derived using

$$\begin{aligned} \begin{aligned} \left( \dot{\overrightarrow{\mathbf {V}}}^{FR}_{K}\right) ^{ET}_T = \left( \dot{\overrightarrow{\mathbf {V}}}^{R}_{K}\right) ^{EK}_T + \left( \overrightarrow{\mathbf {\varvec{\omega }}}^{TK}\right) _T \times \left( \overrightarrow{\mathbf {V}}^{R}_{K}\right) ^{E}_T - \left( \dot{\overrightarrow{\mathbf {V}}}^{F}_{K,des}\right) ^{ET}_T. \end{aligned} \end{aligned}$$
(32)

Employing

$$\begin{aligned} \left( \overrightarrow{\mathbf {\varvec{\omega }}}^{TK}\right) _T = \left( \overrightarrow{\mathbf {\varvec{\omega }}}^{TO}\right) _T + \left( \overrightarrow{\mathbf {\varvec{\omega }}}^{OK}\right) _T, \end{aligned}$$
(33)

we arrive at

$$\begin{aligned} \begin{aligned} \left( \dot{\overrightarrow{\mathbf {V}}}^{FR}_{K}\right) ^{TT}_T&= \left( \dot{\overrightarrow{\mathbf {V}}}^{R}_{K}\right) ^{EK}_T + \left( \overrightarrow{\mathbf {\varvec{\omega }}}^{OK}\right) _T \times \left( \overrightarrow{\mathbf {V}}^{R}_{K}\right) ^{E}_T - \left( \dot{\overrightarrow{\mathbf {V}}}^{F}_{K,des}\right) ^{ET}_T \\&+ \left( \dot{\overrightarrow{{\varvec{\omega }}}}^{TO}\right) ^T_T \times \left( \overrightarrow{\mathbf {r}}^{FR}\right) _T + \left( \overrightarrow{\mathbf {\varvec{\omega }}}^{TO}\right) _T \times \left( \left( \overrightarrow{\mathbf {V}}^{FR}_{K}\right) ^{T}_T + \left( \overrightarrow{\mathbf {V}}^{R}_{K}\right) ^{E}_T \right) , \end{aligned} \end{aligned}$$
(34)

where the terms on the top row of (34) contain the kinematic aircraft dynamics (22) with the control inputs \(\varDelta f_{x,K}\), \(f_{y,K}\), \(\varDelta f_{z,K}\), and the other terms are related to the inputs to the trajectory controller specified in Sect. 2.1.

3.3 Nonlinear Dynamic Inversion Controller

The error dynamics (34) are now transformed using nonlinear dynamic inversion [11, 12]. For this purpose, (34) is written as

$$\begin{aligned} \ddot{\overrightarrow{\mathbf {y}}}_{full} = \mathbf {G}_{full}\left( \gamma _K, \gamma _T, \varDelta \chi \right) \overrightarrow{\mathbf {u}}_{full}+ \overrightarrow{\mathbf {f}}_{full}\left( \gamma _K, \gamma _T, \varDelta \chi ,\kappa \right) , \end{aligned}$$
(35)

where \(\varDelta \chi \triangleq \chi _K - \chi _T\) and

$$\begin{aligned} \kappa \triangleq \left( \left( \overrightarrow{\mathbf {r}}^{FR}\right) _T , \left( \overrightarrow{\mathbf {V}}^{FR}_{K}\right) ^{T}_T\right) \end{aligned}$$
(36)

for brevity. The input vector \(\overrightarrow{\mathbf {u}}_{full} \in \mathbb {R}^3\) is given by

$$\begin{aligned} \overrightarrow{\mathbf {u}}_{full} = \begin{bmatrix} \varDelta f_{x,K}&f_{y,K}&\varDelta f_{z,K} \end{bmatrix}^\mathrm {T}. \end{aligned}$$
(37)

Additionally, the matrix \(\mathbf {G}_{full}\left( \gamma _K, \gamma _T, \varDelta \chi \right) \in \mathbb {R}^{3 \times 3}\) essentially consists of the transformation from the kinematic frame into the trajectory frame

$$\begin{aligned} \mathbf {T}_{TK} = \mathbf {T}_{TK}\left( -\gamma _K,-\varDelta \chi , \gamma _T \right) \in \mathbb {R}^{3 \times 3}, \end{aligned}$$
(38)

and the terms of (34) not containing the aircraft dynamics are incorporated in the nonlinearity vector \(\overrightarrow{\mathbf {f}}_{full}\left( \gamma _K, \gamma _T, \varDelta \chi ,\kappa \right) \in \mathbb {R}^3\). Furthermore, \( \ddot{\overrightarrow{\mathbf {y}}}_{full} \in \mathbb {R}^3\) represents the second time derivative of the output according to the methodology of nonlinear dynamic inversion, where the output is chosen

$$\begin{aligned} \overrightarrow{\mathbf {y}}_{full} = (\overrightarrow{\mathbf {r}}^{FR}) . \end{aligned}$$
(39)

Now, by choosing the control law

$$\begin{aligned} \overrightarrow{\mathbf {u}}_{full} = \mathbf {G}_{full}^{-1}\left( \gamma _K, \gamma _T, \varDelta \chi \right) \left( \overrightarrow{\varvec{\nu }}_{full} - \overrightarrow{\mathbf {f}}_{full}\left( \gamma _K, \gamma _T, \varDelta \chi ,\kappa \right) \right) , \end{aligned}$$
(40)

the plant (35) would be transformed into

$$\begin{aligned} \ddot{\overrightarrow{\mathbf {y}}}_{full} = \overrightarrow{\varvec{\nu }}_{full}, \end{aligned}$$
(41)

where \(\overrightarrow{\varvec{\nu }}_{full} \in \mathbb {R}^3\) denotes the pseudo control, which is chosen by the designer. Note that (41) represents an integrator chain, which is stable for an adequate choice of this pseudo control.

3.3.1 Reduced Dynamic Inversion

In an aircraft application, the focus lies on aerodynamic integrity of the aircraft. Hence, for application to general aviation aircraft with lower never exceedance speeds, the ground speed is not the control variable. Therefore, the control law (40) is not applied in this paper since it is designed for geometric deviations incorporating the kinematic x-axis.

In case of the DA42, the calibrated airspeed is used as a control variable employing an auto-thrust to compute thrust lever commands to the ECU. In case of waypoint navigation and altitude hold modes, the speed is controlled by thrust whereas speed by pitch is active for flight level change modes. Note that the trajectory controller always implies speed by thrust as it is used to drive geometrical deviations in both y- and z axes to zero. A planned climb from one waypoint to another is consequently performed using speed by thrust. In case of unachievable vertical changes, certain protections occur, which are addressed in Ref. [17].

As the x-axis is surrendered in the DA42 set-up, the specific force in direction of the kinematic x-axis, \(\varDelta f_{x,K}\), is not used as a control variable anymore. Consequently, the control system is reduced to second order and \(\varDelta f_{x,K}\) is taken as a measurement. In order to compute the pseudo control \(\nu _{full,x}\), the corresponding row of (40) is employed using \(\varDelta x = 0, \varDelta \dot{x} = 0\) and by insertion of \(\nu _{full,x}\), we arrive at the reduced system given by

$$\begin{aligned} \mathbf {u} = \mathbf {G}^{-1}\left( \gamma _K, \gamma _T, \varDelta \chi \right) \left( \varvec{\nu }- \mathbf {f}\left( \gamma _K, \gamma _T, \varDelta \chi ,\kappa ,\varDelta f_{x,K}\right) \right) , \end{aligned}$$
(42)

where \(\mathbf {u} = \begin{bmatrix} f_{y,K}&\varDelta f_{z,K} \end{bmatrix}^\mathrm {T} \in \mathbb {R}^2\) is the control input, \(\mathbf {f}\left( \gamma _K, \gamma _T, \varDelta \chi ,\kappa ,\varDelta f_{x,K}\right) = \begin{bmatrix} f_y&f_z \end{bmatrix}^\mathrm {T}\in \mathbb {R}^2\) is the nonlinearity vector with the components

$$\begin{aligned} f_y= & {} \varDelta f_{x,K}\mathrm {sin}(\varDelta \chi )\mathrm {cos}(\gamma _T) - V_K^R\dot{\chi }_T\mathrm {cos}(\gamma _K)\mathrm {cos}(\varDelta \chi ) \end{aligned}$$
(43)
$$\begin{aligned}- & {} \left( \dot{\chi }_T\dot{\gamma }_T\mathrm {cos}(\gamma _T)+\ddot{\chi }_T\mathrm {sin}(\gamma _T)\right) \varDelta z -\dot{\chi }\mathrm {sin}(\gamma _T)\varDelta \dot{z}, \nonumber \\ f_z= & {} \varDelta f_{x,K}\left( \mathrm {cos}(\gamma _K)\mathrm {sin}(\gamma _T)\left( \mathrm {cos}(\varDelta \chi ) -1 \right) -\mathrm {sin}(\gamma _K-\gamma _T)\right) \\+ & {} V_K^R\dot{\gamma }_T\left( \mathrm {cos}(\gamma _K)\mathrm {cos}(\gamma _T)\left( \mathrm {cos}(\varDelta \chi ) -1 \right) + \mathrm {cos}(\gamma _K - \gamma _T)\right) \nonumber \\+ & {} V_K^R\dot{\chi }_T\mathrm {cos}(\gamma _K)\mathrm {sin}(\gamma _T)\mathrm {sin}(\varDelta \chi )+\dot{\chi }\mathrm {sin}(\gamma _T)\varDelta \dot{y} \nonumber \\+ & {} \left( \dot{\chi }_T\dot{\gamma }_T\mathrm {cos}(\gamma _T)+\ddot{\chi }_T\mathrm {sin}(\gamma _T)\right) \varDelta y \nonumber , \end{aligned}$$
(44)

and the input matrix \(\mathbf {G} \in \mathbb {R}^{2\times 2}\) is given by

$$\begin{aligned} \mathbf {G} \triangleq \begin{bmatrix} \mathrm {cos}(\varDelta \chi )&\mathrm {sin}(\varDelta \chi )\mathrm {sin}(\gamma _K) \\ -\mathrm {sin}(\varDelta \chi )\mathrm {sin}(\gamma _T)&\mathrm {sin}(\gamma _K)\mathrm {sin}(\gamma _T)\left( \mathrm {cos}(\varDelta \chi ) -1 \right) + \mathrm {cos}(\gamma _K - \gamma _T) \end{bmatrix}.\nonumber \\ \end{aligned}$$
(45)

Note that \(\mathbf {G}\) is not invertible if the determinant given by

$$\begin{aligned} \mathrm {det}\left( \mathbf {G} \right) = \mathrm {cos}(\gamma _K)\mathrm {cos}(\gamma _T)\left( \mathrm {cos}(\varDelta \chi ) -1 \right) + \mathrm {cos}(\gamma _K - \gamma _T) \end{aligned}$$
(46)

is zero. This is the case for \(\left| \varDelta \chi \right| \approx 90^\circ \) as the remaining term \(\mathrm {sin}(\gamma _K)\mathrm {sin}(\gamma _T)\) is zero for horizontal trajectories and / or horizontal flight. Since fly-by trajectories are planned smoothly, no large deviations of the course angles occur and hence, the inverse can be computed in these cases. Actually, the difference in course angles \(\varDelta \chi \) stays close to zero even in the presence of wind as the trajectory controller is designed to follow the geometric path rather than using a cascaded structure with course angle commands as commonly used in the literature [1,2,3,4,5,6,7,8,9,10]. However, since the determinant takes effect as an additional gain, the difference in course angles is limited to \(\left| \varDelta \chi \right| = 60^\circ \), which corresponds to a gain of 2 if \(\gamma _T = 0\) and \(\gamma _K=0\).

For the ideal case of \(\varDelta \chi = 0\), the inverse of the input matrix becomes diagonal

$$\begin{aligned} \mathbf {G}^{-1} = \begin{bmatrix} 1&0 \\ 0&\frac{1}{\mathrm {cos}(\gamma _K - \gamma _T)}, \end{bmatrix} \end{aligned}$$
(47)

which facilitates the possibility to evaluate the effects onto different axes, while the small terms due to the difference in course angles \(\varDelta \chi \) also disappear from the nonlinearity terms (43), (44), yielding

$$\begin{aligned} f_y= & {} - V_K^R\dot{\chi }_T\mathrm {cos}(\gamma _K) -\left( \dot{\chi }_T\dot{\gamma }_T\mathrm {cos}(\gamma _T)+\ddot{\chi }_T\mathrm {sin}(\gamma _T)\right) \varDelta z -\dot{\chi }\mathrm {sin}(\gamma _T)\varDelta \dot{z}, \end{aligned}$$
(48)
$$\begin{aligned} f_z= & {} V_K^R\dot{\gamma }_T\mathrm {cos}(\gamma _K - \gamma _T) +\left( \dot{\chi }_T\dot{\gamma }_T\mathrm {cos}(\gamma _T)+\ddot{\chi }_T\mathrm {sin}(\gamma _T)\right) \varDelta y + \mathrm {sin}(\gamma _T)\varDelta \dot{y} \qquad \qquad \\- & {} \varDelta f_{x,K}\mathrm {sin}(\gamma _K-\gamma _T), \qquad \qquad \nonumber \end{aligned}$$
(49)

where the first terms correspond to feedforward elements due to the curvature \(\dot{\chi }_T\) and \(\dot{\gamma }_T\), respectively, the specific force along the kinematic x-axis \(\varDelta f_{x,K}\) is incorporated by means of another feedforward element to deal with accelerations, and the remaining terms serve as elements to rotate the deviations into the changing trajectory frame. While the ideal case \(\varDelta \chi = 0\) renders Eqs. (47)–(49), the small terms for \(\varDelta \chi \ne 0\) correlate to additional coupling terms.

3.3.2 Pseudo Control Law

The pseudo controls are chosen as PD controller as per

$$\begin{aligned} \varvec{\nu }\triangleq \begin{pmatrix} \nu _y \\ \nu _z \end{pmatrix} = \begin{pmatrix} k_{p,y} \varDelta y +k_{d,y} \varDelta \dot{y}\\ k_{p,z} \varDelta z +k_{d,z} \varDelta \dot{z} \end{pmatrix}, \end{aligned}$$
(50)

where the gains \(k_i\in \mathbb {R}^-\) can be computed according to

$$\begin{aligned} \begin{aligned} k_{p,y}= & {} -\omega _{y}^2,\\ k_{d,y}= & {} -2\omega _y,\\ k_{p,z}= & {} -\omega _{z}^2,\\ k_{d,z}= & {} -2\omega _z,\\ \end{aligned} \end{aligned}$$
(51)

where \(\omega _{y}\) and \(\omega _{y}\) are the desired natural frequencies for an aperiodic second order system since no overshoots are desired for trajectory tracking.

4 Illustrative Examples

In this section, the functionality and the performance is further analyzed using illustrative examples. For this purpose, two simulation studies are presented. In the first simulation example, the nonlinear dynamic inversion controller is compared to a linear controller to evaluate the performance with respect to classical linear approaches. Then, the trajectory controller’s performance when subject to wind is discussed using another example. Both tests are conducted with the same NDI controller as reference at a commanded airspeed of 90kts.

For the comparison with the linear controller, all input and output treatments applied to the controller remain as designed for the NDI approach. That is, the implicit gain given by the inverse of the input matrix, \(\mathbf {G}^{-1}\), the command limits, and the command transformation to the inner loop are retained. Furthermore, the exact same PD control law (51) is used, where \(\omega _{y} = 0.23 \frac{rad}{s}\) and \(\omega _{z} = 0.45 \frac{rad}{s}\) are utilized in both cases. Consequently, the nonlinearity term given in Eq. (43) is the only remaining difference between the control laws.

For the comparison, a flightplan in triangular form, which consists of various fly-by maneuvers and a loiter at the end, is simulated. The flightplan and the actual flight paths are displayed in Fig. 3. Note that the planned trajectory is identical as the kinematic velocity used for radius computation does not vary throughout the simulation as no wind is applied to address the conceptual differences.

Fig. 3
figure 3

Flight plan: triangular pattern

Full size image
Fig. 4
figure 4

Linear controller

Full size image
Fig. 5
figure 5

NDI controller

Full size image

It is evident that the linear controller cannot follow the trajectory in turns and overshoots significantly both when entering and leaving the turn. This fact is further supported by Fig. 4, where large deviations are obvious. In comparison, the NDI controller as illustrated in Fig. 5 remains within deviations of 10 m when building up the roll angle required for turning into or leaving the clothoid, respectively. This deviation can be further reduced by a faster inner loop controller, which, however, is not in the focus right now as conservatism is of higher interest for the current phase of flight testing.

Furthermore, the linear controller exhibits significant steady-state deviations in turns. Although also the NDI controller cannot achieve steady-state accuracy during turns due to the lack of an integrator (in particular during the loiter at the end of the flightplan in Fig. 5), the nonlinearity term approximately succeeds in driving the deviation from the trajectory to zero by means of the feedforward term. Just using a linear error controller as made use of in various applications inherently results in large deviations with respect to curved trajectories as an error needs to be built up for the controller to react. The specific forces commanded by the trajectory controller are in the same magnitude for both controllers, where the NDI controller achieves the slightly faster reaction and the basic turn specific force command by the feedforward term while the linear controller generates its command amplitude by the control error.

Fig. 6
figure 6

Flight plan: spiral pattern, wind comparison

Full size image
Fig. 7
figure 7

NDI controller, no wind

Full size image
Fig. 8
figure 8

NDI controller, 20kts N-wind

Full size image

For the second example, a spiral pattern is considered, which also consists of various fly-by maneuvers and ends with a loiter. This pattern is chosen to solely contain turns with changing kinematic velocities as the trajectory generation module employs a combination of kinematic velocity, wind estimate, airspeed command, and the actual airspeed to compute the trajectory’s curve as noted in Sect. 2.1. Hence, the trajectory controller can be analyzed with respect to changing velocities, especially with respect to the planned curve. The flightplan displayed in Fig. 6 exhibits the different curves at south wind of 0, 20 and 40kts. As the wind streams towards north, a larger kinematic velocity occurs flying “up” whereas flying south employs the minimum kinematic velocity. As the trajectory generation module utilizes the maximum expected velocity throughout a turn, the planned radii become larger with increasing wind. In particular, this is evident for the loiter maneuver at the end of the flightplan.

Figures 7 and 8 show that there are almost no differences in deviations and course angles between no wind and 20kts of wind. The commands, however, change depending on the kinematic velocity, i.e. the commands required to follow the curve vary significantly to achieve the same objectives. Whereas the turn commands in Fig. 7 retain a certain amplitude, the command’s magnitudes change depending on the direction in the case of 20kts wind as displayed in Fig. 8. When performing the loiter at the end of the flightplan, the change is most evident.

As a conclusion, the trajectory controller achieves superior performance with respect of a similar linear PD controller and is robust against the influences of wind.

5 Conclusions

In this paper, a modular trajectory controller was proposed and the concept for fly-by trajectories was described in detail. Specifically, the environment including the interfaces was presented and a brief introduction to the inner loop was given. In simulations, the performance of the nonlinear controller was compared to a linear controller of the same order and the influence of significant wind was analyzed.

In addition to previously-published principal flight tests, further flight tests with the DA42 experimental aircraft of the institute are being conducted focusing on automatic landing by generation of smooth landing trajectories. The modular design of the overall control architecture enables fast applications to other aircraft. Specifically, flight tests with a motor glider and a fixed-wing aircraft of 6 tons are about to be conducted while a small UAV of 150 kg is being tested in hardware-in-the-loop environments.