InPTC: Integrated Planning and Tube-Following Control for Prescribed-Time Collision-Free Navigation of Wheeled Mobile Robots

In this paper, we consider a nonholonomic wheeled mobile robot (WMR) with a control point $\bar{\bm{P}}$ located at the midpoint of the axis connecting the two driving wheels, as depicted in Fig. 1. Let $\bar{\mathbf{x}}:=[\bar{x},\bar{y}]^{\top}\in\mathbb{R}^{2}$ be the position vector of $\bar{\bm{P}}$ in the global coordinate frame, and $\bar{\theta}$ be the WMR’s heading angle. The kinematic model of the WMR is expressed as

where $\mathbf{u}:=[v,\omega]^{\top}\in\mathbb{R}^{2}$ is the the control input vector with $v$ and $\omega$ being the linear and angular velocities of the WMR, respectively, and $\mathbf{u}_{d}\in\mathbb{R}^{2}$ is the matched disturbance that may arise from sensor noise, imperfect wheel alignment, and motor or actuator asymmetry.

To facilitate the subsequent design and analysis, we select an off-axis point $\bm{P}$ as the virtual control point, which locates at a distance $\ell\neq 0$ away from $\bar{\bm{P}}$ along the longitudinal axis of the WMR [26], as shown in Fig. 1. Denote by $\mathbf{x}:=[x,y]^{\top}\in\mathbb{R}^{2}$ and $\theta$ the position and heading angle of $\bm{P}$, respectively. We have the following change of coordinates:

In view of (1) and (2), the kinematics of $\mathbf{x}$ is given by

where $\mathbf{R}(\theta):=[\cos\theta,-\ell\sin\theta;\sin\theta,\ell\cos\theta]$ is full rank. This enables us to freely steer the WMR’s position regardless of nonholonomic constraints. By setting $|\ell|\leq 1$, it follows that $\|\mathbf{R}(\theta)\|=\max\{1,|\ell|\}=1$.

Consider a WMR operating inside a closed compact convex workspace $\mathcal{W}^{\ast}\subset\mathbb{R}^{2}$, punctured by a set of $n$ static obstacles $\mathcal{O}_{i}^{\ast}$, $i\in\mathbb{I}:=\{1,2,...,n\}$, which are represented by open balls with centers $\mathbf{c}_{i}\in\mathbb{R}^{2}$ and radii $r_{i}>0$. A circumscribed circle centered at $\bm{P}$ with radius $r>0$ is constructed, which is the smallest circle enclosing the WMR (see Fig. 1). To ensure that the WMR can navigate freely between any of the obstacles in $\mathcal{W}^{\ast}$, we make the following assumption [4, 20]:

For ease of design, the WMR is considered as a point (i.e., the off-axis point $\bm{P}$), by transferring the volume of the circumscribed circle to the other workspace entities. For the point $\bm{P}$, the workspace is $\mathcal{W}:=\{\mathbf{x}\in\mathbb{R}^{2}\mid\text{d}_{\complement\mathcal{W}^{\ast}}(\mathbf{x})\geq r\}$, and the obstacle regions are $\mathbb{R}^{2}$: $\mathcal{O}_{i}:=\{\mathbf{x}\in\mathbb{R}^{2}\mid\beta_{i}(\mathbf{x})<0\},~i\in\mathbb{I}$, where $\beta_{i}(\mathbf{x}):=\|\mathbf{x}-\mathbf{c}_{i}\|-(r+r_{i})$. Thus, the free space of $\bm{P}$ is given by the closed set $\mathcal{X}:=\mathcal{W}\setminus\mathcal{O}$ with $\mathcal{O}:=\cup_{i=1}^{n}\mathcal{O}_{i}$. To enhance the safety, a safety margin of size $0<\epsilon<h$ is introduced (see the light blue regions in Fig. 2), which results in an eroded workspace $\mathcal{W}^{\epsilon}:=\{\mathbf{x}\in\mathbb{R}^{2}\mid\text{d}_{\complement\mathcal{W}^{\ast}}(\mathbf{x})\geq r+\epsilon\}$ and $n$ augmented obstacle regions $\mathcal{O}_{i}^{\epsilon}:=\{\mathbf{x}\in\mathbb{R}^{2}\mid\beta_{i}(\mathbf{x})<\epsilon\}$, $i\in\mathbb{I}$. Then, the free space of $\bm{P}$ reduces to $\mathcal{X}_{\epsilon}:=\mathcal{W}^{\epsilon}\setminus\mathcal{O}^{\epsilon}$ with $\mathcal{O}^{\epsilon}:=\cup_{i=1}^{n}\mathcal{O}_{i}^{\epsilon}$.

The robot navigation problem is formulated as follows:

The tangent cone $\mathbf{T}_{\mathcal{F}}(\mathbf{x})$ is a set that contains all the vectors pointing from $\mathbf{x}$ inside or tangent to $\mathcal{F}$, while for $\mathbf{x}\notin\mathcal{F}$, $\mathbf{T}_{\mathcal{F}}(\mathbf{x})=\emptyset$. Since for all $\mathbf{x}\in\text{int}(\mathcal{F})$, we have $\mathbf{T}_{\mathcal{F}}(\mathbf{x})=\mathbb{R}^{n}$, the tangent cone $\mathbf{T}_{\mathcal{F}}(\mathbf{x})$ is non-trivial only on the boundary $\partial\mathcal{F}$. Next, we recall the Nagumo’s invariance theorem.

A TSTF squeezes the infinite-time interval $s\in[0,\infty)$ into a prescribed finite time interval $t:=\eta(s)\in[0,T)$, which plays a key role for achieving prescribed-time planning and control. In this work, $\eta(s)$ is of the form

We neglect the disturbance $\mathbf{u}_{d}$ in (3) and consider a control law $\mathbf{u}=\mathbf{R}^{-1}(\theta)\bm{\tau}$, where $\bm{\tau}\in\mathbb{R}^{2}$ is a virtual control law to be designed. Then, substituting it into (3), one gets

A tangent cone based control policy (vector field) $\bm{\tau}=\mathbf{h}(\mathbf{x})$ is designed to achieve collision-free path planning. According to Theorem 1, we consider a nearest point problem

where $\bm{\kappa}_{0}(\mathbf{x})$ is a nominal control law for motion-to-goal and is designed here as

with $k_{0}>0$ being a constant.

The robot workspace $\mathcal{W}^{\ast}$ is a convex set, so does $\mathcal{W}^{\epsilon}$, which suggests that for all $\mathbf{x}\in\partial\mathcal{W}^{\epsilon}$, $\bm{\kappa}_{0}(\mathbf{x})$ points inside the free space $\mathcal{X}_{\epsilon}$ (i.e., $\bm{\kappa}_{0}(\mathbf{x})\in\mathbf{T}_{\mathcal{{X}}_{\epsilon}}(\mathbf{x})$) and thus is a solution to (8). Moreover, for all $\mathbf{x}\in\text{int}(\mathcal{X}_{\epsilon})$, $\bm{\kappa}_{0}(\mathbf{x})$ is also the solution to (8). Next we check the obstacle boundary points $\mathbf{x}\in\partial\mathcal{O}^{\epsilon}$. As $\partial\mathcal{O}^{\epsilon}$ is a smooth hypersurface of $\mathbb{R}^{2}$ and is orientable, there exists a continuously differentiable map (known as Gauss map [30]) $\mathbf{n}:\partial\mathcal{O}^{\epsilon}\to\mathbb{S}^{1}$ such that for all $\mathbf{x}\in\partial\mathcal{O}^{\epsilon}$, $\mathbf{n}(\mathbf{x})$ is the outward unit normal vector to $\partial\mathcal{O}^{\epsilon}$. As clearly seen in Fig. 2, the tangent cone at any $\mathbf{x}\in\partial\mathcal{O}^{\epsilon}$ is a half-space $\mathbf{T}_{\mathcal{X}_{\epsilon}}(\mathbf{x}):=\{\mathbf{y}\in\mathbb{R}^{2}\mid(\mathbf{y}-\mathbf{x})^{\top}\mathbf{n}(\mathbf{x})\leq 0\}$, which is a convex function. Thus, (8) has a unique solution. In summary, if $\mathbf{x}\in\mathcal{X}_{\epsilon}\setminus\partial\mathcal{O}^{\epsilon}$ or $\mathbf{x}\in\partial\mathcal{O}^{\epsilon}\wedge\bm{\kappa}_{0}^{\top}(\mathbf{x})\mathbf{n}(\mathbf{x})\leq 0$, then $\bm{\kappa}_{0}(\mathbf{x})$ is a solution to (8). While if $\mathbf{x}\in\mathcal{O}^{\epsilon}\wedge\bm{\kappa}_{0}^{\top}(\mathbf{x})\mathbf{n}(\mathbf{x})>0$, the closest point of (8) is obtained by the orthogonal projection $\mathbf{Q}(\mathbf{n}(\mathbf{x})):=\mathbf{I}_{2}-\mathbf{n}(\mathbf{x})\mathbf{n}^{\top}(\mathbf{x})$ onto the tangent hyperplane of $\partial\mathcal{X}_{\epsilon}$, defined by $\mathbf{H}_{\partial\mathcal{X}_{\epsilon}}(\mathbf{x}):=\{\mathbf{y}\in\mathbb{R}^{2}\mid(\mathbf{y}-\mathbf{x})^{\top}\mathbf{n}(\mathbf{x})=0\}$. Then, a general solution to (8) is as follows: $\mathbf{h}(\mathbf{x})=\bm{\kappa}_{0}(\mathbf{x})$ if $\text{d}_{\mathcal{O}}(\mathbf{x})>\epsilon$ or $\bm{\kappa}_{0}^{\top}(\mathbf{x})\mathbf{n}(\mathbf{x})\leq 0$, and $\mathbf{h}(\mathbf{x})=\mathbf{Q}(\mathbf{n}(\mathbf{x}))\bm{\kappa}_{0}(\mathbf{x})$ if $\text{d}_{\mathcal{O}}(\mathbf{x})=\epsilon\wedge\bm{\kappa}_{0}^{\top}(\mathbf{x})\mathbf{n}(\mathbf{x})>0$. The resulting vector field is discontinuous at some boundary points $\mathbf{x}\in\partial\mathcal{O}^{\epsilon}$ like the green points in Fig. 2. To address this issue, a continuous control law is proposed in the sequel. Following the line of [4], we specify an influence region for each obstacle (marked by the dashed line in Fig. 2), which is defined as $\{\mathbf{x}\in\mathbb{R}^{2}\mid\text{d}_{\mathcal{O}_{i}}(\mathbf{x})\leq\epsilon^{\ast}\}$, where $\epsilon^{\ast}\in(\epsilon,h]$ and $i\in\mathbb{I}$. Obstacle avoidance is activated only when the position $\mathbf{x}$ of $\bm{P}$ enters the influence regions of obstacles. To proceed, a bearing vector is defined as

where $\mathbf{P}_{\partial\mathcal{O}}(\mathbf{x}):=\{\mathbf{y}\in\partial\mathcal{O}\mid\|\mathbf{y}-\mathbf{x}\|=\text{d}_{\mathcal{O}}(\mathbf{x})\}$ is a set-valued map. Since $\text{d}(\mathcal{O}_{i}^{\ast},\mathcal{O}_{j}^{\ast})>2(r+h)$, $\forall i,j\in\mathbb{I}$, $i\neq j$ (referring to Assumption 2) and $\epsilon^{\ast}\leq h$, there can be only one obstacle $\mathcal{O}_{i}$ such that $\text{d}_{\mathcal{O}}(\mathbf{x})=\text{d}_{\mathcal{O}_{i}}(\mathbf{x})\leq\epsilon^{\ast}$. With this in mind, $\mathbf{b}(\mathbf{x})$ in (10) can be computed by $\mathbf{b}(\mathbf{x})=(\mathbf{c}_{i}-\mathbf{x})/\|\mathbf{c}_{i}-\mathbf{x}\|$. Note that when $\mathbf{x}\in\partial\mathcal{O}_{\epsilon}$, $\mathbf{b}(\mathbf{x})$ is equivalent to the Gauss map $\mathbf{n}(\mathbf{x})$. Now the control law is modified as

with

where $\phi(\text{d}_{\mathcal{O}}(\mathbf{x}))\in\mathbb{R}$ is a $C^{1}$ bump function that smoothly transitions from $1$ to $0$ on the interval $\text{d}_{\mathcal{O}}(\mathbf{x})\in[\epsilon,\epsilon^{\ast}]$. A simple choice of $\phi(\text{d}_{\mathcal{O}}(\mathbf{x}))$ is

where $\lambda(\text{d}_{\mathcal{O}}(\mathbf{x}))=0.5[1-\cos(\pi(\epsilon^{\ast}-\text{d}_{\mathcal{O}}(\mathbf{x}))/(\epsilon^{\ast}-\epsilon))]$. It is clear that the modified control law (11) is continuous and piecewise continuously differentiable on the domain $\mathcal{X}$.

Proof. The proof is relegated to Appendix. $\hfill\blacksquare$

To further achieve prescribed-time path planning, the TSTF $t:=\eta(s)$ defined in (6) is introduced to squeeze and map the collision-free trajectory of (7) from an infinite time interval $s\in[0,\infty)$ to a prescribed finite time interval $t\in[0,T)$. Let $\eta^{\prime}(s):=d\eta(s)/ds$ and define a prescribed-time gain function (PTGF) $\alpha:\mathbb{R}_{\geq 0}\to\mathbb{R}_{>0}$ as

showing that $\alpha(t)>0$ is continuously differentiable on $t\in[0,T)$ and satisfies $\alpha(0)=1$ and $\lim_{t\to T}\alpha(t)=\infty$. Here the prescribed-time control law is designed as $\bm{\tau}=\mathbf{h}^{\ast}(\mathbf{x},t)$, where $\mathbf{h}^{\ast}$ is given by

with $\mathbf{h}(\mathbf{x})$ and $\alpha(t)$ defined in (11) and (14), respectively.

Proof: The proof is conducted over two time intervals.

1) Consider the interval $t\in[0,T)$. The closed-loop system is written as

Let $\bar{\mathbf{x}}(s):=\mathbf{x}(t)=\mathbf{x}(\eta(s))$. Then, the system (16) is rewritten in the stretched infinite-time interval $s\in[0,\infty)$ as

In view of 14 and (16), 17 becomes

The initial conditions satisfy $\bar{\mathbf{x}}^{\prime}(0)=\dot{\mathbf{x}}(0)$ and $\bar{\mathbf{x}}(0)=\mathbf{x}(0)$, where the facts that $\eta(0)=0$ and $\alpha(0)=1$ have been used. As (18) has the same solution as the closed-loop system $\dot{\mathbf{x}}(t)=\mathbf{h}(\mathbf{x}(t))$, it follows from Theorem 2 that $\bar{\mathbf{x}}(s)$ asymptotically converges to the set $\mathcal{C}$. This implies that $\mathbf{x}(t)$ converges to the set $\mathcal{C}$ at the prescribed time $T$, due to $\mathbf{x}(t)=\bar{\mathbf{x}}(s)$ and $t\to T$ as $s\to\infty$. Additionally, Theorem 2 ensures that $\bar{\mathbf{x}}(s)$ is collision-free on $t\in[0,\infty)$. As a result, $\mathbf{x}(t)$ is also collision-free on $t\in[0,T)$.

2) Let us consider the interval $t\in[T,\infty)$, on which the closed-loop system becomes $\dot{\mathbf{x}}(t)=\mathbf{h}(\mathbf{x}(t))$. It can be verified that $\mathbf{h}(\mathbf{x}(t))=\mathbf{0}$ for all $t\geq T$, no matter $\mathbf{x}(t)\to\mathbf{x}^{\ast}$ or $\mathbf{x}(t)\to\bm{s}_{i}$ as $t\to T$, indicating that the system states remain unchanged. This completes the proof. $\hfill\blacksquare$

To avoid conflicts of notations, the reference trajectory is denoted as $\mathbf{x}_{d}$, which is governed by $\dot{\mathbf{x}}_{d}=\bm{\tau}_{d}$, where $\bm{\tau}_{d}$ is given by (19) but with $\mathbf{x}$ replaced by $\mathbf{x}_{d}$. Define the position tracking error as $\mathbf{x}_{e}:=\mathbf{x}-\mathbf{x}_{d}$. It follows from (3) that

To ensure the tracking safety, we impose the following “tube” constraint on $\mathbf{x}_{e}$

where $\rho>0$ is a user-defined constant. At each time instance, the actual position $\mathbf{x}$ of the virtual control point $\bm{P}$ is allowed to stay within a 1-sphere of radius $\rho$ and center $\mathbf{x}_{d}(t)$. Unifying all such spheres along $t$ forms a tube around $\mathbf{x}_{d}(t)$, as shown in Fig. 5. Since a safety margin is considered in the planning module, if the tube radius is taken no larger than the size $\epsilon$ of the safety margin and (22) holds for all $t\geq 0$, then the actual trajectory $\mathbf{x}(t)$ remains within the safe tube without colliding with any obstacles, that is, $\mathbf{x}(t)\in\mathcal{X}$, $\forall t\geq 0$.

In the following, a tube-following controller is developed to achieve prescribed-time trajectory tracking, while ensuring that $\mathbf{x}(t)$ evolves within the tube defined by (22). To this end, we define a transformed error

Taking the time derivative of (23), whilst using (3) and (21), one can easily get

Inspecting (23) reveals that $0\leq\xi(t)<1$ is equivalent to (22), and $\xi(t)=0$ only when $\mathbf{x}_{e}(t)=\mathbf{0}$. Therefore, the prescribed-time tube-following control problem boils down to achieving $\lim_{t\to T_{f}}\xi(t)=0$, while ensuring $0\leq\xi(t)<1$, $\forall t\geq 0$, via a properly-designed controller.

To achieve trajectory tracking within a prescribed finite time $T_{f}>0$, as per Definition 2, we introduce a TSTF $t:=\eta_{f}(s)=T_{f}(1-e^{-s/T_{f}})$, which generates the following PTGF:

Let us define $\mathbf{z}:=\mathbf{x}_{e}/(\rho^{2}(1-\xi))$ and design the prescribed-time control law as

with

where $k_{1},k_{2}>0$ are constant gains, and $\alpha_{f}^{\ast}(t)$ is a continuous PTGF akin to $\alpha^{\ast}(t)$ in (20), given by

with $T_{f}^{\ast}:=T_{f}-\varsigma_{f}>0$ and $0<\varsigma_{f}\ll T_{f}$ being constants.

Proof. We analyze the closed-loop behavior over two time intervals $[0,T_{f}^{\ast})$ and $[T_{f}^{\ast},\infty)$, separately.

1) We first consider the time interval $t\in[0,T_{f}^{\ast})$. Substituting (26) into (21) yields

To proceed, let us define

Further define $\bar{\mathbf{d}}(s):=\mathbf{R}(\theta(\eta_{f}(s)))\mathbf{u}_{d}(\eta_{f}(s))$ for notational concision. Then, the closed-loop system (29) can be rewritten in the stretched infinite time interval $s\in[0,\infty)$ as

with $\bar{\mathbf{x}}_{e}(0)=\mathbf{x}_{e}(0)$ and $\bar{\mathbf{x}}_{e}^{\prime}(0)=\dot{\mathbf{x}}_{e}(0)$.

Consider the following logarithmic barrier function

Taking the derivative of $L$ w.r.t. $s$ along (III-C) yields

From [31, Lemma 3], it follows that $-k_{1}\bar{\mathbf{z}}^{\top}\bar{\mathbf{x}}_{e}=-k_{1}\bar{\xi}/(1-\bar{\xi})\leq-k_{1}L$ holds for all $0\leq\bar{\xi}<1$. In addition, recalling Assumption 1 and the fact that $\|\mathbf{R}(\theta)\|=1$, we have $\bar{\mathbf{z}}^{\top}\bar{\mathbf{d}}\leq k_{2}\|\bar{\mathbf{z}}\|^{2}+d/(4k_{2})$. As a result, (33) reduces to

on the set $0\leq\bar{\xi}<1$. Integrating both sides of (III-C) leads to

where $D:=T_{f}d/(4k_{2}(k_{1}T_{f}-1))>0$ is a constant. From (35), it is clear that $L(\bar{\xi})$ is bounded, indicating that $0\leq\bar{\xi}(s)<1$ for all $s\geq 0$. As a result, $\|\bar{\mathbf{x}}_{e}(s)\|<\rho$ holds for all $s\geq 0$. Since $t\to T_{f}$ as $s\to\infty$, it follows from (30a) that $\mathbf{x}_{e}(t)$ keeps within the safe tube (22) for all $t\in[0,T_{f}^{\ast})$.

In the following, we show that $\mathbf{x}_{e}(t)$ converges to a small neighborhood of origin at $t=T_{f}^{\ast}$. It follows from (35) that $\lim_{s\to s^{\ast}}L(\bar{\xi}(s))\leq L^{\ast}:=L(\bar{\xi}(0))e^{-k_{1}s^{\ast}}+De^{-s^{\ast}/T_{f}}$. As per the definition of $\eta_{f}(s)$, one can easily deduce that $s\to s^{\ast}:=-T_{f}\ln(\varsigma_{f}/T_{f})$ as $t\to T_{f}^{\ast}$. Since $0<\varsigma_{f}\ll T_{f}$, $s^{\ast}$ is a very large value, which renders $L^{\ast}$ very small. By (23) and (32), we get $\lim_{s\to s^{\ast}}\|\bar{\mathbf{x}}_{e}(s)\|\leq\rho\sqrt{1-e^{-2L^{\ast}}}$, which implies that $\bar{\mathbf{x}}_{e}$ converges to a small neighborhood of origin as $s\to s^{\ast}$. Since $t\to T_{f}^{\ast}$ as $s\to s^{\ast}$ and $\bar{\mathbf{x}}_{e}(s)=\mathbf{x}_{e}(\eta_{f}(s))$ (see (30a)), it can be claimed that $\lim_{t\to T_{f}^{\ast}}\|\mathbf{x}_{e}(t)\|\leq\rho\sqrt{1-e^{-2L^{\ast}}}$.

2) Consider the time interval $t\in[T_{f}^{\ast},\infty)$, in which $\alpha_{f}^{\ast}(t)=\alpha_{f}(T_{f}^{\ast})\equiv T_{f}/\varsigma_{f}$ and the closed-loop system becomes

We likewise consider the barrier function $L$ in (32). Taking its derivative w.r.t. $t$ along (36) gets