remarkRemark hypothesisHypothesis claimClaim HIGH-ORDER DLQR and ILQR
METHODSZuodi Xie and Tieqiang Gang ex_supplement

# DISCRETE LQR and ILQR METHODS BASED ON HIGH ORDER RUNGE-KUTTA METHODS

Zuodi Xie Xiamen University, Xiamen<EMAIL_ADDRESS>Tieqiang Gang
Corresponding author. Xiamen University, Xiamen<EMAIL_ADDRESS>
###### Abstract

In this paper, discrete linear quadratic regulator (DLQR) and iterative linear
quadratic regulator (ILQR) methods based on high-order Runge-Kutta (RK)
discretization are proposed for solving linear and nonlinear quadratic optimal
control problems respectively. As discovered in [W. Hager, Runge-Kutta method
in optimal control and the discrete adjoint system, Numer. Math., 2000, pp.
247-282], direct approach with RK discretization is equivalent with indirect
approach based on symplectic partitioned Runge-Kutta (SPRK) integration. In
this paper, we will reconstruct this equivalence by the analogue of continuous
and discrete dynamic programming. Then, based on the equivalence, we discuss
the issue that the internal-stage controls produced by direct approach may
have lower order accuracy than the RK method used. We propose order conditions
for internal-stage controls and then demonstrate that third or fourth order
explicit RK discretization cannot avoid the order reduction phenomenon. To
overcome this obstacle, we calculate node control instead of internal-stage
controls in DLQR and ILQR methods. And numerical examples will illustrate the
validity of our methods. Another advantage of our methods is high
computational efficiency which comes from the usage of feedback technique. In
this paper, we also demonstrate that ILQR is essentially a quasi-Newton method
with linear convergence rate.

###### keywords:

direct approach for optimal control, symplectic partitioned Runge-Kutta
method, LQR, ILQR

93C15, 65L06, 49M25, 49N10

##  1 Introduction

The numerical approaches for solving optimal control problem (OCP) are
generally divided into two branches: one is indirect approach which aims at
solving the canonical equations derived from Pontryagin Maximum Principle
(PMP); the other is direct approach which suggests discretizing the state
equations and cost function directly and transform the original problem into a
finite-dimensional optimization problem [3]. In this paper, we restrict the
cost function as quadratic. In the framework of direct approach, this paper
aims at two tasks:

Task (1): How to make sure the calculated control values have the same
accuracy order as the RK discretization?

Task (2): How to employ linear feedback technique (specifically, DLQR and ILQR
methods) to solve the optimization problem after high-order RK discretization?

During the last years, much work has emerged in studying the convergence
properties of RK discretization in OCP ([4, 7, 10, 21, 24]). Though the
convergence properties of RK methods applied in ordinary differential
equations have been studied deeply, the analysis in OCP is usually more
complicated. As exemplified in [7, 10], some RK discretization may not
converge when solving OCP. In addition, when control or state is under
constraints, stricter algebraic conditions must be satisfied by RK methods to
ensure convergence or high accuracy (related work can be seen in [7, 24]). In
the non-constraint case, there exists an equivalence between direct approach
based on RK discretization and indirect approach with corresponding SPRK
integration [7, 23]. Based on this fact, Hager [10] proposed the notation of
RK method’s order in OCP and computed the order conditions up to four. Then,
Bonnans and Laurent-Varin [4] extended the order conditions to 7. Under the
smooth and coercivity assumptions, a convergence order analysis for RK method
in OCP is given in [10].

When applying a $s$-stage RK discretization in direct approach, one will
obtain $s$ internal-stage controls. The main obstacle to increase the accuracy
by utilizing high-order RK method, as discovered in [10], is that order
reduction phenomenon may happen in these internal control values. Though this
problem has been referred in some papers (see [1, 2, 22]), deep analysis is
still lack as we know. In this paper, we will give the convergence analysis of
internal-stage controls and highlight calculating node control to overcome
this obstacle.

After RK discretization, the OCP with quadratic objective function and linear
state equations will be transformed into a discrete linear quadratic problem
which can be solved by the linear feedback technique called DLQR method in
this paper. When the state equations in OCP and the corresponding constraints
in optimization problem are nonlinear, ILQR method, first proposed by Li and
Todorov [14], can be employed. As a simple version of differential dynamic
programming (DDP) method [17], ILQR uses iterative linearization of nonlinear
system and searches for control modifications by solving discrete linear
quadratic regulator problems [14]. The prominent advantage of ILQR over other
optimization technique such as sequential quadratic programming (SQP) [13] is
that the search direction for iteration is obtained by the linear feedback
technique, which saves the computational cost [27]. Though ILQR method cannot
achieve quadratic convergence as DDP, the formal only requires calculation of
the first order derivative of dynamical system and the matrix operation in
which takes less computational payment [9]. As an efficient method to solve
discrete optimal control problem, ILQR has been successfully applied in the
fields of robotic and stochastic control [8, 15, 26].

As remarked in [18], technique like ILQR, in which state equations are
linearized, cannot attain quadratic convergence. In [9], the convergence rate
of ILQR is claimed as linear but a detailed analysis is lack there.

###  1.1 Overview and notations

The rest of this paper is organized as following.

In Section 2, we will reconstruct the equivalence between direct and indirect
approaches within a dynamic programming viewpoint and discuss an important
prerequisite to ensure this equivalence. In Section 3, we deduce order
conditions for internal-stage controls and reveal that some common RK
discretization cannot avoid the order reduction problem. In Section 4, we
propose DLQR method based on high-order RK discretization for solving the
linear quadratic OCP. Section 5 aims at solving the nonlinear quadratic OCP
with ILQR method and gives a convergence analysis for the iterative progress
of ILQR. The numerical examples will be given in Sections 4 and 5 to show the
advantages of our methods.

In this paper, $x\in\mathbb{R}^{n}$ is regarded as column vector and equipped
with the standard Euclidean norm $||x||=(x^{T}x)^{1/2}$. For several vectors
$x_{1},...,x_{m}$, we use the notation
$(x_{1},...,x_{m})=[x_{1}^{T},...,x_{m}^{T}]^{T}$ as the composed vector. The
$n$-dimension identity matrix is denoted as $I_{n}$. We introduce the
following notation

$\begin{bmatrix}A(1)&&0\\\ &\ddots&\\\ 0&&A(m)\end{bmatrix}$

to signify the diagonal of a quasi-diagonal matrix composed of
$m\in\mathbb{R}$ identical matrices $A$.

Let $n_{i}$ be any positive integer $(i=1,2,3)$. For a differentiable function
with two variables
$F:\mathbb{R}^{n_{1}}\times\mathbb{R}^{n_{2}}\to\mathbb{R}^{n_{3}}$,
$D_{x}F(x,y)$ (or $D_{y}F(x,y)$) denotes the partial derivative. The transpose
of $D_{x}F(x,y)$ is denoted as $D_{x}^{T}F(x,y)$. For a differentiable
function with single variable $G:\mathbb{R}^{n_{1}}\to\mathbb{R}^{n_{2}}$,
$G^{\prime}(x)$ and $G^{\prime\prime}(x)$ denote the first and second order
derivatives respectively. Specially, when $n_{2}=1$, $G^{\prime}(x)$ and
$G^{\prime\prime}(x)$ signify the gradient and Hesse matrix of $G$ at $x$
respectively.

The coefficients of a $s$-stage partitioned RK method are displayed in Table
1. For any $i\in\left\\{1,...,s\right\\}$, the coefficients
$c_{i}={\textstyle\sum_{j=1}^{s}}a_{ij}$ and
$\bar{c}_{i}={\textstyle\sum_{j=1}^{s}}\bar{a}_{ij}$.

Table 1: Coefficients of $s$-stage partitioned RK method $c_{1}$ | $a_{11}$ | $\cdots$ | $a_{1s}$ |  |  | $\bar{c}_{1}$ | $\bar{a}_{11}$ | $\cdots$ | $\bar{a}_{1s}$
---|---|---|---|---|---|---|---|---|---
$\vdots$ | $\vdots$ |  | $\vdots$ |  |  | $\vdots$ | $\vdots$ |  | $\vdots$
$c_{s}$ | $a_{s1}$ | $\cdots$ | $a_{ss}$ |  |  | $\bar{c}_{s}$ | $\bar{a}_{s1}$ | $\cdots$ | $\bar{a}_{ss}$
| $b_{1}$ | $\cdots$ | $b_{s}$ |  |  |  | $\bar{b}_{1}$ | $\cdots$ | $\bar{b}_{s}$

##  2 The equivalence of direct approach and SPRK discretization for PMP
canonical equations

As deduced in [10], direct approach based on RK discretization is eventually
equivalent with the corresponding SPRK integration for PMP canonical
equations. In this section, we will reconstruct the equivalence by the
analogue of continuous and discrete dynamic programming. The main purposes
include two aspects: one is to reveal this equivalence in a more natural way;
the other is to make some theoretical preparations for DLQR and ILQR methods
introduced in Section 4 and 5.

Consider the unconstrained optimal control problem:
Problem 2.1

(1) $\min_{u}J=\min_{u}\int_{0}^{t_{f}}C(x(t),u(t))dt+\Phi(x(t_{f}))$

subject to

(2) $\frac{\mathrm{d}x}{\mathrm{d}t}=f(x,u),\ x(0)=a,\ t\in[0,t_{f}],$

where state variable $x(t)\in\mathbb{R}^{n}$, control $u(t)\in\mathbb{R}^{m}$,
$f:\mathbb{R}^{n}\times\mathbb{R}^{m}\to\mathbb{R}^{n}$ and $J$ denotes the
cost function with running cost
$C:\mathbb{R}^{n}\times\mathbb{R}^{m}\to\mathbb{R}$ and terminal cost
$\Phi:\mathbb{R}^{n}\to\mathbb{R}$. We assure suitable conditions are
satisfied to ensure the existence, uniqueness and smoothness of the solution
to this problem (More details can be seen in [6, 10, 25]).

To study this problem in a continuous form, we first introduce the value
function

(3)
$V(t,x)\triangleq\min_{u_{[t,t_{f}]}}\left\\{\int_{t}^{t_{f}}C(x(s),u(s))ds+\Phi(x(t_{f}))\right\\}$

where $x(s)$ $(t\leq s\leq t_{f})$, satisfying $x(t)=x$, is the state
trajectory under control $u(s)$ $(t\leq s\leq t_{f})$. Then according to the
principle of optimality (refer to Section 5.1.2 in [16]), we have

(4) $V(t,x)=\min_{u_{[t,t+\Delta t]}}\left\\{\int_{t}^{t+\Delta
t}C(x(s),u(s))ds+V(t+\Delta t,x(t+\Delta t))\right\\}$

for every $(t,x)\in[0,t_{f})\times\mathbb{R}^{n}$ and $\Delta
t\in(0,t_{f}-t]$. We additionally assume the value function is smooth enough,
then it can be determined by the Hamilton-Jacobi-Bellman (HJB) equation:

(5) $-V_{t}(t,x)=\min_{u}\left\\{C(x,u)+D_{x}V(t,x)\cdot f(x,u)\right\\}$

with boundary condition $V(t_{f},x)=\Phi(x(t_{f}))$. As mentioned in Section
7.1.2 of [16], the characteristics of this partial differential equation are
actually the canonical equations deduced from the maximum principle, namely,

(6a) $\displaystyle\frac{dx}{dt}$ $\displaystyle=f(x,u),\ x(0)=a,$ (6b)
$\displaystyle\frac{dp}{dt}$ $\displaystyle=-D_{x}^{T}f(x,u)p-D_{x}C(x,u),\
p(t_{f})=\Phi^{\prime}(x(t_{f})),$ (6c) $\displaystyle D_{u}^{T}$
$\displaystyle f(x,u)p+D_{u}C(x,u)=0$

where the costate variable $p(t)$, $t\in[0,t_{f}]$, satisfies

(7) $p(t)=D_{x}V(t,x).$

Suppose the equation (6c) implicitly determines a function $u=\hat{u}(x,p)$,
then we can transform the canonical equations (6a)-(6c) into an autonomous
form

(8a) $\displaystyle\frac{dx}{dt}$ $\displaystyle=f(x,\hat{u}(x,p)),\ x(0)=a,$
(8b) $\displaystyle\frac{dp}{dt}$
$\displaystyle=-D_{x}^{T}f(x,\hat{u}(x,p))p-D_{x}C(x,\hat{u}(x,p)),\
p(t_{f})=\Phi^{\prime}(x(t_{f})).$

To solve Problem 2.1 numerically in the framework of direct approach, we
construct the discrete time sequence $\left\\{t_{k}=kh,\
k=0,\cdots,t_{f}/h\right\\}$ and utilize a $s$-stage RK method with
coefficients $a_{ij},b_{i}$ for discretization, then we can get the following
optimization problem:
Problem 2.2

(9)
$\min_{u_{ki}}J_{d}=\min_{u_{ki}}\left\\{h{\textstyle\sum_{k=0}^{N-1}}{\textstyle\sum_{i=1}^{s}}b_{i}C(x_{ki},u_{ki})+\phi(x_{N})\right\\}$

subject to

(10a) $\displaystyle x_{k+1}$
$\displaystyle=x_{k}+h{\textstyle\sum_{i=1}^{s}}b_{i}f(x_{ki},u_{ki}),$
$\displaystyle k$ $\displaystyle=0,...,N-1,$ (10b) $\displaystyle x_{ki}$
$\displaystyle=x_{k}+h{\textstyle\sum_{j=1}^{s}}a_{ij}f(x_{kj},u_{kj}),$
$\displaystyle k$ $\displaystyle=0,...,N-1,\ i=1,...,s.$

Similar to the continuous case, we define the discrete value function for
Problem 2.2 as following

(11)
$V_{k}(x_{k})=\min_{u_{li}}\left\\{h{\textstyle\sum_{l=k}^{N-1}}{\textstyle\sum_{i=1}^{s}}b_{i}C(x_{li},u_{li})+\Phi(x_{N})\right\\},\
0\leq k\leq N-1,$

and $V_{N}(x_{N})=\Phi(x_{N}).$ Then the discrete HJB equation with internal-
stage controls is given by [20]:

(12) $\displaystyle V_{k}(x_{k})=$
$\displaystyle\min_{u_{k1},...,u_{ks}}\\{h{\textstyle\sum_{i=1}^{s}}b_{i}C(x_{ki},u_{ki})+V_{k+1}(x_{k+1})\\}$
$\displaystyle=$
$\displaystyle\min_{u_{k1},...,u_{ks}}\left\\{h{\textstyle\sum_{i=1}^{s}}b_{i}C(x_{ki},u_{ki})+V_{k+1}(x_{k}+h{\textstyle\sum_{i=1}^{s}}b_{i}f(x_{ki},u_{ki}))\right\\},$

$0\leq k\leq N-1$, with boundary condition $V_{N}(x_{N})=\Phi(x_{N})$.

With all the preparations above, now we can introduce the main deductive
result of this section and the proof is left in Appendix A.

######  Proposition 2.1.

If $b_{i}>0\ (1\leq i\leq s)$ and the time step $h$ is small enough, then the
solution ${\left\\{u_{ki}:0\leq k\leq N-1,\ 1\leq i\leq s\right\\}}$ of
Problem 2.2, can be determined by the following discrete system:

(13a) $\displaystyle x_{k+1}$
$\displaystyle=x_{k}+h{\textstyle\sum_{i=1}^{s}}b_{i}f(x_{ki},u_{ki}),$ (13b)
$\displaystyle x_{ki}$
$\displaystyle=x_{k}+h{\textstyle\sum_{j=1}^{s}}a_{ij}f(x_{kj},u_{ki}),$ (13c)
$\displaystyle p_{k+1}$
$\displaystyle=p_{k}-h{\textstyle\sum_{i=1}^{s}}b_{i}(D_{x}^{T}f(x_{ki},u_{ki})p_{ki}+D_{x}C(x_{ki},u_{ki})),$
(13d) $\displaystyle p_{ki}$
$\displaystyle=p_{k}-h{\textstyle\sum_{i=j}^{s}}\bar{a}_{ij}(D_{x}^{T}f(x_{kj},u_{ki})p_{kj}+D_{x}C(x_{kj},u_{ki})),$

$0\leq k\leq N-1$, $1\leq i\leq s$, with boundary conditions $x_{0}=a$ and
$p_{N}=\Phi^{\prime}(x_{N})$, where

(14a) $\displaystyle u_{ki}$ $\displaystyle=\hat{u}(x_{ki},p_{ki}),$ (14b)
$\displaystyle p_{k}$ $\displaystyle=(V_{k})^{\prime}(x_{k}),$ (14c)
$\displaystyle\bar{a}_{ij}$ $\displaystyle=b_{j}-\frac{b_{j}a_{ji}}{b_{i}}.$

######  Remark 2.2.

The conclusion in Proposition 2.1 also can be obtained by introducing
Lagrangian multiplies for state constraints (10a) and (10b) and then deducing
the KKT conditions for Problem 2.2, as in [10]. In fact, according to (14b),
the Lagrangian multiplies is equivalent to the gradient of discrete value
function, which can be taken as the discrete version of (7) in continuous
setting.

Note that the expression (14c) is actually the symplectic condition for
partitioned RK methods (see Theorem 4.6 in chapter VI of [11]), we can
conclude that the solution of direct approach based on RK discretization also
can be reached by the corresponding SPRK integration of canonical systems (8a)
and (8b). Consequently, it is natural to introduce the following definition
which was first proposed in [10] :

######  Definition 2.3.

A RK method with coefficients $a_{ij},b_{i}>0$ is of order r in OCP, if the
corresponding SPRK method with coefficients $a_{ij}$,
$\bar{a}_{ij}=b_{j}-b_{j}a_{ji}/b_{i}$, $b_{i}=\bar{b}_{i}$ has order $r$.

In Table 2-4, we present three explicit RK methods (notated as A, B and C
methods) and their respective adjoint part. One can verify that A, B and C
methods are of order 2,3 and 4 in OCP, respectively (refer to chapter III.2 in
[11] or Table 1 in [10]).

Table 2: A Method (left) and its adjoint part (right) 0 |  |  |  |  | 0 | 1/2 | -1/2
---|---|---|---|---|---|---|---
1 | 1 |  |  |  | 1 | 1/2 | 1/2
| 1/2 | 1/2 |  |  |  | 1/2 | 1/2
Table 3: B Method (left) and its adjoint part (right) 0 |  |  |  |  |  | 0 | 1/6 | -4/3 | 7/6
---|---|---|---|---|---|---|---|---|---
1/2 | 1/2 |  |  |  |  | 1/2 | 1/6 | 2/3 | -1/3
1 | -1 | 2 |  |  |  | 1 | 1/6 | 2/3 | 1/6
| 1/6 | 2/3 | 1/6 |  |  |  | 1/6 | 2/3 | 1/6
Table 4: C Method (left) and its adjoint part (right) 0 |  |  |  |  |  |  | 0 | 1/6 | -2/3 | 1/3 | 1/6
---|---|---|---|---|---|---|---|---|---|---|---
1/2 | 1/2 |  |  |  |  |  | 1/2 | 1/6 | 1/3 | -1/6 | 1/6
1/2 | 0 | 1/2 |  |  |  |  | 1/2 | 1/6 | 1/3 | 1/3 | -1/3
1 | 0 | 0 | 1 |  |  |  | 1 | 1/6 | 1/3 | 1/3 | 1/6
| 1/6 | 1/3 | 1/3 | 1/6 |  |  |  | 1/6 | 1/3 | 1/3 | 1/6

Though the equivalence revealed by Proposition 2.1 is indeed an elegant
result, one restriction should be stated here. As is shown in Figure 1, the
internal-stage control $u_{ki}$ is usually taken as the final result to
approximate the optimal control at time $t_{k}+c_{i}h$. However, when the
coefficients of the RK method satisfy $c_{i}=c_{j},i\neq j$ (C method in Table
4, with coefficients $c_{2}=c_{3}=1/2$, for example), there may be two
different internal-stage controls for the same time $t_{k}+c_{i}h$. Therefore,
it seems to be more reasonable to add an internal-stage overlap constraint
$u_{ki}=u_{kj}$. However, under this kind of constraint, the equivalence
stated in Proposition 2.1 will not hold any longer and two difficulties will
arise in practice: on the one hand, more stringent algebraic conditions must
be satisfied by the RK method to ensure the convergence or high-order accuracy
(refer to [7]); on the other hand, the optimization problem after RK
discretization will become more elaborate and the linear feedback technique
proposed in Section 4 and 5 fails to work. Fortunately, analysis in the next
section will show that we can achieve high-order accuracy without considering
the internal-stage overlap constraint.

Figure 1: Internal-stage controls $u_{k1},...,u_{ks}$ in the time interval
$[t_{k},t_{k+1}]$.

##  3 Error estimate and internal-stage control problem

Because of the equivalence stated in Section 2, to analysis the convergence of
a direct approach with RK discretization, it suffices to study the numerical
integration for the canonical equations with corresponding SPRK method. By
doing so, Hager proved the following theorem.

######  Theorem 3.1 (Error estimate for node values [10]).

Suppose a $s$-stage RK method of order r in OCP is used in direct approach for
solving problem (1), then there exists unique solution $x_{k},\ p_{k}$ for
discrete system (13a)-(13d) when time step $h$ is small enough, such that

(15) $\displaystyle\max_{0\leq k\leq
N}\left||x_{k}-x^{*}(t_{k})\right||+\left||p_{k}-p^{*}(t_{k})\right||+\left||\hat{u}(x_{k},p_{k})-u^{*}(t_{k})\right||\leq
O(h^{r}),$

where $(x^{*}(t),p^{*}(t))$ is the exact solution of canonical equations (8a)
and (8b), and $u^{*}(t)=\hat{u}(x^{*}(t),p^{*}(t))$ is the optimal control for
Problem (1).

The error estimate above is not about internal-stage controls
$||u_{ki}-u^{*}(t_{k}+c_{i}h)||$ $(1\leq i\leq s)$ but about node control
$||\hat{u}(x_{k},p_{k})-u^{*}(t_{k})||$. As remarked and exemplified in [10],
the node control calculated by $u_{k}=\hat{u}(x_{k},p_{k})$ usually converges
faster than the internal-stage controls $``for\ Runge-Kutta\ schemes\ of\
third\ or\ fourth\ order"$. Indeed, as we will clarify later, such internal-
stage control problem is so common that some RK methods with second order
cannot avoid it. Let us first give the following definition.

######  Definition 3.2.

In the setting of Theorem 3.1, the $i$-th $(i\in\left\\{1,...,s\right\\})$
internal-stage control $u_{ki}$ has order $n$, if for all sufficiently regular
Problems (1), the error satisfies

$\max_{0\leq k\leq N-1}\left\\{||u_{ki}-u^{*}(t_{k}+c_{i}h)||\right\\}\leq
O(h^{n}).$

As a main result of this section, we give the following order conditions for
internal-stage controls and the proof is left to the end of this section.

######  Theorem 3.3 (Order conditions for internal-stage controls).

For $i\in\left\\{1,...,s\right\\}$, suppose the RK method in Theorem 3.1, with
order $r$ in OCP, satisfies

(16a)
$\displaystyle{\textstyle\sum_{j=1}^{s}}a_{ij}c_{j}^{l-2}=\frac{c_{i}^{l-1}}{l-1},\
l=2,...,q_{1}\leq r,$ (16b)
$\displaystyle{\textstyle\sum_{j=1}^{s}}\bar{a}_{ij}c_{j}^{l-2}=\frac{c_{i}^{l-1}}{l-1},\
l=2,...,q_{2}\leq r,$

then the order of the $i$-th internal-stage control $u_{ki}$ is

(17) $n=\begin{cases}1&\text{ if }c_{i}\neq\bar{c}_{i},\\\
\min(q_{1},q_{2})&\text{ if }c_{i}=\bar{c}_{i}.\end{cases}$

Conversely, to ensure $u_{ki}$ has order $n$ $(2\leq n\leq r)$, the conditions
$c_{i}=\bar{c}_{i}$ and $\min(q_{1},q_{2})=n$ must be satisfied for the RK
method.

Some necessary explanations for Theorem 3.3 are given in the remarks below.

######  Remark 3.4.

The orders of internal-stage controls are less than or equal to the order of
RK method used, that is why we emphasize $n\leq r$ in Theorem 3.3.

######  Remark 3.5.

The condition $c_{i}=\bar{c}_{i}$ is crucial for the $i$-th internal-stage
control to attain high order. Note that A, B and C methods in Table 2-4
satisfy $c_{i}=\bar{c}_{i}$ for all $i\in\left\\{1,...,s\right\\}$,
consequently all the internal-stage controls produced by them have at least
second order. Nevertheless, for the second-order implicit trapezoidal method,
as is shown in Table 5 that $c_{1}=0\neq\bar{c}_{1}=1/2$ and
$c_{2}=1\neq\bar{c}_{2}=1/2$, the two internal-stage controls $u_{k1}$ and
$u_{k2}$ have just order 1.

######  Remark 3.6.

The internal-stage controls may converge faster than the result in Theorem 3.3
when Problem (1) is in some special form. For example, when the function
$u=\hat{u}(x,p)$ determined by the third of canonical equation (6c) is
independent with the state variable $x$, namely $u=\hat{u}(p)$, then the
accuracy order of $i$-th internal-stage control will be $q_{2}$ instead of
$\min(q_{1},q_{2})$. The reason can be explained easily after we give the
proof of Theorem 3.3. However, as we have stated in Definition 3.2, we discuss
the orders of internal-stage controls in a general problem setting rather than
some special cases.

Table 5: Implicit trapezoidal method $(left)$ and its adjoint part $(right)$. 0 |  |  |  |  | 1/2 | 1/2 | 0
---|---|---|---|---|---|---|---
1 | 1/2 | 1/2 |  |  | 1/2 | 1/2 | 0
| 1/2 | 1/2 |  |  |  | 1/2 | 1/2

To show the validity of Theorem 3.3, we give a simple example here.

Table 6:  $q_{1}$ and $q_{2}$ of method A, B and C | A | B | C
---|---|---|---
$i$ | 1 | 2 | 1 | 2 | 3 | 1 | 2 | 3 | 4
$q_{1}$ | 2 | 2 | 3 | 2 | 2 | 4 | 2 | 2 | 3
$q_{2}$ | 2 | 2 | 2 | 2 | 3 | 3 | 2 | 2 | 4
Table 7: Max error of internal controls
$(\max_{k=0,...,N-1}||u^{*}(kh+c_{i}h)-u_{ki}||,\ i=1,...,s)$ in Example 3.7.

$h$ | $A:u_{k1}$ | $A:u_{k2}$ | $B:u_{k1}$ | $B:u_{k2}$ | $B:u_{k3}$ | $C:u_{k1}$ | $C:u_{k2}$ | $C:u_{k3}$ | $C:u_{k4}$
---|---|---|---|---|---|---|---|---|---
0.1 | 2.40E-03 | 2.48E-03 | 1.28E-03 | 9.23E-04 | 2.61E-03 | 1.40E-04 | 2.67E-04 | 3.02E-04 | 1.11E-05
0.05 | 5.94E-04 | 6.56E-04 | 4.20E-04 | 2.60E-04 | 6.42E-04 | 1.76E-05 | 7.02E-05 | 7.45E-05 | 1.55E-06
0.04 | 3.79E-04 | 4.25E-04 | 2.82E-04 | 1.70E-04 | 4.09E-04 | 9.05E-06 | 4.53E-05 | 4.75E-05 | 8.08E-07
0.02 | 9.43E-05 | 1.09E-04 | 7.67E-05 | 4.41E-05 | 1.01E-04 | 1.13E-06 | 1.15E-05 | 1.18E-05 | 1.05E-07
0.01 | 2.35E-05 | 2.74E-05 | 1.99E-05 | 1.12E-05 | 2.52E-05 | 1.42E-07 | 2.91E-06 | 2.94E-06 | 1.34E-08

######  Example 3.7.

Consider the problem

$\min_{u}\int_{0}^{1}\frac{1}{2}x^{2}+\frac{1}{2}xu+\frac{1}{2}u^{2}dt,$

subject to

$\dot{x}=u,\ x(0)=1.$

The optimal control of this problem is given by

$u^{*}(t)=\frac{0.5e^{t}-1.5e^{2-t}}{0.5+1.5e^{2}}.$

The max errors of internal-stage controls generated by A,B and C methods
$($Table 2-4$)$ are shown in Table 7. We just discuss the result of C method
here. According to Theorem 3.3 and Table 6, the orders of four internal-stage
controls $u_{k1},\ u_{k2},\ u_{k3},\ u_{k4}$ should have the same numbers as
$\min(q_{1},q_{2}):3,\ 2,\ 2,\ 3$, which coincides with the result in Table 7.

High-order explicit RK methods are advantageous in practice for saving
computational cost. Thus it is necessary to study whether there exists
$s$-stage explicit RK method with $s$-order in OCP can avoid internal-stage
control problem $(s=2,3,4)$. The reason why we restrict $s\leq 4$ is that
according to the well-known result of Butcher (see Theorem 5.1 in [12]),
explicit RK method of $s$-stage cannot attain order $s$ when $s>4$. To save
computational payment, we do not concern the explicit RK method with stage
number greater than the order.

For the second order explicit RK method with coefficients $a_{21},b_{1}$ and
$b_{2}$, to ensure the first internal-stage control $u_{k1}$ attain order 2,
the following conditions must be satisfied:

${\textstyle\sum_{i=1}^{2}}b_{i}=1,\
{\textstyle\sum_{i=1}^{2}}b_{i}c_{i}=\frac{1}{2},\ c_{1}=\bar{c}_{1}.$

The first two conditions can ensure the RK method attain second order in OCP
(see Table 1 in [10]) and the third condition can ensure $u_{k1}$ achieve
second order. After some simple calculations, we can check that only A method
(Table 2) with coefficients $a_{21}=1,b_{1}=b_{2}=\frac{1}{2}$ satisfies these
conditions. In addition, since $c_{2}=\bar{c}_{2}$ is also satisfied for A
method, we can conclude that the second internal-stage control $u_{k2}$ also
has order 2.

Table 8: 3-stage explicit RK method with order 3 in OCP 0 |  |  |
---|---|---|---
$c_{2}$ | $c_{2}$ |  |
1 | $\frac{3c_{2}-1-3c_{2}^{2}}{c_{2}(2-3c_{2})}$ | $\frac{1-c_{2}}{c_{2}(2-3c_{2})}$ |
| $\frac{c_{2}-1/3}{2c_{2}}$ | $\frac{1}{6c_{2}(1-c_{2})}$ | $\frac{2-3c_{2}}{6(1-c_{2})}$

For the third or fourth order explicit methods, we first verify the conditions
$\bar{c}_{i}=c_{i},\ i=1,...,s$ are satisfied.

######  Proposition 3.8.

For s-stage explicit RK methods with s order in OCP $(s=3,4)$, the conditions
$c_{i}=\bar{c}_{i},\ (i=1,...,s)$ hold.

######  Proof 3.9.

Since

$\bar{c}_{i}={\textstyle\sum_{j=1}^{s}}\bar{a}_{ij}={\textstyle\sum_{j=1}^{s}}(b_{j}-\frac{b_{j}a_{ji}}{b_{i}}),\
i=1,...,s,$

to prove $\bar{c_{i}}=c_{i}$, it suffices to verify:

(18)
$b_{i}{\textstyle\sum_{j=1}^{s}}b_{j}-{\textstyle\sum_{j=1}^{s}}b_{j}a_{ji}=b_{i}-{\textstyle\sum_{j=1}^{s}}b_{j}a_{ji}=b_{i}c_{i},\
i=1,...,s,$

where we have applied the first order condition for RK method:
${\textstyle\sum_{j=1}^{s}}b_{j}=1$.

To ensure a 3-stage explicit RK attain third order in OCP, the unique form is
shown in Table 8 (refer [10]). By a direct computation, we can verify that the
coefficients in Table 8 satisfy (18). For the fourth order explicit RK
methods, it is presented in [5] (P178) that (18) is automatically satisfied.

As an immediate result of Remark 3.5 and Proposition 3.8, all internal-stage
controls generated by explicit RK methods with third or fourth order in OCP
have at least second order.

For a third order explicit method, to ensure the $i$-th internal-stage control
$u_{ki}$ $(i\in\\{1,...,s\\})$ owns third order, two additional conditions
($l=3$ in (16a) and (16b)) must be satisfied. However, there is only one
independent coefficient $c_{2}$, as shown in Table 8, thus it is impossible to
avoid order reduction. A similar reasoning can be used for the fourth order
explicit RK methods. Thus we can conclude that the s-order explicit RK methods
with s stages cannot avoid the internal-stage control problem.

When we choose the internal-stage control as final result, the order reduction
problem restricts the choice of RK methods. Of course, one can construct high-
order implicit RK methods satisfying enough conditions in Theorem 3.3. But
this issue is not concerned in this paper. Another way to solve the internal-
stage control problem, as suggested in the introduction of [10], is taking the
node control $u_{k}=\hat{u}(x_{k},p_{k})$ as final result instead of the
internal-stage controls. According to the convergence analysis in Theorem 3.1,
$u_{k}$ have the same accuracy order as the RK method itself. This handling is
suitable for more general RK discretization, and the additional computational
cost, as we will show in the later sections concerning specific numerical
algorithms, will not be huge.

We conclude this section with the proof of theorem 3.3. To begin with, we give
two lemmas on error estimate of internal-stage values generated by RK method.

######  Lemma 3.10.

Consider a differential equation $\dot{y}=f(y),\ y(t_{0})=y_{0}$ with $f:\
\mathbb{R}^{n}\to\mathbb{R}^{n}$ smooth enough. Apply a $s$-stage Runge-Kutta
method with step size $h$ to this equation, then for any $i\in\\{1,...,s\\}$,
the internal-stage value $y_{i}$, calculated by

$y_{i}=y_{0}+h{\textstyle\sum_{j=1}^{s}}a_{ij}f(y_{j}),$

satisfies $||y(t_{0}+c_{i}h)-y_{i}||=O(h^{n})$ if and only if

(19) ${\textstyle\sum_{j=1}^{s}}a_{ij}c_{j}^{l-2}=\frac{c_{i}^{l-1}}{l-1},\
for\ l=2,...,n.$

######  Proof 3.11.

The details of root tree theory in this proof can be seen in Chapter III of
[11], we just list some formulas when necessary. Regard the internal-stage
value $y_{i}(h)=y_{0}+h{\textstyle\sum_{j=1}^{s}}f(y_{j})$ as a function with
variable $h$. We can express the $l$-th derivatives of $y(t)$ and $y_{i}(h)$
as following

$\displaystyle y^{(l)}(t_{0})=$
$\displaystyle{\textstyle\sum_{|\tau|=l}}\alpha(\tau)F(\tau)(y_{0}),$
$\displaystyle y_{i}^{(l)}(0)=$
$\displaystyle{\textstyle\sum_{|\tau|=l}}\gamma(\tau)u_{i}(\tau)\alpha(\tau)F(\tau)(y_{0}),$

where $F(\tau)$ is the elementary differential about tree $\tau$,
$\alpha(\tau)$ and $\gamma(\tau)$ are the integer coefficients, and
$u_{i}(\tau)$ is the factor about $a_{ij}$. According to the Taylor expansion,
$||y(t_{0}+c_{i}h)-y_{i}||=O(h^{n})$ if and only if
$\gamma(\tau)u_{i}(\tau)=c_{i}^{l}$ for each $\tau$ with tree order
$1\leq|\tau|=l\leq n-1$. Specially, when we choose the tree
$\tau=[\tau_{1},...,\tau_{l-1}]$ where $\tau_{1}=...=\tau_{l-1}=\bullet$,
according to the formulas (1.14) and (1.15) in Chapter III of [11]:

$\displaystyle\gamma(\tau)$
$\displaystyle=|\tau|\gamma(\tau_{1})...\gamma(\tau_{l-1}),$ $\displaystyle
u_{i}(\tau)$
$\displaystyle={\textstyle\sum_{j=1}^{s}}a_{ij}u_{j}(\tau_{1})...u_{j}(\tau_{l-1}),$

and $\gamma(\bullet)=1,\
u_{j}(\bullet)={\textstyle\sum_{k=1}^{s}}a_{jk}=c_{j}$, we have

$\displaystyle\gamma(\tau)$ $\displaystyle=l,$ $\displaystyle u_{i}(\tau)$
$\displaystyle={\textstyle\sum_{j=1}^{s}}a_{ij}c_{j}^{l-1},$

thus the condition

$l\cdot{\textstyle\sum_{j=1}^{s}}a_{ij}c_{j}^{l-1}=c_{i}^{l}$

must be satisfied for $1\leq l\leq n-1$, which proves the necessity.

Sufficiency can be proved by induction. For $|\tau|=1$, $\tau$ can only be
$\bullet$, thus the equality $\gamma(\tau)u_{i}(\tau)=c_{i}$ is just the
condition (19) with $l=2$. Suppose $\gamma(\tau)u_{i}(\tau)=c_{i}^{|\tau|}$ is
satisfied for $|\tau|=1,2,...,N<n-1$. For $\tau=[\tau_{1},...,\tau_{m}]$ with
$|\tau|=1+|\tau_{1}|+...+|\tau_{m}|=N+1$, we have $|\tau_{w}|\leq N$ for
$w=1,...,m$. Then

(23) $\displaystyle\gamma(\tau)u_{i}(\tau)$
$\displaystyle=|\tau|{\textstyle\sum_{j=1}^{s}}a_{ij}{\textstyle\prod_{w=1}^{m}}\gamma(\tau_{w})u_{j}(\tau_{w})$
$\displaystyle=|\tau|{\textstyle\sum_{j=1}^{s}}a_{ij}c_{j}^{|\tau_{1}|+...+|\tau_{m}|}$
$\displaystyle=(N+1){\textstyle\sum_{j=1}^{s}}a_{ij}c_{j}^{N}=c_{i}^{N+1},$

where the last equality is due to the condition (19) with $l=N+2$. Now we have
proven the sufficient part of the lemma.

The analogous result for partitioned Runge-Kutta methods requires only minor
modifications. The proof is quite similar to Lemma 3.10 and so is omitted.

######  Lemma 3.12.

Consider differential equations

$\displaystyle\dot{x}$ $\displaystyle=f(x,y),\ x(t_{0})=x_{0},$
$\displaystyle\dot{y}$ $\displaystyle=g(x,y),\ y(t_{0})=y_{0}$

where $f$ and $g:\mathbb{R}^{n}\times\mathbb{R}^{n}\to\mathbb{R}^{n}$ are
smooth enough. When a partitioned RK method with coefficients $a_{ij},\
b_{i},\ \bar{a}_{ij},\ \bar{b}_{i}$ satisfying
$c_{i}={\textstyle\sum_{j=1}^{s}}a_{ij}=\bar{c}_{i}={\textstyle\sum_{j=1}^{s}}\bar{a}_{ij}$
is applied to numerical integration, the intermediate values

$\displaystyle x_{i}$
$\displaystyle=x_{0}+h{\textstyle\sum_{j=1}^{s}}a_{ij}f(x_{j},y_{j}),$
$\displaystyle y_{i}$
$\displaystyle=y_{0}+h{\textstyle\sum_{j=1}^{s}}\bar{a}_{ij}g(x_{j},y_{j})$

satisfy $||x(t_{0}+c_{i}h)-x_{i}||\leq O(h^{q_{1}})$ and
$||y(t_{0}+c_{i}h)-y_{i}||\leq O(h^{q_{2}})$ if and only if

$\displaystyle{\textstyle\sum_{j=1}^{s}}a_{ij}c_{j}^{l-2}=\frac{c_{i}^{l-1}}{l-1},\
l=2,...,q_{1},$
$\displaystyle{\textstyle\sum_{j=1}^{s}}\bar{a}_{ij}c_{j}^{l-2}=\frac{c_{i}^{l-1}}{l-1},\
l=2,...,q_{2}.$

######  Remark 3.13.

When $c_{i}\neq\bar{c}_{i}$ in Lemma 23, we can express the intermediate value
$y_{i}$ by Taylor expansion

$\displaystyle y_{i}=$ $\displaystyle
y_{0}+h{\textstyle\sum_{i=1}^{s}}\bar{a}_{ij}g(x_{0},y_{0})+O(h^{2})$
$\displaystyle=$ $\displaystyle y_{0}+h\bar{c}_{i}g(x_{0},y_{0})+O(h^{2}),$

then it is an immediate result that

$||y(t_{o}+c_{i}h)-y_{i}||\leq
h||(c_{i}-\bar{c}_{i})g(x_{0},y_{0})||+O(h^{2})=O(h).$

Now we are in a position to prove Theorem 3.3.

######  Proof 3.14.

For simple notation, we denote the vector field determined by canonical
equations (8a) and (8b) as $F(z)$ where $z(t)=(x(t),p(t))$ and the exact flow
with any initial condition $z(t_{0})=z_{0}$ as $\phi(t;t_{0},z_{0})$. We also
denote $L$ and $L_{0}$ as the Lipschitz constants for $F(z)$ and $\hat{u}(z)$
in a neighbourhood of the trajectory
$\left\\{z^{*}(t)=(x^{*}(t),p^{*}(t))\in\mathbb{R}^{2n}:0\leq t\leq
t_{f}\right\\}$ respectively. Suppose the conditions in Theorem 3.3 are
satisfied, then the difference between $z_{ki}=(x_{ki},p_{ki})$ and
$z^{*}(t_{k}+c_{i}h)$ satisfies

(26) $\displaystyle||z_{ki}-z^{*}(t_{k}+c_{i}h)||\leq$
$\displaystyle||z_{ki}-\phi(t_{k}+c_{i}h;t_{k},z_{k})||$
$\displaystyle+||\phi(t_{k}+c_{i}h;t_{k},z_{k})-\phi(t_{k}+c_{i}h;t_{k},z^{*}(t_{k})||$
$\displaystyle\leq$ $\displaystyle
O(h^{n})+||\phi(t_{k}+c_{i}h;t_{k},z_{k})-\phi(t_{k}+c_{i}h;t_{k},z^{*}(t_{k})||$

where $n$ satisfies (17) according to the Lemma 3.12 and Remark 3.13. Since

$\displaystyle||\phi(t;t_{k},z_{k})-\phi(t;t_{k},$ $\displaystyle
z^{*}(t_{k}))||$ $\displaystyle=$
$\displaystyle||z_{k}-z^{*}(t_{k})+\int_{t_{k}}^{t}F(\phi(\tau;t_{k},z_{k}))-F(\phi(\tau;t_{k},z^{*}(t_{k})))d\tau||$
$\displaystyle\leq$
$\displaystyle||z_{k}-z^{*}(t_{k})||+L\int_{t_{k}}^{t}||\phi(\tau;t_{k},z_{k})-\phi(\tau;t_{k},z^{*}(t_{k}))||d\tau$

for $t_{k}\leq t\leq t_{k}+c_{i}h$, according to Gronwall’s inequality of
integral form, we can obtain

(27)
$\displaystyle||\phi(t_{k}+c_{i}h;t_{k},z_{k})-\phi(t_{k}+c_{i}h;t_{k},z^{*}(t_{k}))||\leq$
$\displaystyle(1+c_{i}hLe^{c_{i}hL})||z_{k}-z^{*}(t_{k})||$
$\displaystyle\leq$ $\displaystyle(1+c_{i}hLe^{c_{i}hL})O(h^{r})$

where $||z_{k}-z^{*}(t_{k})||\leq O(h^{r})$ is due to the Theorem 3.1.
Plugging (27) into (26), an error estimate for $z_{ki}$ is given by

$\displaystyle||z_{ki}-z^{*}(t_{k}+c_{i}h)||\leq(1+c_{i}hLe^{c_{i}hL})O(h^{r})+O(h^{n}).$

Finally, the convergence rate of $u_{ki}$ is obtained by

$||u_{ki}-u^{*}(t_{k}+c_{i}h)||=||\hat{u}(z_{ki})-\hat{u}(z^{*}(t_{k}+c_{i}h))||\leq
L_{0}||z_{ki}-z^{*}(t_{k}+c_{i}h)||.$

Recall that, in Remark 3.6, we have referred a special case $u=\hat{u}(p)$
where the accuracy order of internal-stage control is determined by $q_{2}$
instead of $\min(q_{1},q_{2})$. This can be verified easily with variable
$z=(x,p)$ substituted by $p$ in the proof above and note that the internal-
stage costate has order $n=q_{2}$ according to Lemma 3.12.

##  4 DLQR method based on high-order RK discretization for linear quadratic
problems

In this section, we restrict the Problem 2.1 as the following linear quadratic
form

$\min_{u}J=\min_{u}\int_{0}^{t_{f}}(\frac{1}{2}x^{T}Qx+\frac{1}{2}u^{T}Ru)dt+\frac{1}{2}(x(t_{f}))^{T}Mx(t_{f})$

subject to

$\dot{x}=Ax+Bu,\ x(0)=a,\ t\in[0,t_{f}],$

where $Q\geq 0,R>0$ and $M\geq 0$ are symmetric matrices.

Based on the discussion of Section 3, we will calculate the node control to
avoid the order reduction problem of internal-stage control. To achieve this
goal, it is necessary to deduce the function $u=\hat{u}(x,p)$. In the present
problem setting, the third canonical equation (6c) is given by

$B^{T}p+Ru=0,$

and thus we can obtain the linear feedback law:

(28) $\hat{u}(x,p)=-R^{-1}B^{T}p.$

###  4.1 Algorithm: DLQR method based on high-order RK discretization

Our method is divided into four steps:

  1. (1)

Discretization and simplification. As the starting step, we apply a $s$-stage
RK method with coefficients $a_{ij},\ b_{i}$ to discretization, and then
obtain the following optimization problem with linear constraints:

(29)
$\displaystyle\min_{u_{ki}}J_{d}=\min_{u_{ki}}\frac{h}{2}{\textstyle\sum_{k=0}^{N-1}}{\textstyle\sum_{i=1}^{s}}b_{i}(x_{ki}^{T}Qx_{ki}+u_{ki}^{T}Qu_{ki})+\frac{1}{2}x_{N}^{T}Mx_{N}$

subject to

(30a) $\displaystyle x_{k+1}$
$\displaystyle=x_{k}+h{\textstyle\sum_{i=1}^{s}}b_{i}(Ax_{ki}+Bu_{ki}),$
$\displaystyle 0$ $\displaystyle\leq k\leq N-1,$ (30b) $\displaystyle x_{ki}$
$\displaystyle=x_{k}+h{\textstyle\sum_{j=1}^{s}}a_{ij}(Ax_{kj}+Bu_{kj}),$
$\displaystyle 0$ $\displaystyle\leq k\leq N-1,\ 1\leq i\leq s.$

For convenience, let us introduce the notations $X_{k}=(x_{k1},...,x_{ks})$
and $U_{k}=(u_{k1},...,u_{ks})$ for the vectors composed by all the internal-
stage state and control values during $[t_{k},t_{k+1}]$ respectively, then we
can transform the discrete cost function (29) into a concise form:

(31)
$J_{d}=\frac{1}{2}{\textstyle\sum_{k=0}^{N-1}}(X_{k}^{T}Q_{h}X_{k}+U_{k}^{T}R_{h}U_{k})+\frac{1}{2}x_{N}^{T}Mx_{N},$

where

(32) $Q_{h}=h\begin{bmatrix}b_{1}Q&&0\\\ &\ddots&\\\ 0&&b_{s}Q\end{bmatrix},\
R_{h}=h\begin{bmatrix}b_{1}R&&0\\\ &\ddots&\\\ 0&&b_{s}R\end{bmatrix}.$

Further, by introducing the notations

$Z=\begin{bmatrix}I_{n}(1)\\\ \vdots\\\ I_{n}(s)\end{bmatrix},\
\mathcal{A}=h\begin{bmatrix}a_{11}A&\cdots&a_{1s}A\\\ \vdots&&\vdots\\\
a_{s1}A&\cdots&a_{ss}A\end{bmatrix},\
\mathcal{B}=h\begin{bmatrix}a_{11}B&\cdots&a_{1s}B\\\ \vdots&&\vdots\\\
a_{s1}B&\cdots&a_{ss}B\end{bmatrix},$

we can rewrite the constraints (30b) as follows

$X_{k}=Zx_{k}+\mathcal{A}X_{k}+\mathcal{B}U_{k},\ 0\leq k\leq N-1,$

or equivalently, denoting $E=(I_{sn}-\mathcal{A})^{-1}Z$ and
$F=(I_{sn}-\mathcal{A})^{-1}\mathcal{B}$, we have

(33) $X_{k}=Ex_{k}+FU_{k},\ 0\leq k\leq N-1.$

Note that the inverse matrix of $I_{sn}-\mathcal{A}$ exists when time step $h$
is small enough. Substituting (33) into (30a), we can obtain the following
formula after combing like terms

(34) $x_{k+1}=Gx_{k}+HU_{k},\ 0\leq k\leq N-1,$

where

$\displaystyle G$
$\displaystyle=I_{n}+h\begin{bmatrix}b_{1}A&\cdots&b_{s}A\end{bmatrix}E,$
$\displaystyle H$
$\displaystyle=h\begin{bmatrix}b_{1}A&\cdots&b_{s}A\end{bmatrix}F+h\begin{bmatrix}b_{1}B&\cdots&b_{s}B\end{bmatrix}.$

  2. (2)

Construction of feedback law. Denoting the discrete value function as
$V_{k}(x_{k})\ (0\leq k\leq N)$, then the discrete HJB equation is given by

(36) $\displaystyle V_{k}(x_{k})=$
$\displaystyle\min_{U_{k}}\\{\frac{1}{2}X_{k}^{T}Q_{h}X_{k}+\frac{1}{2}U_{k}^{T}R_{h}U_{k}+V_{k+1}(x_{k+1})\\}$
$\displaystyle=$
$\displaystyle\min_{U_{k}}\\{\frac{1}{2}(Ex_{k}+FU_{k})^{T}Q_{h}(Ex_{k}+FU_{k})+\frac{1}{2}U_{k}^{T}R_{h}U_{k}$
$\displaystyle+V_{k+1}(Gx_{k}+HU_{k})\\},\ 0\leq k\leq N-1,$

and the boundary condition is $V_{N}(x_{N})=\frac{1}{2}x_{N}^{T}Mx_{N}$.
Suppose the value function has a quadratic form

(37) $V_{k}(x_{k})=\frac{1}{2}x_{k}^{T}M_{k}x_{k},\ 0\leq k\leq N,$

where $M_{k}$ is symmetric, then according to the boundary condition, we have
$M_{N}=M$. Plug (37) into the discrete HJB equation (36) and let the
derivative of the right hand of (36) with respect to $U_{k}$ vanish, we can
obtain

$(F^{T}Q_{h}F+R_{h}+H^{T}M_{k+1}H)U_{k}+(F^{T}Q_{h}E+H^{T}M_{k+1}G)x_{k}=0,$

$0\leq k\leq N-1$, it follows that $U_{k}$ should satisfies a linear feedback
law

(38) $U_{k}=L_{k}x_{k},\ 0\leq k\leq N-1,$

where

$L_{k}=-(F^{T}Q_{h}F+R_{h}+H^{T}M_{k+1}H)^{-1}(F^{T}Q_{h}E+H^{T}M_{k+1}G).$

  3. (3)

Calculation of the value function. Plugging (38) into the discrete HJB
equation (36) and comparing both sides of the equality, we obtain the
iteration formula for matrix $M_{k}$:

$M_{k}=(E+FL_{k})^{T}Q_{h}(E+FL_{k})+L_{k}^{T}R_{h}L_{k}+(G+HL_{k})^{T}M_{k+1}(G+HL_{k}),$

$k=N-1,...,0$. With $M_{N}=M$ known, $M_{N-1},...,M_{0}$ can be calculated
recursively.

  4. (4)

Calculation of the node control. Substituting (38) into (34), we can calculate
the discrete state trajectory by

$x_{k+1}=(G+HL_{k})x_{k},\ x_{0}=a,\ 0\leq k\leq N-1.$

With $M_{k}$ and $x_{k}$ known, we can obtain the discrete costates by (14b):

$p_{k}=(V_{k})^{\prime}(x_{k})=M_{k}x_{k},\ 0\leq k\leq N.$

Finally, the node controls $u_{k}\ (0\leq k\leq N)$ can be calculated by
plugging $p_{k}$ into the formula (28), namely,

$u_{k}=-R^{-1}B^{T}p_{k},\ 0\leq k\leq N.$

###  4.2 Numerical example: Linear spring oscillator

Consider a linear oscillator under control with following state equations and
initial conditions:

$\begin{bmatrix}\dot{x}_{1}\\\ \dot{x}_{2}\end{bmatrix}=\begin{bmatrix}0&1\\\
-1&0\end{bmatrix}\begin{bmatrix}x_{1}\\\
x_{2}\par\end{bmatrix}+\begin{bmatrix}1\\\ 0\par\end{bmatrix}u,\
\begin{bmatrix}x_{1}(0)\\\ x_{2}(0)\end{bmatrix}=\begin{bmatrix}1\\\
1\end{bmatrix}.$

Now search for control $u(t),\ t\in[0,40],$ to minimize the cost function:

$J=\int_{0}^{40}(\frac{1}{2}x^{T}x+1.5u^{2})dt+5x(1)^{T}x(1),$

where $x=(x_{1},x_{2})$.

We apply explicit Euler, implicit trapezoidal and B methods in this numerical
example. As Figure 2 shows, DLQR method based on high order RK discretization
shows remarkable advantage in accuracy compared to the low-order case. In
particular, the implicit trapezoidal method, as we have demonstrated in Remark
3.5, which produces the internal-stage controls with only first order,
generates the node control with second order accuracy in this example.

Figure 2: Comparison of different RK discretization: maximum control error
$(\text{max}\\{||u_{k}-u^{*}(t_{k})||:0\leq k\leq N\\})$ for linear spring
oscillator problem.

##  5 ILQR method based on high-order RK discretization for non-linear
quadratic problems

Consider the following non-linear quadratic problem:
Problem 5.1

(39)
$\min_{u}J=\min_{u}\int_{0}^{t_{f}}(\frac{1}{2}x^{T}Qx+\frac{1}{2}u^{T}Ru)dt+\frac{1}{2}(x(t_{f}))^{T}Mx(t_{f})$

subject to

(40) $\dot{x}=f(x,u),\ x(0)=a,$

where $Q\geq 0,\ R>0,\ M\geq 0$ are symmetric matrices. Applying a $s$-stage
RK method with coefficients $a_{ij},b_{i}$ to discretize Problem 5.1, we can
get the following optimization problem:
Problem 5.2

(41)
$\min_{u_{ki}}J_{d}=\min_{u_{ki}}{\textstyle\sum_{k=0}^{N-1}}h{\textstyle\sum_{i=1}^{s}}b_{i}(\frac{1}{2}x_{ki}^{T}Qx_{ki}+\frac{1}{2}u_{ki}^{T}Ru_{ki})+\frac{1}{2}x_{N}^{T}Mx_{N}$

subject to

(42a) $\displaystyle x_{k+1}$
$\displaystyle=x_{k}+h{\textstyle\sum_{i}^{s}}b_{i}f(x_{ki},u_{ki}),\
x_{0}=a,$ $\displaystyle 0$ $\displaystyle\leq k\leq N-1,$ (42b)
$\displaystyle x_{ki}$
$\displaystyle=x_{k}+h{\textstyle\sum_{j}^{s}}a_{ij}f(x_{kj},u_{kj}),$
$\displaystyle 0$ $\displaystyle\leq k\leq N-1,\ 1\leq i\leq s.$

We denote the solutions of Problem 5.2 as $u^{*}_{ki}$ and the corresponding
discrete states calculated by (42a) and (42b) as $x^{*}_{ki}$ and $x^{*}_{k}$.
Different from the linear case in Section 4, the value function $V_{k}(x_{k})$
for discrete cost function (41) is not in a quadratic form in general, and
consequently we cannot expect to get a linear feedback law as (38). Besides,
note that the third canonical equation (6c) of Problem 5.1 is given by

(43) $D_{u}^{T}f(x,u)p+Ru=0,$

we may be not able to obtain an explicit expression for $u=\hat{u}(x,p)$.

There are many notations in this section, we list them in advance for
convenience. Same as Section 4, we also introduce the notations
$U_{k}=(u_{k1},...,u_{ks})$ and $X_{k}=(x_{k1},...,x_{ks})$ for the vectors
combined by internal-stage state and control values during $[t_{k},t_{k+1}]$
respectively. Further, we denote the vectors composed by all the internal-
stage controls, internal-stage states and node states as
$U=(U_{0},...,U_{N-1})$, $X=(X_{0},...,X_{N-1})$ and
$x_{d}=(x_{0},...,x_{N})$, respectively. When we add a symbol on these new
notations, it just means adding the same symbol to all the elements in
brackets. For example, $x_{d}^{0}=(x^{0}_{0},...,x^{0}_{N})$. In the rest of
this section, we always suppose the time step $h$ in (42a) and (42b) is small
enough to ensure that, according to implicit function theorem, $X$ and $x_{d}$
can be uniquely determined by $U$. Note that the total dimension of
$(U,X,x_{d})$ is $N_{\text{total}}=msN+nsN+n(N+1)$, thus the constraints (42a)
and (42b) determine a submanifold $\mathcal{M}$ of Euclidean space
$\mathbb{R}^{N_{\text{total}}}$:
$\mathcal{M}=\left\\{(U,X,x_{d})\in\mathbb{R}^{N_{\text{total}}}:\text{\eqref{5dstate1},
\eqref{5dstate2}}\right\\}.$

###  5.1 Algorithm: ILQR method based on high-order RK discretization

Before stating the algorithm formally, we give an inspiring example which is
helpful to understand the iterative progress in ILQR method.

######  Example 5.1.

Considering the following optimization problem,

(44) $\min_{u}j(x,u)=\min_{u}\\{\frac{1}{2}x^{2}+\frac{1}{2}u^{2}\\}$

subject to $x=g(u)$, where $x,u\in\mathbb{R}$ and $g:\mathbb{R}\to\mathbb{R}$
is twice differentiable.

As we can see in Figure 3, the function $x=g(u)$ determines a curve, and the
minimal for objective function (44) is just the point on the curve closest to
the origin $(0,0)$. First, we choose an initial value $u^{0}$ and thus
determine a point $(u^{0},x^{0})=(u^{0},g(u^{0}))$ on the curve. Then we can
draw the tangent of curve at point $(u^{0},x^{0})$ and search for minimal
$(\tilde{u},\tilde{x})$ of objective function $j$ over this tangent. Once
$\tilde{u}$ is determined, we obtain a search direction $\tilde{u}-u^{0}$
$($arrow direction in Figure 3$)$. By mean of a linear search technique, we
can obtain the next iterative point $(u^{1},x^{1})=(u^{1},g(u^{1}))$ on the
curve segment
$\gamma(\alpha)=\\{(u(\alpha),x(\alpha)):u(\alpha)=u^{0}+\alpha(\tilde{u}-u^{0}),\
x(\alpha)=g(u(\alpha))\\},\ \alpha\geq 0$.

Figure 3: The search direction for Example 3.

Now we turn to the ILQR method which can be divided into two steps. The first
step aims at solving Problem 5.2 in a similar way as Example 5.1, and the node
control will be calculated in the second step.

  1. (1)

Iterative progress. Choosing an initial value for internal-stage control
$U^{0}$, then we can determine a point $(U^{0},X^{0},x_{d}^{0})$ on
$\mathcal{M}$ where $X^{0}$ and $x_{d}^{0}$ are calculated by (42a) and (42b).
The equations of tangent plane of $\mathcal{M}$ at this point is given by:

$\displaystyle{x}_{k+1}-x^{0}_{k+1}=$
$\displaystyle{x}_{k}-x^{0}_{k}+h{\textstyle\sum_{i=1}^{s}}b_{i}(D_{x}f(x^{0}_{ki},u^{0}_{ki})({x}_{ki}-x^{0}_{ki})$
(45a) $\displaystyle+D_{u}f(x^{0}_{ki},u^{0}_{ki})({u}_{ki}-u^{0}_{ki})),$
$\displaystyle{x}_{ki}-x^{0}_{ki}=$
$\displaystyle{x}_{k}-x^{0}_{k}+h{\textstyle\sum_{j=1}^{s}}a_{ij}(D_{x}f(x^{0}_{kj},u^{0}_{kj})({x}_{kj}-x^{0}_{kj})$
(45b) $\displaystyle+D_{u}f(x^{0}_{kj},u^{0}_{kj})({u}_{kj}-u^{0}_{kj})),$

$0\leq k\leq N-1,\ 1\leq i\leq s$ and $x_{0}^{0}=a$. Similar to Example 5.1,
we search the minimum $(\tilde{U},\tilde{X},\tilde{x}_{d})$ of cost function
(41) over this tangent plane. The progress to obtain $\tilde{U}$ is given in
Appendix B. With $\tilde{U}$ calculated, we get a search direction
$\tilde{U}-U^{0}$ for Problem 5.2. Considering the curve segment on
$\mathcal{M}$:

(46)
$\displaystyle\gamma(\alpha)=\\{(U,X,x_{d}):U=U^{0}+\alpha(\tilde{U}-U^{0}),$
$\displaystyle\text{$X$ and $x_{d}$ are calculated by \eqref{5dstate1} and
\eqref{5dstate2} }\\},\ \alpha\geq 0,$

we can choose the step length $\alpha$ by some linear search technique and
then let $\gamma(\alpha)$ be the next iterative point
$(U^{1},X^{1},x^{1}_{d})$ on $\mathcal{M}$. Repeat this progress until a
preset condition for interruption is satisfied, then we can take the finial
iterative point (suppose it is the $L$-th point) as the approximate solution
of problem 5.2: $(U^{L},X^{L},x^{L}_{d})\approx(U^{*},X^{*},x_{d}^{*})$.

  2. (2)

Calculation of node controls. According to Proposition 2.1, the solution of
problem 5.2 $u^{*}_{ki},\ x^{*}_{ki}$ and $x^{*}_{k}$ will satisfy the
following equations

(47a) $\displaystyle p^{*}_{k+1}$
$\displaystyle=p^{*}_{k}-h{\textstyle\sum_{i=1}^{s}}b_{i}(D_{x}^{T}f(x^{*}_{ki},u^{*}_{ki})p^{*}_{ki}+Qx^{*}_{ki}),$
(47b) $\displaystyle p^{*}_{ki}$
$\displaystyle=p^{*}_{k}-h{\textstyle\sum_{i=1}^{s}}\bar{a}_{ij}(D_{x}^{T}f(x^{*}_{kj},u^{*}_{kj})p^{*}_{kj}+Qx^{*}_{kj}),$

$0\leq k\leq N-1,\ 1\leq i\leq s$, with boundary condition
$p^{*}_{N}=Mx^{*}_{N}$. Since we have obtained $u^{*}_{ki},\ x^{*}_{ki}$ and
$x^{*}_{k}$ in step (1), we can calculate the discrete costates
$p^{*}_{N-1},...,p^{*}_{0}$ recursively by solving (47a) and (47b). Plugging
$x^{*}_{k}$ and $p^{*}_{k}$ into the third canonical equation (43), we can
obtain the node control $u^{*}_{k}$ by solving

$D_{u}^{T}f(x_{k}^{*},u_{k}^{*})p_{k}^{*}+Ru_{k}^{*}=0,\ 0\leq k\leq N.$

###  5.2 Convergence analysis

In this subsection, we give a convergence analysis for the iterative progress
of ILQR method. The main results contain:

  1. (i)

The iterative progress of ILQR technique belongs to the quasi-Newton type
methods and produces descent search direction for Problem 5.2.

  2. (ii)

ILQR cannot attain super-linear convergence rate in general.

To begin with, let us rewritten the cost function (41) in the following form:

(48)
$\min_{U}J_{d}(U,X,x_{d})=\min_{U}\frac{1}{2}X^{T}\mathcal{Q}X+\frac{1}{2}U^{T}\mathcal{R}U+\frac{1}{2}x_{N}^{T}Mx_{N},$

where

$\mathcal{Q}=\begin{bmatrix}Q_{h}(1)&&0\\\ &\ddots&\\\
0&&Q_{h}(N)\end{bmatrix},\ \mathcal{R}=\begin{bmatrix}R_{h}(1)&&0\\\
&\ddots&\\\ 0&&R_{h}(N)\end{bmatrix},$

$Q_{h}$ and $R_{h}$ are the same as (32).

Since the internal-stage state $X$ and node states $x_{k}\ (k=0,...,N)$ are
determined by the internal-stage control $U$ according to equations (42a) and
(42b), it is convenient to introduce the notations $X=F(U)$ and
$x_{k}=F_{d}^{k}(U)\ (k=0,...,N)$. Specially, since $x_{0}=a$, we have
$F_{d}^{0}(U)\equiv a$. Note that when we plug these functions in (48), we can
express the discrete cost function with only variable $U$, namely,

(49)
$J_{d}(U)=\frac{1}{2}(F(U))^{T}\mathcal{Q}F(U)+\frac{1}{2}U^{T}\mathcal{R}U+\frac{1}{2}(F_{d}^{N}(U))^{T}MF_{d}^{N}(U).$

For convenience of the discussion latter, we give the gradient and Hesse
matrix of $J_{d}(U)$ at $U^{0}$:

(50) $\displaystyle(J_{d})^{\prime}(U^{0})=$
$\displaystyle(F^{\prime}(U^{0}))^{T}\mathcal{Q}F(U^{0})+\mathcal{R}U^{0}+((F_{d}^{N})^{\prime}(U^{0}))^{T}MF_{d}^{N}(U^{0}),$
$\displaystyle(J_{d})^{\prime\prime}(U^{0})=$
$\displaystyle(F^{\prime}(U^{0}))^{T}\mathcal{Q}F^{\prime}(U^{0})+(F^{\prime\prime}(U^{0}))^{T}\mathcal{Q}F(U^{0})+\mathcal{R}$
(51)
$\displaystyle+((F_{d}^{N})^{\prime}(U^{0}))^{T}M(F_{d}^{N})^{\prime}(U^{0})+((F_{d}^{N})^{\prime\prime}(U^{0}))^{T}MF_{d}^{N}(U^{0}).$

Next we will calculate the search direction generated by ILQR iterative
progress. The tangent plane of manifold $\mathcal{M}$ at the point
$(U^{0},X^{0},x^{0}_{1},...,x^{0}_{N})$ $=(U^{0},F(U^{0})$
$,F_{d}^{1}(U^{0}),...,F_{d}^{N}(U^{0}))$ is given by

(52) $\displaystyle X-X^{0}$ $\displaystyle=F^{\prime}(U^{0})(U-U^{0}),$ (53)
$\displaystyle x_{k}-x_{k}^{0}$
$\displaystyle=(F_{d}^{k})^{\prime}(U^{0})(U-U^{0}),\ 0\leq k\leq N.$

Particularly, the $(N+1)$-th formula of (53) is

(54) $x_{N}-x_{N}^{0}=(F_{d}^{N})^{\prime}(U^{0})(U-U^{0}).$

Substituting (52) and (54) into (48), we can obtain the following expression
after combing similar terms:

(55)
$\frac{1}{2}(U-U^{0})^{T}W(U^{0})(U-U^{0})+(U-U^{0})^{T}Y(U^{0})+C(U^{0})$

where

(56a) $\displaystyle W(U^{0})=$
$\displaystyle(F^{\prime}(U^{0}))^{T}\mathcal{Q}F^{\prime}(U^{0})+\mathcal{R}+((F_{d}^{N})^{\prime}(U^{0}))^{T}M(F_{d}^{N})^{\prime}(U^{0}),$
(56b) $\displaystyle Y(U^{0})=$
$\displaystyle(F^{\prime}(U^{0}))^{T}\mathcal{Q}X^{0}+\mathcal{R}U^{0}+((F_{d}^{N})^{\prime}(U^{0}))^{T}Mx_{N}^{0},$
(56c) $\displaystyle C(U^{0})=$
$\displaystyle\frac{1}{2}(X^{0})^{T}\mathcal{Q}X^{0}+\frac{1}{2}(U^{0})^{T}\mathcal{R}U^{0}+\frac{1}{2}(x_{N}^{0})^{T}Mx_{N}^{0}.$

Since $W(U^{0})$ is a symmetric matrix, the necessary condition for minimizing
(55) is

$W(U^{0})(U-U^{0})+Y(U^{0})=0,$

then the search direction of ILQR can be expressed as

(57) $U-U^{0}=-(W(U^{0}))^{-1}Y(U^{0}).$

Comparing (50) with (56b), we have $Y(U^{0})=(J_{d})^{\prime}(U^{0})$.
Consequently, we can rewrite the ILQR search direction (57) in a quasi-Newton
type

(58) $U-U^{0}=-(W(U^{0}))^{-1}(J_{d})^{\prime}(U^{0}),$

According to (56a), $W(U^{0})$ is positive-definite, thus the search direction
generated by ILQR is a decrease one for Problem 5.2.

Now we study whether the convergence rate of ILQR is super-linear. Suppose the
sequence $\\{U^{l}\\}$, generated by ILQR iteration
$U^{l+1}=U^{l}-W(U^{l})^{-1}(J_{d})^{\prime}(U^{l})$, converges to the minimum
of Problem 5.2 (denoted by $U^{*}$). According to the necessary and sufficient
condition for super-linear convergence (see Theorem 3.7 in [19]), ILQR can
attain a super-linear convergence rate if and only if

(59)
$\lim_{l\to+\infty}\frac{||(W(U^{l})-(J_{d})^{\prime\prime}(U^{l}))\cdot(U^{l+1}-U^{l})||}{||U^{l+1}-U^{l}||}=0.$

However, (59) cannot be achieved in general. To demonstrate this, let us go
back to Example 5.1 where $W(u^{l})=1+(g^{\prime}(u^{l}))^{2}$ and
$j^{\prime\prime}(u^{l})=1+(g^{\prime}(u^{l}))^{2}+g^{\prime\prime}(u^{l})g(u^{l})$.
We have

$\lim_{l\to+\infty}\frac{|(j^{\prime\prime}(u^{l})-W(u^{l}))\cdot(u^{l+1}-u^{l})|}{|u^{l+1}-u^{l}|}=|j^{\prime\prime}(u^{*})-W(u^{*})|=|g^{\prime\prime}(u^{*})g(u^{*})|.$

It is easy to cite an example in which $|g^{\prime\prime}(u^{*})g(u^{*})|\neq
0$. When $g(u)=u^{2}+1$, we have $u^{*}=0,\ g(u^{*})=1$ and
$g^{\prime\prime}(u^{*})=2$.

###  5.3 Numerical example: inverted pendulum

Consider the state equations of inverted pendulum under control:

$\displaystyle\dot{\theta}$ $\displaystyle=\omega,\ \theta(0)=\frac{\pi}{3},$
$\displaystyle\dot{\omega}$ $\displaystyle=sin\theta+u,\ \omega(0)=0.$

The cost function of this problem is:

$J=\int_{0}^{4}0.025u^{2}dt+2.5x_{f}^{T}x_{f},$

where $x_{f}=(\theta(4),\omega(4))$.

For this non-linear problem, we explore the dependence of initial control
$u_{0}$ (the first node control value calculated by ILQR method) on the node
number for different RK discretization. As we can see in Figure 4, for the B
method with third order in OCP, the calculated initial control $u_{0}$
converges faster than the other two produced by lower order discretization.
When the node number is increased to nearly $800$, the initial control $u_{0}$
generated by the second order implicit trapezoidal method can attain a
satisfying accuracy, while explicit Euler method still behaves badly.

Figure 4: Comparison of different RK discretization: dependence of initial
control on the node number.

##  6 Conclusion

We propose DLQR and ILQR methods based on high-order RK discretization to
solve linear and nonlinear quadratic optimal control problems respectively.
The equivalence between direct approach based on RK discretization and SPRK
integration for canonical equations, is reconstructed in the framework of
dynamic programming and plays important roles in both theoretical analysis and
algorithm implementation. Based on this equivalence, we deduce the order
conditions for internal-stage controls in Theorem 3.3 and discover that some
common RK methods with high order in OCP (third or fourth order explicit
methods; implicit methods such as implicit trapezoidal method) cannot avoid
the order reduction. Then, by mean of the the equivalence in Section 2, we
calculate node control in DLQR and ILQR methods to overcome this obstacle.
Numerical examples show the advantage of our methods on the accuracy increase.
Because both DLQR and ILQR methods are achieved with the feedback technique,
they have high computational efficiency. In the convergence analysis for ILQR
iteration, we demonstrate that ILQR technique is essentially a quasi-Newton
method and the convergence rate is linear in general.

##  Appendix A Proof for proposition 2.1

Denoting the gradient of discrete value function $(V_{k})^{\prime}(x_{k})$ as
$p_{k}$ $(0\leq k\leq N)$, we have
$p_{N}=(V_{N})^{\prime}(x_{N})=\Phi^{\prime}(x_{N})$ according to the boundary
condition $V_{N}(x_{N})=\Phi(x_{N})$. To achieve the minimization of the right
hand of discrete HJB equation (12), we first introduce a multiply
$\varphi_{ki}$ for the $i$-th intermediate equation of (10a) $(1\leq i\leq s)$
and define

$\displaystyle\mathcal{V}_{k}$
$\displaystyle(x_{k},x_{k1},...,x_{ks},u_{k1},...,u_{ks},\varphi_{k1},...,\varphi_{ks})$
$\displaystyle=$ $\displaystyle
h{\textstyle\sum_{i=1}^{s}}b_{i}C(x_{ki},u_{ki})+V_{k+1}(x_{k}+h{\textstyle\sum_{i=1}^{s}}b_{i}f(x_{ki},u_{ki}))$
$\displaystyle+{\textstyle\sum_{i=1}^{s}}\varphi_{ki}^{T}(x_{k}+h{\textstyle\sum_{j=1}^{s}}a_{ij}f(x_{kj},u_{kj})-x_{ki}),$

then the necessary conditions for minimization state that the partial
derivative of $\mathcal{V}_{k}$ with respect to $x_{ki}$, $u_{ki}$ and
$\varphi_{ki}$ $(i=1,...,s)$ should vanish. Specifically, we have,

$\displaystyle
D_{x_{ki}}\mathcal{V}_{k}(x_{k},x_{k1},...,x_{ks},u_{k1},...,u_{ks},\varphi_{k1},...,\varphi_{ks})$
(61a)
$\displaystyle=hb_{i}[D_{x}C(x_{ki},u_{ki})+D_{x}^{T}f(x_{k_{i}},u_{ki})(p_{k+1}+{\textstyle\sum_{j=1}^{s}}\frac{a_{ji}}{b_{i}}\varphi_{kj})]-\varphi_{ki}=0,$
$\displaystyle
D_{u_{ki}}\mathcal{V}_{k}(x_{k},x_{k1},...,x_{ks},u_{k1},...,u_{ks},\varphi_{k1},...,\varphi_{ks})$
(61b)
$\displaystyle=hb_{i}[D_{u}C(x_{ki},u_{ki})+D_{u}^{T}f(x_{k_{i}},u_{ki})(p_{k+1}+{\textstyle\sum_{j=1}^{s}}\frac{a_{ji}}{b_{i}}\varphi_{kj})]=0,$
$\displaystyle
D_{\varphi_{ki}}\mathcal{V}_{k}(x_{k},x_{k1},...,x_{ks},u_{k1},...,u_{ks},\varphi_{k1},...,\varphi_{ks})$
(61c)
$\displaystyle=x_{k}+h{\textstyle\sum_{j=1}^{s}}a_{ij}f(x_{kj},u_{kj})-x_{ki}=0$

for all $1\leq i\leq s$. The discrete HJB equation (12) can now be rewritten
as

(62) $\displaystyle V_{k}(x_{k})=$
$\displaystyle\mathcal{V}_{k}(x_{k},x_{k1},...,x_{ks},u_{k1},...,u_{ks},\varphi_{k1},...,\varphi_{ks})$
$\displaystyle=$ $\displaystyle
h{\textstyle\sum_{i=1}^{s}}b_{i}C(x_{ki},u_{ki})+V_{k+1}(x_{k}+h{\textstyle\sum_{i=1}^{s}}b_{i}f(x_{ki},u_{ki}))$
$\displaystyle+{\textstyle\sum_{i=1}^{s}}\varphi_{ki}^{T}(x_{k}+h{\textstyle\sum_{j=1}^{s}}a_{ij}f(x_{kj},u_{kj})-x_{ki})$

in which $x_{ki},\ u_{ki}$ and $\varphi_{ki}$ $(1\leq i\leq s)$ are functions
with variable $x_{k}$ implicitly determined by (61a), (61b) and (61c).
Derivate both side of (62) with respect to $x_{k}$ and note that the partial
derivatives of $\mathcal{V}$ with respect to $x_{ki}$, $u_{ki}$,
$\varphi_{ki}$ all vanish, we can obtain:

(63) $p_{k}=p_{k+1}+{\textstyle\sum_{i=1}^{s}}\varphi_{ki}.$

By introducing the variables

(64) $p_{ki}\triangleq
p_{k+1}+{\textstyle\sum_{j=1}^{s}}\frac{a_{ji}}{b_{i}}\varphi_{kj},\ 1\leq
i\leq s,$

we can rewrite the conditions (61a) and (61b) as

(65a) $\displaystyle
hb_{i}(D_{x}C(x_{ki},u_{ki})+D_{x}^{T}f(x_{ki},u_{ki})p_{ki})=\varphi_{ki},$
(65b) $\displaystyle
hb_{i}(D_{u}C(x_{ki},u_{ki})+D_{u}^{T}f(x_{ki},u_{ki})p_{ki})=0.$

Since $h>0,\ b_{i}>0$, (65b) is equivalent to the following formula

$D_{u}C(x_{ki},u_{ki})+D_{u}^{T}f(x_{ki},u_{ki})p_{ki}=0$

which has the same form as the third canonical equation (6c), thus we have
$u_{ki}=\hat{u}(x_{ki},p_{ki})$. Substituting (65a) into (63), we can obtain

(66)
$p_{k}=p_{k+1}+h{\textstyle\sum_{k=1}^{s}}b_{i}(D_{x}C(x_{ki},u_{ki})+D_{x}^{T}f(x_{ki},u_{ki})p_{ki}).$

Plugging (65a) and (66) into (64), we can get the expression of $p_{ki}$,
namely,

$p_{ki}=p_{k}-h{\textstyle\sum_{j=1}^{s}}\bar{a}_{ij}[D_{x}^{T}f(x_{kj},u_{kj})p_{kj}+D_{x}C(x_{kj},u_{kj})],\
1\leq i\leq s,$

where $\bar{a}_{ij}=b_{j}-\frac{b_{j}a_{ji}}{b_{i}}$.

Now we have proven that the necessary condition for Problem 2.2 is given by
the equations (13a)-(13d) in Proposition 2.1. The existence and uniqueness of
solution can be ensured when time step $h$ is small enough (more details can
be seen in [10]).

##  Appendix B Calculation of search direction in ILQR method

The search direction $\tilde{U}-U^{0}$ of ILQR in Subsection 5.1 can be
calculated by solving the following optimization problem in which the minimum
is $\tilde{U}$.

$\min_{U_{k}}{\textstyle\sum_{k=0}^{N-1}}(\frac{1}{2}X_{k}^{T}Q_{h}X_{k}+\frac{1}{2}U_{k}^{T}R_{h}U_{k})+\frac{1}{2}x_{N}^{T}Mx_{N}$

subject to

(67a) $\displaystyle x_{k+1}-x^{0}_{k+1}$
$\displaystyle=x_{k}-x^{0}_{k}+B_{k}(X_{k}-X_{k}^{0})+C_{k}(U_{k}-U_{k}^{0}),$
(67b) $\displaystyle X_{k}-X^{0}_{k}$
$\displaystyle=Z(x_{k}-x_{k}^{0})+A^{1}_{k}(X_{k}-X_{k}^{0})+A^{2}_{k}(U_{k}-U_{k}^{0}),$

where $Q_{h}$ and $R_{h}$ have the same definitions as in (31), and

$\displaystyle A^{1}_{k}$
$\displaystyle=h\begin{bmatrix}a_{11}D_{x}f(x^{0}_{k1},u^{0}_{k1})&\cdots&a_{1s}D_{x}f(x^{0}_{ks},u^{0}_{ks})\\\
\vdots&&\vdots\\\
a_{s1}D_{x}f(x^{0}_{k1},u^{0}_{k1})&\cdots&a_{ss}D_{x}f(x^{0}_{ks},u^{0}_{ks})\end{bmatrix},$
$\displaystyle A^{2}_{k}$
$\displaystyle=h\begin{bmatrix}a_{11}D_{u}f(x^{0}_{k1},u^{0}_{k1})&\cdots&a_{1s}D_{u}f(x^{0}_{ks},u^{0}_{ks})\\\
\vdots&&\vdots\\\
a_{s1}D_{u}f(x^{0}_{k1},u^{0}_{k1})&...&a_{ss}D_{u}f(x^{0}_{ks},u^{0}_{ks})\end{bmatrix},Z=\begin{bmatrix}I_{n}(1)\\\
\vdots\\\ I_{n}(s)\end{bmatrix},$

$\displaystyle B_{k}$
$\displaystyle=h\begin{bmatrix}b_{1}D_{x}f(x^{0}_{k1},u^{0}_{k1})&...&b_{s}D_{x}f(x^{0}_{ks},u^{0}_{ks})\end{bmatrix},$
$\displaystyle C_{k}$
$\displaystyle=h\begin{bmatrix}b_{1}D_{u}f(x^{0}_{k1},u^{0}_{k1})&...&b_{s}D_{u}f(x^{0}_{ks},u^{0}_{ks})\end{bmatrix}.$

We mention that the constraints (67a) and (67b) are the same as the tangent
plane equations (45a) and (45b). After some simple operation, we can transform
the constraints (67b) and (67a) into the form

(69a) $\displaystyle X_{k}=E_{k}x_{k}+F_{k}U_{k}+D_{k}^{1},$ (69b)
$\displaystyle x_{k+1}=G_{k}x_{k}+H_{k}U_{k}+D_{k}^{2},$

where

$\displaystyle E_{k}$ $\displaystyle=(I_{sn}-A_{k}^{1})^{-1}Z,$ $\displaystyle
F_{k}$ $\displaystyle=(I_{sn}-A_{k}^{1})^{-1}A_{k}^{2},$ $\displaystyle G_{k}$
$\displaystyle=I_{n}+B_{k}E_{k},\ $ $\displaystyle H_{k}$
$\displaystyle=B_{k}F_{k}+C_{k},$ $\displaystyle D_{k}^{1}$
$\displaystyle=X_{k}^{0}-E_{k}x_{k}^{0}-F_{k}U_{k}^{0},\ $ $\displaystyle
D_{k}^{2}$ $\displaystyle=x_{k+1}^{0}-G_{k}x_{k}^{0}-H_{k}U_{k}^{0}.$

Now we can write the discrete HJB equation as following

(71) $\displaystyle V_{k}(x_{k})=$
$\displaystyle\min_{U_{k}}\\{\frac{1}{2}X_{k}^{T}Q_{h}X_{k}+\frac{1}{2}U_{k}^{T}R_{h}U_{k}+V_{k+1}(x_{k+1})\\}$
$\displaystyle=$
$\displaystyle\min_{U_{k}}\\{\frac{1}{2}(E_{k}x_{k}+F_{k}U_{k}+D_{k}^{1})^{T}Q_{h}(E_{k}x_{k}+F_{k}U_{k}+D_{k}^{1})$
$\displaystyle+\frac{1}{2}U_{k}^{T}R_{h}U_{k}+V_{k+1}(G_{k}x_{k}+H_{k}U_{k}+D_{k}^{2})\\},\
0\leq k\leq N-1,$

with boundary condition $V_{N}(x_{N})=\frac{1}{2}x_{N}^{T}Mx_{N}$. Then, we
suppose $V_{k}(x_{k})$ has a quadratic form

(72) $V_{k}(x_{k})=\frac{1}{2}x_{k}^{T}M_{k}x_{k}+Y_{k}^{T}x_{k}+c_{k},\ 0\leq
k\leq N,$

where $M_{k}$ is symmetric. According to the boundary condition, we have
$M_{N}=M$, $Y_{N}=0$, $c_{N}=0$. Substituting (72) into the discrete HJB
equation (71) and let the derivative of the right hand of equality with
respect to $U_{k}$ vanish, we can get the following linear feedback law after
combing like terms and matrix inversion

(73) $U_{k}=U_{k}^{1}x_{k}+U_{k}^{2}$

where

(74a) $\displaystyle U_{k}^{1}$
$\displaystyle=-(F_{k}^{T}Q_{h}F_{k}+R_{h}+H_{k}^{T}M_{k+1}H_{k})^{-1}(F_{k}^{T}Q_{h}E_{k}+H_{k}^{T}M_{k+1}G_{k}),$
(74b) $\displaystyle U_{k}^{2}$
$\displaystyle=-(F_{k}^{T}Q_{h}F_{k}+R_{h}+H_{k}^{T}M_{k+1}H_{k})^{-1}(F_{k}^{T}Q_{h}D_{k}^{1}+H_{k}^{T}M_{k+1}D_{k}^{2}+H_{k}^{T}Y_{k+1}),$

$0\leq k\leq N-1$. Plugging (73) into the discrete HJB equation (71), we can
obtain the following iterative formulas for $M_{k}$ and $Y_{k}$ by comparing
both sides of equality:

$\displaystyle M_{k}=$
$\displaystyle(E_{k}+F_{k}U_{k}^{1})^{T}Q_{h}(E_{k}+F_{k}U_{k}^{1})+(U_{k}^{1})^{T}R_{h}U_{k}^{1}$
$\displaystyle+(G_{k}+H_{k}U_{k}^{1})^{T}M_{k+1}(G_{k}+H_{k}U_{k}^{1}),$
$\displaystyle Y_{k}=$
$\displaystyle(E_{k}+F_{k}U_{k}^{1})^{T}Q_{h}(F_{k}U_{k}^{2}+D_{k}^{1})+(U_{k}^{1})^{T}R_{h}U_{k}^{2}$
$\displaystyle+(G_{k}+H_{k}U_{k}^{1})^{T}M_{k+1}(H_{k}U_{k}^{2}+D_{k}^{2})+(G_{k}+H_{k}U_{k}^{1})^{T}Y_{k+1},$

$0\leq k\leq N-1$. With $M_{N}$ and $Y_{N}$ known, we can calculate $M_{k}$
and $Y_{k}$ $(k=N-1,...,0)$ recursively. Then according to (74a) and (74b), we
can obtain the coefficient matrices $U_{k}^{1}$ and $U_{k}^{2}$. Plugging the
feedback law (73) into (69b), the discrete state trajectory can be obtained by

$x_{k+1}=(G_{k}+H_{k}U_{k}^{1})x_{k}+H_{k}U_{k}^{2}+D_{k}^{2},\ x_{0}=a,\
0\leq k\leq N-1.$

At last, the internal stage control $U_{k}$ can be calculated by substituting
$x_{k}$ into (73).

## References

  * [1] G. Albi, M. Herty, and L. Pareschi, Linear multistep methods for optimal control problems and applications to hyperbolic relaxation systems, Appl. Math. Comput., 354 (2019), pp. 460–477.
  * [2] I. Almuslimani and G. Vilmart, Explicit stabilized integrators for stiff optimal control problems, SIAM J. Sci. Comput., 43 (2021), pp. A721–A743.
  * [3] J. T. Betts, Survey of numerical methods for trajectory optimization, J GUID CONTROL DYNAM, 21 (1998), pp. 193–207.
  * [4] J. F. Bonnans and J. Laurent-Varin, Computation of order conditions for symplectic partitioned runge-kutta schemes with application to optimal control, Numer. Math., 103 (2006), pp. 1–10.
  * [5] J. C. Butcher, The numerical analysis of ordinary differential equations: Runge-Kutta and general linear methods, Wiley-Interscience, New York, 1987.
  * [6] A. L. Dontchev, W. W. Hager, A. B. Poore, and B. Yang, Optimality, stability, and convergence in nonlinear control, Appl. Math. Optim., 31 (1995), pp. 297–326.
  * [7] A. L. Dontchev, W. W. Hager, and V. M. Veliov, Second-order runge-kutta approximations in control constrained optimal control, SIAM J. Numer. Anal., 38 (2000), pp. 202–226.
  * [8] T. Erez, Y. Tassa, and E. Todorov, Infinite-horizon model predictive control for periodic tasks with contacts, Robotics: Science and systems VII, (2012), p. 73.
  * [9] M. Giftthaler, M. Neunert, M. Stäuble, J. Buchli, and M. Diehl, A family of iterative gauss-newton shooting methods for nonlinear optimal control, in 2018 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS), IEEE, 2018, pp. 1–9.
  * [10] W. W. Hager, Runge-kutta methods in optimal control and the transformed adjoint system, Numer. Math., 87 (2000), pp. 247–282.
  * [11] E. Hairer, C. Lubich, and G. Wanner, Geometric Numerical Integration: Structure Preserving Algorithms for Ordinary Differential Equations, springer, 2 ed., 2006.
  * [12] E. Hairer, S. P. Nørsett, and G. Wanner, Solving ordinary differential equations. 1, Nonstiff problems, Springer-Verlag, 1993.
  * [13] S. P. Han, Superlinearly convergent variable metric algorithms for general nonlinear programming problems, Math. Program., 11 (1976), pp. 263–282.
  * [14] W. Li and E. Todorov, Iterative linear quadratic regulator design for nonlinear biological movement systems, in ICINCO (1), Citeseer, 2004, pp. 222–229.
  * [15] W. Li and E. Todorov, Iterative linearization methods for approximately optimal control and estimation of non-linear stochastic system, Internat. J. Control, 80 (2007), pp. 1439 – 1453.
  * [16] D. Liberzon, Calculus of Variations and Optimal Control Theory: A Concise Introduction, Princeton university press, Princeton, 2011.
  * [17] Mayne and Q. David, A second-order gradient method for determining optimal trajectories of nonlinear discrete-time systems, Internat. J. Control, 3 (1966), pp. 85–95.
  * [18] D. Murray and S. Yakowitz, Differential dynamic programming and newton’s method for discrete optimal control problems, J. Optim. Theory Appl., 43 (1984), pp. 395–414.
  * [19] J. Nocedal and S. Wright, Numerical optimization, Springer Science & Business Media, 2006.
  * [20] T. Ohsawa, A. M. Bloch, and M. Leok, Discrete hamilton-jacobi theory, SIAM J. Control Optim., 49 (2012), pp. 5438 – 5443.
  * [21] G. Reddien, Collocation at gauss points as a discretization in optimal control, SIAM J. Control Optim., 17 (1979), pp. 298–306.
  * [22] A. Sandu, On consistency properties of discrete adjoint linear multistep methods, tech. report, Department of Computer Science, Virginia Polytechnic Institute, 2007.
  * [23] J. M. Sanz-Serna, Symplectic runge-kutta schemes for adjoint equations, automatic differentiation, optimal control, and more, SIAM Rev., 58 (2016), pp. 3–33.
  * [24] A. Schwartz and E. Polak, Consistent approximations for optimal control problems based on runge–kutta integration, SIAM J. Control Optim., 34 (2012), pp. 1235–1269.
  * [25] O. J. Sina Ober-Blöbaum and J. E. Marsden, Discrete mechanics and optimal control: an analysis, ESAIM: COCV, 17 (2008), pp. 322–352.
  * [26] E. Todorov and W. Li, A generalized iterative lqg method for locally-optimal feedback control of constrained nonlinear stochastic systems, in Proceedings of the 2005, American Control Conference, 2005., IEEE, 2005, pp. 300–306.
  * [27] Z. Xie, C. K. Liu, and K. Hauser, Differential dynamic programming with nonlinear constraints, in 2017 IEEE International Conference on Robotics and Automation (ICRA), IEEE, 2017, pp. 695–702.