Digest for "A Step-by-step Guide on Nonlinear Model Predictive Control for Safe Mobile Robot Navigation"

Obstacle avoidance constraints (3) form convex polytopic sets that change over the prediction time \( \tau\) at time \( t\) . A method to compute a single convex polytope is described in [25]. Based on this, [14] leverages a sequence of polytopic obstacle avoidance constraints, which are used in this work. A similar way to represent the obstacle avoidance constraints is recently proposed in [26], in which the constraints are continuously parameterized and differentiable off-centered ellipsoids based on polynomial path segments. This allows them to be more flexible at the cost of increased computation time compared to (3).

is dynamically feasible:

\[ \begin{equation} {\dot{\boldsymbol{x}}^\mathrm{r}}_{{t+\tau}} = f({\boldsymbol{x}}_{{t+\tau}}^\mathrm{r},{\boldsymbol{u}}_{{t+\tau}}^\mathrm{r}); \end{equation} \]

(5)
satisfies a tightened set of the system constraints:

\[ \begin{equation} {\boldsymbol{r}}_{{t+\tau}} \in {\bar{\mathcal{Z}}}, \end{equation} \]

(6)

where \( {\bar{\mathcal{Z}}}\) is defined as

\[ \begin{equation} {\bar{\mathcal{Z}}} \mathrm{:=} {\bar{{\mathcal{U}}}} \times {\bar{{\mathcal{X}}}} = \left\{({{\boldsymbol{u}},{\boldsymbol{x}}}) \in {\mathbb{R}^{{n^\mathrm{u}}+{n^\mathrm{x}}}}~\middle|~{g_{j}^\mathrm{r,s}}({{\boldsymbol{x}},{\boldsymbol{u}}}) \leq 0,~j \in {\mathbb{N}_{[1,{n^\mathrm{s}}]}}\right\} \subseteq \operatorname{int}({\mathcal{Z}}), \end{equation} \]

(7)

with later introduced functions \( {g_{j}^\mathrm{r,s}}({{\boldsymbol{x}},{\boldsymbol{u}}}), j \in {\mathbb{N}_{[1,{n^\mathrm{s}}]}}\) ;
satisfies a tightened set of the obstacle avoidance constraints:

\[ \begin{equation} M {\boldsymbol{x}}_{{t+\tau}}^\mathrm{r} \in \bar{{\mathcal{F}}}_{{\tau|t}}, \end{equation} \]

(8)

where \( \bar{{\mathcal{F}}}_{{\tau|t}}\) is defined as

\[ \begin{equation} \bar{{\mathcal{F}}}_{{\tau|t}} \mathrm{:=} \left\{{\boldsymbol{p}}_{{\tau|t}} \in {\mathbb{R}^{{n^\mathrm{p}}}}~\middle|~{g_{j,{\tau|t}}^\mathrm{r,o}}({\boldsymbol{p}}_{{\tau|t}}) \leq 0,~j \in {\mathbb{N}_{[1,{n^\mathrm{o}}]}}\right\} \subseteq \operatorname{int}({\mathcal{F}}_{{\tau|t}}),~\tau \in [0,T],~t \geq 0, \end{equation} \]

(9)

with later introduced functions \( {g_{j,{\tau|t}}^\mathrm{r,o}}({\boldsymbol{p}}_{{\tau|t}}), j \in {\mathbb{N}_{[1,{n^\mathrm{o}}]}}\) ;

In dynamic environments, the constraints might vary over time. This could break the recursive feasibility property since the previously optimal solution appended by \( {\kappa^\mathrm{f}({\boldsymbol{x}},{\boldsymbol{r}})}\) does not necessarily satisfy the updated constraints. This does not mean the receding-horizon MPC implementation will not find an optimal solution at each time step. This means that we cannot guarantee that a feasible solution will always exist. Therefore, to prove safety in such environments, one must make an assumption about the environment. For example, as long as the robot’s trajectory can be predicted well, the moving obstacles will avoid the robot. Alternatively, and possibly more standard, is the assumption that there exist known sets that over-approximate the dynamic obstacles over time [16]. The latter assumption is usually limited to structured environments with predictable object movements or results in conservative plans. Less conservative motion plans are constructed in more commonly adopted approaches such as stochastic MPC, e.g., [27]. These approaches plan safe trajectories in dynamic environments up to a certain risk level based on future object trajectory predictions. However, the level of safety guarantees decreases for these schemes.

In words, the previously optimal path \( {\boldsymbol{p}}_{{{{T^\mathrm{s}}+\tau}|t}}^*\) should be contained in the corresponding constraint regions \( {\mathcal{F}}_{{\tau|{t+{T^\mathrm{s}}}}}\) applied for \( \tau \in [0,T-{T^\mathrm{s}}]\) at \( {t+{T^\mathrm{s}}}\) . Similarly, the terminal position \( {\boldsymbol{p}}_{{T|t}}^*\) should be contained in the terminal constraint region \( {\mathcal{F}}_{{T|t}}\) . The latter property ensures that the appended part of the candidate solution, given by (16), satisfies the updated obstacle avoidance constraints at time \( {t+{T^\mathrm{s}}}\) . This property is used to show terminal constraint satisfaction below. We assume the ‘corresponding’ constraint regions are constructed using the previously optimal solution.

There exist a feedback law \( {{\kappa^\delta}({{\boldsymbol{x}},{\boldsymbol{z}}})} : {{{\mathcal{X}}^2} \rightarrow {\mathbb{R}^{{n^\mathrm{u}}}}}\) , an incremental Lyapunov function \( {V^\delta({{\boldsymbol{x}},{\boldsymbol{z}}})} : {{{\mathcal{X}}^2} \rightarrow {\mathbb{R}_{\geq 0}}}\) that is continuously differentiable and satisfies \( {V^\delta}({{\boldsymbol{z}},{\boldsymbol{z}}})=0, \forall {\boldsymbol{z}} \in {\mathcal{X}}\) , parameters \( {c^{\delta\mathrm{,l}}}, {c^{\delta\mathrm{,u}}}, \rho > 0\) , and Lipschitz constants \( {c_{j}^\mathrm{s}} > 0, j \in {\mathbb{N}_{[1,{n^\mathrm{s}}]}}\) and \( {c^\mathrm{o}} > 0\) , such that the following properties hold for all \( ({{\boldsymbol{x}},{\boldsymbol{z}},{\boldsymbol{v}}})\in{{\mathcal{X}} \times {\mathcal{Z}}}\) :

Note that CCM methods provide a way to ensure uniform exponential stability, i.e., exponential stability guarantees that hold in the complete state space, even though metric \( {P^\delta({\boldsymbol{x}})}\) is state-dependent. This contrasts with the design of tracking LMI (27), which includes reference-dependent matrix \( P({\boldsymbol{r}})\) , but is only valid in local regions \( \epsilon\) around the reference trajectory. In practice, the value of \( \epsilon\) for which the results hold is hard to obtain. Therefore, to obtain strict safety guarantees in the robust case, it is practically recommended to use CCMs according to the design above or remove the state dependency in \( {P^\delta}\) . Using CCMs, uniform exponential stabilizability follows from computing integral expressions in the metric space. Using state-independent \( {P^\delta}\) , uniform exponential stabilizability follows as a direct consequence of the mean value theorem (MVT) since choosing a constant CCM \( {P^\delta}\) results in a straight line minimizing geodesic \( {{\boldsymbol{\gamma}}^\mathrm{x}(s)}\) between \( {\boldsymbol{z}}\) and \( {\boldsymbol{x}}\) as described above.

There exist a feedback law \( {{\kappa^\delta}({{\hat{\boldsymbol{x}}},{\boldsymbol{z}}})} : {{{\mathcal{X}}^2} \rightarrow {\mathbb{R}^{{n^\mathrm{u}}}}}\) , an incremental Lyapunov function \( {V^\delta({{\hat{\boldsymbol{x}}},{\boldsymbol{z}}})} : {{{\mathcal{X}}^2} \rightarrow {\mathbb{R}_{\geq 0}}}\) that is continuously differentiable and satisfies \( {V^\delta}({{\boldsymbol{z}},{\boldsymbol{z}}})=0, \forall {\boldsymbol{z}} \in {\mathcal{X}}\) , parameters \( {c^{\delta\mathrm{,l}}}, {c^{\delta\mathrm{,u}}}, \rho > 0\) , and Lipschitz constants \( {c_{j}^\mathrm{s}} > 0, j \in {\mathbb{N}_{[1,{n^\mathrm{s}}]}}\) and \( {c^\mathrm{o}} > 0\) , such that the following properties hold for all \( ({{\hat{\boldsymbol{x}}},{\boldsymbol{z}},{\boldsymbol{v}}})\in{{\mathcal{X}} \times {\mathcal{Z}}}\) , \( {\boldsymbol{w}}\in{\boldsymbol{w}^\mathrm{b}}\oplus{\mathcal{W}}\) , and \( {\boldsymbol{\eta}}\in H\) :

with \( {\dot{\hat{\boldsymbol{x}}}} = f({\hat{\boldsymbol{x}}},{\kappa({{\hat{\boldsymbol{x}}},{\boldsymbol{z}},{\boldsymbol{v}}})}) + E{\boldsymbol{w}^\mathrm{b}} + L({\boldsymbol{y}}-C{\hat{\boldsymbol{x}}})\) , \( {\dot{\boldsymbol{z}}} = f({{\boldsymbol{z}},{\boldsymbol{v}}}) + E{\boldsymbol{w}^\mathrm{b}}\) , \( {\boldsymbol{y}}\) defined in (148), and \( {\dot{\boldsymbol{x}}} = f({\boldsymbol{x}},{\kappa({{\hat{\boldsymbol{x}}},{\boldsymbol{z}},{\boldsymbol{v}}})}) + E{\boldsymbol{w}^\mathrm{b}}\) . Furthermore, the following norm-like inequality holds \( \forall {\boldsymbol{x}}_1, {\boldsymbol{x}}_2, {\boldsymbol{x}}_3 \in {\mathbb{R}^{{n^\mathrm{x}}}}\) :

\[ \begin{equation} {\bar{H}}+\begin{bmatrix} {A({\boldsymbol{\zeta}})}{X^\delta}+{B({\boldsymbol{\zeta}})}{Y^\delta({\boldsymbol{x}})}+\left({A({\boldsymbol{\zeta}})}{X^\delta}+{B({\boldsymbol{\zeta}})}{Y^\delta({\boldsymbol{x}})}\right)^\top+{\lambda^\delta}{X^\delta}&LC{X^\delta}&\boldsymbol{0}\\ \left(LC{X^\delta}\right)^\top&-{\lambda^{\delta,\epsilon}}{X^\delta}&\boldsymbol{0}\\ \boldsymbol{0}&\boldsymbol{0}&{\lambda^{\delta,\epsilon}}\epsilon^2-{\lambda^\delta}\delta^2 \end{bmatrix} \preceq 0, ~ {\boldsymbol{\zeta}} \in {\mathcal{Z}}. \end{equation} \]

(192)

Table of contents

Digest for "A Step-by-step Guide on Nonlinear Model Predictive Control for Safe Mobile Robot Navigation"

References