# On change of slow variables at crossing the separatrices

Anatoly Neishtadt

###### Abstract

We consider general (not necessarily Hamiltonian) perturbations of Hamiltonian
systems with one degree of freedom near separatrices of the unperturbed
system. We present asymptotic formulas for change of slow variables at
evolution across separatrices.

##  1 Outline of the problem

We consider systems described by differential equations of the form

$\displaystyle\dot{q}$ $\displaystyle=$ $\displaystyle\frac{\partial
H}{\partial p}+\varepsilon f_{q},\,\dot{p}=-\frac{\partial H}{\partial
q}+\varepsilon f_{p},\,\dot{z}=\varepsilon f_{z}\,,$ (1.1) $\displaystyle H$
$\displaystyle=$ $\displaystyle
H(p,q,z),\,f_{\alpha}=f_{\alpha}(p,q,z,\varepsilon),\alpha=p,q,z,\,(p,q)\in\mathbb{R}^{2},z\in\mathbb{R}^{l-2}\,.\
$

Here $\varepsilon$ is a small parameter, $|\varepsilon|\ll 1$. For
$\varepsilon=0,\,z={\rm const}$ we have an unperturbed system for $p,q$, which
is a Hamiltonian system with one degree of freedom. The function $H$ is an
unperturbed Hamiltonian. For $\varepsilon>0$ we have a perturbed system, and
functions $\varepsilon f_{\alpha}$ are perturbations.

It is supposed that there are a saddle point and passing through it
separatrices in the phase portrait of the unperturbed system, Fig. 1. Under
the action of perturbations the projection of the phase point onto the plane
$(p,q)$ crosses a separatrix.

Figure 1: Phase portrait of the unperturbed system.

Separatrices divide the phase plane of the unperturbed systems into domains
$G_{1}(z),G_{2}(z),G_{3}(z)$, Fig. 1. In each of these domains it is possible
to use variables $h,\varphi$ instead of $p,q$, where $h$ is the difference
between $H$ and its value at the saddle point, and $\varphi$ is “the angle”
(from the pair “action-angle” variables [1] of the unperturbed system). Then
for $h,z,\varphi$ we get the perturbed system having the standard form of
system with one rotating phase [2]: in this system $h,z$ are called slow
variables, $\varphi$ is the rotating phase. It is a classical result that the
averaged with respect to $\varphi$ system describes the evolution of $h,z$ far
from separatrices with accuracy $O(\varepsilon)$ during the time interval of
order $1/\varepsilon$ [2]. For approximate description of evolution of $h,z$
for trajectories that cross the unperturbed separatrices one can use the
averaged system up to the separatrix and then averaged system with initial
conditions on the separatrix in one of domains in which the trajectory is
captured (a certain probability can be assigned to each such continuation).
For majority of initial conditions this procedure describes the behaviour of
slow variables with accuracy $O(\varepsilon\ln\varepsilon)$ during time of
order $1/\varepsilon$; the measure of the “bad” set of initial conditions, for
which this description is not valid, tends to $0$ faster than any given power
of $\varepsilon$ as $\varepsilon\to 0$ [10]. One can make one more step of the
averaging method and use the same procedure for the second order averaged
system (it is shown in [11] that solutions of this system indeed arrive to
separatrices). This improves accuracy up to $O(\varepsilon^{2})$ for motions
far from separatrices. However, for motion with separatrix crossing there is
no improvement. The reason is that there is a change of order at least
$\varepsilon$ of slow variables at crossing a narrow neighbourhood of
separatrices. Because the width of this neighbourhood tends to 0 as
$\varepsilon\to 0$, it is reasonable to call this change a jump of slow
variables at the separatrix. In this note we give asymptotic formulas for this
jump. Such formulas were first obtained in [12] for the pendulum in a slowly
varying gravitational field, then in [5, 7] for the general case of a
Hamiltonian system with one degree of freedom and slowly varying parameters,
in [8] for the general case of a slow-fast Hamiltonian system with two degrees
of freedom, and in [3, 4] for motion in a slowly time-dependent potential with
a dissipation. Jump of slow variables is interpreted as a jump of an adiabatic
invariant for Hamiltonian systems [12, 5, 7, 8] and as a time shift for
systems with a dissipation [3, 4]. We consider the case of general perturbed
system (1.1). For derivation of intermediate estimates used in this note see,
e.g., [10, 11].

##  2 Asymptotic expansions for unperturbed motions near separatrices

In the phase portrait of the unperturbed system there is a saddle point
$C=C(z)$ and passing through it separatrices $l_{1}=l_{1}(z),l_{2}=l_{2}(z)$.
We denote $l_{3}=l_{3}(z)=l_{1}(z)\cup l_{2}(z)$. Denote
$q_{C}=q_{C}(z),p_{C}=p_{C}(z)$ coordinates of the point $C$. Denote

$\displaystyle h_{C}(z)$ $\displaystyle=H(p_{C}(z),q_{C}(z),z),\
E(p,q,z)=H(p,q,z)-h_{C}(z).$

We assume that $E>0$ in $G_{3}$, $E<0$ in $G_{1,2}$.

Denote

$\displaystyle f_{z,C}(z$ $\displaystyle)=f_{z}(p_{C}(z),q_{C}(z),z,0),\
F_{z}(p,q,z)=f(p,q,z,0)-f_{z,C}(z),$ $\displaystyle f_{h}(p,q,z)$
$\displaystyle=\frac{\partial E}{\partial p}f_{p}(p,q,z,0)+\frac{\partial
E}{\partial q}f_{q}(p,q,z,0)+\frac{\partial E}{\partial z}f_{z}(p,q,z,0).$

For the period $T$ of the trajectory $E=h$ in domain $G_{i}$ we have

$\displaystyle T=-a_{i}\ln|h|+b_{i}+O(h\ln|h|),\ a_{1}=a_{2}=a,a_{3}=2a,\
b_{3}=b_{1}+b_{2}.$

Denote

$\oint_{l_{i}}f_{h}(p,q,z)dt=-\Theta_{i}(z),\
\oint_{l_{i}}F_{z}(p,q,z)dt=A_{i}(z).$

Then for integrals along the unperturbed phase trajectory $E=h$ in the domain
$G_{i}$ we have

$\displaystyle\oint_{E=h}f_{h}(p,q,z)dt$
$\displaystyle=-\Theta_{i}(z)+O(h\ln|h|),$
$\displaystyle\oint_{E=h}F_{z}(p,q,z)dt$ $\displaystyle=A_{i}(z)+O(h\ln|h|).$

We assume that $\Theta_{1}(z)>0,\Theta_{2}(z)>0$ for all considered values of
$z$.

Introduce the coordinate system $C\xi\eta$ as shown in Fig. 1. For initial
points on the positive side of the axis $C\eta$ and integrals on the
unperturbed phase trajectory $E=h$ (i.e. in $G_{3}$) we have

$\displaystyle\frac{1}{T}\int_{0}^{T}(t-\frac{T}{2})f_{h}dt$
$\displaystyle=-\frac{a\ln
h(\Theta_{2}-\Theta_{1})/2+(\Theta_{1}b_{2}-\Theta_{2}b_{1})/2+d_{3}}{-2a\ln
h+b_{3}}+O(\sqrt{h}\,),$
$\displaystyle\frac{1}{T}\int_{0}^{T}(t-\frac{T}{2})F_{z}dt$
$\displaystyle=-\frac{a\ln
h(A_{1}-A_{2})/2-(A_{1}b_{2}-A_{2}b_{1})/2+g_{3}}{-2a\ln
h+b_{3}}+O(\sqrt{h}\,).$

For initial points on the axis $C\xi$ and integrals on the unperturbed phase
trajectory $E=h$ in the domain $G_{i},i=1,2$ we have

$\displaystyle\frac{1}{T}\int_{0}^{T}(t-\frac{T}{2})f_{h}dt$
$\displaystyle=-\frac{d_{i}}{-a\ln|h|+b_{1}}+O(\sqrt{|h|}\,),$
$\displaystyle\frac{1}{T}\int_{0}^{T}(t-\frac{T}{2})F_{z}dt$
$\displaystyle=-\frac{g_{i}}{-a\ln|h|+b_{i}}+O(\sqrt{|h|}\,).$

We have $d_{3}=d_{1}+d_{2},g_{3}=g_{1}+g_{2}$.

In line with the general approach of the averaging method, one can make a
change of variables

$\displaystyle\begin{split}h&=\overline{h}+\varepsilon
u_{h,1}(\overline{h},\overline{z},\overline{\varphi})+\varepsilon^{2}u_{h,2}(\overline{h},\overline{z},\overline{\varphi}),\\\
z&=\overline{z}+\varepsilon
u_{z,1}(\overline{h},\overline{z},\overline{\varphi})+\varepsilon^{2}u_{z,2}(\overline{h},\overline{z},\overline{\varphi}),\\\
\varphi&=\overline{\varphi}+\varepsilon
u_{\varphi,1}(\overline{h},\overline{z},\overline{\varphi})\end{split}$ (2.1)

that transforms original equations of motion to the following form:

$\displaystyle\begin{split}\dot{\overline{h}}&=\varepsilon\overline{f}_{h,1}(\overline{h},\overline{z})+\varepsilon^{2}\overline{f}_{h,2}(\overline{h},\overline{z})+\varepsilon^{3}\overline{f}_{h,3}(\overline{h},\overline{z},\overline{\varphi},\varepsilon),\\\
\dot{\overline{z}}&=\varepsilon\overline{f}_{z,1}(\overline{h},\overline{z})+\varepsilon^{2}\overline{f}_{z,2}(\overline{h},\overline{z})+\varepsilon^{3}\overline{f}_{z,3}(\overline{h},\overline{z},\overline{\varphi},\varepsilon),\\\
\dot{\overline{\varphi}}&=\omega(\overline{h},\overline{z})+\varepsilon\overline{f}_{\varphi,1}(\overline{h},\overline{z})+\varepsilon^{2}\overline{f}_{\varphi,2}(\overline{h},\overline{z},\overline{\varphi},\varepsilon).\end{split}$
(2.2)

The first order averaged system is obtained by keeping only the first term in
each of these equations. The second order averaged system is obtained by
neglecting highest order terms in each of these equations.

One can show that (see [11])

$u_{h,1}=\frac{1}{T}\int_{0}^{T}(t-\frac{T}{2})f_{h}dt,\
u_{z,1}=\frac{1}{T}\int_{0}^{T}(t-\frac{T}{2})F_{z}dt.$

It is convenient to consider evolution using both usual time $t$ and slow time
$\varepsilon t$.

##  3 Jump of slow variables

###  3.1 General description of motion

Let a phase point start to move at $t=t_{-}=0$ (thus $\tau=\tau_{-}=0$) in the
domain $G_{3}$ at the distance of order 1 from the separatrix. Denote
$h_{-},z_{-},\varphi_{-}$ initial values of variables $h,z,\varphi$. Denote
$h(t),z(t),\varphi(t)$ solution of the system (1.1) with this initial
condition (written in variables $h,z,\varphi$). The phase point makes rounds
close to unperturbed trajectories in $G_{3}$ while moving closer to the
separatrix with each round, approaches the separatrix, crosses the separatrix
and continues the motion in domain $G_{i}$, $i=1$ or $i=2$. Assume, for the
sake of being definite, that this is motion in $G_{2}$. At
$t=t_{+}=K/\varepsilon$ (thus $\tau=\tau_{+}=K$) the phase point is in $G_{2}$
at the distance of order 1 form the separatrix. Here $K={\rm const}$. Denote
$h_{+}=h(t_{+}),z_{+}=z(t_{+}),\varphi_{+}=\varphi(t_{+})$.

Denote $\overline{h}(\tau),\overline{z}(\tau)$ the solution of the first order
averaged system with initial conditions $h_{-},z_{-}$ glued of solutions of
averaged systems for domains $G_{3}$ and $G_{2}$ (cf. [10]). Denote $\tau_{*}$
the moment of the slow time such that $\overline{h}(\tau_{*})=0$ (i.e.
$\tau_{*}$ is the moment of the slow time for the arrival of this solution to
the separatrix). Denote $z_{*}=\overline{z}(\tau_{*})$.

Denote $\hat{h}_{-}(\tau),\hat{z}_{-}(\tau)$ the solution of the second order
averaged system with initial, at $\tau=0$, conditions corresponding to
$h_{-},z_{-},\varphi_{-}$ (i.e., these initial conditions are obtained from
$h_{-},z_{-},\varphi_{-}$ by transformation (2.1)). Denote
$\hat{h}_{+}(\tau),\hat{z}_{+}(\tau)$ the solution of the second order
averaged system with initial, at $\tau=\tau_{+}\,$, conditions corresponding
to $h_{+},z_{+},\varphi_{+}$. We consider this solution for
$\tau\leq\tau_{+}$. Denote $\hat{\tau}_{*,\mp}$ moments of arrival of these
two solutions to the separatrix, $\hat{h}_{\mp}(\tau_{*,\mp})=0$. Denote
$\hat{z}_{*,\mp}=\hat{z}_{\mp}(\hat{\tau}_{*,\mp})$. Denote

$\Delta\hat{\tau}_{*}=\hat{\tau}_{*,+}-\hat{\tau}_{*,-},\
\Delta\hat{z}_{*}=\hat{z}_{*,+}-\hat{z}_{*,-}.$ (3.1)

We will call these values jumps of slow variables at the separatrix. To
estimate these jumps, we will consider description of dynamics by the second
order averaged system at approaching the separatrix (in $G_{3}$) and at moving
away from the separatrix (in $G_{2}$).

For crossing from domain $G_{3}$ to domain $G_{i}$, $i=1,2$, we use also
notations
$\hat{\tau}_{*,3}=\hat{\tau}_{*,-},\hat{\tau}_{*,i}=\hat{\tau}_{*,+},\hat{z}_{*,3}=\hat{z}_{*,-},\hat{z}_{*,i}=\hat{z}_{*,+}$.

Values $f_{z,C},\Theta_{i},A_{i},a_{i},b_{i},d_{i},g_{i}$ are taken at
$z=z_{*}$ in all expansions below.

###  3.2 Approaching the separatrix

Consider motion of the phase point in $G_{3}$. Projection of the phase point
onto $p,q$ plane makes rounds close to unperturbed trajectories while moving
closer to the separatrix with each round. This projection crosses the ray
$C\eta$ on each such round when it moves close enough to the separatrix. We
enumerate $N+1$ moments of time for these intersections starting with the last
one: $t_{0}>t_{1}>\ldots>t_{N}>0$. The moment of time $t_{N}$ is chosen in
such a way that for $0\leq t\leq t_{N}$ dynamics of $h,z$ is described with a
required (high enough) accuracy by the second order averaged system, while for
$t_{N}\leq t\leq t_{0}$ expansions near the separatrix can be used for
description of motion because the phase point is close enough to the
separatrix.

Denote $\tilde{h}(t),\tilde{z}(t),\tilde{\varphi}(t)$ the result of
transformation of solution $h(t),z(t),\varphi(t)$ via formulas (2.1). Denote

$\displaystyle
h_{n}=h(t_{n}),z_{n}=z(t_{n}),\hat{h}_{n}=\hat{h}(t_{n}),\hat{z}_{n}=\hat{z}(t_{n}),\tilde{h}_{N}=\tilde{h}(t_{N}),\tilde{z}_{N}=\tilde{z}(t_{N}).$
(3.2)

Denote $U_{h}(t)=u_{h,1}+\varepsilon u_{h,2},\ U_{z}(t)=u_{z,1}+\varepsilon
u_{z,2}$ where functions $u_{h,i},u_{z,i}$ are those in (2.1), and they are
calculated at the point $\tilde{h}(t),\tilde{z}(t),\tilde{\varphi}(t)$. Denote
$U_{h,n}=U_{h}(t_{n}),U_{z,n}=U_{z}(t_{n})$.

We will use the symbol $\simeq$ in approximate equalities without indication
of accuracy of the approximation. We have

$z_{N}=\tilde{z}_{N}+\varepsilon U_{z,N}\simeq\hat{z}_{n}+\varepsilon
U_{z,N}.$

Then we have an identity

$\displaystyle z_{0}$
$\displaystyle\simeq\hat{z}_{3,*}+(\hat{z}|_{h=h_{0}}-\hat{z}_{3,*})+\left((\hat{z}|_{h=h_{N}}-\hat{z}|_{h=h_{0}})-(z_{N}-z_{0})\right)+(\hat{z}_{N}-\hat{z}|_{h=h_{N}})+\varepsilon
U_{z,N}.$ (3.3)

Estimate terms in this expression separately.

a) For $(\hat{z}|_{h=h_{0}}-\hat{z}_{3,*})$.

This value is the change of $\hat{z}$ from the moment of time when $\hat{h}=0$
till the moment of time when $\hat{h}=h_{0}$. In the principal approximation

$\displaystyle\dot{\hat{z}}=\varepsilon(f_{z,C}+\frac{1}{T}A_{3}),\
\dot{\hat{h}}=-\varepsilon\frac{1}{T}\Theta_{3}.$

Hence

$\frac{d\hat{z}}{d\hat{h}}=-\frac{1}{\Theta_{3}}(Tf_{z,C}+A_{3})$

and

$\displaystyle\hat{z}|_{h=h_{0}}$
$\displaystyle-\hat{z}_{3,*}=-\frac{1}{\Theta_{3}}\int_{0}^{h_{0}}(Tf_{z,C}+A_{3})dh=-\frac{f_{z,C}}{\Theta_{3}}\int_{0}^{h_{0}}Tdh-\frac{A_{3}}{\Theta_{3}}h_{0}$
$\displaystyle=-\frac{f_{z,C}}{\Theta_{3}}\int_{0}^{h_{0}}(-2a\ln
h+b_{3})dh-\frac{A_{3}}{\Theta_{3}}h_{0}=-\frac{f_{z,C}}{\Theta_{3}}\left[-2a(h_{0}\ln
h_{0}-h_{0})+b_{3}h_{0}\right]-\frac{A_{3}}{\Theta_{3}}h_{0}.$

b) For $\left((\hat{z}|_{h=h_{N}}-\hat{z}|_{h=h_{0}})-(z_{N}-z_{0})\right)$.

To calculate this term one can consider motion round by round, calculate
differences between changes of $\hat{z}$ and $z$ on each round, and sum up
these differences. For changes of $h,z$ one can use

$\displaystyle h_{n+1}-h_{n}\simeq\varepsilon\Theta_{3},$ $\displaystyle
z_{n+1}-z_{n}\simeq-\varepsilon f_{z,C}\left(-\frac{a}{2}\ln
h_{n}-a\ln(h_{n}+\varepsilon\Theta_{1})-\frac{a}{2}\ln
h_{n+1}+b_{3}\right)-\varepsilon A_{3}.$

The change of $\hat{z}$ is calculated as

$\hat{z}|_{h=h_{n+1}}-\hat{z}|_{h=h_{n}}\simeq-\frac{f_{z,C}}{\Theta_{3}}\int_{h_{n}}^{h_{n+1}}(-2a\ln
h+b_{3})dh-\varepsilon A_{3}.$

Thus

$\displaystyle(z_{n+1}-z_{n})-(\hat{z}|_{h=h_{n+1}}-\hat{z}|_{h=h_{n}})$
$\displaystyle\simeq
a\frac{f_{z,C}}{\Theta_{3}}\left[\int_{h_{n}}^{h_{n+1}}(-2\ln
h)dh-\varepsilon\Theta_{3}\left(-\frac{1}{2}\ln
h_{n}-a\ln(h_{n}+\varepsilon\Theta_{1})-\frac{1}{2}\ln h_{n+1}\right)\right]$

The expression in the square brackets is related to calculation of the
integral of $-\ln h$ by the trapezoidal method like in [7]. Thus, we can
directly use the expression for change of an adiabatic invariant from [7].
This gives

$\displaystyle(\hat{z}|_{h=h_{N}}-\hat{z}|_{h=h_{0}})-(z_{N}-z_{0})$
$\displaystyle\simeq 2\varepsilon
af_{z,C}\left[-\frac{1}{2}\ln\frac{2\pi}{\Gamma(\xi_{3})\Gamma(\xi_{3}+\theta_{13})}+\xi_{3}+\left(-\xi_{3}+\frac{1}{2}\theta_{23}\right)\ln\xi_{3}\right]$
$\displaystyle+\varepsilon\frac{1}{2}af_{z,C}(\theta_{23}-\theta_{13})(\ln
h_{N}-\ln h_{0}).$

Here $\Gamma(\cdot)$ is the gamma function, $\xi_{3}=h_{0}/\Theta_{3}$,
$\theta_{ij}=\Theta_{i}/\Theta_{j}$.

c) For $(\hat{z}_{N}-\hat{z}|_{h=h_{N}})$.

We have $h(t_{N})=h_{N}$. Denote $\hat{t}_{N}$ the moment of time such that
$\hat{h}(\hat{t}_{N})=h_{N}$. Find $\hat{t}_{N}-t_{N}$.

We have

$\hat{h}(\hat{t}_{N})=h_{N}=h(t_{N})=\tilde{h}(t_{N})+\varepsilon
U_{h,N}\simeq\hat{h}(t_{N})+\varepsilon U_{h,N}.$

Thus

$\hat{t}_{N}-t_{N}\simeq-\frac{1}{\Theta_{3}}TU_{h,N}.$

Value of $T$ is calculated at $h=h_{N}$. Then

$\displaystyle\hat{z}_{N}$
$\displaystyle-\hat{z}|_{h=h_{N}}=\hat{z}(t_{N})-\hat{z}(\hat{t}_{N})\simeq\varepsilon\left(f_{z,C}+\frac{1}{T}A_{3}\right)(t_{N}-\hat{t}_{N})\simeq\varepsilon\left(f_{z,C}+\frac{1}{T}A_{3}\right)\frac{1}{\Theta_{3}}TU_{h,N}$
$\displaystyle\simeq-\varepsilon\frac{f_{z,C}}{\Theta_{3}}\left(\frac{a}{2}(\Theta_{2}-\Theta_{1})\ln
h_{N}+(\Theta_{1}b_{2}-\Theta_{2}b_{1})/2+d_{3}\right)+\varepsilon\frac{1}{4}A_{3}(\theta_{23}-\theta_{13}).$

d) For $\varepsilon U_{z,N}$.

We have $\varepsilon U_{z,N}\simeq\varepsilon(A_{1}-A_{2})/4$.

Combining results of a) - d) we get from identity (3.3)

$\displaystyle z_{0}$
$\displaystyle\simeq\hat{z}_{3,*}-\frac{f_{z,C}}{\Theta_{3}}\left[-2a(h_{0}\ln
h_{0}-h_{0})+b_{3}h_{0}\right]-\frac{A_{3}}{\Theta_{3}}h_{0}$ (3.4)
$\displaystyle+2\varepsilon
af_{z,C}\left[-\frac{1}{2}\ln\frac{2\pi}{\Gamma(\xi_{3})\Gamma(\xi_{3}+\theta_{13})}+\xi_{3}+\left(-\xi_{3}+\frac{1}{2}\theta_{23}\right)\ln\xi_{3}\right]$
$\displaystyle-\varepsilon\frac{1}{2}af_{z,C}(\theta_{23}-\theta_{13})\ln
h_{0}$
$\displaystyle-\varepsilon\frac{1}{2}f_{z,C}\left((\theta_{13}b_{2}-\theta_{23}b_{1})+2\frac{d_{3}}{\Theta_{3}}\right)+\varepsilon\frac{1}{4}A_{3}(\theta_{23}-\theta_{13})$
$\displaystyle+\varepsilon(A_{1}-A_{2})/4$
$\displaystyle=\hat{z}_{3,*}-\frac{f_{z,C}}{\Theta_{3}}\left[-2ah_{0}\ln(\varepsilon\Theta_{3})+b_{3}h_{0}\right]-\frac{A_{3}}{\Theta_{3}}h_{0}$
$\displaystyle+2\varepsilon
af_{z,C}\left[-\frac{1}{2}\ln\frac{2\pi}{\Gamma(\xi_{3})\Gamma(\xi_{3}+\theta_{13})}+\frac{1}{2}\theta_{23}\ln\xi_{3}\right]$
$\displaystyle-\varepsilon\frac{1}{2}af_{z,C}(\theta_{23}-\theta_{13})\ln
h_{0}$
$\displaystyle-\varepsilon\frac{1}{2}f_{z,C}\left((\theta_{13}b_{2}-\theta_{23}b_{1})+2\frac{d_{3}}{\Theta_{3}}\right)+\varepsilon\frac{1}{4}A_{3}(\theta_{23}-\theta_{13})$
$\displaystyle+\varepsilon(A_{1}-A_{2})/4.$

###  3.3 Passage through the separatrix

We assume that
$k\sqrt{\varepsilon}\leq\xi_{3}\leq\theta_{23}-k\sqrt{\varepsilon}$, where $k$
is a large enough constant. Then for $t>t_{0}$ the phase point makes a round
close to the separatrix $l_{2}$ and arrives to the ray $C\xi$ in $G_{2}$ (Fig.
1) at some moment of time $t^{\prime}_{0}=t_{0}+O(\ln\varepsilon)$. Denote
$h_{0}^{\prime}=h(t^{\prime}_{0}),z_{0}^{\prime}=z(t^{\prime}_{0})$. We have

$h_{0}^{\prime}\simeq h_{0}-\varepsilon\Theta_{2},\ z_{0}^{\prime}\simeq
z_{0}+\varepsilon f_{z,C}\left[-\frac{a}{2}\ln
h_{0}-\frac{a}{2}\ln(-h_{0}^{\prime})+b_{2}\right]+\varepsilon A_{2}.$ (3.5)

Denote
$\xi_{2}=(-h_{0}^{\prime})/(\varepsilon\Theta_{2})\simeq(\varepsilon\Theta_{2}-h_{0})/(\varepsilon\Theta_{2})=(\varepsilon\Theta_{2}-\varepsilon\Theta_{3}\xi_{3})/(\varepsilon\Theta_{2})=1-(\Theta_{3}/\Theta_{2})\xi_{3}$.
Thus $\xi_{3}\simeq\theta_{23}(1-\xi_{2})$.

###  3.4 Moving away from the separatrix

For $t>t_{0}^{\prime}$ the projection of the phase point onto $p,q$ plane
makes rounds close to unperturbed trajectories while moving farther away from
the separatrix with each round. This projection crosses the ray $C\xi$ in
$G_{2}$ on each such round while it moves close enough to the separatrix. We
enumerate $N+1$ moments of time for these intersections starting with the
first one:
$t_{0}^{\prime}<t_{1}^{\prime}<\ldots<t_{N}^{\prime}<K/\varepsilon$. The
moment of time $t_{N}^{\prime}$ is chosen in such a way that for
$t_{N}^{\prime}\leq t\leq K/\varepsilon$ changes of $h,z$ are described with a
required (high enough) accuracy by the second order averaged system, while for
$t_{0}^{\prime}\leq t\leq t_{N}^{\prime}$ expansions near the separatrix can
be used for description of motion because the phase point is close enough to
the separatrix. Calculations here are similar to those for approaching the
separatrix in Section 3.2. In what follows, we omit ‘prime’ in notation for
moments of time and use for variables $h,z$ the same notation as in Section
3.2, except of $h_{0}^{\prime},z_{0}^{\prime}$.

We have

$z_{N}=\tilde{z}_{N}+\varepsilon U_{z,N}\simeq\hat{z}_{n}+\varepsilon
U_{z,N}.$

Then we have an identity

$\displaystyle z_{0}^{\prime}$
$\displaystyle\simeq\hat{z}_{2,*}+(\hat{z}|_{h=h_{0}^{\prime}}-\hat{z}_{2,*})+\left((\hat{z}|_{h=h_{N}}-\hat{z}|_{h=h_{0}^{\prime}})-(z_{N}-z_{0}^{\prime})\right)+(\hat{z}_{N}-\hat{z}|_{h=h_{N}})+\varepsilon
U_{z,N}.$ (3.6)

Estimate terms in this expression separately.

a) For $(\hat{z}|_{h=h_{0}}-\hat{z}_{2,*})$.

Similarly to Section 3.2 we get

$\hat{z}|_{h=h_{0}^{\prime}}-\hat{z}_{2,*}\simeq-\frac{f_{z,C}}{\Theta_{2}}\left[-a(h_{0}^{\prime}\ln|h_{0}^{\prime}|-h_{0}^{\prime})+b_{2}h_{0}^{\prime}\right]-\frac{A_{2}}{\Theta_{2}}h_{0}^{\prime}.$
(3.7)

b) For
$\left((\hat{z}|_{h=h_{N}}-\hat{z}|_{h=h_{0}^{\prime}})-(z_{N}-z_{0}^{\prime})\right)$.

Similarly to Section 3.2 we can use result of [7]. This gives

$(\hat{z}|_{h=h_{N}}-\hat{z}|_{h=h_{0}^{\prime}})-(z_{N}-z_{0}^{\prime})\simeq-\varepsilon
af_{z,C}\left[-\ln\frac{\sqrt{2\pi}}{\Gamma(\xi_{2})}+\xi_{2}+(\frac{1}{2}-\xi_{2})\ln\xi_{2}\right].$
(3.8)

c) For $(\hat{z}_{N}-\hat{z}|_{h=h_{N}})$

Similarly to Section 3.2 we get

$\hat{z}_{N}-\hat{z}|_{h=h_{N}}\simeq-\varepsilon\frac{f_{z,C}}{\Theta_{2}}d_{2}.$
(3.9)

d) For $\varepsilon U_{z,N}$.

We get $\varepsilon U_{z,N}\simeq 0$.

Combining results of a) - d) we get from identity (3.6)

$\displaystyle z_{0}^{\prime}$
$\displaystyle\simeq\hat{z}_{2,*}-\frac{f_{z,C}}{\Theta_{2}}\left[-a(h_{0}^{\prime}\ln|h_{0}^{\prime}|-h_{0}^{\prime})+b_{2}h_{0}^{\prime}\right]-\frac{A_{2}}{\Theta_{2}}h_{0}^{\prime}$
(3.10) $\displaystyle-\varepsilon
af_{z,C}\left[-\ln\frac{\sqrt{2\pi}}{\Gamma(\xi_{2})}+\xi_{2}+(\frac{1}{2}-\xi_{2})\ln\xi_{2}\right]-\varepsilon\frac{f_{z,C}}{\Theta_{2}}d_{2}$
$\displaystyle=\hat{z}_{2,*}-\varepsilon{f_{z,C}}\left[a(\xi_{2}\ln(\varepsilon\Theta_{2}\xi_{2})-\xi_{2})-b_{2}\xi_{2}\right]+\varepsilon{A_{2}}\xi_{2}$
$\displaystyle-\varepsilon
af_{z,C}\left[-\ln\frac{\sqrt{2\pi}}{\Gamma(\xi_{2})}+\xi_{2}+(\frac{1}{2}-\xi_{2})\ln\xi_{2}\right]-\varepsilon\frac{f_{z,C}}{\Theta_{2}}d_{2}$
$\displaystyle=\hat{z}_{2,*}-\varepsilon{f_{z,C}}\left[a\xi_{2}\ln(\varepsilon\Theta_{2})-b_{2}\xi_{2}\right]+\varepsilon{A_{2}}\xi_{2}$
$\displaystyle-\varepsilon
af_{z,C}\left[-\ln\frac{\sqrt{2\pi}}{\Gamma(\xi_{2})}+\frac{1}{2}\ln\xi_{2}\right]-\varepsilon\frac{f_{z,C}}{\Theta_{2}}d_{2}.$

###  3.5 Formula for jump of slow variables

Combining results of Sections 3.2, 3.3, 3.4 (formulas (3.4), (3.5) and (3.10)
) we get

$\displaystyle\hat{z}_{2,*}-\varepsilon{f_{z,C}}\left[a\xi_{2}\ln(\varepsilon\Theta_{2})-b_{2}\xi_{2}\right]+\varepsilon{A_{2}}\xi_{2}$
(3.11) $\displaystyle-\varepsilon
af_{z,C}\left[-\ln\frac{\sqrt{2\pi}}{\Gamma(\xi_{2})}+\frac{1}{2}\ln\xi_{2}\right]-\varepsilon\frac{f_{z,C}}{\Theta_{2}}d_{2}$
$\displaystyle\simeq\hat{z}_{3,*}-\frac{f_{z,C}}{\Theta_{3}}\left[-2ah_{0}\ln(\varepsilon\Theta_{3})+b_{3}h_{0}\right]-\frac{A_{3}}{\Theta_{3}}h_{0}$
$\displaystyle+2\varepsilon
af_{z,C}\left[-\frac{1}{2}\ln\frac{2\pi}{\Gamma(\xi_{3})\Gamma(\xi_{3}+\theta_{13})}+\frac{1}{2}\theta_{23}\ln\xi_{3}\right]$
$\displaystyle-\varepsilon\frac{1}{2}af_{z,C}(\theta_{23}-\theta_{13})\ln
h_{0}$
$\displaystyle-\varepsilon\frac{1}{2}f_{z,C}\left((\theta_{13}b_{2}-\theta_{23}b_{1})+2\frac{d_{3}}{\Theta_{3}}\right)+\varepsilon\frac{1}{4}A_{3}(\theta_{23}-\theta_{13})$
$\displaystyle+\varepsilon(A_{1}-A_{2})/4$ $\displaystyle+\varepsilon
f_{z,C}\left[-\frac{a}{2}\ln
h_{0}-\frac{a}{2}\ln(-h_{0}^{\prime})+b_{2}\right]+\varepsilon A_{2}$

Therefore

$\displaystyle\Delta\hat{z}_{*}=\hat{z}_{*,+}-\hat{z}_{*,-}=\hat{z}_{2,*}-\hat{z}_{3,*}$
(3.12)
$\displaystyle\simeq\varepsilon{f_{z,C}}\left[a\xi_{2}\ln(\varepsilon\Theta_{2})-b_{2}\xi_{2}\right]-\varepsilon{A_{2}}\xi_{2}$
$\displaystyle\varepsilon
af_{z,C}\left[-\ln\frac{\sqrt{2\pi}}{\Gamma(\xi_{2})}+\frac{1}{2}\ln\xi_{2}\right]+\varepsilon\frac{f_{z,C}}{\Theta_{2}}d_{2}$
$\displaystyle-\frac{f_{z,C}}{\Theta_{3}}\left[-2ah_{0}\ln(\varepsilon\Theta_{3})+b_{3}h_{0}\right]-\frac{A_{3}}{\Theta_{3}}h_{0}$
$\displaystyle+2\varepsilon
af_{z,C}\left[-\frac{1}{2}\ln\frac{2\pi}{\Gamma(\xi_{3})\Gamma(\xi_{3}+\theta_{13})}+\frac{1}{2}\theta_{23}\ln\xi_{3}\right]$
$\displaystyle-\varepsilon\frac{1}{2}af_{z,C}(\theta_{23}-\theta_{13})\ln
h_{0}$
$\displaystyle-\varepsilon\frac{1}{2}f_{z,C}\left((\theta_{13}b_{2}-\theta_{23}b_{1})+2\frac{d_{3}}{\Theta_{3}}\right)+\varepsilon\frac{1}{4}A_{3}(\theta_{23}-\theta_{13})$
$\displaystyle+\varepsilon(A_{1}-A_{2})/4$ $\displaystyle+\varepsilon
f_{z,C}\left[-\frac{a}{2}\ln
h_{0}-\frac{a}{2}\ln(-h_{0}^{\prime})+b_{2}\right]+\varepsilon A_{2}.$

For passage form $G_{3}$ to $G_{1}$ we would have relation (LABEL:for_jump_0)
with replacement of index ‘2’ by index ‘1’ . The final result in the form for
passage from $G_{3}$ to $G_{i}$ where $i=1\ {\rm or}\ 2$ is simplified to

$\displaystyle\Delta\hat{z}_{*}=\hat{z}_{i,*}-\hat{z}_{3,*}\simeq\varepsilon{f_{z,C}}a(\xi_{i}-\frac{1}{2})(\ln(\varepsilon\Theta_{i})-2\theta_{i3}\ln(\varepsilon\Theta_{3}))$
(3.13) $\displaystyle-\varepsilon
af_{z,C}\ln{\frac{(2\pi)^{3/2}}{\Gamma(\xi_{i})\Gamma(\theta_{i3}(1-\xi_{i})\Gamma(1-\theta_{i3}\xi_{i})}}$
$\displaystyle-\varepsilon
f_{z,C}(\xi_{i}-\frac{1}{2})(b_{i}-\theta_{i3}b_{3})-\varepsilon(\xi_{i}-\frac{1}{2})(A_{i}-\theta_{i3}A_{3})$
$\displaystyle+\varepsilon\frac{f_{z,C}}{\Theta_{i}}\left({d_{i}}-\theta_{i3}{d_{3}}\right).$

This formula is the main result of the current note. In a similar way one can
write formulas for jumps of slow variables due to other passages between
domains $G_{j}$ that occur for other signs of values $\Theta_{j},j=1,2,3$.

Value $\xi_{i}$ is called a crossing parameter or a pseudo-phase. Asymptotic
formulas for the pseudo-phase were obtained in [6] for Hamiltonian systems
with one degree of freedom and slow time dependence, in [9] for slow-fast
Hamiltonian systems with one degree of freedom corresponding to fast motion,
in [3, 4] for motion in a slowly time-dependent potential with a dissipation,
and in [11] for a general perturbed system of form (1.1).

Remark. We do not indicate accuracy of formula (LABEL:z_3i). One can see that
terms $\sim\varepsilon/\ln h_{N}$ are neglected in some intermediate
relations. However, because the final result should not depend on choice of
$h_{N}$, the accuracy of the final formula should be much better. For
Hamiltonian perturbations the accuracy of the final formula is
$O(\varepsilon^{3/2}(|\ln\varepsilon|+(1-\xi_{i})^{-1}))$ [7, 8].

###  3.6 Shift of slow time

The slow time $\tau$ can be considered as a particular slow variable,
$\dot{\tau}=\varepsilon$. The formula for jump (or sift) of slow time for
passage from $G_{3}$ to $G_{i}$, $i=1\ {\rm or}\ 2$, is a particular case of
(LABEL:z_3i) with $f_{z,C}=1$, $A_{j}=0,j=1,2,3$. Thus we get

$\displaystyle\hat{\tau}_{i,*}-\hat{\tau}_{3,*}\simeq\varepsilon
a(\xi_{i}-\frac{1}{2})(\ln(\varepsilon\Theta_{i})-2\theta_{i3}\ln(\varepsilon\Theta_{3}))$
(3.14) $\displaystyle-\varepsilon
a\ln{\frac{(2\pi)^{3/2}}{\Gamma(\xi_{i})\Gamma(\theta_{i3}(1-\xi_{i})\Gamma(1-\theta_{i3}\xi_{i})}}$
$\displaystyle-\varepsilon(\xi_{i}-\frac{1}{2})(b_{i}-\theta_{i3}b_{3})+\frac{\varepsilon}{\Theta_{i}}\left({d_{i}}-\theta_{i3}{d_{3}}\right).$

##  4 Jump of adiabatic invariant

In this Section we derive formulas for jumps of adiabatic invariants in
Hamiltonian systems from obtained formulas for jumps of slow variables.

###  4.1 Time-dependent Hamiltonian system

Let system (1.1) be a Hamiltonian system with the Hamiltonian $H=H(p,q.\tau)$,
$\tau=\varepsilon t$. Denote $S_{i}(\tau)$ area of the domain $G_{i}$,
$i=1,2$. Denote $S_{3}(\tau)=S_{1}(\tau)\cup S_{2}(\tau)$. Then
$\Theta_{j}=dS_{j}/d\tau$, $j=1,2,3$. Consider motion with passage from
$G_{3}$ to $G_{i}$, $i=1\ {\rm or}\ 2$, as in Section 3. Let $J_{-}$ and
$J_{+}$ be the initial (at $t=0$, in $G_{3}$) and final (at $t=K/\varepsilon$,
in $G_{i}$) values of the improved adiabatic invariant. (For the definition of
the improved adiabatic invariant and related formulas see, e.g., [7]). Then
$S_{3}(\hat{\tau}_{3,*})\simeq 2\pi J_{-}$, $S_{i}(\hat{\tau}_{i,*})\simeq
2\pi J_{+}$. We get

$\displaystyle 2\pi J_{+}\simeq
S_{i}(\hat{\tau}_{i,*})=S_{i}(\hat{\tau}_{3,*}+\hat{\tau}_{i,*}-\hat{\tau}_{3,*})\simeq
S_{i}(\hat{\tau}_{3,*})+\Theta_{i}(\hat{\tau}_{i,*}-\hat{\tau}_{3,*}).$ (4.1)

Substitute $(\hat{\tau}_{i,*}-\hat{\tau}_{3,*})$ from (LABEL:tau_3i). We get

$\displaystyle 2\pi J_{+}\simeq S_{i}(\hat{\tau}_{3,*})+\varepsilon
a\Theta_{i}(\xi_{i}-\frac{1}{2})(\ln(\varepsilon\Theta_{i})-2\theta_{i3}\ln(\varepsilon\Theta_{3}))$
(4.2) $\displaystyle-\varepsilon
a\Theta_{i}\ln{\frac{(2\pi)^{3/2}}{\Gamma(\xi_{i})\Gamma(\theta_{i3}(1-\xi_{i})\Gamma(1-\theta_{i3}\xi_{i})}}$
$\displaystyle-\Theta_{i}(\xi_{i}-\frac{1}{2})(b_{i}-\theta_{i3}b_{3})+{\varepsilon}\left({d_{i}}-\theta_{i3}{d_{3}}\right)$

as in [5, 7]. One can replace $S_{i}(\hat{\tau}_{3,*})$ with
$S_{i}(\tau_{*})+\theta_{i3}(2\pi J_{-}-S_{3}(\tau_{*}))$ here.

###  4.2 Slow-fast Hamiltonian system

Let system (1.1) be a slow-fast Hamiltonian system. The Hamiltonian is
$H(p,q,y,x)$ with pairs of conjugate variables $(p,q)$ and
$(y,\varepsilon^{-1}x)$. Equations of motion are

$\displaystyle\dot{q}=\frac{\partial H}{\partial p},\ \dot{p}=-\frac{\partial
H}{\partial q},\ \dot{x}=\varepsilon\frac{\partial H}{\partial y},\
\dot{y}=-\varepsilon\frac{\partial H}{\partial x}.$ (4.3)

Thus, $z=(y,x)$, $f_{z,C}=(-{\partial h_{C}(y,x)}/{\partial x},{\partial
h_{C}(y,x)}/{\partial x})$. Denote $S_{i}(z)=S_{i}(y,x)$ area of domain
$G_{i}$, $i=1,2$. Denote $S_{3}(z)=S_{1}(z)\cup S_{2}(z)$. Then
$\Theta_{j}=\\{S_{j},h_{c}\\},j=1,2,3$, where $\\{\cdot,\cdot\\}$ is the
Poisson bracket with respect to variables $(y,x)$,
$\\{a,b\\}=a^{\prime}_{x}b^{\prime}_{y}-a^{\prime}_{y}b^{\prime}_{x}$ (see
[8]) and

$A_{j}=\left(-\oint_{l_{j}}\frac{\partial(H-h_{c})}{\partial
x}dt,\oint_{l_{j}}\frac{\partial(H-h_{c})}{\partial
y}dt\right)=\left(\frac{\partial S_{j}}{\partial x},-\frac{\partial
S_{j}}{\partial y}\right).$ (4.4)

(cf. [8]).

Consider motion with passage from $G_{3}$ to $G_{i}$, $i=1\ {\rm or}\ 2$, as
in Section 3. Let $J_{-}$ and $J_{+}$ be the initial (at $t=0$, in $G_{3}$)
and final (at $t=K/\varepsilon$, in $G_{i}$) values of the improved adiabatic
invariant. (For the definition of the improved adiabatic invariant and related
formulas see, e.g., [8]). Then $S_{3}(\hat{z}_{3,*})\simeq 2\pi J_{-}$,
$S_{i}(\hat{z}_{i,*})\simeq 2\pi J_{+}$. Then we get

$\displaystyle 2\pi J_{+}\simeq
S_{i}(\hat{z}_{i,*})=S_{i}(\hat{z}_{3,*}+\hat{z}_{i,*}-\hat{z}_{3,*})\simeq
S_{i}(\hat{z}_{3,*})+({\rm grad}\,S_{i}\cdot(\hat{z}_{i,*}-\hat{z}_{3,*})).$
(4.5)

Here $(\phantom{*}\cdot\phantom{*})$ is the standard scalar product.
Substitute $(\hat{z}_{i,*}-\hat{z}_{3,*})$ from (LABEL:z_3i) and note that

$\displaystyle({\rm grad}\,S_{i}\cdot f_{z,C})=\Theta_{i},\ ({\rm
grad}\,S_{i}\cdot A_{i})=\\{S_{i},S_{i}\\}=0,\ ({\rm grad}\,S_{i}\cdot
A_{3})=-\\{S_{i},S_{3}\\}.$ (4.6)

We get

$\displaystyle 2\pi J_{+}\simeq
S_{i}(\hat{z}_{3,*})+\varepsilon\Theta_{i}a(\xi_{i}-\frac{1}{2})(\ln(\varepsilon\Theta_{i})-2\theta_{i3}\ln(\varepsilon\Theta_{3}))$
(4.7) $\displaystyle-\varepsilon
a\Theta_{i}\ln{\frac{(2\pi)^{3/2}}{\Gamma(\xi_{i})\Gamma(\theta_{i3}(1-\xi_{i})\Gamma(1-\theta_{i3}\xi_{i})}}$
$\displaystyle-\varepsilon\Theta_{i}(\xi_{i}-\frac{1}{2})(b_{i}-\theta_{i3}b_{3})-\varepsilon\theta_{i3}(\xi_{i}-\frac{1}{2})\\{S_{i},S_{3}\\}$
$\displaystyle+\varepsilon\left({d_{i}}-\theta_{i3}{d_{3}}\right).$

For systems with two degrees of freedom one can approximately calculate
$S_{i}(\hat{z}_{3,*})$ via initial value of the improved adiabatic invariant
and solution of the first order averaged system. Consider motion in the energy
level $H=h$. Relations

$S_{3}(\hat{z}_{3,*})\simeq 2\pi J_{-},\quad h_{C}(\hat{z}_{3,*})\simeq
h,\quad h_{C}(z_{*})=h$ (4.8)

imply that

$\displaystyle({\rm grad}\,S_{3}\cdot(\hat{z}_{3,*}-z_{*}))\simeq 2\pi
J_{-}-S_{3}(z_{*}),\quad({\rm grad}\,h_{C}\cdot(\hat{z}_{3,*}-z_{*}))\simeq
0.$ (4.9)

We have

$S_{i}(\hat{z}_{3,*})=S_{i}(z_{*}+\hat{z}_{3,*}-z_{*})\simeq
S_{i}(z_{*})+({\rm grad}\,S_{i}\cdot(\hat{z}_{3,*}-z_{*})).$ (4.10)

Solve equations (4.9) for $(\hat{z}_{3,*}-z_{*})$ and substitute the result to
(4.10). We get

$S_{i}(\hat{z}_{3,*})\simeq S_{i}(z_{*})+\theta_{i3}(2\pi
J_{-}-S_{3}(z_{*})).$ (4.11)

Substitution of this relation to (LABEL:J_sf) gives an expression for jump of
the adiabatic invariant in [8].

##  5 Conclusions

The main result of this note is the asymptotic formula (LABEL:z_3i) for change
of slow variables at evolution across separatrices in systems of form (1.1).
Together with formula for phase change in such systems [11] this gives a
rather complete description of dynamics with separatrix crossings in the
considered class of systems.

## References

  * [1] Arnold V. I. Mathematical Methods of Classical Mechanics: Graduate Texts in Mathematics 60. Springer-Verlag, New York (1978), x+462 pp.
  * [2] Bogolyubov N. N., Mitropolskij Yu. A. Asymptotic Methods in the Theory of Non-Linear Oscillations. Hindustan Publ. Corp., Delhi; Gordon and Breach Sci. Publ., New York (1961), x+537 pp.
  * [3] Bourland F. J., Haberman R. Separatrix crossing: time-invariant potentials with dissipation. SIAM J. Appl. Math., 50, 6, 1716–1744 (1990)
  * [4] Bourland F. J., Haberman R. Connection across a separatrix with dissipation. Stud. Appl. Math., 91, 95–124 (1994)
  * [5] Cary J. R., Escande D. F., Tennyson J. L.  Adiabatic-invariant change due to separatrix crossing.  Physical Review A, 34, 4256–4275 (1986)
  * [6] Cary J. R., Skodje R. T. Phase change between separatrix crossings. Physica D, 36, 3, 287–316 (1989)
  * [7] Neishtadt A. I.  Change of an adiabatic invariant at a separatrix.  Soviet Journal of Plasma Physics, 12, 568–573 (1986)
  * [8] Neishtadt A. I. On the change in the adiabatic invariant on crossing a separatrix in systems with two degrees of freedom. J. Appl. Math. Mech., 51, 5, 586–592 (1987)
  * [9] Neishtadt A. I., Vasiliev A. A. Phase change between separatrix crossings in slow-fast Hamiltonian systems.Nonlinearity, 18, 3, 1393–1406 (2005)
  * [10] Neishtadt A. I. Averaging method for systems with separatrix crossing. Nonlinearity, 30, 7, 2871–2917 (2017)
  * [11] Neishtadt A. I., Okunev A.V. Phase change and order 2 averaging for one-frequency systems with separatrix crossing. Nonlinearity, 35, 8, 4469–4516 (2022)
  * [12] Timofeev A. V. On the constancy of an adiabatic invariant when the nature of the motion changes. Sov. Phys., JETP, 48, 656–659 (1978)

Anatoly Neishtadt

Department of Mathematical Sciences

Loughborough University, Loughborough LE11 3TU, United Kingdom

E-mail<EMAIL_ADDRESS>


# Efficient LQR-CBF-RRT*: Safe and Optimal Motion Planning ††thanks: 1Guang
Yang is with Computer Science, University of Cambridge, Cambridge, CB3 0FD,
UK<EMAIL_ADDRESS>††thanks: 2Mingyu Cai is with Mechanical Engineering,
Lehigh University, Bethlehem, PA, 18015 USA<EMAIL_ADDRESS>††thanks:
3Ahmad Ahmad, Roberto Tron and Calin Belta are with Boston University
<EMAIL_ADDRESS>
Guang Yang1 Mingyu Cai2 Ahmad Ahmad3 Calin Belta3 Roberto Tron3

###### Abstract

Control Barrier Functions (CBF) are a powerful tool for designing safety-
critical controllers and motion planners. The safety requirements are encoded
as a continuously differentiable function that maps from state variables to a
real value, in which the sign of its output determines whether safety is
violated. In practice, the CBFs can be used to enforce safety by imposing
itself as a constraint in a Quadratic Program (QP) solved point-wise in time.
However, this approach costs computational resources and could lead to
infeasibility in solving the QP. In this paper, we propose a novel motion
planning framework that combines sampling-based methods with Linear Quadratic
Regulator (LQR) and CBFs. Our approach does not require solving the QPs for
control synthesis and avoids explicit collision checking during samplings.
Instead, it uses LQR to generate optimal controls and CBF to reject unsafe
trajectories. To improve sampling efficiency, we employ the Cross-Entropy
Method (CEM) for importance sampling (IS) to sample configurations that will
enhance the path with higher probability and store computed optimal gain
matrices in a hash table to avoid re-computation during rewiring procedure. We
demonstrate the effectiveness of our method on nonlinear control affine
systems in simulation.

##  I Introduction

Robot motion planning is essential to real-world autonomous applications, such
as persistent surveillance, self-driving vehicles, and search and rescue
missions. The general formulation of motion planning is to compute a sequence
of controls for a robot with given dynamics and initial state to reach a
desired goal state without colliding with any obstacles or violating any
constraints on its states. A significant challenge in motion planning is
generating controls for the robot with nonlinear systems such that the
generated state trajectories are collision-free and dynamically feasible. Over
the years, sampling-based planners have been widely used to solve this
problem, such as Probabilistic Road Map (PRM) [1] and Rapidly-exploring Random
Trees (RRT) [2]; they have been demonstrated in real-world robotics and are
theoretically proven to be probabilistic complete. In addition, there is the
asymptotically optimal RRT (RRT*) and its variants [3, 4, 5], which produces
asymptotically optimal paths by rewiring the tree nodes to minimize the
distance cost from the initial configuration to the goal region. There is also
work that reduce the sampling space over time, such as Informed RRT* [6] that
outperforms RRT* in terms of rate of convergence. However, these approaches
assume a collision-checking method exists during trajectory extension and
sampling, such that unsafe states can be rejected to ensure safety. In
addition, Informed RRT* does not consider dynamics during planning, therefore,
the feasibility and control efforts are not considered. There are works [7, 8]
that improves the collision checking procedure in sampling-based motion
planner. But it can still be costly and needs to provide a general solution to
different nonlinear systems. Recent work [9] employs geometric RRT* as
guidance of deep reinforcement learning to improve the exploration efficiency.
Our work aims to create a complete sampling-based motion planning framework
that can efficiently generate samples from an optimal distribution while
ensuring optimality and safety in the generated state trajectory.

Control Barrier Functions. There has been extensive research on safety-
critical control and motion planning with CBFs. The significant advantage of
CBFs comes from their guarantees on forward invariance [10], i.e., if the
system trajectory is initialized in a safe set, it will never leave the set if
there exists a corresponding CBF. The standard formulation of the CBF-based
controller uses Quadratic Programs (QP) [11], which utilizes Control Lyapunov
Function (CLF) for stability and CBFs for safety. The resulting controller is
then solved point-wise in time [12, 13, 14]. For motion planning, the first
CBF-based sampling-based motion planning algorithm was introduced in [15]. In
contrast to [16, 17], the forward invariance property from CBFs removes the
requirement of explicit collision checking as all the generated state
trajectory must stay within the pre-defined safety sets. The approach can
generalize to any control affine nonlinear system, and safety sets require no
assumption on linearity or convexity. Later works, such as [18, 19], were
built on top of it to improve its computational and sampling efficiency.
Inspired by RRT∗ [20] and [15], the work in [19] implemented a rewiring
procedure to improve the cost of the produced path. Since they have used a
CBF-QP-based local planner, they do not provide optimality guarantees on the
produced path.

Linear Quadratic Regulator The LQR is a well-known optimal control strategy
[21], [22] that minimizes a quadratic cost function on system states and
control inputs. The algorithm calculates an optimal state-feedback gain given
system dynamics. An algebraic Riccati equation is often used to solve for the
optimal gain. It has demonstrated its effectiveness on robotic systems [23,
24, 25, 26]. There are also variations of the classic LQR control strategy,
such as iterative LQR [27, 21, 28] that handles nonlinear system dynamics by
iteratively calculating a locally optimal control policy. The iLQR uses an
initialized sequence of nominal control to obtain an open-loop state
trajectory. Then, a second-order Taylor expansion is performed to approximate
the cost function, and a first-order Taylor expansion is performed to
approximate system dynamics at each state-control pair. After approximations
are made, the classic LQR can be used to solve. While this control strategy
can work on general nonlinear systems, it is often expensive to implement,
especially for a long time horizon. In the context of sampling-based motion
planning, there have been some researches on combing LQR with RRT like motion
planner [29, 30, 31]. But these approaches rely on underlying collision
checking during edge extension and rewiring procedures.

In prior works [15, 19], the Control Lyapunov Function (CLF) and CBF are used
in the steering function to ensure stability and safety. CBF and CLF are
formulated as inequality constraints for a Quadratic Program (QP). For each
time step, a QP is solved to generate controls that steer the system
trajectory towards the sampled new state while the CBF constraints ensure the
resultant trajectory will never collide with obstacles. The objective of the
QP ensures the control efforts are minimized point-wise in time. However, this
approach has a few problems: (i) Solving a sequence of QPs in the steering
function can be costly, especially when the step size is selected to be small.
(ii) The QP controller only ensures sub-optimal controls as the formulation of
its objective function only guarantees optimality point-wise in time. (iii)
The sampling method in [15] is based on uniform sampling, which can be
inefficient as the entire space can be sampled. (iv.) The CBF-CLF-QP
controller could suffer from feasibility issues [32], which highly depends on
the system dynamics and the CBF parameters chosen apriori.

Contribution In summary, we propose a novel optimal and safe sampling-based
motion planner that accounts for system dynamics during the planning and
generates state trajectories offline, such that they can be later tracked by
feedback controllers online. Our framework can be applied for both linear and
nonlinear control affine systems by integrating LQR control and CBF into
steering functions, and can be easily extended to discrete-time systems with
mild assumptions on the CBFs. Our proposed approach is shown to significantly
improve the efficiency from several aspects, and contributions are listed as
follows:

  * •

For linear systems, we show the benefits of using LQR rather than model
predictive control (MPC) and iterative LQR. For nonlinear systems, we reduce
the frequency of computing LQR for local linearized models by storing the
previously calculated feedback gains and avoiding repetitive LQR calculation
during the rewiring procedure.

  * •

To reduce the computational cost of generating safety-critical trajectories
and mitigates the in-feasibility in traditional CBF-CLF-QP controllers in
sampling-based motion planning [15, 19]. Our framework does not require
formulating a CBF-QP-based controller, and avoids solving numerous
optimization problems.

  * •

For better efficiency, we incorporate stochastic optimization by replacing the
uniform sampling methods from [15, 29] with adaptive sampling using cross-
entropy methods for importance sampling (IS).

  * •

We provide theoretical proof of probabilistic completeness in our framework,
and compare our work with several baselines in terms of efficiency.

##  II Preliminaries

Notation: A function $f:\mathbb{R}^{n}\mapsto\mathbb{R}^{m}$ is called
_Lipschitz continuous_ on $\mathbb{R}^{n}$ if there exists a positive real
constant $L\in\mathbb{R}^{+}$, such that $\|f(y)-f(x)\|\leq L\|y-x\|,\forall
x,y\in\mathbb{R}^{n}$. For a continuously differentiable function
$h:\mathbb{R}^{n}\mapsto\mathbb{R}$, we use $\dot{h}$ to denote the derivative
with respect to time $t$. The
$\pounds^{r}_{f}h(x):=\frac{\partial\pounds^{r-1}h(x)}{\partial
x}f(x),\pounds^{r}_{g}\pounds^{r-1}_{f}h(x):=\frac{\partial\pounds_{f}^{r-1}h(x)}{\partial
x}g(x)$ are the $r$-th-order Lie derivatives [33]. We define continuous
function $\alpha:[-b,a)\mapsto[-\infty,\infty)$, for some $a>0,b>0$, as a
class kappa function, denoted as $\mathcal{K}$. The $\alpha$ is strictly
increasing, and $\alpha(0)=0$. We denote $B_{r}(x)$ as a ball of region with
radius r centered at $x\in\mathbb{R}^{n}$.

###  II-A System Dynamics

Consider a continuous time dynamical control system

$\dot{x}=f(x)+g(x)u,$ (1)

where $x\in\mathcal{X}\subset\mathbb{R}^{n}$,
$u\in\mathcal{U}\subset\mathbb{R}^{m}$, and
$f(x):\mathbb{R}^{n}\mapsto\mathbb{R}^{n}$,
$g(x):\mathbb{R}^{n}\mapsto\mathbb{R}^{n\times m}$ are locally Lipschitz
continuous.

###  II-B Linear Quadratic Regulator

We use LQR to compute optimal control policies. The cost function with the
infinite time horizon is defined as

$J=\int_{0}^{\infty}x^{T}Qx+u^{T}Ru,$ (2)

where $Q=Q^{T}\succeq 0$ and $R=R^{T}\succeq 0$ are weight matrices for state
$x$ and control $u$, respectively. For linear system dynamics, we can compute
the closed form solution for the optimal control for a given linear-time
invariant systems. For general nonlinear system (1), we can obtain its
linearized form using Taylor-expansion near a local state and control as

$\dot{x}=Ax+Bu.$ (3)

Given the system (3) and the cost function (2), we can compute the LQR gain
matrix $K_{LQR}$ by solving the algebraic Riccati equation with cost matrix
$P$ as

$A^{T}P+PA-PBR^{-1}B^{T}P+Q=0,$ (4)

and

$K_{\mathrm{LQR}}=R^{-1}B^{T}P.$ (5)

Finally, the optimal control policy is denoted as

$\pi^{*}(x)=-K_{\mathrm{LQR}}x$ (6)

For nonlinear systems, we a linearization can be made w.r.t. an equilibrium
point $(x_{eq},u_{eq})$ using first-order taylor expansion. Given a nonlinear
system,

$\displaystyle\dot{x}$ $\displaystyle=F(x,u)$ $\displaystyle\approx
F(x_{\mathrm{eq}},u_{\mathrm{eq}})+\frac{\partial
F(x_{\mathrm{eq}},u_{\mathrm{eq}})}{\partial x}(x-x_{\mathrm{eq}})$
$\displaystyle+\frac{\partial F(x_{\mathrm{eq}},u_{\mathrm{eq}})}{\partial
u}(u-u_{\mathrm{eq}})$

Based on the equilibrium point , the linearized system can be re-written as

$\dot{\hat{x}}=\hat{A}\hat{x}+\hat{B}\hat{u},$ (7)

with $\hat{A}=\frac{\partial F(x_{\mathrm{eq}},u_{\mathrm{eq}})}{\partial x}$
and $\hat{B}=\frac{\partial F(x_{\mathrm{eq}},u_{\mathrm{eq}})}{\partial u}$.
Finally, we can compute the optimal gain $K_{\mathrm{LQR}}$ with the Riccati
equation (4).

###  II-C Higher Order Control Barrier Functions

Before we formally introduce the CBF, let us consider a continuously
differentiable function $h(x):\mathbb{R}^{n}\mapsto\mathbb{R}$. We denote the
$r_{b}$-th derivative of $h(x)$ with respect to time $t$ of (1) as

$h^{r_{b}}(x)=\pounds^{r_{b}}_{f}h(x)+\pounds^{r_{b}}_{g}\pounds^{r_{b}-1}_{f}h(x)u,$
(8)

The relative degree $r_{\mathrm{b}}\geq 0$ is defined as the smallest natural
number such that $\pounds_{g}\pounds_{f}^{r_{\mathrm{b}}-1}h(x)u\neq 0$.

Next, we formally introduce the definition of CBF. Given an invariant set
using time-varying function
$h:\mathbb{R}^{n}\times[t_{0},\infty)\mapsto\mathbb{R}$ that is $r_{b}^{th}$
order differentiable as

$\displaystyle\Psi_{0}(x,t)=h(x,t),$ (9) $\displaystyle\dots$
$\displaystyle\Psi_{r_{b}}(x,t)=\dot{\Psi}_{r_{b}-1}+\alpha_{r_{b}}(\Psi_{r_{b}-1}(x,t)).$

Next, we denote a series of safety sets based on $\Psi_{i}$ as

$\displaystyle\mathfrak{C}_{0}=\\{x\in{\mathbb{R}^{n}}|\Psi_{0}(x,t)\geq
0\\},$ (10) $\displaystyle\dots$
$\displaystyle\mathfrak{C}_{r_{b}}=\\{x\in{\mathbb{R}^{n}}|\Psi_{r_{b}}(x,t)\geq
0\\}.$

######  Definition 1.

[34] Given the functions defined in (9) and safety sets (10), the $r_{b}^{th}$
order differentiable function
$h:\mathbb{R}^{n}\times[t_{0},\infty)\mapsto\mathbb{R}$ is a Higher Order
Control Barrier Function (HOCBF) for system (1) if there exists class $K$
functions $\alpha_{1},\dots,\alpha_{r_{b}}$ such that

$\Psi_{r_{b}}(x(t),t)\geq 0,$ (11)

$\forall$
$(x,t)\in\mathfrak{C}_{0}\cap\dots\cap\mathfrak{C}_{r_{b}}\times[t_{0},\infty)$.
The system is forward invariant [10], i.e., the system trajectory remains safe
for all time.

##  III Problem Formulation

######  Problem 1.

Consider a control affine system (1) with state space $\mathcal{X}$ and
control space $\mathcal{U}$. We define the obstacles as the union of regions
in $\mathcal{X}$ in which the robot configurations state variables coincide
with the obstacles,
$\mathcal{X}_{\mathrm{i,obs}}\subset\mathcal{X},i=1,\dots,\mathrm{N_{obs}}$
and the obstacle-free set
$\mathcal{X}_{\mathrm{safe}}:=\mathcal{X}\setminus\cup_{i=1}^{\mathrm{N_{obs}}}\mathcal{X}_{\mathrm{i,obs}}$111We
define the obstacles as their image directly in $\mathcal{X}$ because in the
local motion planner of the proposed algorithm, we use a controller synthesis
that guarantees that $\mathcal{X}_{\mathrm{safe}}$ is forward invariant in
(1), which implies that collision with obstacles is avoided.. We denote time
horizon to be $T\in\mathbb{R}^{+}$.

Given an initial state
$x_{\mathrm{init}}\in\mathcal{X}_{\mathrm{init}}\subset\mathcal{X_{\mathrm{safe}}}$,
where $\mathcal{X}_{\mathrm{init}}$ is an initial obstacle free set, and a
bounded goal region with
$\mathcal{X}_{\mathrm{goal}}=B_{r_{\mathrm{goal}}}(x_{\mathrm{goal}})$ for
some pre-defined radius $r_{\mathrm{goal}}$, such that
$\mathcal{X}_{\mathrm{goal}}\subset\mathcal{X}_{\mathrm{safe}}$. Find feasible
control inputs $\textbf{u}:[0,T]\mapsto\mathcal{X}$ that steers the system to
$x(T)\in\mathcal{X}_{\mathrm{goal}}$ and
$x(t)\notin\mathcal{X}_{\mathrm{obs}},\forall t\in[0,T]$, while minimizing a
cost function $J:\mathcal{X}\times\mathcal{U}\mapsto\mathbb{R}$.

##  IV LQR-CBF-RRT*

In this section, we aim to solve problem 1 using sampling-based motion
planning that first uniformly samples directly in the configuration space and
then uses a steering function based on system dynamics (1). Our framework
encodes goal-reaching and safety requirements during the control synthesis
procedure in the steering function. To ensure asymptotic optimality [35], an
rewiring procedure is required.

We introduce LQR-CBF-RRT* algorithm as an extension of CBF-RRT [15] that
incorporates HOCBF constraints (11) checking for collision avoidance, and LQR
policy (6) for control synthesis.

######  Remark 1.

We select HOCBF[34] to encode safety constraints to ensure our motion planner
can generalize well to systems with arbitrary relative degrees. More
importantly, the proposed framework does not limit enforcing safety in the
state to the same relative degree. For example, we can enforce safety
constraints on position for obstacle avoidance and velocity for additional
safety requirements.

During each iteration, a state, denoted as $x_{\mathrm{samp}}$ is sampled from
an uniform distribution on the configuration space. The $x_{\mathrm{samp}}$
then used as an input in function NearbyNode to find a set
$\mathcal{X}_{\mathrm{near}}$ of its nearby nodes. The Nearest is then used to
select its closest neighbor node $x_{\mathrm{nearest}}$ from the tree. Next,
the function LQR-CBF-Steer is used to synthesize controls based on computed
optimal gain and steer the state from $x_{\mathrm{nearest}}$ to
$x_{\mathrm{samp}}$. The resultant trajectory is denoted as $\sigma$ and its
end node in $\sigma$ is $x_{\mathrm{new}}$. At the stage of ChooseParent, the
function takes in the computed $x_{\mathrm{new}}$ and set
$\mathcal{X}_{\mathrm{near}}$ to compute state trajectories and corresponding
costs from $x_{\mathrm{new}}$ to all the nearby nodes defined in
$\mathcal{X}_{\mathrm{near}}$. The trajectory with the minimum cost, denoted
as $\sigma_{\mathrm{min}}$, is used and its end node is defined as
$x_{\textrm{min}}$. After this step, $x_{\mathrm{new}}$ and its edge are added
to the tree. To asymptotically optimize the existing tree, the Rewire function
is used. During this procedure, the LQR-CBF-Steer is used to check whether the
cost for each nearby node $x_{\mathrm{nn}}\in\mathcal{X}_{\mathrm{near}}$ can
be optimized. The $x_{\mathrm{nn}}$ that returns the minimum costs is rewired
to $x_{\mathrm{new}}$, and the corresponding cost is updated.

###  IV-A LQR-CBF-RRT* Algorithm

The detailed algorithm is introduced as the following. First, we define a tree
$\mathcal{T}=(\mathcal{V},\mathcal{E})$, where $\mathcal{V}$ is a set of nodes
and $\mathcal{E}$ is a set of edges.

  * •

Nearby Node The function utilizes a pre-defined euclidean distance $d$ to find
a set of nearby nodes $\mathcal{X}_{\mathrm{near}}$ in $\mathcal{T}$ that is
closest to $x_{\mathrm{sample}}$.

  * •

Nearest The function returns the nearest node from
$\mathcal{X}_{\mathrm{near}}$ w.r.t. $x_{\mathrm{sample}}$.

  * •

ChooseParent The procedure (Algorithm 1 Line 10-14) tries to find collision-
free paths between $x_{\mathrm{new}}$ w.r.t. all its neighboring nodes. If
there exists a collision-free path between $x_{\mathrm{new}}$ and
$x_{\mathrm{nn}}\in\mathcal{X}_{\mathrm{near}}$, the corresponding cost is
calculated. The ChooseParent procedure then selects the $x_{\mathrm{nn}}$ with
the lowest cost (2) as the parent of $x_{\mathrm{new}}$.

  * •

Rewire The rewiring procedure (Algorithm 1 Line 16-20) evaluates and optimizes
the LQR cost (2). The rewiring function checks a selected node’s neighborhood
and calculates the costs w.r.t. all the neighboring nodes. Given that a
collision-free path exists between the current node and the nearby node, by
taking the path from the current node to the nearby node, the nearby node’s
current path is removed if the new cost is lower than the existing cost. The
new path from the current node to the nearby node is added to the tree
$\mathcal{T}$.

######  Remark 2.

Instead of using Euclidean distance between two nodes as a cost metric, we use
LQR cost (2) as the metric in both ChooseParent and Rewire procedures.
Therefore, the control efforts can be captured as part of the optimization
process, and we can adjust the weight matrices to tune the performance of our
motion planner.

  * •

LQRSolver The method (Algorithm 1 Line 4) computes optimal gain matrix
$K_{\mathrm{LQR}}$ using (5) from Riccati equation (4). For a nonlinear
system, the system can be linearized locally [29] and then computes
$K_{\mathrm{LQR}}$ based on linearized system dynamics.

  * •

LQR-CBF-Steer The steering function (Algorithm 2) contains two components: (i)
The LQR controller (ii) CBF safety constraints. Given two states
$(x_{\mathrm{current}},x_{\mathrm{next}})$, the LQR controller generates a
sequence of optimal controls $u^{*}_{i}$, that steers state trajectory based
on (3). At each time steps, the CBF constraints (11) are checked to ensure
generated path is collision free. If none of the CBF constraints are active,
then the LQR-CBF-steer is used for steering $x_{\mathrm{current}}$ to
$x_{\mathrm{next}}$, and node $x_{\mathrm{next}}$ is added to the tree.
Otherwise, the state trajectory will stop at the first encounter when (11) is
violated and the end state $x_{\mathrm{new}}$ and its trajectory $\sigma$ are
added to the tree.

######  Remark 3.

Thanks to the forward invariance property from CBFs [15], no explicit
collision checking is required. The generated state trajectory and controls
from LQR-CBF-Steer are guaranteed to be safe.

1Initialization: $\mathcal{T}=(\mathcal{V},\mathcal{E})$,
$\mathcal{V}\leftarrow$ $\\{x_{\mathrm{init}}\\}$;
$\mathcal{E}\leftarrow\emptyset$; $i=0$; $\,\texttt{adapFlag}=\texttt{True}$,
$\,\texttt{optDensityFlag}\,=\,\texttt{False}$, and $r=\eta$

2 while  _$i <N$_ do

3
$x_{samp}\leftarrow\texttt{Sample}(\mathcal{G},\mathcal{V},\texttt{adapFlag})$

4 $x_{nearest}\leftarrow\texttt{Nearest}(\mathcal{V},x_{samp})$

5 $x_{\mathrm{new}},\sigma\leftarrow\texttt{LQR-CBF-
Steer}(x_{\mathrm{nearest}},x_{\mathrm{samp}})$

6 $\mathcal{X}_{near}\leftarrow\texttt{NearbyNode}(\mathcal{V},x_{nearest})$

7 $r=\min\\{\lambda(\log(|\mathcal{V}|)/|\mathcal{V}|)^{1/(d+1)},\eta\\}$

8 $\mathcal{X}_{near}\leftarrow\texttt{Near}(\mathcal{V},r,x_{new})$

9 minCost $\leftarrow$ $\infty$;
$x_{\mathrm{min}},\sigma_{\mathrm{min}}\leftarrow\texttt{None},\texttt{None}$

10 foreach  _$x_{\mathrm{near}}\in\mathcal{X}_{\mathrm{near}}$_ do

11 $\sigma$ $\leftarrow$ LQR-CBF-Steer ($x_{\mathrm{new}}$, $x_{\mathrm{nn}}$)

12 if  _$x_{\mathrm{nn}}.\texttt{cost}$ +Cost($\sigma$) $<$ minCost_ then

13 minCost $\leftarrow$ $x_{\mathrm{nn}}.\texttt{cost}$+Cost($\sigma$)

14 $x_{\mathrm{min}}$ $\leftarrow$ $x_{\mathrm{nn}}$;
$\sigma_{\mathrm{min}}\leftarrow\sigma$

15

16 $\mathcal{V}\leftarrow\mathcal{V}\cup\\{x_{new}\\}$;
$\mathcal{E}\leftarrow\mathcal{E}\cup\\{x_{\mathrm{min}},x_{\mathrm{new}}\\}$

17 foreach  _$x_{\mathrm{nn}}\in\mathcal{X}_{near}$_ do

18 $\sigma$ $\leftarrow$ LQR-CBF-Steer($x_{\mathrm{new}}$, $x_{\mathrm{nn}}$)

19 if  _$x_{\mathrm{new}}.\texttt{cost}$ +Cost($\sigma$) $<$
$x_{\mathrm{nn}}.\texttt{cost}$_ then

20 $x_{\mathrm{nn}}.\texttt{parent}\leftarrow x_{\mathrm{new}}$

21 $x_{\mathrm{new}}.\texttt{cost}=$Cost($x_{\mathrm{new}}$)

22

23
$\mathcal{T},\;\mathcal{G}\leftarrow\texttt{extToGoal}(\mathcal{V},x_{\mathrm{new}},\texttt{adapFlag})$;
$i\leftarrow i+1$

return $\mathcal{T}$

Algorithm 1 LQR-CBF-RRT∗

1$\mathbf{x}\leftarrow\texttt{None},\mathbf{u}\leftarrow\texttt{None}$

2 $\mathbf{x}.\texttt{add}(x_{\mathrm{current}})$

3
$\mathrm{K_{LQR}}\leftarrow\texttt{LQRsolver}(x_{\mathrm{current}},x_{\mathrm{next}})$

4 foreach  _$t <\mathrm{T}$_ do

5
$x^{{}^{\prime}},u\leftarrow\texttt{Integrator}(x_{\mathrm{current}},\mathrm{K_{LQR}})$

6 if
_$\textrm{Satisfied}\leftarrow\texttt{CBFconstraints}(x^{{}^{\prime}},u)$_
then

7 $\mathbf{x}.\texttt{add}(x^{{}^{\prime}})$; $\mathbf{u}.\texttt{add}(u)$

8 $x_{\mathrm{current}}\leftarrow x^{{}^{\prime}}$

9 else

10 return $\sigma=(\mathbf{x},\mathbf{u})$

11

Algorithm 2 LQR-CBF-Steer($x_{\mathrm{current}},x_{\mathrm{next}}$)

1$\texttt{u}\sim\texttt{Uniform}(0,1)$

2 if  _$\texttt{u}\;\leq 0.5\;\wedge\;\mathcal{G}\neq\emptyset$_ then

3 if  _optDensityFlag_ then

4 $\mathcal{X}\sim\hat{\mathfrak{g}}^{\ast}(x)$

5 return $(x)$

6 else

7 if  _$\texttt{mod}(|\mathcal{G}|,n_{v})=0$_ then

8 $\mathfrak{E}\leftarrow\texttt{Quantile}(\mathcal{G},\varrho)$
$\triangleright$ Assign the elite set

9 $\hat{\mathfrak{g}}(x)\leftarrow\texttt{CE\\_Estimation}(\mathfrak{E},m)$
$\triangleright$ Compute PDF of $\mathfrak{E}$

10 return $x\sim\hat{\mathfrak{g}}(x)$

11 else

12 return $x\sim\texttt{Uniform}(\mathcal{X})$

13

14

15else

16 return $x\sim\texttt{Uniform}(\textbf{We should define a sampling space})$

Algorithm 3
$x_{\mathrm{samp}}\leftarrow\texttt{Sample}(\mathcal{G},\mathcal{T},m)$

##  V Efficiency Improvements

In this section, we develop three strategies to improve the efficiency of the
sampling-based process. Especially when integrating RRT* with both linear and
nonlinear systems, section V-A shows how the designed LQR steering function
has better performance than using MPC and iterative LQR (iLQR). Moreover,
since repetitively solving CBF-QP is computationally expensive, section V-B
designs a QP-free way to check CBF constraints. Lastly, we propose an adaptive
sampling method to enhance efficiency and optimality. The improved
performances are illusrated in VII.

###  V-A Efficient LQR computation

For each steering process, applying the MPC [36] and iLQR [37] requires
iteratively solving optimization problems or dynamic programs many times to
obtain the optimal control. These are tedious approaches since sampling-based
methods need numerous times of steering processes. Furthermore, formulating a
convex optimization for MPC is also challenging.

That’s the fundamental motivation for applying LQR in our framework, to
provide an efficient steering process. First, for a linear dynamic system,
$\dot{x}=Ax+Bu$. The LQR feedback gain $K_{\mathrm{LQR}}$ can be computed
through Riccati equation in (4) i.e., $K_{\mathrm{LQR}}=LQR(A,B,Q,R)$, which
only depends on pre-modeled matrices $A,B,Q,R$. We only need to compute it
once and use it for every steering process.

For nonlinear systems, we can linearize it around the goal as the equilibrium
point. To reduce the computational burden, we only solve for the gain once for
each steering process. Even though it’s still more efficient than MPC and
iLQR, it becomes time-consuming since the number of linearization and
computation of the optimal control gain on the approximated dynamical system
grow significantly during Rewire procedure. To mitigate this issue, we store
the LQR feedback gain for each local goal of the steering process using a hash
table to reduce computation during the rewiring procedure (lines 16 - 18 in
Algorithm 1)

###  V-B LQR–CBF-RRT* without solving QP

Many existing works [15, 19] formulate the QP controllers and iteratively
solve for the controls point-wise in time during ChooseParent and Rewire
procedure. However, there are two problems with this approach, First, it
requires extensively solving the QPs, which can be problematic when the step
size is small. Second, a lot of the hyper-parameters have to be determined to
ensure the feasibility of the QP [32]. By not choosing them correctly, the QP
could become infeasible even though there might exist a control that can steer
the system to the desired goal safely.

In this work, instead of solving the CBF-CLF-QP or CBF-QP with minimum
perturbations of a given reference controllers, we only check whether the CBF
constraints (8) are satisfied and use them as the termination condition of
steering processes. The steering process terminates when we reach a local goal
or the first encounter of a violation of the CBF constraints. For example,
given a reference trajectory $x_{0}u_{0}x_{1}\ldots u_{n-1}x_{n}$ from LQR
optimal control, to reach the sampled goal from current state
$x_{current}=x_{0}$. We check if moving from $x_{i}$ to $x_{i+1}$ via control
$u_{i}$ for all $i\in\\{0,1,\ldots,n-1\\}$ satisfies the CBF condition. If the
CBF condition is violated at any step $i$, we treat $x_{i}$ as the goal of
this steering process and cut off the rest of the trajectories.

Note that different from [18], we do not ignore the whole reference path if it
violates the CBF condition. Instead, we keep the prefix trajectory where the
CBF constraint is always satisfied. This way will further improve the sample
efficiency.

###  V-C Adaptive Sampling

We leverage LQR-CBF-RRT∗ with an adaptive sampling procedure [19] to focus
sampling promising regions of $\mathcal{X_{\mathrm{safe}}}$ in order to
approximate the solution of Problem 1 with a fewer number of samples. We
define $\mathcal{G}$ as set of control and state trajectories pairs, i.e.,
$\mathcal{G}:=\\{(\mathbf{x(t)},\mathbf{u(t)})|x(t)\in\mathcal{X}_{\mathrm{safe}},\forall
t\in[0,T]\\}$.

The CEM [38] is used for IS, which is a multi-stage stochastic optimization
algorithm that iterates upon two steps: first, it generates samples from a
current Sampling Density Function (SDF) and computes the cost of each sample;
second, it chooses an _elite subset_ , $\mathfrak{E}$, of the generated
samples for which their cost is below some threshold; finally, the elite
subset is used to estimate a probability density function (PDF) as if they
were drawn as i.i.d samples and we define $m$ to be the number of elite
trajectories. The estimated PDF will be used as the SDF for the next
iteration. The algorithm terminates when it converges to a limiting PDF. It
has been proven in [39] that CEM with parametric SDF converges to a limiting
distribution.

The CEM was first used in [40] for RRT∗ importance sampling with Gaussian
mixture models (GMM). We use the CEM IS procedure that we implemented in [19]
in this paper, since it utilizes weighted Gaussian kernel density estimate
(WGKDE) for estimating the PDF of the elite samples. The number of mixtures in
GMM has to be picked based on the workspace and is difficult to tune. Using
WGKDE, however, mitigates this challenge.

Algorithm 3 implements the importance sampling technique. For more technical
details refere to [19].

##  VI Probabilistic Completeness

In this section, we provide the probabilistic proof for our algorithm.
Following [20] we assume that Problem 1 is robustly feasible with minimum
clearance $\varepsilon>0$. Hence, $\exists\mathbf{u}\in\mathcal{U}$ such that
it produces a system (1) trajectory $\phi$, where
$\phi:[0,T]\mapsto\mathcal{X}_{\mathrm{safe}}$, $\phi(0)=x_{\mathrm{init}}$,
$\phi(T)=x_{\mathrm{goal}}$, and $\forall t\in[0,T];$
$\Psi_{r_{b}}(\phi(t))\geq
0,\forall\phi\in\mathfrak{C}_{0}\cap\dots\cap\mathfrak{C}_{r_{b}}$.

######  Remark 4.

The assumption on the sets of initial state $\mathcal{X}_{\mathrm{init}}$ and
goal state $\mathcal{X}_{\mathrm{goal}}$ are required because the CBF
constraints (11) are checked at every time steps during edge extension. If the
assumption does not hold, there’s a probability such that certain states
$x_{\mathrm{init}},x_{\mathrm{goal}}$ will be rejected, and the state
trajectory will never reach the goal. We can ensure the assumption holds by
choosing appropriate HOCBF hyper-parameters.

######  Lemma 1.

[41] Given two trajectories $\phi$ and $\phi^{\prime}$, as well as a period
$T\geq 0$, such that $\phi(0)=\phi^{\prime}(0)=x_{\mathrm{init}}$, the
trajectories can be bounded by control

$\lVert\phi^{\prime}(T)-\phi(T)\rVert<K_{u}\cdot T\cdot e^{K_{x}\cdot
T}\cdot\sup(\lVert u(t)-u^{\prime}(t)\rVert),$ (12)

for $K_{u},K_{x}\in\mathbb{R}>0$.

Lemma 1 guarantees that for any two trajectories with the same initial state,
the distance between their end states at time $T$ is bounded by the largest
difference in their controls.

######  Remark 5.

Consider steering from any $x_{s}\in\mathcal{X}_{safe}$ towards any reachable
$x_{f}\in\mathcal{X}_{safe}$ using Algorithm 2. By carefully selecting
appropriate constants of the class $\mathcal{K}$ functions,
$\alpha_{1},\,...,\,\alpha_{\rho}$, of the HOCBF (see Defintion 11), we can
produce $\textbf{u}=u(t_{0}),t(t_{1}),\dots,u(T_{OL})$, such that the bound
(12) be given as $||x_{f}-x_{f}^{\prime}||=\mu<\frac{\varepsilon}{4}$, where
$x_{f}$ and $x^{\prime}_{f}$ are the states at time $T_{OL}$ of the produced
trajectories under the control inputs $\textbf{u}_{OL}$ and the CBF-QP control
inputs $\textbf{u}_{LP}$, respectively, and $\mu\in\mathbb{R}_{>0}$.

We introduce our theorem as the following:

######  Theorem 1.

The LQR-CBF-RRT* (Algorithm 1) is probabilistically complete.

###### Proof.

The completeness of RRT* is implied by the completeness of RRT [20].
Therefore, we need to prove the completeness of LQR-CBF-RRT, where, using the
same sequence of samples, the tree of LQR-CBF-RRT∗ is nothing but a rewired
tree of LQR-CBF-RRT. One can implement LQR-CBF-RRT∗ by mitigating Lines 1-1
and Lines 1-1 in Algorithm 1.

Given that the local motion planner LQR-CBF-Steer in Algorithm 2 that
generates extending control and state trajectories of the tree $\mathcal{T}$
that are guaranteed to be safe. By leveraging Theorem 2 in [42], we only need
to prove that the incremental state trajectory will propagate to a sequence of
intermediate states until reaching $\mathcal{X}_{goal}$.

Assuming the trajectory $\phi$ of the solution of Problem 1 with $\varepsilon$
clearance and has a length $L$. Considering $m+1$ equidistant states
$x_{i}\in\phi,\;i=1,\dots,m+1$, where
$m=\left\lfloor\frac{4L}{\varepsilon}\right\rfloor$, we define a sequence of
balls with radius $\varepsilon/4$ that are centered at these states. For state
$x_{i}$, such ball is given by:
$\mathfrak{B}_{\frac{\varepsilon}{4}}(x_{i}):=\\{x_{b}\;|\;||x_{i}-x_{b}||\leq\frac{\varepsilon}{4}\\}$.
For the consecutive states $x_{i},x_{i+1}\in\varphi_{x}$, we want to prove
that starting from $x_{s}\in\mathfrak{B}_{\frac{\varepsilon}{2}}(x_{i})$ the
steering function LQR-CBF-Steer is able to generate a control and state
trajectories that its end state $x_{f}^{\prime}$ fall in
$\mathfrak{B}_{\frac{\varepsilon}{4}}(x_{i+1})$. Given Remark 5, we assign
$\eta=\frac{\varepsilon}{4}+\mu+2\iota$ and
$0<\iota<\frac{\varepsilon}{4}-\mu$. Accordingly, we assign
$\mathfrak{B}_{\eta}(x_{s})$ and
$\mathfrak{B}_{\frac{\varepsilon}{4}-\mu-\iota}(x_{i+1})$ at $x_{s}$ and
$x_{i+1}$, respectively. Let
$\mathcal{S}:=\mathfrak{B}_{\eta}(x_{s})\cap\mathfrak{B}_{\frac{\varepsilon}{4}-\mu-\iota}(x_{i+1})$
denotes the successful potential end-states set. For any
$x_{f}\in\mathcal{S}$, LQR-CBF-Steer generates trajectories that fall in
$\mathfrak{B}_{\mu}(x_{f})\subset\mathfrak{B}_{\frac{\varepsilon}{4}}(x_{i+1})$.
We denote $|.|$ as the Lebesgue measure, then, for $x_{s}$, the probability of
generating states in $\mathcal{S}$ is $p=\frac{|\mathcal{S}|}{|\mathcal{X}|}$
and is strictly positive. According to [42], the probability $p$ can be
represented as success probability of the $k$ Bernoulli trials process that
models generating $m$ successful outcomes of sampling states that
incrementally reach $\mathcal{X}_{goal}$. The rest of the proof follows
directly the proof of Theorem 1 in [42]. ∎

(a) Iteration 500

(b) Iteration 2500 (c) SDF and Elite Samples (Iter.500) (d) SDF and Elite
Samples (Iter. 2500)

Figure 1: We performed LQR-CBF-RRT* on a double integrator linear system over
2500 iterations (1a to 1d) with adaptive sampling. The plots 1a and 1b
demonstrate how our sampling-based monition planner explores the workspace.
The initial state $x_{\mathrm{init}}=[2,2]$ from the bottom left corner and
the goal state $x_{\mathrm{goal}}=[30,24]$, with circular obstacles spread out
across the workspace. The 1c and 1d illustrate how SDF distribution is
generated as the number of elite samples increases.

(a) Iteration 500

(b) Iteration 3000 (c) SDF and Elite Samples (Iter. 500) (d) SDF and Elite
Samples (Iter. 3000)

Figure 2:  We performed LQR-CBF-RRT* on an unicycle model over 3000 iterations
( 2a to 2d) with adaptive sampling. The same environment configuration applies
to the nonlinear system, where 2a and 2b show the generated state
trajectories. The 2c and 2d show the SDF level set and elite samples for the
unicycle model.

##  VII Simulation Results

In this section, we illustrate our framework on two different systems. All
experiments have the same workspace configuration, with initial state
$x_{\mathrm{init}}=[2,2]$ and the goal state $x_{\mathrm{goal}}=[30,24]$. We
consider obstacles to be circular in a 2-dimensional workspace. The
environment contains seven obstacles with positions:
$[7,12],[46,10],[25,10],[15,5],[15,15],[37,7],[37,23]$. The simulations are
performed on a MacBook Pro with an Apple Silicon M1 Pro CPU and 16GB of RAM.
Our software is available 222https://github.com/mingyucai/LQR_CBF_rrtStar.

###  VII-A Baseline Summary

Our framework has several metrics, including avoiding the construction QP,
storing previous LQR feedback gain, and adaptive sampling. To illustrate the
advantages, we summarize several baselines for comparison analysis. For the
nonlinear system, we have (1) Our method. (2) LQR-CBF-RRT* without storing
feedback gain for nonlinear systems, i.e., naive adaptive LQR-CBF-RRT*; (3)
LQR-CBF-RRT* without both adaptive sampling and storing feedback gain. (4) QP
based LQR-CBF-RRT*. For the linear system, we have (1) Our method. (5) LQR-
CBF-RRT* with QP-solver during steering processes, i.e., LQR-CBF-RRT*-QP; The
reason that we uses a separate system to do the comparison is because the QP
controller with the non-linear system is too conservative, i.e., most of the
QPs are infeasible to solve and we cannot finish 2000 interactions within a
reasonable time interval.

Nonlinear System We conduct a performance comparison on the unicycle model
(14) to demonstrate the efficiency of our proposed framework. We perform five
experiments with 2000 iterations for each baseline with different Numpy random
seeds. In table I, we list the time it takes to complete the motion planning
for different baselines based on the techniques we use to improve efficiency.

Figure 3: The figure demonstrates how our proposed framework improves the
efficiency of the LQR-CBF-RRT*. The vertical axis indicates the time for the
algorithm to finish at 2000 iteration. The result shows our method outperforms
the other three cases by a significant margin. Figure 4: The figure shows the
comparison with respect to the optimal distance cost of applying our method.

Based on the result, implementing a hash table for optimal gain
$K_{\mathrm{LQR}}$ can significantly improve the performance, as it avoids the
repeated linearization of the original system and recalculation of the gain
matrices during the rewiring procedure. The graphical result can be found in
Figure 3.

TABLE I: Efficiency Comparison (Unit: Seconds)

Baseline | Seed 0 | Seed 20 | Seed 42 | Seed 45 | Seed 100 | Mean | Std
---|---|---|---|---|---|---|---
Ours | 10.04 | 9.06 | 9.56 | 10.27 | 11.14 | 10.01 | 0.70
(2) | 32.07 | 27.8 | 29.41 | 34.86 | 35.66 | 31.96 | 3.03
(3) | 74.34 | 61.6 | 109.92 | 104.64 | 92.24 | 88.55 | 18.20
(4) | 41.32 | T/O | 45.57 | 42.18 | 43.22 | 43.07 | 1.84

Linear System We perform another set of experiments to verify the efficiency
improvement without QP controller on a single-integrator system. It shows that
our proposed method is about 86% faster than using using LQR-CBF-RRT*-QP
(Baseline (5)). The result can be found in Table II.

TABLE II: Efficiency Comparison (Unit: Seconds)

Baseline | Seed 0 | Seed 20 | Seed 42 | Seed 45 | Seed 100 | Mean | Std
---|---|---|---|---|---|---|---
Ours | 11.3 | 11.15 | 9.75 | 10.91 | 12.42 | 11.11 | 0.95
(5) | 80.76 | 79.17 | 78.42 | 81.90 | 80.72 | 80.194 | 1.39

We also compared our method with MPC with time horizon of 5 and discrete time
interval of 0.05. The MPC based method went through 2000 iterations over 41
minutes during the experiment. Finally, we performed an optimal Euclidean
distance comparison Fig. 4. The result shows that LQR-CBF-RRT* does optimize
the euclidean distance compare to CBF-RRT. The implementation detail can be
found in https://github.com/mingyucai/LQR_CBF_rrtStar.

###  VII-B Numerical Example 1: Double Integrator Model

In this example, we performed our sampling-based motion planner on a double
integrator model with the linear dynamics with the state
$[x_{1},x_{2},x_{3},x_{4}]$ : $\ddot{x_{1}}=u_{1}$ and $\ddot{x_{3}}=u_{2}$
where $[x_{1},x_{3}]$ is the position, $[x_{2},x_{4}]$ is the velocity. We
control the system using acceleration $[u_{1},u_{2}]$. For the $i$-th
obstacle, the corresponding $i$-th safety set can be defined based on the
function

$h_{i}(x)=(x_{1}-x_{1,i,o})^{2}+(x_{3}-x_{3,i,o})^{2}-r_{i}^{2},$ (13)

where $[x_{1,i,o},x_{3,i,o}]$ is the centroid of the $i$-th obstacle and
$r_{i}$ is the radius. We define CBF constraint $\zeta_{i}$ for obstacle $i$
as

$\displaystyle\zeta_{i}$
$\displaystyle=2x_{2}^{2}+2x_{4}^{2}+2(x_{1}-x_{1,i,o})u_{1}+2(x_{3}-x_{3,i,o})u_{2}$
$\displaystyle+k_{1}h_{i}+2k_{2}[(x_{1}-x_{1,i,o})x_{2}+(x_{3}-x_{3,i,o})x_{4}]\geq
0.$

We perform constraint checking in LQR-CBF-Steer with $\zeta_{i}$ in both edge
extension and rewiring procedures. The interval between two consecutive
updates is $\delta_{t}=0.05$. The satisfaction of CBF constraints
$\zeta_{i}\geq 0,\forall i$ guarantees the generated controls and state
trajectories are safe. In this example, we select hyper-parameters to be
$k_{1}=6.0$ and $k_{2}=1.5$. We iterate through in total of 2500 steps, and
the final result is shown in Figure 1d.

###  VII-C Numerical Example 2: Unicycle Model

In this example, we validate our framework on a unicycle model that has the
following system dynamics:

$\displaystyle\dot{x_{1}}=v\cos(\theta),$ (14)
$\displaystyle\dot{x_{2}}=v\sin(\theta),$ $\displaystyle\dot{\theta}=\omega,$

the state $x=[x_{1},x_{2},\theta]$ includes the position $[x_{1},x_{2}]$ in
workspace and heading angle $\theta$ with respect to the inertial frame. The
control input $u=[v,\omega]$ consists of the translational and angular
velocity. Given this system dynamics, the control inputs $v$, and $\omega$ has
mixed relative degree ($v$ has a relative degree 1 and $\omega$ has a relative
degree 2), which does not allow us to construct a CBF constraint directly. To
bypass this issue, we fix the translational velocity $v$ and only enforce CBF
constraints on the angular velocity $\omega$. Next, the CBF constraint for the
$i$-th obstacle is constructed as the following

$\displaystyle\zeta_{i}(x)$
$\displaystyle=2x_{1}v^{2}\cos^{2}{\theta}+2x_{2}v^{2}\sin^{2}{\theta}$
$\displaystyle+[2(x_{2}-x_{2,i,o})v\cos{\theta}-2(x_{1}-x_{1,i,o})v\sin{\theta}]\omega$
$\displaystyle+k_{1}h_{i}(x)+k_{2}\pounds_{f}h_{i}(x)\geq 0,$

where

$\displaystyle h_{i}(x)$
$\displaystyle=(x_{1}-x_{1,i,o})^{2}+(x_{2}-x_{2,i,o})^{2}-r_{i}^{2}$
$\displaystyle\pounds_{f}h_{i}(x)$
$\displaystyle=2v(x_{1}-x_{1,i,o})\cos{\theta}+2v(x_{2}-x_{2,i,o})\sin{\theta}.$

The hyper-parameters are set to be $k_{1}=2.0$ and $k_{2}=2.0$. The algorithm
performs 3000 iterations before termination, and the result can be found in
Figure 2. Notice the state trajectory looks different compared to Example
VII-B because the controls and state trajectories must be dynamically feasible
and optimizing the corresponding cost function based on the nonlinear system
(14).

##  VIII Discussion and Conclusion

In this paper, we formulated a sampling-based motion planning problem to
optimize the LQR cost function and ensure safety. We uses LQR optimal
controller to steer the system toward the goal while using CBFs to reject
unsafe states and controls during the steering procedure. To improve our
algorithm’s efficiency, we provide the CEM method to perform multi-stage
stochastic optimization. Moreover, we store the computed LQR gain matrices in
a hash table to avoid re-computation during the rewiring procedure. In
simulations, we showed our framework applies to general control affine systems
with high relative degrees. We notice the adaptive sampling method is
sensitive to its hyper-parameters selection. More study is needed to find a
systematic way to achieve better sampling performance.

## References

  * [1] L. E. Kavraki, P. Svestka, J.-C. Latombe, and M. H. Overmars, “Probabilistic roadmaps for path planning in high-dimensional configuration spaces,” _IEEE transactions on Robotics and Automation_ , vol. 12, no. 4, pp. 566–580, 1996.
  * [2] S. M. LaValle _et al._ , “Rapidly-exploring random trees: A new tool for path planning,” 1998.
  * [3] F. Islam, J. Nasir, U. Malik, Y. Ayaz, and O. Hasan, “Rrt*-smart: Rapid convergence implementation of rrt* towards optimal solution,” in _2012 IEEE international conference on mechatronics and automation_. IEEE, 2012, pp. 1651–1656.
  * [4] D. J. Webb and J. Van Den Berg, “Kinodynamic rrt*: Asymptotically optimal motion planning for robots with linear dynamics,” in _2013 IEEE international conference on robotics and automation_. IEEE, 2013, pp. 5054–5061.
  * [5] Y. Li, W. Wei, Y. Gao, D. Wang, and Z. Fan, “Pq-rrt*: An improved path planning algorithm for mobile robots,” _Expert systems with applications_ , vol. 152, p. 113425, 2020.
  * [6] J. D. Gammell, S. S. Srinivasa, and T. D. Barfoot, “Informed rrt: Optimal sampling-based path planning focused via direct sampling of an admissible ellipsoidal heuristic,” in _2014 IEEE/RSJ International Conference on Intelligent Robots and Systems_. IEEE, 2014, pp. 2997–3004.
  * [7] J. Bialkowski, S. Karaman, M. Otte, and E. Frazzoli, “Efficient collision checking in sampling-based motion planning,” in _Algorithmic Foundations of Robotics X: Proceedings of the Tenth Workshop on the Algorithmic Foundations of Robotics_. Springer, 2013, pp. 365–380.
  * [8] J. Pan and D. Manocha, “Fast probabilistic collision checking for sampling-based motion planning using locality-sensitive hashing,” _The International Journal of Robotics Research_ , vol. 35, no. 12, pp. 1477–1496, 2016\.
  * [9] M. Cai, E. Aasi, C. Belta, and C.-I. Vasile, “Overcoming exploration: Deep reinforcement learning for continuous control in cluttered environments from temporal logic specifications,” _IEEE Robotics and Automation Letters_ , vol. 8, no. 4, pp. 2158–2165, 2023.
  * [10] X. Xu, P. Tabuada, J. W. Grizzle, and A. D. Ames, “Robustness of control barrier functions for safety critical control,” _IFAC-PapersOnLine_ , vol. 48, no. 27, pp. 54–61, 2015.
  * [11] A. D. Ames, X. Xu, J. W. Grizzle, and P. Tabuada, “Control barrier function based quadratic programs for safety critical systems,” _IEEE Transactions on Automatic Control_ , vol. 62, no. 8, pp. 3861–3876, 2016.
  * [12] A. D. Ames, J. W. Grizzle, and P. Tabuada, “Control barrier function based quadratic programs with application to adaptive cruise control,” in _53rd IEEE Conference on Decision and Control_. IEEE, 2014, pp. 6271–6278.
  * [13] L. Wang, A. D. Ames, and M. Egerstedt, “Safe certificate-based maneuvers for teams of quadrotors using differential flatness,” in _2017 IEEE International Conference on Robotics and Automation (ICRA)_. IEEE, 2017, pp. 3293–3298.
  * [14] R. Cheng, M. J. Khojasteh, A. D. Ames, and J. W. Burdick, “Safe multi-agent interaction through robust control barrier functions with learned uncertainties,” in _2020 59th IEEE Conference on Decision and Control (CDC)_. IEEE, 2020, pp. 777–783.
  * [15] G. Yang, B. Vang, Z. Serlin, C. Belta, and R. Tron, “Sampling-based motion planning via control barrier functions,” in _Proceedings of the 2019 3rd International Conference on Automation, Control and Robots_ , 2019, pp. 22–29.
  * [16] A. Weiss, C. Danielson, K. Berntorp, I. Kolmanovsky, and S. Di Cairano, “Motion planning with invariant set trees,” in _2017 IEEE Conference on Control Technology and Applications (CCTA)_. IEEE, 2017, pp. 1625–1630.
  * [17] A. Shkolnik, M. Walter, and R. Tedrake, “Reachability-guided sampling for planning under differential constraints,” in _2009 IEEE International Conference on Robotics and Automation_. IEEE, 2009, pp. 2859–2865.
  * [18] A. Manjunath and Q. Nguyen, “Safe and robust motion planning for dynamic robotics via control barrier functions,” in _2021 60th IEEE Conference on Decision and Control (CDC)_. IEEE, 2021, pp. 2122–2128.
  * [19] A. Ahmad, C. Belta, and R. Tron, “Adaptive sampling-based motion planning with control barrier functions,” in _2022 IEEE 61st Conference on Decision and Control (CDC)_. IEEE, 2022, pp. 4513–4518.
  * [20] S. Karaman and E. Frazzoli, “Sampling-based algorithms for optimal motion planning,” _The international journal of robotics research_ , vol. 30, no. 7, pp. 846–894, 2011.
  * [21] W. Sun, J. van den Berg, and R. Alterovitz, “Stochastic extended lqr for optimization-based motion planning under uncertainty,” _IEEE Transactions on Automation Science and Engineering_ , vol. 13, no. 2, pp. 437–447, 2016.
  * [22] R. Tedrake, I. R. Manchester, M. Tobenkin, and J. W. Roberts, “Lqr-trees: Feedback motion planning via sums-of-squares verification,” _The International Journal of Robotics Research_ , vol. 29, no. 8, pp. 1038–1052, 2010\.
  * [23] L. M. Argentim, W. C. Rezende, P. E. Santos, and R. A. Aguiar, “Pid, lqr and lqr-pid on a quadcopter platform,” in _2013 International Conference on Informatics, Electronics and Vision (ICIEV)_. IEEE, 2013, pp. 1–6.
  * [24] E. Okyere, A. Bousbaine, G. T. Poyi, A. K. Joseph, and J. M. Andrade, “Lqr controller design for quad-rotor helicopters,” _The Journal of Engineering_ , vol. 2019, no. 17, pp. 4003–4007, 2019.
  * [25] X. Xiao, T. Zhang, K. Choromanski, E. Lee, A. Francis, J. Varley, S. Tu, S. Singh, P. Xu, F. Xia _et al._ , “Learning model predictive controllers with real-time attention for real-world navigation,” _arXiv preprint arXiv:2209.10780_ , 2022.
  * [26] J. Chen, W. Zhan, and M. Tomizuka, “Autonomous driving motion planning with constrained iterative lqr,” _IEEE Transactions on Intelligent Vehicles_ , vol. 4, no. 2, pp. 244–254, 2019.
  * [27] J. Van Den Berg, “Iterated lqr smoothing for locally-optimal feedback control of systems with non-linear dynamics and non-quadratic cost,” in _2014 American control conference_. IEEE, 2014, pp. 1912–1918.
  * [28] W. Li and E. Todorov, “Iterative linear quadratic regulator design for nonlinear biological movement systems.” in _ICINCO (1)_. Citeseer, 2004, pp. 222–229.
  * [29] A. Perez, R. Platt, G. Konidaris, L. Kaelbling, and T. Lozano-Perez, “Lqr-rrt*: Optimal sampling-based motion planning with automatically derived extension heuristics,” in _2012 IEEE International Conference on Robotics and Automation_. IEEE, 2012, pp. 2537–2542.
  * [30] B. Doerr and R. Linares, “Motion planning and control for on-orbit assembly using lqr-rrt* and nonlinear mpc,” _arXiv preprint arXiv:2008.02846_ , 2020\.
  * [31] E. Glassman and R. Tedrake, “A quadratic regulator-based heuristic for rapidly exploring state space,” in _2010 IEEE International Conference on Robotics and Automation_. IEEE, 2010, pp. 5021–5028.
  * [32] J. Zeng, B. Zhang, Z. Li, and K. Sreenath, “Safety-critical control using optimal-decay control barrier function with guaranteed point-wise feasibility,” in _2021 American Control Conference (ACC)_. IEEE, 2021, pp. 3856–3863.
  * [33] H. K. Khalil, “Nonlinear systems third edition,” _Patience Hall_ , vol. 115, 2002.
  * [34] W. Xiao and C. Belta, “Control barrier functions for systems with high relative degree,” in _Proc. of 58th IEEE Conference on Decision and Control_ , Nice, France, 2019, pp. 474–479.
  * [35] K. Solovey, L. Janson, E. Schmerling, E. Frazzoli, and M. Pavone, “Revisiting the asymptotic optimality of rrt,” in _2020 IEEE International Conference on Robotics and Automation (ICRA)_. IEEE, 2020, pp. 2189–2195.
  * [36] E. F. Camacho and C. B. Alba, _Model predictive control_. Springer science & business media, 2013.
  * [37] Y. Tassa, T. Erez, and E. Todorov, “Synthesis and stabilization of complex behaviors through online trajectory optimization,” in _2012 IEEE/RSJ International Conference on Intelligent Robots and Systems_. IEEE, 2012, pp. 4906–4913.
  * [38] R. Rubinstein, “The Cross-Entropy Method for Combinatorial and Continuous Optimization,” _Methodology And Computing In Applied Probability_ , vol. 1, no. 2, pp. 127–190, 1999.
  * [39] J. Hu, M. C. Fu, and S. I. Marcus, “A model reference adaptive search method for global optimization,” _Operations Research_ , vol. 55, no. 3, pp. 549–568, 2007.
  * [40] M. Kobilarov, “Cross-entropy motion planning,” _International Journal of Robotics Research_ , vol. 31, no. 7, pp. 855–871, 2012.
  * [41] Y. Li, Z. Littlefield, and K. E. Bekris, “Asymptotically optimal sampling-based kinodynamic planning,” _The International Journal of Robotics Research_ , vol. 35, no. 5, pp. 528–564, 2016.
  * [42] M. Kleinbort, K. Solovey, Z. Littlefield, K. E. Bekris, and D. Halperin, “Probabilistic completeness of rrt for geometric and kinodynamic planning with forward propagation,” _IEEE Robotics and Automation Letters_ , vol. 4, no. 2, pp. x–xvi, 2019.