A Fast and Close-to-Optimal Receding Horizon Control for Trajectory Generation in Dynamic Environments

Abstract

This paper presents a new approach for the optimal trajectory planning of nonlinear systems in a dynamic environment. Given the start and end goals with an objective function, the problem is to find an optimal trajectory from start to end that minimizes the objective while taking into account the changes in the environment. One of the main challenges here is that the optimal control sequence needs to be computed in a limited amount of time and needs to be adapted on-the-fly. The control method presented in this work has two stages: the first-order gradient algorithm is used at the beginning to compute an initial guess of the control sequence that satisfies the constraints but is not yet optimal; then, sequential action control is used to optimize only the portion of the control sequence that will be applied on the system in the next iteration. This helps to reduce the computational effort while still being optimal with regard to the objective; thus, the proposed approach is more applicable for online computation as well as dealing with dynamic environments. We also show that under mild conditions, the proposed controller is asymptotically stable. Different simulated results demonstrate the capability of the controller in terms of solving various tracking problems for different systems under the existence of dynamic obstacles. The proposed method is also compared to the related indirect optimal control approach and sequential action control in terms of cost and computation time to evaluate the improvement of the proposed method.

1. Introduction

Trajectory planning in robotics and automation has attracted a great deal of attention recently due to the new demands in this area. Besides reaching the goal, it is crucial that the controlled robot is able to react to highly dynamic environments, e.g., avoiding vehicles and pedestrians in the case of autonomously driving cars, or avoiding human co-workers to provide safety in the case of humans and robots working in the same workspace. This requires the robot to adapt rapidly to changing situations. Furthermore, it is desired that, besides safety, also other demands are considered, such as optimizing energy and human comfort, etc. Solving trajectory planning problems while taking all of these aspects into consideration is not a trivial task.

In general, the trajectory planning problem is either solved on a kinematic or dynamic level. On the kinematic level, the outcome of trajectory planning is a set of waypoints; each consists of a time stamp and the position/velocity/acceleration of the system. Then, a controller, i.e., a PID controller, is used to generate a control signal applied to the system. In this area, different planning techniques were first presented in automated vehicle demonstrations  [1,2,3]. One of the first techniques was the use of interpolating curve planners, i.e., with the use of clothoid paths in the Eureka Prometheus Project  [4] between 1987 and 1994, where the transitions between linear parts and curves are achieved with a linear change in curvature. However, this method is very time-consuming and results in a continuous but not smooth path. Later, motion planners based on splines emerged, as in the ARGO Project [5], which have a low computational cost but are not optimal with regard to curvature minimization. Then, graph-search-based planners such as D* were used, as in the Darpa PerceptOR program [6,7], where a drawback is that the resulting path is not continuous. At around the same time, sample-based methods such as rapidly exploring random tree (RRT) [8] were introduced and were commonly used later on as motion planners in a large variety of applications, from autonomous vehicles [9,10] to articulated robots [11,12] and multi-agent systems [13]. In [14], the author introduces a path planning framework using a sample-based approach for articulated robots in the presence of moving obstacles. In recent research, different learning methods have been used in combination with sample-based approaches to improve the efficiency of searching the free-collision path [15,16]. Besides sampling-based methods, optimization-based approaches have also been developed to generate smooth trajectories by minimizing cost functions with regard to velocity/acceleration/jerk terms, as presented in [17]. Nevertheless, these algorithms all compute paths that can be tracked by a controller but not a control force/torque that can be applied on the robot; the constraints on the physical behaviors of the robot cannot be considered in these planning methods.

Differing from sampling-based motion planners, optimal control approaches take the physical behaviors of the robot into account and compute a control input with regard to a pre-defined cost function, which results in an optimal trajectory. The cost functions can be utilized to describe different demands, such as human comfort, optimizing energy, etc.; thus, optimal control methods are very suitable for the aforementioned trajectory planning problem. Most of the optimal control methods are categorized into indirect  [18] and direct [19] methods. Indirect methods look at the necessary conditions of optimality of the infinite OCP to derive a boundary value problem (BVP) in ordinary differential equations (ODE). In contrast, direct methods transform the original infinite OCP into a finite nonlinear programming problem and then solve it. However, due to extensive computational effort, these methods are mainly used to compute the trajectories in the offline case and therefore are unable to react to dynamic environments  [20]. To overcome this problem, different modifications have emerged in which some pre-computations are performed offline and then used to reduce the computation time in the online phase. For example, in  [21], a finite number of global optimal solutions is computed offline; then, they are generalized using support vector machines or Gaussian process regression and used as training data in the online phase. Similarly, machine learning and motion primitives are used in  [22,23] to learn precomputed optimal trajectories. However, in real dynamic environments, an infinite number of cases can occur; thus, it is difficult to cover all possibilities with a finite number of precomputed solutions. A solution to deal with dynamic environments is the use of Dynamic Motion Primitives (DMP)  [24], where the parameters of the DMP can be adapted online and therefore react to the dynamic environment. However, in this method, the optimality is lost due to the deformation process and therefore the trajectory is not optimal.

Recently, NMPC [25,26] has received a great deal of attention and has been applied in several applications  [27,28,29,30,31]. One of the main reasons is the development of different numerical toolboxes and computers with powerful CPUs that are able to solve optimal control problems efficiently. NMPC is a feedback optimal control framework, which basically solves an optimal control problem over a finite receding horizon. Then, only the first interval of the computed control signal is applied until new state measurements are available. After this, the horizon is shifted ahead for one interval and the procedure repeats. The major advantages of NMPC are its fast computation time compared to the original optimal control approaches in  [18,19] and its capability of considering dynamic environments. Thus, NMPC has attracted a lot of attention during the last decade. Considerable progress has been made in term of algorithms and software implementations that are able to reduce the computational time of NMPC significantly. In  [32], the authors used MPC with a simplified model to find an optimal reaction force profile for a dynamic legged locomotion system and used a simple PID controller to compute the joint torque, position, and velocity commands based on the reaction forces computed from MPC. In  [33,34], the authors proposed a scheme with a limited number of iterations to obtain an approximate solution. This reduces the computation time but the the solution is only suboptimal. For obstacle avoidance in dynamic environments, the authors in  [35] developed a robust MPC approach to deal with moving obstacles, but the model is only on a kinematic level, while in  [36,37,38] the authors utilized a simple model of the system to reduce the computational effort. In terms of implementation, the ACADO toolkit [39] is a very strong numerical NMPC solver that can handle a wide range of applications and problems and has been used in different research [40,41]. However, even with the strength of powerful computers, the computation time of NMPC is still significant, especially for nonlinear and complex models, such as an articulated robot or a car-like system. Therefore, most of the works only are only successful with simple or slow systems in a static environment.

SAC was introduced in  [42] as a model-based algorithm that is able to compute a sub-optimal control signal for nonlinear systems. It uses the same concept of receding horizon but the main difference between SAC and NMPC, which is also the selling point of SAC, is that it derives a closed-form expression of only one interval of the control signal, which will be applied to the system for a short duration (while NMPC still computes the control signal over the whole horizon). This short duration is usually chosen as equal to a single step size of the control signal. Hence, unlike NMPC, which requires numerical solvers, SAC can derive an analytical expression of this interval. This means that SAC performs faster than NMPC and other optimal control approaches in general, which makes it a promising candidate for online applications. However, this promising analytical solution is only obtainable if there are no constraints in the control problem, the reason being that SAC uses a different method called mode insert gradient  [43], which measures the first-order sensitivity of the cost function when the control signal is applied for a short duration. The author then smartly selects an auxiliary cost function, which utilizes this sensitivity formulation such that the analytical solution can be obtained. However, this procedure by default neglects the possibility of adding constraints into the problem. This limits SAC from applications that require additional constraints, i.e., target or final constraints, which are necessary for trajectory planning tasks. Even though these constraints can be formulated as part of the cost function, there is no guarantee that they can be fulfilled.

Inspired by SAC, the proposed method, TC-SAC, was first introduced in  [44], and is able to compute an optimal solution very quickly and, in addition, can handle target constraints for trajectory planning tasks. TC-SAC consists of two steps: at first, a first-order gradient approach is used to tackle target constraints for the planning problem and generate an initial guess; after this, SAC is used to improve it further with regard to the cost function. This makes it a promising candidate for real-time optimal control in a dynamic environment. In this work, we extend the method in  [44] and evaluate TC-SAC in a broader range of applications. The contribution of this work is as follows:

• We extend the TC-SAC method to cover the cases where target constraints might be violated;
• Different comparisons between TC-SAC, SAC, and indirect optimal control methods are given to show the improvement of the proposed method;
• We show that TC-SAC is able to deal with dynamic environments, which involves avoiding obstacles in our case, and can be applied in different systems without lots of modifications;
• The stability proof of the proposed method is given and discussed.

The remainder of this work is structured as follows. In Section 2, the design of the proposed method is outlined: first, the trajectory tracking problem is formulated in Section 2.1, and then, in Section 2.2, we introduce the basic concept of TC-SAC, as well as the detailed mathematical formulation of the algorithm. Then, the results of the simulation with TC-SAC are given in Section 3. We discuss the stability proof of TC-SAC in Section 4 and conclude our work with the discussion and outlook in Section 5.

2. Materials and Methods

2.1. Problem Formulation

In general, the trajectory tracking task can be formulated as an OCP in the receding horizon, where the terminal constraint is used for the trajectory generation task. For the OCP, a dynamic system with n states and m control inputs is considered and described by a set of ordinary differential equations

$$\dot{\mathbf{x}}\left(t\right)=\mathbf{f}(t,\mathbf{x}\left(t\right),\mathbf{u}\left(t\right)),\mathbf{x}\left(t_0\right)={\mathbf{x}}_0$$

(1)

with $\mathbf{f}:{\mathbb{R}}^{n+m+1}\to {\mathbb{R}}^n$ being nonlinear in state $\mathbf{x}\in {\mathbb{R}}^n$ and control input $\mathbf{u}\in {\mathbb{R}}^m$. The initial state of the system is denoted by ${\mathbf{x}}_0$ and the target configuration ${\mathbf{x}}_{\mathrm{d}}\left(t\right)$ denotes the final goal. The cost functional

$$J_1=m_{\mathrm{f}}\left(\mathbf{x}\left(t_{\mathrm{f}}\right)\right)+{\int }_{t_0}^{t_{\mathrm{f}}}{l}_1(t,\mathbf{x}\left(t\right),\mathbf{u}\left(t\right))dt$$

(2)

with the performance cost $l_1(·)$ and the terminal cost $m_{\mathrm{f}}(·)$ are used to measure the performance in the horizon $[t_0,t_{\mathrm{f}}]$. The optimal control problem over a receding prediction horizon $T_{\mathrm{P}}$ is given by

$$\begin{array}{ccc}\hfill & & \underset{\mathbf{u}}{min}{J}_1=m_{\mathrm{f}}\left(\mathbf{x}\left(t_{\mathrm{f}}\right)\right)+{\int }_{t_0}^{t_{\mathrm{f}}}{l}_{1}dt\hfill \end{array}$$

(3a)

$$\begin{array}{ccc}\hfill \mathrm{s}.\mathrm{t}.& & \dot{\mathbf{x}}=\mathbf{f}(t,\mathbf{x}\left(t\right),\mathbf{u}\left(t\right)),\mathbf{x}\left(t_0\right)={\mathbf{x}}_0,\mathbf{x}\in {\mathbb{R}}^n\hfill \end{array}$$

(3b)

$$\begin{array}{ccc}\hfill & & \Phi =\left(\begin{array}{c}{x}_1\left(t_{\mathrm{f}}\right)-x_{\mathrm{d},1}\left(t_{\mathrm{f}}\right)\\ ⋮\\ x_q\left(t_{\mathrm{f}}\right)-x_{\mathrm{d},q}\left(t_{\mathrm{f}}\right)\end{array}\right)=0,q\le n,\hfill \end{array}$$

(3c)

where $t_0=t_{\mathrm{cur}}$ denotes the current time, $t_{\mathrm{f}}=t_{\mathrm{cur}}+T_{\mathrm{P}}$ is the final time at the end of the prediction horizon, and $(x_{\mathrm{d},1}\left(t_{\mathrm{f}}\right),\dots ,x_{\mathrm{d},q}\left(t_{\mathrm{f}}\right))$ defines q constrained state at the end of the prediction horizon $t_{\mathrm{f}}$. $\Phi$ is a column vector that represents the set of q terminal constraints at $t_{\mathrm{f}}$.

In this paper, the dynamic system in (1) is assumed to be given in control-affine form, i.e.,

$$\dot{\mathbf{x}}=\mathbf{f}\left(t,\mathbf{x}\left(t\right),\mathbf{u}\left(t\right)\right)=\mathbf{g}\left(t,\mathbf{x}\left(t\right)\right)+\mathbf{h}\left(t,\mathbf{x}\left(t\right)\right)\mathbf{u}\left(t\right)$$

(4)

with $\mathbf{f}$ being nonlinear with respect to state $\mathbf{x}$ and linear in control input $\mathbf{u}$.

2.2. Target-Constrained Sequential Action Control

This section outlines the proposed approach, called Target-Constrained Sequential Action Control (TC-SAC)  [44], for trajectory planning tasks. The idea of TC-SAC is to utilize the advantage of SAC in terms of fast computation time and to extend the original method with an additional controller to tackle constraints. The overall structure of TC-SAC is given in Figure 1.

The proposed method consists of two parts:

• A nominal controller based on the first-order gradient algorithm (FOGA)  [45];
• An optimal controller based on Sequential Action Control (SAC).

In TC-SAC, an initial guess is first computed by an indirect optimal control method, which is FOGA in our case, in order to consider terminal constraints. Due to time limitations, FOGA is only run for one iteration to obtain the nominal control ${\mathbf{u}}_1$. Obviously, with one iteration, ${\mathbf{u}}_1$ is not yet close to the optimal solution. The first interval of ${\mathbf{u}}_1$ is then improved by SAC since it will be applied on the system in the next iteration. As mentioned, SAC utilizes the concept of mode insert gradient  [43] and a proper selection of an additional auxiliary cost function to derive an analytical solution for the optimal control ${\mathbf{u}}_2^{\ast }$, which improves the performance over ${\mathbf{u}}_1$. Note that ${\mathbf{u}}_2^{\ast }$ differs from ${\mathbf{u}}_1$ only by the first portion, while the rest remains the same. Then, in the Selection step (see Figure 1), ${\mathbf{u}}_2^{\ast }$ is compared to ${\mathbf{u}}_1$ in terms of performance and terminal constraint costs. If the comparison shows the improvement of ${\mathbf{u}}_2^{\ast }$, then it is applied to the system for the next iteration.

Looking in depth into the difference between TC-SAC and SAC, our method has a better choice of the nominal controller ${\mathbf{u}}_1$. To be precise, the original SAC only computes one interval of the control signal ${\mathbf{u}}_2^{\ast }$ per iteration while assuming ${\mathbf{u}}_1\equiv 0$. In the case of TC-SAC, instead, ${\mathbf{u}}_1$ is updated every iteration. Therefore, TC-SAC always improves the optimality of the control signal over time, while SAC does not. The update of ${\mathbf{u}}_1$ also plays a crucial role for TC-SAC to incorporate target constraints for trajectory tracking tasks and is part of the stability proof that will be discussed later in Section 4. Clearly, this update requires more computational effort than SAC, but it is negligible compared to the benefits that it provides. Next, both parts of our approach will be outlined and explained in detail.

2.2.1. First-Order Gradient Algorithm (FOGA)

The first part of the controller serves the purpose of incorporating constraints, i.e., target constraints, into the OCP, which are crucial for trajectory generation tasks. Since the theoretical background of SAC uses the co-state equation from the Pontryagin principle [46] as part of the calculation (see Section 2.2.2 for more details), FOGA is selected as the solver here to utilize this equation to reduce the amount of steps needed for implementation. FOGA then solves problem (3) for one iteration to find an initial guess. Since this is an optimal control problem with equality constraints at the final state, the idea from [45] is used. First, the dynamic constraint is adjoined to the performance equation $l_1$ by introducing time-varying Lagrange multiplier vector $\rho$, whose elements are called the co-states of the system. This constructs the Hamiltonian $\mathbb{H}\in {\mathbb{R}}^1$ defined for all $t\in [t_0,t_{\mathrm{f}}]$:

$$\mathbb{H}(\mathbf{x},\mathbf{u},\rho ,t)={\rho }^{\mathsf{T}}\mathbf{f}(\mathbf{x},\mathbf{u})+l_1(\mathbf{x},\mathbf{u})$$

(5)

Following Pontryagin’s maximum principle [46], the co-state equation

$$\dot{\rho }=-{\left(\frac{\partial \mathbb{H}}{\partial \mathbf{x}}\right)}^{\mathsf{T}}=-{\left(\frac{\partial l_1}{\partial \mathbf{x}}\right)}^{\mathsf{T}}-{\left(\frac{\partial {\mathbf{f}}_1}{\partial \mathbf{x}}\right)}^{\mathsf{T}}\rho$$

(6)

must be satisfied. Solving (6) requires a terminal condition, which is usually chosen as $\rho \left(t_{\mathrm{f}}\right)={\left(\frac{\partial m_{\mathrm{f}}}{\partial \mathbf{x}}\left(t_{\mathrm{f}}\right)\right)}^{\mathsf{T}}$ if the state $\mathbf{x}$ is not fixed at $t_{\mathrm{f}}$. In our case, this terminal condition is slightly modified to consider the terminal constraints in (3c):

$${\rho }_i\left(t_{\mathrm{f}}\right)=\left\{\begin{array}{cc}0,\hfill & i=1,\cdots ,q,\hfill \\ {\left(\frac{\partial m_{\mathrm{f}}}{\partial {\mathbf{x}}_i}\right)}_{t=t_{\mathrm{f}}}^{\mathsf{T}},\hfill & i=q+1,\cdots ,n,\hfill \end{array}\right.$$

(7)

with n and q as defined above. Next, the matrix of influence functions $\mathbf{R}\in {\mathbb{R}}^{n\times q}$ is introduced

$$\dot{\mathbf{R}}={\left(\frac{\partial \mathbf{f}}{\partial \mathbf{x}}\right)}^{\mathsf{T}}\mathbf{R},$$

(8)

where

$${\mathbf{R}}_{ij}\left(t_{\mathrm{f}}\right)=\left\{\begin{array}{ccc}1,& i=j,& i=1,\cdots ,n,\\ 0,& i\ne j,& j=1,\cdots ,q.\end{array}\right.$$

(9)

By defining the matrix $\mathbf{R}$, we are able to predict how changes in the control input, $\delta \mathbf{u}\left(t\right)$, affect the cost function $J_1$ and the q terminal constraints in $\Phi$ by the following; see [45] for more details.

$$\delta J_1={\int }_{t_0}^{t_{\mathrm{f}}}\left({\rho }^{\mathsf{T}}\frac{\partial \mathbf{f}}{\partial \mathbf{u}}+\frac{\partial l_1}{\partial \mathbf{u}}\right)\delta \mathbf{u}\left(t\right)dt$$

(10)

$$\delta \Phi \triangleq {\left[\begin{array}{c}\delta x_1\\ ⋮\\ \delta x_q\end{array}\right]}_{t=t_{\mathrm{f}}}={\int }_{t_0}^{t_{\mathrm{f}}}{\mathbf{R}}^{\mathsf{T}}\frac{\partial \mathbf{f}}{\partial \mathbf{u}}\delta \mathbf{u}\left(t\right)dt$$

(11)

Now, we want to minimize (10) s.t. constraints (11). However, both have linearized relations with regard to $\delta \mathbf{u}$, so there is no minimum for $\delta J_1$. A simple method to create a minimum is to add a quadratic integral penalty function in $\delta \mathbf{u}$ to (10)

$$\delta J_{\mathrm{ex}}=\delta J_1+\frac{1}{2}{\int }_{t_0}^{t_{\mathrm{f}}}{\left(\delta \mathbf{u}\right)}^{\mathsf{T}}\mathbf{W}\delta \mathbf{u}dt,$$

(12)

where $\mathbf{W}\left(t\right)\in {\mathbb{R}}^{m\times m}$ is an arbitrary positive-definite weighting matrix. The problem then becomes a minimization problem of $\delta J_{\mathrm{ex}}$ subject to (11). Adjoining (11) to (12) with another constant Lagrange multiplier $\nu$, one obtains

$$\delta \overline{J}=\delta J_{\mathrm{ex}}+{\nu }^{\mathsf{T}}\left[{\int }_{t_0}^{t_{\mathrm{f}}}{\mathbf{R}}^{\mathsf{T}}\frac{\partial \mathbf{f}}{\partial \mathbf{u}}\delta \mathbf{u}\left(t\right)dt-\delta \Phi \right].$$

(13)

If we neglect the change in coefficients, the first derivative of (13) is given by

$$\delta \left(\delta \overline{J}\right)={\int }_{t_0}^{t_{\mathrm{f}}}\left[\frac{\partial l_1}{\partial \mathbf{u}}+{(\rho +\mathbf{R}\nu )}^{\mathsf{T}}\frac{\partial \mathbf{f}}{\partial \mathbf{u}}+{\left(\delta \mathbf{u}\right)}^{\mathsf{T}}\mathbf{W}\right]\delta \left(\delta \mathbf{u}\right)dt$$

(14)

Setting (14) to be zero, one can find a solution of

$$\delta \mathbf{u}=-{\mathbf{W}}^{-1}{\left[\frac{\partial l_1}{\partial \mathbf{u}}+{(\rho +\mathbf{R}\nu )}^{\mathsf{T}}\frac{\partial \mathbf{f}}{\partial \mathbf{u}}\right]}^{\mathsf{T}}$$

(15)

that minimizes (13). Substituting this into (11), we find that

$$\delta \Phi =-{\mathbf{I}}_{\Phi \mathbf{J}}-{\mathbf{I}}_{\Phi \Phi }\nu$$

(16)

where ${\mathbf{I}}_{\Phi \Phi }\in {\mathbb{R}}^{n\times n}$ and ${\mathbf{I}}_{\Phi \mathbf{J}}\in {\mathbb{R}}^{1\times n}$ are computed by

$${\mathbf{I}}_{\Phi \Phi }={\int }_{t_0}^{t_{\mathrm{f}}}{\mathbf{R}}^T\frac{\partial \mathbf{f}}{\partial \mathbf{u}}{\mathbf{W}}^{-1}{\left(\frac{\partial \mathbf{f}}{\partial \mathbf{u}}\right)}^{\mathsf{T}}\mathbf{R}dt$$

(17)

and

$${\mathbf{I}}_{\mathbf{J}\Phi }={\mathbf{I}}_{\Phi \mathbf{J}}^{\mathsf{T}}={\int }_{t_0}^{t_{\mathrm{f}}}\left(\rho \frac{\partial \mathbf{f}}{\partial \mathbf{u}}+\frac{\partial l_1}{\partial \mathbf{u}}\right){\mathbf{W}}^{-1}{\left(\frac{\partial \mathbf{f}}{\partial \mathbf{u}}\right)}^{\mathsf{T}}\mathbf{R}dt$$

(18)

Assuming that ${\mathbf{I}}_{\Phi \Phi }$ is non-singular, we can solve (16) for the value of $\nu$

$$\nu =-{\mathbf{I}}_{\Phi \Phi }^{-1}(\delta \Phi +{\mathbf{I}}_{\Phi \mathbf{J}})$$

(19)

with $\delta \Phi =-ϵ\Phi \left[\mathbf{x}\left(t_{\mathrm{f}}\right)\right]$ and a constant $ϵ\in (0,1]$. Note that the existence of the inverse of ${\mathbf{I}}_{\Phi \Phi }$ is the controllability condition. If ${\mathbf{I}}_{\Phi \Phi }^{-1}$ does not exist, it is not possible to control the system with $\mathbf{u}\left(t\right)$ to satisfy one or more of the terminal conditions (see Appendix B in  [45] for more details). Finally, the new control ${\mathbf{u}}_1$ is updated to

$${\mathbf{u}}_1={\mathbf{u}}_{1,\mathrm{old}}+\delta \mathbf{u}$$

(20)

with the old control input ${\mathbf{u}}_{1,\mathrm{old}}$ and the update $\delta \mathbf{u}$ from (15).

In summary, the procedure of FOGA follows these steps:

• Initialize a set of control input $\mathbf{u}\left(t\right)$;
• Forward integrate (4) with the initial conditions $\mathbf{x}\left(t_0\right)$ and the initial guess of the control input from step 1;
• Determine the co-state vector $\rho$ and the matrix of influence functions $\mathbf{R}$ by backward integration through (6) and (8) with the terminal conditions (7) and (9);
• Calculate ${\mathbf{I}}_{\Phi \Phi }$, ${\mathbf{I}}_{\Phi \mathbf{J}}$ through the integrals (17), (18) simultaneously with step 3;
• Determine $\nu$ from (19) and compute an estimation of $\delta \mathbf{u}$ via (15);
• Update the control input ${\mathbf{u}}_1$ using (20).

Remark: In the standard OCP, steps (2)–(6) are repeated until the optimal solution is found. However, since we pursue fast computation and real-time capability, these steps are only performed once. Obviously, ${\mathbf{u}}_1$ is not yet close to the optimal solution. SAC is then used to further improve the performance without sacrificing the computation time.

2.2.2. Sequential Action Control

The motivation of using Sequential Action Control (SAC) is that, instead of computing the control input for the whole prediction horizon, it is more crucial to consider the next interval since it will be applied to the system first. On the other hand, in dynamic environments, the predicted controller is affected by moving obstacles or might be completely changed if the final goal changes on-the-fly. SAC therefore aims to improve the control input for only the next interval, but still uses the same prediction horizon $[t_0,t_{\mathrm{f}}]$ for the performance cost evaluation.

Assuming that the nominal controller ${\mathbf{u}}_1$ is obtained using FOGA, described in Section 2.2.1, we want to find a control ${\mathbf{u}}_2^{\ast }$, denoted as the optimal control, that further improves the cost function (2) with regard to the dynamic system (4). SAC then computes a triplet consisting of the control value ${\mathbf{u}}_2^{\ast }$, its application time ${\tau }_m$, and the application duration $\lambda$. This triplet is called an action, and the control signal can be written as

$$\mathbf{u}\left(t\right)=\left\{\begin{array}{cc}{\mathbf{u}}_1,\hfill & t\notin \left[{\tau }_m-\frac{\lambda }{2},{\tau }_m-\frac{\lambda }{2}\right]\hfill \\ {\mathbf{u}}_2^{\ast },\hfill & t\in \left[{\tau }_m-\frac{\lambda }{2},{\tau }_m-\frac{\lambda }{2}\right]\hfill \end{array}\right.$$

(21)

with the nominal controller ${\mathbf{u}}_1$ and the optimal controller ${\mathbf{u}}_2^{\ast }$. This can be interpreted as a switching controller, where SAC switches between two modes. These two modes are given by

$${\mathbf{f}}_1\left(t\right)\stackrel{\wedge }{=}\mathbf{f}(\mathbf{x}\left(t\right),{\mathbf{u}}_1\left(t\right))$$

(22)

for the nominal controller ${\mathbf{u}}_1$ and

$${\mathbf{f}}_2(t,{\tau }_m)\stackrel{\wedge }{=}\mathbf{f}(\mathbf{x}\left(t\right),{\mathbf{u}}_2^{\ast }\left({\tau }_m\right))$$

(23)

for the optimal controller ${\mathbf{u}}_2^{\ast }$. In this paper, the application time ${\tau }_m$ is deterministic with ${\tau }_m=t_{\mathrm{cur}}+{\tau }_{\mathrm{sample}}/2$, where $t_{\mathrm{cur}}$ denotes the current time and ${\tau }_{\mathrm{sample}}$ denotes the sampling time. Now, recall that our aim is to improve the cost function (2) with the new controller ${\mathbf{u}}_2^{\ast }$ applied within the duration $\lambda$. We then rely on the mode insert gradient [43], which evaluates the first-order sensitivity of the cost (2)

$$\frac{d{J}_1}{d{\lambda }^{+}}\left({\tau }_m\right)=\rho {\left({\tau }_m\right)}^{\mathsf{T}}({\mathbf{f}}_2(·,{\tau }_m)-{\mathbf{f}}_1(·,{\tau }_m))$$

(24)

This equation measures how the cost is influenced by varying the length $\lambda$ of the application of the optimal control ${\mathbf{u}}_2^{\ast }$. The co-state $\rho$ in (24) is computed based on (6) and (7). To reduce the cost $J_1$, (24) should be driven to a desired negative value ${\alpha }_d\in {\mathbb{R}}^{-}$. This can be done by simply introducing an auxiliary cost function

$$\begin{array}{cc}\hfill l_2\left(s\right)& =l_2(\mathbf{x}\left(s\right),{\mathbf{u}}_1\left(s\right),{\mathbf{u}}_2\left(s\right),\rho \left(s\right))\hfill \\ \hfill & =\frac{1}{2}{\left[\frac{\mathrm{d}{J}_1}{\mathrm{d}{\lambda }^{+}}-{\alpha }_d\right]}^2+\frac{1}{2}{∥{\mathbf{u}}_2\left(s\right)∥}_{\mathbf{S}}^2\hfill \end{array}$$

(25)

with $\mathbf{S}>0$ and ${{\mathbf{u}}_2\left(s\right)}_{\mathbf{S}}^2={\mathbf{u}}_2{\left(s\right)}^T\mathbf{S}{\mathbf{u}}_2\left(s\right)$. Solving the minimization problem of (25) results in a control that achieves the desired sensitivity ${\alpha }_d$. For models in control-affine form (4), the solution for this minimization is given analytically by

$${\mathbf{u}}_2^{\ast }={(\Lambda +{\mathbf{S}}^{\mathsf{T}})}^{-1}[\Lambda {\mathbf{u}}_1+\mathbf{h}{\left(\mathbf{x}\right)}^{\mathsf{T}}\rho {\alpha }_d]$$

(26)

with $\Lambda =\mathbf{h}{\left(\mathbf{x}\right)}^{\mathsf{T}}\rho {\rho }^{\mathsf{T}}\mathbf{h}\left(\mathbf{x}\right)$.

2.2.3. Extended Sequential Action Control with Target Constraints

One problem that arises from the theoretical background of SAC, presented in Section 2.2.2, is that it cannot handle constraints, and hence using SAC solely in the next step can lead to the violation of the constraint (3c). To prevent this occurrence, we extend the the original SAC method by using the knowledge from FOGA. We redefine the auxiliary cost function in (25) as

$$\begin{array}{cc}\hfill l_2^{\mathrm{ext}}\left(s\right)& =l_2(\mathbf{x}\left(s\right),{\mathbf{u}}_1\left(s\right),{\mathbf{u}}_2\left(s\right),\rho \left(s\right))\hfill \\ \hfill & =\frac{1}{2}{\left[\frac{\mathrm{d}{J}_1}{\mathrm{d}{\lambda }^{+}}-{\alpha }_d\right]}^2+\frac{1}{2}{\left[\frac{d{l}_{\mathrm{c}}}{d{\lambda }^{+}}-{\alpha }_c\right]}^2+\frac{1}{2}{∥{\mathbf{u}}_2\left(s\right)∥}_{\mathbf{S}}^2\hfill \end{array}$$

(27)

where $l_c=\frac{1}{2}{\Phi }^{\mathsf{T}}{\mathbf{Q}}_c\Phi$, ${\mathbf{Q}}_c\in {\mathbb{R}}^{q\times q}$ is a positive definite matrix and $\Phi$ is the vector of target constraints defined in (3c). $l_c$ can be interpreted as the additional constraint cost and therefore $\frac{d{l}_c}{d{\lambda }^{+}}$ measures how this constraint cost is influenced by varying the length $\lambda$ of the optimal control ${\mathbf{u}}_2^{\ast }$. Hence, solving the new minimization problem of (27) also drives this sensitivity to a negative value ${\alpha }_c\in {\mathbb{R}}^{-}$. This leads to the reduction of $l_c$, which then helps SAC to prevent the constraints from being violated.

The only problem now is the evaluation of $\frac{d{l}_c}{d{\lambda }^{+}}$. We have

$$\frac{d{l}_c}{d{\lambda }^{+}}\left({\tau }_m\right)=\frac{d{l}_c}{d\mathbf{x}\left(t_{\mathrm{f}}\right)}\frac{d\mathbf{x}\left(t_{\mathrm{f}}\right)}{d{\lambda }^{+}}\left({\tau }_m\right)={\Phi }^{\mathsf{T}}{\mathbf{Q}}_c\frac{d\mathbf{x}\left(t_{\mathrm{f}}\right)}{d{\lambda }^{+}}\left({\tau }_m\right)$$

(28)

From (11), we have

$$\begin{array}{cc}\hfill \delta \Phi =\delta \mathbf{x}\left(t_{\mathrm{f}}\right)& ={\int }_{t_0}^{t_{\mathrm{f}}}{\mathbf{R}}^{\mathsf{T}}\frac{\partial \mathbf{f}}{\partial \mathbf{u}}\delta \mathbf{u}\left(t\right)dt\hfill \\ \hfill & ={\int }_{{\tau }_m}^{{\tau }_m+\delta \lambda }{\mathbf{R}}^{\mathsf{T}}\frac{\partial \mathbf{f}}{\partial \mathbf{u}}({\mathbf{u}}_2-{\mathbf{u}}_1)dt\hfill \end{array}$$

(29)

where $[{\tau }_m,{\tau }_m+\delta \lambda ]$ is the infinitesimal duration where ${\mathbf{u}}_2$ is applied. As $\delta \lambda \to 0$, (29) can be written as

$$d\mathbf{x}\left(t_{\mathrm{f}}\right)={\mathbf{R}}^{\mathsf{T}}\frac{d\mathbf{f}}{d\mathbf{u}}\left({\tau }_m\right)({\mathbf{u}}_2-{\mathbf{u}}_1)d\lambda$$

(30)

or

$$\frac{d\mathbf{x}\left(t_{\mathrm{f}}\right)}{d{\lambda }^{+}}\left({\tau }_m\right)={\mathbf{R}}^{\mathsf{T}}\mathbf{h}\left({\tau }_m\right)({\mathbf{u}}_2-{\mathbf{u}}_1)$$

(31)

Substituting this into (28) and solving the minimization problem of (27) in the same way as in Section 2.2.2, we obtain a new analytical solution of ${\mathbf{u}}_2^{\ast }$

$${\mathbf{u}}_2^{\ast }={({\Lambda }_1+{\Lambda }_2+\mathbf{S})}^{-1}({\Lambda }_1{\mathbf{u}}_1+{\Lambda }_2{\mathbf{u}}_1+{\mathbf{h}}^{\mathsf{T}}\rho {\alpha }_d+{\mathbf{h}}^{\mathsf{T}}{\mathbf{R}}_c^{\mathsf{T}}{\alpha }_c)$$

(32)

with ${\Lambda }_1={\mathbf{h}}^{\mathsf{T}}\rho {\rho }^{\mathsf{T}}\mathbf{h}$, ${\Lambda }_2={\mathbf{h}}^{\mathsf{T}}{\mathbf{R}}_c^{\mathsf{T}}{\mathbf{R}}_c\mathbf{h}$, ${\mathbf{R}}_c={\Phi }^{\mathsf{T}}{\mathbf{Q}}_c{\mathbf{R}}^{\mathsf{T}}$.

In this work, the application time $\lambda$ is set to be equal to ${\tau }_{\mathrm{sample}}$ such that ${\mathbf{u}}_2^{\ast }$ is applied for one interval. Additionally, ${\mathbf{u}}_2^{\ast }$ is only applied if it results in both a smaller performance cost $J_1$ and terminal constraint cost $l_c$ over the prediction horizon $T_{\mathrm{P}}$ compared to the nominal control ${\mathbf{u}}_1$. This is considered as the Selection procedure in the control scheme, as illustrated in Figure 1.

In order to obtain a feasible solution, a boundary of control signals is given by box constraints, i.e., $U={[u_{\min },u_{\max }]}^m$, by simply saturating the control output computed above. Overall, Algorithm 1 outlines the general structure of the proposed method.

Algorithm 1: TC-SAC.

Initialize ${\mathbf{x}}_0$, ${\mathbf{x}}_{\mathrm{d}}$, current time $t_{\mathrm{curr}}$, prediction horizon $T_{\mathrm{P}}$, sampling time ${\tau }_{\mathrm{sample}}$, end time $T_{\mathrm{end}}$, initial guess for nominal control ${\mathbf{u}}_1$. while  $t_{\mathrm{curr}}<T_{\mathrm{end}}$do    if $t_{\mathrm{curr}}\ge {\tau }_m$ then      ${\tau }_m=t_{\mathrm{curr}}+{\tau }_{\mathrm{sample}}/2$      Simulate $(\mathbf{x},\rho ,\mathbf{R})$ using ${\mathbf{u}}_1$ for $t\in [t_0,t_{\mathrm{f}}]$      Compute ${\mathbf{I}}_{\psi \psi }$, ${\mathbf{I}}_{\mathbf{J}\psi }$ as in (17) and (18), $\nu$ by (19), change in control $\delta \mathbf{u}$ by (15)      Update ${\mathbf{u}}_1\leftarrow {\mathbf{u}}_1+\delta \mathbf{u}$      Saturate ${\mathbf{u}}_1$ to $[u_{\min },u_{\max }]$      Simulate $(\mathbf{x},\rho )$ using new ${\mathbf{u}}_1$ for $t\in [t_0,t_f]$      Compute new costs $J_{1,\mathrm{init}},l_{c,\mathrm{init}}$      Specify ${\alpha }_d$      Compute ${\mathbf{u}}_2^{\ast }$ from (26) and saturate it to $[u_{\min },u_{\max }]$      Initialize $k=0$, $J_{1,\min }\leftarrow J_{1,\mathrm{init}}$, $J_{1,\mathrm{SAC}}\leftarrow \infty$, $l_{c,\min }\leftarrow l_{c,\mathrm{init}}$, $l_{c,\mathrm{SAC}}\leftarrow \infty$      while $k<k_{\max }$ do         $\lambda ={\tau }_{\mathrm{sample}}$         $({\tau }_0,{\tau }_f)=({\tau }_m-\frac{\lambda }{2},{\tau }_m+\frac{\lambda }{2})$         Re-simulate $(\mathbf{x},\rho )$ applying control (21)         Compute new cost $J_{1,\mathrm{SAC}},l_{c,\mathrm{SAC}}$         $k=k+1$         if $J_{1,\mathrm{SAC}}<J_{1,\mathrm{init}}$$l_{c,\mathrm{SAC}}<l_{c,\mathrm{init}}$ then           $J_{1,\min }\leftarrow J_{1,\mathrm{SAC}},l_{c,\min }\leftarrow l_{c,\mathrm{SAC}}$         end if      end while      if $J_{1,\mathrm{SAC}}<J_{1,\mathrm{init}}$$l_{c,\mathrm{SAC}}<l_{c,\mathrm{init}}$ then         ${\mathbf{u}}_1\leftarrow {\mathbf{u}}_2^{\ast }$ for $t\in ({\tau }_m-\frac{\lambda }{2},{\tau }_m+\frac{\lambda }{2})$      end if    else      Apply control ${\mathbf{u}}_1$    end if    $(t_0,t_f)=(t_0+{\tau }_{\mathrm{sample}},t_0+T_{\mathrm{pre}}+{\tau }_{\mathrm{sample}})$ end while

3. Results

This section presents different examples to highlight the improvement of the proposed approach. We apply our method on the reaching task and trajectory tracking task for the 2DOF robotic arm and compare the results with the original SAC. An interesting question that arises from a theoretical point of view is the extent to which the extended SAC step in Section 2.2.3 affects the performance of the system. Therefore, two additional controllers are added for comparison: first, only the extended SAC with target constraint is used to compare to SAC, and second, FOGA with multi-steps is used to compare to TC-SAC.

3.1. Reaching Motion Task

In this task, we evaluate the performance of the proposed approach on a 2DOF robotic arm in the vertical plane, as shown in Figure 2. The full dynamics of the robot are considered, including friction and gravitation. A detailed mathematical explanation of this model can be found in [47]. The task of the robot is to reach a pre-defined position. The performance cost is chosen to penalize the error between the current and desired states

$$l_1\left(t\right)={(\mathbf{x}\left(t\right)-{\mathbf{x}}_d)}^T\mathbf{Q}(\mathbf{x}\left(t\right)-{\mathbf{x}}_d),$$

(33)

where $\mathbf{Q}⪰0$ is the weighting matrix. The target constraint is also set to be the desired position. The prediction horizon is chosen $T_{\mathrm{p}}=0.3\mathrm{s}$ and the sampling rate is set to $1\mathrm{kHz}$. The initial position of the robot is ${\mathbf{x}}_0=(-\frac{\pi }{2},0,0,0)$ and the control $\mathbf{u}$ is bounded by $\mathbf{u}\in [-50,50]$ Nm. The sensitivity ${\alpha }_{\mathrm{d}}$ is chosen proportional to the cost $J_1$, ${\alpha }_{\mathrm{d}}={\omega }_{{\alpha }_d}{J}_1$ with ${\omega }_{{\alpha }_d}=-10$. Similarly, the sensitivity ${\alpha }_c$ is set to be ${\alpha }_c={\omega }_{{\alpha }_c}{l}_c$ with ${\omega }_{{\alpha }_c}=-5$.

3.1.1. Upright as Desired Position

First, the results are evaluated in the case of ${\mathbf{x}}_{\mathrm{d}}$ being the upright position, ${\mathbf{x}}_{\mathrm{d}}=(\frac{\pi }{2},0,0,0)$. The weighting matrix for the performance cost (33) is $\mathbf{Q}=\mathrm{diag}\left(\right[100$, 50, $0.0001$, $0.0001\left]\right)$. Having the first two values of $\mathbf{Q}$ much higher than the latter means that the position errors are penalized more heavily than the velocity errors, which is necessary for tracking tasks. The weighting matrix for $l_c$ in (27) is ${\mathbf{Q}}_c=\mathrm{diag}\left(\right[10$, 1000, 10, $10\left]\right)$. For FOGA, we run in two iterations to allow a fair comparison to TC-SAC, in terms of cost improvement and computation time. The simulation is run for $1.5\mathrm{s}$ and the result is shown in Figure 3. All methods succeed in controlling and stabilizing the robot. However, TC-SAC and FOGA need only $0.5\mathrm{s}$ to converge to ${\mathbf{x}}_d$, while SAC and the extended SAC take more than $1\mathrm{s}$ to converge. The overall cost is shown in

Table 1. Total cost and computation time.

	SAC	Extended SAC	TC-SAC	FOGA
Total cost	$316.74$	$291.02$	$152.21$	$150.60$
Computation time	$18.18\mathrm{s}$	$19.60\mathrm{s}$	$89.82\mathrm{s}$	$124.63\mathrm{s}$

. Furthermore, each method is run 100 times to obtain the average computation time. Note that the code is run on the Matlab environment so the computation time is only used to evaluate the speed of these methods relatively.

Comparing between SAC and the extended SAC, the new sensitivity term helps to improve the constraint in the states of the robot. This can be seen clearly in ${\theta }_2$, where the extended SAC keeps the deviation between the current and desired state at a smaller value compared to SAC. This shows the effectiveness of the extended SAC in terms of tightening the target constraints. Comparing between TC-SAC and FOGA, the performance of both is quite similar, but the computation time of TC-SAC is $34\mathrm{s}$ less than FOGA with two iterations. This is because one iteration of SAC takes much less time than one iteration of FOGA, since SAC has a direct analytical solution. Therefore, TC-SAC is preferable since it can achieve the same performance in a shorter amount of time.

3.1.2. Arbitrary Position

In this section, we want to analyze the performance of the methods in the case of any arbitrary desired position. In this case, the controller has to compensate the gravitational force, which is zero if the robot is at the upright position (equilibrium point). This highlights the role of the target constraint in TC-SAC and FOGA in terms of convergence. The desired position is set to be ${\mathbf{x}}_{\mathrm{d}}=(\frac{3\pi }{4},\frac{-3\pi }{4},0,0)$ and other parameters are set to be the same as in the case of the upright position. The simulation is run for $2\mathrm{s}$ and the result is shown in Figure 4. It can be seen that TC-SAC and FOGA quickly converge to the desired position in approximately $0.5\mathrm{s}$, while both SAC and the extended SAC methods cannot converge to the desired position in ${\theta }_1$ and ${\theta }_2$. These offsets appear in any desired position that is not the equilibrium point. Therefore, TC-SAC and FOGA are preferable for reaching/tracking tasks, even though the computation time is longer.

3.2. Tracking an Ellipse Trajectory

Moving one step further, we evaluate the methods in a trajectory tracking task. Since SAC and the extended SAC are incapable of reaching an arbitrary position, they are excluded from this task. The trajectory is set to be an ellipse where the radii along the x-axis and y-axis are 0.3 m and 0.18 m, respectively. For the sake of simplicity, the robot follows the ellipse with a constant velocity and the whole ellipse takes $3\mathrm{s}$ to finish. The starting position is set to be upright. In this simulation, the weighting matrices $\mathbf{Q},{\mathbf{Q}}_c$ are set to be $\mathbf{Q}=\mathrm{diag}\left(\right[100,50,10,10\left]\right)$ and ${\mathbf{Q}}_c=\mathrm{diag}\left([10,100,0.1,0.1]\right)$. Other parameters remain the same. Figure 5 shows the tracking performance between TC-SAC and FOGA in the XY graph, while Figure 6 shows this in each state of the robot. Figure 5 also illustrates the configuration of the robot at different positions along the ellipse.

It can be seen that both controllers are able to track the given trajectory; however, TC-SAC performs better than FOGA in this task, especially on the right side of the ellipse. From a mathematical point of view, TC-SAC can be interpreted as a combination of one iteration of FOGA plus an update from SAC afterward. Hence, it can be said that the SAC step improves the cost further compared to the second iteration step of FOGA, and it is achieved in a much shorter amount of time. This highlights the advantage of TC-SAC in the case where the controller needs to be computed rapidly due to time restrictions, i.e., in dynamic environments. TC-SAC helps to improve the overall cost substantially, without losing too much computation time. Figure 7 also shows the control signal of TC-SAC on the first and second joint of the robot. The last graph in Figure 7 displays at which time step SAC is activated (represented as 1), which indicates when the control signal computed by SAC reduces the cost. By looking at how often SAC overtakes FOGA (${\mathbf{u}}_2^{\ast }$ is applied instead of ${\mathbf{u}}_1$), we can justify the effectiveness of the additional SAC step in TC-SAC. It can be seen that ${\mathbf{u}}_2^{\ast }$ is used most of the time, which means that SAC often improves the solution of FOGA (${\mathbf{u}}_{\mathbf{1}}$). This proves the effectiveness of this additional step since the computation of SAC is fast. In conclusion, TC-SAC is preferable in applications where the controller needs to be computed quickly and optimally. In the next section, we will show that TC-SAC is also applicable in different applications.

3.3. Trajectory Tracking in Dynamic Environment of a Car-Like System

Autonomous driving recently has received a lot of attention from industry and researchers. In the field of autonomous cars, it is crucial that the controlled car is able to react to highly dynamic environments. Since other participants in traffic cannot be predicted beforehand, the car has to adapt rapidly to changing situations. This section presents the implementation of TC-SAC for the trajectory tracking task, with an emphasis on obstacle avoidance in both static and dynamic cases. We use the single-track model (also known as the bicycle model [2,28,48,49,50]), as shown in Figure 8, as this model is widely used to represent vehicles.

The single-track model used in this work is nonlinear, with 7 states and 2 control input

$$\mathbf{x}=\left[\begin{array}{c}X,Y,{\Psi }_c,v,\alpha ,{\dot{\Psi }}_c,\delta \end{array}\right]$$

(34)

$$\mathbf{u}=\left[\begin{array}{c}{\dot{\delta }}_{\mathrm{input}}\\ M\end{array}\right]$$

(35)

with

$$\dot{\delta }={\dot{\delta }}_{\mathrm{input}}$$

(36)

with the x and y-coordinates X and Y, the orientation ${\Psi }_c$, the velocity v, the side slip angle $\alpha$, the change in orientation ${\dot{\Psi }}_c$, the steering angle $\delta$, and the applied torque M. The last state in x is the additional state added so that the single-track model is in control-affine form. Further details about this car model are given in Appendix A.

A Lissajous curve with a ratio of the frequencies of $\frac{2}{3}$ and a phase shift of $\frac{\pi }{2}$ is used for this tracking task.

$$\begin{array}{cc}\hfill x_{\mathrm{ref}}\left(t\right)& =50\sin \left(0.05\pi t\right)\hfill \\ \hfill y_{\mathrm{ref}}\left(t\right)& =50\cos \left(0.075\pi t\right)\hfill \\ \hfill v_{\mathrm{ref}}\left(t\right)& =\sqrt{{\dot{x}}_{\mathrm{ref}}^2\left(t\right)+{\dot{y}}_{\mathrm{ref}}^2\left(t\right)}\hfill \end{array}$$

(37)

This reference trajectory is chosen since it results in different curvature and velocity at every point, which makes it challenging for tracking. The velocity varies between approximately $3.65\frac{\mathrm{m}}{\mathrm{s}}$ and $14.16\frac{\mathrm{m}}{\mathrm{s}}$, which does not exceed the limit of the dynamic of the car. To be able to avoid the obstacles, an additional term is added into the cost function, which is defined as

$$l_{1,\mathrm{avoid}}=\left\{\begin{array}{cc}{C}_{\mathrm{obstacle}}·{(\mathrm{dist}(·)-r)}^2,\hfill & \mathrm{dist}(·)<r\hfill \\ 0,\hfill & \mathrm{dist}(·)>r\hfill \end{array}\right.$$

(38)

with the distance between the car from the center to an obstacle $\mathrm{dist}(·)=\mathrm{dist}(x,y,x_c,y_c)=\sqrt{{(x-x_c)}^2+{(y-y_c)}^2}$, the weighting factor $C_{\mathrm{obstacle}}$, and the obstacle radius r. The overall performance cost $l_1$ is then given by $l_1=l_{1,\mathrm{track}}+l_{1,\mathrm{avoid}}$. Since obstacle avoidance is most crucial for the safety of passengers and other traffic participants, $C_{\mathrm{obstacle}}$ is set to ${10}^6$ and therefore it is considered with a much higher weighting factor than the tracking task.

First, we test our method in the case of trajectory tracking with static obstacle avoidance. It is assumed that the obstacles are 2 m in diameter, which is close to the width of real cars. However, since avoidance cannot completely be guaranteed with soft constraints, a safety margin of 1 m around the obstacle is added, which leads to an obstacle radius r = 2 m to increase safety. The position of the obstacles is given in Figure 9 and marked by the circles.

It can be seen from the plot that the car successfully avoids all obstacles, while it is still able to track the reference trajectory. Looking at the cost measurement, there are two peaks at around 51 s and 91 s, which are the obstacles in the top right and bottom right of the curve. This means that the obstacles actually interfere with the safety margin around the car, although there is no collision. Therefore, choosing a proper safety margin is very important to ensure safety.

For dynamic obstacle avoidance, again, the Lissajous curve is chosen as the reference trajectory. The prediction horizon is increased to 2 s to consider the more difficult task of dynamic obstacle avoidance. 13 obstacles are included for the scenario: one at the center, which is static, 6 on a circular trajectory with a radius of 40 m, and 6 on a circular trajectory with 50 m radius. Both circles have the same center as the Lissajous curve. The obstacles are equally distributed on these two circles and take 20 s for a full lap. It is assumed that the future path of the obstacle is known by the car and therefore can be considered exactly by TC-SAC. The safety margin around the obstacles is increased to 4 m.

A short frame of the dynamic behavior is given in Figure 10, which shows the scenario at time stamps between 7 s and 10 s with 1 s between the single pictures.

It can be seen that the car is pushed away and afterwards is able to return back to the reference trajectory. A video as proof of the simulated result has been uploaded at https://github.com/khoilsr/tcsac_trajectory_generation (accessed on 30 May 2022). This scenario can be seen as a proof of concept since it creates many different situations of obstacle avoidance, i.e., obstacles from the front, back, side, and successive obstacle avoidance situations, before the car could reach the reference trajectory again. Thus, this scenario is not close to situations that might occur in reality, but more difficult. This also shows the capability of TC-SAC in rapidly solving optimal control problems in different applications.

4. Stability Analysis

The class of systems to be controlled is described by the following general nonlinear set of ODEs

$$\dot{\mathbf{x}}\left(t\right)=\mathbf{f}(\mathbf{x}\left(t\right),\mathbf{u}\left(t\right)),\mathbf{x}\left(0\right)={\mathbf{x}}_0$$

(39)

with state vector $\mathbf{x}\left(t\right)\in {\mathbb{R}}^n$, input vector $\mathbf{u}\left(t\right)\in {\mathbb{R}}^m$. We also assume that

• $\mathbf{f}:{\mathbb{R}}^n\times {\mathbb{R}}^m\to {\mathbb{R}}^n$ is twice continuously differentiable and $\mathbf{f}(\mathbf{0},\mathbf{0})=\mathbf{0}$—thus, $\mathbf{0}\in {\mathbb{R}}^n$ is an equilibrium of the system;
• system (39) has a unique solution for any initial condition ${\mathbf{x}}_0\in {\mathbb{R}}^n$ and any piecewise continuous $\mathbf{u}(·)\in {\mathbf{R}}^m$.

We then analyze and discuss the stability of TC-SAC, presented in Figure 1. For the sake of simplicity, we consider the stability of the system around the origin $\mathbf{x}=\mathbf{0}$. The stability of an arbitrary position is achieved similarly by shifting the state and control signals such that this arbitrary position becomes the origin. In addition, to keep the notation the same as in the literature, the objective cost function is denoted as V, which is equivalent to the notation $J_1$ presented in previous sections. Recall the OCP with target constraints

$$\begin{array}{ccc}\hfill & & \underset{\mathbf{u}}{min}V(\mathbf{x},t;\mathbf{u})={\int }_0^{T}L(\mathbf{x},\mathbf{u},t)dt\hfill \end{array}$$

(40a)

$$\begin{array}{ccc}\hfill \mathrm{s}.\mathrm{t}.& & \dot{\mathbf{x}}=\mathbf{f}(t,\mathbf{x}\left(t\right),\mathbf{u}\left(t\right)),\mathbf{x}\left(0\right)={\mathbf{x}}_0,\hfill \end{array}$$

(40b)

$$\begin{array}{ccc}\hfill & & \mathbf{x}\left(T\right)=\mathbf{0}\hfill \end{array}$$

(40c)

where L has a quadratic form:

$$L(\mathbf{x},\mathbf{u},t)=\frac{1}{2}\left[{\mathbf{x}}^{\mathsf{T}}\left(t\right)\mathbf{Q}\mathbf{x}\left(t\right)+{\mathbf{u}}^{\mathsf{T}}\left(t\right)\mathbf{R}\mathbf{u}\left(t\right)\right]$$

(41)

where $\mathbf{Q}$ and $\mathbf{R}$ are a positive definite matrix.

Here, let $\mathbf{x}(·;{\mathbf{x}}_0,0)$ denote the corresponding trajectory of (39) with initial condition $\mathbf{x}\left(0\right)={\mathbf{x}}_0$ and $\widehat{\mathbf{u}}(·;{\mathbf{x}}_0,0)$ denote the optimal control sequence that minimizes the objective function $V({\mathbf{x}}_0,0;\mathbf{u})$. From a methodological point of view, TC-SAC is a combination of FOGA in the first step and the extended SAC in the subsequent one. Hence, an intuitive method to analyze the stability of TC-SAC is to establish the stability conditions for FOGA first, and then the stability of TC-SAC can be concluded after this. Furthermore, TC-SAC uses the same concept of receding horizon as MPC; therefore, it is straightforward to derive the stability conditions of TC-SAC from the stability literature of MPC. In the following, we first look at the stability conditions of FOGA in the absence of the extended SAC in Section 4.1. Then, we discuss the stability of TC-SAC in Section 4.2.

4.1. Stability of FOGA

There have been several works that investigate and deploy sufficient conditions for the closed-loop MPC system to be stable. An overview of most of the works in this area can be found in [51]. Specifically, for our problem in (40), we are seeking the stability conditions of a receding horizon control with terminal constraint for nonlinear continuous systems. The constraint imposed at terminal time T provides a relatively simple procedure to establish the stability of the closed-loop system. In fact, Chen and Shaw [52] derived sufficient conditions for the closed-loop receding horizon control of (40) to be asymptotically stable. These conditions are described by the following assumptions.

Assumption 1.

There exists an optimal control function $\widehat{\mathbf{u}}(·;{\mathbf{x}}_0,0)$, which gives the minimal cost $\widehat{V}({\mathbf{x}}_0,0;\mathbf{u})$ and satisfies the terminal constraint (40c).

Assumption 2.

The optimal cost $\widehat{V}({\mathbf{x}}_0,0;\mathbf{u})$ satisfies the following conditions for any $T>0$

(a) 

$\widehat{V}({\mathbf{x}}_0,0;\mathbf{u})=0$ and $\widehat{V}(\mathbf{x},t;\mathbf{u})>0$ for $\mathbf{x}\ne \mathbf{0}$;

(b) 

$\widehat{V}(\mathbf{x},t;\mathbf{u})\to \infty$ when $∥\mathbf{x}∥\to \infty$;

(c) 

$\frac{\partial \widehat{V}(\mathbf{x},t;\mathbf{u})}{\partial \mathbf{x}}$ exists for any $\mathbf{x}$.

Theorem 1.

Suppose that Assumptions 1 and 2 are satisfied; then, for any fixed$T>0$, the closed-loop system is asymptotically stable at the origin.

Proof. 

See Appendix B. □

Assumption 2 can be fulfilled easily by having the cost function in quadratic form, i.e.,

$$V(\mathbf{x},t;\mathbf{u})=\frac{1}{2}{\int }_t^{t+T}\left({∥\mathbf{x}\left(\tau \right)∥}_{\mathbf{Q}}^2+{∥\mathbf{u}\left(\tau \right)∥}_{\mathbf{R}}^2\right)d\tau$$

(42)

Assumption 1 is, however, more difficult to be satisfied. It requires the optimal solution of (40), in which the terminal constraint (40c) also has to be satisfied for each receding horizon. We will first prove that there exists an optimal solution that satisfies the terminal constraint when using FOGA as the solver.

Recall the changes in the cost function $J_1$ and the terminal constraints $\Phi$

$$\delta J_1={\int }_{t_0}^{t_{\mathrm{f}}}\left({\rho }^{\mathsf{T}}\frac{\partial \mathbf{f}}{\partial \mathbf{u}}+\frac{\partial l_1}{\partial \mathbf{u}}\right)\delta \mathbf{u}\left(t\right)dt$$

(43)

$$\delta \Phi \triangleq {\left[\begin{array}{c}\delta x_1\\ ⋮\\ \delta x_q\end{array}\right]}_{t=t_{\mathrm{f}}}={\int }_{t_0}^{t_{\mathrm{f}}}{\mathbf{R}}^{\mathsf{T}}\frac{\partial \mathbf{f}}{\partial \mathbf{u}}\delta \mathbf{u}\left(t\right)dt$$

(44)

and adjoining (44) to (43), we obtain

$$\delta J_1+\nu \delta \Phi ={\int }_{t_0}^{t_{\mathrm{f}}}\left[\frac{\partial l_1}{\partial \mathbf{u}}+{(\rho +\mathbf{R}\nu )}^{\mathsf{T}}\frac{\partial \mathbf{f}}{\partial \mathbf{u}}\right]\delta \mathbf{u}\left(t\right)dt$$

(45)

where $\nu$ is also a Lagrange multiplier. The equation (45) represents the adjoined cost of the OCP. With the solution of $\delta \mathbf{u}\left(t\right)$ in (15), we have

$$\delta J_1+\nu \delta \Phi =-{\mathbf{W}}^{-1}{\int }_{t_0}^{t_{\mathrm{f}}}{∥\frac{\partial l_1}{\partial \mathbf{u}}+{(\rho +\mathbf{R}\nu )}^{\mathsf{T}}\frac{\partial \mathbf{f}}{\partial \mathbf{u}}∥}^{2}dt<0$$

(46)

which is negative unless the integrand vanishes over the whole integration interval. The Lagrange multiplier $\nu$ is computed as

$$\nu =-{\mathbf{I}}_{\Phi \Phi }^{-1}(\delta \Phi +{\mathbf{I}}_{\Phi \mathbf{J}})$$

(47)

with $\delta \Phi =-ϵ\Phi \left[\mathbf{x}\left(t_{\mathrm{f}}\right)\right]$ and a constant $ϵ\in (0,1]$. The choice of $\delta \Phi$ guarantees that the constructed control signal $\mathbf{u}\left(t\right)$ drives the system close to the desired state at the final time $t_f$. Thus, the terminal constraints can be satisfied after a finite amount of iterations. Since the system is fully controllable, ${\mathbf{I}}_{\Phi \Phi }^{-1}$ exists; thus, there always exists $\nu$ and $\delta \mathbf{u}$ such that the cost function is decreased and the terminal constraints are satisfied. As the optimal solution is approached and $\delta \Phi =0$, it is clear that

$$\nu \to -{\mathbf{I}}_{\Phi \Phi }^{-1}{\mathbf{I}}_{\Phi \mathbf{J}}$$

(48)

$$\frac{\partial l_1}{\partial \mathbf{u}}+{(\rho +\mathbf{R}\nu )}^{\mathsf{T}}\frac{\partial \mathbf{f}}{\partial \mathbf{u}}\to 0$$

(49)

FOGA is therefore able to construct an optimal control signal $\mathbf{u}\left(t\right)$ that minimizes the cost and satisfies terminal constraints. Hence, Assumption 1 is satisfied.

A problem that arises here is that Assumption 2 uses the optimal cost $\widehat{V}(\mathbf{x};T)$ as the Lyapunov function to establish the closed-loop stability. This requires FOGA to be performed repeatedly until the optimal solution is found, which is not practical in our case since we want to achieve fast computation for online capability. However, this optimal solution is actually not necessary. Indeed, we will show that, with a proper “warm start”, the closed-loop system is still stable even if the cost is not optimal.

Definition 1

(Warm start). An admissible warm start, $\tilde{\mathbf{u}}$, must steer the current state $\mathbf{x}$ to the origin, i.e., satisfy terminal constraint $\mathbf{x}\left(T\right)=\mathbf{0}$.

A warm start needs to satisfy the constraints but does not have to be optimal; hence, it can be acquired much faster. Furthermore, a warm start only needs to be computed once and can be done offline. In our approach, a warm start is achieved by performing FOGA for a couple of iterations. The process can be sped up with a proper choice of $ϵ$ to calculate the Lagrange multiplier $\nu$ in (47). A controller algorithm using a warm start is as follows.

Controller algorithm with warm start

Data: ${\mathbf{x}}_0\in {\mathbb{R}}^n$, $\delta \in (0,\infty )$, where $\delta$ is the sampling interval

Initialization: At time $t_0=0$, if ${\mathbf{x}}_0$ is at the origin, i.e., ${\mathbf{x}}_0=\mathbf{0}$, meaning that the system is already at the equilibrium, then employ $\mathbf{u}=\mathbf{0}$ to maintain the current state. Else, perform FOGA for a couple of iterations to compute a feasible warm start ${\mathbf{u}}_0$ for the OCP problem in (40). Apply the control ${\mathbf{u}}_0$ to the real system over the interval $\left[t_0,t_0+\delta \right]$.

Repeat:

• At any time t, if $\mathbf{x}\left(t\right)=\mathbf{0}$, meaning that the system reaches the origin, employ $\mathbf{u}=\mathbf{0}$. Else:
• At any time $t_i\triangleq i\delta ,i\in \mathbb{N}$:
  • Obtain an admissible control ${\mathbf{u}}_i^{\prime }$ as an initial guess

    $${\mathbf{u}}_i^{\prime }=\left\{\begin{array}{c}{\mathbf{u}}_{i-1}\mathrm{for}t\in \left[t_i,t_i+T-\delta \right]\\ \mathbf{0}\mathrm{for}t\in \left[t_i+T-\delta ,t_i+T\right]\end{array}\right.$$

    (50)
  • Compute an admissible control horizon that is better than the preceding control horizon in the sense that

    $$V({\mathbf{x}}_i,t_i,{\mathbf{u}}_i)\le V({\mathbf{x}}_i,t_i,{\mathbf{u}}_i^{\prime })$$

    (51)
  • Apply the control ${\mathbf{u}}_i$ to the real system over the interval $\left[t_i,t_i+\delta \right]$

The stability proof of MPC with the warm start is presented in [53]. With a choice of admissible warm starts, the controller is asymptotically stable. The terminal constraints are satisfied from the beginning and the cost is improved over time. Even in the situation where the cost increases due to numerical errors, we simply return to the warm start of the previous iteration and continue from there.

4.2. Stability of TC-SAC

As mentioned, TC-SAC can be interpreted as a combination of FOGA in the first step and the extended SAC in the subsequent one. Since FOGA was proven to be stable in Section 4.1, we only need to confirm that the extended SAC in the second step does not violate the stability property of FOGA. In other words, we need to fulfill two conditions: (1) the performance cost (40a) is decreased when applying the extended SAC, and (2) the terminal constraint (40c) is not violated.

It can be easily seen that both of the conditions are guaranteed naturally by the procedure of TC-SAC presented in Section 2.2. From a methodological point of view, TC-SAC only applies the control signal computed by the extended SAC if it results in a smaller value in both performance cost $V(\mathbf{x},t;\mathbf{u})$ and terminal cost $l_c=\frac{1}{2}{\Phi }^{\mathsf{T}}{\mathbf{Q}}_c\Phi$, when compared to FOGA. The reduction in $V(\mathbf{x},t;\mathbf{u})$ guarantees that condition 1 is satisfied. Similarly, the reduction in terminal cost $l_c$ means that the extended SAC drives the system closer to the origin at time T, thus fulfilling condition 2. If any of the conditions is not met, the control signal computed by FOGA is used instead. In both cases, we ensure that TC-SAC inherits the stability property of FOGA and, therefore, we can conclude that TC-SAC is asymptotically stable.

5. Discussion and Conclusions

In this paper, we presented a fast and close-to-optimal control method called TC-SAC. We also evaluated the proposed method in different scenarios and situations to test its capability. In detail, TC-SAC is able to fulfill most of the reaching/tracking tasks, even in critical scenarios such as the Lissajous trajectory. Furthermore, it was shown that TC-SAC is able to avoid static and dynamic obstacles efficiently with only soft constraints, i.e., with obstacle avoidance costs. In terms of computational effort, TC-SAC is able to find a sub-optimal solution that is close to the optimal one, without significantly affecting the computation time. Therefore, our proposed method has a lot of potential for applications that require online capability and performance costs to be optimal. Note that TC-SAC is a model-based control method; hence, it can be applied to different linear/nonlinear systems, and is not only limited to the applications presented in this paper. Moreover, the stability proof presented in this work shows TC-SAC to be a reliable controller when it comes to safety aspects. The only downside is that TC-SAC is not able to consider inequality constraints, which might limit its use to systems that have strict requirements in this type of constraint.

Although the results in this work were only achieved in simulation, we believe that this method can be easily transferred to real systems due to its high potential. For real-time applications, the algorithm needs to be written in a system programming level such as C programming, so that it can be computed as quickly as possible. The prediction horizon $T_p$ also needs to be properly selected to achieve good results in terms of optimality, without losing too much of the computational effort. For complex systems such as high-DOF or humanoid robots, a simplified model might be needed to reduce the computation time. In addition, since our approach is model-based, a precise model of the system will play an important role in the quality of the controller. Thus, for systems with unknown parameters, a model identification procedure needs to be performed first.

As for the future work of this paper, TC-SAC will be first implemented on a remote control car system and on a 2DOF robot for testing the trajectory tracking problems and obstacle avoidance behaviors. We also plan to investigate further the influence of the inaccuracy between the real and simulated models on the controller and the robustness of TC-SAC in the existence of noise and disturbances.