Finite-time obstacle avoidance for unicycle-like robot subject to additive input disturbances

Abstract

The problem of avoiding obstacles while navigating within an environment for a Unicycle-like wheeled mobile robot is of prime importance in robotics; the aim of this work is to solve such a problem proposing a perturbed version of the standard kinematic model able to compensate for the neglected dynamics of the robot. The disturbances are considered additive on the inputs and the solution is based on the supervisory control framework, finite-time stability and a robust multi-output regulation. The effectiveness of the solution is proved, supported by experiments and finally compared with the dynamic window approach to show how the proposed method can perform better than standard methods.

1 Introduction

Real-time obstacle avoidance for wheeled mobile robots (WMRs) is an extensively researched field in robotics due to an undeniable need for practical applications. In addition, a large variety of WMRs is subject to non-holonomic kinematic constraints; such robots (that cannot be stabilized by smooth time-invariant static state feedback as proven in Brockett 1983) cannot follow all trajectories at any time, making the task more challenging.

Several algorithms and methods have been proposed to tackle the problem depending on accessible informations. Global solutions operate off-line, giving suitable optimized paths between a starting point and a target; they perform better while no unexpected situations occur. However, these emerging situations may be handled by local path planners which react faster using real-time sensors data; the main issue with local planners comes from the non-optimal solution they could provide using just local information. Local solutions were typically based on Potential Field (PF) approaches derived from (Khatib 1985), in which the robot is driven along a potential field whose minimum is at the target position and each obstacle generates an additional repellent force. Within the local planners directly derived from the PF approach, the Vector Field Histogram (VFH) method firstly presented in Borenstein and Koren (1991) (see also its more recent modifications Ulrich and Borenstein 1998, 2000) represents a widely used solution to real-time obstacle avoidance. The first experiments run on WMR showed the shortcomings inherited after the PF approach: presence of traps and local minima (Koren and Borenstein 1991). Thus, modifications like VFH+ were proposed; this method, starting from a grid map, evaluates the PF at each iteration for a subset of active cells of the map, builds an obstacle histogram and reduces it to a polar form to finally compute the velocity commands. Alternative approaches were also presented, such as the Dynamic Window Approach (DWA) (Fox et al. 1997), a predictive technique in which the kinematics and the dynamics properties of the robot are taken into account and the control inputs are derived from what is called velocity space after an optimization step. As stated above, local planners cannot guarantee optimal results in robot navigation, and they are commonly coupled with global planners, examples for both VFH and DWA could be found in Ulrich and Borenstein (2000), Kiss and Tevesz (2011), Maroti et al. (2013). Ulrich and Borenstein (2000) proposes the VFH* approach, where the VFH method is used to real time obstacle avoidance and A* algorithm handles the global planning, while in Kiss and Tevesz (2011) a receding horizon control is applied to global DWA using global information to avoid local minima. For the sake of completeness we cite also the velocity obstacles (VO) method presented in Fiorini and Shillert (1998) used for the navigation in dynamic environment and its recent modification (Wilkie et al. 2009) which proposes the generalized VO including kinematic constraints of the considered robot.

In addition, to deal with model inaccuracies in literature various type of perturbations are considered; in Tayebi and Rachid (2000), for instance, a constant input additive disturbance is studied and compensated with an adaptive approach. In Danwei and Chang (2008) a most rigorous study is carried out where slipping and skidding behaviours are characterized and a study on the controllability is illustrated; (Rubagotti et al. 2011) uses the formulation of (Danwei and Chang 2008) and compensates the effects of matched and unmatched perturbations with an integral sliding mode approach. The contribution of this work is, thus, twofold. Firstly, the authors consider a new formulation of the unicycle model. In the considered representation, perturbations are added to the classic kinematic model to compensate for the neglected dynamics. These perturbations are additive to the inputs, that comes from the intuition that if no command is given to the WMR, then the robot will not move and, thus, no perturbations have to be considered independently of the control. Such a kind of disturbances could come from the settling time of the PID controller, that translates the velocity commands in current inputs and sends them to the motors. To the best of the authors’ knowledge such a model has not been considered before. In addition, a novel approach to local real-time obstacle avoidance for unicycle-like WMR is presented, which is proven to be, under appropriate assumptions, robust and finite-time convergent, as well as computationally low cost. In the proposed solution, inspired by Efimov et al. (2009), the controller and the kinematics of the WMR are strictly tied to guarantee the avoidance. A supervisory switching control is used to regulate two outputs: the first objective is the stabilization of the system by regulating the first output (distance from a target), the second objective is related to the proximity of eventual obstacles while trying to complete the main task. A supervisor has been designed to oversee the switches between the two components of the control starting from (Efimov et al. 2009) and relaxing it with no more need for dwell time and adding input disturbances. The result is presented taking into account the notions of stability for switched system (Liberzon 2003) and input-to-output stability (Sontag and Yuan 1999; Hespanha et al. 2003).

2 Theoretical formulation

Consider the following system (a WMR model):

$$\begin{aligned} \dot{x}=f(x,u,d),\; z_{1}=h_{1}(x),\; z_{2}=h_{2}(x), \end{aligned}$$

(1)

where $x\in \mathbb {R}^{n}$ is the state, $u\in \mathbb {R}^{m}$ is the control input and $d\in \mathbb {R}^{m}$ is a disturbance, the signal $d\in \Omega =\lbrace d\in \mathcal {L}_{m}^{\infty }:||d||\le D\rbrace$ for some $D\in \mathbb {R}_{+}$.

We want to regulate the outputs $z_{1}\in \mathbb {R}^{p_{1}}$ and $z_{2}\in \mathbb {R}^{p_{2}}$ assuming that the functions $h_{1}$ and $h_{2}$ are continuous with $f:\mathbb {R}^{n+2m}\rightarrow \mathbb {R}^{n}$. It is needed to design a control $u:\mathbb {R}^{n}\rightarrow \mathbb {R}^{m}$ that will provide the uOS ¹ property with respect to the output $z_{1}$, and will keep the second output $z_{2}$ in a predefined limit. In other words, to achieve the desired tasks it is needed that for all initial conditions $x_{0}\in \mathbb {R}^{n}$, $d\in \Omega$ and $t\ge t_{0}\ge 0$:

$$\begin{aligned}&|z_{1}(t,x_{0},d)|\le \beta _1(|h_{1}(x_{0})\vert ,t-t_{0}), \end{aligned}$$

(2)

$$\begin{aligned}&|z_{2}(t,x_{0},d)|\le \max \lbrace \sigma (\varDelta ),\beta _2(|h_2(x_0)|,t-t_0)\rbrace , \end{aligned}$$

(3)

the value of $\varDelta$ is given, $\beta$ and $\beta _1$ are $\mathcal {KL}$ ² functions whereas $\sigma$ is a function of class $\mathcal {K}$. It can be noted that (2) is exactly the definition of the uOS property. The second output must be smaller than $\sigma (\varDelta )$. In the case $|h_{2}(x_{0})|>\varDelta$ the trajectory should converge to a subset where $|h_{2}(x)|\le \sigma (\varDelta )$. In addition, to solve the problem we need that the intersection between the sets $h_{1}(x)=0$ and $|h_{2}(x)|\le \sigma (\varDelta )$ would not be empty, thus we assume the existence of a function $\rho$ of class $\mathcal {K}$ and a scalar $0<\rho _{0}<\sigma (\varDelta )$ such that: $|h_{2}(x)|\le \rho (|h_{1}(x)|)+\rho _{0}$.

2.1 Description of independent controls

Thus the problem consists in an output uniform stabilization under constraints imposed on another output. Following Efimov et al. (2009), assume that two right-continuous controls $u_{i}:\mathbb {R}^{n}\rightarrow \mathbb {R}^{m}$, $i\in \lbrace 1,2\rbrace$ are given solving an independent stabilization for the corresponding output $z_{i}$, i.e. the system

$$\begin{aligned} \dot{x}=f(x,u_{i}(x),d),\; z_{i}=h_{i}(x), \end{aligned}$$

is forward complete and has continuous solution $x(t,x_{0},d)$, in addition the system is uOS with respect to the output $z_{i}$ and disturbance $d\in \Omega$. We also assume that during an activation of $u_{2}$ for all $t\ge 0$

$$\begin{aligned} |z_{1}(t,x_{0},d)|\le |h_{1}(x_{0})|. \end{aligned}$$

Next subsection is devoted to uniting of these controls in order to solve the posed problem.

2.2 Supervisory control

Under the assumption of having two controls which solve the output regulation for $z_{1}$ and $z_{2}$ independently, a supervisor is proposed to oversee the activation of the controls to achieve both required condition (2) and (3) simultaneously. The idea is that the controller $u_{2}$ is activated when $|z_{2}(x)|$ reaches a threshold $\varDelta$ and remains active until the constraint $|z_{2}(x)|\le \delta$ is satisfied, where $0<\delta <\varDelta$ is a given parameter. For this reason we define two sets

$$\begin{aligned} \begin{array}{c} \mathbf {X}_{1}=\lbrace x\in \mathbb {R}^{n}:|h_{2}(x)|\le \delta \rbrace ,\\ \mathbf {X}_{2}=\lbrace x\in \mathbb {R}^{n}:|h_{2}(x)|\le \varDelta \rbrace ,\\ \mathbf {X}_{1}\subset \mathbf {X}_{2}. \end{array} \end{aligned}$$

Then the control

$$\begin{aligned} U(t)=u_{i(t)}(x(t)),\; i:\mathbb {R}_{+}\rightarrow \lbrace 1,2\rbrace \end{aligned}$$

(4)

is ruled by

$$\begin{aligned} t_{0}=0,\; i(t_{0})={\left\{ \begin{array}{ll} 1 &{} \;\text {if}\; x(t_{0})\in \mathbf {X}_{2},\\ 2 &{} \;\text{ otherwise }, \end{array}\right. } \end{aligned}$$

while $i(t)=i(t_{j})$ for $t\in [t_{j}\, t_{j+1})$, and

$$\begin{aligned} i(t_{j+1})={\left\{ \begin{array}{ll} 1 &{} \;\text {if}\; x(t_{j+1})\in \mathbf {X}_{1}\\ 2 &{} \;\text {if}\; x(t_{j+1})\not \in \mathbf {X}_{2} \end{array}\right. }, \end{aligned}$$

(5)

where $t_{j}$ is a generic switching instant defined as follows:

$$\begin{aligned} t_{j}={\left\{ \begin{array}{ll} \mathop {\mathrm{arg\, inf}}\limits _{t\ge t_{j}}\, \lbrace x(t)\not \in \mathbf {X}_{2} \rbrace &{} \;\text{ if }\; i(t_{j})=1\\ \mathop {\mathrm{arg\, inf}}\limits _{t\ge t_{j}}\, \; \lbrace x(t)\in \mathbf {X}_{1} \rbrace &{} \;\text{ if }\; i(t_{j})=2 \end{array}\right. }. \end{aligned}$$

The control U has the $u_{1}$ part active if $|z_{2}|<\varDelta$, which means that we are stabilizing the output $z_{1}$ according to condition (2). If $|z_{2}|$ becomes greater or equal than $\varDelta$, then $u_{2}$ will be activated driving $z_{2}$ to a value less than $\delta$ according to condition (3). Inside the set $H=\mathbf {X}_{2}\mathbf {\setminus X}_{1}$ the control will not be switched, this set acts as a hysteresis zone being helpful to avoid a chattering phenomena of switching between $u_{1}$ and $u_{2}$. A similar supervisor has been introduced in Efimov et al. (2009), but in the present work the dwell time condition is not imposed replaced by the following assumption:

Assumption 1

$\sup _{x\in H,d\in \Omega ,i\in \{1,2\}}|f(x,u_{i}(x),d)|=F<+\infty$.

This assumption states that the system velocity on the set H is finite, then since $F<+\infty$ and $d\in \Omega$ there exists a dwell-time delay $\tau _{D}>0$ between any two switches, i.e. $t_{j+1}-t_{j}\ge \tau _{D}$ for all $j\ge 0$. The conditions for solution of the posed problem using the supervisory control algorithm (4), (5) are described in the following theorem.

Theorem 1

Let Assumption 1 be satisfied and $\beta _{1}(s,\tau _{D})=\lambda s$ for all $s\in \mathbb {R}_{+}$ and some $0\le \lambda <1$. Then the system (1) with supervisor (5) and control (4) is forward complete and for all initial conditions $x_{0}\in \mathbb {R}^{n}$, $d\in \Omega$ and $t\ge 0$:

$$\begin{aligned}&|z_{1}(t,x_{0},d)|\le \beta _{1}(|h_{1}(x_{0}))|,0),\\&|z_{2}(t,x_{0},d)|\le \max \lbrace \sigma (\varDelta ),\beta _2(|h_2(x_0)|,t-t_0)\rbrace ,\\&\lim _{t\rightarrow +\infty }|z_{1}(t,x_{0},d)|=0, \end{aligned}$$

where $\sigma (s)=\beta _2(s,0).$

Proof

The existence of dwell-time $\tau _{D}>0$ implies right-continuity of the switching signal i(t), the same property for the right-hand side of the system (1), (4), (5) (due to composition limit rule) and continuity of the system solutions with the absence of chattering. Since for both $u_{i}$, $i\in \{1,2\}$ the solutions of the system are well defined for all $t\ge 0$, then a finite-time escape phenomenon is impossible and solutions of the switched system (1), (4), (5) are well defined for all $t\ge 0$.

By definition of a function from class $\mathcal {KL}$, there exists $0<T_{2}<+\infty$ such that $\delta =\beta _{2}(\varDelta ,T_{2})$. For both controls the following inequalities are satisfied for the outputs:

$$\begin{aligned}&\left\{ \begin{array}{c} i(t)=1\quad \forall t\in [t_{j},t_{j+1}),\\ |z_{1}(t,x(t_{j}),d)|\le \beta _{1}(|h_{1}(x(t_{j}))\vert ,t-t_{j}),\\ |z_{2}(t,x(t_{j}),d)|\le \rho \circ \beta _{1}(|h_{1}(x(t_{j}))\vert ,t-t_{j})+\rho _{0}, \end{array}\right. \end{aligned}$$

(6)

$$\begin{aligned}&\left\{ \begin{array}{c} i(t)=2\quad \forall t\in [t_{j},t_{j+1}),\\ |z_{2}(t,x(t_{j}),d)|\le \beta _{2}(|h_{2}(x(t_{j}))\vert ,t-t_{j}),\\ |z_{1}(t,x(t_{j}),d)|\le |h_{1}(x(t_{j}))|. \end{array}\right. \end{aligned}$$

(7)

Therefore, the following scenarios are possible. First, $x(0)\in \Xi =\{x\in \mathbb {R}^{n}:\rho \circ \beta _{1}(|h_{1}(x)\vert ,0)+\rho _{0}\le \varDelta \}$, then $x(0)\in \mathbf {X_{2}}$, $i(0)=1$ and, according to (6), $i(t)=1$ with $|z_{2}(t,x(0),d)|\le \varDelta$ for all $t\ge 0$ (the estimates (6) are satisfied for all $t\ge 0$, this scenario corresponds to the case without collisions in the WMR example). Second, $x(0)\in \mathbf {X}_{2}\setminus \Xi$ and there exists $0<t_{1}<+\infty$ such that (6) is satisfied for $t\in [t_{0},t_{1})$ and

$$\begin{aligned} |h_{2}(x(t_{1}))\vert= & {} \varDelta ,\\ |z_{1}(t_1,x(t_{0}),d)|\le & {} \beta _{1}(|h_{1}(x(t_{0}))\vert ,t_{1}-t_{0})\\\le & {} \beta _{1}(|h_{1}(x(t_{0}))\vert ,\tau _{D})\\= & {} \lambda |h_{1}(x(t_{0}))\vert <|h_{1}(x(t_{0}))\vert . \end{aligned}$$

Note that if $t_{1}=+\infty$, then this case is identical to the first scenario. Thus according to (7)

$$\begin{aligned}&|z_{2}(t,x(t_{1}),d)|\le \beta _{2}(\varDelta ,t-t_{1})\quad \forall t\in [t_{1},t_{2}),\\&|z_{2}(t_{2},x(t_{1}),d)|=\delta ,\\&|z_{1}(t,x(t_{1}),d)|\le |z_{1}(t_{1})|\le \lambda |h_{1}(x(t_{0}))\vert \quad \forall t\in [t_{1},t_{2}), \end{aligned}$$

where $t_{1}<t_{2}\le t_{1}+T_{2}$. Summarizing these estimates we get

$$\begin{aligned}&|z_{1}(t,x(t_{0}),d)|\le \beta _{1}(|h_{1}(x(t_{0}))|,0)\quad \forall t\in [t_{0},t_{2}),\\&|z_{2}(t,x(t_{0}),d)|\le \beta _{2}(\varDelta ,0)\quad \forall t\in [t_{0},t_{2}),\\&|z_{1}(t_{2},x(t_{0}),d)|\le \lambda |h_{1}(x(t_{0}))\vert . \end{aligned}$$

Next, there exists a sequence of time instants $t_{2k}$, $0\le k\le K\le +\infty$ with $i(t)=1$ for all $t\in [t_{2k},t_{2k+1})$ and $i(t)=2$ for all $t\in [t_{2k+1},t_{2k+2})$. Repeating the arguments above we obtain

$$\begin{aligned}&|z_{1}(t,x(t_{0}),d)|\le \beta _{1}(|h_{1}(x(t_{0}))|,0)\quad \forall t\in [t_{0},t_{2k+2}),\nonumber \\&|z_{2}(t,x(t_{0}),d)|\le \beta _{2}(\varDelta ,0)\quad \forall t\in [t_{0},t_{2k+2}),\\&|z_{1}(t_{2k+2},x(t_{0}),d)|\le \lambda ^{k}|h_{1}(x(t_{0}))\vert \nonumber \end{aligned}$$

(8)

for any $0\le k\le K$. Assume that $K<+\infty$, then it means that $i(t)=1$ for all $t\ge t_{2K}$ (the control $u_{2}$ can be active on a finite interval only by its definition) and from (6)

$$\begin{aligned}&|z_{1}(t,x(t_{0}),d)|\le \beta _{1}(|h_{1}(x(t_{2K}))|,0)\\&\le \beta _{1}(|h_{1}(x(t_{0}))|,0)\quad \forall t\ge t_{2K},\\&|z_{2}(t,x(t_{0}),d)|\le \varDelta \le \beta _{2}(\varDelta ,0)\quad \forall t\ge t_{2K},\\&\lim _{t\rightarrow +\infty }|z_{1}(t,x(t_{0}),d)|=0, \end{aligned}$$

i.e. it is a situation similar to the first scenario. If $K=+\infty$, then from (8) with $k\rightarrow +\infty$ we have the same properties, consequently

$$\begin{aligned}&|z_{1}(t,x(t_{0}),d)|\le \beta _{1}(|h_{1}(x(t_{0}))|,0)\quad \forall t\ge t_{0},\nonumber \\&|z_{2}(t,x(t_{0}),d)|\le \beta _{2}(\varDelta ,0)\quad \forall t\ge t_{0},\\&\lim _{t\rightarrow +\infty }|z_{1}(t,x(t_{0}),d)|=0.\nonumber \end{aligned}$$

(9)

Third, $x(0)\notin \mathbf {X}_{2}$ and in this case there is a time instant $t_{1}>t_{0}$ such that the estimates (7) are satisfied for all $t\in [t_{0},t_{1})$

$$\begin{aligned} |z_{2}(t,x(t_{0}),d)|\le & {} \beta _{2}(|h_{2}(x(t_{0}))\vert ,t-t_{0}),\\ |z_{1}(t,x(t_{0}),d)|\le & {} |h_{1}(x(t_{0}))| \end{aligned}$$

and $|z_{2}(t_{1},x(t_{0}),d)|=\delta$. Since $x(t_{1})\in \mathbf {X}_{2}$ the following system behaviour is similar to the second scenario and from (9) we obtain

$$\begin{aligned}&|z_{1}(t,x(t_{0}),d)|\le \beta _{1}(|h_{1}(x(t_{0}))|,0)\quad \forall t\ge t_{0},\\&|z_{2}(t,x(t_{0}),d)|\le \beta _{2}(\max \{\varDelta ,|h_{2}(x(t_{0}))\vert \},0)\quad \forall t\ge t_{0},\\&\lim _{t\rightarrow +\infty }|z_{1}(t,x(t_{0}),d)|=0. \end{aligned}$$

Therefore, these estimates are satisfied in all three possible scenarios for all $t\ge 0$, that is necessary to prove. $\square$

3 Application to unicycle-like WMR

Let us consider the following unicycle-like WMR kinematic perturbed model, in which the input is affected by an additive disturbance as explained in Sect. 1:

$$\begin{aligned} \dot{q}_{x}&=\cos (q_{\theta })v(1+d_{1}),\nonumber \\ \dot{q}_{y}&=\sin (q_{\theta })v(1+d_{1}),\\ \dot{q}_{\theta }&=\omega (1+d_{2}),\nonumber \end{aligned}$$

(10)

where $(q_{x},q_{y})$, $q_{x},q_{y}\in \mathbb {R}$, define the Cartesian position of the robot, and $q_{\theta }\in \left[ 0,2\pi \right]$ is the orientation of the robot with respect of the world reference frame, v and $\omega$ are the control inputs, respectively the linear velocity and the angular velocity. The additive disturbances on the inputs are unknown, but supposed to be bounded, $-1<d_{min}\le d_{i}\le d_{max}$, $i=1,2$. The lower bound, $d_{min}>-1$, ensures that the disturbance does not induce a change of sign of the control v. To achieve the tasks the robot has to be driven to the origin avoiding obstacles that it could, eventually, encounter during the path. As a solution, two independent controllers can be designed to reach the goals (i.e. stabilization at the origin and collision avoidance) with their posterior uniting (Efimov et al. 2009). These controls can be designed in order to regulate two different outputs:

$$\begin{aligned}&z_{1}(q_{x},q_{y}) =\sqrt{q_{x}^{2}+q_{y}^{2}}, \end{aligned}$$

(11)

$$\begin{aligned}&z_{2}(q_{x},q_{y})\nonumber \\&\quad =\min \left[ Y,\max _{1\le i\le N}\left( \sqrt{(q_{x}-x_{o_{i}})^{2}+(q_{y}-y_{o_{i}})^{2}}\right) ^{-1}\right] , \end{aligned}$$

(12)

where $z_{1}$ is the distance from the origin and $z_{2}$ is the inverse of the distance from the closest obstacle represented by its Cartesian position $(x_{o_{i}},y_{o_{i}})_{i,\ldots ,N}$, with N is a finite number of obstacles, $Y>0$ is a parameter ensuring global boundedness of $z_{2}$ and related with dimensions of the obstacles. Clearly, driving $z_{2}$ to a sufficiently small value means to move away from an obstacle avoiding it. Under the assumption that between the obstacles there was enough space we can consider one obstacle each time without loosing generality.

3.1 Control tasks

In this subsection two finite-time controllers ($u_{i}$, $i\in \{1,2\}$) are designed for (10); the former one is designed to regulate the output $z_{1}$ (11), for the stabilization part, and the second one is to regulate the output $z_{2}$ (12), providing the collision avoidance. The main feature of these controls is that all control tasks are solved not asymptotically, but in a finite time (Guerra et al. 2014).

3.1.1 Stabilization

It is required to design a control $u_{1}$ in order to drive the robot to the desired point, in our case the origin. Consider the output $z_{1}(q_{x},q_{y})$ and the Lyapunov function $V_{1}=0.5z_{1}^{2}$. Let us define $\theta _{0}=\text {arctan}\left( \dfrac{q_{y}}{q_{x}}\right)$ ³ and $\alpha =q_{\theta }-\theta _{0}-\pi$, $\alpha \in [-\pi ,\pi )$, the deviance from the desired final orientation $\theta _{0}+\pi$, then:

$$\begin{aligned} \dot{V}_{1}= & {} \dot{z_1}z_{1} \end{aligned}$$

(13)

$$\begin{aligned}= & {} \frac{q_x\dot{q_x}+q_y\dot{q_y}}{\sqrt{q_x^2+q_y^2}}\sqrt{q_x^2+q_y^2} \end{aligned}$$

(14)

$$\begin{aligned}= & {} q_x\dot{q_x}+q_y\dot{q_y}; \end{aligned}$$

(15)

by applying simple trigonometric rules, one can rewrite $q_x=z_1\cos \theta _0$ and $q_y=z_1\sin \theta _0$. Using the equalities just depicted and by substitution from (10), it follows that

$$\begin{aligned} \dot{V}_{1} = v(1+d_1)z_1\left( \cos q_{\theta } \cos \theta _0 + \sin q_{\theta } \sin \theta _0 \right) . \end{aligned}$$

(16)

Using the equality $\cos \left( \alpha -\beta \right) = \left( \cos \alpha \cos \beta + \sin \alpha \sin \beta \right)$ we can rewrite:

$$\begin{aligned} \dot{V}_{1} = \cos (q_{\theta }-\theta _{0})v(1+d_{1})z_{1}. \end{aligned}$$

(17)

it follows that:

$$\begin{aligned} \dot{V_{1}}=-\cos (\alpha )v(1+d_{1})z_{1}, \end{aligned}$$

Fig. 1

Definition of the angles for the Stabilization control law

(Fig. 1 shows a schematic unicycle, the black shape, and a visual description of the listed angles). The proposed control is

$$\begin{aligned} v={\left\{ \begin{array}{ll} k_{1}z_{1} &{} \,\,\text {if}|\alpha |\le k\pi \\ 0 &{} \,\,\text{ otherwise } \end{array}\right. }, \end{aligned}$$

with $k_{1}$ positive and $0<k\le 0.5$. Then we can ensure the semi-definiteness of $\dot{V_{1}}$:

$$\begin{aligned} {\left\{ \begin{array}{ll} \dot{V_{1}}\le -\cos (\alpha )(1-d_{min})k_{1}z_{1}^{2} &{} \,\,\text {if}\,\,|\alpha |\le k\pi \\ \dot{V_{1}}=0 &{} \,\,\text {otherwise} \end{array}\right. }. \end{aligned}$$

Then

$$\begin{aligned} \dot{V_{1}}\le -2c_{1}V_{1}\;\;for|\alpha |\le k\pi , \end{aligned}$$

where $c_{1}=k_{1}\cos (k\pi )(1-d_{min})$. The control v operates only on the linear velocity of the robot, to steer it we act on the $\alpha$ dynamics that can be expressed like:

$$\begin{aligned} \dot{\alpha }=\omega (1+d_{2})+\sin (\alpha )z_{1}^{-1}(1+d_{1})v. \end{aligned}$$

Using the Lyapunov function $V_{2}=0.5\alpha ^{2}$ and analyzing the equation $\dot{V_{2}}=\omega \alpha (1+d_{2})+\alpha \sin (\alpha ) v(1+d_{1})z_{1}^{-1}$, we notice that the term depending on v is equal to zero most of the time, and it differs from zero for $|\alpha |\le k\pi$ ($\alpha$ is small enough):

$$\begin{aligned} \dot{V}_{2}={\left\{ \begin{array}{ll} \omega \alpha (1+d_{2})+k_{1}\alpha \sin (\alpha )(1+d_{1}) &{} \;\text {if}\;|\alpha |\le k\pi \\ \omega \alpha (1+d_{2}) &{} \;\text {otherwise} \end{array}\right. }. \end{aligned}$$

Thus the control applied to steer the robot is

$$\begin{aligned}&\omega =-k_{2}\zeta (\alpha )\text {sign}(\alpha ),\; k_{2}\ge \dfrac{(1+d_{max})k_{1}+2^{-3/4}\eta _{1}}{1-d_{min}},\nonumber \\&\zeta (\alpha )=\max \{|\alpha |^{0.5},|\alpha |\},\;\eta _{1}>0, \end{aligned}$$

(18)

then

$$\begin{aligned} \dot{V}_{2}\le {\left\{ \begin{array}{ll} -k_{2}|\alpha |\zeta (\alpha )(1-d_{min})\\ +k_{1}\alpha ^{2}(1+d_{max}) &{} \text {if}\;|\alpha |\le k\pi \\ -k_{2}|\alpha |\zeta (\alpha )(1-d_{min}) &{} \text {otherwise}. \end{array}\right. } \end{aligned}$$

Since $|\alpha |\zeta (\alpha )\ge \alpha ^{2}$ we obtain:

$$\begin{aligned} \dot{V}_{2}\le -\eta _{1}\max \left\{ V_{2},V_{2}^{3/4}\right\} , \end{aligned}$$

the $k_{2}$ gain allows us to compensate the disturbances. Therefore, solving the differential equation for $V_2(t)$ and obtaining $\alpha (t)$ from the $V_2$ definition, one can find out the following upper estimate:

$$\begin{aligned}&|\alpha (t)|\le {\left\{ \begin{array}{ll} |\alpha _{0}|e^{-0.5\eta _{1}(t-t_{0})} &{} \text {if}\; t\in [t_{0},t_{1}],\\ \sqrt{2}[\min \{1,2^{-1/4}\sqrt{|\alpha _{0}|}\}\\ -2^{-2}\eta _{1}(t-t_{1})]^{2} &{} \text {if}\; t\in (t_{1},t_{2}],\\ 0 &{} \text {if}\; t>t_{2}, \end{array}\right. }\\&t_{1}=t_{0}+\max \left\{ 0,\eta _{1}^{-1}\ln (0.5\alpha _{0}^{2})\right\} ,\nonumber \\&t_{2}=t_{1}+2^{-2}\eta _{1}^{-1}\min \left\{ 1,2^{-1/4}\sqrt{|\alpha _{0}|}\right\} ,\nonumber \end{aligned}$$

(19)

where $t_{0}\ge 0$ is the instant when this control has been activated and $\alpha _{0}=\alpha (t_{0})\in [-\pi ,\pi )$ is the initial condition. Therefore, there exists $0\le T_{1}(\alpha _{0})<\infty$ for all $\alpha _{0}\in [-\pi ,\pi )$ such that $|\alpha (t)|<k\pi$ for all $t\ge t_{0}+T_{1}(\alpha _{0})$. Indeed, the dynamics of $\alpha$ with $v=0$ helps to compute

$$\begin{aligned} T_{1}(\alpha _{0})\le {\left\{ \begin{array}{ll} \max \left\{ 0,\frac{2}{\eta _{1}}\ln \left( \dfrac{|\alpha _{0}|}{k\pi }\right) \right\} &{} \text {if}\; k\pi \ge 1,\\ t_{1}+4\eta _{1}^{-1}[\min \{1,2^{-1/4}\sqrt{|\alpha _{0}|}\}\\ -2^{-1/4}\sqrt{k\pi }] &{} \text {otherwise}. \end{array}\right. } \end{aligned}$$

Since $|\alpha _{0}|\le \pi$ we finally obtain

$$\begin{aligned} T_{1}(\pi )=\eta _{1}^{-1}{\left\{ \begin{array}{ll} -2\ln (k) &{} \text {if}\; k\pi \ge 1,\\ \ln (0.5\pi ^{2})+4(1-2^{-1/4}\sqrt{k\pi }) &{} \text {otherwise}. \end{array}\right. } \end{aligned}$$

Thus it follows that

$$\begin{aligned} z_{1}(t)\le z_{1}(t_{0})\min \{1,e^{-c_{1}(t-T_{1}(\pi )-t_{0})}\}\;\forall t\ge t_{0}. \end{aligned}$$

(20)

Based on the expressions for v and $\omega$, the control $u_{1}$ can be summarized as follows:

$$\begin{aligned} u_{1}={\left\{ \begin{array}{ll} v={\left\{ \begin{array}{ll} k_{1}z_{1} &{} \,\,\text {if}\;|\alpha |\le k\pi \\ 0 &{} \,\,\text {otherwise} \end{array}\right. },\\ \omega =-k_{2}\zeta (\alpha )\text {sign}(\alpha ). \end{array}\right. } \end{aligned}$$

(21)

The following result has been proven.

Lemma 1

In the system (10) with control (21) the estimates (20) and (19) are satisfied (a uniform exponential stabilization for $z_{1}$ and a uniform finite-time stabilization for $\alpha$).

3.1.2 Obstacle avoidance

To design a controller for collision avoidance we define each obstacle as a point in the plane and a safe distance to be maintained. Each obstacle is an element of the set $O=\lbrace (x_{o_{i}},y_{o_{i}},\rho _{i,min})\rbrace _{i=1,\ldots ,N}$, with N number of possible obstacles, $Y=1/\min _{1\le i\le N}\{\rho _{i,min}\}$. Each obstacle is entirely contained in the circle of radius $\rho _{i,min}$ which is a distance designed considering the radius of the obstacle itself and a distance equal to the radius of the circle in which the robot can be inscribed.

The switch (collision avoidance algorithm activation) occurs when the robot reaches a distance $\rho _{i}>\rho _{i,min}$, which adds an additional safety level to the collision avoidance maneuver. We want to ensure the avoidance thus augmenting the distance from $\rho _{i}$ to a predefined $R_{i}>\rho _{i}$ using a control $u_{2}$, which has to be designed. In terms of the output $z_{2}$ we want to decrease it from $\varDelta _{i}=\rho _{i}^{-1}$ to $\delta _{i}=R_{i}^{-1}$, moreover it is required that the output $z_{1}$ will not increase during/after the activation of the control $u_{2}$. Assume that $\max _{1\le i\le N}1/\sqrt{x_{o_{i}}^{2}+y_{o_{i}}^{2}}<\min _{1\le i\le N}\delta _{i}$, i.e. the origin is well separated from an obstacle. It is also assumed that $\varUpsilon _{i}\cap \varUpsilon _{j}=\emptyset$ for any $i\ne j\in \{1,\dots ,N\}$, where $\varUpsilon _{i}=\{(q_{x},q_{y})\in \mathbb {R}^{2}:(q_{x}-x_{o_{i}})^{2}+(q_{y}-y_{o_{i}})^{2}\le R_{i}^{2}\}=\{(q_{x},q_{y})\in \mathbb {R}^{2}:z_{2}(q_{x},q_{y})\ge \delta _{i}\}$, i.e. any two obstacles are separated and the collision avoidance problem can be addressed for an isolated obstacle.

In order to design the control $u_{2}$ we need to plan a strategy to move from $\varDelta _{i}$ to $\delta _{i}$. The goal of the collision avoidance control will be to reach a new point $B=(x_{B},y_{B})$ such that $z_{1}(x_{B},y_{B})\le z_{1}(q_{x}(t_{ca}),q_{y}(t_{ca}))$ and $z_{2}(x_{B},y_{B})\le \delta _{i}$, where $t_{ca}$ is the instant of time in which the control $u_{2}$ is switched on, i.e. $z_{2}(q_{x}(t_{ca}),q_{y}(t_{ca}))=\varDelta _{i}$. An algorithm for the point B selection is explained below.

Fig. 2

Circles used in the definition of the $B^{-}$, $B^{lim}$ and then the B points with the “Tangent” approach

A preliminary point $B^{-}$ is defined as an intersection point of the circle centered in $(x_{o_{i}},y_{o_{i}})$ of radius $R_{i}$ and the tangent line to the circle centered at $(x_{o_{i}},y_{o_{i}})$ of radius $\varDelta _{i}$ (the red one in Fig. 2). Although this approach is very efficient, it does not provide the second requirement of the control, i.e. $\dot{z}_{1}(t)\le 0$, in the case when the robot, the obstacle center and the origin are on the same straight line. In this case a preliminary $B^{-}$ point will be in the intersection of two circles: the first centered in $(x_{o_{i}},y_{o_{i}})$ of radius $R_{i}$ (the green one in Fig. 2) and the second circle centered at the origin of radius $|z_{1}(t_{ca})|$ (the black one in Fig. 2).

In both cases to determine the final coordinates of B, the distance $\rho _{i,min}$ is used (as a limit not to cross), represented in Fig. 2 with the blue circle, which provides, on the circle with the center $(x_{o_{i}},y_{o_{i}})$ and radius $R_{i}$, the point $B^{lim}$. The point $B^{lim}$ is an intersection of a straight line initiated at the robot position and tangent to the circle centered in $(x_{o_{i}},y_{o_{i}})$ with radius $\rho _{i,min}$. Finally the point $B'$ is selected on the circle of radius $R_{i}$ between the points $B^{lim}$ and $B^{-}$ taking a safe distance from them proportional to $d_{max}$ (in order to avoid the risk of being steered backward due to a disturbance). Finally, the point $B=(x_{B},y_{B})$ is selected on the line passing the current robot position and the point $B'$ with the condition that $z_{2}(x_{B},y_{B})<\delta$ (outside the set $\varUpsilon _{i}$, green circle Fig. 2). With such a selection of the point B it is possible to achieve the avoidance and to keep, in addition, the condition $\dot{z}_{1}(t)\le 0$. Once the point B is defined, the control $u_{2}$ must drive the robot to this point. Thus the collision avoidance problem can be solved using a similar approach as the one in the previous subsection, replacing the origin by the point B and taking into account the additional constraint dealing with non-increasing of $z_{1}$. Define the distance from the robot to the point B as

$$\begin{aligned} D_{B}(q_{x},q_{y})=\sqrt{(q_{x}-x_{B})^{2}+(q_{y}-y_{B})^{2}}; \end{aligned}$$

in such a formulation the imposed restriction $z_{2}(x_{B},y_{B})<\delta$ becomes crucial and the point B will not be reached during the collision avoidance manoeuvre. It is important, since in the desired point the robot loses stabilizability. Define $\vartheta =\inf _{(x,y)\in \varUpsilon _{i}}D_{B}(q_{x},q_{y})$ the distance from the point B to the set $\varUpsilon _{i}$. Let us define the angle of desired orientation of the robot towards the point B as $\theta _{g}=\tan ^{-1}\left( \dfrac{q_{y}-y_{B}}{q_{x}-x_{B}}\right)$, then the deviance from the desired angle for the collision avoidance control can be defined as $\gamma =\theta _{g}-q_{\theta }$, then $\dot{D}_{B}=-\cos (\gamma )v(1+d_{1})$. The expression for v is proposed as follows:

$$\begin{aligned} v={\left\{ \begin{array}{ll} k_{3}D_{B} &{} \;\text {if}\;\cos (\alpha )\ge 0\;\text {and}\;|\gamma |\le \epsilon \pi \\ 0 &{} \;\text {otherwise} \end{array}\right. }, \end{aligned}$$

where $k_{3}>0$ and $0<\epsilon <0.5$. Since this control has to be applied into the set $\varUpsilon _{i}$ only, then $v\ge k_{3}\vartheta$. Due to the application and the velocities reached by the WMR setting instantly $v=0$ does not represent a problem because, although it call off the considered disturbances, the deceleration from any velocity to the zero could be considered instantaneous with no loss of generality.⁴ For the designed control v, a Lyapunov function $W_{1}(D_{B})=0.5D_{B}^{2}$ has the derivative (after some manipulations):

$$\begin{aligned} \dot{W}_{1}\le {\left\{ \begin{array}{ll} -2c_{2}W_{1} &{} \;\text {if}\;\cos (\alpha )\ge 0\;\text {and}\;|\gamma |\le \epsilon \pi \\ 0 &{} \;\text {otherwise} \end{array}\right. }, \end{aligned}$$

where $c_{2}=k_{3}\cos (\epsilon \pi )(1-d_{min})$. The angle $\gamma$ has the following dynamics

$$\begin{aligned} \dot{\gamma }=-\omega (1+d_{2})+\dfrac{\sin \gamma }{y_{B}}v(1+d_{1}). \end{aligned}$$

(22)

Using the Lyapunov function $W_{2}=0.5\gamma ^{2}$ we obtain:

$$\begin{aligned} \dot{W}_{2}=-\omega \gamma (1+d_{2})+\gamma \dfrac{\sin \gamma }{y_{B}}v(1+d_{1}). \end{aligned}$$

Being $\zeta (\gamma )=\max \{|\gamma |^{0.5},|\gamma |\}$, the proposed expression for the control $\omega$ has the form:

$$\begin{aligned} \omega= & {} k_{d}\dot{\gamma }+\frac{\sin \gamma }{D_{B}}v+k_{ca}\zeta (\gamma )sign(\gamma ),\; k_{d}>0,\nonumber \\ k_{ca}\ge & {} k_{3}\frac{\sqrt{\pi }(d_{max}-d_{min})[1+k_{d}(1+d_{max})]}{(1-d_{min})[1+k_{d}(1-d_{min})]}\\&+\,2^{-3/4}\eta _{2}\frac{1+k_{d}(1+d_{max})}{1-d_{min}},\;\eta _{2}>0.\nonumber \end{aligned}$$

(23)

A straightforward calculation shows that

$$\begin{aligned} \dot{W_{2}}\le -\eta _{2}\max \left\{ W_{2},W_{2}^{3/4}\right\} , \end{aligned}$$

thus, as for (19), an estimation for $\gamma (t)$ can be provided:

$$\begin{aligned}&|\gamma (t)|\le {\left\{ \begin{array}{ll} |\gamma _{0}|e^{-0.5\eta _{2}(t-t_{ca})} &{} \text {if}\; t\in [t_{ca},t_{3}],\\ \sqrt{2}[\min \{1,2^{-1/4}\sqrt{|\gamma _{0}|}\}\\ -2^{-2}\eta _{2}(t-t_{3})]^{2} &{} \text {if}\; t\in (t_{3},t_{4}],\\ 0 &{} \text {if}\; t>t_{4}, \end{array}\right. }\\&t_{3}=t_{ca}+\max \left\{ 0,\eta _{2}^{-1}\ln (0.5\gamma _{0}^{2})\right\} ,\nonumber \\&t_{4}=t_{3}+2^{-2}\eta _{2}^{-1}\min \left\{ 1,2^{-1/4}\sqrt{|\gamma _{0}|}\right\} ,\nonumber \end{aligned}$$

(24)

where $t_{ca}\ge 0$ is an instant of activation of the control $u_{2}$ and $\gamma _{0}=\gamma (t_{ca})$. Thus the proposed control steers the robot in a finite time to the desired orientation, indeed there exists $0<T_{2}<\infty$ such that $\gamma (t_{ca}+T_{2})<\epsilon \pi$ for all $\gamma \in [-\pi ,\pi )$:

$$\begin{aligned} T_{2}=\eta _{1}^{-1}{\left\{ \begin{array}{ll} -2\ln (\epsilon ) &{} \text {if}\;\epsilon \pi \ge 1,\\ \ln (0.5\pi ^{2})+4(1-2^{-1/4}\sqrt{\epsilon \pi }) &{} \text {otherwise}. \end{array}\right. } \end{aligned}$$

Following the geometric construction of the point B the inequality $\cos (\alpha )|_{\gamma =0}>0$ is verified, then there is a time instant $t_{ca}\le \bar{t}\le T_{2}$ such that the conditions $\cos (\alpha (t))\ge 0$ and $|\gamma (t)|\le \epsilon \pi$ (involved in the control v activation) are satisfied for $t\ge \bar{t}$, starting from the instant $\bar{t}$ the robot starts to move without an interruption since $v\ge k_{3}\vartheta$. Therefore, with the decreasing properties of $\gamma$, the distance $D_{B}$ is decreasing and admits an estimate:

$$\begin{aligned} D_{B}(t)\le D_{B}(t_{ca})e^{-c_{2}(t-T_{2}-t_{ca})},\;\;\forall t\ge t_{ca}. \end{aligned}$$

Since the point B is located outside the set $\varUpsilon _{i}$, then there is a finite time $T_{ca}>t_{ca}$ such that $z_{2}(T_{ca})=\delta$, hence the collision avoiding is accomplished. It is worth to stress that it is possible to have a local increment of the regulated output $z_{2}$ due to the geometric construction of the point B. On the other hand, after a certain amount of time the output $z_{2}$ decreases with the controller v. In addition, as it has been shown above, it is not possible to steer the robot toward the obstacle, and the robot itself will not enter the circle of radius $\rho _{i,min}$. The output $y_{1}$ does not increase during the collision avoiding maneuver since the constraint $\cos (\alpha )\ge 0$ has been introduced in the control v (and v is positive).

The controller $u_{2}$ for the two control inputs v and $\omega$ pushes the robot in a finite time toward a point far from the obstacle, while keeping the distance $y_{1}$. The control $u_{2}$ can be summarized as follows:

$$\begin{aligned} u_{2}={\left\{ \begin{array}{ll} {\left\{ \begin{array}{ll} k_{3}D_{B} &{} \;\text {if}\;\cos (\alpha )\ge 0\;\text {and}\;|\gamma |\le \epsilon \pi \\ 0 &{} \;\text {otherwise} \end{array}\right. },\\ \omega =k_{d}\dot{\gamma }+\frac{\sin \gamma }{D_{B}}v+k_{ca}\zeta (\gamma )\text {sign}(\gamma ). \end{array}\right. } \end{aligned}$$

(25)

The following properties have been substantiated.

Lemma 2

The system (10) with control (25) has the properties for $t_{ca}\ge 0$:

• 1. Uniform finite-time stability with respect to the variable $\gamma (t)$ (see the estimate (24)).

• 2. There exists $T_{ca}>t_{ca}$ such that $\delta _{i}\le z_{2}(t)<\rho _{i,min}^{-1}$ for all $t\in [t_{ca},T_{ca}]$ and $z_{2}(T_{ca})=\delta _{i}$.

• 3. $\dot{V}_{1}(t)\le 0$ for all $t\in [t_{ca},T_{ca}]$.

3.2 Supervision

For commutation between the controls (21) and (25) the following supervisor is proposed:

$$\begin{aligned}&U(t)=u_{i(t)}[q_{X}(t),q_{y}(t),q_{\theta }(t)],\; i:\mathbb {R}_{+}\rightarrow \lbrace 1,2\rbrace \end{aligned}$$

(26)

$$\begin{aligned}&t_{0}=0,\; i(t_{0})={\left\{ \begin{array}{ll} 1 &{} \;\text {if}\;(q_{x}(t_{0}),q_{y}(t_{0})\in \mathbf {X_{2}},\\ 2 &{} \;{\hbox {otherwise}}, \end{array}\right. }\nonumber \\&i(t)=i(t_{j})\quad \forall t\in [t_{j}\, t_{j+1}),\nonumber \\&i(t_{j+1})={\left\{ \begin{array}{ll} 1 &{} \;\text {if}\; q(t_{j+1})\in \mathbf {X_{1}} \\ 2 &{} \;\text {if}\; q(t_{j+1})\not \in \mathbf {X_{2}} \end{array}\right. },\\&t_{j}={\left\{ \begin{array}{ll} \mathop {\mathrm{arg\, inf}}\limits _{t\ge t_{j}}\, \lbrace q(t)\not \in \mathbf {X_{2}} \rbrace &{} \;\text{ if }\; i(t_{j})=1\\ \mathop {\mathrm{arg\, inf}}\limits _{t\ge t_{j}}\, \lbrace q(t)\in \mathbf {X_{1}} \rbrace &{} \;\text{ if }\; i(t_{j})=2 \end{array}\right. }.\nonumber \end{aligned}$$

(27)

where $\mathbf {X_{1}}:\{(q_{x},q_{y})\in \mathbb {R}^{2}:\mathbb {R}^{2}\setminus \cup _{j=1}^{N}\varUpsilon _{j}\}$ and $\mathbf {X_{2}}:\left\{ (q_{x},q_{y})\in \mathbb {R}^{2}:\mathbb {R}^{2}\setminus \cup _{j=1}^{N}\Xi _{j}\right\}$, $\Xi _{j}=\{(q_{x},q_{y})\in \mathbb {R}^{2}:(q_{x}-x_{o_{j}})^{2}+(q_{y}-y_{o_{j}})^{2}\le \rho _{i}^{2}\}$. Thus the control $u_{1}$ is applied if $z_{2}<\delta _{j}$ and the control $u_{2}$ has to be activated if $z_{2}=\varDelta _{j}$ for some $j\in \{1,\dots ,N\}$. The following corollary represents the main result applied to a unicycle-like WMR.

Corollary 1

Consider the system (10) with the supervisor (27) and control (26), then:

$$\begin{aligned}&z_{1}(t)\le z_{1}(0)\quad \forall t\ge 0,\\&\lim _{t\rightarrow \infty }z_{1}(t)=0,\\&z_{2}(t)\le \max \{\sigma (\varDelta ),z_{2}(0)\}\quad \forall t\ge 0, \end{aligned}$$

where $\varDelta =\max _{1\le i\le N}\varDelta _{i}$ and $\sigma (s)=s/(\varDelta Y)$.

4 Experimental results

The presented strategy has been implemented on a Turtlebot2 (http://www.turtlebot.com/) mobile robot. The WMR was equipped with a Hokuyo® (http://www.hokuyo-aut.jp) UTM-30LX LIDAR device. The necessary libraries to communicate with the WMR were found on Robotic Operating System (ROS), “Groovy” release (www.ros.org). An easy LIDAR based obstacle detection algorithm has been implemented to get obstacle(s)’ positions $(x_{o_{i}},y_{o_{i}})$ and radius $r_{i}$ used to define also the values of $\rho _{i,min}=r_{i}+0.3$, $\rho _{i}=\rho _{i,min}+0.3$ and $R_{i}=\rho _{i}+0.35$. The values of $\epsilon$ and k are equal to 1 / 30, the control gains are $k_{1}=\eta =0.5$ for the stabilization controller and $k_{3}=1.5$, $k_{ca}=0.1$ , $k_{d}=0.05$ for the collision avoidance one while $d_{max}=-d_{min}=0.5$. Several scenarios have been tested for the presented Finite Time Obstacle Avoidance (FTOA) technique, in addition the performances have been compared with the well know DWA (Fox et al. 1997) which is included in the ROS.

The implementation of the algorithm can be divided in two main parts. Firstly, Algorithm 1 translates in pseudo-code the supervisor (27) which regulates the switching between the two controls. The if statement determines the activation of the collision avoidance control and it can be noticed that such controller is kept active until the condition in the elseif statement is verified, that is when the WMR exits the circle of radius $R_{i}$. The function getbp(state, $z_{2}$ ) follows the instructions presented in Sect. 3.1.2 to determine the B point, while the variable Flag assures that the controller is not switched when the WMR is in the zone between $\varDelta _{i}$ and $\delta _{i}$. Secondly, Algorithm 2 represents the complete proposed algorithm where firstly the laser scans are used to localize the robot in a map (getpos(laser_scan)) then to evaluate the presence of an eventual obstacle (getz_2(laser_scan)). As can be seen by analyzing the two algorithms the FTOA method is also very simple to implement requiring very few steps and, as a result, very low computational power.

4.1 Experimental behaviour

Figures 3 and 4 show the different trajectories followed by the WMR using two different methods on different soil’s condition. The decision to run tests on two different soils has been taken for two main reasons: firstly, to justify the choice of the perturbed model (10), then to show the robustness of the controller designed in Sect. 3 with respect to this change. Without adapting the PID gains which translates the velocities commands in current inputs, the soil properties represent a real perturbation for the system, because for the same velocity input the WMR reacts differently following different trajectories. It can be seen how the proposed method handles these perturbations in a better way than traditional strategies producing repeatable trajectories. In Fig. 3, for a coarse soil (moquette), it can be noticed that all the trajectories generated by the FTOA are close to each other and that they change (even if not much) for the DWA. Repeating the experiment on a smooth soil (linoleum), Fig. 4, the changes in the trajectories are more clear, that it is given by the reduced friction between the wheels and the soil that caused grip issues for the WMR. Again, the FTOA method gives better results (repeatability, time spent, distance traveled) than the DWA.

Fig. 3

Left Real scenario with coarse soil (moquette). Right Zoom on trajectories

Fig. 4

Left Real scenario with smooth soil (linoleum). Right Zoom on trajectories

The comparison between FTOA and DWA for the scenarios showed in Figs. 3 and 4, with equal maximal linear and angular velocities, based on 10 trials, showed that the FTOA can achieve the avoiding in a faster (wrt time) and shorter (wrt distance) way; detailed results are presented in Tables 1 and 2.

In addition, Fig. 5 shows how the outputs behave in a typical stabilization execution from a point to the origin; the vertical black lines represent the switching instants. The data comes from an execution for the scenario shown in Fig. 3. The $z_{2}$ output (12) decreases between two successive switches and it is worth to remark that $z_{1}$ output (11) never increases, not even during the collision avoidance maneuver; that is exactly how the controllers (21) and (25) overseen by the supervisor (27) should work.

4.2 Discussion

Table 1 Comparison between FTOA method and DWA on coarse soil

Trying not to increase the distance from the target point (in this work the origin with any loss of generality) over the complete maneuver causes the robot having trajectories less smooth than other methods but, as it has been proven, this behavior does not worsen the overall performances. The smoothness of the trajectories can be adjusted augmenting the parameters k for (21) and $\epsilon$ for (25) that will cause a decay in the performances wrt time spent and distance traveled as it is obvious. Moreover, under the assumption to deal with the single obstacle, the proposed method has no issues related to small non-convex obstacles because of the way the collision avoidance is realized and the obstacle described, nevertheless, being a reactive (local) method, there is a high probability that it fails if the WMR is trapped in a U-shaped trap, like others local approaches. Other inconvenient could appear if the obstacle has a very long shape (a wall), that could cause an unwanted oscillatory behaviour. All the issues listed above could be solved integrating the algorithm in a global planner which gives suitable points as targets to stabilize progressively.

Table 2 Comparison between FTOA method and DWA on smooth soil

Fig. 5

Outputs evolution

5 Conclusion and future work

The paper presented a switching based solution to stabilize a unicycle-like WMR, locally avoiding obstacles and rejecting disturbances due to neglected dynamics. A supervisor orchestrates two different controls to regulate two respective outputs. It has been shown how the cooperation of the two controls leads to a practically finite-time robot deployment, while the collision avoidance is always achieved in a finite time. In addition, the results of this work have been proven and tested on a real platform to show the effectiveness of the method also comparing it with a well-known method as the DWA. The presented solution treats the case of static obstacles but several experiments have been run with slow moving obstacles with preliminary results; it is in the intention of the authors to extend the results for moving obstacles. Future work will also involve the integration of the proposed strategy in a global planner as specified in Sect. 4.2 to overcome the limitations of the method and to relax the hypothesis of circular shaped obstacles. Moreover the introduction of perturbations to adapt the model for higher velocities will be considered.