Safe Motion Planning and Learning for Unmanned Aerial Systems

Abstract

To control unmanned aerial systems, we rarely have a perfect system model. Safe and aggressive planning is also challenging for nonlinear and under-actuated systems. Expert pilots, however, demonstrate maneuvers that are deemed at the edge of plane envelope. Inspired by biological systems, in this paper, we introduce a framework that leverages methods in the field of control theory and reinforcement learning to generate feasible, possibly aggressive, trajectories. For the control policies, Dynamic Movement Primitives (DMPs) imitate pilot-induced primitives, and DMPs are combined in parallel to generate trajectories to reach original or different goal points. The stability properties of DMPs and their overall systems are analyzed using contraction theory. For reinforcement learning, Policy Improvement with Path Integrals ($P{I}^2$) was used for the maneuvers. The results in this paper show that $P{I}^2$ updated policies are a feasible and parallel combination of different updated primitives transfer the learning in the contraction regions. Our proposed methodology can be used to imitate, reshape, and improve feasible, possibly aggressive, maneuvers. In addition, we can exploit trajectories generated by optimization methods, such as Model Predictive Control (MPC), and a library of maneuvers can be instantly generated. For application, 3-DOF (degrees of freedom) Helicopter and 2D-UAV (unmanned aerial vehicle) models are utilized to demonstrate the main results.

1. Introduction

Unmanned aerial systems have gained significant importance in the last few decades. Inherently, they are more agile and more suitable for working in dangerous areas in comparison with manned systems. Accordingly, optimization and learning techniques are employed in their applications to increase the number of UAV types and extend their operation range. In [1], centralized path planning based on reinforcement learning is implemented in a combat aerial vehicle fleet in order to avoid enemy defense systems. To localize radio frequency emitting targets in the operation area, multiple UAVs were deployed in [2] using Particle Filter and Extended Kalman Filter algorithms and vision-based detection. For the design optimization and mathematical modeling of unmanned aerial vehicles, a nonlinear lifting line method is proposed in [3]. This new modeling methodology is applied on the Aerospace Research Center (ARC) UAV in order to demonstrate that faster and cheaper prototyping is possible for future UAV models.

Despite enormous advancements in learning and optimization research, generating safe and aggressive movements is a rather hard problem for engineers to solve. Actuator saturation and nonlinear and nonholonomic dynamics are key issues. Most of the applications of UAVs, such as agriculture, intelligence, surveillance, and reconnaissance, only require trimmed flight. However, humans, as well as animals, are experts in generating seemingly hard but stable trajectories despite environmental and dynamic challenges. In acrobatic maneuvers, expert pilots demonstrate maneuvers that are deemed at the edge of the flight envelope.

To generate feasible maneuvers, on the other hand, pilot-executed trajectories are used in [4] to develop nonlinear dynamic models of helicopters. Using this model, intuitive control strategies derived from pilot behaviors are demonstrated in a hardware-in-the-loop simulation environment. Multiple expert executed flight demonstrations are also used in [5] to generate a desired trajectory. Additionally, a receding horizon differential dynamic programming controller is employed for a local model of the helicopter, which is learned from previous flight demonstrations. In [6,7], optimization methods are first employed and then performance is enhanced iteratively through experimentation. In recent years, nonlinear controller designs [8,9,10,11] have also been employed to generate safe maneuvers. For safe path planning [12,13,14,15], on the other hand, cost functions are utilized to minimize the flight envelope. Under these approaches, however, conservative maneuvers are generated in comparison with pilot-induced trajectories.

In general, choosing an optimization/learning methodology is only one aspect of the problem. In many scenarios, it is crucial to guarantee the safety of the system while learning desired behaviors. As a possible solution, Lyapunov-based methods [16,17,18] were proposed to solve reinforcement learning problems. However, there is no common rule for finding a suitable Lyapunov function and using Lyapunov functions; if any such function exists, it does not necessarily result in aggressive maneuvers.

For optimization problems, the stability of the receding horizon scheme can be guaranteed by adding a terminal cost function consisting of a Control Lyapunov Function (CLF) or by applying a sufficient prediction horizon without a terminal cost function, where a general cost function qualifies as Lyapunov function [19]. In [20], Nonlinear Model Predictive Control is applied to a nonholonomic mobile robot with a kinematic model for path following. The stability of the system is guaranteed by appropriately choosing a prediction horizon [21]. As shown in [20], the maneuvers were carefully selected to find the prediction horizon. Similar to reinforcement learning applications, finding a CLF in optimal control problems is not an easy task, and CLF eliminates other possible, may be more effective, solutions. Therefore, such methods lead to conservative solutions in order to guarantee stability.

For biological systems, on the other hand, one can naively guess that learning is achieved by following two sequential steps. First, the movement is imitated, and, next, the imitated behavior is improved through trial and error. It was observed in humans pilots that maneuvers performed with remote helicopters are often repeatable [4]. Each specific maneuver type is predictable in its own sequencing and duration, which suggests a primitive structure. In animals, experiments also show that kittens that have been exposed to adult cats retrieve food much faster in comparison to the control group [22]. Moreover, experiments on monkeys [23,24] also suggest that movements follow a virtual trajectory, which is independent of initial conditions and closely related to feedback control. This finding is also supported by other experiments conducted on frogs [25,26].

The most interesting discovery in the experiments concerning frogs was that the fields generated by activation on the spinal cord follow a principle vectorial summation. Simultaneous stimulation of two different areas of the spinal cord resembles the vectorial summation of the separate activation of each of these areas (See Figure 1) [27]. Generally, it is found that simultaneous stimulation fields and vectorial summations were similar in 87% of the experiments. As mentioned in [28], the vector summation of force fields suggests that nonlinear forms of interactions among neurons and between neurons and muscles is more or less eliminated. With few force fields stored in the spinal cord, one may represent motor primitives using the superposition of spinal fields:

$$D(q,\dot{q},\ddot{q})=u\sum _{i=1}^K{\alpha }_i{\rho }_i(q,\dot{q},\ddot{q}),$$

(1)

where D and u represent system dynamics and force, respectively. In this equation, force fields (${\rho }_i$) are weighted by scalar and positive coefficients, (${\alpha }_i$). In [28], it is also suggested that these spinal fields are selected using supraspinal signals by evaluating how much each field contributes to the overall field.

As a result, this study is focused on learning about and improving feasible trajectories inspired by biological systems. We introduce a framework that leverages methods in the field of control theory and reinforcement learning. We show that we are in the stable flight regime. In practice, inner-loop nonlinear controllers can mainly be used to avoid unmodeled dynamics and disturbances. For primitive structures, DMPs [29] are selected to imitate such primitives of motion. In DMPs, the kinematic trajectories (i.e., positions, velocities, and accelerations) of the motion are generated and converted to motor commands using feedforward controllers, probably by an inverse dynamic model and stabilized by feedback controllers. It is assumed that other non-linearities shall be handled by the controller at the motor execution phase. Detailed definitions of the DMPs can be found in Appendix A. In our study, movement primitives are also combined in parallel to generate feasible trajectories to reach original or different goal points.

Instead of constructing Lyapunov functions for stability, we applied contraction analysis [30] for DMPs, as well as for the reinforcement learning. In general, contraction is a form of stability where all trajectories with different initial conditions contract into a single trajectory. As mentioned in the discussion of previous experiments above, this form of stability is also implied in the structure of virtual trajectories. Additionally, we believe that contraction analysis should be studied for learning, as it was hypothesized in [31] that humans generate stable trajectories so that perturbations have less effect and require little correction; as a result, humans exploit contraction regions.

For reinforcement learning, $P{I}^2$[32] is used. In $P{I}^2$, trajectories are updated using stochastic Hamilton–Jacobi–Bellman (HJB) equations and direct policies are learned by approximating a path integral, in which the statistical inference problem is solved using sample roll-outs. Only one open parameter, exploration noise, is required in this learning method, and updates are not limited by numerical instabilities, since neither matrix inversions nor gradient learning rates are needed. As a result, it is shown that the performance of $P{I}^2$ is significantly better than that of gradient-based policy learning, and it can be applied in high-dimensional problems [32].

Although, there are successful studies using pilot-induced trajectories for UAVs, our goal in this research is to design a stable biologically inspired framework, which can be used to imitate and regenerate feasible, possibly aggressive, trajectories. The parallel combination of dynamic movement primitives coupled with reinforcement learning is firstly introduced for motion planning, and contraction analysis is used for the stability analysis. In general, the contribution of this paper is three-fold. First, while DMPs with contraction analysis [33] and $P{I}^2$ with DMPs [32] have been studied before, our proposed methodology in Section 2 encompasses all three methods to generate biologically inspired, feasible and possibly aggressive maneuvers. Second, DMPs are combined in parallel to guarantee stability. Third, it is shown that $P{I}^2$ updated policies are feasible and parallel combination of different updated primitives transfer the learning in contraction regions.

The remainder of the paper is structured as follows. In Section 2, we introduce the methodology and make several remarks for our study. First, DMP can be directly used in controller design, where additional feedback controllers as proposed in [34] are not required. Second, offline learning can be performed using feasible trajectories. Third, having library of motion primitives, one can combine primitives instantly in order to generate feasible trajectories in volatile environments. In Section 3 we demonstrate our results on 3-DOF helicopter and 2D UAV models.

2. Methodology

As mentioned in Section 1, our approach mainly encompasses the methods listed below:

• Contraction Theory.
• Dynamic Movement Primitives.
• $P{I}^2$ Reinforcement Learning Algorithm.

In our methodology, firstly, maneuvers generated by human operators or optimization programs such as MPC are imitated by DMPs. These primitives are then combined in parallel in such a way that the resultant primitive is also contracting. Subsequently, $P{I}^2$ is applied to improve the performance of the combined primitive. For the methodology, we will analyze the compatibility and applicability of these methods and theories below.

2.1. Contraction Analysis of Systems with DMPs

For a system defined below

$$\begin{array}{c}\hfill \dot{x}=f(x,t),\end{array}$$

(2)

the system is contracting [30], if the infinitesimal displacement at a fixed time:

$$\begin{array}{c}\hfill \delta \dot{x}=\frac{\partial f(x,t)}{\partial x}\delta x,\end{array}$$

(3)

decays over time. Therefore, the Jacobian $\frac{\partial f}{\partial x}$ should be a negative definite in that region.

Similar to linearizing the system at equilibrium points, we can use Jacobian to study the convergence of trajectories irrespective of initial conditions. This requirement, however, is a sufficient, but not a necessary, condition. One can define $\delta z=\mathrm{\Theta }\delta x$, where $\mathrm{\Theta }$ is a square matrix, and length is defined $\delta z^T\delta z=\delta x^{T}M\delta x$, where M is a metric. Hence, the negative definitiveness of a general Jacobian, $F=(\dot{\mathrm{\Theta }}+\mathrm{\Theta }\frac{\partial f}{\partial x}){\mathrm{\Theta }}^{-1}$, is a necessary and sufficient condition for the system to contract [30].

In [33], it is demonstrated that discrete DMPs contract in hierarchy. As DMPs are learned from feasible trajectories, our goal is to study the stability of the system. In other words, we aim to find out if the overall system will contract to the desired trajectories. For the analysis, the system differential equation can be defined in the following form:

$$\dot{x}=f(x)+B(x)u$$

(4)

Then, virtual displacement dynamics is defined as:

$$\delta \dot{x}(t)=\frac{\partial \left(f(x)+B(x)u\right)}{\partial x}\delta x+B(x)\delta u(t).$$

(5)

In a scalar case where $Bu=-f(x_{ref})+K_p(x_{ref}-x)$, the virtual displacement dynamics is reformulated as:

$$\delta \dot{x}(t)=\frac{\partial \left(f(x)-K_{p}x\right)}{\partial x}\delta x+\frac{\partial \left(K_p{x}_{ref}-f(x_{ref})\right)}{\partial x_{ref}}\delta x_{ref}.$$

(6)

If the reference system is contracting, its effect will be bounded and will decay [30] as $\delta x_{ref}\to 0$, and the system will contract in hierarchy with a proper choice of $K_p$, so that $\frac{\partial \left(f(x)-K_{p}x\right)}{\partial x}$ is uniformly negative. This is analogous to pole placement in linear control theory. Overall, if the system is designed to contract and a particular solution of the system is $x_{ref}$, the system will converge to the reference trajectory. In general, the stability of these derived feasible trajectories can be studied using several methods in contraction theory, as summarized in Appendix B.

For DMPs, let’s assume the standard manipulator equation [35]:

$$\begin{array}{c}\hfill \ddot{q}=H{(q)}^{-1}(C(q,\dot{q})\dot{q}+g(q)+u)\end{array}$$

(7)

$$\begin{array}{c}\hfill u=-g(q)+K_p(q_d-q)+K_d({\dot{q}}_d-\dot{q})\end{array}$$

(8)

$$\begin{array}{c}\hfill {\ddot{q}}_d=D(q_d,{\dot{q}}_d),\end{array}$$

(9)

where $H(q)$ is a positive definite inertia matrix, C represents the Coriolis and centripetal forces, g is the force of gravity, q is the joint angle, and u is forces/torques. D also stands for the DMP equations. Since DMP is contracting, its effect on Equation (7) will be bounded, and DMP will act as an exponential decaying disturbance to the original system. As a result, the system contracts to a particular solution with appropriate choices of K.

For the combination of primitives defined in Appendix B, let’s again take the standard manipulator equation with DMPs. If any two or more primitives are contracting, then the linear combination of the reference system in the system dynamics is also contracting such that:

$$\ddot{q}=H{(q)}^{-1}(C(q,\dot{q})\dot{q}-K_{p}q-K_d\dot{q}+\sum _{i=1}^n{\alpha }_i(K_p{q}_{d_i}+K_d{\dot{q}}_{d_i}),$$

(10)

where ${\sum }_{i=1}^n{\alpha }_i=1$. It is important to note that, if there is no singularity, any combination of these torques will be within the limits of the torque constraints.

We can combine primitives for the same or different goals as shown in Figure 2. In both approaches, the areas in between primitives are reachable. It is possible to combine primitives that generate trajectories that pass through these areas.

One can observe that the combination of system differential equations necessitates the combination of primitives. As a result, only the controlled section of differential equations is modified in Equation (10). Since DMP weights are designed linearly, a simple combination of weights is enough to achieve the desired results. In addition, primitives can also be combined serially. It was shown in [33] that obstacle avoidance maneuvers can be divided into primitives. For this type of combination, it is also possible to modify DMPs using their own parameters.

2.2. Using Reinforcement Learning with Contraction Analysis

In the last few decades, Reinforcement learning has attracted attention as a learning method for studying movement planning and control [36]. Reinforcement learning is a concept that is based on trial and error, typically the constant evaluation of performance in a surrounding environment is also required. In general, reinforcement learning requires an unambiguous representation of states and actions, as well as a scalar reward function.

Reinforcement learning is simple in its form, but it is mostly regarded as impractical due to the curse of dimensionality. Since value function approaches remain problematic in high dimensions, direct policy learning is utilized as an alternative method. Even for direct policy learning, however, numerical issues and the handling of different parameters still need to be addressed.

To compensate for such issues, a new methodology of probabilistic learning was derived on the basis of stochastic optimal control and path integrals. Policy Improvement with Path Integrals ($P{I}^2$) [32] connects between value function approximation using the stochastic HJB equations and direct policy learning by approximating a path integral, that is, by solving a statistical inference problem from sample roll-outs. The resulting algorithm is numerically robust, and only one parameter, exploration noise, is needed for its application.

Such methodology was first applied in stochastic optimal control. For optimal control, the cost functions are defined as:

$$\begin{array}{c}\hfill R({\tau }_i)={\omega }_{t_N}+{\int }_{t_i}^{t_N}{r}_{t}dt\end{array}$$

(11)

$$\begin{array}{c}\hfill r_t=r(x_t,u_t,t)=q_t+\frac{1}{2}{u}_t^{T}R{u}_t,\end{array}$$

(12)

where ${\omega }_{t_N}=\omega (x_{t_N})$ is a terminal cost at time $t_N$, and $r_t$ denotes the immediate cost at time t. Moreover, $q(t)$ and R represent the immediate cost at time t and the semi-definite weight matrix of control cost, respectively. For a general system,

$$\begin{array}{c}\hfill {\dot{\mathbf{x}}}_{\mathbf{t}}=\mathbf{f}({\mathbf{x}}_t,t)+\mathbf{G}({\mathbf{x}}_t)({\mathbf{u}}_t+{\epsilon }_t)={\mathbf{f}}_t+{\mathbf{G}}_t({\mathbf{u}}_t+{\epsilon }_t),\end{array}$$

(13)

we aimed to find a control input, ${\mathbf{u}}_{\mathbf{t}}$, which minimized the value function:

$$\begin{array}{c}\hfill V({\mathbf{x}}_{t_i})=V_{t_i}=\underset{{\mathbf{u}}_{t_i:t_N}}{min}{E}_{{\tau }_i}\left[R({\tau }_i)\right],\end{array}$$

(14)

where $E_{{\tau }_i}$ is (${\tau }_i$) for all trajectories. The stochastic HJB equation [37,38] for the optimal control problem is shown below:

$$\begin{array}{c}\hfill -{\partial }_t{V}_t=\underset{\mathbf{u}}{min}(r_t+{({\nabla }_x{V}_t)}^T+{\mathbf{F}}_t+\frac{1}{2}trace(({\nabla }_{xx}{V}_t){\mathbf{G}}_t{\Sigma }_{\epsilon }{\mathbf{G}}_t^T)),\end{array}$$

(15)

where ${\mathbf{F}}_t=\mathbf{f}({\mathbf{x}}_t,t)+\mathbf{G}({\mathbf{x}}_t){\mathbf{u}}_t$.

In [32], optimal control is studied extensively. Subsequently, their study was extended to reinforcement learning, in which a system model is not used. In their approach, the desired state is taken as an input, as action is viewed as any input to the control system. For such a control structure:

$$\begin{array}{c}\hfill \ddot{q}=H{(q)}^{-1}(-C(q,\dot{q})-v(q))+H{(q)}^{-1}u\end{array}$$

(16)

$$\begin{array}{c}\hfill u=K_p(q_d-q)+K_D({\dot{q}}_d-\dot{q})\end{array}$$

(17)

$$\begin{array}{c}\hfill {\ddot{q}}_d=G(q_d,{\dot{q}}_d)(\theta +{\epsilon }_{t_i}).\end{array}$$

(18)

Here, Equation (18) generates the desired trajectories. Since only the controlled differential equation is used in path integral formulation, only Equation (18) is needed to apply reinforcement learning. Uncontrolled sections of the system dynamics are not required.

Applying the $P{I}^2$ algorithm to the controlled section of DMPs, the equations of the algorithm [32] are formulated below:

$$\begin{array}{c}\hfill S({\tau }_{i,k})={\omega }_{t_N,k}+{\Sigma }_{j=i}^{N-1}{q}_{t_j,k}+\frac{1}{2}{\Sigma }_{j=i+1}^{N-1}{(\theta +{\mathbf{M}}_{t_j,k}{\epsilon }_{t_j,k})}^T\mathbf{R}(\theta +{\mathbf{M}}_{t_j,k}{\epsilon }_{t_j,k})\end{array}$$

(19)

$$\begin{array}{c}\hfill {\mathbf{M}}_{t_j,k}=\frac{{\mathbf{R}}^{-1}{g}_{t_j,k}{g}_{t_j,k}^T}{g_{t_j,k}^T{\mathbf{R}}^{-1}{g}_{t_j,k}}\end{array}$$

(20)

$$\begin{array}{c}\hfill P({\tau }_{i,k})=\frac{e^{-\frac{1}{\lambda }S{\tau }_{i,k}}}{{\Sigma }_{k=1}^K{e}^{-\frac{1}{\lambda }S{\tau }_{i,k}}}\end{array}$$

(21)

$$\begin{array}{c}\hfill \delta {\theta }_{t_i}={\Sigma }_{k=1}^K\left[P({\tau }_{i,k}){\mathbf{M}}_{t_i,k}{\epsilon }_{t_i,k}\right]\end{array}$$

(22)

$$\begin{array}{c}\hfill \delta {\theta }_j=\frac{{\Sigma }_{i=0}^{N-1}(N-i)w_{j,t_i}{\left[\delta {\theta }_{t_i}\right]}_j}{{\Sigma }_{i=0}^{N-1}(N-i)w_{j,t_i}}\end{array}$$

(23)

$$\begin{array}{c}\hfill {\theta }^{(new)}={\theta }^{(old)}+\delta \theta \end{array}$$

(24)

Under this setup, Equation (19) computes the cost function at each time step, i, for each trial, k, in the epoch. Equation (20) represents the projection matrix onto the $g_{j,k}$, i.e., the basis function from the system dynamics, under the metric $R^{-1}$. Equation (21) determined the probability of each trial. We can simply infer from $P({\tau }_{i,k})$ that lower cost trajectories have higher probabilities. Equation (22) determines the parameter update at each time step. It can easily be seen that trajectories with lower cost contribute more to the parameter update. Equation (23) averages the parameter update, $\delta {\theta }_{t_i}$, of each time step. Finally, Equation (24) updates the old parameter.

Using this formulation, we aim to understand the stability aspect of the system, and whether the system contracts from the initial conditions. It was already shown in [33] that discrete DMPs contract. Using hierarchical and other properties of contraction theory, our goal is to find contracting regions for the system setup. One may again find that the probability distribution of $P({\tau }_{i,k})$ serves as a tool for the parallel combination of inputs while updating the desired trajectories. If each roll-out is contracting, we can conclude that the updated trajectories will also be contracting. This is analogous to creating an envelope as shown in Figure 2. We can use controllers, such as the one defined in Equation (44), in order to generate contracting trajectories and apply them to reinforcement learning.

Moreover, each learned DMP is combined in order to generate a new primitive with a different goal point. Such combination is shown below:

$$\begin{array}{cc}\hfill {\dot{y}}_d=& {\alpha }_1(f(g_1,y_t,z_t)+g{\theta }_1)+\cdots +{\alpha }_n(f(g_n,y_t,z_t)+g{\theta }_n)\hfill \\ \hfill & =f(\sum _{i=1}^n{\alpha }_i{g}_i,y_t,z_t)+g\sum _{i=1}^n{\alpha }_i{\theta }_i,\hfill \end{array}$$

(25)

where ${\sum }_{i=1}^n{\alpha }_i=1$ and ${\alpha }_i>0$. Since the new primitive is contracting, we can further continue learning using $P{I}^2$ updates for this primitive. In the $P{I}^2$ learning algorithm (Equation (22)), we see that each parameter, ${\theta }_{t_i}$’s, for K roll-outs is updated as shown below:

$$\delta {\theta }_{t_i}={\Sigma }_{k=1}^K\left[P({\tau }_{i,k}){\mathbf{M}}_{t_i,k}{\epsilon }_{t_i,k}\right].$$

(26)

Here, probability of the negative cost function is used to weight only the open parameter, i.e., the exploration noise $\epsilon$. We assume that the system is contracting with feasible DMPs. Thus, the system always reaches a goal point at the end of the trajectory, and the cost function ${\omega }_{t_N,k}$ in $S({\tau }_{i,k})$ can safely beignored. We also assume that q’s will contract to the desired trajectory $q_d$’s. Therefore, in a contracting region, we define q’s as a combination of $\theta$ and $\epsilon$’s, which the only parameters to be used in the cost function $S({\tau }_{i,k})$ as shown below:

$$S({\tau }_{i,k})={\Sigma }_{j=i}^{N-1}{q}_{d_j,k}+\frac{1}{2}{\Sigma }_{j=i+1}^{N-1}{(\theta +{\mathbf{M}}_{t_j,k}{\epsilon }_{t_j,k})}^T\mathbf{R}(\theta +{\mathbf{M}}_{t_j,k}{\epsilon }_{t_j,k}).$$

(27)

At each update sequence in a contraction region, the primitive updates are weighted with $\theta$s. For the combination of primitives (Equation (25)), on the other hand, updates are weighted with $\alpha$s and $\theta$s, and the probability functions are taken as the average of the probability functions of separate primitives, if we assume that we use the same exploration noise, $ϵ$, for all primitives. Similarly, when we update primitives separately and combine them, we also average the probability functions. As a result, the learning curve of a combined primitive would be improved even further as the primitive learning process continues.

2.3. Remarks

Generally, contraction analysis yields simpler results which can be directly applied to systems/methodologies. In this section, we make a few application remarks, in which contraction analysis is used in combination with DMPs and possible learning methods.

2.3.1. DMP as a Controller

DMPs were originally used in a combination of feedback and feedforward controllers. However, it can be deduced from contraction analysis that one can directly use a DMP in a controller. Consider a system:

$$\dot{x}=f(x)+bu,$$

(28)

where we learn DMPs, and a controller, u, can be generated as $u=\frac{{\dot{x}}_d-f(x_d)}{b}$. In the closed loop system, particular solution of the system is $x=x_d$. Since DMPs are contracting, and the DMP parameters act as PD terms for the overall system, contraction to movement primitives is guaranteed. This controller can be regarded as a system with added virtual springs and dampers, and the path-to-goal point is modified by the basis functions and weights.

2.3.2. Offline Learning

A combination of primitives can be optimized offline without a need for the system model. For example, two primitives for the same goal point can be combined offline, where the weights, $\alpha s$, used for the combination are added to 1, such that ${\sum }_{i=1}^n{\alpha }_i=1$, where $\alpha >0$. As shown in Section 2.1, a combined primitive is contracting, given that the original primitives are contracting. Using this setup, one can optimize the combination of the weights in order to improve the performance of the resultant primitive. Subsequently, reinforcement learning can be applied to reshape the primitive again.

2.3.3. Maneuver Library

As defined in Appendix A, DMPs that we used are point attractors. In other words, the system converges to a goal state or equilibrium point. In real life, however, planning requires a change of maneuvers, and we require behaviors where transitions between primitives are needed. Using a controller that guarantees contraction, smooth transitions between primitives can be achieved. Therefore, a library of feasible trajectories must be stored, and we can exploit them in planning. We can create such library by learning feasible trajectories and generate more using the proposed methodology.

3. Application

3.1. The Combination of Primitives for a 3-DOF Helicopter

We used a simplified model derived from a Quanser Helicopter (see Figure 3) in our study. The helicopter is an under-actuated and minimum-phase system with two propellers at the end of its arm. Two DC motors are mounted below the propellers to create the forces which drive propellers. The motors’ axes are parallel and their thrust is vertical to the propellers. There are three degrees of freedom: pitch (the vertical movement of the propellers), roll (the circular movement around the axis of the propellers), and travel (the movement around the vertical base) in contrast with conventional helicopters, which have six degrees of freedom.

In the model shown below (see details in [40]), the origin of the coordinate system is at the bearing and slip-ring assembly. The combinations of actuators form the collective ($T_{col}=T_L+T_R$) and cyclic ($T_{cyc}=T_L-T_R$) forces, and they are used as inputs in the system. The pitch and roll motions are controlled by collective and cyclic thrust, respectively. The motion in the travel angle is controlled by the components of thrust. A positive roll results in a positive change of angle.

$$\begin{array}{ccc}\hfill \ddot{\alpha }& =& \frac{-Mg{l}_{\alpha }}{J_{yy}}sin(\alpha +{\alpha }_0)+\frac{L}{J_{yy}}cos(\varphi )T_{col}\hfill \end{array}$$

(29)

$$\begin{array}{ccc}\hfill \ddot{\gamma }& =& \frac{L}{J_{zz}}cos(\alpha )sin(\varphi )T_{col}-\frac{l_h}{J_{zz}}(T_L-T_R)sin(\alpha )sin(\varphi )\hfill \end{array}$$

(30)

$$\begin{array}{ccc}\hfill \ddot{\varphi }& =& \frac{l_h}{J_{xx}}{T}_{cyc}-\frac{mg{l}_{\varphi }}{J_{xx}}sin(\varphi )\hfill \end{array}$$

(31)

For such a system, feedback linearization (see details in [41]) is used. Control inputs are:

$$\begin{array}{ccc}\hfill T_{cyc}& =& \frac{V_{\varphi }-c_{3}sin(\varphi )}{c_2}\hfill \end{array}$$

(32)

$$\begin{array}{ccc}\hfill T_{col}& =& \frac{V_{\alpha }-c_{0}sin(\alpha +{\alpha }_0)}{c_{1}cos(\alpha )},\hfill \end{array}$$

(33)

where V is the equivalent input, which can be calculated such that $V_{\alpha }={\ddot{\alpha }}_d-2{\lambda }_{\alpha }(\dot{\alpha }-{\dot{\alpha }}_d)-{\lambda }_{\alpha }^2(\alpha -{\alpha }_d)$. For the system model described above, aggressive maneuvers mimicking human-operator-generated maneuvers are designed to elevate the pitch angle, $\alpha$, from $-5.7^{\circ }$ to $13.7^{\circ }$ and $20.3^{\circ }$ for a possible obstacle-avoidance problem. To reach a goal point between primitives, as shown in in Figure 2, primitives are combined in parallel using proper choices of $\alpha$. Such a maneuver is followed by a 3-DOF Helicopter (see Figure 4). Here, the reference pitch angle reaches $16.7^{\circ }$.

3.2. MPC Application of Contraction Theory for DMPs

In application, a simple kinematics model will be used to characterize the planar motion of a UAV (see Figure 5). Although there are no dynamics involved, the model is still available to demonstrate nonholonomic motion. Model equations are shown below:

$$\left[\begin{array}{c}\dot{x}\\ \dot{y}\\ \dot{\beta }\end{array}\right]=\left[\begin{array}{cc}\cos (\beta (t))& 0\\ \sin (\beta (t))& 0\\ 0& 1\end{array}\right]\left[\begin{array}{c}v(t)\\ w(t)\end{array}\right]$$

(34)

In this 2D model, x and y define the location of the UAV, and $\beta$ is the heading angle. The controller inputs v and w represent the air and angular speed of the UAV, respectively.

The problem can be reformulated as:

$$\left[\begin{array}{c}\dot{x}\\ \dot{y}\\ \dot{\beta }\end{array}\right]=g_1(q)v+g_2(q)w=\left[\begin{array}{c}cos(\beta (t))\\ sin(\beta (t))\\ 0\end{array}\right]v+\left[\begin{array}{c}0\\ 0\\ 1\end{array}\right]w.$$

(35)

The linearized system of the above kinematics cannot be controlled locally since the information of the nonlinear model is lost in system linearization. However, for the nonlinear system, the following result implies that the system is controllable:

$$rank\left[g_1{g}_2\left[g_1{g}_2\right]\right]=3,$$

(36)

where $\left[g_1{g}_2\right]$ is the Lie bracket of vectors $g_1$ and $g_2$.

For the model, the desired commands are:

$$\begin{array}{c}\hfill v_d(t)=\pm \sqrt{{\dot{x}}_d^2(t)+{\dot{y}}_d^2(t)}\end{array}$$

(37)

$$\begin{array}{c}\hfill w_d(t)=\frac{{\ddot{y}}_d(t){\dot{x}}_d(t)-{\ddot{x}}_d(t){\dot{y}}_d(t)}{{\dot{x}}_d^2(t)+{\dot{y}}_d^2(t)},\end{array}$$

(38)

where $w_d(t)$ is the differentiation of $\beta$ = $atan2(\dot{y},\dot{x})$. Using DMPs, the trajectories can be extracted for each state, and controllers can be generated thereafter.

In practice, we will derive feasible trajectories by combining contracting primitives (i.e., DMPs). Our system in Equation (34) can be written with desired inputs:

$$\begin{array}{c}\hfill \dot{x}=v_{d}cos(\beta )\end{array}$$

(39)

$$\begin{array}{c}\hfill \dot{y}=v_{d}sin(\beta )\end{array}$$

(40)

$$\begin{array}{c}\hfill \dot{\beta }=w_d.\end{array}$$

(41)

In this setup, $\beta$ is contracting as $w_d$ is contracting. Additionally, x and y are contracting, since the components of the system equations are contracting. Moreover, DMPs are extracted from feasible trajectories. Hence, we assume that the system is contracting in the close vicinity of the derived trajectories. These primitives can be combined such that:

$$\dot{x}={\alpha }_{1}B(x)u_{d_1}+\cdots +{\alpha }_{n}B(x)u_{d_n}=B(x)(\sum _{i=1}^n{\alpha }_i{u}_{d_i}),$$

(42)

where ${\sum }_{i=1}^n{\alpha }_i=1$ and ${\alpha }_i>0$. We can infer from Equations (37) and (38) that any parallel-combined primitive will be within the controller limits of the system. From construction of DMP formulas, we can find that the equilibrium point will be ${\sum }_{i=1}^n{\alpha }_i{g}_i$ for DMPs.

For such a system, it is also possible to use feedback linearization. For input–output linearization [42], we define an output vector, $\eta =(x,y)$, and differentiate the output vector two times in order to find input, thus:

$$\ddot{\eta }=\dot{\xi }\left[\begin{array}{cc}cos(\beta )& -\xi sin(\beta )\\ sin(\beta )& \xi cos(\beta )\end{array}\right]\left[\begin{array}{c}a\\ w\end{array}\right],$$

(43)

where an integrator with a state $\xi =v$ is added. Defining the system as:

$$\left[\begin{array}{c}a\\ w\end{array}\right]={\left[\begin{array}{cc}cos(\beta )& -\xi sin(\beta )\\ sin(\beta )& \xi cos(\beta )\end{array}\right]}^{-1}\left[\begin{array}{c}{u}_1\\ u_2\end{array}\right],$$

(44)

one should note that the resulting controller has a singularity point where $\xi =v=0$.

$$\begin{array}{c}\hfill u_1={\ddot{x}}_d(t)+k_{p1}(x_d(t)-x)+k_{d1}({\dot{x}}_d(t)-\dot{x})\end{array}$$

(45)

$$\begin{array}{c}\hfill u_2={\ddot{y}}_d(t)+k_{p2}(y_d(t)-y)+k_{d2}({\dot{y}}_d(t)-\dot{y})\end{array}$$

(46)

Through the construction of the DMP formulas, the system will converge to a weighted desired location, ${\sum }_{i=1}^n{\alpha }_i{g}_i$, if the desired end points, $(g_{i}s)$, are different.

In practice, the flight maneuver defined for $(x,y,\beta )$ starts from the origin, $(0,0,0)$, and ends at specified goal point $(1.5,1.5,0)$. Firstly, the MPC application [43] using the CasADi [44] optimization tool is used to generate trajectories (Figure 6).

Then, the DMP is used to imitate each primitive (see Figure 7).

For parallel combinations, two primitives, as shown in Figure 8, with goal points $(1,1,0)$ and $(1.5,1.5,0)$ are combined using a weighting parameter, ${\alpha }_i$, to generate a primitive with a goal point $(1.25,1.25,0)$.

Finally, MPC is once again used to follow the combined feasible trajectory (See Figure 9).

3.3. The Application of Contraction Theory in Reinforcement Learning ($P{I}^2$)

For application, the $P{I}^2$ reinforcement learning software [45] is used for a system with a kinematics model defined in Equation (34). For an initial primitive of a flight maneuver with an end point $(1.5,1.5,0)$, the DMP imitated the trajectories generated by the MPC algorithm [43]. A total of 100 updates and 10 basis functions/weights was applied. For each update, 10 trials were used. We applied importance sampling in which the five best trials from the previous update were rerun in the next update. The cost function is:

$$\begin{array}{c}\hfill J=\frac{1}{2}{\dot{\mathbf{x}}}^{\mathbf{T}}\dot{\mathbf{x}}Q+\frac{1}{2}{\mathbf{w}}^{\mathbf{T}}\mathbf{w}R+r_T,\end{array}$$

(47)

where $\mathbf{x}$ and $\mathbf{w}$ are the velocity state vector and weights, respectively. The constants $Q=1000$, and $R=1$ are used for penalization. The terminal cost is shown as $r_T$. The resulting trajectory for x and y is shown in Figure 10.

We can also imitate and learn a primitive for a goal point, $(1,1)$, using same process. Learning curves for both primitives are shown in Figure 11. After training for both primitives, the primitives were then combined in parallel as defined in Equation (25) in order to generate a stable trajectory, which has a goal point $(1.25,1.25)$. We observe that learning is transferred in the combined primitive (see Figure 12).

4. Conclusions

Our aim in this paper was to propose a framework that combined several methods to generate maneuvers inspired by biological experiments. In our methodology, stability is addressed with contraction theory, and reinforcement learning is used to improve maneuvers in contraction regions. Our main results are demonstrated on 3-DOF-helicopter and 2D-UAV models. We believe that learning in contraction regions is a key aspect for achieving stable trajectories. Our approach exploits such contracting regions using a combination of primitives. It is shown that trajectories updated by reinforcement learning are feasible, and learning is transferred in contraction regions. It is also possible to employ several other methods, as discussed in our paper, to study the contraction regions around trajectories.

As a part of our future research, we aim to apply this methodology to actual flight platforms. We will focus particularly on scenario-based problems, such as obstacle avoidance, in which serial and parallel combinations of different primitives will be deployed using a maneuver library.