Structure-exploiting Newton-type method for optimal control of switched systems

ABSTRACT

This study proposes an efficient Newton-type method for the optimal control of switched systems under a given mode sequence. A mesh-refinement-based approach is utilised to discretise continuous-time optimal control problems (OCPs) and formulate a nonlinear program (NLP), which guarantees the local convergence of a Newton-type method. A dedicated structure-exploiting algorithm (Riccati recursion) is proposed to perform a Newton-type method for the NLP efficiently because its sparsity structure is different from a standard OCP. The proposed method computes each Newton step with linear time-complexity for the total number of discretization grids as the standard Riccati recursion algorithm. Additionally, the computation is always successful if the solution is sufficiently close to a local minimum. Conversely, general quadratic programming (QP) solvers cannot accomplish this because the Hessian matrix is inherently indefinite. Moreover, a modification on the reduced Hessian matrix is proposed using the nature of the Riccati recursion algorithm as the dynamic programming for a QP subproblem to enhance the convergence. A numerical comparison is conducted with off-the-shelf NLP solvers, which demonstrates that the proposed method is up to two orders of magnitude faster. Whole-body optimal control of quadrupedal gaits is also demonstrated and shows that the proposed method can achieve the whole-body model predictive control (MPC) of robotic systems with rigid contacts.

KEYWORDS:

• Optimal control
• switched systems
• numerical optimization
• model predictive control: legged robots

1. Introduction

Switched systems are a class of hybrid systems consisting of a finite number of subsystems and switching laws of active subsystems. Many practical control systems are modeled as switched systems, such as real-world complicated process systems (Bürger et al., 2019), automotive systems with gear shifts (Robuschi et al., 2021), and robotic systems with rigid contacts (Farshidian et al., 2017; Li & Wensing, 2020). It is difficult to solve the optimal control problems (OCPs) of such systems because these OCPs generally involve mixed-integer nonlinear programs (MINLPs) (Belotti et al., 2013). Therefore, model predictive control (MPC) (Rawlings et al., 2017), in which OCPs must be solved in real-time, is particularly difficult for switched systems. One of the most practical approaches for solving the OCPs of switched systems is the combinatorial integral approximation (CIA) decomposition method (Bürger et al., 2019; Sager et al., 2011). CIA decomposition relaxes the MINLP into a nonlinear program (NLP) in which the binary variables are relaxed into continuous variables. Subsequently, the integer variables are approximately reconstructed from the relaxed NLP solution by solving a mixed-integer linear program. However, vanishing constraints, which typically model mode-dependent path constraints, raise numerical issues due to the violation of the linear independence constraint qualification (LICQ) (Jung et al., 2013). A similar smoothing approach is used for discrete natures when solving OCPs for mechanical systems with rigid contacts, which are often approximately formulated as mathematical programs with complementarity constraints (MPCCs) (Posa et al., 2014; Yunt, 2011). However, this case has the same LICQ problem as the vanishing constraints, requiring a significant amount of computational time to alleviate the numerical ill-conditioning. Furthermore, it often suffers from undesirable stationary points (Nurkanović et al., 2020).

Another tractable and practical approach for the optimal control of switched systems is to fix the switching (mode) sequence. For example, in robotics applications, a high-level planner computes the feasible contact sequence taking perception into account. Subsequently, the sequence is provided to a lower-level optimal control-based dynamic motion planner or MPC controller (Grandia et al., 2021; Jenelten et al., 2020; Kuindersma et al., 2016). The lower-level dynamic planner or MPC then discovers the optimal switching times and optimal control input. An advantage of this approach over the CIA decomposition and MPCC approaches is that the optimization problem is smooth and can, therefore, be efficiently solved without suffering from the LICQ problem. For example, Johnson and Murphey (2011) and Stellato et al. (2017) proposed efficient Newton-type methods for OCPs of switched systems with autonomous subsystems. That is, the switched systems only included a switching signal but no continuous control input, which is called the switching-time optimization (STO) problem. The STO approach can be more efficient than the CIA decomposition because the STO problem can be formulated as a smooth and tractable NLP, as numerically shown in Stellato et al. (2017). However, once the switched system includes a continuous control input, the efficient methods of Johnson and Murphey (2011) and Stellato et al. (2017) cannot be applied. To the best of our knowledge, there is no efficient numerical method for OCPs of such systems. As a result, many real-world robotic applications are limited to focusing on dynamic motion planning with fixed switching instants (Di Carlo et al., 2018; Farshidian et al., 2017; Mastalli et al., 2020).

The two-stage approach has been studied for the OCPs of switched systems with continuous control input (Farshidian et al., 2017; Fu & Zhang, 2021; Li & Wensing, 2020; Xu & Antsaklis, 2002, 2004). A general two-stage approach was proposed in Xu and Antsaklis (2002, 2004). In this approach, the optimization problem to determine the control input and switching instants was decomposed into an STO problem with a fixed control input (upper-level problem) and standard OCP that only determined the control input with fixed switching instants (lower-level problem). In this framework, off-the-shelf OCP solvers can be used for a lower-level problem. Xu and Antsaklis (2004) formulated the lower-level continuous-time OCP as a two-point boundary-value problem and leveraged off-the-shelf shooting methods to solve it. Farshidian et al. (2017), Li and Wensing (2020), and Xu and Antsaklis (2002) formulated the lower-level problem as an NLP by using the direct single shooting method and solved it using off-the-shelf Newton-type methods. However, these studies still lack convergence speed because each of these two stages does not take the other stage into account when solving its own optimization problem. As a result, the application examples of Farshidian et al. (2017) and Li and Wensing (2020) were limited to off-line computation for the trajectory optimization problems of simplified robot models. Moreover, they could not guarantee the local convergence or address inequality constraints. The study of Fu and Zhang (2021) was the first study that guaranteed convergence with a finite number of iterations and treated inequality constraints within a two-stage framework. However, it still required extensive computational time until convergence, even for a considerably simple linear quadratic example.

Other approaches simultaneously optimise the switching instants and other variables, such as the state and control input (Betts, 2010; Katayama et al., 2020; Katayama & Ohtsuka, 2021c; Patterson & Rao, 2014). A multi-phase trajectory optimization (Betts, 2010; Patterson & Rao, 2014) naturally incorporated the STO problem into the direct transcription, solving the NLP to simultaneously determine all variables (including the state, control input, and switching instants) using general-purpose off-the-shelf NLP solvers, such as Ipopt (Wächter & Biegler, 2006). However, the computational speed of general-purpose linear solvers used in off-the-shelf NLPs can typically be further improved, particularly, for large-scale systems because they have a certain sparsity structure (Rao et al., 1998; Wang & Boyd, 2010; Zanelli et al., 2020). In Katayama et al. (2020), we applied the simultaneous approach with the direct single-shooting method and achieved real-time MPC for a simple walking robot using the Newton–Krylov type method (Ohtsuka, 2004). In Katayama and Ohtsuka (2021c), we formulated an NLP using the direct multiple-shooting method (Bock & Plitt, 1984) and proposed a Riccati recursion algorithm for the NLP, which achieved faster computational time compared with the two-stage methods. However, these methods lacked the convergence guarantee due to the irregular discretization of the continuous-time OCP, which changed the problem structure throughout the Newton-type iterations. As a result, the method from Katayama and Ohtsuka (2021c) could only converge when the initial guess of the switching instants was close to the optimal one in the numerical example.

This study proposes an efficient Newton-type method for optimal control of switched systems under a given mode sequence. First, a mesh-refinement-based approach is proposed to discretise the continuous-time OCP using the direct multiple-shooting method (Bock & Plitt, 1984) to formulate an NLP that facilitates the local convergence of the Newton-type methods. Second, a dedicated efficient structure-exploiting algorithm (Riccati recursion algorithm Rao et al., 1998) is proposed for the Newton-type method because the sparsity structure of the NLP is different from the standard OCP. The proposed method computes each Newton step with linear time-complexity of the total number of the discretization grids as the standard Riccati recursion algorithm. Additionally, it can always solve the Karush–Kuhn–Tucker (KKT) systems arising in the Newton-type method if the solution is sufficiently close to a local minimum, so that the second-order sufficient condition (SOSC) holds. This is in contrast to some general quadratic programming (QP) solvers that cannot treat the proposed formulation because the Hessian matrix is inherently indefinite. Third, a modification on the reduced Hessian matrix is proposed to enhance the convergence using the nature of the Riccati recursion algorithm as the dynamic programming (Bertsekas, 2005) for a QP subproblem. Two numerical experiments were conducted to demonstrate the efficiency of the proposed method: a comparison with off-the-shelf NLP solvers and testing the whole-body optimal control of quadrupedal gaits. The comparison with off-the-shelf NLP solvers showed that the proposed method could solve the OCPs that the sequential quadratic programming (SQP) method with qpOASES (Ferreau et al., 2014) or OSQP (Stellato et al., 2020) failed to solve and was up to two orders of magnitude faster than a general NLP solver, Ipopt (Wächter & Biegler, 2006). The whole-body optimal control of quadrupedal gaits showed that the proposed method achieved the whole-body MPC of robotic systems with rigid contacts.

The remainder of this paper is organised as follows. A mesh-refinement-based discretization method of the continuous-time OCP is presented in Section 2. The KKT system to be solved to compute the Newton-step is discussed in Section 3, and the Riccati recursion algorithm to solve the KKT system and its convergence properties are described in Section 4. The reduced Hessian modification using the Riccati recursion algorithm is also described in this section. The above formulations and Riccati recursion algorithm are extended to switched systems with state jumps and switching conditions in Section 5, representing robotic systems with contacts. A numerical comparison of the proposed method with off-the-shelf NLP solvers and examples of whole-body optimal control of a quadrupedal robot are presented in Section 6. Finally, a brief summary and mention of future work are presented in Section 7.

Notation and preliminaries: The Jacobians and Hessians of a differentiable function using certain vectors are described as follows: ${\mathrm{\nabla }}_{x}f\left(x\right)$ denotes $\left(\frac{\mathrm{\partial }f}{\mathrm{\partial }x}{)}^{\mathrm{T}}\right(x)$, and ${\mathrm{\nabla }}_{xy}g(x,y)$ denotes $\frac{{\mathrm{\partial }}^{2}g}{\mathrm{\partial }x\mathrm{\partial }y}(x,y)$. A diagonal matrix whose elements are a vector x is denoted as $\mathrm{d}\mathrm{i}\mathrm{a}\mathrm{g}\left(x\right)$. A vector of an appropriate size with all components presented by $\alpha \in \mathbb{R}$ is denoted as $\alpha 1$. All functions are assumed to be twice-differentiable.

2. Problem formulation

We consider a switched system consisting of K + 1 ($K>0$) subsystems, which is expressed as (1) $\begin{array}{r}\dot{x}\left(t\right)=f_k\left(x\right(t),u(t\left)\right),t\in [t_{k-1},t_k],k\in \mathcal{K}.\end{array}$(1) The system is subject to the following constraints: (2) $\begin{array}{r}{g}_k\left(x\right(t),u(t\left)\right)\le 0,t\in [t_{k-1},t_k],k\in \mathcal{K},\end{array}$(2) where $x\left(t\right)\in {\mathbb{R}}^{n_x}$ denotes the state, $u\left(t\right)\in {\mathbb{R}}^{n_u}$ denotes the control input, $f_k:{\mathbb{R}}^{n_x}\times {\mathbb{R}}^{n_u}\to {\mathbb{R}}^{n_x}$, and $g_k:{\mathbb{R}}^{n_x}\times {\mathbb{R}}^{n_u}\to {\mathbb{R}}^{n_g}$. $\mathcal{K}:=\{1,\dots ,K+1\}$ denotes the indices of the active subsystems. Note that the index k is also referred to as a ‘phase’ in this study. $t_0$ and $t_{K+1}$ denote the fixed initial and terminal times of the horizon, respectively. $t_k$, $k\in \{1,\dots ,K\}$ denotes the switching instant from phase k to phase k + 1. We assume that the active subsystems switch in order from 1 to K + 1 over the horizon $[t_0,t_f]$, that is, $t_0\le t_1\le \cdots \le t_K\le t_{K+1}$. We further assume that each subsystem $k\in \{0,\dots ,K\}$ has to be active, at least for a given time duration ${\underset{\_}{\mathrm{\Delta }}}_k\ge 0$, called the minimum dwell-time. The OCP of the switched system for a given initial state $x\left(t_0\right)\in {\mathbb{R}}^{n_x}$ is expressed as (3a) $\begin{array}{rl}\underset{u(\cdot ),t_1,\dots ,t_K}{min}& J=V_f\left(x\right(t_{K+1}\left)\right)+\sum _{k=1}^{K+1}{\int }_{t_{k-1}}^{t_k}{l}_k\left(x\right(\tau ),u(\tau \left)\right)\mathrm{d}\tau \end{array}$(3a) (3b) $\begin{array}{rl}\mathrm{s}.\mathrm{t}.& \left(1\right),\left(2\right),\\ & t_{k-1}+{\underset{\_}{\mathrm{\Delta }}}_k\le t_k,k\in \mathcal{K}.\end{array}$(3b) Next, a discretization method and mesh-refinement-based solution approach are proposed for the OCP to guarantee the local convergence. Figure 1 illustrates the proposed discretization method. The OCP is discretised using the direct multiple-shooting method (Bock & Plitt, 1984) based on the forward Euler method so that all the time-steps in phase $k\in \mathcal{K}$ are equal. Note that it is easy to extend the proposed method for higher-order explicit integration schemes to the direct multiple-shooting method, such as the fourth-order explicit Runge–Kutta method, as long as the Riccati recursion algorithm can be applied to the integration schemes. We introduce N grid points over the horizon, the discretised state $X:=\{x_0,\dots ,x_N\}$, discretised control input $U:=\{u_0,\dots ,u_{N-1}\}$, and switching instants $T:=\{t_1,\dots ,t_K\}$. The NLP is then expressed as (4a) $\begin{array}{rl}\underset{X,U,T}{min}& J=V_f\left(x_N\right)+\sum _{k\in \mathcal{K}}\sum _{i\in {\mathcal{I}}_k}{l}_k(x_i,u_i)\mathrm{\Delta }{\tau }_k,\end{array}$(4a) (4b) $\begin{array}{rl}\mathrm{s}.\mathrm{t}.& x_0-\overline{x}=0,\end{array}$(4b) (4c) $\begin{array}{rl}& x_i+f_k(x_i,u_i)\mathrm{\Delta }{\tau }_k-x_{i+1}=0,i\in {\mathcal{I}}_k,k\in \mathcal{K},\end{array}$(4c) (4d) $\begin{array}{rl}& g_k(x_i,u_i)\le 0,i\in {\mathcal{I}}_k,k\in \mathcal{K},\end{array}$(4d) (4e) $\begin{array}{rl}& t_{k-1}+{\underset{\_}{\mathrm{\Delta }}}_k-t_k\le 0,k\in \mathcal{K}.\end{array}$(4e) Here, ${\mathcal{I}}_k$ is the set of stage indices at phase k (that is, the set of time stages where the subsystem equation $f_k(x_i,u_i)$ is active). Note that the last stage N is not included in any ${\mathcal{I}}_k$. That is, $0\in {\mathcal{I}}_0$, $(max{\mathcal{I}}_k)+1=min{\mathcal{I}}_{k+1}$ for $k\in \{1,\dots ,K\}$, and $N\notin {\mathcal{I}}_{K+1}$ hold for ${\mathcal{I}}_k$. $\mathrm{\Delta }{\tau }_k$ is the time step at phase k, defined as (4f) $\begin{array}{r}\mathrm{\Delta }{\tau }_k:=\frac{t_k-t_{k-1}}{N_k},k\in \mathcal{K},\end{array}$(4f) where $N_k$ is the number of grids in phase k, that is, $N_k=max{\mathcal{I}}_k-min{\mathcal{I}}_k$. Finally, the initial state condition is lifted as (4b(4b) $\begin{array}{rl}\mathrm{s}.\mathrm{t}.& x_0-\overline{x}=0,\end{array}$(4b) ), possibly for a real-time MPC implementation (Diehl et al., 2005).

Figure 1. Proposed discretization method for the OCPs of switched systems.

The continuous-time OCP (3(3a) $\begin{array}{rl}\underset{u(\cdot ),t_1,\dots ,t_K}{min}& J=V_f\left(x\right(t_{K+1}\left)\right)+\sum _{k=1}^{K+1}{\int }_{t_{k-1}}^{t_k}{l}_k\left(x\right(\tau ),u(\tau \left)\right)\mathrm{d}\tau \end{array}$(3a) ) is solved using an adaptive mesh-refinement approach (Betts, 2010), which consists of a solution of the NLP (4(4a) $\begin{array}{rl}\underset{X,U,T}{min}& J=V_f\left(x_N\right)+\sum _{k\in \mathcal{K}}\sum _{i\in {\mathcal{I}}_k}{l}_k(x_i,u_i)\mathrm{\Delta }{\tau }_k,\end{array}$(4a) ) using a Newton-type method and mesh-refinement due to the changes of $\mathrm{\Delta }{\tau }_k$, as shown in Algorithm 1. After solving the NLP, the size of the discretization steps $\mathrm{\Delta }{\tau }_k$ is checked for each $k\in \mathcal{K}$. If the step is too large, that is, if it exceeds the specified threshold $\mathrm{\Delta }{\tau }_{\mathrm{m}\mathrm{a}\mathrm{x}}$, the solution at phase k may be not accurate. Therefore, the number of grids for phase k is increased. Conversely, if the step is too small ($\mathrm{\Delta }\tau <\mathrm{\Delta }{\tau }_{\mathrm{m}\mathrm{i}\mathrm{n}}$), the number of grids for phase k is reduced to decrease the computational time of the next NLP step. The algorithm terminates if some criteria (for example, $l_2$ norm of the KKT residual), which is denoted as ‘NLP error’ in Algorithm 1, is smaller than a predefined threshold $ϵ>0$ and the discretization steps pass the checks. By appropriately choosing $\mathrm{\Delta }{\tau }_{\mathrm{m}\mathrm{i}\mathrm{n}}$, the NLP dimension is almost constant throughout Algorithm 1. This is an advantage of the proposed method over direct transcription methods, such as Patterson and Rao (2014), whose NLP dimension is unknown before being solved.

It is worth noting that the NLP (4(4a) $\begin{array}{rl}\underset{X,U,T}{min}& J=V_f\left(x_N\right)+\sum _{k\in \mathcal{K}}\sum _{i\in {\mathcal{I}}_k}{l}_k(x_i,u_i)\mathrm{\Delta }{\tau }_k,\end{array}$(4a) ) is smooth and its structure does not change within each NLP step. Therefore, the Newton-type method for the NLP (4(4a) $\begin{array}{rl}\underset{X,U,T}{min}& J=V_f\left(x_N\right)+\sum _{k\in \mathcal{K}}\sum _{i\in {\mathcal{I}}_k}{l}_k(x_i,u_i)\mathrm{\Delta }{\tau }_k,\end{array}$(4a) ) is always tractable.

Remark 2.1

It is trivial to show the local (superlinear) convergence of Newton-type methods for the NLP (4(4a) $\begin{array}{rl}\underset{X,U,T}{min}& J=V_f\left(x_N\right)+\sum _{k\in \mathcal{K}}\sum _{i\in {\mathcal{I}}_k}{l}_k(x_i,u_i)\mathrm{\Delta }{\tau }_k,\end{array}$(4a) ) under some reasonable assumptions (Nocedal & Wright, 2006). Moreover, if the solution guess after the mesh-refinement of Algorithm 1 is sufficiently close to a local minimum of the NLP after the mesh-refinement (that is, the mesh-refinement is sufficiently accurate), then the local convergence of the overall Algorithm 1 is also guaranteed. In contrast, previous methods cannot guarantee the local convergence, such as the two-stage approaches (Farshidian et al., 2017; Li & Wensing, 2020; Xu & Antsaklis, 2002, 2004) and simultaneous approaches with the other discretization methods (Katayama et al., 2020; Katayama & Ohtsuka, 2021c).

Note that it is assumed throughout this study that the state Equation (1(1) $\begin{array}{r}\dot{x}\left(t\right)=f_k\left(x\right(t),u(t\left)\right),t\in [t_{k-1},t_k],k\in \mathcal{K}.\end{array}$(1) ), inequality constraints (2(2) $\begin{array}{r}{g}_k\left(x\right(t),u(t\left)\right)\le 0,t\in [t_{k-1},t_k],k\in \mathcal{K},\end{array}$(2) ), and cost function (3a(3a) $\begin{array}{rl}\underset{u(\cdot ),t_1,\dots ,t_K}{min}& J=V_f\left(x\right(t_{K+1}\left)\right)+\sum _{k=1}^{K+1}{\int }_{t_{k-1}}^{t_k}{l}_k\left(x\right(\tau ),u(\tau \left)\right)\mathrm{d}\tau \end{array}$(3a) ) do not depend on the time. That is, they are time invariant for notational simplicity. However, it is possible to extend the proposed method to time-varying cases, where the time of each grid i that depends on the duration of phases $t_k-t_{k-1}$ in (1(1) $\begin{array}{r}\dot{x}\left(t\right)=f_k\left(x\right(t),u(t\left)\right),t\in [t_{k-1},t_k],k\in \mathcal{K}.\end{array}$(1) ), (2(2) $\begin{array}{r}{g}_k\left(x\right(t),u(t\left)\right)\le 0,t\in [t_{k-1},t_k],k\in \mathcal{K},\end{array}$(2) ), and (3a(3a) $\begin{array}{rl}\underset{u(\cdot ),t_1,\dots ,t_K}{min}& J=V_f\left(x\right(t_{K+1}\left)\right)+\sum _{k=1}^{K+1}{\int }_{t_{k-1}}^{t_k}{l}_k\left(x\right(\tau ),u(\tau \left)\right)\mathrm{d}\tau \end{array}$(3a) ) must be taken into account. Then, additional sensitivities of (1(1) $\begin{array}{r}\dot{x}\left(t\right)=f_k\left(x\right(t),u(t\left)\right),t\in [t_{k-1},t_k],k\in \mathcal{K}.\end{array}$(1) ), (2(2) $\begin{array}{r}{g}_k\left(x\right(t),u(t\left)\right)\le 0,t\in [t_{k-1},t_k],k\in \mathcal{K},\end{array}$(2) ), and (3a(3a) $\begin{array}{rl}\underset{u(\cdot ),t_1,\dots ,t_K}{min}& J=V_f\left(x\right(t_{K+1}\left)\right)+\sum _{k=1}^{K+1}{\int }_{t_{k-1}}^{t_k}{l}_k\left(x\right(\tau ),u(\tau \left)\right)\mathrm{d}\tau \end{array}$(3a) ) with respect to the switching times are introduced in the KKT conditions and systems, which are derived in the next section. The formulations and methods of this study can be directly applied to such cases as long as the structure of the KKT system has the same form as the next section.

3. KKT system for Newton-type method

Next, the KKT system is derived to compute the Newton step of the NLP (4(4a) $\begin{array}{rl}\underset{X,U,T}{min}& J=V_f\left(x_N\right)+\sum _{k\in \mathcal{K}}\sum _{i\in {\mathcal{I}}_k}{l}_k(x_i,u_i)\mathrm{\Delta }{\tau }_k,\end{array}$(4a) ). The inequality constraints are treated with the primal–dual interior point method (Nocedal & Wright, 2006; Wächter & Biegler, 2006). That is, the slack variables $z_0,\dots ,z_{N-1}\in {\mathbb{R}}^{n_g}$ and $w_1,\dots ,w_{K+1}\in \mathbb{R}$ are introduced for (4d(4d) $\begin{array}{rl}& g_k(x_i,u_i)\le 0,i\in {\mathcal{I}}_k,k\in \mathcal{K},\end{array}$(4d) ) and (4e(4e) $\begin{array}{rl}& t_{k-1}+{\underset{\_}{\mathrm{\Delta }}}_k-t_k\le 0,k\in \mathcal{K}.\end{array}$(4e) ), respectively. The equality constraints are then considered, which are expressed as (5a) $\begin{array}{rl}& r_{g,i}:=g_k(x_i,u_i)+z_i=0,i\in {\mathcal{I}}_k,k\in \mathcal{K},\end{array}$(5a) (5b) $\begin{array}{rl}& r_{\mathrm{\Delta },k}:=t_{k-1}+{\underset{\_}{\mathrm{\Delta }}}_k-t_k+w_k=0,k\in \mathcal{K},\end{array}$(5b) instead of the inequality constraints (4d(4d) $\begin{array}{rl}& g_k(x_i,u_i)\le 0,i\in {\mathcal{I}}_k,k\in \mathcal{K},\end{array}$(4d) ) and (4e(4e) $\begin{array}{rl}& t_{k-1}+{\underset{\_}{\mathrm{\Delta }}}_k-t_k\le 0,k\in \mathcal{K}.\end{array}$(4e) ). The barrier functions $-ϵ\sum _{i=0}^{N-1}\ln z_i-ϵ\sum _{k=1}^{K+1}\ln w_k$ are also added to the cost function (4a(4a) $\begin{array}{rl}\underset{X,U,T}{min}& J=V_f\left(x_N\right)+\sum _{k\in \mathcal{K}}\sum _{i\in {\mathcal{I}}_k}{l}_k(x_i,u_i)\mathrm{\Delta }{\tau }_k,\end{array}$(4a) ), where $ϵ>0$ is the barrier parameter. An NLP associated with the barrier parameter ϵ is then expressed as (6a) $\begin{array}{rl}\underset{X,U,T,Z,W}{min}& \tilde{J}=V_f\left(x_N\right)+\sum _{k\in \mathcal{K}}\sum _{i\in {\mathcal{I}}_k}{l}_k(x_i,u_i)\mathrm{\Delta }{\tau }_k\\ & -ϵ\sum _{i=0}^{N-1}\ln z_i-ϵ\sum _{k=1}^{K+1}\ln w_k\end{array}$(6a) (6b) $\begin{array}{rl}\mathrm{s}.\mathrm{t}.& \left(4\mathrm{b}\right),\left(4\mathrm{c}\right),\left(5\mathrm{a}\right),\left(5\mathrm{b}\right),\end{array}$(6b) where $Z:=\{z_0,\dots ,z_{N-1}\}$ and $W:=\{w_1,\dots ,w_{K+1}\}$.

We then derive the KKT conditions for the NLP (6(6a) $\begin{array}{rl}\underset{X,U,T,Z,W}{min}& \tilde{J}=V_f\left(x_N\right)+\sum _{k\in \mathcal{K}}\sum _{i\in {\mathcal{I}}_k}{l}_k(x_i,u_i)\mathrm{\Delta }{\tau }_k\\ & -ϵ\sum _{i=0}^{N-1}\ln z_i-ϵ\sum _{k=1}^{K+1}\ln w_k\end{array}$(6a) ). To this end, ${\lambda }_0,\dots ,{\lambda }_N\in {\mathbb{R}}^{n_x}$ are introduced as the Lagrange multipliers with respect to (4b(4b) $\begin{array}{rl}\mathrm{s}.\mathrm{t}.& x_0-\overline{x}=0,\end{array}$(4b) ) and (4c(4c) $\begin{array}{rl}& x_i+f_k(x_i,u_i)\mathrm{\Delta }{\tau }_k-x_{i+1}=0,i\in {\mathcal{I}}_k,k\in \mathcal{K},\end{array}$(4c) ); ${\nu }_0,\dots ,{\nu }_{N-1}\in {\mathbb{R}}^{n_g}$ are with respect to (5a(5a) $\begin{array}{rl}& r_{g,i}:=g_k(x_i,u_i)+z_i=0,i\in {\mathcal{I}}_k,k\in \mathcal{K},\end{array}$(5a) ), and ${\upsilon }_1,\dots ,{\upsilon }_{K+1}\in \mathbb{R}$ are with respect to (5b(5b) $\begin{array}{rl}& r_{\mathrm{\Delta },k}:=t_{k-1}+{\underset{\_}{\mathrm{\Delta }}}_k-t_k+w_k=0,k\in \mathcal{K},\end{array}$(5b) ). The Lagrangian of the NLP (6(6a) $\begin{array}{rl}\underset{X,U,T,Z,W}{min}& \tilde{J}=V_f\left(x_N\right)+\sum _{k\in \mathcal{K}}\sum _{i\in {\mathcal{I}}_k}{l}_k(x_i,u_i)\mathrm{\Delta }{\tau }_k\\ & -ϵ\sum _{i=0}^{N-1}\ln z_i-ϵ\sum _{k=1}^{K+1}\ln w_k\end{array}$(6a) ) is then given by (7) $\begin{array}{rl}\mathcal{L}& :=V_f\left(x_N\right)+\sum _{k\in \mathcal{K}}\sum _{i\in {\mathcal{I}}_k}{H}_k(x_i,u_i,{\lambda }_i)\mathrm{\Delta }{\tau }_k\\ & +\sum _{i=0}^{N-1}{\lambda }_{i+1}^{\mathrm{T}}(x_i-x_{i+1})+{\lambda }_0^{\mathrm{T}}(\overline{x}-x_0)\\ & -ϵ\sum _{i=0}^{N-1}\ln z_i+\sum _{k\in \mathcal{K}}\sum _{i\in {\mathcal{I}}_k}{\nu }_i^{\mathrm{T}}\left(g_k\right(x_i,u_i)+z_i)\\ & -ϵ\sum _{k=1}^{K+1}\ln w_k+\sum _{k=1}^{K+1}{\upsilon }_k(t_{k-1}+{\underset{\_}{\mathrm{\Delta }}}_k-t_k+w_k),\end{array}$(7) where (8) $\begin{array}{r}{H}_k(x_i,u_i,{\lambda }_{i+1}):=l_k(x_i,u_i)+{\lambda }_{i+1}^{\mathrm{T}}{f}_k(x_i,u_i)\end{array}$(8) is the Hamiltonian at phase $k\in \mathcal{K}$. The KKT conditions of the NLP (6a(6a) $\begin{array}{rl}\underset{X,U,T,Z,W}{min}& \tilde{J}=V_f\left(x_N\right)+\sum _{k\in \mathcal{K}}\sum _{i\in {\mathcal{I}}_k}{l}_k(x_i,u_i)\mathrm{\Delta }{\tau }_k\\ & -ϵ\sum _{i=0}^{N-1}\ln z_i-ϵ\sum _{k=1}^{K+1}\ln w_k\end{array}$(6a) ) are then composed by the equality constraints (4b(4b) $\begin{array}{rl}\mathrm{s}.\mathrm{t}.& x_0-\overline{x}=0,\end{array}$(4b) ), (4c(4c) $\begin{array}{rl}& x_i+f_k(x_i,u_i)\mathrm{\Delta }{\tau }_k-x_{i+1}=0,i\in {\mathcal{I}}_k,k\in \mathcal{K},\end{array}$(4c) ), (5a(5a) $\begin{array}{rl}& r_{g,i}:=g_k(x_i,u_i)+z_i=0,i\in {\mathcal{I}}_k,k\in \mathcal{K},\end{array}$(5a) ), and (5b(5b) $\begin{array}{rl}& r_{\mathrm{\Delta },k}:=t_{k-1}+{\underset{\_}{\mathrm{\Delta }}}_k-t_k+w_k=0,k\in \mathcal{K},\end{array}$(5b) ) and the first-order derivatives of the Lagrangian (7(7) $\begin{array}{rl}\mathcal{L}& :=V_f\left(x_N\right)+\sum _{k\in \mathcal{K}}\sum _{i\in {\mathcal{I}}_k}{H}_k(x_i,u_i,{\lambda }_i)\mathrm{\Delta }{\tau }_k\\ & +\sum _{i=0}^{N-1}{\lambda }_{i+1}^{\mathrm{T}}(x_i-x_{i+1})+{\lambda }_0^{\mathrm{T}}(\overline{x}-x_0)\\ & -ϵ\sum _{i=0}^{N-1}\ln z_i+\sum _{k\in \mathcal{K}}\sum _{i\in {\mathcal{I}}_k}{\nu }_i^{\mathrm{T}}\left(g_k\right(x_i,u_i)+z_i)\\ & -ϵ\sum _{k=1}^{K+1}\ln w_k+\sum _{k=1}^{K+1}{\upsilon }_k(t_{k-1}+{\underset{\_}{\mathrm{\Delta }}}_k-t_k+w_k),\end{array}$(7) ) with respect to X, U, T, Z, and W, which are expressed as (9a) $\begin{array}{rl}& r_{x,N}:={\mathrm{\nabla }}_x{V}_f\left(x_N\right)-{\lambda }_N=0,\end{array}$(9a) (9b) $\begin{array}{rl}& r_{x,i}:={\mathrm{\nabla }}_x{H}_k(x_i,u_i,{\lambda }_{i+1})\mathrm{\Delta }{\tau }_k\\ & +{\mathrm{\nabla }}_x{g}_k^{\mathrm{T}}(x_i,u_i){\nu }_i+{\lambda }_{i+1}-{\lambda }_i=0,i\in {\mathcal{I}}_k,k\in \mathcal{K},\end{array}$(9b) (9c) $\begin{array}{rl}& r_{u,i}:={\mathrm{\nabla }}_u{H}_k(x_i,u_i,{\lambda }_{i+1})\mathrm{\Delta }{\tau }_k\\ & +{\mathrm{\nabla }}_u{g}_k^{\mathrm{T}}(x_i,u_i){\nu }_i=0,i\in {\mathcal{I}}_k,k\in \mathcal{K},\end{array}$(9c) (9d) $\begin{array}{rl}& r_{z,i}:=\mathrm{d}\mathrm{i}\mathrm{a}\mathrm{g}\left(z_i\right){\nu }_i-ϵ1=0,i\in {\mathcal{I}}_k,k\in \mathcal{K},\end{array}$(9d) (9e) $\begin{array}{rl}& r_{w,k}:=w_k{\upsilon }_k-ϵ=0,k\in \mathcal{K},\end{array}$(9e) and (9f) $\begin{array}{rl}& \frac{1}{N_k}\sum _{i\in {\mathcal{I}}_k}{H}_k(x_i,u_i,{\lambda }_{i+1})\\ & -\frac{1}{N_{k+1}}\sum _{i\in {\mathcal{I}}_{k+1}}{H}_{k+1}(x_i,u_i,{\lambda }_{i+1})\\ & -{\upsilon }_k+{\upsilon }_{k+1}=0,k\in \{1,\dots ,K\}.\end{array}$(9f) We refer to the above KKT conditions as the perturbed KKT conditions in the following because the complementary slackness regarding the inequality constraints are perturbed with ϵ as (9d(9d) $\begin{array}{rl}& r_{z,i}:=\mathrm{d}\mathrm{i}\mathrm{a}\mathrm{g}\left(z_i\right){\nu }_i-ϵ1=0,i\in {\mathcal{I}}_k,k\in \mathcal{K},\end{array}$(9d) ) and (9e(9e) $\begin{array}{rl}& r_{w,k}:=w_k{\upsilon }_k-ϵ=0,k\in \mathcal{K},\end{array}$(9e) ) (Nocedal & Wright, 2006). Next, the KKT system is derived to compute the Newton steps of all variables, that is, $\mathrm{\Delta }{x}_0,\dots ,\mathrm{\Delta }{x}_N\in {\mathbb{R}}^{n_x}$, $\mathrm{\Delta }{u}_0,\dots ,\mathrm{\Delta }{u}_{N-1},\in {\mathbb{R}}^{n_u}$ $\mathrm{\Delta }{t}_1,\dots ,\mathrm{\Delta }{t}_K\in \mathbb{R}$, and $\mathrm{\Delta }{\lambda }_0,\dots ,\mathrm{\Delta }{\lambda }_N\in {\mathbb{R}}^{n_x}$. Note that the KKT system is herein considered as the standard primal–dual interior point method (Nocedal & Wright, 2006; Wächter & Biegler, 2006), in which the Newton steps regarding the inequality constraints are eliminated (that is, $\mathrm{\Delta }{z}_0,\dots ,z_{N-1},\mathrm{\Delta }{\nu }_0,\dots ,\mathrm{\Delta }{\nu }_{N-1}\in {\mathbb{R}}^{n_g}$ and $\mathrm{\Delta }{w}_1,\dots ,\mathrm{\Delta }{w}_{K+1},\mathrm{\Delta }{\upsilon }_1,\dots ,\mathrm{\Delta }{\upsilon }_{K+1}\in \mathbb{R}$). The KKT system of interest is then expressed as (note that $\mathrm{\Delta }{t}_0=\mathrm{\Delta }{t}_{k+1}=0$) (10a) $\begin{array}{rl}& \mathrm{\Delta }{x}_0+x_0-\overline{x}=0,\end{array}$(10a) (10b) $\begin{array}{rl}& A_i\mathrm{\Delta }{x}_i+B_i\mathrm{\Delta }{u}_i+a_i(\mathrm{\Delta }{t}_k-\mathrm{\Delta }{t}_{k-1})\\ & -\mathrm{\Delta }{x}_{i+1}+{\overline{x}}_i=0,i\in {\mathcal{I}}_k,k\in \mathcal{K},\end{array}$(10b) (10c) $\begin{array}{rl}& Q_{xx,N}\mathrm{\Delta }{x}_N-\mathrm{\Delta }{\lambda }_N+{\overline{l}}_{x,N}=0,\end{array}$(10c) (10d) $\begin{array}{rl}& Q_{xx,i}\mathrm{\Delta }{x}_i+Q_{xu,i}\mathrm{\Delta }{u}_i+A_i^{\mathrm{T}}\mathrm{\Delta }{\lambda }_{i+1}\\ & -\mathrm{\Delta }{\lambda }_i+h_{x,i}(\mathrm{\Delta }{t}_k-\mathrm{\Delta }{t}_{k-1})+{\overline{l}}_{x,i}=0,\\ & i\in {\mathcal{I}}_k,k\in \mathcal{K},\end{array}$(10d) (10e) $\begin{array}{rl}& Q_{xu,i}^{\mathrm{T}}\mathrm{\Delta }{x}_i+Q_{uu,i}\mathrm{\Delta }{u}_i+B_i^{\mathrm{T}}\mathrm{\Delta }{\lambda }_{i+1}\\ & +h_{u,i}(\mathrm{\Delta }{t}_k-\mathrm{\Delta }{t}_{k-1})+{\overline{l}}_{u,i}=0,i\in {\mathcal{I}}_k,k\in \mathcal{K},\end{array}$(10e) and (10f) $\begin{array}{rl}& \frac{1}{N_k}\sum _{i\in {\mathcal{I}}_k}(h_{x,i}^{\mathrm{T}}\mathrm{\Delta }{x}_i+h_{u,i}^{\mathrm{T}}\mathrm{\Delta }{u}_i+a_i^{\mathrm{T}}\mathrm{\Delta }{\lambda }_{i+1}+{\overline{h}}_i)\\ & -\frac{1}{N_{k+1}}\sum _{i\in {\mathcal{I}}_{k+1}}(h_{x,i}^{\mathrm{T}}\mathrm{\Delta }{x}_i+h_{u,i}^{\mathrm{T}}\mathrm{\Delta }{u}_i+a_i^{\mathrm{T}}\mathrm{\Delta }{\lambda }_{i+1}+{\overline{h}}_i)\\ & +Q_{tt,k}(\mathrm{\Delta }{t}_k-\mathrm{\Delta }{t}_{k-1})-Q_{tt,k+1}(\mathrm{\Delta }{t}_{k+1}-\mathrm{\Delta }{t}_k)\\ & +{\overline{q}}_{t,k}-{\overline{q}}_{t,k+1}=0,k\in \{1,\dots ,K\},\end{array}$(10f) where $\begin{array}{rl}{Q}_{xx,i}& :={\mathrm{\nabla }}_{xx}{H}_k(x_i,u_i,{\lambda }_{i+1})\mathrm{\Delta }{\tau }_k\\ & +{\mathrm{\nabla }}_x{g}_k^{\mathrm{T}}(x_i,u_i)\mathrm{d}\mathrm{i}\mathrm{a}\mathrm{g}\left(z_i{)}^{-1}\mathrm{d}\mathrm{i}\mathrm{a}\mathrm{g}\right({\nu }_i\left){\mathrm{\nabla }}_x{g}_k^{\mathrm{T}}\right(x_i,u_i),\\ Q_{xu,i}& :={\mathrm{\nabla }}_{xu}{H}_k(x_i,u_i,{\lambda }_{i+1})\mathrm{\Delta }{\tau }_k\\ & +{\mathrm{\nabla }}_x{g}_k^{\mathrm{T}}(x_i,u_i)\mathrm{d}\mathrm{i}\mathrm{a}\mathrm{g}\left(z_i{)}^{-1}\mathrm{d}\mathrm{i}\mathrm{a}\mathrm{g}\right({\nu }_i\left){\mathrm{\nabla }}_u{g}_k^{\mathrm{T}}\right(x_i,u_i),\\ Q_{uu,i}& :={\mathrm{\nabla }}_{uu}{H}_k(x_i,u_i,{\lambda }_{i+1})\mathrm{\Delta }{\tau }_k\\ & +{\mathrm{\nabla }}_u{g}_k^{\mathrm{T}}(x_i,u_i)\mathrm{d}\mathrm{i}\mathrm{a}\mathrm{g}\left(z_i{)}^{-1}\mathrm{d}\mathrm{i}\mathrm{a}\mathrm{g}\right({\nu }_i\left){\mathrm{\nabla }}_u{g}_k^{\mathrm{T}}\right(x_i,u_i),\\ {\overline{l}}_{x,i}& :=r_{x,i}+{\mathrm{\nabla }}_x{g}_k^{\mathrm{T}}(x_i,u_i)\mathrm{d}\mathrm{i}\mathrm{a}\mathrm{g}\left(z_i{)}^{-1}\right(\mathrm{d}\mathrm{i}\mathrm{a}\mathrm{g}\left({\nu }_i\right)r_{g,i}-r_{z,i}),\\ {\overline{l}}_{u,i}& :=r_{u,i}+{\mathrm{\nabla }}_u{g}_k^{\mathrm{T}}(x_i,u_i)\mathrm{d}\mathrm{i}\mathrm{a}\mathrm{g}\left(z_i{)}^{-1}\right(\mathrm{d}\mathrm{i}\mathrm{a}\mathrm{g}\left({\nu }_i\right)r_{g,i}-r_{z,i}),\\ Q_{tt,k}& :=w_k^{-1}{\upsilon }_k,{\overline{q}}_{t,k}:=-{\upsilon }_k-w_k^{-1}({\upsilon }_k{r}_{\mathrm{\Delta },k}-r_{w,k}),\\ A_i& :=I+{\mathrm{\nabla }}_x{f}_k(x_i,u_i)\mathrm{\Delta }{\tau }_k,B_i:={\mathrm{\nabla }}_x{f}_k(x_i,u_i)\mathrm{\Delta }{\tau }_k,\\ a_i& :=\frac{1}{N_k}{f}_k(x_i,u_i),h_i:=\frac{1}{N_k}{H}_k(x_i,u_i,{\lambda }_{i+1}),\end{array}$and $h_{x,i}:=\frac{1}{N_k}{\mathrm{\nabla }}_x{H}_k(x_i,u_i,{\lambda }_{i+1}),h_{u,i}:=\frac{1}{N_k}{\mathrm{\nabla }}_u{H}_k(x_i,u_i,{\lambda }_{i+1}).$In addition, ${\overline{x}}_i$ is residual in (4c(4c) $\begin{array}{rl}& x_i+f_k(x_i,u_i)\mathrm{\Delta }{\tau }_k-x_{i+1}=0,i\in {\mathcal{I}}_k,k\in \mathcal{K},\end{array}$(4c) ). Note that some of the phase index k is omitted from the KKT system Equation (10(10a) $\begin{array}{rl}& \mathrm{\Delta }{x}_0+x_0-\overline{x}=0,\end{array}$(10a) ) because the phase index k is determined from i, and the matrices and vectors (other than the Newton steps in (10(10a) $\begin{array}{rl}& \mathrm{\Delta }{x}_0+x_0-\overline{x}=0,\end{array}$(10a) )) are fixed once they are computed and therefore do not depend on the phase index k to solve the KKT system. The equations of the KKT system (10a(10a) $\begin{array}{rl}& \mathrm{\Delta }{x}_0+x_0-\overline{x}=0,\end{array}$(10a) )–(10f(10f) $\begin{array}{rl}& \frac{1}{N_k}\sum _{i\in {\mathcal{I}}_k}(h_{x,i}^{\mathrm{T}}\mathrm{\Delta }{x}_i+h_{u,i}^{\mathrm{T}}\mathrm{\Delta }{u}_i+a_i^{\mathrm{T}}\mathrm{\Delta }{\lambda }_{i+1}+{\overline{h}}_i)\\ & -\frac{1}{N_{k+1}}\sum _{i\in {\mathcal{I}}_{k+1}}(h_{x,i}^{\mathrm{T}}\mathrm{\Delta }{x}_i+h_{u,i}^{\mathrm{T}}\mathrm{\Delta }{u}_i+a_i^{\mathrm{T}}\mathrm{\Delta }{\lambda }_{i+1}+{\overline{h}}_i)\\ & +Q_{tt,k}(\mathrm{\Delta }{t}_k-\mathrm{\Delta }{t}_{k-1})-Q_{tt,k+1}(\mathrm{\Delta }{t}_{k+1}-\mathrm{\Delta }{t}_k)\\ & +{\overline{q}}_{t,k}-{\overline{q}}_{t,k+1}=0,k\in \{1,\dots ,K\},\end{array}$(10f) ) correspond to the KKT conditions (4b(4b) $\begin{array}{rl}\mathrm{s}.\mathrm{t}.& x_0-\overline{x}=0,\end{array}$(4b) ), (4c(4c) $\begin{array}{rl}& x_i+f_k(x_i,u_i)\mathrm{\Delta }{\tau }_k-x_{i+1}=0,i\in {\mathcal{I}}_k,k\in \mathcal{K},\end{array}$(4c) ), (9a(9a) $\begin{array}{rl}& r_{x,N}:={\mathrm{\nabla }}_x{V}_f\left(x_N\right)-{\lambda }_N=0,\end{array}$(9a) )–(9c(9c) $\begin{array}{rl}& r_{u,i}:={\mathrm{\nabla }}_u{H}_k(x_i,u_i,{\lambda }_{i+1})\mathrm{\Delta }{\tau }_k\\ & +{\mathrm{\nabla }}_u{g}_k^{\mathrm{T}}(x_i,u_i){\nu }_i=0,i\in {\mathcal{I}}_k,k\in \mathcal{K},\end{array}$(9c) ), and (9f(9f) $\begin{array}{rl}& \frac{1}{N_k}\sum _{i\in {\mathcal{I}}_k}{H}_k(x_i,u_i,{\lambda }_{i+1})\\ & -\frac{1}{N_{k+1}}\sum _{i\in {\mathcal{I}}_{k+1}}{H}_{k+1}(x_i,u_i,{\lambda }_{i+1})\\ & -{\upsilon }_k+{\upsilon }_{k+1}=0,k\in \{1,\dots ,K\}.\end{array}$(9f) ), respectively.

Note that the KKT system (10(10a) $\begin{array}{rl}& \mathrm{\Delta }{x}_0+x_0-\overline{x}=0,\end{array}$(10a) ) is equivalent to the KKT conditions of a QP subproblem (11) $\begin{array}{rl}\underset{\begin{array}{c}\mathrm{\Delta }{u}_0,\dots ,\mathrm{\Delta }{u}_{N-1}\\ \mathrm{\Delta }{t}_1,\dots ,\mathrm{\Delta }{t}_{K+1}\end{array}}{min}& \sum _{k\in \mathcal{K}}\sum _{i\in {\mathcal{I}}_k,i\ne N}\frac{1}{2}\{{\left[\begin{array}{c}\mathrm{\Delta }{t}_k-\mathrm{\Delta }{t}_{k-1}\\ \mathrm{\Delta }{x}_i\\ \mathrm{\Delta }{u}_i\end{array}\right]}^{\mathrm{T}}\\ & \times \left[\begin{array}{ccc}0& h_{x,i}^{\mathrm{T}}& h_{u,i}^{\mathrm{T}}\\ h_{x,i}& Q_{xx,i}& Q_{xu,i}\\ h_{u,i}& Q_{ux,i}& Q_{uu,i}\end{array}\right]\left[\begin{array}{c}\mathrm{\Delta }{t}_k-\mathrm{\Delta }{t}_{k-1}\\ \mathrm{\Delta }{x}_i\\ \mathrm{\Delta }{u}_i\end{array}\right]\\ & +{\left[\begin{array}{c}{\overline{h}}_i\\ {\overline{l}}_{x,i}\\ {\overline{l}}_{u,i}\end{array}\right]}^{\mathrm{T}}\left[\begin{array}{c}\mathrm{\Delta }{t}_k-\mathrm{\Delta }{t}_{k-1}\\ \mathrm{\Delta }{x}_i\\ \mathrm{\Delta }{u}_i\end{array}\right]\}\\ & +\frac{1}{2}\mathrm{\Delta }{x}_N^{\mathrm{T}}{Q}_{xx,N}\mathrm{\Delta }{x}_N+{\overline{l}}_{x,N}^{\mathrm{T}}\mathrm{\Delta }{x}_N\\ & +\sum _{k\in \mathcal{K}}\left\{\frac{1}{2}{Q}_{tt,k}\right(\mathrm{\Delta }{t}_k-\mathrm{\Delta }{t}_{k-1}{)}^2\\ & +{\overline{q}}_{t,k}(\mathrm{\Delta }{t}_k-\mathrm{\Delta }{t}_{k-1})\}\\ \mathrm{s}.\mathrm{t}.& \left(10\mathrm{a}\right),\left(10\mathrm{b}\right),\end{array}$(11) which is a quadratic approximation of the NLP (4(4a) $\begin{array}{rl}\underset{X,U,T}{min}& J=V_f\left(x_N\right)+\sum _{k\in \mathcal{K}}\sum _{i\in {\mathcal{I}}_k}{l}_k(x_i,u_i)\mathrm{\Delta }{\tau }_k,\end{array}$(4a) ). Subsequently, $\mathrm{\Delta }{\lambda }_0,\dots ,\mathrm{\Delta }{\lambda }_N$ can be regarded as the Lagrange multipliers of the QP with respect to (10a(10a) $\begin{array}{rl}& \mathrm{\Delta }{x}_0+x_0-\overline{x}=0,\end{array}$(10a) ) and (10b(10b) $\begin{array}{rl}& A_i\mathrm{\Delta }{x}_i+B_i\mathrm{\Delta }{u}_i+a_i(\mathrm{\Delta }{t}_k-\mathrm{\Delta }{t}_{k-1})\\ & -\mathrm{\Delta }{x}_{i+1}+{\overline{x}}_i=0,i\in {\mathcal{I}}_k,k\in \mathcal{K},\end{array}$(10b) ) (Nocedal & Wright, 2006).

Remark 3.1

The Hessian matrix of (11(11) $\begin{array}{rl}\underset{\begin{array}{c}\mathrm{\Delta }{u}_0,\dots ,\mathrm{\Delta }{u}_{N-1}\\ \mathrm{\Delta }{t}_1,\dots ,\mathrm{\Delta }{t}_{K+1}\end{array}}{min}& \sum _{k\in \mathcal{K}}\sum _{i\in {\mathcal{I}}_k,i\ne N}\frac{1}{2}\{{\left[\begin{array}{c}\mathrm{\Delta }{t}_k-\mathrm{\Delta }{t}_{k-1}\\ \mathrm{\Delta }{x}_i\\ \mathrm{\Delta }{u}_i\end{array}\right]}^{\mathrm{T}}\\ & \times \left[\begin{array}{ccc}0& h_{x,i}^{\mathrm{T}}& h_{u,i}^{\mathrm{T}}\\ h_{x,i}& Q_{xx,i}& Q_{xu,i}\\ h_{u,i}& Q_{ux,i}& Q_{uu,i}\end{array}\right]\left[\begin{array}{c}\mathrm{\Delta }{t}_k-\mathrm{\Delta }{t}_{k-1}\\ \mathrm{\Delta }{x}_i\\ \mathrm{\Delta }{u}_i\end{array}\right]\\ & +{\left[\begin{array}{c}{\overline{h}}_i\\ {\overline{l}}_{x,i}\\ {\overline{l}}_{u,i}\end{array}\right]}^{\mathrm{T}}\left[\begin{array}{c}\mathrm{\Delta }{t}_k-\mathrm{\Delta }{t}_{k-1}\\ \mathrm{\Delta }{x}_i\\ \mathrm{\Delta }{u}_i\end{array}\right]\}\\ & +\frac{1}{2}\mathrm{\Delta }{x}_N^{\mathrm{T}}{Q}_{xx,N}\mathrm{\Delta }{x}_N+{\overline{l}}_{x,N}^{\mathrm{T}}\mathrm{\Delta }{x}_N\\ & +\sum _{k\in \mathcal{K}}\left\{\frac{1}{2}{Q}_{tt,k}\right(\mathrm{\Delta }{t}_k-\mathrm{\Delta }{t}_{k-1}{)}^2\\ & +{\overline{q}}_{t,k}(\mathrm{\Delta }{t}_k-\mathrm{\Delta }{t}_{k-1})\}\\ \mathrm{s}.\mathrm{t}.& \left(10\mathrm{a}\right),\left(10\mathrm{b}\right),\end{array}$(11) ) is inherently indefinite, which makes solving the KKT system (10(10a) $\begin{array}{rl}& \mathrm{\Delta }{x}_0+x_0-\overline{x}=0,\end{array}$(10a) ) difficult when using off-the-shelf QP solvers because they typically require a positive definite Hessian matrix. This can be explained with a block diagonal of the Hessian matrix (11(11) $\begin{array}{rl}\underset{\begin{array}{c}\mathrm{\Delta }{u}_0,\dots ,\mathrm{\Delta }{u}_{N-1}\\ \mathrm{\Delta }{t}_1,\dots ,\mathrm{\Delta }{t}_{K+1}\end{array}}{min}& \sum _{k\in \mathcal{K}}\sum _{i\in {\mathcal{I}}_k,i\ne N}\frac{1}{2}\{{\left[\begin{array}{c}\mathrm{\Delta }{t}_k-\mathrm{\Delta }{t}_{k-1}\\ \mathrm{\Delta }{x}_i\\ \mathrm{\Delta }{u}_i\end{array}\right]}^{\mathrm{T}}\\ & \times \left[\begin{array}{ccc}0& h_{x,i}^{\mathrm{T}}& h_{u,i}^{\mathrm{T}}\\ h_{x,i}& Q_{xx,i}& Q_{xu,i}\\ h_{u,i}& Q_{ux,i}& Q_{uu,i}\end{array}\right]\left[\begin{array}{c}\mathrm{\Delta }{t}_k-\mathrm{\Delta }{t}_{k-1}\\ \mathrm{\Delta }{x}_i\\ \mathrm{\Delta }{u}_i\end{array}\right]\\ & +{\left[\begin{array}{c}{\overline{h}}_i\\ {\overline{l}}_{x,i}\\ {\overline{l}}_{u,i}\end{array}\right]}^{\mathrm{T}}\left[\begin{array}{c}\mathrm{\Delta }{t}_k-\mathrm{\Delta }{t}_{k-1}\\ \mathrm{\Delta }{x}_i\\ \mathrm{\Delta }{u}_i\end{array}\right]\}\\ & +\frac{1}{2}\mathrm{\Delta }{x}_N^{\mathrm{T}}{Q}_{xx,N}\mathrm{\Delta }{x}_N+{\overline{l}}_{x,N}^{\mathrm{T}}\mathrm{\Delta }{x}_N\\ & +\sum _{k\in \mathcal{K}}\left\{\frac{1}{2}{Q}_{tt,k}\right(\mathrm{\Delta }{t}_k-\mathrm{\Delta }{t}_{k-1}{)}^2\\ & +{\overline{q}}_{t,k}(\mathrm{\Delta }{t}_k-\mathrm{\Delta }{t}_{k-1})\}\\ \mathrm{s}.\mathrm{t}.& \left(10\mathrm{a}\right),\left(10\mathrm{b}\right),\end{array}$(11) ), expressed as (12) $\begin{array}{r}\left[\begin{array}{ccc}0& h_{x,i}^{\mathrm{T}}& h_{u,i}^{\mathrm{T}}\\ h_{x,i}& Q_{xx,i}& Q_{uu,i}\\ h_{u,i}& Q_{xu,i}^{\mathrm{T}}& Q_{uu,i}\end{array}\right].\end{array}$(12) This is indefinite due to the off-diagonal terms $h_{x,i}$ and $h_{u,i}$, even when: $\left[\begin{array}{cc}{Q}_{xx,i}& Q_{uu,i}\\ Q_{xu,i}^{\mathrm{T}}& Q_{uu,i}\end{array}\right]\succ O$.

4. Riccati recursion to solve KKT systems

In this section, a Riccati recursion algorithm is presented to compute the Newton step of the NLP (4(4a) $\begin{array}{rl}\underset{X,U,T}{min}& J=V_f\left(x_N\right)+\sum _{k\in \mathcal{K}}\sum _{i\in {\mathcal{I}}_k}{l}_k(x_i,u_i)\mathrm{\Delta }{\tau }_k,\end{array}$(4a) ) by solving the KKT system (10(10a) $\begin{array}{rl}& \mathrm{\Delta }{x}_0+x_0-\overline{x}=0,\end{array}$(10a) ). The sparsity structure of the KKT system (10(10a) $\begin{array}{rl}& \mathrm{\Delta }{x}_0+x_0-\overline{x}=0,\end{array}$(10a) ) is no longer the same as the standard OCP, which prevents applying the off-the-shelf efficient Newton-type algorithms for OCPs (Rao et al., 1998; Wang & Boyd, 2010; Zanelli et al., 2020). Moreover, as stated in Remark 3.1, the Hessian matrix of the KKT system is indefinite and unsolvable using general QP solvers. Motivated by these problems, we propose a Riccati recursion algorithm that efficiently solves the KKT system, specifically with $O\left(N\right)$ computational time, whenever the SOSC holds. Moreover, a reduced Hessian modification method is proposed to enhance the convergence when the SOSC does not hold, that is, when the reduced Hessian matrix is indefinite. As the standard Riccati recursion (Rao et al., 1998), the proposed method is composed of backward and forward recursions, which recursively eliminate the variables from the KKT system (10(10a) $\begin{array}{rl}& \mathrm{\Delta }{x}_0+x_0-\overline{x}=0,\end{array}$(10a) ) backward in time and recursively compute the Newton step forward in time, respectively.

4.1. Backward recursion

In the backward recursion, the Newton steps are recursively eliminated from stages N to 0. Specifically, expressions of $\mathrm{\Delta }{x}_{i+1}$, $\mathrm{\Delta }{u}_i$, and $\mathrm{\Delta }{\lambda }_i$ are derived at each stage $i\in I_k$ for $\mathrm{\Delta }{x}_i$, $\mathrm{\Delta }{t}_k$, and $\mathrm{\Delta }{t}_{k-1}$, respectively, from (10b(10b) $\begin{array}{rl}& A_i\mathrm{\Delta }{x}_i+B_i\mathrm{\Delta }{u}_i+a_i(\mathrm{\Delta }{t}_k-\mathrm{\Delta }{t}_{k-1})\\ & -\mathrm{\Delta }{x}_{i+1}+{\overline{x}}_i=0,i\in {\mathcal{I}}_k,k\in \mathcal{K},\end{array}$(10b) )–(10e(10e) $\begin{array}{rl}& Q_{xu,i}^{\mathrm{T}}\mathrm{\Delta }{x}_i+Q_{uu,i}\mathrm{\Delta }{u}_i+B_i^{\mathrm{T}}\mathrm{\Delta }{\lambda }_{i+1}\\ & +h_{u,i}(\mathrm{\Delta }{t}_k-\mathrm{\Delta }{t}_{k-1})+{\overline{l}}_{u,i}=0,i\in {\mathcal{I}}_k,k\in \mathcal{K},\end{array}$(10e) ). Moreover, variables are recursively eliminated from (10f(10f) $\begin{array}{rl}& \frac{1}{N_k}\sum _{i\in {\mathcal{I}}_k}(h_{x,i}^{\mathrm{T}}\mathrm{\Delta }{x}_i+h_{u,i}^{\mathrm{T}}\mathrm{\Delta }{u}_i+a_i^{\mathrm{T}}\mathrm{\Delta }{\lambda }_{i+1}+{\overline{h}}_i)\\ & -\frac{1}{N_{k+1}}\sum _{i\in {\mathcal{I}}_{k+1}}(h_{x,i}^{\mathrm{T}}\mathrm{\Delta }{x}_i+h_{u,i}^{\mathrm{T}}\mathrm{\Delta }{u}_i+a_i^{\mathrm{T}}\mathrm{\Delta }{\lambda }_{i+1}+{\overline{h}}_i)\\ & +Q_{tt,k}(\mathrm{\Delta }{t}_k-\mathrm{\Delta }{t}_{k-1})-Q_{tt,k+1}(\mathrm{\Delta }{t}_{k+1}-\mathrm{\Delta }{t}_k)\\ & +{\overline{q}}_{t,k}-{\overline{q}}_{t,k+1}=0,k\in \{1,\dots ,K\},\end{array}$(10f) ) using these expressions. When the stage of interest changes from k to k−1, $\mathrm{\Delta }{t}_k$ is further eliminated from (10f(10f) $\begin{array}{rl}& \frac{1}{N_k}\sum _{i\in {\mathcal{I}}_k}(h_{x,i}^{\mathrm{T}}\mathrm{\Delta }{x}_i+h_{u,i}^{\mathrm{T}}\mathrm{\Delta }{u}_i+a_i^{\mathrm{T}}\mathrm{\Delta }{\lambda }_{i+1}+{\overline{h}}_i)\\ & -\frac{1}{N_{k+1}}\sum _{i\in {\mathcal{I}}_{k+1}}(h_{x,i}^{\mathrm{T}}\mathrm{\Delta }{x}_i+h_{u,i}^{\mathrm{T}}\mathrm{\Delta }{u}_i+a_i^{\mathrm{T}}\mathrm{\Delta }{\lambda }_{i+1}+{\overline{h}}_i)\\ & +Q_{tt,k}(\mathrm{\Delta }{t}_k-\mathrm{\Delta }{t}_{k-1})-Q_{tt,k+1}(\mathrm{\Delta }{t}_{k+1}-\mathrm{\Delta }{t}_k)\\ & +{\overline{q}}_{t,k}-{\overline{q}}_{t,k+1}=0,k\in \{1,\dots ,K\},\end{array}$(10f) ). That is, an expression of $\mathrm{\Delta }{t}_k$ is derived with respect to $\mathrm{\Delta }{x}_{i_k}$ and $\mathrm{\Delta }{t}_{k-1}$, where $i_k:=min{\mathcal{I}}_k$. This can be seen as the extension of the standard Riccati recursion that also recursively derives expressions of $\mathrm{\Delta }{x}_{i+1}$, $\mathrm{\Delta }{u}_i$, and $\mathrm{\Delta }{\lambda }_i$ with respect to $\mathrm{\Delta }{x}_i$ (Rao et al., 1998; Rawlings et al., 2017).

4.1.1. Terminal stage

From (10c(10c) $\begin{array}{rl}& Q_{xx,N}\mathrm{\Delta }{x}_N-\mathrm{\Delta }{\lambda }_N+{\overline{l}}_{x,N}=0,\end{array}$(10c) ) at stage N, we obtain: (13a) $\begin{array}{r}\mathrm{\Delta }{\lambda }_N=P_N\mathrm{\Delta }{x}_N-s_N,\end{array}$(13a) where (13b) $\begin{array}{r}{P}_N=Q_{xx,N},s_N=-{\overline{l}}_{x,N}.\end{array}$(13b)

4.1.2. Intermediate stages

Consider $i,i+1\in {\mathcal{I}}_k$, $k\in \mathcal{K}$. Suppose that we have the expression of $\mathrm{\Delta }{\lambda }_{i+1}$ with respect to $\mathrm{\Delta }{x}_{i+1}$, $\mathrm{\Delta }{t}_k$, and $\mathrm{\Delta }{t}_{k-1}$ as (14a) $\begin{array}{rl}\mathrm{\Delta }{\lambda }_{i+1}& =P_{i+1}\mathrm{\Delta }{x}_{i+1}-s_{i+1}+{\mathrm{\Psi }}_{i+1}(\mathrm{\Delta }{t}_k-\mathrm{\Delta }{t}_{k-1})\\ & +{\mathrm{\Phi }}_{i+1}(-\mathrm{\Delta }{t}_k),\end{array}$(14a) where $P_{i+1}\in {\mathbb{R}}^{n_x\times n_x}$ and $s_{i+1},{\mathrm{\Psi }}_{i+1},{\mathrm{\Phi }}_{i+1}\in {\mathbb{R}}^{n_x}$. Furthermore, suppose that we have Equation (10f(10f) $\begin{array}{rl}& \frac{1}{N_k}\sum _{i\in {\mathcal{I}}_k}(h_{x,i}^{\mathrm{T}}\mathrm{\Delta }{x}_i+h_{u,i}^{\mathrm{T}}\mathrm{\Delta }{u}_i+a_i^{\mathrm{T}}\mathrm{\Delta }{\lambda }_{i+1}+{\overline{h}}_i)\\ & -\frac{1}{N_{k+1}}\sum _{i\in {\mathcal{I}}_{k+1}}(h_{x,i}^{\mathrm{T}}\mathrm{\Delta }{x}_i+h_{u,i}^{\mathrm{T}}\mathrm{\Delta }{u}_i+a_i^{\mathrm{T}}\mathrm{\Delta }{\lambda }_{i+1}+{\overline{h}}_i)\\ & +Q_{tt,k}(\mathrm{\Delta }{t}_k-\mathrm{\Delta }{t}_{k-1})-Q_{tt,k+1}(\mathrm{\Delta }{t}_{k+1}-\mathrm{\Delta }{t}_k)\\ & +{\overline{q}}_{t,k}-{\overline{q}}_{t,k+1}=0,k\in \{1,\dots ,K\},\end{array}$(10f) ) for k and k−1 in which $\mathrm{\Delta }{x}_j$ for $j\ge i+2$, $\mathrm{\Delta }{u}_j$ for $j\ge i+1$, and $\mathrm{\Delta }{\lambda }_j$ for $j\ge i+1$ are eliminated, that is, Equation (10f(10f) $\begin{array}{rl}& \frac{1}{N_k}\sum _{i\in {\mathcal{I}}_k}(h_{x,i}^{\mathrm{T}}\mathrm{\Delta }{x}_i+h_{u,i}^{\mathrm{T}}\mathrm{\Delta }{u}_i+a_i^{\mathrm{T}}\mathrm{\Delta }{\lambda }_{i+1}+{\overline{h}}_i)\\ & -\frac{1}{N_{k+1}}\sum _{i\in {\mathcal{I}}_{k+1}}(h_{x,i}^{\mathrm{T}}\mathrm{\Delta }{x}_i+h_{u,i}^{\mathrm{T}}\mathrm{\Delta }{u}_i+a_i^{\mathrm{T}}\mathrm{\Delta }{\lambda }_{i+1}+{\overline{h}}_i)\\ & +Q_{tt,k}(\mathrm{\Delta }{t}_k-\mathrm{\Delta }{t}_{k-1})-Q_{tt,k+1}(\mathrm{\Delta }{t}_{k+1}-\mathrm{\Delta }{t}_k)\\ & +{\overline{q}}_{t,k}-{\overline{q}}_{t,k+1}=0,k\in \{1,\dots ,K\},\end{array}$(10f) ) for k and k−1 are reduced to (14b) $\begin{array}{rl}& \sum _{j\in {\mathcal{I}}_k,j\le i}(h_{x,j}^{\mathrm{T}}\mathrm{\Delta }{x}_j+h_{u,j}^{\mathrm{T}}\mathrm{\Delta }{u}_j+a_j^{\mathrm{T}}\mathrm{\Delta }{\lambda }_{j+1}+{\overline{h}}_j)\\ & +{\mathrm{\Psi }}_{i+1}^{\mathrm{T}}\mathrm{\Delta }{x}_{i+1}+{\xi }_{i+1}(\mathrm{\Delta }{t}_k-\mathrm{\Delta }{t}_{k-1})+{\chi }_{i+1}(-\mathrm{\Delta }{t}_k)+{\eta }_{i+1}\\ & -{\mathrm{\Phi }}_{i+1}^{\mathrm{T}}\mathrm{\Delta }{x}_{i+1}-{\chi }_{i+1}(\mathrm{\Delta }{t}_k-\mathrm{\Delta }{t}_{k-1})\\ & -{\rho }_{i+1}(-\mathrm{\Delta }{t}_k)-{\iota }_{i+1}=0,\end{array}$(14b) and (14c) $\begin{array}{rl}& \sum _{j\in {\mathcal{I}}_{k-1}}(h_{x,j}^{\mathrm{T}}\mathrm{\Delta }{x}_j+h_{u,j}^{\mathrm{T}}\mathrm{\Delta }{u}_j+a_j^{\mathrm{T}}\mathrm{\Delta }{\lambda }_{j+1}+{\overline{h}}_j)\\ & -\sum _{j\in {\mathcal{I}}_k,j\le i}(h_{x,j}^{\mathrm{T}}\mathrm{\Delta }{x}_j+h_{u,j}^{\mathrm{T}}\mathrm{\Delta }{u}_j+a_j^{\mathrm{T}}\mathrm{\Delta }{\lambda }_{j+1}+{\overline{h}}_j)\\ & +Q_{tt,k-1}(\mathrm{\Delta }{t}_{k-1}-\mathrm{\Delta }{t}_{k-2})+{\overline{q}}_{t,k-1}\\ & -{\mathrm{\Psi }}_{i+1}^{\mathrm{T}}\mathrm{\Delta }{x}_{i+1}-{\xi }_{i+1}(\mathrm{\Delta }{t}_k-\mathrm{\Delta }{t}_{k-1})\\ & -{\chi }_{i+1}(-\mathrm{\Delta }{t}_k)-{\eta }_{i+1}=0,\end{array}$(14c) where ${\xi }_{i+1},{\chi }_{i+1},{\rho }_{i+1},{\eta }_{i+1},{\iota }_{i+1}\in \mathbb{R}$. Note that ${\mathrm{\Psi }}_{i+1}=0$, ${\mathrm{\Phi }}_{i+1}=0$, ${\xi }_{i+1}=Q_{tt,K+1}$, ${\chi }_{i+1}=0$, ${\rho }_{i+1}=0$, ${\eta }_{i+1}={\overline{q}}_{t,k+1}$, and ${\iota }_{i+1}=0$ at stage $i=N-1$. Moreover, ${\mathrm{\Psi }}_{i+1}=0$, ${\xi }_{i+1}=Q_{tt,k+1}$, ${\chi }_{i+1}=0$, and ${\eta }_{i+1}={\overline{q}}_{t,k+1}$ at stages $i=min{\mathcal{I}}_k-1$ and $k\in \{2,\dots ,K\}$, as explained in 4.1.3. First, the following equations are introduced: (15a) $\begin{array}{rl}& F_i:=Q_{xx,i}+A_i^{\mathrm{T}}{P}_{i+1}{A}_i,\end{array}$(15a) (15b) $\begin{array}{rl}& H_i:=Q_{xu,i}+A_i^{\mathrm{T}}{P}_{i+1}{B}_i,\end{array}$(15b) (15c) $\begin{array}{rl}& G_i:=Q_{uu,i}+B_i^{\mathrm{T}}{P}_{i+1}{B}_i,\end{array}$(15c) (15d) $\begin{array}{rl}& {\psi }_{x,i}:=h_{x,i}+A_i^{\mathrm{T}}{P}_{i+1}{a}_i+A_i^{\mathrm{T}}{\mathrm{\Psi }}_{i+1},\end{array}$(15d) (15e) $\begin{array}{rl}& {\psi }_{u,i}:=h_{u,i}+B_i^{\mathrm{T}}{P}_{i+1}{a}_i+B_i^{\mathrm{T}}{\mathrm{\Psi }}_{i+1},\end{array}$(15e) and (15f) $\begin{array}{r}{\varphi }_{x,i}:=A_i^{\mathrm{T}}{\mathrm{\Phi }}_{i+1},{\varphi }_{u,i}:=B_i^{\mathrm{T}}{\mathrm{\Phi }}_{i+1}.\end{array}$(15f) Second, $\mathrm{\Delta }{\lambda }_{i+1}$ and $\mathrm{\Delta }{x}_{i+1}$ are eliminated from (10e(10e) $\begin{array}{rl}& Q_{xu,i}^{\mathrm{T}}\mathrm{\Delta }{x}_i+Q_{uu,i}\mathrm{\Delta }{u}_i+B_i^{\mathrm{T}}\mathrm{\Delta }{\lambda }_{i+1}\\ & +h_{u,i}(\mathrm{\Delta }{t}_k-\mathrm{\Delta }{t}_{k-1})+{\overline{l}}_{u,i}=0,i\in {\mathcal{I}}_k,k\in \mathcal{K},\end{array}$(10e) ) using (14a(14a) $\begin{array}{rl}\mathrm{\Delta }{\lambda }_{i+1}& =P_{i+1}\mathrm{\Delta }{x}_{i+1}-s_{i+1}+{\mathrm{\Psi }}_{i+1}(\mathrm{\Delta }{t}_k-\mathrm{\Delta }{t}_{k-1})\\ & +{\mathrm{\Phi }}_{i+1}(-\mathrm{\Delta }{t}_k),\end{array}$(14a) ) and (10b(10b) $\begin{array}{rl}& A_i\mathrm{\Delta }{x}_i+B_i\mathrm{\Delta }{u}_i+a_i(\mathrm{\Delta }{t}_k-\mathrm{\Delta }{t}_{k-1})\\ & -\mathrm{\Delta }{x}_{i+1}+{\overline{x}}_i=0,i\in {\mathcal{I}}_k,k\in \mathcal{K},\end{array}$(10b) ). Subsequently, we can express $\mathrm{\Delta }{u}_i$ using $\mathrm{\Delta }{x}_i$, $\mathrm{\Delta }{t}_k$, and $\mathrm{\Delta }{t}_{k-1}$ as (16a) $\begin{array}{r}\mathrm{\Delta }{u}_i=K_i\mathrm{\Delta }{x}_i+k_i+T_i(\mathrm{\Delta }{t}_k-\mathrm{\Delta }{t}_{k-1})+W_i(-\mathrm{\Delta }{t}_k),\end{array}$(16a) where (16b) $\begin{array}{rl}& K_i:=-G_i^{-1}{H}_i^{\mathrm{T}},\end{array}$(16b) (16c) $\begin{array}{rl}& k_i:=-G_i^{-1}(B_i^{\mathrm{T}}{P}_{i+1}{\overline{x}}_i-B_i^{\mathrm{T}}{z}_{i+1}+{\overline{l}}_{u,i}),\end{array}$(16c) and (16d) $\begin{array}{r}{T}_i:=-G_i^{-1}{\psi }_{u,i},W_i:=-G_i^{-1}{\varphi }_{u,i}.\end{array}$(16d) Third, $\mathrm{\Delta }{\lambda }_{i+1}$, $\mathrm{\Delta }{x}_{i+1}$, and $\mathrm{\Delta }{u}_i$ are eliminated from (10d(10d) $\begin{array}{rl}& Q_{xx,i}\mathrm{\Delta }{x}_i+Q_{xu,i}\mathrm{\Delta }{u}_i+A_i^{\mathrm{T}}\mathrm{\Delta }{\lambda }_{i+1}\\ & -\mathrm{\Delta }{\lambda }_i+h_{x,i}(\mathrm{\Delta }{t}_k-\mathrm{\Delta }{t}_{k-1})+{\overline{l}}_{x,i}=0,\\ & i\in {\mathcal{I}}_k,k\in \mathcal{K},\end{array}$(10d) ) using (14a(14a) $\begin{array}{rl}\mathrm{\Delta }{\lambda }_{i+1}& =P_{i+1}\mathrm{\Delta }{x}_{i+1}-s_{i+1}+{\mathrm{\Psi }}_{i+1}(\mathrm{\Delta }{t}_k-\mathrm{\Delta }{t}_{k-1})\\ & +{\mathrm{\Phi }}_{i+1}(-\mathrm{\Delta }{t}_k),\end{array}$(14a) ), (10b(10b) $\begin{array}{rl}& A_i\mathrm{\Delta }{x}_i+B_i\mathrm{\Delta }{u}_i+a_i(\mathrm{\Delta }{t}_k-\mathrm{\Delta }{t}_{k-1})\\ & -\mathrm{\Delta }{x}_{i+1}+{\overline{x}}_i=0,i\in {\mathcal{I}}_k,k\in \mathcal{K},\end{array}$(10b) ), and (16a(16a) $\begin{array}{r}\mathrm{\Delta }{u}_i=K_i\mathrm{\Delta }{x}_i+k_i+T_i(\mathrm{\Delta }{t}_k-\mathrm{\Delta }{t}_{k-1})+W_i(-\mathrm{\Delta }{t}_k),\end{array}$(16a) ), respectively. As a result, $\mathrm{\Delta }{\lambda }_i$ using $\mathrm{\Delta }{x}_i$, $\mathrm{\Delta }{t}_k$, and $\mathrm{\Delta }{t}_{k-1}$ is expressed as (17a) $\begin{array}{r}\mathrm{\Delta }{\lambda }_i=P_i\mathrm{\Delta }{x}_i-s_i+{\mathrm{\Psi }}_i(\mathrm{\Delta }{t}_k-\mathrm{\Delta }{t}_{k-1})+{\mathrm{\Phi }}_i(-\mathrm{\Delta }{t}_k),\end{array}$(17a) where (17b) $\begin{array}{rl}& P_i:=F_i-K_i^{\mathrm{T}}{G}_i{K}_i,\end{array}$(17b) (17c) $\begin{array}{rl}& s_i:=-\{{\overline{l}}_{x,i}+A_i^{\mathrm{T}}(P_{i+1}{\overline{x}}_i-s_{i+1})+H_i{k}_i\},\end{array}$(17c) and (17d) $\begin{array}{r}{\mathrm{\Psi }}_i:={\psi }_{x,i}+K_i{\psi }_{u,i},{\mathrm{\Phi }}_i:={\varphi }_{x,i}+K_i{\varphi }_{u,i}.\end{array}$(17d) Moreover, by eliminating $\mathrm{\Delta }{\lambda }_{i+1}$, $\mathrm{\Delta }{x}_{i+1}$, and $\mathrm{\Delta }{u}_i$ from (14b(14b) $\begin{array}{rl}& \sum _{j\in {\mathcal{I}}_k,j\le i}(h_{x,j}^{\mathrm{T}}\mathrm{\Delta }{x}_j+h_{u,j}^{\mathrm{T}}\mathrm{\Delta }{u}_j+a_j^{\mathrm{T}}\mathrm{\Delta }{\lambda }_{j+1}+{\overline{h}}_j)\\ & +{\mathrm{\Psi }}_{i+1}^{\mathrm{T}}\mathrm{\Delta }{x}_{i+1}+{\xi }_{i+1}(\mathrm{\Delta }{t}_k-\mathrm{\Delta }{t}_{k-1})+{\chi }_{i+1}(-\mathrm{\Delta }{t}_k)+{\eta }_{i+1}\\ & -{\mathrm{\Phi }}_{i+1}^{\mathrm{T}}\mathrm{\Delta }{x}_{i+1}-{\chi }_{i+1}(\mathrm{\Delta }{t}_k-\mathrm{\Delta }{t}_{k-1})\\ & -{\rho }_{i+1}(-\mathrm{\Delta }{t}_k)-{\iota }_{i+1}=0,\end{array}$(14b) ) and (14c(14c) $\begin{array}{rl}& \sum _{j\in {\mathcal{I}}_{k-1}}(h_{x,j}^{\mathrm{T}}\mathrm{\Delta }{x}_j+h_{u,j}^{\mathrm{T}}\mathrm{\Delta }{u}_j+a_j^{\mathrm{T}}\mathrm{\Delta }{\lambda }_{j+1}+{\overline{h}}_j)\\ & -\sum _{j\in {\mathcal{I}}_k,j\le i}(h_{x,j}^{\mathrm{T}}\mathrm{\Delta }{x}_j+h_{u,j}^{\mathrm{T}}\mathrm{\Delta }{u}_j+a_j^{\mathrm{T}}\mathrm{\Delta }{\lambda }_{j+1}+{\overline{h}}_j)\\ & +Q_{tt,k-1}(\mathrm{\Delta }{t}_{k-1}-\mathrm{\Delta }{t}_{k-2})+{\overline{q}}_{t,k-1}\\ & -{\mathrm{\Psi }}_{i+1}^{\mathrm{T}}\mathrm{\Delta }{x}_{i+1}-{\xi }_{i+1}(\mathrm{\Delta }{t}_k-\mathrm{\Delta }{t}_{k-1})\\ & -{\chi }_{i+1}(-\mathrm{\Delta }{t}_k)-{\eta }_{i+1}=0,\end{array}$(14c) ) using (14a(14a) $\begin{array}{rl}\mathrm{\Delta }{\lambda }_{i+1}& =P_{i+1}\mathrm{\Delta }{x}_{i+1}-s_{i+1}+{\mathrm{\Psi }}_{i+1}(\mathrm{\Delta }{t}_k-\mathrm{\Delta }{t}_{k-1})\\ & +{\mathrm{\Phi }}_{i+1}(-\mathrm{\Delta }{t}_k),\end{array}$(14a) ), (10b(10b) $\begin{array}{rl}& A_i\mathrm{\Delta }{x}_i+B_i\mathrm{\Delta }{u}_i+a_i(\mathrm{\Delta }{t}_k-\mathrm{\Delta }{t}_{k-1})\\ & -\mathrm{\Delta }{x}_{i+1}+{\overline{x}}_i=0,i\in {\mathcal{I}}_k,k\in \mathcal{K},\end{array}$(10b) ), and (16a(16a) $\begin{array}{r}\mathrm{\Delta }{u}_i=K_i\mathrm{\Delta }{x}_i+k_i+T_i(\mathrm{\Delta }{t}_k-\mathrm{\Delta }{t}_{k-1})+W_i(-\mathrm{\Delta }{t}_k),\end{array}$(16a) ), we obtain: (18a) $\begin{array}{rl}& \sum _{j\in {\mathcal{I}}_k,j\le i-1}(h_{x,j}^{\mathrm{T}}\mathrm{\Delta }{x}_j+h_{u,j}^{\mathrm{T}}\mathrm{\Delta }{u}_j+a_j^{\mathrm{T}}\mathrm{\Delta }{\lambda }_{j+1}+{\overline{h}}_j)\\ & +{\mathrm{\Psi }}_i^{\mathrm{T}}\mathrm{\Delta }{x}_i+{\xi }_i(\mathrm{\Delta }{t}_k-\mathrm{\Delta }{t}_{k-1})+{\chi }_i(-\mathrm{\Delta }{t}_k)+{\eta }_i\\ & -{\mathrm{\Phi }}_i^{\mathrm{T}}\mathrm{\Delta }{x}_i-{\chi }_i(\mathrm{\Delta }{t}_k-\mathrm{\Delta }{t}_{k-1})-{\rho }_i(-\mathrm{\Delta }{t}_k)-{\iota }_i=0\end{array}$(18a) and (18b) $\begin{array}{rl}& \sum _{j\in {\mathcal{I}}_{k-1}}(h_{x,j}^{\mathrm{T}}\mathrm{\Delta }{x}_j+h_{u,j}^{\mathrm{T}}\mathrm{\Delta }{u}_j+a_j^{\mathrm{T}}\mathrm{\Delta }{\lambda }_{j+1}+{\overline{h}}_j)\\ & -\sum _{j\in {\mathcal{I}}_k,j\le i-1}(h_{x,j}^{\mathrm{T}}\mathrm{\Delta }{x}_j+h_{u,j}^{\mathrm{T}}\mathrm{\Delta }{u}_j+a_j^{\mathrm{T}}\mathrm{\Delta }{\lambda }_{j+1}+{\overline{h}}_j)\\ & +Q_{tt,k-1}(\mathrm{\Delta }{t}_{k-1}-\mathrm{\Delta }{t}_{k-2})+{\overline{q}}_{t,k-1}\\ & -{\mathrm{\Psi }}_i^{\mathrm{T}}\mathrm{\Delta }{x}_i-{\xi }_i(\mathrm{\Delta }{t}_k-\mathrm{\Delta }{t}_{k-1})-{\chi }_i(-\mathrm{\Delta }{t}_k)-{\eta }_i=0,\end{array}$(18b) where (19a) $\begin{array}{rl}& {\xi }_i:={\xi }_{i+1}+a_i^{\mathrm{T}}(P_{i+1}{a}_i+2{\mathrm{\Psi }}_{i+1})+{\psi }_{u,i}^{\mathrm{T}}{T}_i,\end{array}$(19a) (19b) $\begin{array}{rl}& {\eta }_i:={\eta }_{i+1}+{\overline{h}}_i+a_i^{\mathrm{T}}(P_{i+1}{\overline{x}}_i-s_{i+1})+{\mathrm{\Psi }}_{i+1}^{\mathrm{T}}{\overline{x}}_i+{\psi }_{u,i}^{\mathrm{T}}{k}_i,\end{array}$(19b) (19c) $\begin{array}{rl}& {\chi }_i:={\chi }_{i+1}+{\mathrm{\Phi }}_{i+1}^{\mathrm{T}}{a}_i+{\psi }_{u,i}^{\mathrm{T}}{W}_i,\end{array}$(19c) (19d) $\begin{array}{rl}& {\rho }_i:={\rho }_{i+1}+{\varphi }_{u,i}^{\mathrm{T}}{W}_i,\end{array}$(19d) and (19e) $\begin{array}{r}{\iota }_i:={\iota }_{i+1}+{\mathrm{\Phi }}_{i+1}^{\mathrm{T}}{\overline{x}}_i+{\varphi }_{u,i}^{\mathrm{T}}{k}_i.\end{array}$(19e) Therefore, we have Equations (17a(17a) $\begin{array}{r}\mathrm{\Delta }{\lambda }_i=P_i\mathrm{\Delta }{x}_i-s_i+{\mathrm{\Psi }}_i(\mathrm{\Delta }{t}_k-\mathrm{\Delta }{t}_{k-1})+{\mathrm{\Phi }}_i(-\mathrm{\Delta }{t}_k),\end{array}$(17a) ), (18a(18a) $\begin{array}{rl}& \sum _{j\in {\mathcal{I}}_k,j\le i-1}(h_{x,j}^{\mathrm{T}}\mathrm{\Delta }{x}_j+h_{u,j}^{\mathrm{T}}\mathrm{\Delta }{u}_j+a_j^{\mathrm{T}}\mathrm{\Delta }{\lambda }_{j+1}+{\overline{h}}_j)\\ & +{\mathrm{\Psi }}_i^{\mathrm{T}}\mathrm{\Delta }{x}_i+{\xi }_i(\mathrm{\Delta }{t}_k-\mathrm{\Delta }{t}_{k-1})+{\chi }_i(-\mathrm{\Delta }{t}_k)+{\eta }_i\\ & -{\mathrm{\Phi }}_i^{\mathrm{T}}\mathrm{\Delta }{x}_i-{\chi }_i(\mathrm{\Delta }{t}_k-\mathrm{\Delta }{t}_{k-1})-{\rho }_i(-\mathrm{\Delta }{t}_k)-{\iota }_i=0\end{array}$(18a) ), and (18b(18b) $\begin{array}{rl}& \sum _{j\in {\mathcal{I}}_{k-1}}(h_{x,j}^{\mathrm{T}}\mathrm{\Delta }{x}_j+h_{u,j}^{\mathrm{T}}\mathrm{\Delta }{u}_j+a_j^{\mathrm{T}}\mathrm{\Delta }{\lambda }_{j+1}+{\overline{h}}_j)\\ & -\sum _{j\in {\mathcal{I}}_k,j\le i-1}(h_{x,j}^{\mathrm{T}}\mathrm{\Delta }{x}_j+h_{u,j}^{\mathrm{T}}\mathrm{\Delta }{u}_j+a_j^{\mathrm{T}}\mathrm{\Delta }{\lambda }_{j+1}+{\overline{h}}_j)\\ & +Q_{tt,k-1}(\mathrm{\Delta }{t}_{k-1}-\mathrm{\Delta }{t}_{k-2})+{\overline{q}}_{t,k-1}\\ & -{\mathrm{\Psi }}_i^{\mathrm{T}}\mathrm{\Delta }{x}_i-{\xi }_i(\mathrm{\Delta }{t}_k-\mathrm{\Delta }{t}_{k-1})-{\chi }_i(-\mathrm{\Delta }{t}_k)-{\eta }_i=0,\end{array}$(18b) ) for stage i having the same form as that of Equation (14(14a) $\begin{array}{rl}\mathrm{\Delta }{\lambda }_{i+1}& =P_{i+1}\mathrm{\Delta }{x}_{i+1}-s_{i+1}+{\mathrm{\Psi }}_{i+1}(\mathrm{\Delta }{t}_k-\mathrm{\Delta }{t}_{k-1})\\ & +{\mathrm{\Phi }}_{i+1}(-\mathrm{\Delta }{t}_k),\end{array}$(14a) ) for stage i + 1.

4.1.3. Phase transition stages

Here, we consider phase $k\in \{1,\dots ,K\}$ and stage $i_k:=min{\mathcal{I}}_k$. In this stage, the phase of interest changes from k to k−1 when $k>1$, and the backward recursion terminates when k = 1. Equation (14(14a) $\begin{array}{rl}\mathrm{\Delta }{\lambda }_{i+1}& =P_{i+1}\mathrm{\Delta }{x}_{i+1}-s_{i+1}+{\mathrm{\Psi }}_{i+1}(\mathrm{\Delta }{t}_k-\mathrm{\Delta }{t}_{k-1})\\ & +{\mathrm{\Phi }}_{i+1}(-\mathrm{\Delta }{t}_k),\end{array}$(14a) ) until $i_k$ are expressed as (20a) $\begin{array}{rl}& \mathrm{\Delta }{\lambda }_{i_k}=P_{i_k}\mathrm{\Delta }{x}_{i_k}-s_{i_k}+{\mathrm{\Psi }}_{i_k}(\mathrm{\Delta }{t}_k-\mathrm{\Delta }{t}_{k-1})+{\mathrm{\Phi }}_{i_k}(-\mathrm{\Delta }{t}_k),\end{array}$(20a) (20b) $\begin{array}{rl}& {\mathrm{\Psi }}_{i_k}^{\mathrm{T}}\mathrm{\Delta }{x}_{i_k}+{\xi }_{i_k}(\mathrm{\Delta }{t}_k-\mathrm{\Delta }{t}_{k-1})+{\chi }_{i_k}(-\mathrm{\Delta }{t}_k)+{\eta }_{i_k}\\ & -{\mathrm{\Phi }}_{i_k}^{\mathrm{T}}\mathrm{\Delta }{x}_{i_k}-{\chi }_{i_k}(\mathrm{\Delta }{t}_k-\mathrm{\Delta }{t}_{k-1})-{\rho }_{i_k}(-\mathrm{\Delta }{t}_k)-{\iota }_{i_k}=0,\end{array}$(20b) and (20c) $\begin{array}{rl}& \sum _{j\in {\mathcal{I}}_{k-1}}(h_{x,j}^{\mathrm{T}}\mathrm{\Delta }{x}_j+h_{u,j}^{\mathrm{T}}\mathrm{\Delta }{u}_j+a_j^{\mathrm{T}}\mathrm{\Delta }{\lambda }_{j+1}+{\overline{h}}_j)\\ & +Q_{tt,k-1}(\mathrm{\Delta }{t}_{k-1}-\mathrm{\Delta }{t}_{k-2})+{\overline{q}}_{t,k-1}\\ & -{\mathrm{\Psi }}_{i_k}^{\mathrm{T}}\mathrm{\Delta }{x}_{i_k}-{\xi }_{i_k}(\mathrm{\Delta }{t}_k-\mathrm{\Delta }{t}_{k-1})-{\chi }_{i_k}(-\mathrm{\Delta }{t}_k)-{\eta }_{i_k}=0.\end{array}$(20c) Note that $\mathrm{\Delta }{t}_{k-1}=\mathrm{\Delta }{t}_0=0$ when k = 1. Therefore, from (20b(20b) $\begin{array}{rl}& {\mathrm{\Psi }}_{i_k}^{\mathrm{T}}\mathrm{\Delta }{x}_{i_k}+{\xi }_{i_k}(\mathrm{\Delta }{t}_k-\mathrm{\Delta }{t}_{k-1})+{\chi }_{i_k}(-\mathrm{\Delta }{t}_k)+{\eta }_{i_k}\\ & -{\mathrm{\Phi }}_{i_k}^{\mathrm{T}}\mathrm{\Delta }{x}_{i_k}-{\chi }_{i_k}(\mathrm{\Delta }{t}_k-\mathrm{\Delta }{t}_{k-1})-{\rho }_{i_k}(-\mathrm{\Delta }{t}_k)-{\iota }_{i_k}=0,\end{array}$(20b) ), $\mathrm{\Delta }{t}_k$ is determined as (21) $\begin{array}{rl}\mathrm{\Delta }{t}_k& =-{\sigma }_{i_k}^{-1}({\mathrm{\Psi }}_{i_k}-{\mathrm{\Phi }}_{i_k}{)}^{\mathrm{T}}\mathrm{\Delta }{x}_{i_k}\\ & -{\sigma }_{i_k}^{-1}({\xi }_{i_k}-{\chi }_{i_k})(-\mathrm{\Delta }{t}_{k-1})-{\sigma }_{i_k}^{-1}({\eta }_{i_k}-{\iota }_{i_k}).\end{array}$(21) Here, the following is defined: (22) $\begin{array}{r}{\sigma }_{i_k}:={\xi }_{i_k}-2{\chi }_{i_k}+{\rho }_{i_k}.\end{array}$(22) The backward recursion is completed when k = 1 because $i_1=min{\mathcal{I}}_1=0$. When k>1, (21(21) $\begin{array}{rl}\mathrm{\Delta }{t}_k& =-{\sigma }_{i_k}^{-1}({\mathrm{\Psi }}_{i_k}-{\mathrm{\Phi }}_{i_k}{)}^{\mathrm{T}}\mathrm{\Delta }{x}_{i_k}\\ & -{\sigma }_{i_k}^{-1}({\xi }_{i_k}-{\chi }_{i_k})(-\mathrm{\Delta }{t}_{k-1})-{\sigma }_{i_k}^{-1}({\eta }_{i_k}-{\iota }_{i_k}).\end{array}$(21) ) is further substituted into (20a(20a) $\begin{array}{rl}& \mathrm{\Delta }{\lambda }_{i_k}=P_{i_k}\mathrm{\Delta }{x}_{i_k}-s_{i_k}+{\mathrm{\Psi }}_{i_k}(\mathrm{\Delta }{t}_k-\mathrm{\Delta }{t}_{k-1})+{\mathrm{\Phi }}_{i_k}(-\mathrm{\Delta }{t}_k),\end{array}$(20a) ) and (20c(20c) $\begin{array}{rl}& \sum _{j\in {\mathcal{I}}_{k-1}}(h_{x,j}^{\mathrm{T}}\mathrm{\Delta }{x}_j+h_{u,j}^{\mathrm{T}}\mathrm{\Delta }{u}_j+a_j^{\mathrm{T}}\mathrm{\Delta }{\lambda }_{j+1}+{\overline{h}}_j)\\ & +Q_{tt,k-1}(\mathrm{\Delta }{t}_{k-1}-\mathrm{\Delta }{t}_{k-2})+{\overline{q}}_{t,k-1}\\ & -{\mathrm{\Psi }}_{i_k}^{\mathrm{T}}\mathrm{\Delta }{x}_{i_k}-{\xi }_{i_k}(\mathrm{\Delta }{t}_k-\mathrm{\Delta }{t}_{k-1})-{\chi }_{i_k}(-\mathrm{\Delta }{t}_k)-{\eta }_{i_k}=0.\end{array}$(20c) ), which produces: (23a) $\begin{array}{r}\mathrm{\Delta }{\lambda }_{i_K}={\tilde{P}}_{i_K}\mathrm{\Delta }{x}_{i_K}-{\tilde{s}}_{i_K}+{\tilde{\mathrm{\Psi }}}_{i_K}(-\mathrm{\Delta }{t}_{k-1})\end{array}$(23a) and (23b) $\begin{array}{rl}& \sum _{i\in {\mathcal{I}}_{K-1}}(h_{x,i}^{\mathrm{T}}\mathrm{\Delta }{x}_i+h_{u,i}^{\mathrm{T}}\mathrm{\Delta }{u}_i+a_i^{\mathrm{T}}\mathrm{\Delta }{\lambda }_{i+1}+{\overline{h}}_i)\\ & +Q_{tt,k-1}(\mathrm{\Delta }{t}_{k-1}-\mathrm{\Delta }{t}_{k-2})+{\overline{q}}_{t,k-1}\\ & -{\tilde{\mathrm{\Phi }}}_{i_k}^{\mathrm{T}}\mathrm{\Delta }{x}_{i_k}-{\tilde{\rho }}_{i_k}(-\mathrm{\Delta }{t}_{k-1})-{\tilde{\iota }}_{i_k}=0,\end{array}$(23b) where (23c) $\begin{array}{rl}& {\tilde{P}}_{i_k}:=P_{i_k}-{\sigma }_{i_k}^{-1}({\mathrm{\Psi }}_{i_k}-{\mathrm{\Phi }}_{i_k})({\mathrm{\Psi }}_{i_k}-{\mathrm{\Phi }}_{i_k}{)}^{\mathrm{T}},\end{array}$(23c) (23d) $\begin{array}{rl}& {\tilde{s}}_{i_k}:=s_{i_k}+{\sigma }_{i_k}^{-1}({\eta }_{i_k}-{\iota }_{i_k})({\mathrm{\Psi }}_{i_k}-{\mathrm{\Phi }}_{i_k}),\end{array}$(23d) (23e) $\begin{array}{rl}& {\tilde{\mathrm{\Phi }}}_{i_k}:={\mathrm{\Psi }}_{i_k}-{\sigma }_{i_k}^{-1}({\xi }_{i_k}-{\chi }_{i_k})({\mathrm{\Psi }}_{i_k}-{\mathrm{\Phi }}_{i_k}),\end{array}$(23e) (23f) $\begin{array}{rl}& {\tilde{\rho }}_{i_k}:={\xi }_{i_k}-{\sigma }_{i_k}^{-1}({\xi }_{i_k}-{\chi }_{i_k}{)}^2,\end{array}$(23f) and (23g) $\begin{array}{r}{\tilde{\iota }}_{i_k}:={\eta }_{i_k}-{\sigma }_{i_k}^{-1}({\xi }_{i_k}-{\chi }_{i_k})({\eta }_{i_k}-{\iota }_{i_k}).\end{array}$(23g) Therefore, we obtain Equations (17a(17a) $\begin{array}{r}\mathrm{\Delta }{\lambda }_i=P_i\mathrm{\Delta }{x}_i-s_i+{\mathrm{\Psi }}_i(\mathrm{\Delta }{t}_k-\mathrm{\Delta }{t}_{k-1})+{\mathrm{\Phi }}_i(-\mathrm{\Delta }{t}_k),\end{array}$(17a) ), (18a(18a) $\begin{array}{rl}& \sum _{j\in {\mathcal{I}}_k,j\le i-1}(h_{x,j}^{\mathrm{T}}\mathrm{\Delta }{x}_j+h_{u,j}^{\mathrm{T}}\mathrm{\Delta }{u}_j+a_j^{\mathrm{T}}\mathrm{\Delta }{\lambda }_{j+1}+{\overline{h}}_j)\\ & +{\mathrm{\Psi }}_i^{\mathrm{T}}\mathrm{\Delta }{x}_i+{\xi }_i(\mathrm{\Delta }{t}_k-\mathrm{\Delta }{t}_{k-1})+{\chi }_i(-\mathrm{\Delta }{t}_k)+{\eta }_i\\ & -{\mathrm{\Phi }}_i^{\mathrm{T}}\mathrm{\Delta }{x}_i-{\chi }_i(\mathrm{\Delta }{t}_k-\mathrm{\Delta }{t}_{k-1})-{\rho }_i(-\mathrm{\Delta }{t}_k)-{\iota }_i=0\end{array}$(18a) ), and (18b(18b) $\begin{array}{rl}& \sum _{j\in {\mathcal{I}}_{k-1}}(h_{x,j}^{\mathrm{T}}\mathrm{\Delta }{x}_j+h_{u,j}^{\mathrm{T}}\mathrm{\Delta }{u}_j+a_j^{\mathrm{T}}\mathrm{\Delta }{\lambda }_{j+1}+{\overline{h}}_j)\\ & -\sum _{j\in {\mathcal{I}}_k,j\le i-1}(h_{x,j}^{\mathrm{T}}\mathrm{\Delta }{x}_j+h_{u,j}^{\mathrm{T}}\mathrm{\Delta }{u}_j+a_j^{\mathrm{T}}\mathrm{\Delta }{\lambda }_{j+1}+{\overline{h}}_j)\\ & +Q_{tt,k-1}(\mathrm{\Delta }{t}_{k-1}-\mathrm{\Delta }{t}_{k-2})+{\overline{q}}_{t,k-1}\\ & -{\mathrm{\Psi }}_i^{\mathrm{T}}\mathrm{\Delta }{x}_i-{\xi }_i(\mathrm{\Delta }{t}_k-\mathrm{\Delta }{t}_{k-1})-{\chi }_i(-\mathrm{\Delta }{t}_k)-{\eta }_i=0,\end{array}$(18b) ) for stage $i_k$ in the same form as Equation (14(14a) $\begin{array}{rl}\mathrm{\Delta }{\lambda }_{i+1}& =P_{i+1}\mathrm{\Delta }{x}_{i+1}-s_{i+1}+{\mathrm{\Psi }}_{i+1}(\mathrm{\Delta }{t}_k-\mathrm{\Delta }{t}_{k-1})\\ & +{\mathrm{\Phi }}_{i+1}(-\mathrm{\Delta }{t}_k),\end{array}$(14a) ) for stage i + 1 with $P_{i+1}={\tilde{P}}_{i_k}$, $s_{i+1}={\tilde{s}}_{i_k}$, ${\mathrm{\Psi }}_{i+1}=0$, ${\mathrm{\Phi }}_{i+1}={\tilde{\mathrm{\Psi }}}_{i_k}$, ${\xi }_{i+1}=Q_{tt,k-1}$, ${\chi }_{i+1}=0$, ${\rho }_{i+1}={\tilde{\rho }}_{i_k}$, ${\eta }_{i+1}={\overline{q}}_{t,k+1}$, and ${\iota }_{i+1}={\tilde{\iota }}_{i_k}$.

4.2. Forward recursion

After the backward recursion up to the initial stage (i = 0), all Newton steps are recursively computed from stage 0 to N forward in time using the results of the backward recursion. First, the Newton step of the initial state $\mathrm{\Delta }{x}_0$ is computed from (10a(10a) $\begin{array}{rl}& \mathrm{\Delta }{x}_0+x_0-\overline{x}=0,\end{array}$(10a) ). Second, the Newton step of the first switching time $\mathrm{\Delta }{t}_1$ is computed from (21(21) $\begin{array}{rl}\mathrm{\Delta }{t}_k& =-{\sigma }_{i_k}^{-1}({\mathrm{\Psi }}_{i_k}-{\mathrm{\Phi }}_{i_k}{)}^{\mathrm{T}}\mathrm{\Delta }{x}_{i_k}\\ & -{\sigma }_{i_k}^{-1}({\xi }_{i_k}-{\chi }_{i_k})(-\mathrm{\Delta }{t}_{k-1})-{\sigma }_{i_k}^{-1}({\eta }_{i_k}-{\iota }_{i_k}).\end{array}$(21) ) with $\mathrm{\Delta }{t}_0=0$. Third, $\mathrm{\Delta }{\lambda }_i$, $\mathrm{\Delta }{u}_i$, and $\mathrm{\Delta }{x}_{i+1}$ are computed forward in time in each phase k using (14a(14a) $\begin{array}{rl}\mathrm{\Delta }{\lambda }_{i+1}& =P_{i+1}\mathrm{\Delta }{x}_{i+1}-s_{i+1}+{\mathrm{\Psi }}_{i+1}(\mathrm{\Delta }{t}_k-\mathrm{\Delta }{t}_{k-1})\\ & +{\mathrm{\Phi }}_{i+1}(-\mathrm{\Delta }{t}_k),\end{array}$(14a) ), (16a(16a) $\begin{array}{r}\mathrm{\Delta }{u}_i=K_i\mathrm{\Delta }{x}_i+k_i+T_i(\mathrm{\Delta }{t}_k-\mathrm{\Delta }{t}_{k-1})+W_i(-\mathrm{\Delta }{t}_k),\end{array}$(16a) ), and (10b(10b) $\begin{array}{rl}& A_i\mathrm{\Delta }{x}_i+B_i\mathrm{\Delta }{u}_i+a_i(\mathrm{\Delta }{t}_k-\mathrm{\Delta }{t}_{k-1})\\ & -\mathrm{\Delta }{x}_{i+1}+{\overline{x}}_i=0,i\in {\mathcal{I}}_k,k\in \mathcal{K},\end{array}$(10b) ), respectively. Fourth, after this procedure, $\mathrm{\Delta }{t}_{k+1}$ is computed using (21(21) $\begin{array}{rl}\mathrm{\Delta }{t}_k& =-{\sigma }_{i_k}^{-1}({\mathrm{\Psi }}_{i_k}-{\mathrm{\Phi }}_{i_k}{)}^{\mathrm{T}}\mathrm{\Delta }{x}_{i_k}\\ & -{\sigma }_{i_k}^{-1}({\xi }_{i_k}-{\chi }_{i_k})(-\mathrm{\Delta }{t}_{k-1})-{\sigma }_{i_k}^{-1}({\eta }_{i_k}-{\iota }_{i_k}).\end{array}$(21) ) until stage $i_{k+1}-1=min{\mathcal{I}}_{k+1}-1$. The third and fourth steps are repeated from phase k = 1 to k = K + 1, and the forward recursion is completed by computing $\mathrm{\Delta }{\lambda }_N$ using (13a(13a) $\begin{array}{r}\mathrm{\Delta }{\lambda }_N=P_N\mathrm{\Delta }{x}_N-s_N,\end{array}$(13a) ).

The following lemma states the equivalence of the dynamic programming and the proposed Riccati recursion:

Lemma 4.1

Consider a phase $k\in \mathcal{K}$ and stage $i\in {\mathcal{I}}_k$. Suppose that $G_j\succ O$ for all $j\ge i$, and ${\sigma }_{i_l}>0$ for all l>k, where $G_j$ and ${\sigma }_{i_l}$ are defined in (15c(15c) $\begin{array}{rl}& G_i:=Q_{uu,i}+B_i^{\mathrm{T}}{P}_{i+1}{B}_i,\end{array}$(15c) ) and (22(22) $\begin{array}{r}{\sigma }_{i_k}:={\xi }_{i_k}-2{\chi }_{i_k}+{\rho }_{i_k}.\end{array}$(22) ), respectively. Then, the cost-to-go function of stage i of dynamic programming for the QP (11(11) $\begin{array}{rl}\underset{\begin{array}{c}\mathrm{\Delta }{u}_0,\dots ,\mathrm{\Delta }{u}_{N-1}\\ \mathrm{\Delta }{t}_1,\dots ,\mathrm{\Delta }{t}_{K+1}\end{array}}{min}& \sum _{k\in \mathcal{K}}\sum _{i\in {\mathcal{I}}_k,i\ne N}\frac{1}{2}\{{\left[\begin{array}{c}\mathrm{\Delta }{t}_k-\mathrm{\Delta }{t}_{k-1}\\ \mathrm{\Delta }{x}_i\\ \mathrm{\Delta }{u}_i\end{array}\right]}^{\mathrm{T}}\\ & \times \left[\begin{array}{ccc}0& h_{x,i}^{\mathrm{T}}& h_{u,i}^{\mathrm{T}}\\ h_{x,i}& Q_{xx,i}& Q_{xu,i}\\ h_{u,i}& Q_{ux,i}& Q_{uu,i}\end{array}\right]\left[\begin{array}{c}\mathrm{\Delta }{t}_k-\mathrm{\Delta }{t}_{k-1}\\ \mathrm{\Delta }{x}_i\\ \mathrm{\Delta }{u}_i\end{array}\right]\\ & +{\left[\begin{array}{c}{\overline{h}}_i\\ {\overline{l}}_{x,i}\\ {\overline{l}}_{u,i}\end{array}\right]}^{\mathrm{T}}\left[\begin{array}{c}\mathrm{\Delta }{t}_k-\mathrm{\Delta }{t}_{k-1}\\ \mathrm{\Delta }{x}_i\\ \mathrm{\Delta }{u}_i\end{array}\right]\}\\ & +\frac{1}{2}\mathrm{\Delta }{x}_N^{\mathrm{T}}{Q}_{xx,N}\mathrm{\Delta }{x}_N+{\overline{l}}_{x,N}^{\mathrm{T}}\mathrm{\Delta }{x}_N\\ & +\sum _{k\in \mathcal{K}}\left\{\frac{1}{2}{Q}_{tt,k}\right(\mathrm{\Delta }{t}_k-\mathrm{\Delta }{t}_{k-1}{)}^2\\ & +{\overline{q}}_{t,k}(\mathrm{\Delta }{t}_k-\mathrm{\Delta }{t}_{k-1})\}\\ \mathrm{s}.\mathrm{t}.& \left(10\mathrm{a}\right),\left(10\mathrm{b}\right),\end{array}$(11) ) is expressed as (24) $\begin{array}{rl}& \frac{1}{2}{\left[\begin{array}{c}-\mathrm{\Delta }{t}_k\\ \mathrm{\Delta }{t}_k-\mathrm{\Delta }{t}_{k-1}\\ \mathrm{\Delta }{x}_i\end{array}\right]}^{\mathrm{T}}\left[\begin{array}{ccc}{\rho }_i& {\chi }_i& {\mathrm{\Phi }}_i^{\mathrm{T}}\\ {\chi }_i& {\xi }_i& {\mathrm{\Psi }}_i^{\mathrm{T}}\\ {\mathrm{\Phi }}_i& {\mathrm{\Psi }}_i& P_i\end{array}\right]\left[\begin{array}{c}-\mathrm{\Delta }{t}_k\\ \mathrm{\Delta }{t}_k-\mathrm{\Delta }{t}_{k-1}\\ \mathrm{\Delta }{x}_i\end{array}\right]\\ & +{\left[\begin{array}{c}{\iota }_i\\ {\eta }_i\\ -s_i\end{array}\right]}^{\mathrm{T}}\left[\begin{array}{c}-\mathrm{\Delta }{t}_k\\ \mathrm{\Delta }{t}_k-\mathrm{\Delta }{t}_{k-1}\\ \mathrm{\Delta }{x}_i\end{array}\right],\end{array}$(24) where ${\xi }_i$, ${\rho }_i$, ${\mathrm{\Phi }}_i$, ${\mathrm{\Psi }}_i$, $P_i$, ${\iota }_i$, ${\eta }_i$, and $s_i$ are defined by the Riccati recursion algorithm presented in subsection 4.1. Moreover, the subproblem of the dynamic programming to determine $\mathrm{\Delta }{u}_i$ is represented by (25) $\begin{array}{rl}\underset{\mathrm{\Delta }{u}_i}{min}& \frac{1}{2}{\left[\begin{array}{c}-\mathrm{\Delta }{t}_k\\ \mathrm{\Delta }{t}_k-\mathrm{\Delta }{t}_{k-1}\\ \mathrm{\Delta }{x}_i\\ \mathrm{\Delta }{u}_i\end{array}\right]}^{\mathrm{T}}\left[\begin{array}{c}\\ & Q_{tt,i}& h_{x,i}^{\mathrm{T}}& h_{u,i}^{\mathrm{T}}\\ & h_{x,i}& Q_{xx,i}& Q_{xu,i}\\ & h_{u,i}& Q_{ux,i}& Q_{uu,i}\end{array}\right]\\ & \times \left[\begin{array}{c}-\mathrm{\Delta }{t}_k\\ \mathrm{\Delta }{t}_k-\mathrm{\Delta }{t}_{k-1}\\ \mathrm{\Delta }{x}_i\\ \mathrm{\Delta }{u}_i\end{array}\right]+{\left[\begin{array}{c}\\ {\overline{h}}_i\\ {\overline{l}}_{x,i}\\ {\overline{l}}_{u,i}\end{array}\right]}^{\mathrm{T}}\left[\begin{array}{c}-\mathrm{\Delta }{t}_k\\ \mathrm{\Delta }{t}_k-\mathrm{\Delta }{t}_{k-1}\\ \mathrm{\Delta }{x}_i\\ \mathrm{\Delta }{u}_i\end{array}\right]\\ & +\frac{1}{2}{\left[\begin{array}{c}-\mathrm{\Delta }{t}_k\\ \mathrm{\Delta }{t}_k-\mathrm{\Delta }{t}_{k-1}\\ \mathrm{\Delta }{x}_{i+1}\end{array}\right]}^{\mathrm{T}}\left[\begin{array}{ccc}{\rho }_{i+1}& {\chi }_{i+1}& {\mathrm{\Phi }}_{i+1}^{\mathrm{T}}\\ {\chi }_{i+1}& {\xi }_{i+1}& {\mathrm{\Psi }}_{i+1}^{\mathrm{T}}\\ {\mathrm{\Phi }}_{i+1}& {\mathrm{\Psi }}_{i+1}& P_{i+1}\end{array}\right]\\ & \times \left[\begin{array}{c}-\mathrm{\Delta }{t}_k\\ \mathrm{\Delta }{t}_k-\mathrm{\Delta }{t}_{k-1}\\ \mathrm{\Delta }{x}_{i+1}\end{array}\right]+{\left[\begin{array}{c}{\iota }_{i+1}\\ {\eta }_{i+1}\\ -s_{i+1}\end{array}\right]}^{\mathrm{T}}\left[\begin{array}{c}-\mathrm{\Delta }{t}_k\\ \mathrm{\Delta }{t}_k-\mathrm{\Delta }{t}_{k-1}\\ \mathrm{\Delta }{x}_{i+1}\end{array}\right]\\ \mathrm{s}.\mathrm{t}.& A_i\mathrm{\Delta }{x}_i+B_i\mathrm{\Delta }{u}_i+a_i(\mathrm{\Delta }{t}_k-\mathrm{\Delta }{t}_{k-1})-\mathrm{\Delta }{x}_{i+1}+{\overline{x}}_i=0,\end{array}$(25) and determining $\mathrm{\Delta }{t}_k$ is expressed as (26) $\begin{array}{r}\underset{\mathrm{\Delta }{t}_k}{min}\left(24\right).\end{array}$(26)

Proof.

The proof is achieved by induction. At the terminal stage (i = N), the cost-to-go function is represented by (24(24) $\begin{array}{rl}& \frac{1}{2}{\left[\begin{array}{c}-\mathrm{\Delta }{t}_k\\ \mathrm{\Delta }{t}_k-\mathrm{\Delta }{t}_{k-1}\\ \mathrm{\Delta }{x}_i\end{array}\right]}^{\mathrm{T}}\left[\begin{array}{ccc}{\rho }_i& {\chi }_i& {\mathrm{\Phi }}_i^{\mathrm{T}}\\ {\chi }_i& {\xi }_i& {\mathrm{\Psi }}_i^{\mathrm{T}}\\ {\mathrm{\Phi }}_i& {\mathrm{\Psi }}_i& P_i\end{array}\right]\left[\begin{array}{c}-\mathrm{\Delta }{t}_k\\ \mathrm{\Delta }{t}_k-\mathrm{\Delta }{t}_{k-1}\\ \mathrm{\Delta }{x}_i\end{array}\right]\\ & +{\left[\begin{array}{c}{\iota }_i\\ {\eta }_i\\ -s_i\end{array}\right]}^{\mathrm{T}}\left[\begin{array}{c}-\mathrm{\Delta }{t}_k\\ \mathrm{\Delta }{t}_k-\mathrm{\Delta }{t}_{k-1}\\ \mathrm{\Delta }{x}_i\end{array}\right],\end{array}$(24) ) with ${\mathrm{\Psi }}_N={\mathrm{\Phi }}_N=0$ and ${\xi }_N={\chi }_N={\rho }_N={\eta }_N={\iota }_N=0$. Next, suppose that we have the cost-to-go function (24(24) $\begin{array}{rl}& \frac{1}{2}{\left[\begin{array}{c}-\mathrm{\Delta }{t}_k\\ \mathrm{\Delta }{t}_k-\mathrm{\Delta }{t}_{k-1}\\ \mathrm{\Delta }{x}_i\end{array}\right]}^{\mathrm{T}}\left[\begin{array}{ccc}{\rho }_i& {\chi }_i& {\mathrm{\Phi }}_i^{\mathrm{T}}\\ {\chi }_i& {\xi }_i& {\mathrm{\Psi }}_i^{\mathrm{T}}\\ {\mathrm{\Phi }}_i& {\mathrm{\Psi }}_i& P_i\end{array}\right]\left[\begin{array}{c}-\mathrm{\Delta }{t}_k\\ \mathrm{\Delta }{t}_k-\mathrm{\Delta }{t}_{k-1}\\ \mathrm{\Delta }{x}_i\end{array}\right]\\ & +{\left[\begin{array}{c}{\iota }_i\\ {\eta }_i\\ -s_i\end{array}\right]}^{\mathrm{T}}\left[\begin{array}{c}-\mathrm{\Delta }{t}_k\\ \mathrm{\Delta }{t}_k-\mathrm{\Delta }{t}_{k-1}\\ \mathrm{\Delta }{x}_i\end{array}\right],\end{array}$(24) ) of stage i + 1. Then, it is clear that the subproblem of the dynamic programming is represented by (25(25) $\begin{array}{rl}\underset{\mathrm{\Delta }{u}_i}{min}& \frac{1}{2}{\left[\begin{array}{c}-\mathrm{\Delta }{t}_k\\ \mathrm{\Delta }{t}_k-\mathrm{\Delta }{t}_{k-1}\\ \mathrm{\Delta }{x}_i\\ \mathrm{\Delta }{u}_i\end{array}\right]}^{\mathrm{T}}\left[\begin{array}{c}\\ & Q_{tt,i}& h_{x,i}^{\mathrm{T}}& h_{u,i}^{\mathrm{T}}\\ & h_{x,i}& Q_{xx,i}& Q_{xu,i}\\ & h_{u,i}& Q_{ux,i}& Q_{uu,i}\end{array}\right]\\ & \times \left[\begin{array}{c}-\mathrm{\Delta }{t}_k\\ \mathrm{\Delta }{t}_k-\mathrm{\Delta }{t}_{k-1}\\ \mathrm{\Delta }{x}_i\\ \mathrm{\Delta }{u}_i\end{array}\right]+{\left[\begin{array}{c}\\ {\overline{h}}_i\\ {\overline{l}}_{x,i}\\ {\overline{l}}_{u,i}\end{array}\right]}^{\mathrm{T}}\left[\begin{array}{c}-\mathrm{\Delta }{t}_k\\ \mathrm{\Delta }{t}_k-\mathrm{\Delta }{t}_{k-1}\\ \mathrm{\Delta }{x}_i\\ \mathrm{\Delta }{u}_i\end{array}\right]\\ & +\frac{1}{2}{\left[\begin{array}{c}-\mathrm{\Delta }{t}_k\\ \mathrm{\Delta }{t}_k-\mathrm{\Delta }{t}_{k-1}\\ \mathrm{\Delta }{x}_{i+1}\end{array}\right]}^{\mathrm{T}}\left[\begin{array}{ccc}{\rho }_{i+1}& {\chi }_{i+1}& {\mathrm{\Phi }}_{i+1}^{\mathrm{T}}\\ {\chi }_{i+1}& {\xi }_{i+1}& {\mathrm{\Psi }}_{i+1}^{\mathrm{T}}\\ {\mathrm{\Phi }}_{i+1}& {\mathrm{\Psi }}_{i+1}& P_{i+1}\end{array}\right]\\ & \times \left[\begin{array}{c}-\mathrm{\Delta }{t}_k\\ \mathrm{\Delta }{t}_k-\mathrm{\Delta }{t}_{k-1}\\ \mathrm{\Delta }{x}_{i+1}\end{array}\right]+{\left[\begin{array}{c}{\iota }_{i+1}\\ {\eta }_{i+1}\\ -s_{i+1}\end{array}\right]}^{\mathrm{T}}\left[\begin{array}{c}-\mathrm{\Delta }{t}_k\\ \mathrm{\Delta }{t}_k-\mathrm{\Delta }{t}_{k-1}\\ \mathrm{\Delta }{x}_{i+1}\end{array}\right]\\ \mathrm{s}.\mathrm{t}.& A_i\mathrm{\Delta }{x}_i+B_i\mathrm{\Delta }{u}_i+a_i(\mathrm{\Delta }{t}_k-\mathrm{\Delta }{t}_{k-1})-\mathrm{\Delta }{x}_{i+1}+{\overline{x}}_i=0,\end{array}$(25) ). $\mathrm{\Delta }{u}_i$ can be uniquely determined from (25(25) $\begin{array}{rl}\underset{\mathrm{\Delta }{u}_i}{min}& \frac{1}{2}{\left[\begin{array}{c}-\mathrm{\Delta }{t}_k\\ \mathrm{\Delta }{t}_k-\mathrm{\Delta }{t}_{k-1}\\ \mathrm{\Delta }{x}_i\\ \mathrm{\Delta }{u}_i\end{array}\right]}^{\mathrm{T}}\left[\begin{array}{c}\\ & Q_{tt,i}& h_{x,i}^{\mathrm{T}}& h_{u,i}^{\mathrm{T}}\\ & h_{x,i}& Q_{xx,i}& Q_{xu,i}\\ & h_{u,i}& Q_{ux,i}& Q_{uu,i}\end{array}\right]\\ & \times \left[\begin{array}{c}-\mathrm{\Delta }{t}_k\\ \mathrm{\Delta }{t}_k-\mathrm{\Delta }{t}_{k-1}\\ \mathrm{\Delta }{x}_i\\ \mathrm{\Delta }{u}_i\end{array}\right]+{\left[\begin{array}{c}\\ {\overline{h}}_i\\ {\overline{l}}_{x,i}\\ {\overline{l}}_{u,i}\end{array}\right]}^{\mathrm{T}}\left[\begin{array}{c}-\mathrm{\Delta }{t}_k\\ \mathrm{\Delta }{t}_k-\mathrm{\Delta }{t}_{k-1}\\ \mathrm{\Delta }{x}_i\\ \mathrm{\Delta }{u}_i\end{array}\right]\\ & +\frac{1}{2}{\left[\begin{array}{c}-\mathrm{\Delta }{t}_k\\ \mathrm{\Delta }{t}_k-\mathrm{\Delta }{t}_{k-1}\\ \mathrm{\Delta }{x}_{i+1}\end{array}\right]}^{\mathrm{T}}\left[\begin{array}{ccc}{\rho }_{i+1}& {\chi }_{i+1}& {\mathrm{\Phi }}_{i+1}^{\mathrm{T}}\\ {\chi }_{i+1}& {\xi }_{i+1}& {\mathrm{\Psi }}_{i+1}^{\mathrm{T}}\\ {\mathrm{\Phi }}_{i+1}& {\mathrm{\Psi }}_{i+1}& P_{i+1}\end{array}\right]\\ & \times \left[\begin{array}{c}-\mathrm{\Delta }{t}_k\\ \mathrm{\Delta }{t}_k-\mathrm{\Delta }{t}_{k-1}\\ \mathrm{\Delta }{x}_{i+1}\end{array}\right]+{\left[\begin{array}{c}{\iota }_{i+1}\\ {\eta }_{i+1}\\ -s_{i+1}\end{array}\right]}^{\mathrm{T}}\left[\begin{array}{c}-\mathrm{\Delta }{t}_k\\ \mathrm{\Delta }{t}_k-\mathrm{\Delta }{t}_{k-1}\\ \mathrm{\Delta }{x}_{i+1}\end{array}\right]\\ \mathrm{s}.\mathrm{t}.& A_i\mathrm{\Delta }{x}_i+B_i\mathrm{\Delta }{u}_i+a_i(\mathrm{\Delta }{t}_k-\mathrm{\Delta }{t}_{k-1})-\mathrm{\Delta }{x}_{i+1}+{\overline{x}}_i=0,\end{array}$(25) ) as (16(16a) $\begin{array}{r}\mathrm{\Delta }{u}_i=K_i\mathrm{\Delta }{x}_i+k_i+T_i(\mathrm{\Delta }{t}_k-\mathrm{\Delta }{t}_{k-1})+W_i(-\mathrm{\Delta }{t}_k),\end{array}$(16a) ) because $G_i\succ O$. The cost-to-go function of stage i is represented by (24(24) $\begin{array}{rl}& \frac{1}{2}{\left[\begin{array}{c}-\mathrm{\Delta }{t}_k\\ \mathrm{\Delta }{t}_k-\mathrm{\Delta }{t}_{k-1}\\ \mathrm{\Delta }{x}_i\end{array}\right]}^{\mathrm{T}}\left[\begin{array}{ccc}{\rho }_i& {\chi }_i& {\mathrm{\Phi }}_i^{\mathrm{T}}\\ {\chi }_i& {\xi }_i& {\mathrm{\Psi }}_i^{\mathrm{T}}\\ {\mathrm{\Phi }}_i& {\mathrm{\Psi }}_i& P_i\end{array}\right]\left[\begin{array}{c}-\mathrm{\Delta }{t}_k\\ \mathrm{\Delta }{t}_k-\mathrm{\Delta }{t}_{k-1}\\ \mathrm{\Delta }{x}_i\end{array}\right]\\ & +{\left[\begin{array}{c}{\iota }_i\\ {\eta }_i\\ -s_i\end{array}\right]}^{\mathrm{T}}\left[\begin{array}{c}-\mathrm{\Delta }{t}_k\\ \mathrm{\Delta }{t}_k-\mathrm{\Delta }{t}_{k-1}\\ \mathrm{\Delta }{x}_i\end{array}\right],\end{array}$(24) ), where $P_i$, $s_i$, ${\mathrm{\Psi }}_i$, ${\mathrm{\Phi }}_i$, ${\xi }_i$, ${\chi }_i$, ${\rho }_i$, ${\eta }_i$, and ${\iota }_i$ are defined as (15(15a) $\begin{array}{rl}& F_i:=Q_{xx,i}+A_i^{\mathrm{T}}{P}_{i+1}{A}_i,\end{array}$(15a) ), (16(16a) $\begin{array}{r}\mathrm{\Delta }{u}_i=K_i\mathrm{\Delta }{x}_i+k_i+T_i(\mathrm{\Delta }{t}_k-\mathrm{\Delta }{t}_{k-1})+W_i(-\mathrm{\Delta }{t}_k),\end{array}$(16a) ), (17(17a) $\begin{array}{r}\mathrm{\Delta }{\lambda }_i=P_i\mathrm{\Delta }{x}_i-s_i+{\mathrm{\Psi }}_i(\mathrm{\Delta }{t}_k-\mathrm{\Delta }{t}_{k-1})+{\mathrm{\Phi }}_i(-\mathrm{\Delta }{t}_k),\end{array}$(17a) ), and (19(19a) $\begin{array}{rl}& {\xi }_i:={\xi }_{i+1}+a_i^{\mathrm{T}}(P_{i+1}{a}_i+2{\mathrm{\Psi }}_{i+1})+{\psi }_{u,i}^{\mathrm{T}}{T}_i,\end{array}$(19a) ). When $i=i_k:=min{I}_k$, $\mathrm{\Delta }{t}_k$ can be further uniquely determined by solving (26(26) $\begin{array}{r}\underset{\mathrm{\Delta }{t}_k}{min}\left(24\right).\end{array}$(26) ) under the assumption ${\sigma }_{i_k}>0$ after determining $\mathrm{\Delta }{u}_{i_k}$ and obtaining the cost-to-go function of stage $i_k$ (24(24) $\begin{array}{rl}& \frac{1}{2}{\left[\begin{array}{c}-\mathrm{\Delta }{t}_k\\ \mathrm{\Delta }{t}_k-\mathrm{\Delta }{t}_{k-1}\\ \mathrm{\Delta }{x}_i\end{array}\right]}^{\mathrm{T}}\left[\begin{array}{ccc}{\rho }_i& {\chi }_i& {\mathrm{\Phi }}_i^{\mathrm{T}}\\ {\chi }_i& {\xi }_i& {\mathrm{\Psi }}_i^{\mathrm{T}}\\ {\mathrm{\Phi }}_i& {\mathrm{\Psi }}_i& P_i\end{array}\right]\left[\begin{array}{c}-\mathrm{\Delta }{t}_k\\ \mathrm{\Delta }{t}_k-\mathrm{\Delta }{t}_{k-1}\\ \mathrm{\Delta }{x}_i\end{array}\right]\\ & +{\left[\begin{array}{c}{\iota }_i\\ {\eta }_i\\ -s_i\end{array}\right]}^{\mathrm{T}}\left[\begin{array}{c}-\mathrm{\Delta }{t}_k\\ \mathrm{\Delta }{t}_k-\mathrm{\Delta }{t}_{k-1}\\ \mathrm{\Delta }{x}_i\end{array}\right],\end{array}$(24) ). Then, the cost-to-go function of stage $i_k$ is in the form of (24(24) $\begin{array}{rl}& \frac{1}{2}{\left[\begin{array}{c}-\mathrm{\Delta }{t}_k\\ \mathrm{\Delta }{t}_k-\mathrm{\Delta }{t}_{k-1}\\ \mathrm{\Delta }{x}_i\end{array}\right]}^{\mathrm{T}}\left[\begin{array}{ccc}{\rho }_i& {\chi }_i& {\mathrm{\Phi }}_i^{\mathrm{T}}\\ {\chi }_i& {\xi }_i& {\mathrm{\Psi }}_i^{\mathrm{T}}\\ {\mathrm{\Phi }}_i& {\mathrm{\Psi }}_i& P_i\end{array}\right]\left[\begin{array}{c}-\mathrm{\Delta }{t}_k\\ \mathrm{\Delta }{t}_k-\mathrm{\Delta }{t}_{k-1}\\ \mathrm{\Delta }{x}_i\end{array}\right]\\ & +{\left[\begin{array}{c}{\iota }_i\\ {\eta }_i\\ -s_i\end{array}\right]}^{\mathrm{T}}\left[\begin{array}{c}-\mathrm{\Delta }{t}_k\\ \mathrm{\Delta }{t}_k-\mathrm{\Delta }{t}_{k-1}\\ \mathrm{\Delta }{x}_i\end{array}\right],\end{array}$(24) ), where $P_{i_k}$, $s_{i_k}$, ${\mathrm{\Phi }}_{i_k}$, ${\rho }_{i_k}$, and ${\iota }_{i_k}$ are defined as (23(23a) $\begin{array}{r}\mathrm{\Delta }{\lambda }_{i_K}={\tilde{P}}_{i_K}\mathrm{\Delta }{x}_{i_K}-{\tilde{s}}_{i_K}+{\tilde{\mathrm{\Psi }}}_{i_K}(-\mathrm{\Delta }{t}_{k-1})\end{array}$(23a) ), ${\mathrm{\Psi }}_{i_k}=0$, ${\xi }_{i_k}=Q_{tt,k-1}$, ${\chi }_{i_k}=0$, and ${\eta }_{i_k}={\overline{q}}_{t,k-1}$, respectively, which completes the proof.

Note that the equivalence cannot be shown without the positive definiteness of $G_i$ and ${\sigma }_{i_k}$: if they are not positive definite, a unique solution does not exist and the cost-to-go function is not defined. The following theorem is obtained based on Lemma 4.1. Note that the discussion herein is restricted to the exact Hessian matrix to analyze the SOSC.

Theorem 4.2

We suppose that the SOSC and LICQ hold at the current iterate and consider that the exact Hessian matrix is used. Then, $G_i$, as defined in (15c(15c) $\begin{array}{rl}& G_i:=Q_{uu,i}+B_i^{\mathrm{T}}{P}_{i+1}{B}_i,\end{array}$(15c) ), is positive definite for all $i\in \{0,\dots ,N-1\}$. Moreover, ${\sigma }_{i_k}$, as defined in (22(22) $\begin{array}{r}{\sigma }_{i_k}:={\xi }_{i_k}-2{\chi }_{i_k}+{\rho }_{i_k}.\end{array}$(22) ), satisfies ${\sigma }_{i_k}>0$ for all $k\in \{0,\dots ,K\}$.

Proof.

The QP subproblem (11(11) $\begin{array}{rl}\underset{\begin{array}{c}\mathrm{\Delta }{u}_0,\dots ,\mathrm{\Delta }{u}_{N-1}\\ \mathrm{\Delta }{t}_1,\dots ,\mathrm{\Delta }{t}_{K+1}\end{array}}{min}& \sum _{k\in \mathcal{K}}\sum _{i\in {\mathcal{I}}_k,i\ne N}\frac{1}{2}\{{\left[\begin{array}{c}\mathrm{\Delta }{t}_k-\mathrm{\Delta }{t}_{k-1}\\ \mathrm{\Delta }{x}_i\\ \mathrm{\Delta }{u}_i\end{array}\right]}^{\mathrm{T}}\\ & \times \left[\begin{array}{ccc}0& h_{x,i}^{\mathrm{T}}& h_{u,i}^{\mathrm{T}}\\ h_{x,i}& Q_{xx,i}& Q_{xu,i}\\ h_{u,i}& Q_{ux,i}& Q_{uu,i}\end{array}\right]\left[\begin{array}{c}\mathrm{\Delta }{t}_k-\mathrm{\Delta }{t}_{k-1}\\ \mathrm{\Delta }{x}_i\\ \mathrm{\Delta }{u}_i\end{array}\right]\\ & +{\left[\begin{array}{c}{\overline{h}}_i\\ {\overline{l}}_{x,i}\\ {\overline{l}}_{u,i}\end{array}\right]}^{\mathrm{T}}\left[\begin{array}{c}\mathrm{\Delta }{t}_k-\mathrm{\Delta }{t}_{k-1}\\ \mathrm{\Delta }{x}_i\\ \mathrm{\Delta }{u}_i\end{array}\right]\}\\ & +\frac{1}{2}\mathrm{\Delta }{x}_N^{\mathrm{T}}{Q}_{xx,N}\mathrm{\Delta }{x}_N+{\overline{l}}_{x,N}^{\mathrm{T}}\mathrm{\Delta }{x}_N\\ & +\sum _{k\in \mathcal{K}}\left\{\frac{1}{2}{Q}_{tt,k}\right(\mathrm{\Delta }{t}_k-\mathrm{\Delta }{t}_{k-1}{)}^2\\ & +{\overline{q}}_{t,k}(\mathrm{\Delta }{t}_k-\mathrm{\Delta }{t}_{k-1})\}\\ \mathrm{s}.\mathrm{t}.& \left(10\mathrm{a}\right),\left(10\mathrm{b}\right),\end{array}$(11) ) with an exact Hessian matrix must have a unique global solution because the SOSC and LICQ hold (Nocedal & Wright, 2006). Therefore, the dynamic programming subproblems (25(25) $\begin{array}{rl}\underset{\mathrm{\Delta }{u}_i}{min}& \frac{1}{2}{\left[\begin{array}{c}-\mathrm{\Delta }{t}_k\\ \mathrm{\Delta }{t}_k-\mathrm{\Delta }{t}_{k-1}\\ \mathrm{\Delta }{x}_i\\ \mathrm{\Delta }{u}_i\end{array}\right]}^{\mathrm{T}}\left[\begin{array}{c}\\ & Q_{tt,i}& h_{x,i}^{\mathrm{T}}& h_{u,i}^{\mathrm{T}}\\ & h_{x,i}& Q_{xx,i}& Q_{xu,i}\\ & h_{u,i}& Q_{ux,i}& Q_{uu,i}\end{array}\right]\\ & \times \left[\begin{array}{c}-\mathrm{\Delta }{t}_k\\ \mathrm{\Delta }{t}_k-\mathrm{\Delta }{t}_{k-1}\\ \mathrm{\Delta }{x}_i\\ \mathrm{\Delta }{u}_i\end{array}\right]+{\left[\begin{array}{c}\\ {\overline{h}}_i\\ {\overline{l}}_{x,i}\\ {\overline{l}}_{u,i}\end{array}\right]}^{\mathrm{T}}\left[\begin{array}{c}-\mathrm{\Delta }{t}_k\\ \mathrm{\Delta }{t}_k-\mathrm{\Delta }{t}_{k-1}\\ \mathrm{\Delta }{x}_i\\ \mathrm{\Delta }{u}_i\end{array}\right]\\ & +\frac{1}{2}{\left[\begin{array}{c}-\mathrm{\Delta }{t}_k\\ \mathrm{\Delta }{t}_k-\mathrm{\Delta }{t}_{k-1}\\ \mathrm{\Delta }{x}_{i+1}\end{array}\right]}^{\mathrm{T}}\left[\begin{array}{ccc}{\rho }_{i+1}& {\chi }_{i+1}& {\mathrm{\Phi }}_{i+1}^{\mathrm{T}}\\ {\chi }_{i+1}& {\xi }_{i+1}& {\mathrm{\Psi }}_{i+1}^{\mathrm{T}}\\ {\mathrm{\Phi }}_{i+1}& {\mathrm{\Psi }}_{i+1}& P_{i+1}\end{array}\right]\\ & \times \left[\begin{array}{c}-\mathrm{\Delta }{t}_k\\ \mathrm{\Delta }{t}_k-\mathrm{\Delta }{t}_{k-1}\\ \mathrm{\Delta }{x}_{i+1}\end{array}\right]+{\left[\begin{array}{c}{\iota }_{i+1}\\ {\eta }_{i+1}\\ -s_{i+1}\end{array}\right]}^{\mathrm{T}}\left[\begin{array}{c}-\mathrm{\Delta }{t}_k\\ \mathrm{\Delta }{t}_k-\mathrm{\Delta }{t}_{k-1}\\ \mathrm{\Delta }{x}_{i+1}\end{array}\right]\\ \mathrm{s}.\mathrm{t}.& A_i\mathrm{\Delta }{x}_i+B_i\mathrm{\Delta }{u}_i+a_i(\mathrm{\Delta }{t}_k-\mathrm{\Delta }{t}_{k-1})-\mathrm{\Delta }{x}_{i+1}+{\overline{x}}_i=0,\end{array}$(25) ) and (26(26) $\begin{array}{r}\underset{\mathrm{\Delta }{t}_k}{min}\left(24\right).\end{array}$(26) ) must have unique solutions. Subsequently, the Hessian matrix with respect to $\mathrm{\Delta }{u}_i$ in (25(25) $\begin{array}{rl}\underset{\mathrm{\Delta }{u}_i}{min}& \frac{1}{2}{\left[\begin{array}{c}-\mathrm{\Delta }{t}_k\\ \mathrm{\Delta }{t}_k-\mathrm{\Delta }{t}_{k-1}\\ \mathrm{\Delta }{x}_i\\ \mathrm{\Delta }{u}_i\end{array}\right]}^{\mathrm{T}}\left[\begin{array}{c}\\ & Q_{tt,i}& h_{x,i}^{\mathrm{T}}& h_{u,i}^{\mathrm{T}}\\ & h_{x,i}& Q_{xx,i}& Q_{xu,i}\\ & h_{u,i}& Q_{ux,i}& Q_{uu,i}\end{array}\right]\\ & \times \left[\begin{array}{c}-\mathrm{\Delta }{t}_k\\ \mathrm{\Delta }{t}_k-\mathrm{\Delta }{t}_{k-1}\\ \mathrm{\Delta }{x}_i\\ \mathrm{\Delta }{u}_i\end{array}\right]+{\left[\begin{array}{c}\\ {\overline{h}}_i\\ {\overline{l}}_{x,i}\\ {\overline{l}}_{u,i}\end{array}\right]}^{\mathrm{T}}\left[\begin{array}{c}-\mathrm{\Delta }{t}_k\\ \mathrm{\Delta }{t}_k-\mathrm{\Delta }{t}_{k-1}\\ \mathrm{\Delta }{x}_i\\ \mathrm{\Delta }{u}_i\end{array}\right]\\ & +\frac{1}{2}{\left[\begin{array}{c}-\mathrm{\Delta }{t}_k\\ \mathrm{\Delta }{t}_k-\mathrm{\Delta }{t}_{k-1}\\ \mathrm{\Delta }{x}_{i+1}\end{array}\right]}^{\mathrm{T}}\left[\begin{array}{ccc}{\rho }_{i+1}& {\chi }_{i+1}& {\mathrm{\Phi }}_{i+1}^{\mathrm{T}}\\ {\chi }_{i+1}& {\xi }_{i+1}& {\mathrm{\Psi }}_{i+1}^{\mathrm{T}}\\ {\mathrm{\Phi }}_{i+1}& {\mathrm{\Psi }}_{i+1}& P_{i+1}\end{array}\right]\\ & \times \left[\begin{array}{c}-\mathrm{\Delta }{t}_k\\ \mathrm{\Delta }{t}_k-\mathrm{\Delta }{t}_{k-1}\\ \mathrm{\Delta }{x}_{i+1}\end{array}\right]+{\left[\begin{array}{c}{\iota }_{i+1}\\ {\eta }_{i+1}\\ -s_{i+1}\end{array}\right]}^{\mathrm{T}}\left[\begin{array}{c}-\mathrm{\Delta }{t}_k\\ \mathrm{\Delta }{t}_k-\mathrm{\Delta }{t}_{k-1}\\ \mathrm{\Delta }{x}_{i+1}\end{array}\right]\\ \mathrm{s}.\mathrm{t}.& A_i\mathrm{\Delta }{x}_i+B_i\mathrm{\Delta }{u}_i+a_i(\mathrm{\Delta }{t}_k-\mathrm{\Delta }{t}_{k-1})-\mathrm{\Delta }{x}_{i+1}+{\overline{x}}_i=0,\end{array}$(25) ) must be positive definite, and the quadratic term with respect to $\mathrm{\Delta }{t}_k$ in (26(26) $\begin{array}{r}\underset{\mathrm{\Delta }{t}_k}{min}\left(24\right).\end{array}$(26) ) must be positive. Then, Lemma 4.1 recursively shows that the Hessian matrix is $G_i$ with respect to $\mathrm{\Delta }{u}_i$. Additionally, the lemma also shows that the quadratic term with respect to $\mathrm{\Delta }{t}_k$ is ${\sigma }_{i_k}$, which completes the proof.

Theorem 4.2 indicates that $G_i^{-1}$ can always be efficiently computed using Cholesky factorizations if the current iterate is sufficiently close to a local minimum. This fact also leads to the local (superlinear) convergence of the proposed method for NLP (4(4a) $\begin{array}{rl}\underset{X,U,T}{min}& J=V_f\left(x_N\right)+\sum _{k\in \mathcal{K}}\sum _{i\in {\mathcal{I}}_k}{l}_k(x_i,u_i)\mathrm{\Delta }{\tau }_k,\end{array}$(4a) ) under the SOSC and LICQ. The proof for this is omitted because it is trivial.

4.3. Reduced Hessian modification via Riccati recursion

The recused Hessian matrix can be indefinite when the solution is not sufficiently close to a local minimum, such that the SOSC does not hold. Subsequently, the KKT matrix is no longer invertible and the local convergence is not guaranteed. Efficient Cholesky factorization cannot be used to compute $G_i^{-1}$. Therefore, an algorithmic modification of the Riccati recursion is proposed to make the algorithm numerically robust and efficient for such situations, which can be considered a modification on the reduced Hessian matrix. To consider practical situations, Hessian approximations are allowed in the following, while Theorem 4.2 analyzes the exact Hessian matrix. First, we introduce the following practical assumption:

Assumption 4.1

$\left[\begin{array}{cc}{Q}_{xx,i}& Q_{xu,i}\\ Q_{xu,i}^{\mathrm{T}}& Q_{uu,i}\end{array}\right]⪰O$ and $Q_{uu,i}\succ O$ for all $i\in \{0,\dots ,N-1\}$, $Q_{tt,k}\ge 0$ for all $k\in \{0,\dots ,K\}$, and $Q_{xx,N}⪰O$.

This assumption is easily satisfied with the Gauss-Newton Hessian approximation or, more generally, with sequential convex programming (Messerer et al., 2021) for ${\mathrm{\nabla }}_{xx}{H}_k(x_i,u_i,{\lambda }_{i+1})$, ${\mathrm{\nabla }}_{xu}{H}_k(x_i,u_i,{\lambda }_{i+1})$, and ${\mathrm{\nabla }}_{uu}{H}_k(x_i,u_i,{\lambda }_{i+1})$. Under Assumption 4.1, a unique solution of the dynamic programming subproblem (25(25) $\begin{array}{rl}\underset{\mathrm{\Delta }{u}_i}{min}& \frac{1}{2}{\left[\begin{array}{c}-\mathrm{\Delta }{t}_k\\ \mathrm{\Delta }{t}_k-\mathrm{\Delta }{t}_{k-1}\\ \mathrm{\Delta }{x}_i\\ \mathrm{\Delta }{u}_i\end{array}\right]}^{\mathrm{T}}\left[\begin{array}{c}\\ & Q_{tt,i}& h_{x,i}^{\mathrm{T}}& h_{u,i}^{\mathrm{T}}\\ & h_{x,i}& Q_{xx,i}& Q_{xu,i}\\ & h_{u,i}& Q_{ux,i}& Q_{uu,i}\end{array}\right]\\ & \times \left[\begin{array}{c}-\mathrm{\Delta }{t}_k\\ \mathrm{\Delta }{t}_k-\mathrm{\Delta }{t}_{k-1}\\ \mathrm{\Delta }{x}_i\\ \mathrm{\Delta }{u}_i\end{array}\right]+{\left[\begin{array}{c}\\ {\overline{h}}_i\\ {\overline{l}}_{x,i}\\ {\overline{l}}_{u,i}\end{array}\right]}^{\mathrm{T}}\left[\begin{array}{c}-\mathrm{\Delta }{t}_k\\ \mathrm{\Delta }{t}_k-\mathrm{\Delta }{t}_{k-1}\\ \mathrm{\Delta }{x}_i\\ \mathrm{\Delta }{u}_i\end{array}\right]\\ & +\frac{1}{2}{\left[\begin{array}{c}-\mathrm{\Delta }{t}_k\\ \mathrm{\Delta }{t}_k-\mathrm{\Delta }{t}_{k-1}\\ \mathrm{\Delta }{x}_{i+1}\end{array}\right]}^{\mathrm{T}}\left[\begin{array}{ccc}{\rho }_{i+1}& {\chi }_{i+1}& {\mathrm{\Phi }}_{i+1}^{\mathrm{T}}\\ {\chi }_{i+1}& {\xi }_{i+1}& {\mathrm{\Psi }}_{i+1}^{\mathrm{T}}\\ {\mathrm{\Phi }}_{i+1}& {\mathrm{\Psi }}_{i+1}& P_{i+1}\end{array}\right]\\ & \times \left[\begin{array}{c}-\mathrm{\Delta }{t}_k\\ \mathrm{\Delta }{t}_k-\mathrm{\Delta }{t}_{k-1}\\ \mathrm{\Delta }{x}_{i+1}\end{array}\right]+{\left[\begin{array}{c}{\iota }_{i+1}\\ {\eta }_{i+1}\\ -s_{i+1}\end{array}\right]}^{\mathrm{T}}\left[\begin{array}{c}-\mathrm{\Delta }{t}_k\\ \mathrm{\Delta }{t}_k-\mathrm{\Delta }{t}_{k-1}\\ \mathrm{\Delta }{x}_{i+1}\end{array}\right]\\ \mathrm{s}.\mathrm{t}.& A_i\mathrm{\Delta }{x}_i+B_i\mathrm{\Delta }{u}_i+a_i(\mathrm{\Delta }{t}_k-\mathrm{\Delta }{t}_{k-1})-\mathrm{\Delta }{x}_{i+1}+{\overline{x}}_i=0,\end{array}$(25) ) exists (that is, $G_i\succ O$) and $P_i⪰O$ if $P_{i+1}⪰O$, which is the same discussion as the dynamic programming for standard linear quadratic OCPs (Bertsekas, 2005). The solution to (26(26) $\begin{array}{r}\underset{\mathrm{\Delta }{t}_k}{min}\left(24\right).\end{array}$(26) ) also exists if ${\sigma }_{i_k}>0$. Based on these observations, a modification of the Riccati recursion algorithm is proposed, whereby ${\tilde{P}}_{i_k}$ is updated at each phase-transition stage $i_k=min{\mathcal{I}}_k$. This is expressed as (27) $\begin{array}{r}{\tilde{P}}_{i_k}=P_{i_k}\end{array}$(27) instead of (23c(23c) $\begin{array}{rl}& {\tilde{P}}_{i_k}:=P_{i_k}-{\sigma }_{i_k}^{-1}({\mathrm{\Psi }}_{i_k}-{\mathrm{\Phi }}_{i_k})({\mathrm{\Psi }}_{i_k}-{\mathrm{\Phi }}_{i_k}{)}^{\mathrm{T}},\end{array}$(23c) ). Then, ${\tilde{P}}_{i_k}⪰O$, as long as $P_{i_k}⪰O$. Note that a different method of (27(27) $\begin{array}{r}{\tilde{P}}_{i_k}=P_{i_k}\end{array}$(27) ) can be used to make ${\tilde{P}}_{i_k}⪰O$, which eliminates the negative curvature from $P_{i_k}$ through eigenvalue decomposition (Quirynen et al., 2014). However, this requires considerably more computational time than (27(27) $\begin{array}{r}{\tilde{P}}_{i_k}=P_{i_k}\end{array}$(27) ). Nevertheless, the computationally cheap modification (27(27) $\begin{array}{r}{\tilde{P}}_{i_k}=P_{i_k}\end{array}$(27) ) works surprisingly well in practise. We also modify ${\sigma }_{i_k}$ instead of (22(22) $\begin{array}{r}{\sigma }_{i_k}:={\xi }_{i_k}-2{\chi }_{i_k}+{\rho }_{i_k}.\end{array}$(22) ), which is expressed as (28) $\begin{array}{r}{\tilde{\sigma }}_{i_k}=\{\begin{array}{ll}{\sigma }_{i_k}& {\sigma }_{i_k}>{\sigma }_{min}\\ |{\sigma }_{i_k}|+\overline{\sigma }& {\sigma }_{i_k}\le {\sigma }_{min}\end{array},\end{array}$(28) where ${\sigma }_{min}\ge 0$ and $\overline{\sigma }>0$. A practical rule to choose ${\sigma }_{\mathrm{m}\mathrm{i}\mathrm{n}}$ and $\overline{\sigma }$ is as follows. We empirically observed that numerical ill-conditioning in the Newton-type method produced a large $\mathrm{\Delta }{t}_s$, that is, a too small ${\sigma }_{i_k}$ in (21(21) $\begin{array}{rl}\mathrm{\Delta }{t}_k& =-{\sigma }_{i_k}^{-1}({\mathrm{\Psi }}_{i_k}-{\mathrm{\Phi }}_{i_k}{)}^{\mathrm{T}}\mathrm{\Delta }{x}_{i_k}\\ & -{\sigma }_{i_k}^{-1}({\xi }_{i_k}-{\chi }_{i_k})(-\mathrm{\Delta }{t}_{k-1})-{\sigma }_{i_k}^{-1}({\eta }_{i_k}-{\iota }_{i_k}).\end{array}$(21) ). From this observation, a desired maximum absolute value of $\mathrm{\Delta }{t}_s$ was chosen as $\mathrm{\Delta }{t}_{s,\mathrm{m}\mathrm{a}\mathrm{x}}>0$. Subsequently, ${\sigma }_{\mathrm{m}\mathrm{i}\mathrm{n}}$ and $\overline{\sigma }$ were chosen such that the absolute value of ${\sigma }_{i_k}^{-1}({\eta }_{i_k}-{\iota }_{i_k})$ did not exceed $\mathrm{\Delta }{t}_{s,\mathrm{m}\mathrm{a}\mathrm{x}}$. This is expressed as (29) $\begin{array}{r}{\sigma }_{min}=\overline{\sigma }=\mathrm{\Delta }{t}_{s,\mathrm{m}\mathrm{a}\mathrm{x}}^{-1}|{\eta }_{i_k}-{\iota }_{i_k}|\end{array}$(29) for each $k\in \mathcal{K}$. For example, it was discovered that choosing $\mathrm{\Delta }{t}_{s,\mathrm{m}\mathrm{a}\mathrm{x}}$ from a range of 0.1 to 1.0 worked well in practise.

The following Theorem claims that the proposed algorithmic modification makes the reduced Hessian matrix of the KKT system (10(10a) $\begin{array}{rl}& \mathrm{\Delta }{x}_0+x_0-\overline{x}=0,\end{array}$(10a) ) positive definite and the KKT matrix invertible:

Theorem 4.3

Suppose that Assumption 4.1 holds. Then, the reduced Hessian matrix of the KKT system (10a(10a) $\begin{array}{rl}& \mathrm{\Delta }{x}_0+x_0-\overline{x}=0,\end{array}$(10a) ) with the modifications (27(27) $\begin{array}{r}{\tilde{P}}_{i_k}=P_{i_k}\end{array}$(27) ) and (28(28) $\begin{array}{r}{\tilde{\sigma }}_{i_k}=\{\begin{array}{ll}{\sigma }_{i_k}& {\sigma }_{i_k}>{\sigma }_{min}\\ |{\sigma }_{i_k}|+\overline{\sigma }& {\sigma }_{i_k}\le {\sigma }_{min}\end{array},\end{array}$(28) ) is positive definite. If, in addition, the LICQ holds, then the KKT matrix is invertible.

Proof.

Under Assumption 4.1 and with the Hessian modifications (27(27) $\begin{array}{r}{\tilde{P}}_{i_k}=P_{i_k}\end{array}$(27) ) and (28(28) $\begin{array}{r}{\tilde{\sigma }}_{i_k}=\{\begin{array}{ll}{\sigma }_{i_k}& {\sigma }_{i_k}>{\sigma }_{min}\\ |{\sigma }_{i_k}|+\overline{\sigma }& {\sigma }_{i_k}\le {\sigma }_{min}\end{array},\end{array}$(28) ), $P_i⪰O$ for all $i=0,\dots ,N$, $G_i\succ O$ for all $i=0,\dots ,N-1$, and ${\sigma }_{i_k}>0$ for all $k=1,\dots ,K$ hold. Therefore, the QP subproblems of the dynamic programming (25(25) $\begin{array}{rl}\underset{\mathrm{\Delta }{u}_i}{min}& \frac{1}{2}{\left[\begin{array}{c}-\mathrm{\Delta }{t}_k\\ \mathrm{\Delta }{t}_k-\mathrm{\Delta }{t}_{k-1}\\ \mathrm{\Delta }{x}_i\\ \mathrm{\Delta }{u}_i\end{array}\right]}^{\mathrm{T}}\left[\begin{array}{c}\\ & Q_{tt,i}& h_{x,i}^{\mathrm{T}}& h_{u,i}^{\mathrm{T}}\\ & h_{x,i}& Q_{xx,i}& Q_{xu,i}\\ & h_{u,i}& Q_{ux,i}& Q_{uu,i}\end{array}\right]\\ & \times \left[\begin{array}{c}-\mathrm{\Delta }{t}_k\\ \mathrm{\Delta }{t}_k-\mathrm{\Delta }{t}_{k-1}\\ \mathrm{\Delta }{x}_i\\ \mathrm{\Delta }{u}_i\end{array}\right]+{\left[\begin{array}{c}\\ {\overline{h}}_i\\ {\overline{l}}_{x,i}\\ {\overline{l}}_{u,i}\end{array}\right]}^{\mathrm{T}}\left[\begin{array}{c}-\mathrm{\Delta }{t}_k\\ \mathrm{\Delta }{t}_k-\mathrm{\Delta }{t}_{k-1}\\ \mathrm{\Delta }{x}_i\\ \mathrm{\Delta }{u}_i\end{array}\right]\\ & +\frac{1}{2}{\left[\begin{array}{c}-\mathrm{\Delta }{t}_k\\ \mathrm{\Delta }{t}_k-\mathrm{\Delta }{t}_{k-1}\\ \mathrm{\Delta }{x}_{i+1}\end{array}\right]}^{\mathrm{T}}\left[\begin{array}{ccc}{\rho }_{i+1}& {\chi }_{i+1}& {\mathrm{\Phi }}_{i+1}^{\mathrm{T}}\\ {\chi }_{i+1}& {\xi }_{i+1}& {\mathrm{\Psi }}_{i+1}^{\mathrm{T}}\\ {\mathrm{\Phi }}_{i+1}& {\mathrm{\Psi }}_{i+1}& P_{i+1}\end{array}\right]\\ & \times \left[\begin{array}{c}-\mathrm{\Delta }{t}_k\\ \mathrm{\Delta }{t}_k-\mathrm{\Delta }{t}_{k-1}\\ \mathrm{\Delta }{x}_{i+1}\end{array}\right]+{\left[\begin{array}{c}{\iota }_{i+1}\\ {\eta }_{i+1}\\ -s_{i+1}\end{array}\right]}^{\mathrm{T}}\left[\begin{array}{c}-\mathrm{\Delta }{t}_k\\ \mathrm{\Delta }{t}_k-\mathrm{\Delta }{t}_{k-1}\\ \mathrm{\Delta }{x}_{i+1}\end{array}\right]\\ \mathrm{s}.\mathrm{t}.& A_i\mathrm{\Delta }{x}_i+B_i\mathrm{\Delta }{u}_i+a_i(\mathrm{\Delta }{t}_k-\mathrm{\Delta }{t}_{k-1})-\mathrm{\Delta }{x}_{i+1}+{\overline{x}}_i=0,\end{array}$(25) ) and (26(26) $\begin{array}{r}\underset{\mathrm{\Delta }{t}_k}{min}\left(24\right).\end{array}$(26) ) always have unique global solutions. Theorem 16.2 of Nocedal and Wright (2006) completes the first claim. The second claim then directly follows from Theorem 16.1 of Nocedal and Wright (2006).

Remark 4.1

The KKT matrix is always invertible under Assumption 4.1 and the proposed reduced Hessian modifications. Therefore, the local convergence of the Newton-type method is ensured if the resultant reduced Hessian is sufficiently close to the exact one (Messerer et al., 2021; Nocedal & Wright, 2006) and the LICQ holds.

4.4. Summary of algorithm

The single Newton iteration using the proposed Riccati recursion algorithm is summarised in Algorithm 2. After computing the KKT system (10(10a) $\begin{array}{rl}& \mathrm{\Delta }{x}_0+x_0-\overline{x}=0,\end{array}$(10a) ) (line 1), the proposed method recursively eliminates the Newton steps from the KKT system (10(10a) $\begin{array}{rl}& \mathrm{\Delta }{x}_0+x_0-\overline{x}=0,\end{array}$(10a) ) in the backward recursion (lines 3–14) and recursively computes the Newton steps in the forward recursion (lines 16–27). Finally, the Newton steps of the slack variables and Lagrange multipliers are computed from the other Newton steps (line 28) according to the primal–dual interior point method (Nocedal & Wright, 2006; Wächter & Biegler, 2006). Calculations in the proposed algorithm, such as matrix inversions or matrix–matrix multiplications, do not grow with respect to the length of the horizon N. Therefore, the computational time of the proposed method is $O\left(N\right)$ as the standard Riccati recursion algorithm (Rao et al., 1998). After computing the Newton steps, we can employ typical line-search methods, such as the merit function-based methods or filter-based methods (Nocedal & Wright, 2006), for the smooth constrained NLP (6(6a) $\begin{array}{rl}\underset{X,U,T,Z,W}{min}& \tilde{J}=V_f\left(x_N\right)+\sum _{k\in \mathcal{K}}\sum _{i\in {\mathcal{I}}_k}{l}_k(x_i,u_i)\mathrm{\Delta }{\tau }_k\\ & -ϵ\sum _{i=0}^{N-1}\ln z_i-ϵ\sum _{k=1}^{K+1}\ln w_k\end{array}$(6a) ).

5. State jumps and switching conditions

In this section, the proposed NLP formulations and Riccati recursion algorithm are further extended to switched systems involving state jumps and switching conditions, which have not yet been considered. Some classes of switched systems involve state jumps at the same time as the switch, which is expressed as (30) $\begin{array}{r}x\left(t_k\right)=f_j\left(x\right(t_k-\left)\right),f_j:{\mathbb{R}}^{n_x}\to {\mathbb{R}}^{n_x},\end{array}$(30) where $t_k-$ is the instant immediately before $t_k$. We also consider that there is a stage cost on the state immediately before the state jump. That is, it is assumed that $l_j\left(x\right(t_k-\left)\right)$ is added to the cost function (3a(3a) $\begin{array}{rl}\underset{u(\cdot ),t_1,\dots ,t_K}{min}& J=V_f\left(x\right(t_{K+1}\left)\right)+\sum _{k=1}^{K+1}{\int }_{t_{k-1}}^{t_k}{l}_k\left(x\right(\tau ),u(\tau \left)\right)\mathrm{d}\tau \end{array}$(3a) ). Moreover, such switches are often state-dependent, that is, the switch occurs if the state satisfies some condition (hereafter called the switching condition), which is expressed as (31) $\begin{array}{r}e\left(x\right(t_k-\left)\right)=0,e:{\mathbb{R}}^{n_x}\to {\mathbb{R}}^{n_e}.\end{array}$(31) For example, a legged robot can be modeled as a system with switches involving state jumps and switching conditions. When the distance between the robot's foot and the ground becomes zero (the switching condition), the generalised velocity instantly changes due to the impact (the state jump) (Farshidian et al., 2017; Li & Wensing, 2020).

If there is a state jump at the kth switch, a new grid point $i_k-$ is introduced corresponding to time $t_k-$, which is added to the KKT conditions and expressed as (32a) $\begin{array}{r}{x}_{i_k}=f_j\left(x_{i_k-}\right)\end{array}$(32a) and (32b) $\begin{array}{r}{\mathrm{\nabla }}_x{l}_j\left(x_{i_k-}\right)+{\mathrm{\nabla }}_x{f}_j^{\mathrm{T}}\left(x_{i_k-}\right){\lambda }_{i_k}-{\lambda }_{i_k-}=0.\end{array}$(32b) The KKT systems regarding the state jump are therefore expressed as (33a) $\begin{array}{r}\mathrm{\Delta }{x}_{i_k}=A_{i_k-}\mathrm{\Delta }{x}_{i_k-}+{\overline{x}}_{i_k-}\end{array}$(33a) and (33b) $\begin{array}{r}\mathrm{\Delta }{\lambda }_{i_k-}=Q_{xx,i_k-}\mathrm{\Delta }{x}_{i_k-}+A_{i_k-}^{\mathrm{T}}\mathrm{\Delta }{\lambda }_{i_k}+{\overline{l}}_{x,i_k-},\end{array}$(33b) where $A_{i_k-}:={\mathrm{\nabla }}_x{f}_j\left(x_{i_k-}\right)$ and $Q_{xx,i_k-}$ is the Hessian of the Lagrangian with respect to $x_{i_k-}$. ${\overline{x}}_{i_k}$ and ${\overline{l}}_{x,i_k-}$ are the residuals in (32a(32a) $\begin{array}{r}{x}_{i_k}=f_j\left(x_{i_k-}\right)\end{array}$(32a) ) and (32b(32b) $\begin{array}{r}{\mathrm{\nabla }}_x{l}_j\left(x_{i_k-}\right)+{\mathrm{\nabla }}_x{f}_j^{\mathrm{T}}\left(x_{i_k-}\right){\lambda }_{i_k}-{\lambda }_{i_k-}=0.\end{array}$(32b) ), respectively. When (33a(33a) $\begin{array}{r}\mathrm{\Delta }{x}_{i_k}=A_{i_k-}\mathrm{\Delta }{x}_{i_k-}+{\overline{x}}_{i_k-}\end{array}$(33a) ) and (33b(33b) $\begin{array}{r}\mathrm{\Delta }{\lambda }_{i_k-}=Q_{xx,i_k-}\mathrm{\Delta }{x}_{i_k-}+A_{i_k-}^{\mathrm{T}}\mathrm{\Delta }{\lambda }_{i_k}+{\overline{l}}_{x,i_k-},\end{array}$(33b) ) are considered in the backward recursion, we have the equations until stage $i_k=min{\mathcal{I}}_k$. That is, the equations are in the form of (23a(23a) $\begin{array}{r}\mathrm{\Delta }{\lambda }_{i_K}={\tilde{P}}_{i_K}\mathrm{\Delta }{x}_{i_K}-{\tilde{s}}_{i_K}+{\tilde{\mathrm{\Psi }}}_{i_K}(-\mathrm{\Delta }{t}_{k-1})\end{array}$(23a) ), (23b(23b) $\begin{array}{rl}& \sum _{i\in {\mathcal{I}}_{K-1}}(h_{x,i}^{\mathrm{T}}\mathrm{\Delta }{x}_i+h_{u,i}^{\mathrm{T}}\mathrm{\Delta }{u}_i+a_i^{\mathrm{T}}\mathrm{\Delta }{\lambda }_{i+1}+{\overline{h}}_i)\\ & +Q_{tt,k-1}(\mathrm{\Delta }{t}_{k-1}-\mathrm{\Delta }{t}_{k-2})+{\overline{q}}_{t,k-1}\\ & -{\tilde{\mathrm{\Phi }}}_{i_k}^{\mathrm{T}}\mathrm{\Delta }{x}_{i_k}-{\tilde{\rho }}_{i_k}(-\mathrm{\Delta }{t}_{k-1})-{\tilde{\iota }}_{i_k}=0,\end{array}$(23b) ), and (10f(10f) $\begin{array}{rl}& \frac{1}{N_k}\sum _{i\in {\mathcal{I}}_k}(h_{x,i}^{\mathrm{T}}\mathrm{\Delta }{x}_i+h_{u,i}^{\mathrm{T}}\mathrm{\Delta }{u}_i+a_i^{\mathrm{T}}\mathrm{\Delta }{\lambda }_{i+1}+{\overline{h}}_i)\\ & -\frac{1}{N_{k+1}}\sum _{i\in {\mathcal{I}}_{k+1}}(h_{x,i}^{\mathrm{T}}\mathrm{\Delta }{x}_i+h_{u,i}^{\mathrm{T}}\mathrm{\Delta }{u}_i+a_i^{\mathrm{T}}\mathrm{\Delta }{\lambda }_{i+1}+{\overline{h}}_i)\\ & +Q_{tt,k}(\mathrm{\Delta }{t}_k-\mathrm{\Delta }{t}_{k-1})-Q_{tt,k+1}(\mathrm{\Delta }{t}_{k+1}-\mathrm{\Delta }{t}_k)\\ & +{\overline{q}}_{t,k}-{\overline{q}}_{t,k+1}=0,k\in \{1,\dots ,K\},\end{array}$(10f) ), with the phase index k replaced with k−1. By eliminating $\mathrm{\Delta }{\lambda }_{i_k}$ and $\mathrm{\Delta }{x}_{i_k}$ from these equations using (33a(33a) $\begin{array}{r}\mathrm{\Delta }{x}_{i_k}=A_{i_k-}\mathrm{\Delta }{x}_{i_k-}+{\overline{x}}_{i_k-}\end{array}$(33a) ) and (33b(33b) $\begin{array}{r}\mathrm{\Delta }{\lambda }_{i_k-}=Q_{xx,i_k-}\mathrm{\Delta }{x}_{i_k-}+A_{i_k-}^{\mathrm{T}}\mathrm{\Delta }{\lambda }_{i_k}+{\overline{l}}_{x,i_k-},\end{array}$(33b) ), we then obtain (34a) $\begin{array}{rl}& \mathrm{\Delta }{\lambda }_{i_k-}=P_{i_k-}\mathrm{\Delta }{x}_{i_k-}+{\mathrm{\Phi }}_{i_k-}(-\mathrm{\Delta }{t}_{k-1})-s_{i_k-},\end{array}$(34a) (34b) $\begin{array}{rl}& \sum _{i\in {\mathcal{I}}_{k-1}}(h_{x,i}^{\mathrm{T}}\mathrm{\Delta }{x}_i+h_{u,i}^{\mathrm{T}}\mathrm{\Delta }{u}_i+a_i^{\mathrm{T}}\mathrm{\Delta }{\lambda }_{i+1}+{\overline{h}}_i)\\ & +Q_{tt,k-1}(\mathrm{\Delta }{t}_{k-1}-\mathrm{\Delta }{t}_{k-2})+{\overline{q}}_{t,k-1}\\ & -{\mathrm{\Phi }}_{i_k-}^{\mathrm{T}}\mathrm{\Delta }{x}_{i_k-}-{\rho }_{i_k-}\left(\mathrm{\Delta }{t}_{k-1}\right)-{\eta }_{i_k-}=0,\end{array}$(34b) and (34c) $\begin{array}{rl}& \sum _{i\in {\mathcal{I}}_{k-2}}(h_{x,i}^{\mathrm{T}}\mathrm{\Delta }{x}_i+h_{u,i}^{\mathrm{T}}\mathrm{\Delta }{u}_i+a_i^{\mathrm{T}}\mathrm{\Delta }{\lambda }_{i+1}+{\overline{h}}_i)\\ & -\sum _{i\in {\mathcal{I}}_{k-1}}(h_{x,i}^{\mathrm{T}}\mathrm{\Delta }{x}_i+h_{u,i}^{\mathrm{T}}\mathrm{\Delta }{u}_i+a_i^{\mathrm{T}}\mathrm{\Delta }{\lambda }_{i+1}+{\overline{h}}_i)\\ & +Q_{tt,k-2}(\mathrm{\Delta }{t}_{k-2}-\mathrm{\Delta }{t}_{k-3})-Q_{tt,k-1}(\mathrm{\Delta }{t}_{k-1}-\mathrm{\Delta }{t}_{k-2})\\ & +{\overline{q}}_{t,k-2}-{\overline{q}}_{t,k-1}=0,\end{array}$(34c) where (34d) $\begin{array}{rl}& P_{i_k-}:=Q_{xx,i_k-}+A_{i_k-}^{\mathrm{T}}{\tilde{P}}_{i_k}{A}_{i_k-},\end{array}$(34d) (34e) $\begin{array}{rl}& {\mathrm{\Phi }}_{i_k-}:=A_{i_k-}^{\mathrm{T}}{\tilde{\mathrm{\Phi }}}_{i_k},\end{array}$(34e) (34f) $\begin{array}{rl}& s_{i_k-}:=A_{i_k-}^{\mathrm{T}}({\tilde{s}}_{i_k}-{\tilde{P}}_{i_k}{\overline{x}}_{i_k-})-{\overline{l}}_{x,i_k-},\end{array}$(34f) and (34g) $\begin{array}{r}{\rho }_{i_k-}={\tilde{\rho }}_{i_k},{\eta }_{i_k-}={\tilde{\eta }}_{i_k}+{\tilde{\mathrm{\Phi }}}_{i_k}^{\mathrm{T}}{\overline{x}}_{i_k-}.\end{array}$(34g) Therefore, the same equation as (17(17a) $\begin{array}{r}\mathrm{\Delta }{\lambda }_i=P_i\mathrm{\Delta }{x}_i-s_i+{\mathrm{\Psi }}_i(\mathrm{\Delta }{t}_k-\mathrm{\Delta }{t}_{k-1})+{\mathrm{\Phi }}_i(-\mathrm{\Delta }{t}_k),\end{array}$(17a) ) is achieved at stage $i_k-$, and we can proceed to stage $i_k-1$. In the forward recursion, $\mathrm{\Delta }{x}_{i_k-}$ is computed from $\mathrm{\Delta }{x}_{i_k-1}$ and $\mathrm{\Delta }{u}_{i_k-1}$ using (10b(10b) $\begin{array}{rl}& A_i\mathrm{\Delta }{x}_i+B_i\mathrm{\Delta }{u}_i+a_i(\mathrm{\Delta }{t}_k-\mathrm{\Delta }{t}_{k-1})\\ & -\mathrm{\Delta }{x}_{i+1}+{\overline{x}}_i=0,i\in {\mathcal{I}}_k,k\in \mathcal{K},\end{array}$(10b) ) at stage $i_k-1$. Then, $\mathrm{\Delta }{x}_{i_k}$ and $\mathrm{\Delta }{\lambda }_{i_k-}$ are computed from $\mathrm{\Delta }{x}_{i_k-}$ according to (33a(33a) $\begin{array}{r}\mathrm{\Delta }{x}_{i_k}=A_{i_k-}\mathrm{\Delta }{x}_{i_k-}+{\overline{x}}_{i_k-}\end{array}$(33a) ) and (17a(17a) $\begin{array}{r}\mathrm{\Delta }{\lambda }_i=P_i\mathrm{\Delta }{x}_i-s_i+{\mathrm{\Psi }}_i(\mathrm{\Delta }{t}_k-\mathrm{\Delta }{t}_{k-1})+{\mathrm{\Phi }}_i(-\mathrm{\Delta }{t}_k),\end{array}$(17a) ) for $i=i_k-$ with ${\mathrm{\Psi }}_{i_k}=0$.

5.1. Switching conditions

The switching conditions are typically presented as pure-state equality constraints, which are difficult to treat efficiently with the Riccati recursion algorithm (specifically with the $O\left(N\right)$ computational burden) (Sideris & Rodriguez, 2011). In this section, we assume that the state is partitioned into the generalised coordinate and velocity as $x={\left[\begin{array}{cc}{q}^{\mathrm{T}}& v^{\mathrm{T}}\end{array}\right]}^{\mathrm{T}}$, $q,v\in {\mathbb{R}}^n$ and the state equation of each subsystem is expressed as (35) $\begin{array}{r}f(x,u):=\left[\begin{array}{c}{f}^{\left(q\right)}\left(x\right)\\ f^{\left(v\right)}(x,u)\end{array}\right],\end{array}$(35) where $f^{\left(q\right)}:{\mathbb{R}}^{n_x}\to {\mathbb{R}}^n$ and $f^{\left(v\right)}:{\mathbb{R}}^{n_x}\times {\mathbb{R}}^{n_u}\to {\mathbb{R}}^n$. Moreover, we assume that the pure-state equality constraints representing the switching conditions have a relative degree of two, that is, the Jacobian of (31(31) $\begin{array}{r}e\left(x\right(t_k-\left)\right)=0,e:{\mathbb{R}}^{n_x}\to {\mathbb{R}}^{n_e}.\end{array}$(31) ) is expressed as (36) $\begin{array}{r}{\mathrm{\nabla }}_{x}e\left(x\right)=\left[\begin{array}{cc}{\mathrm{\nabla }}_{q}e\left(q\right)& O\end{array}\right],{\mathrm{\nabla }}_{q}e\left(q\right)\in {\mathbb{R}}^{n_e\times n}.\end{array}$(36) These assumptions mainly represent the position-level constraints on mechanical systems, such as robotic systems having rigid contacts. To use the Newton-type method with $O\left(N\right)$ complexity under these assumptions, we did not consider the pure-state equality constraint (37) $\begin{array}{r}e(x_{i_k}-)=0\end{array}$(37) directly as was considered by Sideris and Rodriguez (2011). Instead, the constraint was transformed into a mixed state-input constraint at the two-stage grid point before the switch (Katayama & Ohtsuka, 2021a), which is expressed as (38) $\begin{array}{r}{e}_i:=e(x_i+f(x_i,u_i)\mathrm{\Delta }{\tau }_k+g(x_i+f(x_i,u_i)\mathrm{\Delta }{\tau }_k\left)\mathrm{\Delta }{\tau }_k\right)=0,\end{array}$(38) where i is two-stage before the grid $i_k-$, that is, i satisfies $i+2=i_k-$. The Lagrange multiplier is introduced with respect to (38(38) $\begin{array}{r}{e}_i:=e(x_i+f(x_i,u_i)\mathrm{\Delta }{\tau }_k+g(x_i+f(x_i,u_i)\mathrm{\Delta }{\tau }_k\left)\mathrm{\Delta }{\tau }_k\right)=0,\end{array}$(38) ), ${\zeta }_i\in {\mathbb{R}}^{n_e}$. The KKT conditions regarding this stage are then represented by (4c(4c) $\begin{array}{rl}& x_i+f_k(x_i,u_i)\mathrm{\Delta }{\tau }_k-x_{i+1}=0,i\in {\mathcal{I}}_k,k\in \mathcal{K},\end{array}$(4c) ), (38(38) $\begin{array}{r}{e}_i:=e(x_i+f(x_i,u_i)\mathrm{\Delta }{\tau }_k+g(x_i+f(x_i,u_i)\mathrm{\Delta }{\tau }_k\left)\mathrm{\Delta }{\tau }_k\right)=0,\end{array}$(38) ), ${\tilde{r}}_{x,i}:=r_{x,i}+C_i^{\mathrm{T}}{\zeta }_i=0$, and ${\tilde{r}}_{u,i}:=r_{u,i}+D_i^{\mathrm{T}}{\zeta }_i=0$, where $C_i:={\mathrm{\nabla }}_{x}e(\cdot )$ and $D_i:={\mathrm{\nabla }}_{u}e(\cdot )$. Furthermore, the KKT conditions of (9f(9f) $\begin{array}{rl}& \frac{1}{N_k}\sum _{i\in {\mathcal{I}}_k}{H}_k(x_i,u_i,{\lambda }_{i+1})\\ & -\frac{1}{N_{k+1}}\sum _{i\in {\mathcal{I}}_{k+1}}{H}_{k+1}(x_i,u_i,{\lambda }_{i+1})\\ & -{\upsilon }_k+{\upsilon }_{k+1}=0,k\in \{1,\dots ,K\}.\end{array}$(9f) ) related to $t_{k-1}$ are replaced with: $\begin{array}{rl}& E_i^{\mathrm{T}}{\zeta }_i+\frac{1}{N_{k-1}}\sum _{j\in {\mathcal{I}}_{k-1}}{H}_{k-1}(x_j,u_j,{\lambda }_{j+1})\\ & -\frac{1}{N_k}\sum _{i\in {\mathcal{I}}_k}{H}_k(x_j,u_j,{\lambda }_{j+1})-{\upsilon }_{k-1}+{\upsilon }_k=0\end{array}$and $\begin{array}{rl}& \frac{1}{N_{k-2}}\sum _{j\in {\mathcal{I}}_{k-2}}{H}_{k-2}(x_j,u_j,{\lambda }_{j+1})-E_i^{\mathrm{T}}{\zeta }_i\\ & -\frac{1}{N_{k-1}}\sum _{i\in {\mathcal{I}}_{k-1}}{H}_{k-1}(x_j,u_j,{\lambda }_{j+1})-{\upsilon }_{k-2}+{\upsilon }_{k-1}=0,\end{array}$where $E_i:=\frac{1}{N_{k-1}}{\mathrm{\nabla }}_{t_k}e(\cdot )$. The KKT systems regarding this stage are then expressed as (10b(10b) $\begin{array}{rl}& A_i\mathrm{\Delta }{x}_i+B_i\mathrm{\Delta }{u}_i+a_i(\mathrm{\Delta }{t}_k-\mathrm{\Delta }{t}_{k-1})\\ & -\mathrm{\Delta }{x}_{i+1}+{\overline{x}}_i=0,i\in {\mathcal{I}}_k,k\in \mathcal{K},\end{array}$(10b) ), (39a) $\begin{array}{rl}& C_i\mathrm{\Delta }{x}_i+D_i\mathrm{\Delta }{u}_i+E_i(\mathrm{\Delta }{t}_k-\mathrm{\Delta }{t}_{k-1})+{\overline{e}}_i=0,\end{array}$(39a) (39b) $\begin{array}{rl}& Q_{xx,i}\mathrm{\Delta }{x}_i+Q_{xu,i}\mathrm{\Delta }{u}_i+A_i^{\mathrm{T}}\mathrm{\Delta }{\lambda }_{i+1}+C_i^{\mathrm{T}}\mathrm{\Delta }{\zeta }_i-\mathrm{\Delta }{\lambda }_i\\ & +h_{x,i}(\mathrm{\Delta }{t}_k-\mathrm{\Delta }{t}_{k-1})+{\tilde{l}}_{x,i}=0,\end{array}$(39b) and (39c) $\begin{array}{rl}& Q_{xu,i}^{\mathrm{T}}\mathrm{\Delta }{x}_i+Q_{uu,i}\mathrm{\Delta }{u}_i+B_i^{\mathrm{T}}\mathrm{\Delta }{\lambda }_{i+1}+D_i^{\mathrm{T}}\mathrm{\Delta }{\zeta }_i\\ & +h_{u,i}(\mathrm{\Delta }{t}_k-\mathrm{\Delta }{t}_{k-1})+{\tilde{l}}_{u,i}=0,\end{array}$(39c) where ${\tilde{l}}_{x,i}:={\tilde{r}}_{x,i}+{\mathrm{\nabla }}_x{g}^{\mathrm{T}}(x_i,u_i)\mathrm{d}\mathrm{i}\mathrm{a}\mathrm{g}\left(z_i{)}^{-1}\right(\mathrm{d}\mathrm{i}\mathrm{a}\mathrm{g}\left({\nu }_i\right)r_{g,i}-r_{z,i})$and ${\tilde{l}}_{u,i}:={\tilde{r}}_{u,i}+{\mathrm{\nabla }}_u{g}^{\mathrm{T}}(x_i,u_i)\mathrm{d}\mathrm{i}\mathrm{a}\mathrm{g}\left(z_i{)}^{-1}\right(\mathrm{d}\mathrm{i}\mathrm{a}\mathrm{g}\left({\nu }_i\right)r_{g,i}-r_{z,i}).$In addition, ${\overline{e}}_i$ is residual in (38(38) $\begin{array}{r}{e}_i:=e(x_i+f(x_i,u_i)\mathrm{\Delta }{\tau }_k+g(x_i+f(x_i,u_i)\mathrm{\Delta }{\tau }_k\left)\mathrm{\Delta }{\tau }_k\right)=0,\end{array}$(38) ).

In the backward recursion at stage i, we have (14a(14a) $\begin{array}{rl}\mathrm{\Delta }{\lambda }_{i+1}& =P_{i+1}\mathrm{\Delta }{x}_{i+1}-s_{i+1}+{\mathrm{\Psi }}_{i+1}(\mathrm{\Delta }{t}_k-\mathrm{\Delta }{t}_{k-1})\\ & +{\mathrm{\Phi }}_{i+1}(-\mathrm{\Delta }{t}_k),\end{array}$(14a) ), (40a) $\begin{array}{rl}& \sum _{j\in {\mathcal{I}}_{k-1},j\le i}(h_{x,j}^{\mathrm{T}}\mathrm{\Delta }{x}_j+h_{u,j}^{\mathrm{T}}\mathrm{\Delta }{u}_j+f_j^{\mathrm{T}}\mathrm{\Delta }{\lambda }_{j+1}+{\overline{h}}_j)\\ & +E_i^{\mathrm{T}}(\mathrm{\Delta }{\zeta }_i+{\zeta }_i)+{\mathrm{\Psi }}_{i+1}^{\mathrm{T}}\mathrm{\Delta }{x}_{i+1}+{\xi }_{i+1}(\mathrm{\Delta }{t}_{k-1}-\mathrm{\Delta }{t}_{k-2})\\ & +{\chi }_{i+1}(-\mathrm{\Delta }{t}_{k-1})+{\eta }_{i+1}\\ & -{\mathrm{\Phi }}_{i+1}^{\mathrm{T}}\mathrm{\Delta }{x}_{i+1}-{\chi }_{i+1}(\mathrm{\Delta }{t}_{k-1}-\mathrm{\Delta }{t}_{k-2})\\ & -{\rho }_{i+1}(-\mathrm{\Delta }{t}_{k-1})-{\iota }_{i+1}=0,\end{array}$(40a) and (40b) $\begin{array}{rl}& \sum _{j\in {\mathcal{I}}_{k-2}}(h_{x,j}^{\mathrm{T}}\mathrm{\Delta }{x}_j+h_{u,j}^{\mathrm{T}}\mathrm{\Delta }{u}_j+f_j^{\mathrm{T}}\mathrm{\Delta }{\lambda }_{j+1}+{\overline{h}}_j)\\ & -\sum _{j\in {\mathcal{I}}_{k-1},j\le i}(h_{x,j}^{\mathrm{T}}\mathrm{\Delta }{x}_j+h_{u,j}^{\mathrm{T}}\mathrm{\Delta }{u}_j+f_j^{\mathrm{T}}\mathrm{\Delta }{\lambda }_{j+1}+{\overline{h}}_j)\\ & +Q_{tt,k-2}(\mathrm{\Delta }{t}_{k-2}-\mathrm{\Delta }{t}_{k-3})+{\overline{q}}_{t,k-2}\\ & -{\mathrm{\Psi }}_{i+1}^{\mathrm{T}}\mathrm{\Delta }{x}_{i+1}-{\xi }_{i+1}(\mathrm{\Delta }{t}_{k-1}-\mathrm{\Delta }{t}_{k-2})-{\chi }_{i+1}(-\mathrm{\Delta }{t}_{k-1})\\ & -{\eta }_{i+1}+E_i^{\mathrm{T}}(\mathrm{\Delta }{\zeta }_i+{\zeta }_i)=0.\end{array}$(40b) $\mathrm{\Delta }{u}_i$ and $\mathrm{\Delta }{\zeta }_i$ are then eliminated, resulting in: (41a) $\begin{array}{rl}\left[\begin{array}{c}\mathrm{\Delta }{u}_i\\ \mathrm{\Delta }{\zeta }_i\end{array}\right]& =\left[\begin{array}{c}{K}_i^{\mathrm{T}}\\ M_i\end{array}\right]\mathrm{\Delta }{x}_i+\left[\begin{array}{c}{T}_i\\ L_i\end{array}\right](\mathrm{\Delta }{t}_{k-1}-\mathrm{\Delta }{t}_{k-2})\\ & +\left[\begin{array}{c}{W}_i\\ N_i\end{array}\right](-\mathrm{\Delta }{t}_{k-1})+\left[\begin{array}{c}{k}_i\\ m_i\end{array}\right],\end{array}$(41a) where (41b) $\begin{array}{rl}& \left[\begin{array}{c}{K}_i\\ M_i\end{array}\right]:=-{\left[\begin{array}{cc}{G}_i& D_i^{\mathrm{T}}\\ D_i& \end{array}\right]}^{-1}\left[\begin{array}{c}{H}_i^{\mathrm{T}}\\ C_i\end{array}\right],\end{array}$(41b) (41c) $\begin{array}{rl}& \left[\begin{array}{c}{T}_i\\ L_i\end{array}\right]:=-{\left[\begin{array}{cc}{G}_i& D_i^{\mathrm{T}}\\ D_i& \end{array}\right]}^{-1}\left[\begin{array}{c}{\psi }_{u,i}\\ E_i\end{array}\right],\end{array}$(41c) (41d) $\begin{array}{rl}& \left[\begin{array}{c}{W}_i\\ N_i\end{array}\right]:=-{\left[\begin{array}{cc}{G}_i& D_i^{\mathrm{T}}\\ D_i& \end{array}\right]}^{-1}\left[\begin{array}{c}{\varphi }_{u,i}\\ 0\end{array}\right],\end{array}$(41d) and (41e) $\begin{array}{r}\left[\begin{array}{c}{k}_i\\ m_i\end{array}\right]:=-{\left[\begin{array}{cc}{G}_i& D_i^{\mathrm{T}}\\ D_i& \end{array}\right]}^{-1}\left[\begin{array}{c}{B_i}^{\mathrm{T}}(P_{i+1}{\overline{x}}_i-s_i)+{\tilde{l}}_{u,i}\\ {\overline{e}}_i\end{array}\right].\end{array}$(41e) Therefore, we obtain the form equations of (17a(17a) $\begin{array}{r}\mathrm{\Delta }{\lambda }_i=P_i\mathrm{\Delta }{x}_i-s_i+{\mathrm{\Psi }}_i(\mathrm{\Delta }{t}_k-\mathrm{\Delta }{t}_{k-1})+{\mathrm{\Phi }}_i(-\mathrm{\Delta }{t}_k),\end{array}$(17a) ), (18a(18a) $\begin{array}{rl}& \sum _{j\in {\mathcal{I}}_k,j\le i-1}(h_{x,j}^{\mathrm{T}}\mathrm{\Delta }{x}_j+h_{u,j}^{\mathrm{T}}\mathrm{\Delta }{u}_j+a_j^{\mathrm{T}}\mathrm{\Delta }{\lambda }_{j+1}+{\overline{h}}_j)\\ & +{\mathrm{\Psi }}_i^{\mathrm{T}}\mathrm{\Delta }{x}_i+{\xi }_i(\mathrm{\Delta }{t}_k-\mathrm{\Delta }{t}_{k-1})+{\chi }_i(-\mathrm{\Delta }{t}_k)+{\eta }_i\\ & -{\mathrm{\Phi }}_i^{\mathrm{T}}\mathrm{\Delta }{x}_i-{\chi }_i(\mathrm{\Delta }{t}_k-\mathrm{\Delta }{t}_{k-1})-{\rho }_i(-\mathrm{\Delta }{t}_k)-{\iota }_i=0\end{array}$(18a) ), and (18b(18b) $\begin{array}{rl}& \sum _{j\in {\mathcal{I}}_{k-1}}(h_{x,j}^{\mathrm{T}}\mathrm{\Delta }{x}_j+h_{u,j}^{\mathrm{T}}\mathrm{\Delta }{u}_j+a_j^{\mathrm{T}}\mathrm{\Delta }{\lambda }_{j+1}+{\overline{h}}_j)\\ & -\sum _{j\in {\mathcal{I}}_k,j\le i-1}(h_{x,j}^{\mathrm{T}}\mathrm{\Delta }{x}_j+h_{u,j}^{\mathrm{T}}\mathrm{\Delta }{u}_j+a_j^{\mathrm{T}}\mathrm{\Delta }{\lambda }_{j+1}+{\overline{h}}_j)\\ & +Q_{tt,k-1}(\mathrm{\Delta }{t}_{k-1}-\mathrm{\Delta }{t}_{k-2})+{\overline{q}}_{t,k-1}\\ & -{\mathrm{\Psi }}_i^{\mathrm{T}}\mathrm{\Delta }{x}_i-{\xi }_i(\mathrm{\Delta }{t}_k-\mathrm{\Delta }{t}_{k-1})-{\chi }_i(-\mathrm{\Delta }{t}_k)-{\eta }_i=0,\end{array}$(18b) ) for stage i with the following: (42a) $\begin{array}{rl}& P_i=F_i-{\left[\begin{array}{c}{K}_i\\ M_i\end{array}\right]}^{\mathrm{T}}\left[\begin{array}{cc}{G}_i& D_i^{\mathrm{T}}\\ D_i& \end{array}\right]\left[\begin{array}{c}{K}_i\\ M_i\end{array}\right],\end{array}$(42a) (42b) $\begin{array}{rl}& s_i=-\{{\tilde{l}}_{x,i}+A_i^{\mathrm{T}}(P_{i+1}{\overline{x}}_i-s_{i+1})+H_i{k}_i+C_i^{\mathrm{T}}{m}_i\},\end{array}$(42b) and (42c) $\begin{array}{r}{\mathrm{\Psi }}_i={\psi }_{x,i}+{\left[\begin{array}{c}{K}_i\\ M_i\end{array}\right]}^{\mathrm{T}}\left[\begin{array}{c}{\psi }_{u,i}\\ E_i\end{array}\right],{\mathrm{\Phi }}_i={\varphi }_{x,i}+{\left[\begin{array}{c}{K}_i\\ M_i\end{array}\right]}^{\mathrm{T}}\left[\begin{array}{c}{\varphi }_{u,i}\\ 0\end{array}\right].\end{array}$(42c) (43a) $\begin{array}{rl}& {\xi }_i={\xi }_{i+1}+a_i^{\mathrm{T}}(P_{i+1}{a}_i+2{\mathrm{\Psi }}_{i+1})+{\psi }_{u,i}^{\mathrm{T}}{T}_i+E_i^{\mathrm{T}}{L}_i,\end{array}$(43a) (43b) $\begin{array}{rl}& {\eta }_i={\eta }_{i+1}+{\overline{h}}_i+E_i^{\mathrm{T}}{\zeta }_i+a_i^{\mathrm{T}}(P_{i+1}{\overline{x}}_i-s_{i+1})\\ & +{\mathrm{\Psi }}_{i+1}^{\mathrm{T}}{\overline{x}}_i+{\psi }_{u,i}^{\mathrm{T}}{k}_i+E_i^{\mathrm{T}}{m}_i,\end{array}$(43b) (43c) $\begin{array}{rl}& {\chi }_i={\chi }_{i+1}+{\mathrm{\Phi }}_{i+1}^{\mathrm{T}}{a}_i+{\psi }_{u,i}^{\mathrm{T}}{W}_i+E_i^{\mathrm{T}}{N}_i,\end{array}$(43c) (43d) $\begin{array}{rl}& {\rho }_i={\rho }_{i+1}+{\varphi }_{u,i}^{\mathrm{T}}{W}_i,\end{array}$(43d) and (43e) $\begin{array}{r}{\iota }_i={\iota }_{i+1}+{\mathrm{\Phi }}_{i+1}^{\mathrm{T}}{\overline{x}}_i+{\varphi }_{u,i}^{\mathrm{T}}{k}_i.\end{array}$(43e) At the forward recursion, $\mathrm{\Delta }{u}_i$ and $\mathrm{\Delta }{\zeta }_i$ are computed using (41a(41a) $\begin{array}{rl}\left[\begin{array}{c}\mathrm{\Delta }{u}_i\\ \mathrm{\Delta }{\zeta }_i\end{array}\right]& =\left[\begin{array}{c}{K}_i^{\mathrm{T}}\\ M_i\end{array}\right]\mathrm{\Delta }{x}_i+\left[\begin{array}{c}{T}_i\\ L_i\end{array}\right](\mathrm{\Delta }{t}_{k-1}-\mathrm{\Delta }{t}_{k-2})\\ & +\left[\begin{array}{c}{W}_i\\ N_i\end{array}\right](-\mathrm{\Delta }{t}_{k-1})+\left[\begin{array}{c}{k}_i\\ m_i\end{array}\right],\end{array}$(41a) ) instead of only computing $\mathrm{\Delta }{u}_i$ as (16a(16a) $\begin{array}{r}\mathrm{\Delta }{u}_i=K_i\mathrm{\Delta }{x}_i+k_i+T_i(\mathrm{\Delta }{t}_k-\mathrm{\Delta }{t}_{k-1})+W_i(-\mathrm{\Delta }{t}_k),\end{array}$(16a) ).

5.2. Summary of algorithm

We herein summarise the proposed algorithm for the OCPs with state jumps and switching conditions as an extension of Algorithm 2. The overall algorithm is the same as Algorithm 2 except for the computations around $i=i_{k-}$ and $i=i_{k-}-2$ for $k=1,\dots ,K+1$ The computation of the KKT system (line 1 of Algorithm 2) now includes the computation regarding state jumps such as (33a(33a) $\begin{array}{r}\mathrm{\Delta }{x}_{i_k}=A_{i_k-}\mathrm{\Delta }{x}_{i_k-}+{\overline{x}}_{i_k-}\end{array}$(33a) ) and (33b(33b) $\begin{array}{r}\mathrm{\Delta }{\lambda }_{i_k-}=Q_{xx,i_k-}\mathrm{\Delta }{x}_{i_k-}+A_{i_k-}^{\mathrm{T}}\mathrm{\Delta }{\lambda }_{i_k}+{\overline{l}}_{x,i_k-},\end{array}$(33b) ) when $i=i_{k-}$ and these regarding switching conditions such as (39a(39a) $\begin{array}{rl}& C_i\mathrm{\Delta }{x}_i+D_i\mathrm{\Delta }{u}_i+E_i(\mathrm{\Delta }{t}_k-\mathrm{\Delta }{t}_{k-1})+{\overline{e}}_i=0,\end{array}$(39a) )–(39c(39c) $\begin{array}{rl}& Q_{xu,i}^{\mathrm{T}}\mathrm{\Delta }{x}_i+Q_{uu,i}\mathrm{\Delta }{u}_i+B_i^{\mathrm{T}}\mathrm{\Delta }{\lambda }_{i+1}+D_i^{\mathrm{T}}\mathrm{\Delta }{\zeta }_i\\ & +h_{u,i}(\mathrm{\Delta }{t}_k-\mathrm{\Delta }{t}_{k-1})+{\tilde{l}}_{u,i}=0,\end{array}$(39c) ) when $i=i_{k-}-2$ for $k=1,\dots ,K+1$. In the backward recursion (lines 3–14 of Algorithm 2), we compute the terms such as $P_i$ and $s_i$ using (34d(34d) $\begin{array}{rl}& P_{i_k-}:=Q_{xx,i_k-}+A_{i_k-}^{\mathrm{T}}{\tilde{P}}_{i_k}{A}_{i_k-},\end{array}$(34d) )–(34g(34g) $\begin{array}{r}{\rho }_{i_k-}={\tilde{\rho }}_{i_k},{\eta }_{i_k-}={\tilde{\eta }}_{i_k}+{\tilde{\mathrm{\Phi }}}_{i_k}^{\mathrm{T}}{\overline{x}}_{i_k-}.\end{array}$(34g) ) when $i=i_{k-}$ and (42a(42a) $\begin{array}{rl}& P_i=F_i-{\left[\begin{array}{c}{K}_i\\ M_i\end{array}\right]}^{\mathrm{T}}\left[\begin{array}{cc}{G}_i& D_i^{\mathrm{T}}\\ D_i& \end{array}\right]\left[\begin{array}{c}{K}_i\\ M_i\end{array}\right],\end{array}$(42a) )–(43e(43e) $\begin{array}{r}{\iota }_i={\iota }_{i+1}+{\mathrm{\Phi }}_{i+1}^{\mathrm{T}}{\overline{x}}_i+{\varphi }_{u,i}^{\mathrm{T}}{k}_i.\end{array}$(43e) ) when $i=i_{k-}-2$ for $k=1,\dots ,K+1$. In the forward recursion (lines 16–27 of Algorithm 2), we additionally utilise (33a(33a) $\begin{array}{r}\mathrm{\Delta }{x}_{i_k}=A_{i_k-}\mathrm{\Delta }{x}_{i_k-}+{\overline{x}}_{i_k-}\end{array}$(33a) ) and (33b(33b) $\begin{array}{r}\mathrm{\Delta }{\lambda }_{i_k-}=Q_{xx,i_k-}\mathrm{\Delta }{x}_{i_k-}+A_{i_k-}^{\mathrm{T}}\mathrm{\Delta }{\lambda }_{i_k}+{\overline{l}}_{x,i_k-},\end{array}$(33b) ) to compute $\mathrm{\Delta }{x}_{i_{k-}}$ and $\mathrm{\Delta }{\lambda }_{i_{k-}}$ when $i=i_{k-}$ and (41a(41a) $\begin{array}{rl}\left[\begin{array}{c}\mathrm{\Delta }{u}_i\\ \mathrm{\Delta }{\zeta }_i\end{array}\right]& =\left[\begin{array}{c}{K}_i^{\mathrm{T}}\\ M_i\end{array}\right]\mathrm{\Delta }{x}_i+\left[\begin{array}{c}{T}_i\\ L_i\end{array}\right](\mathrm{\Delta }{t}_{k-1}-\mathrm{\Delta }{t}_{k-2})\\ & +\left[\begin{array}{c}{W}_i\\ N_i\end{array}\right](-\mathrm{\Delta }{t}_{k-1})+\left[\begin{array}{c}{k}_i\\ m_i\end{array}\right],\end{array}$(41a) ) to compute $\mathrm{\Delta }{u}_i$ and $\mathrm{\Delta }{\zeta }_i$ when $i=i_{k-}-2$ for $k=1,\dots ,K+1$.

6. Numerical experiments

6.1. Comparison with off-the-shelf solvers

6.1.1. Problem settings

The effectiveness of the proposed method was demonstrated through two numerical experiments. The first experiment was a comparison with off-the-shelf NLP solvers on the switched system, consisting of three nonlinear subsystems treated in Xu and Antsaklis (2002, 2004). The dynamics of the subsystems are expressed as $\begin{array}{rl}& f_1(x,u)=\left[\begin{array}{c}{x}_1+u_1\sin \left(x_1\right)\\ -x_2-u_1\cos \left(x_2\right)\end{array}\right],\\ & f_2(x,u)=\left[\begin{array}{c}{x}_2+u_1\sin \left(x_2\right)\\ -x_1-u_1\cos \left(x_1\right)\end{array}\right],\end{array}$and $f_3(x,u)=\left[\begin{array}{c}-x_1-u_1\sin \left(x_1\right)\\ x_2+u_1\cos \left(x_2\right)\end{array}\right].$The stage cost is expressed as $l_k(x,u)=\frac{1}{2}\Vert x-x_{\mathrm{r}\mathrm{e}\mathrm{f}}{\Vert }^2+\Vert u{\Vert }^2,k\in \{1,2,3\}.$In addition, the terminal cost is expressed as $V_f\left(x\right)=\frac{1}{2}\Vert x-x_{\mathrm{r}\mathrm{e}\mathrm{f}}{\Vert }^2,$where $x_{\mathrm{r}\mathrm{e}\mathrm{f}}=[1,-1{]}^{\mathrm{T}}$. The initial and terminal times of the horizon are denoted by $t_0=0$ and $t_3=3$, respectively. The initial state is denoted by $x\left(t_0\right)=[2,3{]}^{\mathrm{T}}$.

Benchmarks were set for the solution times of the NLP (4(4a) $\begin{array}{rl}\underset{X,U,T}{min}& J=V_f\left(x_N\right)+\sum _{k\in \mathcal{K}}\sum _{i\in {\mathcal{I}}_k}{l}_k(x_i,u_i)\mathrm{\Delta }{\tau }_k,\end{array}$(4a) ) example against the Ipopt (Wächter & Biegler, 2006) and a SQP methods with qpOASES (Ferreau et al., 2014), both of which were used through the CasADi (Andersson et al., 2019) interface. The mesh-refinement was not considered in this comparison to focus on a pure comparison of the abilities to solve the NLP problems. That is, only the single NLP step of Algorithm 1 was considered. Ipopt was used with the default settings of CasADi. For example, MUMPS (Amestoy et al., 2001) was used to solve the linear systems. The built-in SQP solver of CasADi was used with the exact Hessian matrix and qpOASES as the backend QP solver. The Hessian regularization was enabled within the qpOASES options and the Hessian type was set to ‘indefinite’. The proposed method was written in C++, used Eigen (Guennebaud & Jacob, 2010) for the linear algebra, and used an exact Hessian matrix and reduced Hessian modifications (27(27) $\begin{array}{r}{\tilde{P}}_{i_k}=P_{i_k}\end{array}$(27) ) and (28(28) $\begin{array}{r}{\tilde{\sigma }}_{i_k}=\{\begin{array}{ll}{\sigma }_{i_k}& {\sigma }_{i_k}>{\sigma }_{min}\\ |{\sigma }_{i_k}|+\overline{\sigma }& {\sigma }_{i_k}\le {\sigma }_{min}\end{array},\end{array}$(28) ) throughout the experiments. ${\sigma }_{\mathrm{m}\mathrm{i}\mathrm{n}}$ and $\overline{\sigma }$ were chosen using (29(29) $\begin{array}{r}{\sigma }_{min}=\overline{\sigma }=\mathrm{\Delta }{t}_{s,\mathrm{m}\mathrm{a}\mathrm{x}}^{-1}|{\eta }_{i_k}-{\iota }_{i_k}|\end{array}$(29) ) with $\mathrm{\Delta }{t}_{k,\mathrm{m}\mathrm{a}\mathrm{x}}=0.5$. The proposed method only used the fraction-to-boundary rule (Nocedal & Wright, 2006; Wächter & Biegler, 2006) for the step size selection and monotonically decreased the barrier parameter ϵ when the $l_2$ norm of the KKT residual (in the perturbed KKT conditions (9(9a) $\begin{array}{rl}& r_{x,N}:={\mathrm{\nabla }}_x{V}_f\left(x_N\right)-{\lambda }_N=0,\end{array}$(9a) )) was smaller than 0.1. The solution times were compared for these algorithms with the total number of grids set as N = 10, 50, 100, and 500. N was divided into $N_1$, $N_2$, and $N_3$, so that each phase had an almost equal number of discretization grids. The values of $[N_1,N_2,N_3]$ were set to $[4,3,3]$, $[17,17,16]$, $[34,33,33]$, and $[167,167,166]$ for N = 10, 50, 100, and 500, respectively. The minimum dwell-times were set as ${\underset{\_}{\mathrm{\Delta }}}_1={\underset{\_}{\mathrm{\Delta }}}_2={\underset{\_}{\mathrm{\Delta }}}_3=0.01$ in the constraints (4e(4e) $\begin{array}{rl}& t_{k-1}+{\underset{\_}{\mathrm{\Delta }}}_k-t_k\le 0,k\in \mathcal{K}.\end{array}$(4e) ) to avoid ill-conditioned problems. The initial guess of the solution was set as $x_0=x_1=\cdots =x_N=x\left(t_0\right)$, $u_0=u_1=\cdots u_{N-1}=0$, $t_1=1.0$, and $t_2=2.0$ for all solvers. It was assumed that the proposed method converged when the solution satisfied the default convergence criteria of Ipopt (max norm of the residual in the unperturbed KKT conditions (Nocedal & Wright, 2006; Wächter & Biegler, 2006)). All solvers were run 100 times for each N on a laptop with a quad-core CPU Intel Core i7-10510U @1.80 GHz, and the average computational time until convergence was measured.

6.1.2. Results

The average computational times of each solver for the different total number of grids N are listed in Table 1. As shown in the table, the proposed method converged the fastest of all the cases, and was up to two orders of magnitude faster than Ipopt. The proposed method was extremely fast because the dimension of the control input of each subsystem was 1 and it did not involve any explicit matrix inversions. The SQP method with qpOASES failed to converge for all cases, even the regularization and indefinite-Hessian mode were enabled. Note that another QP solver OSQP (Stellato et al., 2020) was also tested, but it failed to treat the indefinite Hessian matrix.

Table 1. Average computational time (ms) of each solver for different total number of grids.

N	Proposed	Ipopt	SQP with qpOASES
10	0.08	4.3	failed
50	0.27	24.6	failed
100	0.47	45.1	failed
500	1.93	127	failed

The total computational time of Algorithm 1 until convergence was also measured, including the mesh-refinement steps. The thresholds for the discretization step sizes were $\mathrm{\Delta }{\tau }_{\mathrm{m}\mathrm{a}\mathrm{x}}=0.35$, 0.065, 0.035, and 0.0065 for cases N = 10, 50, 100, and 500, respectively. The total computational times of Algorithm 1 for N = 10, 50, 100, and 500, were 0.13, 0.57, 0.92, and 4.3 ms, respectively. The mesh-refinements were only conducted a few times in each case, and a similar solution was achieved compared to that of the continuous-time counterpart reported in Xu and Antsaklis (2004). As expected, the solution accuracy improved as N increased.

6.2. Whole-body optimal control of quadrupedal gaits

6.2.1. Problem settings

Numerical experiments on the whole-body optimal control of a quadrupedal robot ANYmal (Hutter et al., 2017) were conducted to demonstrate the efficiency of the proposed method in practical and complicated examples. A trotting motion with a step length of 0.15 m and aggressive jumping motion with a jump length of 0.8 m also investigated. The contact patterns of the trotting and jumping motions are shown in Figure 2. The state equation of the robot switched based on the combination of the support feet, that is, it switched when the feet lifted off from the ground or touched down onto the ground. Therefore, the trotting motion had four switches and the jump motion had two switches over the horizon, as shown in Figure 2. The lift-off did not include the state jump or the switching condition, but the touch-down included both (Farshidian et al., 2017; Li & Wensing, 2020; Schultz & Mombaur, 2010). The switching condition was treated using the proposed method in Section 5.1: the switching conditions were the position constraints on the foot whose Jacobian took the form of (36(36) $\begin{array}{r}{\mathrm{\nabla }}_{x}e\left(x\right)=\left[\begin{array}{cc}{\mathrm{\nabla }}_{q}e\left(q\right)& O\end{array}\right],{\mathrm{\nabla }}_{q}e\left(q\right)\in {\mathbb{R}}^{n_e\times n}.\end{array}$(36) ), and the state equation of the robot under an acceleration-level rigid-contact constraint took the form of (35(35) $\begin{array}{r}f(x,u):=\left[\begin{array}{c}{f}^{\left(q\right)}\left(x\right)\\ f^{\left(v\right)}(x,u)\end{array}\right],\end{array}$(35) ). The details of the above formulations can be found in Li and Wensing (2020); Schultz and Mombaur (2010). Note also that the acceleration-level contact-consistent dynamics of the robot were lifted to improve the convergence speed without changing the structure of the KKT system (10(10a) $\begin{array}{rl}& \mathrm{\Delta }{x}_0+x_0-\overline{x}=0,\end{array}$(10a) ) (Katayama & Ohtsuka, 2021b). To consider a practical situation, the limitations that were imposed on the joint angles, velocities, and torques were considered to be the inequality constraints. The polyhedral-approximated friction cone constraint was also considered for each contact force expressed in the world frame $\left[f_x{f}_y{f}_z\right]$ as: (44) $\begin{array}{r}\left[\begin{array}{c}{f}_x+{\displaystyle \frac{\mu }{\sqrt{2}}}{f}_z\\ -f_x+{\displaystyle \frac{\mu }{\sqrt{2}}}{f}_z\\ f_y+{\displaystyle \frac{\mu }{\sqrt{2}}}{f}_z\\ -f_y+{\displaystyle \frac{\mu }{\sqrt{2}}}{f}_z\\ f_z\end{array}\right]\ge 0,\end{array}$(44) where $\mu >0$ is the friction coefficient, which was set as $\mu =0.7$. Note that the friction cone constraint (44(44) $\begin{array}{r}\left[\begin{array}{c}{f}_x+{\displaystyle \frac{\mu }{\sqrt{2}}}{f}_z\\ -f_x+{\displaystyle \frac{\mu }{\sqrt{2}}}{f}_z\\ f_y+{\displaystyle \frac{\mu }{\sqrt{2}}}{f}_z\\ -f_y+{\displaystyle \frac{\mu }{\sqrt{2}}}{f}_z\\ f_z\end{array}\right]\ge 0,\end{array}$(44) ) also switched depending on the active subsystems (that is, depending on the combination of the support feet). The initial and terminal times of the horizon were set as $t_0=0$ and $t_5=1.0$ for the trotting problem and $t_0=0$ and $t_2=1.7$ for the jumping problem, respectively.

Figure 2. Contact patterns of trotting and jumping motions. Gray and white cells indicate the intervals where the leg is standing and swinging, respectively. Thick lines illustrate the switches of the active subsystems. Black triangles indicate the types of switches (lift-off: L-O, touch-down: T-D).

To design the stage cost of the trotting motion, the state $x\in {\mathbb{R}}^{36}$ was partitioned into the generalised coordinate and velocity $[q^{\mathrm{T}}{v}^{\mathrm{T}}{]}^{\mathrm{T}}$, $q,v\in {\mathbb{R}}^{18}$. Additionally, the position (i.e. forward kinematics) of the foot $i\in$ {Left-front, Left-hip, Right-front, Right-hip} was considered in the world frame $p_i\left(q\right)\in {\mathbb{R}}^3$. The stage cost was then designed for the trotting motion, which is expressed as (45) $\begin{array}{rl}& \frac{1}{2}(q-q_{\mathrm{r}\mathrm{e}\mathrm{f}}{)}^{\mathrm{T}}{W}_q(q-q_{\mathrm{r}\mathrm{e}\mathrm{f}})\\ & +\frac{1}{2}(v-v_{\mathrm{r}\mathrm{e}\mathrm{f}}{)}^{\mathrm{T}}{W}_v(v-v_{\mathrm{r}\mathrm{e}\mathrm{f}})+\frac{1}{2}{a}^{\mathrm{T}}{W}_{a}a\\ & +\sum _{i\in \left\{\mathrm{S}\mathrm{w}\mathrm{i}\mathrm{n}\mathrm{g}\mathrm{l}\mathrm{e}\mathrm{g}\mathrm{s}\right\}}\frac{1}{2}\left(p_i\right(q)-p_{i,\mathrm{r}\mathrm{e}\mathrm{f}}{)}^{\mathrm{T}}{W}_p(p_i\left(q\right)-p_{i,\mathrm{r}\mathrm{e}\mathrm{f}}),\end{array}$(45) where $a\in {\mathbb{R}}^{18}$ is the generalised acceleration, $q_{\mathrm{r}\mathrm{e}\mathrm{f}},v_{\mathrm{r}\mathrm{e}\mathrm{f}}\in {\mathbb{R}}^{18}$ and $p_{i,\mathrm{r}\mathrm{e}\mathrm{f}}\in {\mathbb{R}}^3$ are reference values, and $W_q,W_v,W_a\in {\mathbb{R}}^{18\times 18}$ and $W_p\in {\mathbb{R}}^{3\times 3}$ are diagonal weight matrices. $q_{\mathrm{r}\mathrm{e}\mathrm{f}}$ was chosen as the generalised coordinate of the standing pose of the robot. We set the desired translational velocity of the floating base of the robot as $v_{\mathrm{r}\mathrm{e}\mathrm{f}}$ and set the other element of $v_{\mathrm{r}\mathrm{e}\mathrm{f}}$ as zero. The plane coordinate of $p_{i,\mathrm{r}\mathrm{e}\mathrm{f}}$ was set as a middle point of the two successive predefined ground locations of the foot, and its vertical coordinate was set as the desired height of the swing foot. The last term in (45(45) $\begin{array}{rl}& \frac{1}{2}(q-q_{\mathrm{r}\mathrm{e}\mathrm{f}}{)}^{\mathrm{T}}{W}_q(q-q_{\mathrm{r}\mathrm{e}\mathrm{f}})\\ & +\frac{1}{2}(v-v_{\mathrm{r}\mathrm{e}\mathrm{f}}{)}^{\mathrm{T}}{W}_v(v-v_{\mathrm{r}\mathrm{e}\mathrm{f}})+\frac{1}{2}{a}^{\mathrm{T}}{W}_{a}a\\ & +\sum _{i\in \left\{\mathrm{S}\mathrm{w}\mathrm{i}\mathrm{n}\mathrm{g}\mathrm{l}\mathrm{e}\mathrm{g}\mathrm{s}\right\}}\frac{1}{2}\left(p_i\right(q)-p_{i,\mathrm{r}\mathrm{e}\mathrm{f}}{)}^{\mathrm{T}}{W}_p(p_i\left(q\right)-p_{i,\mathrm{r}\mathrm{e}\mathrm{f}}),\end{array}$(45) ) changed depending on the swing legs, that is, the stage cost also switched as the active subsystem. The terminal cost $V_f\left(x_N\right)$ and impulse cost $l\left(x_{i_k-}\right)$ were set as the sums of the first and second terms of (45(45) $\begin{array}{rl}& \frac{1}{2}(q-q_{\mathrm{r}\mathrm{e}\mathrm{f}}{)}^{\mathrm{T}}{W}_q(q-q_{\mathrm{r}\mathrm{e}\mathrm{f}})\\ & +\frac{1}{2}(v-v_{\mathrm{r}\mathrm{e}\mathrm{f}}{)}^{\mathrm{T}}{W}_v(v-v_{\mathrm{r}\mathrm{e}\mathrm{f}})+\frac{1}{2}{a}^{\mathrm{T}}{W}_{a}a\\ & +\sum _{i\in \left\{\mathrm{S}\mathrm{w}\mathrm{i}\mathrm{n}\mathrm{g}\mathrm{l}\mathrm{e}\mathrm{g}\mathrm{s}\right\}}\frac{1}{2}\left(p_i\right(q)-p_{i,\mathrm{r}\mathrm{e}\mathrm{f}}{)}^{\mathrm{T}}{W}_p(p_i\left(q\right)-p_{i,\mathrm{r}\mathrm{e}\mathrm{f}}),\end{array}$(45) ), respectively. Similarly, the stage cost was designed for the jumping motions, which is expressed as (46) $\begin{array}{r}\frac{1}{2}(q-q_{\mathrm{r}\mathrm{e}\mathrm{f}}{)}^{\mathrm{T}}{W}_q(q-q_{\mathrm{r}\mathrm{e}\mathrm{f}})+\frac{1}{2}{v}^{\mathrm{T}}{W}_{v}v+\frac{1}{2}{a}^{\mathrm{T}}{W}_{a}a.\end{array}$(46) In addition, the terminal cost $V_f\left(x_N\right)$ and impulse cost $l\left(x_{i_k-}\right)$ were set as the sums of the first and second terms of (46(46) $\begin{array}{r}\frac{1}{2}(q-q_{\mathrm{r}\mathrm{e}\mathrm{f}}{)}^{\mathrm{T}}{W}_q(q-q_{\mathrm{r}\mathrm{e}\mathrm{f}})+\frac{1}{2}{v}^{\mathrm{T}}{W}_{v}v+\frac{1}{2}{a}^{\mathrm{T}}{W}_{a}a.\end{array}$(46) ), respectively, but with different weight parameters. The minimum-dwell times in (4e(4e) $\begin{array}{rl}& t_{k-1}+{\underset{\_}{\mathrm{\Delta }}}_k-t_k\le 0,k\in \mathcal{K}.\end{array}$(4e) ) for the trotting motion were set as ${\underset{\_}{\mathrm{\Delta }}}_1={\underset{\_}{\mathrm{\Delta }}}_3={\underset{\_}{\mathrm{\Delta }}}_5=0.02$ and ${\underset{\_}{\mathrm{\Delta }}}_2={\underset{\_}{\mathrm{\Delta }}}_4=0.2$. The minimum-dwell times were set for the jumping motion as ${\underset{\_}{\mathrm{\Delta }}}_1={\underset{\_}{\mathrm{\Delta }}}_2=0.2$ and ${\underset{\_}{\mathrm{\Delta }}}_3=0.7$. A large ${\underset{\_}{\mathrm{\Delta }}}_3$ was set in the jumping motion to sufficiently observe the whole-body motion after touch-down. This was because the optimiser tended to make the touch-down time very close to the end of the horizon to minimise the overall cost of the OCP without a large ${\underset{\_}{\mathrm{\Delta }}}_3$.

The proposed algorithms were implemented in C++, and Eigen was used for the linear algebra. The efficient Pinocchio C++ library was used (Carpentier et al., 2019) for rigid-body dynamics (Featherstone, 2008) and its analytical derivatives (Carpentier & Mansard, 2018) to compute the dynamics and its derivatives of the quadrupedal robot. The Gauss-Newton Hessian approximation was used to avoid computing the second-order derivatives of the rigid-body dynamics. The reduced Hessian modifications (27(27) $\begin{array}{r}{\tilde{P}}_{i_k}=P_{i_k}\end{array}$(27) ) and (28(28) $\begin{array}{r}{\tilde{\sigma }}_{i_k}=\{\begin{array}{ll}{\sigma }_{i_k}& {\sigma }_{i_k}>{\sigma }_{min}\\ |{\sigma }_{i_k}|+\overline{\sigma }& {\sigma }_{i_k}\le {\sigma }_{min}\end{array},\end{array}$(28) ) were used in the proposed Riccati recursion. ${\sigma }_{\mathrm{m}\mathrm{i}\mathrm{n}}$ and $\overline{\sigma }$ were chosen using (29(29) $\begin{array}{r}{\sigma }_{min}=\overline{\sigma }=\mathrm{\Delta }{t}_{s,\mathrm{m}\mathrm{a}\mathrm{x}}^{-1}|{\eta }_{i_k}-{\iota }_{i_k}|\end{array}$(29) ) with $\mathrm{\Delta }{t}_{k,\mathrm{m}\mathrm{a}\mathrm{x}}=0.1$. Only the fraction-to-boundary rule (Nocedal & Wright, 2006; Wächter & Biegler, 2006) was used for the step-size selection. The barrier parameter was fixed at $ϵ=1.0\times {10}^{-3}$, which is a common suboptimal MPC setting (Wang & Boyd, 2010). OpenMP (Dagum & Menon, 1998) was used for parallel computing of the KKT system (10(10a) $\begin{array}{rl}& \mathrm{\Delta }{x}_0+x_0-\overline{x}=0,\end{array}$(10a) ) in stage-wise and eight threads through the following experiments. These two experiments were conducted on a desktop computer with an octa-core CPU Intel Core i9-9900 @3.10 GHz. $\mathrm{\Delta }{\tau }_{\mathrm{m}\mathrm{a}\mathrm{x}}=0.02$ was used for the mesh refinement. The total number of horizon grids was fixed to N = 50 and N = 90 for the trotting and jumping problems, respectively. That is, when grids were added to the mesh-refinement phase, the same number of grids were removed from the other phases. The proposed method converged when the $l_2$ norm of the KKT residual was smaller than a sufficiently small threshold $1.0\times {10}^{-7}$ and each $\mathrm{\Delta }{\tau }_k$ was smaller than $\mathrm{\Delta }{\tau }_{\mathrm{m}\mathrm{a}\mathrm{x}}$. Mesh refinement was performed when the $l_2$ norm of the KKT residual became moderately small (smaller than 0.1). After the mesh refinement, the iterates $x_i$, $u_i$, and ${\lambda }_i$ were reconstructed by the linear interpolation of these variables that were stored before the mesh refinement. This implementation is available online as a part of our efficient optimal control solvers for robotic systems, called robotoc (Katayama, 2020).

6.2.2. Results

The convergence results of the trotting and jumping motions are shown in Figures 3 and 4, respectively. For the trotting motion that included a mesh-refinement step, the proposed method converged after 40 iterations. The total computational time was 52 ms (1.3 ms per Newton iteration). For the jumping motion that included two mesh-refinement steps, the proposed method converged after 82 iterations. The total computational time was 172 ms (2.1 ms per Newton iteration). The snapshots of the solution trajectory of the aggressive jumping motion are shown in Figure 5, and the time histories of the contact forces of the four feet in the jumping motion are shown in Figure 6. We can see that the contact forces lie inside the friction cone constraints throughout the jumping motion. These two figures show that the friction cone constraints (44(44) $\begin{array}{r}\left[\begin{array}{c}{f}_x+{\displaystyle \frac{\mu }{\sqrt{2}}}{f}_z\\ -f_x+{\displaystyle \frac{\mu }{\sqrt{2}}}{f}_z\\ f_y+{\displaystyle \frac{\mu }{\sqrt{2}}}{f}_z\\ -f_y+{\displaystyle \frac{\mu }{\sqrt{2}}}{f}_z\\ f_z\end{array}\right]\ge 0,\end{array}$(44) ) were active at time intervals immediately before and after the jumping, and the solution strictly satisfied the constraints.

Figure 3. Convergence of the proposed method for the trotting motion of a quadrupedal robot: (a) switching instants and (b) $l_2$ norm of the residual in the perturbed KKT conditions over the iterations. Vertical dotted lines indicate that the mesh refinement was performed.

Figure 4. Convergence of the proposed method for the jumping motion of the quadrupedal robot: (a) switching instants and (b) $l_2$ norm of the residual in the perturbed KKT conditions over the iterations. Vertical dotted lines indicate that the mesh refinement was carried out.

Figure 5. Snapshots of solution trajectory of the whole-body optimal control of ANYmal's 0.8 m aggressive jumping motion. The yellow arrows and blue polyhedrons represent the contact forces and friction cone constraints, respectively, which are not illustrated while the robot is flying.

Figure 6. Time histories of the contact force expressed in the world frame $\left[f_x{f}_y{f}_z\right]$ of each leg in the jumping motion. The infeasible regions of $f_x$ (solid lines) and $f_y$ (dashed lines) due to the friction cone constraints are the filled gray hatches. The infeasible region of $f_z\ge 0$ (dash-dotted lines) is in the lower-half of each plot.

The above results showed that the proposed method could achieve fast convergence and computational times per Newton iteration, even for large-scale (36-dimensional state, 12-dimensional control input) and complicated problems. In MPC, the shorter length of the horizon and smaller number of discretization grids can typically be considered, and warm-starting can be leveraged. For example, the computational time per Newton iteration for the trotting problem can be expected to be less than half of 1.3 ms if the number of grids was reduced to 20. Therefore, the proposed method is a promising approach that can achieve the whole-body MPC of robotics systems having rigid contacts within a milliseconds-range sampling time.

7. Conclusion

This study proposed an efficient Newton-type method for optimal control of switched systems under a given mode sequence. A direct multiple-shooting method with adaptive mesh refinement was formulated to guarantee the local convergence of the Newton-type method for the NLP. A dedicated structure-exploiting Riccati recursion algorithm was proposed that performed the Newton-type method for the NLP with the linear time-complexity of the total number of discretization grids. Moreover, the proposed method could always solve the KKT systems if the solution was sufficiently close to a local minimum. Conversely, general QP solvers cannot be guaranteed to solve the KKT systems because the Hessian matrix is inherently indefinite. Moreover, to enhance the convergence, a modification on the reduced Hessian matrix was proposed using the nature of the Riccati recursion algorithm as the dynamic programming for a QP subproblem. A numerical comparison was conducted with off-the-shelf solvers and demonstrated that the proposed method was up to two orders of magnitude faster. Whole-body optimal control of quadrupedal gaits was also investigated and it was demonstrated that the proposed method could achieve the whole-body MPC of robotic systems with rigid contacts.

A possible future extension of the proposed method is OCPs with free final time, including minimum-time OCPs (Bryson & Ho, 1975), because the NLP structure of such problems is expected to be similar to the proposed formulation.

Disclosure statement

No potential conflict of interest was reported by the author(s).

Data availability statement

The data of 6.2 can be reproduced by our open-source software https://github.com/mayataka/robotoc. Other data that support the findings of this study are also available from the corresponding author, Sotaro Katayama, upon reasonable request.

Additional information

Funding

This work was partly supported by JST SPRING, Grant Number JPMJSP2110, and JSPS KAKENHI, Grant Numbers JP22J11441 and JP22H01510.