Symmetry and Motion Primitives in Model Predictive Control

TL;DR

This paper explores how symmetries in mechanical systems can be leveraged within Model Predictive Control to improve stability and control strategies, demonstrated through a mobile robot example and motion primitives.

Contribution

It establishes the foundation for integrating symmetries into MPC, proving stability in a mobile robot scenario and providing guidelines for stability when symmetry is not aligned with the cost function.

Findings

Asymptotic stability of a set point in MPC with symmetries is proven.

Numerical validation using motion primitives in a parallel parking scenario.

Guidelines for stability guarantees when the optimization criterion conflicts with symmetry actions.

Abstract

Symmetries, e.g. rotational and translational invariances for the class of mechanical systems, allow to characterize solution trajectories of nonlinear dynamical systems. Thus, the restriction to symmetry-induced dynamics, e.g. by using the concept of motion primitives, may be considered as a quantization of the system. Symmetry exploitation is well-established in both motion planning and control. However, the linkage between the respective techniques to optimal control is not yet fully explored. In this manuscript, we want to lay the foundation for the usage of symmetries in Model Predictive Control (MPC). To this end, we investigate a mobile robot example in detail where our contribution is twofold: Firstly, we establish asymptotic stability of a desired set point w.r.t. the MPC closed loop, which is also demonstrated numerically by using motion primitives applied to the parallel parking scenario. Secondly, if the optimization criterion is not consistent with the symmetry action, we provide guidelines to rigorously derive stability guarantees based on symmetry exploitation.

Tables

Table 1. Table 1: Development of the value function and the MPC closed-loop costs for the example presented in Subsection  5.1 .

$i$	$t=i\delta$	${𝐱}_1^{\text{MPC}}(t;{𝐱}^0)$	$u_1^{\star }\left(0\right)$	$V\left(x^{\text{MPC}}(t;{𝐱}^0)\right)$	$\dots +{\int }_0^t{u}^{\text{MPC}}{\left(t\right)}^{2}dt$
0	0.0	-2.00	2.0	4.00	4.000
1	0.1	-1.80	1.8	3.24	3.640
2	0.2	-1.62	1.7	2.626	3.350
3	0.3	-1.45	1.5	2.105	3.118
4	0.4	-1.30	1.3	1.690	2.928
5	0.5	-1.17	1.2	1.371	2.778
6	0.6	-1.05	1.1	1.105	2.656
7	0.7	-0.94	1.0	0.886	2.558
8	0.8	-0.84	0.9	0.708	2.480
9	0.9	-0.75	0.8	0.565	2.418
10	1.0	-0.67	0.7	0.451	2.368
⋮	⋮	⋮	⋮	⋮	⋮
20	2.0	-0.21	0.3	0.045	2.192
⋮	⋮	⋮	⋮	⋮	⋮
35	3.5	-0.00	0.0	0.000	2.182

Table 2. Table 2: Library of trim primitives for numerical tests in Section  5.2 .

No.	${(u_1,u_2)}^{\top }$	Trim primitive
1	${(0,0)}^{\top }$	rest
2	${(1.5,0)}^{\top }$	move straight
3	${(-1/4,-1)}^{\top }$	circle clockwise
4	${(-1/4,1)}^{\top }$	circle anti-clockwise
5	${(0,1)}^{\top }$	turn on the spot

Equations

$$\dot{\mathbf{x}}(t)$$

$$f(\mathbf{x},\mathbf{u})$$

$${\phi }_u(t;{\mathbf{x}}^0)+\Delta x={\phi }_u(t;{\mathbf{x}}^0+\Delta \mathbf{x})\forall (t,u)\in {\mathbb{R}}_{\ge 0}\times {\mathcal{L}}_{\mathrm{loc}}^{\infty }([0,\infty ),{\mathbb{R}}^2)$$

$${\phi }_u(t;\Psi (g,{\mathbf{x}}^0))=\Psi (g,{\phi }_u(t;{\mathbf{x}}^0))\forall (t,g,{\mathbf{x}}^0)\in {\mathbb{R}}_{\ge 0}\times \mathcal{G}\times M$$

$$\mathbf{x}\mapsto EA\left(\begin{array}{c}\mathbf{x}\\ 1\end{array}\right)\text{ with }\{\begin{array}{l}A=\left(\begin{array}{cc}{R}^{}& \Delta \mathbf{x}\\ 0^{\top }& 1\end{array}\right):R\in SO(n),\Delta \mathbf{x}\in {\mathbb{R}}^n\\ E=\left(\begin{array}{cc}I& 0\end{array}\right)\in {\mathbb{R}}^{n\times (n+1)}\end{array}$$

$$\mathcal{G}:=\left\{\leavevmode\hbox to124.71pt{\vbox to47.88pt{\pgfpicture\makeatletter\hbox{\hskip 62.3563pt\lower-22.8411pt\hbox to0.0pt{\pgfsys@beginscope\pgfsys@invoke{ }\definecolor{pgfstrokecolor}{rgb}{0,0,0}\pgfsys@color@rgb@stroke{0}{0}{0}\pgfsys@invoke{ }\pgfsys@color@rgb@fill{0}{0}{0}\pgfsys@invoke{ }\pgfsys@setlinewidth{0.4pt}\pgfsys@invoke{ }\nullfont\hbox to0.0pt{\pgfsys@beginscope\pgfsys@invoke{ } {}{}{}{{}}{{}}{{}}{{}}{ {}}{{}}{{}}\hbox{\hbox{\hbox{\hbox{{\pgfsys@beginscope\pgfsys@invoke{ }{\offinterlineskip{}{}{{{}}{{}}{{}}{{}}{{}}{{}}{{}}{{}}{{}}{{}}{{}}{{}}{{}}{{}}{{}}{{}}}{{{}}}{{}{}{{ {}{}}}{ {}{}} {{}{{}}}{{}{}}{}{{}{}} { }{{{{}}\pgfsys@beginscope\pgfsys@invoke{ }\pgfsys@transformcm{1.0}{0.0}{0.0}{1.0}{-56.66183pt}{-21.54109pt}\pgfsys@invoke{ }\hbox{\vbox{\halign{\pgf@matrix@init@row\pgf@matrix@step@column{\pgf@matrix@startcell#\pgf@matrix@endcell}&#\pgf@matrix@padding&&\pgf@matrix@step@column{\pgf@matrix@startcell#\pgf@matrix@endcell}&#\pgf@matrix@padding\cr\hfil\hskip 20.00766pt\hbox{{\pgfsys@beginscope\pgfsys@invoke{ }{{}}\hbox{\hbox{{\pgfsys@beginscope\pgfsys@invoke{ }{{}{}{{ {}{}}}{ {}{}} {{}{{}}}{{}{}}{}{{}{}} { }{{{{}}\pgfsys@beginscope\pgfsys@invoke{ }\pgfsys@transformcm{1.0}{0.0}{0.0}{1.0}{-19.00766pt}{0.0pt}\pgfsys@invoke{ }\hbox{{\definecolor{pgfstrokecolor}{rgb}{0,0,0}\pgfsys@color@rgb@stroke{0}{0}{0}\pgfsys@invoke{ }\pgfsys@color@rgb@fill{0}{0}{0}\pgfsys@invoke{ }\hbox{${\mathstrut\cos(\Delta x_{3})}$} }}\pgfsys@invoke{\lxSVG@closescope }\pgfsys@endscope}}} \pgfsys@invoke{\lxSVG@closescope }\pgfsys@endscope}}} \pgfsys@invoke{\lxSVG@closescope }\pgfsys@endscope{}}}&\hskip 20.00766pt\hfil&\hfil\hskip 22.22986pt\hbox{{\pgfsys@beginscope\pgfsys@invoke{ }{{}}\hbox{\hbox{{\pgfsys@beginscope\pgfsys@invoke{ }{{}{}{{ {}{}}}{ {}{}} {{}{{}}}{{}{}}{}{{}{}} { }{{{{}}\pgfsys@beginscope\pgfsys@invoke{ }\pgfsys@transformcm{1.0}{0.0}{0.0}{1.0}{-21.22986pt}{0.0pt}\pgfsys@invoke{ }\hbox{{\definecolor{pgfstrokecolor}{rgb}{0,0,0}\pgfsys@color@rgb@stroke{0}{0}{0}\pgfsys@invoke{ }\pgfsys@color@rgb@fill{0}{0}{0}\pgfsys@invoke{ }\hbox{${\mathstrut-\sin(\Delta x_{3})}$} }}\pgfsys@invoke{\lxSVG@closescope }\pgfsys@endscope}}} \pgfsys@invoke{\lxSVG@closescope }\pgfsys@endscope}}} \pgfsys@invoke{\lxSVG@closescope }\pgfsys@endscope{}}}&\hskip 22.22986pt\hfil&\hfil\hskip 5.0pt\hbox{{\pgfsys@beginscope\pgfsys@invoke{ }{{}}\hbox{\hbox{{\pgfsys@beginscope\pgfsys@invoke{ }{{}{}{{ {}{}}}{ {}{}} {{}{{}}}{{}{}}{}{{}{}} { }{{{{}}\pgfsys@beginscope\pgfsys@invoke{ }\pgfsys@transformcm{1.0}{0.0}{0.0}{1.0}{-2.5pt}{0.0pt}\pgfsys@invoke{ }\hbox{{\definecolor{pgfstrokecolor}{rgb}{0,0,0}\pgfsys@color@rgb@stroke{0}{0}{0}\pgfsys@invoke{ }\pgfsys@color@rgb@fill{0}{0}{0}\pgfsys@invoke{ }\hbox{${\mathstrut 0}$} }}\pgfsys@invoke{\lxSVG@closescope }\pgfsys@endscope}}} \pgfsys@invoke{\lxSVG@closescope }\pgfsys@endscope}}} \pgfsys@invoke{\lxSVG@closescope }\pgfsys@endscope{}}}&\hskip 5.0pt\hfil&\hfil\hskip 9.42432pt\hbox{{\pgfsys@beginscope\pgfsys@invoke{ }{{}}\hbox{\hbox{{\pgfsys@beginscope\pgfsys@invoke{ }{{}{}{{ {}{}}}{ {}{}} {{}{{}}}{{}{}}{}{{}{}} { }{{{{}}\pgfsys@beginscope\pgfsys@invoke{ }\pgfsys@transformcm{1.0}{0.0}{0.0}{1.0}{-8.42432pt}{0.0pt}\pgfsys@invoke{ }\hbox{{\definecolor{pgfstrokecolor}{rgb}{0,0,0}\pgfsys@color@rgb@stroke{0}{0}{0}\pgfsys@invoke{ }\pgfsys@color@rgb@fill{0}{0}{0}\pgfsys@invoke{ }\hbox{${\mathstrut\Delta x_{1}}$} }}\pgfsys@invoke{\lxSVG@closescope }\pgfsys@endscope}}} \pgfsys@invoke{\lxSVG@closescope }\pgfsys@endscope}}} \pgfsys@invoke{\lxSVG@closescope }\pgfsys@endscope}}&\hskip 9.42432pt\hfil\cr\vskip 0.0pt\cr\hfil\hskip 19.45212pt\hbox{{\pgfsys@beginscope\pgfsys@invoke{ }{{}}\hbox{\hbox{{\pgfsys@beginscope\pgfsys@invoke{ }{{}{}{{ {}{}}}{ {}{}} {{}{{}}}{{}{}}{}{{}{}} { }{{{{}}\pgfsys@beginscope\pgfsys@invoke{ }\pgfsys@transformcm{1.0}{0.0}{0.0}{1.0}{-18.45212pt}{0.0pt}\pgfsys@invoke{ }\hbox{{\definecolor{pgfstrokecolor}{rgb}{0,0,0}\pgfsys@color@rgb@stroke{0}{0}{0}\pgfsys@invoke{ }\pgfsys@color@rgb@fill{0}{0}{0}\pgfsys@invoke{ }\hbox{${\mathstrut\sin(\Delta x_{3})}$} }}\pgfsys@invoke{\lxSVG@closescope }\pgfsys@endscope}}} \pgfsys@invoke{\lxSVG@closescope }\pgfsys@endscope}}} \pgfsys@invoke{\lxSVG@closescope }\pgfsys@endscope}}&\hskip 19.45212pt\hfil&\hfil\hskip 21.67433pt\hbox{{\pgfsys@beginscope\pgfsys@invoke{ }{{}}\hbox{\hbox{{\pgfsys@beginscope\pgfsys@invoke{ }{{}{}{{ {}{}}}{ {}{}} {{}{{}}}{{}{}}{}{{}{}} { }{{{{}}\pgfsys@beginscope\pgfsys@invoke{ }\pgfsys@transformcm{1.0}{0.0}{0.0}{1.0}{-20.67433pt}{0.0pt}\pgfsys@invoke{ }\hbox{{\definecolor{pgfstrokecolor}{rgb}{0,0,0}\pgfsys@color@rgb@stroke{0}{0}{0}\pgfsys@invoke{ }\pgfsys@color@rgb@fill{0}{0}{0}\pgfsys@invoke{ }\hbox{${\mathstrut\phantom{-}\cos(\Delta x_{3})}$} }}\pgfsys@invoke{\lxSVG@closescope }\pgfsys@endscope}}} \pgfsys@invoke{\lxSVG@closescope }\pgfsys@endscope}}} \pgfsys@invoke{\lxSVG@closescope }\pgfsys@endscope}}&\hskip 21.67433pt\hfil&\hfil\hskip 5.0pt\hbox{{\pgfsys@beginscope\pgfsys@invoke{ }{{}}\hbox{\hbox{{\pgfsys@beginscope\pgfsys@invoke{ }{{}{}{{ {}{}}}{ {}{}} {{}{{}}}{{}{}}{}{{}{}} { }{{{{}}\pgfsys@beginscope\pgfsys@invoke{ }\pgfsys@transformcm{1.0}{0.0}{0.0}{1.0}{-2.5pt}{0.0pt}\pgfsys@invoke{ }\hbox{{\definecolor{pgfstrokecolor}{rgb}{0,0,0}\pgfsys@color@rgb@stroke{0}{0}{0}\pgfsys@invoke{ }\pgfsys@color@rgb@fill{0}{0}{0}\pgfsys@invoke{ }\hbox{${\mathstrut 0}$} }}\pgfsys@invoke{\lxSVG@closescope }\pgfsys@endscope}}} \pgfsys@invoke{\lxSVG@closescope }\pgfsys@endscope}}} \pgfsys@invoke{\lxSVG@closescope }\pgfsys@endscope}}&\hskip 5.0pt\hfil&\hfil\hskip 9.42432pt\hbox{{\pgfsys@beginscope\pgfsys@invoke{ }{{}}\hbox{\hbox{{\pgfsys@beginscope\pgfsys@invoke{ }{{}{}{{ {}{}}}{ {}{}} {{}{{}}}{{}{}}{}{{}{}} { }{{{{}}\pgfsys@beginscope\pgfsys@invoke{ }\pgfsys@transformcm{1.0}{0.0}{0.0}{1.0}{-8.42432pt}{0.0pt}\pgfsys@invoke{ }\hbox{{\definecolor{pgfstrokecolor}{rgb}{0,0,0}\pgfsys@color@rgb@stroke{0}{0}{0}\pgfsys@invoke{ }\pgfsys@color@rgb@fill{0}{0}{0}\pgfsys@invoke{ }\hbox{${\mathstrut\Delta x_{2}}$} }}\pgfsys@invoke{\lxSVG@closescope }\pgfsys@endscope}}} \pgfsys@invoke{\lxSVG@closescope }\pgfsys@endscope}}} \pgfsys@invoke{\lxSVG@closescope }\pgfsys@endscope}}&\hskip 9.42432pt\hfil\cr\vskip 0.0pt\cr\hfil\hskip 5.0pt\hbox{{\pgfsys@beginscope\pgfsys@invoke{ }{{}}\hbox{\hbox{{\pgfsys@beginscope\pgfsys@invoke{ }{{}{}{{ {}{}}}{ {}{}} {{}{{}}}{{}{}}{}{{}{}} { }{{{{}}\pgfsys@beginscope\pgfsys@invoke{ }\pgfsys@transformcm{1.0}{0.0}{0.0}{1.0}{-2.5pt}{0.0pt}\pgfsys@invoke{ }\hbox{{\definecolor{pgfstrokecolor}{rgb}{0,0,0}\pgfsys@color@rgb@stroke{0}{0}{0}\pgfsys@invoke{ }\pgfsys@color@rgb@fill{0}{0}{0}\pgfsys@invoke{ }\hbox{${\mathstrut 0}$} }}\pgfsys@invoke{\lxSVG@closescope }\pgfsys@endscope}}} \pgfsys@invoke{\lxSVG@closescope }\pgfsys@endscope}}} \pgfsys@invoke{\lxSVG@closescope }\pgfsys@endscope}}&\hskip 5.0pt\hfil&\hfil\hskip 5.0pt\hbox{{\pgfsys@beginscope\pgfsys@invoke{ }{{}}\hbox{\hbox{{\pgfsys@beginscope\pgfsys@invoke{ }{{}{}{{ {}{}}}{ {}{}} {{}{{}}}{{}{}}{}{{}{}} { }{{{{}}\pgfsys@beginscope\pgfsys@invoke{ }\pgfsys@transformcm{1.0}{0.0}{0.0}{1.0}{-2.5pt}{0.0pt}\pgfsys@invoke{ }\hbox{{\definecolor{pgfstrokecolor}{rgb}{0,0,0}\pgfsys@color@rgb@stroke{0}{0}{0}\pgfsys@invoke{ }\pgfsys@color@rgb@fill{0}{0}{0}\pgfsys@invoke{ }\hbox{${\mathstrut 0}$} }}\pgfsys@invoke{\lxSVG@closescope }\pgfsys@endscope}}} \pgfsys@invoke{\lxSVG@closescope }\pgfsys@endscope}}} \pgfsys@invoke{\lxSVG@closescope }\pgfsys@endscope}}&\hskip 5.0pt\hfil&\hfil\hskip 5.0pt\hbox{{\pgfsys@beginscope\pgfsys@invoke{ }{{}}\hbox{\hbox{{\pgfsys@beginscope\pgfsys@invoke{ }{{}{}{{ {}{}}}{ {}{}} {{}{{}}}{{}{}}{}{{}{}} { }{{{{}}\pgfsys@beginscope\pgfsys@invoke{ }\pgfsys@transformcm{1.0}{0.0}{0.0}{1.0}{-2.5pt}{0.0pt}\pgfsys@invoke{ }\hbox{{\definecolor{pgfstrokecolor}{rgb}{0,0,0}\pgfsys@color@rgb@stroke{0}{0}{0}\pgfsys@invoke{ }\pgfsys@color@rgb@fill{0}{0}{0}\pgfsys@invoke{ }\hbox{${\mathstrut 1}$} }}\pgfsys@invoke{\lxSVG@closescope }\pgfsys@endscope}}} \pgfsys@invoke{\lxSVG@closescope }\pgfsys@endscope}}} \pgfsys@invoke{\lxSVG@closescope }\pgfsys@endscope}}&\hskip 5.0pt\hfil&\hfil\hskip 9.42432pt\hbox{{\pgfsys@beginscope\pgfsys@invoke{ }{{}}\hbox{\hbox{{\pgfsys@beginscope\pgfsys@invoke{ }{{}{}{{ {}{}}}{ {}{}} {{}{{}}}{{}{}}{}{{}{}} { }{{{{}}\pgfsys@beginscope\pgfsys@invoke{ }\pgfsys@transformcm{1.0}{0.0}{0.0}{1.0}{-8.42432pt}{0.0pt}\pgfsys@invoke{ }\hbox{{\definecolor{pgfstrokecolor}{rgb}{0,0,0}\pgfsys@color@rgb@stroke{0}{0}{0}\pgfsys@invoke{ }\pgfsys@color@rgb@fill{0}{0}{0}\pgfsys@invoke{ }\hbox{${\mathstrut\Delta x_{3}}$} }}\pgfsys@invoke{\lxSVG@closescope }\pgfsys@endscope}}} \pgfsys@invoke{\lxSVG@closescope }\pgfsys@endscope}}} \pgfsys@invoke{\lxSVG@closescope }\pgfsys@endscope}}&\hskip 9.42432pt\hfil\cr\vskip 0.0pt\cr\hfil\hskip 5.0pt\hbox{{\pgfsys@beginscope\pgfsys@invoke{ }{{}}\hbox{\hbox{{\pgfsys@beginscope\pgfsys@invoke{ }{{}{}{{ {}{}}}{ {}{}} {{}{{}}}{{}{}}{}{{}{}} { }{{{{}}\pgfsys@beginscope\pgfsys@invoke{ }\pgfsys@transformcm{1.0}{0.0}{0.0}{1.0}{-2.5pt}{0.0pt}\pgfsys@invoke{ }\hbox{{\definecolor{pgfstrokecolor}{rgb}{0,0,0}\pgfsys@color@rgb@stroke{0}{0}{0}\pgfsys@invoke{ }\pgfsys@color@rgb@fill{0}{0}{0}\pgfsys@invoke{ }\hbox{${\mathstrut 0}$} }}\pgfsys@invoke{\lxSVG@closescope }\pgfsys@endscope}}} \pgfsys@invoke{\lxSVG@closescope }\pgfsys@endscope}}} \pgfsys@invoke{\lxSVG@closescope }\pgfsys@endscope}}&\hskip 5.0pt\hfil&\hfil\hskip 5.0pt\hbox{{\pgfsys@beginscope\pgfsys@invoke{ }{{}}\hbox{\hbox{{\pgfsys@beginscope\pgfsys@invoke{ }{{}{}{{ {}{}}}{ {}{}} {{}{{}}}{{}{}}{}{{}{}} { }{{{{}}\pgfsys@beginscope\pgfsys@invoke{ }\pgfsys@transformcm{1.0}{0.0}{0.0}{1.0}{-2.5pt}{0.0pt}\pgfsys@invoke{ }\hbox{{\definecolor{pgfstrokecolor}{rgb}{0,0,0}\pgfsys@color@rgb@stroke{0}{0}{0}\pgfsys@invoke{ }\pgfsys@color@rgb@fill{0}{0}{0}\pgfsys@invoke{ }\hbox{${\mathstrut 0}$} }}\pgfsys@invoke{\lxSVG@closescope }\pgfsys@endscope}}} \pgfsys@invoke{\lxSVG@closescope }\pgfsys@endscope}}} \pgfsys@invoke{\lxSVG@closescope }\pgfsys@endscope}}&\hskip 5.0pt\hfil&\hfil\hskip 5.0pt\hbox{{\pgfsys@beginscope\pgfsys@invoke{ }{{}}\hbox{\hbox{{\pgfsys@beginscope\pgfsys@invoke{ }{{}{}{{ {}{}}}{ {}{}} {{}{{}}}{{}{}}{}{{}{}} { }{{{{}}\pgfsys@beginscope\pgfsys@invoke{ }\pgfsys@transformcm{1.0}{0.0}{0.0}{1.0}{-2.5pt}{0.0pt}\pgfsys@invoke{ }\hbox{{\definecolor{pgfstrokecolor}{rgb}{0,0,0}\pgfsys@color@rgb@stroke{0}{0}{0}\pgfsys@invoke{ }\pgfsys@color@rgb@fill{0}{0}{0}\pgfsys@invoke{ }\hbox{${\mathstrut 0}$} }}\pgfsys@invoke{\lxSVG@closescope }\pgfsys@endscope}}} \pgfsys@invoke{\lxSVG@closescope }\pgfsys@endscope}}} \pgfsys@invoke{\lxSVG@closescope }\pgfsys@endscope}}&\hskip 5.0pt\hfil&\hfil\hskip 5.0pt\hbox{{\pgfsys@beginscope\pgfsys@invoke{ }{{}}\hbox{\hbox{{\pgfsys@beginscope\pgfsys@invoke{ }{{}{}{{ {}{}}}{ {}{}} {{}{{}}}{{}{}}{}{{}{}} { }{{{{}}\pgfsys@beginscope\pgfsys@invoke{ }\pgfsys@transformcm{1.0}{0.0}{0.0}{1.0}{-2.5pt}{0.0pt}\pgfsys@invoke{ }\hbox{{\definecolor{pgfstrokecolor}{rgb}{0,0,0}\pgfsys@color@rgb@stroke{0}{0}{0}\pgfsys@invoke{ }\pgfsys@color@rgb@fill{0}{0}{0}\pgfsys@invoke{ }\hbox{${\mathstrut 1}$} }}\pgfsys@invoke{\lxSVG@closescope }\pgfsys@endscope}}} \pgfsys@invoke{\lxSVG@closescope }\pgfsys@endscope}}} \pgfsys@invoke{\lxSVG@closescope }\pgfsys@endscope}}&\hskip 5.0pt\hfil\cr}}}\pgfsys@invoke{\lxSVG@closescope }\pgfsys@endscope}}}{{{{}}}{{}}{{}}{{}}{{}}{{}}{{}}{{}}{{}}{{}}{{}}{{}}{{}}{{}}{{}}{{}}{{}}}} \pgfsys@invoke{\lxSVG@closescope }\pgfsys@endscope}}}\hbox{{\pgfsys@beginscope\pgfsys@invoke{ }{{}{}{{ }}{ } {{}{{}}}{{}{}}{}{{}{}} { }{{{{}}\pgfsys@beginscope\pgfsys@invoke{ }\pgfsys@transformcm{1.0}{0.0}{0.0}{1.0}{-62.3563pt}{-20.04109pt}\pgfsys@invoke{ }\hbox{{\definecolor{pgfstrokecolor}{rgb}{0,0,0}\pgfsys@color@rgb@stroke{0}{0}{0}\pgfsys@invoke{ }\pgfsys@color@rgb@fill{0}{0}{0}\pgfsys@invoke{ }\hbox{{{\nullfont{{{ {}{}{}}}{{ {}{}{}}}}}$\left[\vbox{\hrule height=22.54109pt,depth=22.54109pt,width=0.0pt}\right.$}} }}\pgfsys@invoke{\lxSVG@closescope }\pgfsys@endscope}}} \pgfsys@invoke{\lxSVG@closescope }\pgfsys@endscope}}}\hbox{{\pgfsys@beginscope\pgfsys@invoke{ }{{}{}{{}}{} {{}{{}}}{{}{}}{}{{}{}} { }{{{{}}\pgfsys@beginscope\pgfsys@invoke{ }\pgfsys@transformcm{1.0}{0.0}{0.0}{1.0}{56.80072pt}{-20.04109pt}\pgfsys@invoke{ }\hbox{{\definecolor{pgfstrokecolor}{rgb}{0,0,0}\pgfsys@color@rgb@stroke{0}{0}{0}\pgfsys@invoke{ }\pgfsys@color@rgb@fill{0}{0}{0}\pgfsys@invoke{ }\hbox{{{\nullfont{{{ {}{}{}}}{{ {}{}{}}}}}$\left.\vbox{\hrule height=22.54109pt,depth=22.54109pt,width=0.0pt}\right]$}} }}\pgfsys@invoke{\lxSVG@closescope }\pgfsys@endscope}}} \pgfsys@invoke{\lxSVG@closescope }\pgfsys@endscope}}} \par { {}}{}{{}}{} { {}}{}{{}}{}{}{}{}{{}}{}\pgfsys@beginscope\pgfsys@invoke{ }\pgfsys@setdash{0.6pt,1.0pt}{0.0pt}\pgfsys@invoke{ }\pgfsys@setlinewidth{0.6pt}\pgfsys@invoke{ }{}\pgfsys@moveto{-56.66183pt}{20.54109pt}\pgfsys@moveto{-56.66183pt}{20.54109pt}\pgfsys@lineto{-56.66183pt}{-12.29222pt}\pgfsys@lineto{37.8132pt}{-12.29222pt}\pgfsys@lineto{37.8132pt}{20.54109pt}\pgfsys@closepath\pgfsys@moveto{37.8132pt}{-12.29222pt}\pgfsys@stroke\pgfsys@invoke{ } \pgfsys@invoke{\lxSVG@closescope }\pgfsys@endscope { {}}{}{{}}{} { {}}{}{{}}{}{}{}{}{{}}{}\pgfsys@beginscope\pgfsys@invoke{ }\pgfsys@setdash{0.6pt,1.0pt}{0.0pt}\pgfsys@invoke{ }\pgfsys@setlinewidth{0.6pt}\pgfsys@invoke{ }{}\pgfsys@moveto{42.23752pt}{-14.09666pt}\pgfsys@moveto{42.23752pt}{-14.09666pt}\pgfsys@lineto{42.23752pt}{-22.54109pt}\pgfsys@lineto{52.23752pt}{-22.54109pt}\pgfsys@lineto{52.23752pt}{-14.09666pt}\pgfsys@closepath\pgfsys@moveto{52.23752pt}{-22.54109pt}\pgfsys@stroke\pgfsys@invoke{ } \pgfsys@invoke{\lxSVG@closescope }\pgfsys@endscope \par \pgfsys@invoke{\lxSVG@closescope }\pgfsys@endscope{}{}{}\hss}\pgfsys@discardpath\pgfsys@invoke{\lxSVG@closescope }\pgfsys@endscope\hss}}\lxSVG@closescope\endpgfpicture}}:\Delta\mathbf{x}\in\mathbb{R}^{2}\times S^{1}\right\},$$

$${\phi }_u(t;{\mathbf{x}}^0)$$

$$R_{\Delta x_3}{\mathbf{x}}^0+\Delta \mathbf{x}+{\int }_0^t\left(\begin{array}{c}\cos (x_3^0+{\int }_0^s{u}_2(\tau )\mathrm{d}\tau +\Delta x_3)u_1(s)\\ \sin (x_3^0+{\int }_0^s{u}_2(\tau )\mathrm{d}\tau +\Delta x_3)u_1(s)\\ u_2(s)\end{array}\right)\mathrm{d}s$$

$${\phi }_u(t;{\mathbf{x}}^0)=\Psi (g,{\phi }_u(t;{\bar{\mathbf{x}}}^0))\forall t\ge 0.$$

$${\phi }_u(t;{\mathbf{x}}^0)=\Psi (\exp (\xi t),{\mathbf{x}}^0)\text{ and }u(t)\equiv \bar{u}=\text{const. }\forall t\ge 0.$$

$${\phi }_u(t;{\mathbf{x}}^0)={\Psi }_{g_t}({\mathbf{x}}^0):=\Psi (g_t,{\mathbf{x}}^0)$$

$$v_1$$

$$v_2$$

$${\mathbf{b}}_{g_t}:=\left(\begin{array}{c}{u}_2^{-1}\left(v_1\sin (u_{2}t)-v_2(1-\cos (u_{2}t))\right)\\ u_2^{-1}\left(v_1(1-\cos (u_{2}t))+v_2\sin (u_{2}t)\right)\\ u_{2}t\end{array}\right)\text{ and }{\mathbf{b}}_{g_t}:=\left(\begin{array}{c}{v}_{1}t\\ v_{2}t\\ 0\end{array}\right)$$

$$\left(\begin{array}{ccc}0& -u_2& 0\\ u_2& 0& 0\\ 0& 0& 0\end{array}\right).$$

$$\left(\begin{array}{cccc}0& -u_2& 0& v_1\\ u_2& 0& 0& v_2\\ 0& 0& 0& u_2\\ 0& 0& 0& 0\end{array}\right)$$

$$\exp (\xi t)=\left(\begin{array}{cccc}\cos (u_{2}t)& -\sin (u_{2}t)& 0& \frac{1}{u_2}(v_1\sin (u_{2}t)-v_2(1-\cos (u_{2}t)))\\ \sin (u_{2}t)& \cos (u_{2}t)& 0& \frac{1}{u_2}(v_1(1-\cos (u_{2}t)-v_2\sin (u_{2}t)))\\ 0& 0& 1& u_{2}t\\ 0& 0& 0& 1\end{array}\right),$$

$${\mathbf{x}}^0+\left(\begin{array}{c}\frac{u_1}{u_2}(\sin (x_3^0)-\sin (x_3^0+t{u}_2))\\ \frac{u_1}{u_2}(\cos (x_3^0+t{u}_2)-\cos (x_3^0))\\ t{u}_2(s)\end{array}\right)$$

$${\Psi }^{{\mathcal{T}}_{\mathbf{x}}M}:\mathcal{G}\times {\mathcal{T}}_{x}M\to {\mathcal{T}}_{\mathbf{x}}M,{\Psi }^{{\mathcal{T}}_{\mathbf{x}}M}\left(g,\mathbf{v}\right)=\frac{\mathrm{d}{\Psi }_g}{\mathrm{d}\mathbf{x}}(\mathbf{x})\cdot \mathbf{v}.$$

$$f({\Psi }_g(\mathbf{x}),\mathbf{u})$$

$$\stackrel{\text{eqref}NotationCommutativityFlowSymmetry}{=}\frac{\mathrm{d}{\Psi }_g}{\mathrm{d}x}({\phi }_u(t;{\mathbf{x}}^0))\frac{\mathrm{d}{\phi }_u}{\mathrm{d}t}(t;{\mathbf{x}}^0)={\Psi }_g^{{\mathcal{T}}_{x}M}(f(\mathbf{x},\mathbf{u})).$$

$$f({\Psi }_g(\mathbf{x}),\mathbf{u})$$

$$=\left(\begin{array}{ccc}\cos (\Delta x_3)& -\sin (\Delta x_3)& 0\\ \sin (\Delta x_3)& \cos (\Delta x_3)& 0\\ 0& 0& 1\end{array}\right)\left(\begin{array}{c}\cos (x_3)u_1\\ \sin (x_3)u_1\\ u_2\end{array}\right)={\Psi }_g^{{\mathcal{T}}_{x}M}(f(\mathbf{x},\mathbf{u})).$$

$$\boxed{\begin{array}{rlr}\text{Minimize}& {\int }_0^T\ell (\mathbf{x}(t),\mathbf{u}(t))\mathrm{d}t\text{w.r.t. }u\in {\mathcal{L}}^{\infty }([0,T],{\mathbb{R}}^m),x\in \mathcal{A}\mathcal{C}([0,T],M)& \\ \text{subject to}& r(\mathbf{x}(0),\mathbf{x}(T))=0& \text{(boundary condition)}\\ & g(\mathbf{x}(t),\mathbf{u}(t))\le 0,t\in [0,T],& \text{(state \& control constraint)}\\ & \dot{\mathbf{x}}(t)=f(\mathbf{x}(t),\mathbf{u}(t)),t\in [0,T]& \text{(system dynamics)}\end{array}}$$

$$\ell ({\Psi }_g(\mathbf{x}),\mathbf{u})$$

$$g({\Psi }_g(\mathbf{x}),\mathbf{u})$$

$$r({\Psi }_g(\mathbf{x}),{\Psi }_g(\bar{\mathbf{x}}))$$

$${\int }_0^T\ell (\mathbf{x}(t),\mathbf{u}(t))\mathrm{d}t={\int }_0^T\ell ({\Psi }_g(\mathbf{x}(t)),\mathbf{u}(t))\mathrm{d}t.$$

$${\int }_0^T\ell (\mathbf{x}(t),\mathbf{u}(t))\mathrm{d}t=T\cdot \ell (\mathbf{x}(0),\bar{\mathbf{u}}).$$

$${\int }_0^T\ell (\mathbf{x}(t),\mathbf{u}(t))\mathrm{d}t={\int }_0^T\ell ({\Psi }_{\exp (\xi t)}(\mathbf{x}(0)),\bar{\mathbf{u}})\mathrm{d}t=T\cdot \ell (\mathbf{x}(0),\bar{\mathbf{u}}).$$

$$r_i(\mathbf{x}(0),\mathbf{x}(T)):={\mathbf{x}}_i(0)-{\mathbf{x}}_i^0=0\forall i\in \{1,\dots ,n\}.$$

$$\ell (\mathbf{x},\mathbf{u}):=(\mathbf{x}-{\mathbf{x}}^{\star }{)}^{\top }Q(\mathbf{x}-{\mathbf{x}}^{\star })+(\mathbf{u}-{\mathbf{u}}^{\star }{)}^{\top }R(\mathbf{u}-{\mathbf{u}}^{\star })$$

$$0=t_0\le t_1\le t_2\le \dots \le t_S=T$$

Full text

1 Introduction

In Model Predictive Control (MPC), a sequence of Optimal Control Problems (OCPs) on finite horizons is iteratively solved to approximately solve an OCP on an infinite time horizon while continuously taking into account state measurements, see, e.g. [17, 38] for further details. It has to be guaranteed though that the resulting feedback law stabilizes the system at a desired set point despite the challenging task of solving OCPs for nonlinear dynamical systems in real-time, see, e.g. [22, 47, 16, 24] and [6] for techniques to speed up the numerical solution of the OCPs to be solved in each MPC step.

The approach pursued in this paper is based on exploiting structural properties of the underlying dynamical system in MPC. While, in contrast to linear systems, nonlinear systems cannot be (globally) characterized by evaluating the spectrum at a desired set point, it is still possible to identify characteristic properties which describe global system behavior and are useful in motion planning and optimal control [12, 13]. An important system property is the existence of symmetries, namely continuous symmetries represented by Lie groups. These induce invariances, i.e. the system dynamics are invariant w.r.t. the corresponding symmetry actions. Mechanical systems, such as cars or helicopters, for instance, are typically invariant w.r.t. translations or rotations. Consequently, translations or rotations of a trajectory lead to another trajectory of the mechanical system. Further, symmetries induce the existence of basic motions, e.g. going straight at constant speed or turning with constant rotational velocity in mechanical systems. These basic motions will be called trim primitives or trims, for short, see [14]. Trims can be represented very conveniently, even if general solutions of the dynamical systems cannot be computed by hand. We will quantize the nonlinear system dynamics by choosing a finite set of basic motions, which will be called motion primitives, to which the system is restricted in order to approximately solve the original OCP.

Symmetry exploitation is well-established in control of nonlinear dynamical systems (see e.g. [3, 2, 31]), where the notion of symmetry is based on Noether’s theorem, stating that a symmetry of a dynamical system induces a first integral, i.e. a quantity that is preserved along the system trajectory. In [23], Sussmann introduced a definiton of symmetry for optimal control problems which allows to identify first integrals, i.e. quantities which are preserved along the state and adjoint trajectories (the so called biextremals). This is a useful tool to solve equations of motion for dynamical systems or control problems because finding first integrals can be used to reduce the dimension of the problem which is the main motivation in the aforementioned works.

Classical planning methods only perform geometrical path planning, see e.g. [29] for an overview, and do not take the dynamics of the control system into account. However, already Dubin showed that solutions consisting of arcs of circles and straight lines are optimal w.r.t. path length for system dynamics with constrained turning radius, [7]. This has been extended by Reeds and Shepp in [40] to explicit solution formulas for shortest paths of system dynamics that allow going forward and backward. More recently, control methodologies exploiting the multi-body system dynamics [15] or geometric mechanics [21] were proposed.

The exploitation of symmetry-induced motion primitives for planning problems of nonlinear dynamical systems has been first proposed by Frazzoli et al. [12, 14]. Moreover, in Frazzoli’s approach, optimal motion plans are searched for. Following the idea of quantization (see also [13, 28, 10]), the motion primitives are partly generated by solving optimal control problems for intermediate problems. Finding the best motion primitive sequence can then be written as a mixed-integer optimization problem. Thus, heuristic approaches for globally solving the sequencing problem can be applied, such as sampling-based road-map algorithms [28]. While quantizing itself can be seen as reformulating the dynamics as a hybrid system, the approach can also be applied to systems with intrinsic hybrid, i.e. mixed discrete-continuous, behavior [9]. Recently, the idea of motion primitives has also been applied to autonomous driving problems, see [35] for motion planning and [37, 34] for a multiobjective MPC framework. The latter work numerically exploits symmetries to reduce the computational effort for generating a library of motion primitives used within an explicit MPC framework. In contrast to this contribution, trim primitives and stability questions regarding MPC are not considered.

Besides their utility in motion planning problems, we have seen that basic motions may help in the analysis and the design of MPC schemes as well as the numerical solution of the underlying OCPs: in [11] and [20] concatenations of such basic motions allowed the construction of stabilizing terminal regions and costs for several examples with non-stabilizable linearization (with the peculiarity that the desired set point was not contained in the interior of the terminal region). In particular, the mobile robot was used as a prototype application since its nonholonomic nature ”makes the stabilization of this system challenging; see [1]” according to [11, p. 136]. More recently, basic motions were also utilized in the analysis of MPC schemes without terminal costs and constraints, see, e.g. [45]. However, a formal connection between motion primitives and the proposed techniques has not been established yet. The main goal of this work is to lay the foundation for a link between the already quite mature technique of motion primitives and MPC. To this end, we revisit the example of the mobile robot in detail in order to directly illustrate the appropriateness of the proposed methodology. Here, after introducing the notion of symmetries for optimal control problems based on the definitions for dynamical systems, the contribution of this work is twofold: Firstly, for stage cost, which are consistent with the symmetry action, we show recursive feasibility and asymptotic stability of the origin w.r.t. the MPC closed loop. The key idea is to show that the control effort is uniformly distributed, which also explains why the iterative nature of MPC leads to reduced costs in comparison to the finite horizon optimal control problems. Moreover, we prove finite time convergence if motion primitives are used. Secondly, we show that the basic motions used in [44] were trims and we show that – besides not using the wording – the characteristic properties of trims, namely that a suitable quantization of the system dynamics allows to derive sufficiently good bounds on the value function, was of key importance for the deduced results. This may, e.g., also pave the way for a verification of the distributed controllability assumption introduced in [19].

The remainder of the paper is organized as follows. In Section 2, symmetry, motion and trim primitives are introduced for dynamical control systems and illustrated for the example of the mobile robot. In Section 3, the terminology of symmetries and invariances is extended to optimal control problems. Then, in Section 4, the MPC scheme and a class of admissible control functions are introduced which guarantee the restriction to control trajectories along trim primitives. Convergence of the MPC closed loop trajectory to the origin is shown. In Section 5 we illustrate the MPC approach with motion primitives for the mobile robot.

2 Symmetries and Motion Primitives

Let the system dynamics be given by the ordinary differential equation

[TABLE]

with initial condition $\mathbf{x}(0)=\mathbf{x}^{0}$. Here, $\mathbf{x}(t)\in M\subseteq\mathbb{R}^{n}$ and $\mathbf{u}(t)\in\mathbb{R}^{m}$ denote the state and the control at time $t\geq 0$ respectively, where $M$ is an $n$-dimensional manifold. Let $\mathcal{T}M$ denote the tangent bundle of $M$. We assume that the map $f:M\times\mathbb{R}^{m}\rightarrow\mathcal{T}M$ is continuous and locally Lipschitz w.r.t. its first argument in order to guarantee existence and uniqueness of the solution $\varphi_{u}(\cdot;\mathbf{x}^{0})$ on its maximal interval of existence $\mathcal{I}_{\mathbf{x}^{0},u}$ for $u\in\mathcal{L}^{\infty}_{\operatorname{loc}}([0,\infty),\mathbb{R}^{m})$. $\mathcal{L}^{\infty}_{\operatorname{loc}}([0,\infty),\mathbb{R}^{m})$, $m\in\mathbb{N}$, denotes the space of Lebesgue-measurable and locally absolutely integrable functions. If we restrict the domain of the control function $u$ to the compact interval $[0,T]$, $u|_{[0,T]}\in\mathcal{L}^{\infty}([0,T],\mathbb{R}^{m})$ and the solution uniquely exists on $\mathcal{I}_{\mathbf{x}^{0},u}\cap[0,T]$.

Throughout this manuscript, we consider the following example to illustrate the definitions, concepts, and results. The robot is modeled by a kinematic model for a $4$-wheeled vehicle which autonomously moves in the 2-dimensional plane under nonholonomic constraints due to the wheels.

Example 1** (Mobile Robot; $n=3$, $m=2$).**

The system dynamics of the mobile robot are governed by

[TABLE]

where $x_{1}$ and $x_{2}$ denote the position of the robot in the plane while $x_{3}$ represents its orientation and thus, $M=\mathbb{R}^{2}\times S^{1}$. Since $f$ is globally Lipschitz continuous, finite escape times can be excluded such that existence and uniqueness of the solution $\varphi_{u}(t;\mathbf{x}^{0})$, $t\in[0,\infty)$, is guaranteed.

For Example 1, a translational invariance (w.r.t. the position) can be observed, i.e.

[TABLE]

holds for all $\Delta\mathbf{x}=(\Delta x_{1},\Delta x_{2},0)^{\top}$ with $\Delta x_{1}$, $\Delta x_{2}\in\mathbb{R}$. This equation states that the translation commutes with the flow, i.e. we may first translate the initial state $\mathbf{x}^{0}$ by $\Delta\mathbf{x}$ and then compute the flow or first solve the differential equation and then apply the translation $\Delta\mathbf{x}$. We formalize this commutativity in the following definition. To this end, recall that a Lie group is a group $(\mathcal{G},\circ)$, which is also a smooth manifold, for which the group operations $(g,h)\mapsto g\circ h$ and $g\mapsto g^{-1}$ are smooth. If, in addition, a smooth manifold $M$ is given, we call a map $\Psi:\mathcal{G}\times M\rightarrow M$ a left-action of $\mathcal{G}$ on $M$ if and only if the following properties hold:

• •

$\Psi(e,\mathbf{x})=\mathbf{x}$ for all $\mathbf{x}\in M$ where $e$ denotes the neutral element of $(\mathcal{G},\circ)$.

• •

$\Psi(g,\Psi(h,\mathbf{x}))=\Psi(g\circ h,\mathbf{x})$ for all $g,h\in\mathcal{G}$ and $\mathbf{x}\in M$.

For convenience, we define $\Psi_{g}:M\rightarrow M$ with $\Psi_{g}(x):=\Psi(g,x)$ for $g\in G$ and $x\in M$.

Definition 2** (Symmetry Group).**

Let $M$ be a smooth manifold, $(\mathcal{G},\circ)$ a Lie-group, and $\Psi$ a left-action of $\mathcal{G}$ on $M$. Then, we call the triple $(\mathcal{G},M,\Psi)$ a symmetry group of the system $\dot{\mathbf{x}}(t)=f(\mathbf{x}(t),\mathbf{u}(t))$ if the property

[TABLE]

holds for all $u\in\mathcal{L}^{\infty}_{\operatorname{loc}}([0,\infty),\mathbb{R}^{m})$.

Next, we compute the symmetry group of the mobile robot introduced in Example 1 to illustrate Definition 2. Here, a formal inspection reveals that rotational invariance is combined with translational invariance. In general, the symmetry group of mechanical systems is a subgroup of $SE(n):=T(n)\rtimes SO(n)$, where $T(n)$ is the group of translations and $SO(n)$ is the special orthogonal group, which can be represented by the set of matrices $\{R\in\mathbb{R}^{n\times n}:R^{\top}R=I\text{ and }\det(R)=1\}$. Since this is a subgroup of the affine group of $n$ dimensions, there are two ways to represent the elements of $SE(n)$:

• •

Either by a pair $(R,\Delta\mathbf{x})$ with $R\in SO(n)$ and $\Delta\mathbf{x}\in\mathbb{R}^{n}$. Then, the group action can by represented by $\mathbf{x}\mapsto R\mathbf{x}+\Delta\mathbf{x}$.

• •

Or by the single matrix $A\in\mathbb{R}^{(n+1)\times(n+1)}$ via

[TABLE]

where we have first rewritten $\mathbf{x}$ in homogeneous coordinates (see [33]) before multiplying by $A$. The projection on the state space $\mathbb{R}^{n}$ can then by represented by the matrix $E$.

In addition to the translational invariance observed in Equation (3), the planar mobile robot also has rotational symmetry as specified in the following proposition.

Proposition 3**.**

For given $u\in\mathcal{L}^{\infty}_{\operatorname{loc}}([0,\infty),\mathbb{R}^{2})$, we consider the flow $\varphi_{u}$ generated by the system dynamics (2). Then,

[TABLE]

is a symmetry group of the mobile robot with the matrix multiplication as group action, i.e. $\Psi(g,\mathbf{x})=Eg\begin{pmatrix}\mathbf{x}\\ 1\end{pmatrix},g\in\mathcal{G}$, (with $\mathbf{x}\in M\subset\mathbb{R}^{3}$ represented in homogeneous coordinates).

Proof.

As a subgroup of $SE(2)\times S^{1}$, $\mathcal{G}$ is a Lie group. The flow of the mobile robot is given by

[TABLE]

Then, direct calculations, using the angle sum formula for sine and cosine, show the Identity (4) since both terms can be written as

[TABLE]

where the rotation matrix is denoted by $R_{\Delta x_{3}}$. ∎

If Property (4) holds, the flow of the system is said to be equivariant w.r.t. the symmetry action $\Psi$. As a consequence, given a trajectory $\varphi_{u}(\cdot,\mathbf{x}^{0})$, new trajectories $\Psi(g,\varphi_{u}(\cdot,\mathbf{x}^{0}))$, $g\in\mathcal{G}$, can be generated using $\Psi$. This family of trajectories – parametrized in the group element $g$ – forms an equivalence class.

Definition 4** (Motion Primitive).**

Let $(\mathcal{G},M,\Psi)$ be a symmetry group in the sense of Definition 2. Then, two trajectories $\varphi_{u}(\cdot;\mathbf{x}^{0})$ and $\varphi_{u}(\cdot;\bar{\mathbf{x}}^{0})$ are called equivalent, if there exists $g\in\mathcal{G}$ such that

[TABLE]

A motion primitive is the equivalence class of all trajectories equivalent to $\varphi_{u}(\cdot;\mathbf{x}^{0})$ w.r.t. the left action $\Psi$.

Note that $u$ is an arbitrary but fixed control function in Definition 4, i.e. $u$ is identical for all members of the same motion primitive. By slight abuse of notation, we will use the term motion primitive also for a representative of the equivalence class. The symmetry action of the mobile robot is illustrated in Figure 1.

Considering constant control functions $\bar{u}$, we may link trajectories $\varphi_{\bar{u}}(t;\mathbf{x}^{0})$, $t\geq 0$, to (the positive part of) a corresponding one-parameter subgroup $g_{t}:\mathbb{R}\to\mathcal{G}$. Trajectories $\varphi_{\bar{u}}(\cdot;\mathbf{x}^{0})$, which exhibit this link are called trim primitives, trims in short, and can be considered as basic motions. To be more precise, a particular element $\xi$ of the Lie algebra $\mathfrak{g}$ is scaled by time $t$ and then mapped via the exponential function to the Lie group $\mathcal{G}$ to generate the corresponding one-parameter subgroup. We show this connection for the symmetry group of the mobile robot in combination with constant control functions.

Definition 5** (Trim Primitive).**

Let $(\mathcal{G},M,\Psi)$ be a symmetry group in the sense of Definition 2. Then, a trajectory $\varphi_{u}(\cdot;\mathbf{x}^{0})$ is called a trim primitive if there exists a Lie algebra element $\xi\in\mathfrak{g}$ such that

[TABLE]

Proposition 6**.**

Let $(\mathcal{G},M,\Psi)$ be the symmetry group of Proposition 3 and $\mathfrak{g}$ denote the corresponding Lie algebra. Then, for the constant control function $u:[0,\delta]\rightarrow\mathbb{R}^{2}$, $\delta>0$, defined by $u(t)=(u_{1}\ u_{2})^{\top}$ for all $t\in[0,\delta]$ and the initial value $\mathbf{x}^{0}$, we get

[TABLE]

for $g_{t}=\exp(\xi t)$ with $\xi=(v_{1}\ v_{2}\ u_{2})^{\top}\in\mathfrak{g}$ defined by

[TABLE]

In particular, $\Psi_{g_{t}}(\mathbf{x}^{0})=R_{u_{2}t}\mathbf{x}^{0}+\mathbf{b}_{g_{t}}$ with the translation vector

[TABLE]

for $u_{2}\neq 0$ and $u_{2}=0$, respectively.

Proof.

The Lie algebra for a Lie group that consists of rotation matrices is given by skew symmetric matrices [33]. The corresponding Lie algebra is one-dimensional and can be represented as

[TABLE]

The Lie algebra that corresponds to translations in $\mathbb{R}^{n}$ and $S^{1}$ is simply $\mathbb{R}^{n}$ and $\mathbb{R}$, respectively [33]. Together, we obtain that every element $\xi$ of the Lie algebra can be represented by the matrix

[TABLE]

using the triple $(v_{1}\ v_{2}\ u_{2})^{\top}$. Then, since we are using the homogeneous representation, the linear scaling $t\xi$ by a factor $t\in\mathbb{R}$ and the exponential $\exp:\mathfrak{g}\rightarrow\mathcal{G}$ can by directly calculated by first scaling the representation matrix and, then, computing the matrix exponential ($u$ is constant). Doing so yields

[TABLE]

which shows the claimed representation of $\Psi_{g_{t}}(\mathbf{x}^{0})$. Moreover, the left hand side of Equation (7) equals

[TABLE]

by solving the integrals in the representation (6). Then, direct calculations using the angle sum formula for sine and cosine show Equation 7. ∎

Proposition 6 shows that the mobile robot exhibits trims according to Definition 5 for all constant control functions and arbitrary initial values. If the rotational speed $u_{2}(t)$ is constant and not equal to zero, trims result in a circular motion (otherwise it is a straight line), see e.g. the black curve in Figure 1. In general, trims are attractive since particular solutions of a nonlinear system can be computed while no analytic expression for the general solution is available.

Remark 7** (Dynamical Systems & Trims).**

*In dynamical systems without control input, motions generated by symmetry actions are called relative equilibria since these motions have to be constant in coordinates which are non-symmetric [31, 2]. Constructive approaches to find trims can be deduced from symmetry reduction methods. For mechanical systems with cyclic coordinates, this has been worked out in [10].

Note that any solution with constant control generates a trim for Example 1 since it is a kinematic model (and, thus, the velocities are directly controlled). If actuator dynamics are added, it becomes a second order mechanical system. For this system class, trims typically correspond to motions with constant velocities in body-fixed frame.*

Remark 8** (Alternative Characterization of Symmetry Groups).**

Let $f(\cdot,\mathbf{u})$ maps from a smooth manifold $M$ to the tangential bundle $\mathcal{T}M$ of $M$. The symmetry action as a map $\Psi:\mathcal{G}\times M\rightarrow M$ can be lifted to $\mathcal{T}_{\mathbf{x}}M$ for $\mathbf{x}\in M$ via

[TABLE]

Then, the vector field is said to be equivariant w.r.t. the symmetry action $\Psi$ if $f(\Psi_{g}(\mathbf{x}),\mathbf{u})=\Psi_{g}^{\mathcal{T}_{x}M}(f(\mathbf{x},\mathbf{u}))\ \forall\mathbf{x}\in M$. A direct calculation shows the equivalence of this condition to Property (4): Application of Equation (4) and its time derivative yields

[TABLE]

Remark 9** (Alternative proof of Proposition 3).**

Instead of showing the invariance of the flow $\varphi_{u}$ (Property (4)) we can, alternatively, show the equivariance of the vector field $f$ to prove Proposition 3. With $\Psi_{g}(\mathbf{x})=R\mathbf{x}+\Delta\mathbf{x}$ the lifted action $\Psi_{g}^{\mathcal{T}_{x}M}$ is given by $\frac{\mathrm{d}\Psi_{g}}{\mathrm{d}\mathbf{x}}(\mathbf{x})=R$ and thus, it follows with the vector field $f$ given in (2)

[TABLE]

3 Symmetry and Optimal Control

For given optimization horizon $T$, $T\in\mathbb{R}_{>0}$, we consider the Optimal Control Problem (OCP)

[TABLE]

with continuous stage cost $\ell:\mathbb{R}^{n}\times\mathbb{R}^{m}\to\mathbb{R}$ and functions $r:\mathbb{R}^{n}\times\mathbb{R}^{n}\to\mathbb{R}^{2n}$ and $g:\mathbb{R}^{n}\times\mathbb{R}^{m}\to\mathbb{R}^{q}$, $q\in\mathbb{N}_{0}$. Moreover, $\mathcal{AC}([0,T],M)$, is the space of absolutely continuous functions on the manifold $M$.

Recall that we identified symmetries of the system dynamics in Section 2. Now, our aim is to take advantage of these symmetries in optimal control. Therefore, we are interested in functions $\ell$, $r$, and $g$ that share the invariance properties w.r.t. a symmetry of the system dynamics, see Subsection 3.1. Then, in Subsection 3.2, we are concerned with limitations and possible remedies, which might occur if a constraint function or the stage cost do not share the invariance. Finally, in Subsection 3.3, we formulate the OCP for the example of the mobile robot. Here, we define a set of admissible control functions and show existence of an optimal control.

3.1 OCPs Consistent with the Invariance of the System Dynamics

In the following definition, we precisely state what we mean by saying that the constraints and the stage cost share the invariance of the system dynamics.

Definition 10**.**

Consider the system dynamics (1), i.e. $\dot{\mathbf{x}}(t)=f(\mathbf{x}(t),\mathbf{u}(t))$, with symmetry group $(\mathcal{G},M,\Psi)$. We call a function invariant w.r.t. the symmetry, if every state can be replaced by its image under the symmetry action $\Psi_{g}$ without changing its value - independent of the particular choice of $g\in\mathcal{G}$ and $\mathbf{u}\in\mathbb{R}^{m}$, i.e.

[TABLE]

If the stage cost $\ell$ and the constraint functions $r,g$ are invariant, all equivalent trajectories, i.e. each motion primitive, have the same costs and remain feasible, which directly follows from Definition 10. Then, we call (OCP) consistent (with the invariance property of the system dynamics), which is motivated by the following proposition. As a direct consequence, also optimality is preserved if (OCP) is consistent.

Proposition 11**.**

Consider (OCP) and let $(\mathcal{G},M,\Psi)$ be a symmetry group of the system dynamics. Furthermore, let $u\in\mathcal{L}^{\infty}([0,T],\mathbb{R}^{m})$ and $x\in\mathcal{AC}([0,T],M)$ satisfy the constraints. Then, if the stage cost $\ell$ and the constraint functions $r,g$ are invariant, all pairs $(u,\Psi_{g}(x(\cdot)))$, $g\in\mathcal{G}$, also satisfy the constraints and yield the same costs, i.e.

[TABLE]

Next, we show that the cost function is particularly simple to evaluate along trim primitives, which also explains why $\ell(\mathbf{x}(0),\bar{u})$ is sometimes called unit cost of a trim, see [14].

Proposition 12**.**

Consider a continuous stage cost $\ell:\mathbb{R}^{n}\times\mathbb{R}^{m}\to\mathbb{R}$ and let $(\mathcal{G},M,\Psi)$ be a symmetry group of the system dynamics (1). Further, let a constant control function $u\equiv\bar{\mathbf{u}}$ and a corresponding element $\xi\in\mathfrak{g}$ of the generating Lie algebra $\mathfrak{g}$ be given. Then, for given $T>0$, for each state trajectory $x\in\mathcal{AC}([0,T],M)$ such that $\mathbf{x}(t):=\varphi_{u}(t;\mathbf{x}(0))=\Psi_{\exp(\xi t)}(\mathbf{x}(0))$ holds, i.e. for each trim primitive, the invariant stage cost $\ell$ satisfies

[TABLE]

Proof.

We have

[TABLE]

Here, the first equality follows from the trim definition and $u\equiv\bar{\mathbf{u}}$ and the second equality from the invariance of $\ell$. ∎

Possible choices for invariant stage costs are the following. Note that also a weighted sum leads to an invariant cost function.

• •

Minimal control effort: $\ell(\mathbf{x},\mathbf{u})=\|\mathbf{u}\|_{R}^{2}$ with $R$ being some symmetric positive definite matrix.

• •

Minimum path length

• •

Minimum “fuel consumption”, i.e. $\ell(\mathbf{x},\mathbf{u})=\|\mathbf{u}\|$. For some systems, e.g. when $u$ models the fuel, it might be desirable to minimize the $L_{1}$ norm of $u$ instead of the $L_{2}$ norm, or a combination of both.

• •

Minimal time, i.e. $\ell(\mathbf{x},\mathbf{u})=1$. Here, we get $\int_{0}^{T}1\,\mathrm{d}t=T$, i.e. the final time $T$ is free. Then, $T$ is an additional real-valued optimization variable.

3.2 Pitfalls, Inconsistency, and Remedies

In Subsection 3.1, we have seen a variety of invariant stage costs. A typical representative for invariant constraints would be a (geometrical) path bridging a certain distance, which explains why motion primitives are often employed for path planning objectives, see, e.g. [14]. However, initial value problems with a fixed desired terminal state or quadratic stage costs, which are typically used for stabilization task, lead to inconsistent OCPs as shown in the following. Moreover, we explicate a modified/shifted OCP, which allows to recover consistency of the OCP if desired.

Let us start with a (classical) initial condition, i.e.  $\mathbf{x}(0)=\mathbf{x}^{0}$ or, equivalently,

[TABLE]

Note that only the first $n$ components of the function $r$ describing the boundary conditions are used in this example. The remaining $n$ components would be typically employed to enforce meeting the terminal constraint. Plugging in $\Psi_{g}(\mathbf{x}(0))$ (and also $\Psi_{g}(\mathbf{x}(T))$ for completeness) yields the condition $\Psi_{g}(\mathbf{x}_{i}(0))-\mathbf{x}^{0}_{i}=0$, which is for $g\in\mathcal{G}\setminus\{e\}$, in general, not satisfied. However, a shifted OCP with initial condition $\Psi_{g}(\mathbf{x}^{0})$ – a shift with the particular group element $g$, $g\in\mathcal{G}$ – is invariant, meaning that feasibility (and optimality) are preserved for the OCP with shifted constraints. This can be explained using the representation of the symmetry action in homogeneous coordinates: the translation vector cancels out while the distance is preserved under the respective orthogonal transformation. Similar arguments apply to terminal conditions and quadratic stage costs exemplarily defined as

[TABLE]

with symmetric matrices $Q\in\mathbb{R}^{n\times n}$ and $R\in\mathbb{R}^{m\times m}$ (with $Q\succcurlyeq 0$ and $R\succ 0$) and references $\mathbf{x}^{\star}$ and $\mathbf{u}^{\star}$ for state and control respectively.

Example 13** (Shifted OCP).**

The robot of Example 1 shall be controlled into the final state $\mathbf{x}^{\star}=(0,\,0,\,0)^{\top}$. First, consider $\mathbf{\hat{x}}=(-1,\,0,\,0)$ as a starting point. It can be easily checked that constant control $(u_{1},\,u_{2})=(1/T,\,0)^{T}$ drives the system to zero at final time $T$. Now, recall the symmetry group of the robot computed in Proposition 3 and say we choose $g\in\mathcal{G}$ defined by $\Delta\mathbf{x}=(1,\,1,\,0)$. The shifted initial point is then given by $\Psi_{g}(\mathbf{\hat{x}})=(0,\,1,\,0)^{\top}$, see Figure 2.

Now, the control problem is fundamentally different: $(u_{1},\,u_{2})=(1/T,\,0)^{\top}$ does not drive the system to $\mathbf{x}^{\star}$, in fact we are faced with the famous parallel parking problem, see, e.g. [36]. A solution could be obtained by a sequence ”turn-move-turn”, as presented in [44], but, at the moment, our focus is on finding invariances. As proposed above, to this aim we modify the terminal condition, i.e. the shifted final state becomes $\Psi_{g}(\mathbf{x}^{\star})=(1,\,1,\,0)^{\top}$. Then, the previously computed solution $(u_{1},\,u_{2})=(1/T,\,0)^{\top}$ can drive the system from $\Psi_{g}(\mathbf{\hat{x}})$ to $\Psi_{g}(\mathbf{x}^{\star})$. Consequently, solutions of the OCP would remain the same, given that the stage costs are either invariant or analogously modified.

3.3 Optimal Control Problem for the Mobile Robot

In Proposition 6 we identified characteristic motions of the mobile robot, i.e. circles and straight lines, which are formally trim primitives. Since any pair $(u_{1},u_{2})$ of constant control values generates a trim we now switch perspective and define a class of admissible controls which guarantees that the system evolves on a finite sequence of trims. Furthermore, we show that an optimal control function exists for this class of admissible control functions.

Let us suppose that the control has to be piece-wise constant corresponding to the use of at most $S$, $S\in\mathbb{N}$, trims (which do not have to be pairwise different), i.e., for each control function $u$, there exists a finite partition

[TABLE]

such that the control function $u|_{[t_{i-1},t_{i})}$ is constant for all $i\in\{1,\ldots,S\}$. Furthermore, the boundary condition is replaced by an initial and a terminal state constraint, i.e. $\mathbf{x}(0)=\hat{\mathbf{x}}$ and $\mathbf{x}(T)=\mathbf{x}^{\star}$. Moreover, instead of the (mixed) control-state constraint $g(\mathbf{x}(t),\mathbf{u}(t))$, we consider the state constraint $\mathbf{x}(t)\in\mathbb{X}\subset\mathbb{R}^{n}$ and the control constraint $\mathbf{u}(t)\in\mathbb{U}\subset\mathbb{R}^{m}$ for a compact and convex set $\mathbb{X}$ and a compact set $\mathbb{U}$ with $\mathbf{x}^{\star}\in\mathbb{X}$. Then, for a given optimization horizon $T$, $T>0$, and initial value $\hat{\mathbf{x}}$, $\hat{\mathbf{x}}\in\mathbb{R}^{n}$, we define the set of admissible control functions

[TABLE]

where the set $\mathbb{U}$ of feasible control values is given by $[-\bar{\mathbf{u}},\bar{\mathbf{u}}]\subset\mathbb{R}^{2}$ with $\bar{u}_{i}>0$ for all $i\in\{1,2\}$. Moreover, if the time horizon $T$ is considered as an optimization variable, the set of admissible control functions is given as $\bigcup_{T>0}\mathcal{U}_{T}^{S}(\hat{\mathbf{x}})$. Furthermore, for weighting coefficients $c_{1}\geq 0$, $c_{2}\geq 0$, and $c_{3}\geq 0$, let the stage cost $\ell:\mathbb{R}^{n}\times\mathbb{R}^{m}\rightarrow\mathbb{R}_{\geq 0}$ by defined as

[TABLE]

with $\|u\|_{R}^{2}:=u^{\top}Ru$ for a positive definite, symmetric matrix $R$ and ${|\kern-1.07639pt|\kern-1.07639pt|\mathbf{u}|\kern-1.07639pt|\kern-1.07639pt|}$ an arbitrary norm.

We consider the Optimal Control Problem

[TABLE]

for $c_{3}=0$ and $c_{3}>0$ respectively. Since the stage cost is positive definite w.r.t. $\mathbf{u}$, the infimum is bounded from below by zero and labeled $V_{T}(\hat{\mathbf{x}})$.

The following lemmata show basic properties of the OCP (14) for Example 1; namely, existence of a feasible solution and finite cost.

Lemma 14**.**

Consider Example 1 and the OCP (14). Then, if $S\geq 3$ and the (finite) set of control values contains the values $(0,\ 0)^{\top}$, $(\bar{u}_{1},\ 0)^{\top}$, and $(0,\ \bar{u}_{2})^{\top}$, there exists, for each $\hat{\mathbf{x}}\in\mathbb{X}\setminus\{\mathbf{x}^{\star}\}$, a time horizon $T$, $T>0$, and a control function $u\in\mathcal{U}^{S}_{T}(\hat{\mathbf{x}})$ such that $J_{T}(\hat{\mathbf{x}},u)<\infty$ holds.

Proof.

Essentially, we can proceed analogously to [44, 41]. Due to convexity of $\mathbb{X}$ and the fact that the length of the optimization horizon is not fixed, we can simply prolong $T$ such that the sequence

turn, i.e. use $(0,\bar{u}_{2})^{\top}$ until the robot points towards the origin, 2. 2.

move, i.e. use $(\bar{u}_{1},0)^{\top}$ until the robot reaches the origin, and 3. 3.

turn, i.e. use $(0,\bar{u}_{2})^{\top}$ until the desired orientation is attained,

becomes feasible and ensures that $\mathcal{U}^{S}_{T}(\hat{\mathbf{x}})$ is non-empty. Due to compactness of $\mathbb{X}$ and $\mathbb{U}$ and continuity of $\ell$, this ensures finite costs. If $c_{3}=0$, i.e. minimal/short time is not weigthed in the stage cost, the third control value $\mathbf{0}$ is important because it allows to stay at $\mathbf{x}^{\star}$ until the final time $T$ is reached (which is typically not an optimization variable for $c_{3}=0$). ∎

In particular, Lemma 14 ensures existence and finiteness of the infimum of OCP (14). Hence, the value function $V:\mathbb{X}\rightarrow\mathbb{R}_{\geq 0}$ is well-defined. The following lemma shows that the optimum is attained, see, e.g. [30] for a proof.

Lemma 15**.**

Consider Example 1 and the OCP (14) and let $\hat{\mathbf{x}}\in\mathbb{X}\setminus\{\mathbf{x}^{\star}\}$ be given. Moreover, let $S\geq 3$ and the (finite) set of control values contain the values $(0,\ 0)^{\top}$, $(\bar{u}_{1},\ 0)^{\top}$, and $(0,\ \bar{u}_{2})^{\top}$.

For $c_{3}>0$, there exists a time horizon $T^{\star}$, $T^{\star}>0$, and a control function $u^{\star}\in\mathcal{U}^{S}_{T^{\star}}(\hat{\mathbf{x}})$ satisfying $J_{T^{\star}}(\hat{\mathbf{x}},u^{\star})=V(\hat{\mathbf{x}})$.

For $c_{3}=0$ and optimization horizon $T$ such that $\mathcal{U}^{S}_{T}(\hat{\mathbf{x}})\neq\emptyset$, there exists an admissible control function $u^{\star}\in\mathcal{U}^{S}_{T^{\star}}(\hat{\mathbf{x}})$ with $J_{T}(\hat{\mathbf{x}},u^{\star})=V(\hat{\mathbf{x}})$.

The preceding lemmata justify a restriction to control functions with piecewise constant control values and finitely many switches, since existence of admissible control functions can still be guaranteed. Restricting to $\mathcal{U}^{S}_{T}(\hat{\mathbf{x}})$ is a first step in the direction of a finite maneuver automaton, as proposed by Frazzoli. In [14], two types of motion primitives are distinguished: trim primitives, as we defined them in Definition 5, and maneuvers. Maneuvers are arbitrarily controlled trajectories that start and end on trims. Thus, they allow to concatenate trims and maneuvers alternatingly. The resulting trajectories are admissible to the system dynamics, in particular, switching from trim to maneuver or from a preceding maneuver to a trim is continuous. The finite set of motion primitives is called a maneuver automaton. It can be represented as a graph: Trim primitives are the nodes and edges correspond to maneuvers which have been designed to connect the preceding and succeeding trim.

The robot example is special, since any fixed pair of controls $(u_{1},u_{2})^{\top}$ leads to a trim and no maneuvers are necessary to switch from one trim to another. Therefore, we focus on constructing sequences which solely consist of trim primitives.

Remark 16** (Sequence of Trim Primitives).**

Any partition of type (9) together with an arbitrary set of $S$ control values $\mathbf{u}_{i}=(u^{i}_{1},u^{i}_{2})^{\top}$ generates a sequence of trim primitives for the mobile robot, which can be transcribed as a trajectory  $x\in\mathcal{AC}([0,T],M)$ via

[TABLE]

with $u_{i}|_{[t_{i-1},t_{i})}\equiv(u^{i}_{1},u^{i}_{2})^{\top}$ for $i=1,\dots,S$, $\xi_{i}$ being the Lie algebra element defined by $(u^{i}_{1},u^{i}_{2})^{\top}$ and $\mathbf{x}(t_{i-1};\hat{\mathbf{x}};u)$ according to Proposition 6, and $\mathbf{x}(t_{i})$ being a short notation for $\mathbf{x}(t_{i}):=\mathbf{x}(t_{i};\hat{\mathbf{x}},u)$.

4 Model Predictive Control for the Mobile Robot

This section is essentially split into two parts. Firstly, we demonstrate the effectiveness of the techniques proposed in the preceeding two sections by considering Model Predictive Control (MPC) for the mobile robot. To this end, we consider the following MPC scheme where the terminal state $\mathbf{x}^{\star}$ is, w.l.o.g., set to zero.

Algorithm 17**.**

*Let $\mathbf{x}^{0}\in\mathbb{X}\setminus\{\mathbf{0}\}$ and $\delta>0$ be given.

Set $i=0$, $t_{0}=0$, and $\hat{\mathbf{x}}=\mathbf{x}^{0}$.*

Solve OCP (14) to compute a minimizer $T^{\star}$, $u^{\star}$ and implement $u^{\star}|_{[0,\min\{\delta,T^{\star}\})}$. 2. 2.

Set $t_{i+1}:=t_{i}+\min\{\delta,T^{\star}\}$ and $\hat{\mathbf{x}}:=\mathbf{x}(t_{i+1}-t_{i};\hat{\mathbf{x}},u^{\star})$ 3. 3.

If $\hat{\mathbf{x}}=\mathbf{0}$ stop. Otherwise increment $i$ and go to Step (1)

Algorithm 17 yields the MPC closed-loop trajectory

[TABLE]

where we emphasized the dependence of the optimal $T^{\star}$ and $u^{\star}$ on the initial value $\hat{\mathbf{x}}=\mathbf{x}^{\operatorname{MPC}}(t_{i};\mathbf{x}^{0})$, which we omitted in the exposition of Algorithm 17. The concatenated control function is denoted by $u^{\operatorname{MPC}}$. Key properties in MPC are recursive feasibility and convergence of the closed-loop trajectory to the desired set point $\mathbf{x}^{\star}=\mathbf{0}$. The former is important to guarantee that the feasible set of the OCP to be solved in Step (1) of Algorithm 17 is non-empty provided it was non-empty at $t=0$ (initial feasibility), see, e.g. [38]. For Algorithm 17, recursive feasibility is ensured by the terminal constraint, see, e.g. [27].

For the convergence of the closed-loop trajectory, we distinguish whether the coefficient $c_{3}$ weighting the process time is present in the stage cost or not. If it is, the proof is very simple and presented in Subsection 4.3. Here, we even prove finite time convergence of the MPC closed-loop trajectory given by (15) to the origin.

Secondly, in Subsection 4.4 we give further comments on the usefullness of the proposed techniques if the stage cost is inconsistent with the invariance induced by the symmetry action. Here, we also eschew the terminal equality constraint, which is – in general – not desirable from a numerical point of view.

4.1 Minimizing Energy and Fuel Consumption

We consider the Optimal Control Problem

[TABLE]

Before we rigorously show that the origin is asymptotically stable w.r.t. the MPC closed loop (in Theorem 20), we establish that, for each optimal solution of the OCP (16), the control effort is uniformly distributed on the whole time interval $[0,T]$ in the following proposition.

Proposition 18** (Necessary Optimality Condition).**

Let $u^{\sharp}\in\mathcal{U}^{S}_{T}(\hat{\mathbf{x}})$ be an admissible control function for the OCP (16). Then, for given weighting coefficients $c_{1}>0$ and $c_{2}\geq 0$, $u^{\sharp}$ either exhibits uniform control effort, i.e.

[TABLE]

or we can construct an admissible control function $\bar{u}\in\mathcal{U}^{S}_{T}(\hat{\mathbf{x}})$ with

[TABLE]

i.e. strictly smaller objective value.

Proof.

Since $u^{\sharp}$ is piecewise constant there exists a finite partition

[TABLE]

such that $u^{\sharp}$ is constant on each interval $(t_{i-1},t_{i})$, $i\in\{1,2,\ldots,S\}$. Assume, w.l.o.g., that the control function $u^{\sharp}$ exhibits values $u^{(1)}$ on $(t_{0},t_{1})$ and $u^{(2)}$ on $(t_{1},t_{2})$ such that $\|u^{(1)}\|^{2}_{R}\neq\|u^{(2)}\|_{R}^{2}$ holds. Then, we have the costs

[TABLE]

on the time interval $[0,t_{2})$. In the following, we construct a piecewise constant control such that the corresponding trajectory reaches the same point $\mathbf{x}(t_{2};\hat{\mathbf{x}},u^{\star})$ at time $t_{2}$ but produces less costs — a contradiction to optimality of $u^{\sharp}$, which shows the claimed assertion. To this end, we exploit the property

[TABLE]

for arbitrary $\tau>0$ if $u$ is constant on $(0,\tau)$, see [41].

W.l.o.g. let $\|u^{(2)}\|_{R}>\|u^{(1)}\|_{R}$ hold. Then, we replace $u^{(2)}$ by a scaled version; namely $\alpha u^{(2)}$ with $\alpha\in(0,1)$ sufficiently close to one such that all following quantities are well-defined. Moreover, we enlarge the length of the interval $[t_{1},t_{2})$ by the factor $\alpha^{-1}$. This implies, using the Identity (19), that the same path — starting at $\mathbf{x}(t_{1};\hat{\mathbf{x}},u^{\sharp})$ — is traversed in the state space but with a slower speed. Simultaneously, we enlarge $u^{(1)}$ and reduce the length of the respective time interval $[0,t_{1})$ such that the same path is traversed and the overall length of both intervals remains unchanged. The latter implies $\alpha^{-1}(t_{1}-(1-\alpha)t_{2})=t_{1}/\beta$ if the scaling factor used for $u^{(1)}$ is called $\beta$. In particular, we get the equations

[TABLE]

Using the representation of $\beta$ displayed in (20) to rewrite the factor $t_{1}/\beta$ in front of the first summand, we get the costs

[TABLE]

Subtracting the original value (18) and showing that the resulting expression is strictly less than zero completes the proof. Hence, we have to establish the inequality

[TABLE]

Then, replacing $(\beta-1)$ using (20) and dividing by $c_{1}(t_{2}-t_{1})(1-\alpha)$ yields

[TABLE]

The right hand side is independent of the scaling factor $\alpha$ and strictly larger than zero, while the left hand side converges to zero for $\alpha\rightarrow 1$. In conclusion, the inequality is satisfied for sufficiently small $\alpha>0$. ∎

The following corollary extends the assertion of Proposition 18 to a more general set of admissible control functions, which turns out to be helpful for the proof of the following theorem.

Corollary 19**.**

Proposition 18 also holds for $u\in\mathcal{L}^{\infty}([0,T],\mathbb{R}^{2})$ except on a set of measure zero.

Proof.

Note that we have not used that there were at most $S$ switches. Hence, the line of reasoning works for arbitrary piecewise control functions — a class, which is dense in $\mathcal{L}^{\infty}([0,T],\mathbb{R}^{2})$, which allows us to conclude the assertion. ∎

Proposition 18 and its extension formulated in Corollary 19 are key ingredients to prove convergence of the MPC closed-loop trajectory to the origin since they allow us to derive a decrease of the value function.

Theorem 20**.**

Let $c_{1}>0$, $c_{2}\geq 0$, and an initial state $\mathbf{x}^{0}$ be given. Then, we have recursive feasibility for the MPC algorithm 17 and, if $\mathbf{x}^{0}$ is initially feasible, the corresponding MPC closed-loop trajectory converges to the origin, i.e. $\mathbf{x}^{\operatorname{MPC}}(t;x^{0})$ exists for all $t\geq 0$ and $\lim_{t\rightarrow\infty}\mathbf{x}^{\operatorname{MPC}}(t;x^{0})=\mathbf{0}$ holds.

Proof.

Since the optimal control problem contains a terminal equality constraint, recursive feasibility holds provided that initial feasibility is given. To prove the convergence, note that Proposition 18 implies that, for an optimal control function $u^{\star}$, $\|\mathbf{u}^{\star}(t)\|_{R}^{2}$ is constant for almost all $t\in[0,T]$. Hence, adding this as a constraint to the optimal control problem, does not change the set of all minimizers (the minimizer).

If we now modify the optimal control problem by setting $c_{2}=0$ (but leaving $c_{1}$ as it is), each admissible control function remains admissible and is assigned to an objective value, which is upper bounded by its counterpart of the original OCP. In addition, we further relax the constraints by allowing arbitrary $\mathcal{L}^{\infty}$-functions, see Corollary 19. Clearly, the set of minimizers may change. So far, we get the relation

[TABLE]

for the optimal control of the original OCP and the optimal value $\widetilde{V}(\hat{\mathbf{x}})$ of the modified optimal control problem. Then, we can estimate that the control effort is only decreasing for each admissible control function if we replace the cost function by $\lambda_{R}\|u\|^{2}$ where $\lambda_{R}$ denotes the smallest eigenvalue of the positive definite matrix $R$. Then, dropping the artificially introduced constraint on uniform control effort w.r.t. $\|\cdot\|_{R}^{2}$, we get

[TABLE]

where $\widetilde{\widetilde{V}}(\hat{\mathbf{x}})$ denotes the minimal value of the OCP (16) with stage costs $\|u(t)\|^{2}$. Then, further reducing this value by using the simplified dynamics, see Proposition 25 in Appendix A (and still using the notation $\widetilde{\widetilde{V}}$ for the optimal value), we have derived the Lyapunov inequality

[TABLE]

Then, standard arguments, see, e.g. [42, 18] can be used to conclude the assertion. ∎

Next, we extend Proposition 18 to the case with additional control and state constraints as a preliminary step to show that also the assertions of Theorem 20 remain valid.

Corollary 21**.**

Let control constraints $g(\mathbf{u})\leq\mathbf{0}$ with $g:\mathbb{R}^{m}\rightarrow\mathbb{R}^{p}$ be given such that the set $\{\mathbf{u}\in\mathbb{R}^{m}:g(\mathbf{u})\leq\mathbf{0}\}$ is closed, convex, and contains the origin in its interior. Then, if the control function $u^{\sharp}$ exhibits neither uniform control effort, i.e. (17), nor satisfies $\|\mathbf{u}(t)\|_{R}^{2}\geq r^{\star}$ for almost all $t\in[0,T]$ with the threshold value

[TABLE]

then the alternative proposed in Proposition 18 holds, i.e. $u^{\sharp}$ is not optimal.

Proof.

The proof is a direct adaptation of the arguments used in the proof of Proposition 18 since the proposed construction is still doable as long as there exists an interval, on which the boundary of the control constraints is not yet active (for which reaching/exceeding the threshold value $r^{\star}$ is a necessary condition). ∎

Remark 22** (State Constraints).**

Note that adding state constraints $h(\mathbf{x})\leq\mathbf{0}$ with $h:\mathbb{R}^{n}\rightarrow\mathbb{R}^{q}$, $q\in\mathbb{N}$, does not affect the assertions of Proposition 18 and Corollary 21 since we have shown the following in the respective proofs: Each path in the $x_{1}$-$x_{2}$-plane remains feasible but the optimal time parametrization w.r.t. the cost functional is attained only if the proposed necessary optimality condition holds. Therefore, feasibility w.r.t. the state constraint set is maintained (the angle is also invariant on a given path).

Also Theorem 20 remains valid. The only changes needed in the proof are the following: Firstly, one has to argue that a certain minimal decrease is automatically achieved if the condition $\|\mathbf{u}^{\star}(t)\|_{R}^{2}\geq r^{\star}$ with $r^{\star}$ defined by (21) holds for almost all $t\in[0,T]$. Furthermore, dropping the control and state constraints before Proposition 25 (see Appendix) is applied, leads to the same lower bound and is, thus, doable. Furthermore, note that all results presented in this section also hold if control functions of class $\mathcal{L}^{\infty}$ are used instead of $\mathcal{U}^{S}_{T}(\cdot)$.

Example 23**.**

We consider again the mobile robot example with states $x_{1},x_{2},x_{3}$ and define a control problem from initial state $\hat{\mathbf{x}}=(0.1,\,1.0,\,0.8)$ to final state $\mathbf{x}^{\star}=(0,\,0,\,0)$. Stage costs are defined as

[TABLE]

We transform from Lagrange to Bolza form by introducing a new state $x_{4}$ and a second auxiliary state $x_{5}$ by

[TABLE]

Then, the cost function is $J=x_{4}(T)$ and we fix $T=50$. A solution is computed numerically by the graphical interface WORHP Lab of the optimization software WORHP [5, 4]. The robot dynamics are transcribed by the trapezoidal rule on an equidistant time grid with $50$ time points. An optimal solution is found after 56 outer-loop iterations of an SQP method and resulting costs are $J=0.5141$. In Figure 3, the optimal trajectories and controls are shown. Our focus is on auxiliary state $x_{5}$: The optimal control satisfies $\|\mathbf{u}(t)\|_{R}^{2}=\text{const.}$ for all $t\in[0,T]$, which illustrates the result of Proposition 18. Thus, the quadratic part of the control effort increases linearly with time. Note, however, that the integrated stage cost, i.e.  $x_{4}(t)$, does not increase linearly, nor do the individual controls.

4.2 Energy and Fuel Consumption for Finite Sets of Motion Primitives

Let us now focus on the motion primitives setting. That is, we further restrict $\mathbb{U}$ in the definition of the set of admissible control functions (13). Feasible control values have to belong to a finite set $\{\mathbf{u}^{(1)},\ldots,\mathbf{u}^{(M)}\}\subset[-\bar{\mathbf{u}},\bar{\mathbf{u}}]$, $M\in\mathbb{N}$. Moreover, in accordance with our existence results, see Lemmata 14 and 15, we assume that $\mathbf{u}^{(i)}=\mathbf{0}$ holds if and only if $i=1$.

To this end, let us observe that, for $\hat{x}\in\mathbb{X}\setminus\{\mathbf{0}\}$, there exists at least one interval with non-zero control in view of the terminal equality condition. Then, we reorder the optimal control such that all indices

[TABLE]

are shifted to the end of the sequence, which can be done without loss of optimality. Using this additional condition, we know that we either reach the origin within the sampling interval $[0,\delta)$ or use a control function, which is non-zero for each $t\in[0,\delta)$. The latter, however, implies

[TABLE]

which ensures a decrease of at least $\tilde{c}$ in each MPC step. Since $V(\mathbf{x}^{0})$ is finite, we get finite time convergence.

4.3 Penalization of the Process Time

In this subsection, we present a convergence proof for stage costs, in which the process time is penalized. While convergence is clear, MPC may contribute to further reduce the costs while still ensuring finite time convergence as shown in the following proposition.

Proposition 24**.**

Consider the OCP (14) with $c_{3}>0$ and let $\mathbf{x}^{0}\in\mathbb{X}\setminus\{\mathbf{0}\}$ be given. If the OCP (14) is initially feasible, i.e. if there exists a time $T$ and a control function $u$, $u\in\mathcal{U}_{T}^{S}$, the MPC closed-loop trajectory is well-defined. Moreover, there exists a time $T^{\sharp}$, $T^{\sharp}\in(0,\infty)$, such that $\mathbf{x}^{\operatorname{MPC}}(T^{\sharp};\mathbf{x}^{0})=\mathbf{0}$ holds.

Proof.

We use the abbreviations $\hat{\mathbf{x}}:=\mathbf{x}^{\operatorname{MPC}}(t_{i};\mathbf{x}^{0})$ and $\Delta t:=t_{i+1}-t_{i}$. Recursive feasibility can be directly concluded from the admissibility of the shifted control sequence $u^{\star}_{\hat{\mathbf{x}}}(\cdot+\Delta t)$ at the successor time instant. Moreover, we have

[TABLE]

If there exists an index $i$ such that $V(\mathbf{x}^{\operatorname{MPC}}(t_{i};\mathbf{x}^{0}))=0$ holds, we are done. Otherwise, taking into account that $V$ is positive definite, we get

[TABLE]

using a telescope sum argument. Then, for $i\rightarrow\infty$, the term $i\delta c_{3}$ grows unboundedly, which implies that the right hand side becomes smaller than zero for sufficiently large $i$ (e.g., $i:=\lceil V(\mathbf{x}^{0})(\delta c_{3})^{-1}\rceil$) — a contradiction. ∎

4.4 MPC without Terminal Constraint and Outlook

Here, we want to highlight that motion primitives (or trims if the terminology is supposed to be solely adapted to the example of the mobile robot) were already used in [45] and the follow-up paper [44] to rigorously ensure asymptotic stability for the setting in which neither terminal constraints nor terminal costs were used. The key idea was to derive the controllability assumption initially proposed by Tuna et al. in [43] in combination with the suboptimality estimates from [18], see also [39] and [46] for the extension to the continuous-time setting. In [45], bounds on the value function in dependence of the initial condition were deduced by using the simple sequence turn-move-turn as explicated in Subsection 3.3. A key element was to use a parametric representation of the solution trajectory, which nicely corresponds to the explanation provided in Section 2.

In conclusion, combining the blueprint outlined in [44, 45] and the wording and deeper insight in the use of motion primitives to quantize the nonlinear system dynamics seems to be a very promising approach to tackle systems, for which the linearization does not contain sufficient information to fulfill the stabilization task. Here, it is worth mentioning that purely quadratic costs do, in general, not work for the example of the mobile robot, see [32]. Moreover, the proposed symmetry exploiting technique can also be used to verify initial feasibility, to characterize a set of initially feasible states, and to rigorously treat obstacle avoidance problems.

5 Predictive Control based on Trim Primitives: Numerical Results

Numerical results for the mobile robot example are shown to illustrate the effect of quantizing the set of control values to trim primitives.

5.1 Quantization of the Set of Feasible Control Values

Let the initial value $\mathbf{x}^{0}=(-2,0,0)^{\top}$, the terminal set $\mathbb{X}=\{(0,0,0)^{\top}\}$, and the stage cost $\ell(\mathbf{x},\mathbf{u})=\|\mathbf{u}\|^{2}$ be given. Moreover, we use the set

[TABLE]

with $\Delta u=0.1$. Then, the minimal optimization horizon $T$ such that initial feasibility is ensured is $T=1$. In the following, we use the time shift $\delta=0.1$. Moreover, if the optimal control function is not unique, we choose a sequence with maximal costs on the interval $[0,\delta)$:

At time $t=0$, we get $u^{\star}\equiv(2,0)^{\top}$ and $V(\mathbf{x}^{0})=4$. Hence, we have $\mathbf{x}^{\text{MPC}}(\delta;\mathbf{x}^{0})=\mathbf{x}(\delta;\mathbf{x}^{0},u^{\star})=(-1.8,0)^{\top}$ and the closed-loop costs given by $\int_{0}^{\delta}\ell(\mathbf{x}(t;\mathbf{x}^{0},u^{\star}),\mathbf{u}^{\star}(t))\,\mathrm{d}t=0.4$. 2. 2.

At time $t=\delta$, we get $u^{\star}=u^{\star}(\mathbf{x}^{\text{MPC}}(\delta;\mathbf{x}^{0}))\equiv(1.8,0)^{\top}$, $x^{\text{MPC}}(2\delta;\mathbf{x}^{0})=(-1.62,0)^{\top}$, and $V(\mathbf{x}^{\text{MPC}}(\delta;\mathbf{x}^{0}))=3.24$. Hence, we have reduced the overall costs from $4.00$ to $0.4+3.24=3.64$. The closed-loop costs on $[0,2\delta)$ are $0.4+0.324=0.724$. 3. 3.

At time $t=2\delta$, we get $\mathbf{u}^{\star}(t)=(1.7,0)^{\top}$ on $[0,0.2)$ and $\mathbf{u}^{\star}(t)=(1.6,0)^{\top}$ for $t\in[0.2,1)$ (using our convention since $u^{\star}$ is not unique).

The following values are summarized in Table 1.

The closed-loop cost for reaching the origin are significantly less than the costs associated to the OCP at time $t=0$ ($2.182$ in comparison to $4.000$). The reason is that the control effort is constantly reduced by using MPC since there is some additional freedom to satisfy the terminal equality constraint after each MPC iteration. If a coarser discretization, e.g. $\Delta u=0.5$, and – as a consequence – a smaller trim library is used, the origin is reached after $20$ steps and the closed-loop costs are $3.000$. In conclusion, there is a trade-off between the numerical effort for solving the combinatorial OCP online (which is drastically increasing for a refined quantization) and the closed-loop performance.

5.2 Optimal Control with Trim Primitives

We consider again the dynamics of the mobile robot, cf. Example 1, to illustrate qualitatively different optimal solution built of trim primitive sequences. The parallel parking problem from $x^{0}=(0,1,0)^{\top}$ to $x^{\star}=(0,0,0)^{\top}$ shall be solved in $T=8.0$ time units by optimization w.r.t. various cost functionals.

We restrict to the library of trim primitives as given in Table 2, stored as tuples $(u_{1},u_{2})^{\top}$ and we consider sequences with at most 4 switches. Since algorithmic performance is not in the focus of this work, we globally search through all possible combinations of trim primitives and compute the optimal switching times in each case for which a solution can be found. Note that sequences with fewer switches can be found since two succeeding primitives might be identical. The rest trim plays an important role so that solutions which would not need $T=8$ time units can be prolonged so that they become feasible.

We show the solutions for minimizing the cost $\ell(\mathbf{x},\mathbf{u})=\|\mathbf{u}\|^{2}_{I}+0.5\cdot\|\mathbf{u}\|_{2}$. The best solution is given in Figure 4. It uses the trim sequence $(5,2,4,1)$ (cf. Table 2), as can be seen in the right top picture. Alternative solutions, also with at most 4 switches, are given in Figure 5. When searching for time minimal solutions, the results depicted in Figure 6 are obtained. The control curves code the switching sequence of the solution. Note that the state plot is a projection to the $(x_{1},x_{2})$-plane, i.e. non-smooth turns (edges) in trajectory have in fact a turning phase, such that the mobile robot does fulfill its nonholonomic constraints.

It can be seen that even a small library of primitives can generate different types of solutions. In Figure 5, the red solution is the turn-move-turn sequence. However, there exists a solution with lower costs (green). Other solutions have much higher costs and would not be chosen in the unconstrained scenario. However, they might become of importance as soon as obstacle avoidance is included in the problem.

In Figure 6, one can see that the optimal time is depending on the considered sequence of trim primitives.

5.3 MPC with Trim Primitives

We keep the library of trim primitives which was chosen in the previous subsection to solve the parallel parking problem by an MPC scheme now. Here we compute $12$ MPC steps and set $\delta=1$. Exemplarily, we show the solution for minimizing $\ell=|u|^{2}$ with fixed horizon $T=8$ in every MPC step in Figure 7. The MPC scheme is able to stabilize the system in $8$ time instances. In every MPC step, the cost decreases. Additionally, at $t=2$ and $t=3$ a replanning occurs, i.e. another sequence of motion primitives becomes more efficient than the old solution. (Note that a previous solution can always be prolonged to a valid new solution with the help of the rest trim.) In the future, we would like to investigate the interplay between quantization and closed-loop performance. Larger libraries tend to higher computationally costs but potentially to a better closed-loop performance. Thus, one is interested in the trade-off of these two conflicting optimization goals.

6 Conclusions

We propose to exploit inherent symmetries by using motion primitives in the design, analysis, and numerical treatment of MPC schemes. The key advantage in doing so is that trajectories for genuine nonlinear systems can be easily represented by one-parameter groups. W.r.t. the design and analysis of MPC schemes, we in particular re-interpreted the results presented in [11, 20] (with terminal constraints and costs) and [45] (without terminal constraints and costs) to lay the foundation for their generalization to a larger class of systems. Hereby, it is important to check consistency of stage costs and constraints w.r.t. the symmetries of the system dynamics. Regarding algorithmic aspects, the quantization of the state space by choosing tailored maneuvers encoded by motion primitives is an essential and helpful concept to encounter, on the one hand, the curse of dimensionality in dynamic programming. On the other hand, nonlinear MPC is typically realized via local optimization methods. Thus, getting stuck in local optima is a common problem which can be circumvented by finding (approximations to) alternative solutions via globally searching on a motion primitive graph, cf. e.g. [26]. A potential next step to further enhance the compatability of motion primitives and MPC is to take tailored numerical techniques, see, e.g. [25], into account.

We studied the representative example of the mobile robot in depth in order to illustrate our findings. Here, we derived new necessary optimality conditions for the open-loop OCP and provided numerical simulations to shed some light on the trade-off between performance and numerical effort, which corresponds to setting up a suitable library of motion primitives and solving the respective mixed integer OCP.

In conclusion, motion primitives seem to be a (very) promising approach to systematically verify stability conditions like cost controllability as outlined in [44] without using the proper wording. Furthermore, trims correspond to inherent optimality/turnpike properties of trims, which are useful for the structural analysis of optimal control problems, see, e.g. [8]. Moreover, the proposed combination of motion primitives and MPC is also beneficial if (moving) obstacles or potentially non-convex constraints have to be considered as, e.g., in distributed MPC and for the efficient construction of alternative solutions in order to avoid local minima in the numerical solution of the OCP to be solved in each MPC step.

Appendix

Appendix A OCP with Simplified Dynamics

In this section, we consider an auxiliary OCP, which is needed in order to prove our main result Theorem 20. The auxiliary OCP is constrained by the system dynamics (2), in which the $x_{3}$-dependence of the first two components is replaced by the additional control $u_{3}$. Moreover, $u_{3}$ is not penalized in the objective function.

Proposition 25**.**

We consider the optimal control problem

[TABLE]

w.r.t. $u\in\mathcal{L}^{\infty}([0,T],\mathbb{R}^{3})$ subject to the boundary conditions $\mathbf{x}(0)=\hat{\mathbf{x}}$, $\mathbf{x}(T)=0$ and, for almost all $t\in[0,T]$, the differential equation

[TABLE]

Then, the value function $\tilde{\tilde{V}}(\hat{x}):\mathbb{R}^{3}\rightarrow\mathbb{R}$ is given by the positive definite function

[TABLE]

Proof.

Since the OCP in consideration is decoupled, we can split it into the following two optimal control problems:

[TABLE]

[TABLE]

Firstly, we solve (OCP 1): We assume $\min\{|\hat{x}_{1}|,|\hat{x}_{2}|\}>0$, i.e. a non-zero initial condition. Otherwise the assertion holds trivially since the stage cost is bounded from below by zero. The Hamiltonian is given by

[TABLE]

Differentiation of the Hamiltonian $\mathcal{H}$ w.r.t. the state variables $x_{1}$, $x_{2}$ yields the adjoint equation $\dot{\mathbf{\lambda}}(t)=\mathbf{0}$, i.e. the adjoints $\lambda_{1}$ and $\lambda_{2}$ are constant. Moreover, Pontryagin’s maximum principle also yields the necessary optimality conditions

1. (1)

\mathcal{H}_{u_{1}}=0\Longleftrightarrow\lambda_{0}u_{1}^{\star}(t)=-\frac{1}{2}\Big{(}\lambda_{1}\cos(u_{3}^{\star}(t))+\lambda_{2}\sin(u^{\star}_{3}(t))\Big{)} 2. (3)

\mathcal{H}_{u_{3}}=0\Longleftrightarrow u_{1}^{\star}(t)\Big{(}\lambda_{2}\cos(u^{\star}_{3}(t))-\lambda_{1}\sin(u^{\star}_{3}(t))\Big{)}=0

Firstly, we observe that the right hand side of the differential equation is equal to zero if $u_{1}^{\star}(t)=0$ holds. Combining this observation with the assumed non-zero initial condition and the imposed terminal constraint implies that the set

[TABLE]

has strictly positive measure $|S|$.

Next, we show that $\lambda_{0}\neq 0$, which allows us to set $\lambda_{0}:=1$ w.l.o.g. in the following: Suppose that $\lambda_{0}=0$ holds. Moreover, let us assume that also $\lambda_{1}$ equals zero. Then, Conditions (1) and (3) imply $\sin(u_{3}^{\star}(t))=0=\cos(u_{3}^{\star}(t))$ in view of $\lambda_{2}\neq 0$ for all $t\in S$ — a contradiction since $S$ has non-zero measure. Analogously, we also get a contradiction for $\lambda_{2}=0$. Hence, we have $\lambda_{1}\neq 0\neq\lambda_{2}$. Then, Conditions (1) and (3) imply $\tan(u_{3}^{\star}(t))=-\lambda_{1}/\lambda_{2}$ and $\tan(u_{3}^{\star}(t))=\lambda_{2}/\lambda_{1}$. Combining these two equations yields $-\lambda_{1}^{2}=\lambda_{2}^{2}$ — again a contradiction. Thus, let $\lambda_{0}=1$ in the following.

For each $t\in S\subseteq[0,T]$, Condition (3) yields $\lambda_{1}\sin(u_{3}^{\star}(t))=\lambda_{2}\cos(u_{3}^{\star}(t))$, which implies

[TABLE]

Then, the terminal conditions read

[TABLE]

which can be rewritten as

[TABLE]

by using Condition (1). Then, plugging (30) into these equations leads to

[TABLE]

using the formulas

[TABLE]

Necessarily, this leads either to $\hat{x}_{1}=0$ for $\lambda_{1}=0$ or to a contradiction otherwise. Consequently, we get

[TABLE]

for $\lambda_{1}\neq 0$ and $u_{1}^{\star}=\pm\hat{x}_{2}/|S|$ for $\lambda_{1}=0$ and $\hat{x}_{1}=0$. Hence, in both cases we obtain the objective value

[TABLE]

which is minimal for $|S|=T$. Hence, $S=[0,T]$ holds for the optimal control, which shows that the optimal value of (OCP 1) is $(\hat{x}_{1}^{2}+\hat{x}_{2}^{2})/T$.

Next, we consider (OCP 2), which is a linar quadratic OCP with zero-terminal constraint. Here, the Hamiltonian is $\mathcal{H}(x_{3},\lambda_{3},u_{2})=\lambda_{0}u_{2}^{2}+\lambda_{3}u_{2}$. Again, the differentiation of $\mathcal{H}$ w.r.t. the state $x_{3}$ yields that the adjoint $\lambda_{3}$ is constant. Moreover, the abnormal multiplier can be set to one (otherwise $\mathcal{H}_{u_{2}}=0$ imposes also $\lambda_{3}=0$ — a contradiction). Hence, we get $\lambda_{3}=-2u_{2}$ from the necessary optimality condition $\mathcal{H}_{u_{2}}=0$. Then, the terminal constraint implies $u^{\star}_{2}(t)=-\hat{x}_{3}/T$ and, thus, $\int_{0}^{T}u_{2}^{\star}(t)^{2}\,\mathrm{d}t=\hat{x}^{2}_{3}/T$.

Adding up the two computed optimal values shows the assertion. ∎

Acknowledgements

K. Flaßkamp thanks L. Lüttgens and S. Roy for helpful discussions on the mobile robot example, in particular for the derivation of the Lie algebra representation used to derive the trim primitives. K. Worthmann thanks F. Rußwurm for helpful discussions on the mobile robot example.