Robust Convex Model Predictive Control with collision avoidance guarantees for robot manipulators

TL;DR

This paper introduces a convex, robust MPC approach for robot manipulators that guarantees collision avoidance and safe, fast motion in cluttered environments despite model uncertainties.

Contribution

A novel convex MPC framework combining robust tube MPC and corridor planning for collision-free, high-speed robot manipulator control under uncertainties.

Findings

Outperforms benchmark methods in handling higher model uncertainties.

Enables faster motion while maintaining safety.

Validated in simulation with a 6 DOF industrial robot.

Abstract

Industrial manipulators are normally operated in cluttered environments, making safe motion planning important. Furthermore, the presence of model-uncertainties make safe motion planning more difficult. Therefore, in practice the speed is limited in order to reduce the effect of disturbances. There is a need for control methods that can guarantee safe motions that can be executed fast. We address this need by suggesting a novel model predictive control (MPC) solution for manipulators, where our two main components are a robust tube MPC and a corridor planning algorithm to obtain collision-free motion. Our solution results in a convex MPC, which we can solve fast, making our method practically useful. We demonstrate the efficacy of our method in a simulated environment with a 6 DOF industrial robot operating in cluttered environments with uncertainties in model parameters. We outperform benchmark methods, both in terms of being able to work under higher levels of model uncertainties, while also yielding faster motion.

Tables

Table 1. TABLE I : Average computation times for the offline tasks.

Offline task	Computation time [s]
Learn nSCDF	32000
Convex acceleration set (3)	4
Model error constants (11)-(13)	7900
Computing controller (Appendix -B)	4600

Equations

$$\text{u}=\text{M}(\text{q})\ddot{\text{q}}+\text{C}(\text{q},\dot{\text{q}})\dot{\text{q}}+\text{g}(\text{q}).$$

$$\text{M}(\text{q})$$

$$\text{C}(\text{q},\dot{\text{q}})$$

$$\text{g}(\text{q})$$

$$\text{u}={\pi }_{\text{FL}}(\text{q},\dot{\text{q}},\text{a})={\text{M}}_0(\text{q})\text{a}+{\text{C}}_0(\text{q},\dot{\text{q}})\dot{\text{q}}+{\text{g}}_0(\text{q}),$$

$${\pi }_{\text{FL}}(\text{q},\dot{\text{q}},\text{a})\in \mathcal{U}\forall \text{q}\in \mathcal{C},\dot{\text{q}}\in \mathcal{V},\text{a}\in \mathcal{A}.$$

$$\ddot{\text{q}}=$$

$${\Delta }_{\mathbf{\theta }}(\text{q},\dot{\text{q}},\text{a})=$$

$$\text{x}(k+1)$$

$$+{\Delta }_{\text{disc}}(\text{x}(k),\text{a}(k)),$$

$$\text{a}=\bar{\text{a}}+\text{K}(\text{x}-\bar{\text{x}}).$$

$${\Vert (\text{A}+\text{B}\text{K})\text{x}\Vert }_{\text{P}}\le \rho {\Vert \text{x}\Vert }_{\text{P}},\forall \text{x}\in {\mathbb{R}}^{n_x},$$

$$\mathcal{E}(\delta )=$$

$${\mathcal{E}}_{\text{q}}(\delta )=$$

$${\mathcal{E}}_{\text{v}}(\delta )=$$

$${\mathcal{E}}_{\text{a}}(\delta )=$$

$${\Vert \text{B}{\Delta }_{\mathbf{\theta }}(\text{x},\text{a})+{\Delta }_{\text{disc}}(\text{x},\text{a})\Vert }_{\text{P}}$$

$$\le$$

$$a$$

$$b$$

$$c$$

$${\delta }_{+}=$$

$$L_{\beta }=$$

$$\displaystyle{\color[rgb]{0,0,0}\definecolor[named]{pgfstrokecolor}{rgb}{0,0,0}\pgfsys@color@gray@stroke{0}\pgfsys@color@gray@fill{0}\delta_{f}:=c/(1-\tilde{\rho})}.$$

$$\displaystyle{\color[rgb]{0,0,0}\definecolor[named]{pgfstrokecolor}{rgb}{0,0,0}\pgfsys@color@gray@stroke{0}\pgfsys@color@gray@fill{0}\textbf{0}_{n_{c}}\in\mathcal{A}\ominus\mathcal{E}_{\textbf{a}}(\delta_{f}+\epsilon),\quad\textbf{0}_{n_{c}}\in\mathcal{V}\ominus\mathcal{E}_{\textbf{v}}(\delta_{f}+\epsilon).}$$

$$\underset{\bar{\text{X}},\bar{\text{A}},\mathbf{\delta }}{\min }$$

$${\Vert {\bar{\text{x}}}_0-\text{x}(k)\Vert }_{\text{P}}\le {\delta }_0,$$

$${\bar{\text{x}}}_{i+1}=\text{A}{\bar{\text{x}}}_i+\text{B}{\bar{\text{a}}}_i,$$

$$\dot{\text{q}}({\bar{\text{x}}}_H)=0_{n_c},{\delta }_H\ge {\delta }_f,{\bar{\text{x}}}_H\in \mathcal{X}\ominus \mathcal{E}({\delta }_H+ϵ)$$

$${\delta }_{i+1}\ge \tilde{\rho }{\delta }_i+\beta ({\bar{\text{x}}}_i,{\bar{\text{a}}}_i),$$

$${\bar{\text{x}}}_i\in \mathcal{X}\ominus \mathcal{E}({\delta }_i),$$

$${\bar{\text{a}}}_i\in \mathcal{A}\ominus {\mathcal{E}}_{\text{a}}({\delta }_i),i\in {\mathbb{N}}_{0:H-1},$$

$${\mathcal{J}}^{\star }(k+1)-{\mathcal{J}}^{\star }(k)\le -{\Vert {\bar{\text{x}}}_0^{\star }-{\bar{\text{x}}}_H^{\star }\Vert }_{\text{Q}}^2-{\Vert {\bar{\text{a}}}_0^{\star }\Vert }_{\text{R}}^2.$$

$${\Vert {\bar{\text{x}}}_0^{\star }-{\bar{\text{x}}}_H^{\star }\Vert }_{\text{Q}}^2\le d{\Vert {\bar{\text{x}}}_H-{\text{x}}_{\text{g}}\Vert }_{{\text{Q}}_e}^2.$$

$$\text{r}(\text{q})=\{\begin{array}{ll}-{\min }_{{\text{q}}_{\text{c}}\in \partial {\mathcal{C}}_{\text{o}}}\Vert \text{q}-{\text{q}}_{\text{c}}\Vert & \text{if }\text{q}\in {\mathcal{C}}_{\text{o}},\\ {\min }_{{\text{q}}_{\text{c}}\in \partial {\mathcal{C}}_{\text{o}}}\Vert \text{q}-{\text{q}}_{\text{c}}\Vert & \text{otherwise},\end{array}$$

$$\mathcal{B}(\text{c})=\{\text{q}\in {\mathbb{R}}^{n_c}|\Vert \text{q}-\text{c}\Vert \le \text{r}(\text{c})\}\subseteq {\mathcal{C}}_{\text{f}},$$

$$\begin{array}{rl}\text{q}({\bar{\text{x}}}_i)& \in {\mathcal{B}}_i\ominus {\mathcal{E}}_{\text{q}}({\delta }_i),i\in {\mathbb{N}}_{0:H-1},\\ \text{q}({\bar{\text{x}}}_H)& \in {\mathcal{B}}_H\ominus {\mathcal{E}}_{\text{q}}({\delta }_H+ϵ),\end{array}$$

$${\Vert {\bar{\text{x}}}_0-{\widehat{\bar{\text{x}}}}_{n_a}\Vert }_{\text{P}}\le {\delta }_0-{\hat{\delta }}_{n_a}.$$

$${\color[rgb]{0,0,0}\definecolor[named]{pgfstrokecolor}{rgb}{0,0,0}\pgfsys@color@gray@stroke{0}\pgfsys@color@gray@fill{0}\hat{\delta}_{0}=\left\lVert\hat{\bar{\textbf{x}}}_{0}-\textbf{x}(k-n_{a})\right\rVert_{\textbf{P}}.}$$

Full text

I Introduction

Motion planning for robot manipulators is an important problem in industrial applications due to the natural presence of surrounding obstacles. This problem has been extensively studied and it is typically addressed with an efficient pipeline that decouples the problem into smaller sub-problems [13]. First, a collision-free path is found using a sampling-based planner [14, 10], which can also be post-processed [7]. Next, the path is time scaled using the model of our system, e.g., by solving a convex optimization problem [24], which results in a dynamically feasible trajectory. Finally, feedback-linearization is used [23, 16] with the full dynamics model to track the trajectory closely.

Although the approach is efficient and successfully solves the problem, accurate modeling becomes paramount. Furthermore, since the path planning approach relies on collision-detection, we are forced to stay exactly on the path, since it is only there it has been certified to be collision-free. This becomes more challenging through uncertainties in the model parameters, which propagates through the coupled dynamics, resulting in state and input dependent model mismatch. The current pipeline addresses this issue by moving sufficiently slow, such that the effect of model mismatch can be effectively attenuated. A key limiting factor is the lack of methods to effectively ensure robustness also for faster motions.

We address the missing robustness guarantees, allowing us to provide safe and fast motion planning in cluttered environments. We realize this through two main contributions:

• •

We use feedback linearization to obtain a linear prediction model and derive a state and input dependent upper bound on the resulting model error. We design a tube based model predictive controller (MPC) that utilizes this bound and optimizes the tube size to reduce conservatism.

• •

To propagate the tube in the configuration space, we employ a signed configuration distance function (SCDF), which outputs collision-free balls in the configuration space. This allows us to formulate simple convex constraints for obstacle avoidance, while also propagating the tube in a collision-free region, enabling quick collision-free progress towards the goal.

The result is a convex MPC problem, which we can solve efficiently, and a simple approach to guide it to the goal state, resulting in fast and safe motion. This is the first construction of a convex MPC solution that guarantees collision avoidance for nonlinear uncertain dynamics of robots manipulators. We demonstrate the practical applicability of our approach through simulations of a proprietary 6 DOF industrial robot (see Figure 1). In addition, we provide an open-source implementation of the proposed method for general manipulators:

https://github.com/whiterabbitfollow/rob_cvx_mpc_rob_man.

Outline

Section II presents related work. We introduce the notation in Section III and the problem formulation in Section IV. Next, we derive our novel robust motion planning solution for manipulators in Section V. Obstacle avoidance is included in Section VI by proposing the concept of corridor planning and deriving simple convex constraints ensuring collision-free motion. We verify our approach in numerical experiments (Section VII) and end with conclusions (Section VIII).

II Related work

Forming collision-free regions in the configuration space is challenging for manipulators, due to the non-trivial mapping of world space obstacles to the configuration space. In the past decade, a lot of methods [1, 20, 15, 25] have been developed in order to produce convex collision-free regions in the configuration space. In [1, 20] an optimization problem is solved iteratively to enlarge an ellipsoid, representing a collision-free region, while [15, 25] instead uses learning to produce collision-free balls. These tools enable new approaches to motion planning, e.g. trajectory planning [17], path planning [25], manipulation planning [15]. Our work makes use of these new representation capabilities to formulate convex obstacle avoidance constraints and combines it with a novel robust control formulation to ensure safe and efficient motions.

Our approach relies on MPC [2], which guarantees satisfaction of state and input constraints. In particular, we build on MPC-for-tracking formulations [12], which can progressively reach far away targets by optimizing artificial references. Obstacle avoidance constraints can in principle also be directly added to such a formulation [5, 19]. However, especially for robot manipulators these constraints are highly non-linear and non-convex, thus increasing the computational demand. Similar to the proposed approach, the work in [22] use convex collision-free balls, however, the methodology is limited to single-body robots with known dynamics. Applications of MPC to robot manipulators are, for example, presented in [19, 4, 9]. However, these approaches are computationally expensive, can only cover simple collision avoidance constraints or none at all, or are conservative. In particular, [19, 4] formulate non-linear MPC schemes, which becomes computationally expensive. Only simplified collision avoidance constraints are treated in [19], while [4, 9] do not address obstacle avoidance at all. The robustness guarantees in [19, 9] are based on a constant (worst-case) bound on the model-mismatch, which neglects the state/input dependent nature of modeling errors, resulting in significant conservatism, while [4] lacks robustness guarantees. In contrast, we construct a collision-free corridor in the configuration space through a SCDF [25], resulting in convex obstacle avoidance constraints. We address model error through a robust design, which is tube based. Compared to standard rigid tube MPC [18], which uses a polytopic invariant sets to bound the state trajectories, we use scaled ellipsoids, resulting in a more scalable design and efficient formulation for online control. In particular, we exploit a state and input dependent bound on the model error, allowing us scale the tube in a flexible way, while also reducing conservatism. We end up with an MPC formulation that is convex, which we can solve fast, observing real-time capabilities in numerical experiments.

III Notation

The set of positive real numbers is denoted by $\mathbb{R}_{+}$. We denote the set of integers $a$ to $b$, i.e. $\{a,a+1,\ldots,b\}$ by $\mathbb{N}_{a:b}$. The set of $n$ dimensional positive definite matrices is denoted by $\mathbb{S}_{++}^{n}$. For a vector $\textbf{x}\in\mathbb{R}^{n}$, we denote the 2-norm and infinity-norm as $\left\lVert\textbf{x}\right\rVert=\sqrt{\textbf{x}^{\top}\textbf{x}}$ and $\left\lVert\textbf{x}\right\rVert_{\infty}=\underset{i}{\max}\>|\textbf{x}_{i}|$. For a matrix $\textbf{A}\in\mathbb{R}^{n\times m}$, $\left\lVert\textbf{A}\right\rVert$ is the induced matrix norm, i.e., the largest singular value of A. The weighted vector norm is defined as $\left\lVert\textbf{x}\right\rVert_{\textbf{A}}=\sqrt{\textbf{x}^{\top}\textbf{A}\textbf{x}}$. We denote the symmetric matrix square root of a positive semi-definite matrix A as $\textbf{A}^{1/2}$. The vectors $\textbf{1}_{n}$ and $\textbf{0}_{n}$ denotes an $n$ dimensional vector of ones and zeros, respectively. The identify matrix is denoted as $\mathbf{I}$. The operation of stacking two column vectors is expressed as $(\textbf{a},\textbf{b})=[\textbf{a}^{\top},\textbf{b}^{\top}]^{\top}$. Finally, the function $\text{diag}(\cdot):\mathbb{R}^{n}\mapsto\mathbb{R}^{n\times n}$, maps an $n$ dimensional vector into a diagonal matrix.

IV Problem formulation

Consider a robot operating in the world space, $\mathcal{W}\subset\mathbb{R}^{3}$. The exact description of the robot body is given by its configuration $\textbf{q}\in\mathcal{C}$, where $\mathcal{C}\subset\mathbb{R}^{n_{c}}$ is the configuration space. The robot dynamics has the following form

[TABLE]

In the above, $\dot{\textbf{q}}\in\mathcal{V}$, $\textbf{u}\in\mathcal{U}$, denote the velocity and control input, constrained to lie in their corresponding sets $\mathcal{V}\subset\mathbb{R}^{n_{c}}$ and $\mathcal{U}\subset\mathbb{R}^{n_{c}}$. Furthermore, $\textbf{M}:\mathbb{R}^{n_{c}}\mapsto\mathbb{R}^{n_{c}\times n_{c}}$, $\textbf{C}:\mathbb{R}^{n_{c}}\times\mathbb{R}^{n_{c}}\mapsto\mathbb{R}^{n_{c}\times n_{c}}$, $\textbf{g}:\mathbb{R}^{n_{c}}\mapsto\mathbb{R}^{n_{c}}$, denote the mass matrix, coupling matrix (Coriolis and damping), and gravity vector functions, respectively. We assume incomplete knowledge of the model parameters (1), separating the model into nominal (known) and uncertain terms:

[TABLE]

where a null index denotes nominal terms, and error terms are indexed by a parameter vector $\bm{\theta}\in\Theta$, where $\Theta\subset\mathbb{R}^{n_{p}}$ is a known parameter set and $\bm{\theta}$ is the unknown model parameter. We assume that the sets $\mathcal{C}$, $\mathcal{V}$ and $\mathcal{U}$ are hyperboxes, and that measurements of q and $\dot{\textbf{q}}$ are readily available. The robot is surrounded by $n_{o}$ obstacles $\mathcal{O}=\bigcup_{i=1}^{n_{o}}\mathcal{O}_{i}$, where $\mathcal{O}_{i}\subset\mathcal{W}$. The set of points covered by the robot body in configuration q is expressed as $\mathcal{FK}(\textbf{q})\subset\mathcal{W}$. We define the free space as $\mathcal{C}_{\text{f}}=\{\textbf{q}\in\mathcal{C}\;|\;\mathcal{FK}(\textbf{q})\cap\mathcal{O}=\emptyset\}$ and the obstacle region as $\mathcal{C}_{\text{o}}=\mathcal{C}\setminus\mathcal{C}_{\text{f}}$. Our goal is to design a controller that steers the robot from a given start configuration $\textbf{q}_{\text{s}}$ to a goal configuration $\textbf{q}_{\text{g}}$, while ensuring that the resulting trajectory satisfies constraints on velocity, torque, and is collision-free, i.e. $\textbf{q}(t)\in\mathcal{C}_{\text{f}}$, $\dot{\textbf{q}}(t)\in\mathcal{V}$, $\textbf{u}(t)\in\mathcal{U}$, $\forall t\geq 0$, for any considered model parameters $\bm{\theta}\in\Theta$.

V Robust convex MPC for manipulators

In this section, we will derive a motion planning solution that comes with robustness guarantees, for the moment ignoring obstacle avoidance, which we address in the subsequent section.

A key feature we aim for in our design is that the resulting MPC problem can be solved fast. Hence, we want a convex formulation, which requires convex constraints and linear prediction models. First, we utilize feedback linearziation to obtain a linear model and a suitable bound on the un-cancelled non-linearities (Section V-A). To ensure robustness, we predict a scaled tube around a nominal prediction such that it contains the true (unknown) system response. We use an auxiliary controller and an ellipsoid tube to derive an expression of the tube dynamics (Section V-B). Finally, we present our suggested convex robust MPC in Section V-C, including the theoretical analysis.

V-A Feedback linearization and model error

In order to obtain a convex optimization problem, we have to obtain a linear prediction model. We use feedback linearization (FL [23]) to realize this, i.e., we cancel the (known) non-linear terms in the dynamics using feedback

[TABLE]

where $\textbf{a}\in\mathbb{R}^{n_{c}}$ is the desired acceleration. To ensure that the resulting torque u satisfies the torque constraints $\mathcal{U}$, we introduce a convex acceleration constraint set $\mathcal{A}\subset\mathbb{R}^{n_{c}}$, which satisfies

[TABLE]

Using the feedback (2) in the dynamics (1) yields

[TABLE]

where $\ddot{\textbf{q}}\in\mathbb{R}^{n_{c}}$ is the acceleration. The functions $\tilde{\textbf{M}}_{\bm{\theta}}:\mathbb{R}^{n_{c}}\mapsto\mathbb{R}^{n_{c}\times n_{c}}$, $\tilde{\textbf{C}}_{\bm{\theta}}:\mathbb{R}^{n_{c}}\mapsto\mathbb{R}^{n_{c}\times n_{c}}$ and $\tilde{\textbf{g}}_{\bm{\theta}}:\mathbb{R}^{n_{c}}\mapsto\mathbb{R}^{n_{c}}$ are errors parameterized by the uncertain model parameter $\bm{\theta}\in\Theta$, see Appendix -A for the derivation. We note that (5) does not introduce any approximations, it simply states that the uncertain robot dynamics (1) with the feedback (3) are equivalent to a double integrator subject to an additional nonlinear perturbation $\Delta_{\bm{\theta}}$, that depends on the uncertain model parameters $\bm{\theta}$. Integrating the model (5) over the sampling time $T_{\text{s}}$ with a (piece-wise) constant desired acceleration a yields

[TABLE]

where, $k\in\mathbb{N}$ is the discrete time index, $\textbf{x}=(\textbf{q},\dot{\textbf{q}})\in\mathcal{X}:=\mathcal{C}\times\mathcal{V}\subset\mathbb{R}^{n_{x}}$ is the state with $n_{x}=2\cdot n_{c}$. To simplify the notation, we access the configuration and velocity from the state by $\textbf{q}(\textbf{x})$ and $\dot{\textbf{q}}(\textbf{x})$, respectively. $\textbf{A}\in\mathbb{R}^{n_{x}\times n_{x}}$ and $\textbf{B}\in\mathbb{R}^{n_{x}\times n_{c}}$ denote the dynamics and control matrices, respectively. The error term $\Delta_{\mathrm{disc}}$ is an additional discretization error accounting for the fact that $\Delta_{\theta}$ is not constant over the sampling time.

V-B Auxiliary controller and tube dynamics

To attenuate the effects of the model errors over a prediction horizon, we use an auxiliary controller, with the following control law

[TABLE]

The matrix $\textbf{K}\in\mathbb{R}^{n_{c}\times n_{x}}$, is referred to as the gain matrix, $\bar{\textbf{a}}\in\mathbb{R}^{n_{c}}$ and $\bar{\textbf{x}}\in\mathbb{R}^{n_{x}}$ are the reference control and states, respectively. We compute the gain matrix that satisfies the following requirement

[TABLE]

where $\rho\in(0,1)$ is a contraction rate and $\textbf{P}\in\mathbb{S}_{++}^{n_{x}}$ is referred to as the Lyapunov matrix. We obtain P and K satisfying (8) by solving a convex optimization problem offline, see Appendix -B for details. The Lyapunov matrix P allows us to form an ellipsoid in the state space which, together with its projection on to the configuration, velocity and control input space, are

[TABLE]

The size of the ellipsoid is controlled by the scaling $\delta\in\mathbb{R}_{+}$.

The following proposition provides a state and input dependent bound on the prediction error.

Proposition 1**.**

For all $\textbf{x}=(\textbf{q},\dot{\textbf{q}})\in\mathcal{X}$, $\textbf{a}\in\mathcal{A}$ and $\bm{\theta}\in\Theta$, we have

[TABLE]

with $\Delta_{\bm{\theta}},\Delta_{\text{disc}}$ from (V-A) and $a,b,c$ according to (11)-(13).

Proof.

Using the triangle inequality and the property $||\textbf{A}\textbf{x}||\leq||\textbf{A}||||\textbf{x}||$ [8] on the individual terms of expression (5), we end up with the bound in (10), where $a,b,c\in\mathbb{R}_{+}$ are computed according to

[TABLE]

The closed-form expressions of the functions introduced above are presented in Appendix -A. ∎

This bound highlights that the error depends significantly on the velocity and acceleration, crucial knowledge that the controller will leverage for safe and efficient planning.

Having obtained a state and input dependent bound, we now focus on how to adjust the scaling such that the uncertain system (V-A) remains inside the ellipsoid (9) around the nominal prediction , which we present in the following proposition.

Proposition 2**.**

For any $\left\lVert\textbf{x}-\bar{\textbf{x}}\right\rVert_{\textbf{P}}\leq\delta$, $\bm{\theta}\in\Theta$, we have that $\left\lVert\textbf{x}_{+}-\bar{\textbf{x}}_{+}\right\rVert_{\textbf{P}}\leq\delta_{+}$ with $\textbf{x}_{+}$ according to (V-A), $\bar{\textbf{x}}_{+}=\textbf{A}\bar{\textbf{x}}+\textbf{B}\bar{\textbf{a}}$, $\textbf{a}=\bar{\textbf{a}}+\textbf{K}(\textbf{x}-\bar{\textbf{x}})$, $\textbf{V}=[\textbf{0}_{n_{c}\times n_{c}},\textbf{I}_{n_{c}\times n_{c}}]$ and

[TABLE]

The proof can be found in Appendix -C. In the following, we abbreviate $\tilde{\rho}:=\rho+L_{\beta}$ and we assume that $\tilde{\rho}<1$. Given $\rho<1$, this holds if the parametric uncertainty $\Theta$ is sufficiently small. Finally, for a steady state with zero velocity/acceleration, the tube size $\delta$ converges to a steady state tube size

[TABLE]

We assume that this steady-state tube size is smaller than the velocity and acceleration constraints:

[TABLE]

V-C Robust convex MPC problem

With a convex expression for the model error propagation and convex input constraints established, we can now formulate our resulting robust MPC problem as follows:

[TABLE]

using the current state $\textbf{x}(k)$, the goal state $\textbf{x}_{\text{g}}$, user chosen positive definite weight matrices $\textbf{Q},\textbf{Q}_{e},\textbf{R}\in\mathbb{S}_{++}$, and a horizon $H\geq 2$. The user-chosen offset $\epsilon>0$ ensures that the system cannot get stuck at a steady-state close to the constraints [12]. The decision variables are the nominal trajectory, $\bar{\textbf{X}}=[\bar{\textbf{x}}_{0},\ldots,\bar{\textbf{x}}_{H}]\in\mathbb{R}^{n_{x}\times(H+1)}$, the nominal control inputs $\bar{\textbf{A}}=[\bar{\textbf{a}}_{0},\ldots,\bar{\textbf{a}}_{H-1}]\in\mathbb{R}^{n_{c}\times H}$ and the tube size $\bm{\delta}=[\delta_{0},\ldots,\delta_{H}]^{\top}\in\mathbb{R}^{H+1}$. In closed-loop operation, we solve (18) with the current measured state $\textbf{x}(k)$, and then apply $\textbf{a}(k)=\textbf{a}^{\star}_{0}$, the first optimized input, to the system. Note that Problem (18) is a convex second-order cone problem, which can be efficiently solved.

The objective, (18a), consists of two parts. The first part drives the predicted trajectory to the steady-state $\bar{\textbf{x}}_{H}$, which acts as an artificial reference (cf. [12]), while keeping the controls small. A second term pushes this artificial reference to the desired goal $\textbf{x}_{\text{g}}$.

The nominal trajectory starts with a tube around the measured state $\textbf{x}(k)$ (18b) and evolves according to the linear dynamics (18c). The final state is a steady-state with zero velocity (18d). The tube (18e) is scaled according to Proposition 2. Finally, the state and control limits are tightened by the tube size in (18f) and (18g), respectively. The operator $\ominus$ denotes the Pontryagin differences, see Appendix -B2 for implementation details. The following theorem summarizes the closed-loop properties.

Theorem 1**.**

Consider the nonlinear system (V-A) with $\bm{\theta}\in\Theta$ and $\textbf{x}_{\text{g}}\in\mathcal{X}\ominus\mathcal{E}(\epsilon+\delta_{f})$. Suppose that Problem (18) is feasible at $k=0$, then the closed-loop system satisfies:

• •

Recursive feasibility: Problem (18) is feasible $\forall k\in\mathbb{N}$;

• •

Constraint satisfaction: $\textbf{x}(k)\in\mathcal{X}$, $\textbf{a}(k)\in\mathcal{A}$, $\forall k\in\mathbb{N}$;

• •

Convergence: $\underset{k\rightarrow\infty}{\lim\sup}~\{\textbf{x}(k)-\textbf{x}_{\text{g}}\}\subseteq\mathcal{E}(\delta_{f})$.**

Proof.

The proof merges concepts from robust MPC using homothetic tube [21] and MPC for tracking [12, 11].

**Part I: ** Given the optimal solution to problem (18) at time $k$, we consider the following feasible candidate solution $\bar{\textbf{X}}=[\bar{\textbf{x}}^{\star}_{1},\ldots,\bar{\textbf{x}}_{H}^{\star},\bar{\textbf{x}}_{H}^{\star}]$, $\bar{\textbf{A}}=[\bar{\textbf{a}}_{1}^{\star},\ldots,\bar{\textbf{a}}_{H-1}^{\star},\textbf{0}_{n_{c}}]$, $\bm{\delta}=[\delta_{1}^{\star},\ldots,\delta_{H}^{\star},\delta_{H}]$. Here, $\delta_{H}=\delta^{\star}_{H}\tilde{\rho}+c$ according to (18e) with $\beta(\bar{\textbf{x}}_{H},0)=c$, given that $\bar{\textbf{x}}_{H}^{\star}$ is a steady-state using (18d). Furthermore, $\tilde{\rho}<1$ and $\delta_{H}^{\star}\geq\delta_{f}$ (16) ensure that $\delta_{H}\leq\delta_{H}^{\star}$ and thus this appended solution also satisfies the tightened constraints (18e) at $i=H$. Lastly, Proposition 2 ensures that this candidate solution also satisfies the initial state constraint (18b) with the new measured state $\textbf{x}(k+1)$ for any $\bm{\theta}\in\Theta$.

**Part II: **Closed-loop constraint satisfaction follows from the feasibility of Problem (18), the tightened constraints (18f), (18g), and the fact that $\textbf{x}(k)-\bar{\textbf{x}}_{0}^{\star}\in\mathcal{E}(\delta_{0}^{\star})$, $\textbf{a}(k)-\bar{\textbf{a}}_{0}^{\star}\in\mathcal{E}_{\textbf{a}}(\delta_{0}^{\star})$ using the definition of the ellipsoids (9) and the initial state constraint (18b).

**Part III: **Let us denote the optimal cost of Problem (18) at time $k$ by $\mathcal{J}^{\star}(k)$. The feasible candidate solution implies

[TABLE]

Given $\mathcal{J}^{\star}$ non-negative and $\mathcal{J}^{\star}_{0}$ finite, using this condition in a telescopic sum ensures that, as $k\rightarrow\infty$, $\bar{\textbf{x}}^{\star}_{0}$ converges to a steady-state. Lastly, to ensure that this steady-state corresponds to $\textbf{x}_{\text{g}}$, we use [11, Lemma 1], to ensure the existence of a uniform constant $d>0$, such that

[TABLE]

This result applies given the convex steady-state manifold, the strictly convex quadratic cost, the fact that steady-states are the interior of the (tightened) constraints with $\epsilon>0$, and controllability of $(\textbf{A},\textbf{B})$ with $H\geq 2$. Thus, $\bar{\textbf{x}}^{\star}_{0}$ converges to $\textbf{x}_{\text{g}}$ and $\textbf{x}(k)$ converges to $\textbf{x}_{\text{g}}\oplus\mathcal{E}(\delta_{f})$. ∎

Notably, Theorem 1 relies only on convex optimization, provides robustness guarantees for uncertain nonlinear manipulators, accounts for velocity/acceleration dependence of the model error, and provides a larger region of attraction.111Given (17), any steady-state $\textbf{x}(0)\in\mathcal{X}\ominus\mathcal{E}(\epsilon+\delta_{f})$ is a feasible initial condition of the MPC (18). Furthermore, in case the uncertainty in gravity and the discretization error is small, we get $c\approx 0,\delta_{f}\approx 0$. With $\epsilon\approx 0$ this implies that any steady-state in the constraints is a feasible initial condition.

VI Real-time MPC with convex obstacle avoidance constraints

In this section, we add the two missing pieces of the approach presented in Section V: (i) How to enforce obstacle avoidance? (ii) How to execute the optimization in parallel to ensure real-time applicability? Furthermore, we present an algorithm that guides the robot through a collision-free corridor, only requiring the solution to a convex MPC problem that is solved in parallel during execution. Finally, we provide a theoretical analysis, showing that this approach robustly ensures safe operation.

VI-A Obstacle avoidance through SCDF

The SCDF is defined as

[TABLE]

which is the distance to the boundary of the obstacle region $\partial\mathcal{C}_{\text{o}}$. Given a collision-free point $\textbf{c}\in\mathcal{C}_{\text{f}}$, the SCDF allows us to define a collision-free region as

[TABLE]

which is parametrized by the tuple $(\textbf{c},r)$. The region is an Euclidean norm ball in the configuration space. Obtaining an analytical expression for the SCDF is non-trivial, which is why we resort to approximate methods[25, 15]. To adapt the proposed MPC such that it guarantees obstacle avoidance, we simply add the following constraints

[TABLE]

to the MPC problem in (18). The computation of the constraint tightening is given in Appendix -D. In the above, each state in the nominal trajectory is constrained to lie within an allocated ball $\mathcal{B}_{i}$. How these are computed is presented in Section VI-C.

VI-B Parallel MPC formulation

To fulfill the real-time constraints, we solve the MPC problem (18) in parallel while running the auxiliary controller (7) for $n_{a}>1$ steps. To enable this parallelism, we need to use a forward projection of the initial condition [6], since the measured state $\textbf{x}(k)$ in (18b) is not yet available. Thus, we replace the measured state in the constraint (18b) with a tube prediction which will contain the future state:

[TABLE]

Here, $\hat{\bar{\textbf{x}}}_{n_{a}}\in\mathcal{X}$ and $\hat{\delta}_{n_{a}}\in\mathbb{R}_{+}$ are the nominal state and tube size predicted at time $k-n_{a}$. In particular, $\hat{\bar{\textbf{x}}}_{n_{a}}$ corresponds to the prediction $\bar{\textbf{x}}^{\star}_{n_{a}}$ made $n_{a}$ steps ago. Similarly, the tube is predicted using (18e), but with the initial condition based on the most up-to-date information at time $k-n_{a}$:

[TABLE]

VI-C Robust corridor planning

Next, we propose an algorithm that ensures that the robot reaches a given goal configuration, $\textbf{q}_{\text{g}}$, without collisions. The high-level idea is to first generate a collision-free region that connects the current configuration and the goal configuration. We refer to this region as a corridor, which is produced using the SCDF around a collision-free path and is illustrated in the left part of Figure 2. Then, at each iteration, we constrain the predicted MPC trajectory to stay within this corridor and pull it towards a temporary virtual goal state, $\tilde{\textbf{x}}_{\text{g}}$, such that we make progress to the global goal state.

Our approach is sketched in Algorithm 1. Before any planning can take place, we perform the offline tasks already described in the previous sections. That is, we compute the acceleration constraints $\mathcal{A}$, line 2, compute the auxiliary controller, line 3 , and compute the model error constants, line 4.

During online operation, we start at a steady-state configuration $\textbf{q}_{\text{s}}$, receive a goal configuration $\textbf{q}_{\text{g}}$ and first execute the corridor planning, lines 6-7. Then, we plan a high-level collision-free path, $\bm{\gamma}(s):~[0,1]~\mapsto~\mathcal{C}_{\text{f}}$, to the goal, e.g. by using a sampling-based planner, line 8. Next, in line 9, we discretize the corresponding path, resulting in the sequence $\text{C}=(\textbf{c}_{1},\ldots,\textbf{c}_{M})\in\mathcal{C}_{\text{f}}^{M}$, and precompute the collision-free balls with the SCDF according to (20), resulting in a discretized corridor $\text{B}=((\textbf{c}_{1},r_{1}),\ldots,(\textbf{c}_{M},r_{M}))$.
In order to guarantee convergence, the sequence of balls needs to be sufficiently overlapping, which is captured by the following condition

[TABLE]

We verify (24) for all neighbouring balls in the corridor and use a finer discretization for pairs where it is not yet fulfilled. Then, we initialize a feasible trajectory, control inputs, and a predicted tube around the current state, line 10. Next, we enter the real-time control loop, line 12-23, which is illustrated in the mid-left part of Figure 2. From a feasible trajectory we loop over all its states $\bar{\textbf{x}}_{i}\in\bar{\textbf{X}}$ and assign a corresponding ball to it, line 12. This is done according to

[TABLE]

which returns the ball $\mathcal{B}_{i}=\mathcal{B}(\textbf{c}_{i})$ that contains $\textbf{q}(\bar{\textbf{x}}_{i})$ with the largest margin. The assignment rule is conceptualized in the mid right part of Figure 2. The process is repeated for $i\in\mathbb{N}_{0:H}$, resulting in the sequence $\bar{\text{B}}=(\mathcal{B}_{0},\ldots,\mathcal{B}_{H})$.

Next, we compute a virtual goal state $\tilde{\textbf{x}}_{\text{g}}$, line 13, which serves the purpose of pulling the trajectory in a direction that makes progress to the global goal state $\textbf{x}_{\text{g}}$. This is done by selecting the configuration that has made the most progress along the path and is contained in the last ball, i.e. according to

[TABLE]

where the last ball is tightened with the steady state tube size in order to be compliant with the convergence properties of Theorem 1. Having computed a virtual goal configuration, we define the resulting state as $\tilde{\textbf{x}}_{\text{g}}=(\tilde{\textbf{c}}_{\text{g}},\textbf{0}_{n_{c}})$. We illustrate this process in the right part of Figure 2. We continue with solving the MPC problem in parallel, line 14, where we solve Problem (18) with the collision-avoidance constraints (21) and initial tube constraint (22). The inputs are the predicted tube, $\hat{\bar{\textbf{x}}}$ and $\hat{\delta}$, balls $\bar{\text{B}}$ and the virtual goal $\tilde{\textbf{x}}_{\text{g}}$. Solving the problem results in an optimized nominal trajectory and control inputs. While the MPC solver is running, we continue by predicting our future tube, line 15, and run our auxiliary controller, line 16-21. Next, we shift our nominal trajectory $n_{a}$ times and append the last state $n_{a}$ times, line 22, serving as a feasible trajectory for the next iteration, line 23. The number of auxiliary steps defines a tunable time slot, $n_{a}T_{\text{s}}$, which enables parallel execution of the auxiliary controller and the MPC solver. It is defined by the user and helps to meet any real-time requirements.

The following theorem shows that running our proposed algorithm guarantees feasibility and convergence to the goal state.

Theorem 2**.**

Consider the nonlinear system (V-A) with $\bm{\theta}\in\Theta$ and Algorithm 1. Suppose further that the corridor reaches from $\textbf{q}_{\text{s}}$ to the target $\textbf{q}_{\text{g}}$. Then,

• •

Feasibility: All the optimization problems in Algorithm 1 are feasible for all $k\in\mathbb{N}$;

• •

Constraint satisfaction: $\textbf{q}(k)\in\mathcal{C}_{\text{f}}$, $\dot{\textbf{q}}(k)\in\mathcal{V}$, $\textbf{u}(k)\in\mathcal{U}$, $\forall k\in\mathbb{N}$;

• •

Convergence: $\underset{k\rightarrow\infty}{\lim\sup}~\{\textbf{x}(k)-\textbf{x}_{\text{g}}\}\subseteq\mathcal{E}(\delta_{f})$.

Proof.

The proof is analogous to Theorem 1. The main change is ensuring that the allocated ball constraints (21) do not adversely impact convergence and feasibility and that the virtual goal $\tilde{\textbf{x}}_{\text{g}}$ converges to the global goal $\textbf{x}_{\text{g}}$ in finite time. The detailed proof can be found in Appendix -E. ∎

VII Numerical experiments

VII-A Setup

The robot we use for our simulations is an IRB 1100 which is a 6 DOF robot, illustrated in the left part of Figure 3. The configuration space $\mathcal{C}$ is defined by its joint limits and the velocity constraint set is defined as $\mathcal{V}=\{\dot{\textbf{q}}\in\mathbb{R}^{n_{c}}\>|\>\left\lVert\dot{\textbf{q}}\right\rVert_{\infty}\leq 2\}$. The dynamics is based on a proprietary model of an IRB 1100 robot with an additional damping term proportional to the velocity, defined as $\text{diag}([10^{-1},10^{-1},10^{-1},10^{-2},10^{-2},10^{-4}])~\cdot~2$. We simulate the continuous-time dynamics (4) with an Explicit Runge-Kutta method of order 5. We consider parametric uncertainty in the mass of each link and the damping. Gravity error is assumed to be negligible, i.e. $\tilde{\textbf{g}}_{\bm{\theta}}\approx 0$ in (13), since it is usually easy to compensate with a disturbance observer. We demonstrate our proposed method on a common robotic application, illustrated in the left part of Figure 3, which shows a pick and place scenario.

VII-A1 Convex acceleration set

We compute the convex acceleration constraint $\mathcal{A}$ in (3) using sampling. We start with a nominal box constraint set for the acceleration, $\mathcal{A}=\{\textbf{a}\in\mathbb{R}^{n_{c}}\>|\>\left\lVert\textbf{a}\right\rVert_{\infty}\leq 20\}$, represented in vertex form. Then, we uniformly sample $10^{5}$ states $(\textbf{q},\dot{\textbf{q}})\in\mathcal{X}$. For each sampled state and control input vertex $\textbf{a}\in\mathcal{A}$, we verify condition (3). If the condition is violated, we shrink the acceleration set uniformly by 1 % and then repeat the process until it is satisfied for all states and control inputs.

VII-A2 Model error constants

The model error constants are also computed using a sampling-based approach. We compute a batch containing $10^{6}$ random states and control inputs, from which we compute (11)-(13) by using a running max. We check for convergence between the batches by checking the largest difference of the constants in-between the batches. The process is stopped if the difference is less than $10^{-5}$.

VII-A3 Corridor planning

To produce a corridor, we use the hybrid solution presented in [25]. That is, we fist learn a deep neural network representing the SCDF, referred to as an nSCDF, by over-approximating the wrist and tool with a sphere to reduce the dimensionality. When the nSCDF is negative, we fallback on a conventional collision-detector to refine the collision query. To find an initial path, we start by creating a roadmap where the nSCDF is positive. Then, we connect the query points to the graph. To produce a corridor around the path, we discretize the path and query the nSCDF for the distances. For parts where the nSCDF is negative, we use the collision detector, where we sample $10^{3}$ configuration within a ball of size $0.1$ [rad], shrinking the ball if any collision is detected until all points within the ball are collision-free.

VII-A4 Methods

In our experiments we compare the following methods.

• •

Flexible: Our approach, i.e. (18) with (21) and (22).

• •

Rigid: Standard tube MPC, i.e. same as ours but executed with a constant tube size $\delta$ based on the worst-case model mismatch.

• •

Nominal: The MPC in (18) with (21), but without robustness features, i.e., $\delta=0$.

• •

Oracle: The same as nominal, but with a perfect model. This provides a lower bound on the achievable performance.

All methods are executed with the following parameters: $H=15$, $\textbf{Q}=\text{diag}((\textbf{1}_{n_{c}}\cdot 10,\textbf{1}_{n_{c}}\cdot 0.01))$, $\textbf{Q}_{e}=\text{diag}(\textbf{1}_{n_{x}}\cdot 10^{4})$ and $\textbf{R}=\text{diag}(\textbf{1}_{n_{c}}\cdot 10^{-3})$.

VII-A5 Simulations

A constant control input is applied every $T_{\text{s}}=10$ [ms]. The robust MPC methods are optimized every $n_{a}=4$ steps. All MPC schemes are solved on a laptop with an Intel i5-1155G7 CPU. We repeat each simulation $3$ times, re-sampling the uncertainty parameter $\bm{\theta}\in\Theta$ uniformly for each run. To evaluate the scaling capabilities, the methods are tested with different levels of uncertainty. If the method was not able to reach the goal within $100$ seconds, it was stopped and labeled as unsuccessful. To verify the SCDF, we collision-checked all trajectories returned from the methods with a conventional collision checker using the exact obstacle and robot geometries. The robot was defined to have reached the goal if its state is within an $\varepsilon$-ball of radius $0.01$ around the goal state. We measure performance based on the time it takes to reach the goal region and how the method scales with increasing uncertainty.

VII-B Results

The offline computation times are presented in Table I.

Corridor: Finding a path takes $40$ [ms] and to produce a corridor with the hybrid SCDF takes roughly $5-6$ [s]. Robustness & performance: The times to reach the goal for different level of uncertainty is presented in the middle of Figure 3. None of the methods resulted in trajectories that were in collision. The nominal MPC is not included in the figure since it resulted in infeasibility issues in each run due to the model errors. This shows the importance of including robustness in the design. For the robust methods, we see an intuitive trend, larger uncertainty yields longer time to reach the goal. This is a design feature to ensure feasible and safe motion. For our method, this behavior is directly controlled by the constants in (11)-(13), which become larger as the uncertainty is increased. Comparing the performance between the robust methods, we see that for lower uncertainties, the methods perform basically the same, but with increasing uncertainties, the tightening becomes too conservative for the rigid rube. Our method shows superior scaling capabilities compared to the rigid tube, being able to scale to more than $3$ times higher level of uncertainty.

Online complexity: Assigning the balls and computing the virtual goal was observed to take max 1 [ms]. The main computational bottleneck is in solving the MPC problem. We present the statistics of the solvers computation times in the right part of Figure 3. Observing the plot, we see that solving the nominal MPC is done very fast, with a max time of roughly $15$ [ms]. Naturally, adding additional robustness features adds more computation time, roughly a factor $2$. We observe that our flexible tube has a max computation time below $33$ [ms], which therefore fulfills our real-time requirement, since we take $n_{a}=4$ time steps with the auxiliary controller in the inner loop.

VIII Conclusions

We have presented a novel convex robust motion planning solution for manipulators that gives robustness guarantees to bounded model errors and results in collision-free motion. One of the main benefits is that we derived a convex optimization problem, which can be solved fast and reliably. From the numerical experiments, we observed that a robust design of the MPC is necessary to maintain feasibility. Compared to a more standard robust MPC formulation, our approach was less conservative and scaled to over three times larger levels of uncertainty. For future work, we want to focus on validation in hardware experiments and data-driven estimation of model uncertainty.

-A Model error derivation

In the following, to increase readability, we drop the input arguments to the functions, e.g. $\textbf{M}=\textbf{M}(\textbf{q})$. From the manipulator dynamics (1) we obtain

[TABLE]

In the above, FL denotes feedback linearization. The $\star$ denotes the step where we decompose the matrix in front of the acceleration input into $\mathbf{I}+\tilde{\textbf{M}}_{\bm{\theta}}$ as follows

[TABLE]

The function $\Delta_{\bm{\theta}}(\textbf{q},\dot{\textbf{q}},\textbf{a}):\mathbb{R}^{n_{x}}\times\mathbb{R}^{n_{c}}\mapsto\mathbb{R}^{n_{c}}$ defines the state and input dependent model error on the following form

[TABLE]

-B Auxiliary controller and Lyapunov matrix

We present the optimization problem to compute $\textbf{P},\textbf{K}$ in Appendix -B1, and derive it in Appendix -B2. How we selected a suitable pair P and K in the experiments is presented in Appendix -B3.

-B1 Convex optimization program

We solve the following semidefinite program to produce the gain K and Lyapunov matrix P:

[TABLE]

with the objective function defined as

[TABLE]

The gain and Lyapunov matrix is computed from the following relationship $\textbf{E}=\textbf{P}^{-1}$ and $\textbf{Y}=\textbf{K}\textbf{E}$. The decision variables $c_{x,i}^{2},i\in\mathbb{N}_{1:m}$ and $c_{u,j}^{2},j\in\mathbb{N}_{1:n}$, control the amount of tightening on the state and control constraints, respectively. In this context, the set $\mathcal{W}$ is a box set of the model error, including both the model error due to model uncertainty (5), and the discretization error (V-A). Here $\text{vertex}(\cdot)$ corresponds to the vertices of this box. The inputs are the state constraints, $\textbf{A}_{x}\in\mathbb{R}^{m\times n_{x}}$ and $\textbf{b}_{x}\in\mathbb{R}^{m}$, the control input constraints, $\textbf{A}_{u}\in\mathbb{R}^{n\times n_{c}}$ and $\textbf{b}_{u}\in\mathbb{R}^{n}$, and the contraction rate, $\rho\in(0,1)$. We assume polytopic constraints, represented in its half-plane form, i.e. for the state constraints the form is

[TABLE]

where $[\textbf{A}_{x}]_{i}$ is the $i$:th row of the matrix $\textbf{A}_{x}$ and $b_{x,i}$ is the $i$:th element of $\textbf{b}_{x}$.

-B2 Derivation of optimization problem

Our goal is to derive an optimization problem where the amount of tightening is included in the cost, thereby allowing us to reduce the conservatism in the tightening.

In order to achieve this, we first have to derive an expression for the tightened state and input constraints. Then, we present LMI’s which allows us to include the tightening in the optimization. This results in a non-convex objective which we as a last step convexify, ending up with our proposed optimization problem.

First, the following linear matrix inequalities (LMIs) are a standard reformulation of inequality (8) (cf. [3]):

[TABLE]

The worst case disturbance is defined as

[TABLE]

A robust positive invariant (RPI) set can be computed that contains the worst case disturbance, having the following tube size

[TABLE]

The rigid tube MPC in Section VII-A4 uses the above tube size to tighten its constraints.

Next, we focus on how to introduce the tightenings into the optimization problem. We start by deriving an expression for the state constraint tightening. We require that around a reference state, $\bar{\textbf{x}}$, the tube with size $\delta$ should not violate the state constraints. That is, for each $i\in\mathbb{N}_{1:m}$, we want

[TABLE]

The error is defined as $\textbf{e}=\textbf{x}-\bar{\textbf{x}}$. We introduce

[TABLE]

Now, we input the above into the condition of (38) resulting in

[TABLE]

The worst case error maximizer is the following

[TABLE]

Thus, the tightened constraints become

[TABLE]

and we define the tightening constants as

[TABLE]

Next, we focus on the control input tightening. Using the control law in (7), $\textbf{a}=\bar{\textbf{a}}+\textbf{K}(\textbf{x}-\bar{\textbf{x}})$, into the above we get

[TABLE]

This is on the same form as for the state constraints. Thus, following the same approach, the tightening constant can be expressed as

[TABLE]

Having introduced the expressions for both the state and control input tightening, we now address how to include them into the optimization problem. We start by re-writing our tightenings. Focusing on the state inputs, for $i\in\mathbb{N}_{1:m}$ then (43) and (37) defines our tightening, which we express as

[TABLE]

and split into two inequalities

[TABLE]

where $\textbf{E}=\textbf{P}^{-1}$. We rewrite the above into LMI’s using the Schur complement [8], ending up with the LMI’s in (32c) and (32e). The control input tightenings follows the same reasoning. Now, we include all the state and control input tightenings in the objective, resulting in

[TABLE]

The bi-linear terms makes the cost non-convex. To make it convex we use the inequality of the arithmetic and geometric means [8], which results in

[TABLE]

ending up with the loss in (-B1).

-B3 Candidate selection

We normalized the objective function by dividing the constraint tightenings of the configuration, velocity and control with a representative normalizing factor. For the configuration tightening, we used $0.1$ [rad], which was the padded clearance added in the path planning. For the velocity and control tightening, they were divided with their corresponding max values from the constraints, i.e. $2$ [rad/s] and $20$ [rad/s2], respectively.

We compute $20$ values of $\rho$, equally spaced between 0.8 and 0.99. Having solved the optimization problem for all values of $\rho$ resulted in $20$ pairs of K and P. First, all pairs where $\tilde{\rho}\geq 1$ was satisfied were removed. From the remaining pairs, we selected the pair that resulted in the smallest max tightening. For the rigid tube MPC, the condition $\tilde{\rho}\geq 1$ was ignored, otherwise the same selection rule was used.

-C Proof of Proposition 2

It holds that

[TABLE]

with $\textbf{A}_{\text{cl}}=\textbf{A}+\textbf{B}\textbf{K}$. The uncertainty bound $\beta$ satisfies

[TABLE]

Combining both bounds yields

[TABLE]

-D Tube in ball constraint

To compute the tightened ball constraints in (21), we compute a ball that over-approximates the projection of the tube on the configuration space:

[TABLE]

with a suitable radius $r_{p}>0$. To ensure the above, we start by projecting $\mathcal{E}$ onto the configuration space. The Lyapunov matrix is structured as

[TABLE]

The projection of the ellipsoid onto the configuration space, $\mathcal{E}_{\textbf{q}}$, is done with the Schur-complement

[TABLE]

The eigenvalues give the principal axes of the resulting ellipsoid. We compute a radius that encompasses the projected ellipsoid as

[TABLE]

Now, to fulfill condition (52), we simply shrink the given ball’s radius, $r$, by $r_{p}\delta$. Thus our homothetic constraint tightening becomes

[TABLE]

-E Proof of Theorem 2

The proof follows the same steps as Theorem 1 and we only highlight the differences related to the added and updated ball constraints (21) and the virtual goal (26).

Part I: The system starts at steady-state, the allocation rule (25) picks balls with largest margin around the initial trajectory. Note that the corridor satisfies (24), which also ensures that the ball $\mathcal{B}$ around the initial condition is larger than $\mathcal{E}_{\textbf{q}}(2\epsilon+\delta_{f})$. Hence, a feasible solution to Problem (18) with the added obstacles avoidance constraints (21) is given by staying at the steady-state with $\textbf{a}=\textbf{0}_{n_{c}}$, i.e., we are guaranteed that the MPC problem is feasible at the start.

Next, we show recursive feasibility. Analogous to the proof of Theorem 1, the candidate solution is given by shifting the previous optimal solution by $n_{a}$ steps. Compared to Theorem 1, we need to account for the change in the initial condition (23) and the added obstacle avoidance constraints (21). For the initial constraint (23), the shifted candidate solution $\delta_{0}=\delta^{\star}_{n_{a}}$, $\bar{\textbf{x}}_{0}=\bar{\textbf{x}}^{\star}_{n_{a}}$ also provides a feasible solution. In particular, Proposition 2 also ensures that $\hat{\delta}_{0}\leq\delta^{\star}_{0}$ and monotonicity of (18e) ensures $\hat{\delta}_{n_{a}}\leq\delta^{\star}_{n_{a}}$.

Regarding the added ball constraints (21): If we would simply shift the previously allocated balls, i.e., $\mathcal{B}_{i}\leftarrow\mathcal{B}_{i+n_{a}}$, then the fact that the candidate sequence is equally shifted compared to the previous feasible solution, ensures feasibility of the candidate sequence, The ball assignment (25) is such that the distance to the boundary of the ball is non-decreasing and thus (21) is also feasible with the new assigned balls.

Part II: For all $i\in\mathbb{N}_{0:n_{a}-1}$, $\left\lVert\textbf{x}(k+i)-\bar{\textbf{x}}^{\star}_{i}\right\rVert_{\textbf{P}}\leq\delta_{i}^{\star}$ due to the initial constraint (22) and the tube construction (18e). Hence, the tightened constraints (18f), (18g), and (21) ensure $\textbf{q}(k)\in\mathcal{B}$, $\textbf{x}(k)\in\mathcal{X}$, $\textbf{a}(k)\in\mathcal{A}$. Condition (3) ensures $\textbf{u}(k)\in\mathcal{U}$, i.e., the torque limits are respected at each discrete time. The construction of the balls (20) with the SCDF (19) ensures $\textbf{q}(k)\in\mathcal{C}_{\text{f}}$, i.e., the robot operates in a collision-free configuration,

Part III: We need to ensure that $\tilde{\textbf{x}}_{\text{g}}$ converges to $\textbf{x}_{\text{g}}$ in finite time. For contradiction, suppose that the intermediate goal $\tilde{\textbf{x}}_{\text{g}}=\textbf{c}_{i}$ is not updated for some arbitrarily long time. Then, $\tilde{\textbf{x}}_{\text{g}}-\bar{\textbf{x}}_{0}^{\star}$ converges exponentially to zero, analogous to the proof of Theorem 1, given that $\tilde{\textbf{c}}_{\text{g}}\in(\mathcal{C}\cap\mathcal{B}_{H})\ominus\mathcal{E}_{\textbf{q}}(\epsilon+\delta_{f})$. Thus, in finite time, $\left\lVert\bar{\textbf{x}}_{H}^{\star}-\tilde{\textbf{x}}_{\text{g}}\right\rVert_{\textbf{P}}\leq\epsilon$, for any $\epsilon>0$. From (24), we know that $\textbf{c}_{i+1}\in\mathcal{B}(\textbf{c}_{i})\ominus\mathcal{E}_{\textbf{q}}(2\epsilon+\delta_{f})$. The ball selection (25) for $\mathcal{B}_{H}$ maximizes the distance around $\bar{\textbf{x}}_{H}$, which in combination with $\left\lVert\textbf{c}_{i}-\bar{\textbf{x}}_{H}\right\rVert_{\textbf{P}}\leq\epsilon$ ensures that $\textbf{c}_{i+1}\in\mathcal{B}_{H}\ominus\mathcal{E}_{\textbf{q}}(\epsilon+\delta_{f})$. Hence, $\textbf{c}_{i+1}$ satisfies condition (26), ensuring that the virtual goal is updated. Thus, $\tilde{\textbf{x}}_{\text{g}}$ is updated in finite time. Given the finite number of possible virtual goals based on the discretization, $\tilde{\textbf{x}}_{\text{g}}=\textbf{x}_{\text{g}}$ in finite time. Convergence of $\textbf{x}(k)$ to the neighborhood of the goal follows analogous to Theorem 1.