Set based velocity shaping for robotic manipulators

Planning of robotic manipulators has received much attention over the years with a wide range of approaches available [5]. The challenge lies in computing collision free paths in a nonconvex environment, and in a sufficiently fast time [6].

Established approaches to producing safe controllers aim to place constraints on speed, momentum and potential collision energies of the system in order to ensure safety [7, 8, 9]. These controllers rely on the trajectory planner found within the typical motion controller.

Trajectory planning, which is the focus of this article, is concerned with shaping the velocity profile as the robot moves from one configuration to another. Typically, the enforcement of hard constraints on the system dynamics is imposed on the velocity profile, see, e.g., [10, 11, 12, 13], where an optimal velocity profile is calculated. These optimal approaches have also been extended to account for other state dependent and time dependent constraints [14], [15].

Optimal control approaches, a version of which concerns time scaling algorithms, produce a single optimal trajectory fed to a robot to execute the desired motion. These algorithms have been successfully utilised in the last decades, however, they inevitably come with shortcomings, for example: They are discontinuous, they are restricted to kinodynamic planning problems that specify a unique acceptable velocity for the end of the path, and offer little ability to adapt the profile along the path, as most versions generate open-loop controllers.

To address these shortcomings, we investigate a larger class of state-feedback controllers, using the same reasoning for projecting the dynamics of the robotic manipulator on a prespecified path as in the traditional aforementioned approaches. However, instead of finding time-optimal solutions for the zero initial condition, we characterise the whole set of admissible states, namely, the distance traversed in the path and its pseudo velocity, that can reach a target set in finite time and satisfy constraints throughout.

By projecting the dynamics on a path, any $N$-degree of freedom (DOF) manipulator is represented by a constrained double integrator. The control input corresponds to the pseudo acceleration across the path and is subject to non linear and nonconvex constraints, induced mainly by the physical constraints on the torques of the actuators. At this stage additional constraints can be imposed, related, e.g., to potential collision energy, time constraints, or constraints relating to end effector forces that could prove useful in high precision applications, e.g., machining [9], [16].

Technically, rather than solving a single optimal control problem as, e.g., in [10, 11], our main objective is to answer the question: Can we compute the subset of states for which a state feedback controller exists driving the system to a target set without violating any constraints in finite time?

Sets with these properties are called reach-avoid sets [17] [18]. Many issues related to robots and path planning are often naturally expressed in the context of reach-avoid problems, examples include motion planning, collision avoidance, space vehicle docking, and object tracking applications [19, 20, 21, 22]. To the best of our knowledge, this is the first time it is suggested to cast the kinodynamic planning problem as a reach-avoid set computation.

Highly relevant for safety-critical systems, reach-avoid problems are not trivial. Efficient solutions depend on the nature of the system, with several techniques available for a variety of system types [23, 24, 25]. In our case, we are exploiting the monotonicity of the projected trajectories with respect to a novel parameterisation of the control input. This parameterisation allows ordering relations between generated closed-loop trajectories to be established in terms of both the initial conditions and the control input. These observations constitute the building blocks of an algorithmic procedure that recovers an approximation of the largest reach-avoid set. Compared to interesting related approaches [26], [27], our work deals directly with the nonlinear dynamics and constraints, and constructs the boundaries of the reach-avoid set using a combination of extreme trajectories. Thus, there is no added conservatism caused by the linearisation of dynamics.

We also establish set-based state feedback controllers, induced by the parameterisation of the control strategy. Taking into account the preference for continuous, smooth, and robust state feedback control reducing wear on actuators, we establish general properties the control template must satisfy, and show how simple, Lipschitz continuous, continuous, and sliding mode-like controllers can be established. Last, we apply our framework to numerical experiments on commercial robotic manipulators, and discuss practical considerations in algorithmic implementation to show the efficacy of our approach. Preliminary ideas on computing reach-avoid sets for the studied problem are reported in [28]. In comparison, we formally show continuity properties of the suggested control template, establish analytic algorithms on computations of the reach-avoid set, propose new families of stabilizing feedback controllers, highlight practical challenges in implementation, and perform a formal comparison of our results with the conventional time optimal control approach.

Section II discusses preliminaries. In Section III we introduce the problem and the control input parameterisation, and establish the Lipshitz properties of the closed-loop system dynamics. Section IV establishes ordering relations between generated trajectories, while section V illustrates in detail the algorithmic procedure for producing the reach-avoid set. Section VI deals with the characterisation and implementation of families of safe state feedback controllers. Section VII presents examples of the algorithms running on a Python based simulation using the model for the Universal Robots UR5 [29] available within the Matlab file exchange [30]. Conclusions are drawn in Section VIII. For ease of exposition the proofs are placed in the Appendix.

where $q\in\mathbb{R}^{N}$ corresponds to joint positions and $\tau\in\mathbb{R}^{N}$ collects the actuator torques for each joint. The matrices $M_{L}(q)\in\mathbb{R}^{N\times N}$, $C_{L}(q,\dot{q})\in\mathbb{R}^{N\times N}$ are the inertial and centrifugal/Coriolis matrices respectively. The gravitational force effects are represented by $g_{L}(q)$. The matrix and vector functions $M_{L}(q)$, $C_{L}(q)$ and $g_{L}(q)$ are globally Lipschitz, [31, 32, 33].

The Lagrangian dynamics can be projected to path dynamics: The first step is to consider an end effector path described by a function $q(s):[0,1]\rightarrow\mathbb{R}^{n}$ known as a path parameterisation where $s\in[0,1]$ is a scalar representing each configuration of the robot as it moves along the path.

Details can be found in the seminal papers [10, 11] and the book [34, Chapter 9].

Assumption 1 is not restrictive and is common in the literature [10, 11], [34]. Intuitively, it requires that the configuration must be able to change such that the end effector can traverse a path in finite time.

The parameterised system becomes $M(s)\ddot{s}+C(s)\dot{s}^{2}+g(s)=\tau$ where $M(s)\in\mathbb{R}^{N}$, $C(s)\in\mathbb{R}^{N}$, $g(s)\in\mathbb{R}^{N}$, are the transformed vectors, and $\tau\in\mathbb{R}^{N}$ describes the torques and forces produced by the actuators. We define $x=\begin{bmatrix}s&\dot{s}\end{bmatrix}^{T}=\begin{bmatrix}x_{1}&x_{2}\end{bmatrix}^{\top}$. The torques are limited by the actuator dynamics as $\tau^{\min}_{i}(x)\leq\tau_{i}(x)\leq\tau^{\max}_{i}(x)$ where $\tau^{\min}_{i}(x)$ and $\tau^{\max}_{i}(x)$ describe the maximum torque that can be generated in either direction. The $i^{th}$ element, $i=1,...,N$, of the vectors within the parameterised system generates constraints of the form

Assumption 2 is not restrictive for electrical motors due to their continuous relationships between torque and speed, see, e.g., [35] [36, Chapter 10].

Rearranging (1), the limits on possible accelerations $\dot{x}_{2}=\ddot{s}$ from any given state $x$ can be formed. We set $A_{i}(x)=\frac{\tau^{\min}_{i}(x)-C_{i}(x_{1})x_{2}^{2}-g_{i}(x_{1})}{M_{i}(x_{1})}$, $D_{i}(x)=\frac{\tau^{\max}_{i}(x)-C_{i}(x_{1})x_{2}^{2}-g_{i}(x_{1})}{M_{i}(x_{1})}$ for $M_{i}(x_{1})<0$, and $A_{i}(x)=\frac{\tau^{\max}_{i}(x)-C_{i}(x_{1})x_{2}^{2}-g_{i}(x_{1})}{M_{i}(x_{1})}$, $D_{i}(x)=\frac{\tau^{\min}_{i}(x)-C_{i}(x_{1})x_{2}^{2}-g_{i}(x_{1})}{M_{i}(x_{1})}$ for $M_{i}(x_{1})>0$. The case of zero inertia points, i.e., when $M_{i}(x_{1})=0$, for one or more indices $i=1,..,N$ is taken into account by posing algebraic constraints on $x$ [34] of the form $\tau^{\min}_{i}(x)\leq C_{i}(x_{1})x_{2}^{2}+g_{i}(x_{1})\leq\tau^{\max}_{i}(x).$ By grouping the above constraints, we can write

where

The projected system is a double integrator

with $\Phi=\begin{bmatrix}0&1\\ 0&0\end{bmatrix}$, $E=\begin{bmatrix}0\\ 1\end{bmatrix}$. The state feedback $u(\cdot):\mathbb{R}^{2}\rightarrow\mathbb{R}$ accounts for the acceleration $\ddot{s}$, whose admissible range is state-dependent (2). The solution of (5) at time $t$ starting from an initial condition $x_{0}$, under the control function $u(x)$ is

The state and input constraints are $x\in\mathcal{X}^{\prime},\ \ u(x)\in\mathcal{U}_{x},$ where $\mathcal{X}^{\prime}=\mathcal{C}\cap\mathcal{C}_{0}$, with $\mathcal{C}=\left\{x\in\mathbb{R}^{2}:B(x)\leq 0\right\}$, $B(x)=\begin{bmatrix}-x_{1}&x_{1}-1&-x_{2}&D(x)-A(x)\end{bmatrix}^{\top}$. Moreover, $\mathcal{C}_{0}=\{x\in\mathbb{R}^{2}:(\exists j\in[1,N]:M_{j}(x_{1})=0,\quad\tau^{min}_{j}-C_{j}(x)x_{2}^{2}-g_{j}(x)\leq 0,\quad-\tau^{max}_{j}+C_{j}(x)x_{2}^{2}+g_{j}(x)\leq 0)\}$, and $\mathcal{U}_{x}=\{u(x)\in\mathbb{R}:D(x)\leq u(x)\leq A(x)\}$. We note that within our framework we can incorporate additional constraints. We assume we can eventually describe all constraints via two functions $C^{u}(x_{1}):[0,1]\to\mathbb{R}$ and $C^{l}(x_{1}):[0,1]\to\mathbb{R}$, representing upper and lower bounds respectively. This leads to the description of the constraint set

The overall state (we note $\mathcal{X}\subseteq\mathcal{X}^{\prime})$ and input constraints are

We note Assumption 3 is indeed somewhat restrictive, as, for example, it does not allow islanding. Nevertheless, it covers many realistic and challenging cases, including polynomial and trigonometric constraints.

In this section, we propose a parameterisation of the state feedback controller as a convex combination of the state dependent input constraints. The generally non-restrictive Assumptions 1, 2 allow us to characterize the parameterisation with Lipschitz continuity, which constitutes our first main technical result. Preliminary results and the proof of Proposition 1 can be found in the Appendix.

In agreement with approaches in the literature [10], [34], we set $u(x)=\dot{x_{2}}$ as the input variable. We build all subsequent results in the following parameterisation of the control law via the actuation level function $\lambda(x):\mathcal{X}\rightarrow[0,1]$ as follows

The following result is a consequence of Proposition 1.

The forward and backward dynamics of the system are

respectively, with $\Phi$, $E$ defined in (5).

We consider target sets of the form

where $0<c\leq 1$ and $x^{\ast}_{u},x^{\ast}_{l}\in\mathbb{R}$. Given a particular choice of the actuation level function $\lambda(x)$ (9) and an initial condition $x_{0}\in\mathcal{X}$, the solution of (10) is $\phi_{\text{f}}(t;x_{0},\lambda(x))=e^{\Phi t}x_{0}+\int_{0}^{t}e^{\Phi(t-\tau)}Eu(x,\lambda(x))d\tau$ where $e^{\Phi t}=\begin{bmatrix}1&t\\ 0&1\end{bmatrix}$. Correspondingly, the solution of the backwards dynamics (11) is $\phi_{\text{b}}(t;x_{0},\lambda(x)))=e^{-\Phi t}x_{0}-\int_{0}^{t}e^{-\Phi(t-\tau)}Eu(x,\lambda(x))d\tau$.

We collect the admissible trajectories of the closed loop system in the forward and backwards direction in the sets

respectively. In the remainder, and to discuss trajectories without needing to specify the direction in which they are formed, we write $\mathcal{T}(x_{0},\lambda(x))$. We define intervals $\mathcal{I}=[(x_{a})_{1},(x_{b})_{1}]$ $(x_{a})_{1}\leq(x_{b})_{1}\leq 1$. The slice of a set $\mathcal{X}$ on an interval $\mathcal{I}$ is

The mapping (16) is valid also when $\mathcal{I}=\{x_{a}\}$ is a singleton. In this case, we write for simplicity $\mathcal{W}(\mathcal{X},x_{a})$.

A straightforward consequence of Lemma 2 is that for any two trajectories evolving from a single state with different constant actuation levels the one with the higher actuation level will remain above the other for the duration of the motion. Specifically, for any $x_{1}^{\star}\in[0,1]$, any $\lambda_{b}\leq\lambda_{a}\leq 1$ it holds that $x_{b}\leq x_{a}$, where $x_{a}=\mathcal{W}(\mathcal{X},x_{1}^{\ast})\cap\mathcal{T}_{f}(x_{0},\lambda_{a})$, $x_{b}=\mathcal{W}(\mathcal{X},x_{1}^{\ast})\cap\mathcal{T}_{f}(x_{0},\lambda_{b})$. This is summarised in the following corollary.

Moreover, Lemma 2 implies that in the case where $(x_{u})_{2}>(x_{l})_{2}$, we have $\mathcal{T}_{1}\cap\mathcal{T}_{2}=\emptyset$ for any two trajectories $\mathcal{T}_{1}=\mathcal{T}(x_{u},\lambda_{u}),\mathcal{T}_{2}=\mathcal{T}(x_{l},\lambda_{l})$, where $x_{u}$, $x_{l}$ lie on a slice $\mathcal{W}(\mathcal{X},x_{1})$ where $x_{1}\in[0,1]$ such that $x_{u}\neq x_{l}$ and $\lambda_{u}>\lambda_{l}$.

We first define the hypograph and epigraph of a set $\mathcal{Z}\subseteq\mathcal{X}$ respectively

We also define the set-valued operation $\mathcal{Q}(\cdot)$ that returns the ‘leftmost’ state of a trajectory $\mathcal{T}=\mathcal{T}(x_{0},\lambda(x))$ in $\mathcal{X}$

The sets representing the upper and lower boundaries of the $\mathcal{X}$ (7) are defined below

The results of Section IV lay the foundation for the development of the reach avoid set computation, outlined in Algrorithm 1. Initially, the two trajectories $T_{l}^{*}$ and $T_{u}^{*}$, produced from backward integration of the extreme points of $\mathcal{X}_{\text{T}}$, are computed in Lines 3 and 5 respectively. We denote with $x_{a}$ and $x_{d}$ the intersections of these trajectories with the constraint set (Lines 4, 6), with $x^{*}_{u}$, $x^{*}_{l}$ defining the interval $\mathcal{X}_{\text{T}}$ (12). Lines 7, l0 and 13 represent the different cases that can occur, depending on where $x_{d}$ and $x_{a}$ lie. Figure 2 illustrates all possible situations: Line 7 covers Figure 2(a), where $x_{d},x_{a}$ are in $\mathcal{V}^{l}$. In this case no additional computations are needed as $\mathcal{Z}_{u}=\mathcal{T}^{*}_{u}$. On the other hand, the lower bound needs to be extended to obtain $\mathcal{Z}_{l}$. Line 10 covers Figure 2(b). In this case, the lower bound is complete, however, the upper bound possibly needs to be modified. Lines 13-21 cover the remaining cases shown in Figures 2(c) - 2(f), that occur when $\mathcal{T}^{*}_{u}$ and $\mathcal{T}^{*}_{l}$ do not intersect the same boundary $\mathcal{V}^{u}$ or $\mathcal{V}^{l}$. For these instances, the extend operation is applied, which is described in detail below in Algorithm 2. After all the relevant extensions have been made Line 22 returns $\mathcal{R}_{\epsilon}(\mathcal{X}_{\text{T}})$ which is constructed as the intersection of the sets $\mathcal{R}_{l}(\mathcal{Z}_{u})$, and $\mathcal{R}_{u}(\mathcal{Z}_{l})$.

We note that the target set $\mathcal{X}_{\text{T}}$ is the only admissible exit facet, i.e., the area of the boundary where the trajectory $\mathcal{T}_{f}(x_{0},\lambda(x))$ will escape $\mathcal{R}_{\epsilon}(\mathcal{X}_{T})$ in finite time under appropriate control $\lambda(x)$. Several existing works on control-to-facet strategies focus on non-linear hybrid systems with sets defined by simplices or other simple polynomial descriptions of the set boundaries [37, 38, 39].