Robust Collision Avoidance for Autonomous Mobile Robots in Unknown Environments

Abstract

This paper presents a new collision avoidance method for mobile robots operating in unknown cluttered environments. The proposed method computes the steering angle based on the location of all obstacles surrounding the robot, not just the closest one. Hence, our technique is capable of generating smooth robot trajectories, particularly for unstructured environments. Moreover, the stability of the robot’s motion is improved by providing a smoother bridge between avoiding obstacles and approaching the goal. Oscillations occurring in narrow corridors are reduced by considering the distribution of obstacles to both sides of the direction of motion. Simulation and experimental results are presented to demonstrate the performance of the proposed approach.

1 Introduction

Nowadays, applications such as exploration, search and rescue, or mining require autonomous mobile robots to perform difficult or dangerous tasks without human intervention. In such applications, the environment is usually unknown and changes over time. Moreover, unpredictable obstacles may block the robot’s trajectory during mission operations. In order to guarantee a safe navigation in these environments, it is necessary to incorporate the sensory perceptions within the motion planning and the control loop. By this means, robots can detect environmental changes and re-plan dynamically to reach a goal safely. This is achieved by reactive obstacle avoidance navigation techniques.

The earlier obstacle avoidance methods for mobile robots as well as for manipulators were based on the concept of Artificial Potential Field APF (e.g. [1, 2]). Within this concept, obstacles exert a repulsive force while the target asserts an attractive force onto the robot. The vector sum of these forces determines the steering angle. APF methods are considered fast and computationally efficient. However, failure to find an oscillation-free motion and getting stuck in local minima are well-known problems in these methods. Recent works try to overcome such drawbacks, such as [3], by using a modified Newton method, and [4], by employing a family of 2D smooth vector fields.

Other common techniques add constraints, coming from physical limitations and sensory data, to the velocity space, and choose the speed that maximizes an objective function and satisfies all constraints. The Dynamic Window [5, 6] and Curvature Velocity [7, 8] are popular methods that follow this strategy. Despite the fact that these methods drive the robot faster with smoother behavior, they are unsuitable for cluttered environments as they fail to drive robots in between closely spaced obstacles. Moreover, they still can be trapped in local minima.

Several methods are based on the concept of Velocity Obstacles VO [9, 10] or Inevitable Collision States ICS [11, 12]. These methods perform collision avoidance by identifying the set of robot’s velocities/states that may cause collision at a future time, and choose velocities outside of this set. A VO variant, RVO [13, 14], deals with the problem of cooperative obstacle avoidance. Although these methods explicitly consider the velocity of obstacles, they assume known or predictable future of the scene which is hard to achieve in real applications [15]. Additionally, it is difficult to determine the suitable time horizon for VO-based techniques, particularly in cluttered environments [16].

The Nearness-Diagram ND approach [17] and its improved version ND+ [18] use a “divide and conquer” strategy to identify the current navigational situation and then generate the corresponding motion law. These methods can successfully drive the robot in dense and cluttered environments and avoid local trap situations. The Smooth Nearness-Diagram SND method [19] was then developed enhancing the smoothness of the paths generated by ND+. Unfortunately, SND can be trapped in narrow corridors if one of its sides has a large number of obstacle points compared to the other. The Closest Gap CG method [20] addresses this problem by providing a stricter deviation against the nearest obstacles and by considering the ratio of threats on the both sides of the robot’s heading.

The above mentioned ND variants avoid collisions with nearby obstacles using an idea inspired from the APF concept, which is likely to cause an oscillatory robot motion in narrow passages. This may also lead to longer trajectories and more execution time. The Tangential Closest Gap TCG [21] was then developed solving this drawback by integrating two concepts: the Closest Gap for extracting openings surrounding the robot and the Tangential Escape [22] for reactive collision avoidance navigation. A similar approach [23] solves this problem by taking into account the angular width of the chosen gap, as seen by the robot, and the closest obstacle in collision with the path towards this gap. This approach has been enhanced in [24] by considering all colliding obstacle points.

In this paper, we introduce the TCG+ navigation approach which is an enhancement of the TCG method. Compared to the TCG, our new approach takes into account the location of all close obstacle points in determining the steering angle, rather than the closest one. As a consequence, a smoother robot motion is achieved. Furthermore, the TCG+ provides a smoother bridge between avoiding obstacles (circumnavigating an obstacle) and approaching the goal (satisfying the leaving condition), resulting in a more stable behavior. Considering the distribution of obstacles to both sides of the direction of motion reduces oscillations occurring in narrow passages. The ability to generate smooth and stable robot trajectories reduces the possibility of wheel skidding over ramps, and thus is extremely beneficial in performing RoboCup rescue missions. The TCG+ is deployed as the reactive layer for our autonomous robot GETbot in the RoboCup Rescue Robot League competitions.

This paper presents the TCG+ reactive collision avoidance method design in Sect. 2. In Sects. 3 and 4, we show and discuss the simulation and experimental results. Subsequently, in Sect. 5 the performance of the proposed TCG+ approach is evaluated. Finally, Sect. 6 highlights our conclusions.

2 Reactive Collision Avoidance Technique

This section presents the TCG+ collision avoidance method for mobile robots operating in unknown cluttered environments which serves as a reactive planner in a hybrid navigation system. The TCG+ approach works as follows: first, the sensory data is analyzed to determine the structure of obstacles surrounding the robot. Based on this analysis, an instantaneous waypoint which makes progress towards the goal is located in a collision-free area as described in Sect. 2.2. The location of the waypoint is then adjusted to avoid nearby obstacles as explained in Sect. 2.3.

2.1 Definitions and Notations

The robot and goal locations are denoted by $\mathbf {p}_{\mathrm {r}}$ and $\mathbf {p}_{\mathrm {g}}$, respectively. The robot is wrapped into a circle whose radius is denoted by R. A scan point is denoted by $\mathbf {p}_{i}$, $i = 1,\ldots , n$. The polar coordinates of $\mathbf {p}_{i}$ are denoted by $(r_{i}, \theta _{i})$.

For any two angles ($\theta _1, \theta _2$), the minimum angular distance between them is $\text {dist}$ ($\theta _1$, $\theta _2$) = min ($\text {dist}_{\mathrm {c}}$ ($\theta _1$, $\theta _2$), $\text {dist}_{\mathrm {cc}}$ ($\theta _1$, $\theta _2$)), where $\text {dist}_{\mathrm {c}}$ ($\theta _1$, $\theta _2$) = ($\theta _1 - \theta _2$) $\text {mod}$ $2\pi$ and $\text {dist}_{\mathrm {cc}}$ ($\theta _1$, $\theta _2$) = ($\theta _2 - \theta _1$) $\text {mod}$ $2\pi$.

In order to normalize an angle $\theta$ to the range $[-\pi , \pi [$, a projection function is defined as:

$$\begin{aligned} \mathrm {proj}(\theta ) = ((\theta + \pi )\bmod 2\pi ) - \pi \end{aligned}$$

(1)

Assume that $a < b$, a saturation function is defined as:

$$\begin{aligned} \text {sat}_{[a, b]} (x) = \left\{ \begin{array}{l l} a, &{} \quad \text {if}\,\, x\,\, \le \,\, a\\ x, &{} \quad \text {if}\,\, a\,\,< x <\,\, b\\ b, &{} \quad \text {if}\,\, x\,\, \ge \,\, b \end{array} \right. \end{aligned}$$

(2)

2.2 Locating a Waypoint

If the direct path towards the goal is blocked¹, it is required to drive the robot towards an intermediate goal, referred to as a waypoint, rather than towards the goal itself. Locating the waypoint $\mathbf {p}_{\mathrm {cg}}$ is based on studying the distribution of obstacles surrounding the robot: First, the sensory data is searched for gaps which indicate potential free areas where the robot fits through. Several methodologies can be found in the literature for extracting gaps (e.g. [17, 20]). Here, we follow the CG method [20] due to its simplicity and computational efficiency. The navigable gap closest to the goal, called closest gap, is then selected to navigate through. At each time instance, the gap is characterized by two points (sides), one of them is closer to the goal than the other. We refer to it by $\mathbf {p}_{\mathrm {cs}}$ and the other by $\mathbf {p}_{\mathrm {os}}$ (see Fig. 1). In general, $\mathbf {p}_{\mathrm {cg}}$ is located on the line segment connecting $\mathbf {p}_{\mathrm {cs}}$ to $\mathbf {p}_{\mathrm {os}}$ in such a way that a safe distance $d_{\mathrm {safe}}$ is preserved between $\mathbf {p}_{\mathrm {cg}}$ and $\mathbf {p}_{\mathrm {cs}}$ as the robot moves towards the closest gap. The value of $d_{\mathrm {safe}}$ is set to half of the width of the gap. However, when the gap is wide enough, it is better to limit $d_{\mathrm {safe}}$ so that the resultant trajectory is shorter:

$$\begin{aligned} d_{\mathrm {safe}} = \left\{ \begin{array}{l l} R + D_\mathrm {s}, &{} \quad \text {if}\,\, \left\| \mathbf {p}_{\mathrm {cs}} - \mathbf {p}_{\mathrm {os}} \right\| > 2(R + D_\mathrm {s})\\ \frac{1}{2}\left\| \mathbf {p}_{\mathrm {cs}} - \mathbf {p}_{\mathrm {os}} \right\| , &{} \quad \text {otherwise} \end{array} \right. \end{aligned}$$

(3)

where $D_\mathrm {s}$ is a suitable clearance to be maintained between the robot and $\mathbf {p}_{\mathrm {cs}}$.

Let $\theta _{\mathrm {cs}}$ and $\theta _{\mathrm {os}}$ be the angles between the x-axis of the robot and $\mathbf {p}_{\mathrm {cs}}$ and $\mathbf {p}_{\mathrm {os}}$, respectively, and $r_\mathrm {cs}$ the distance between $\mathbf {p}_{\mathrm {r}}$ and $\mathbf {p}_{\mathrm {cs}}$. The polar coordinates of $\mathbf {p}_{\mathrm {cg}}$ are defined as follows:

$$\begin{aligned} \theta _{\mathrm {cg}}&= \left\{ \begin{array}{l l} \mathrm {proj} \, (\theta _{\mathrm {cs}} - \psi ), &{} \text {if}\,\, \theta _{\mathrm {cs}} > \theta _{\mathrm {os}}\\ \mathrm {proj} \, (\theta _{\mathrm {cs}} + \psi ), &{} \text {otherwise} \end{array} \right. \end{aligned}$$

(4)

$$\begin{aligned} r_{\mathrm {cg}}&= \sqrt{d_\mathrm {safe}^2 + r_{\mathrm {cs}}^2 - 2 d_\mathrm {safe} r_{\mathrm {cs}} \cos (\varphi )} \end{aligned}$$

(5)

with $\varphi$ and $\psi$ are defined by:

$$\begin{aligned} \varphi&= \arccos \left( \frac{w^2 + r_{\mathrm {cs}}^2 - r_{\mathrm {os}}^2}{2 w r_{\mathrm {cs}}} \right) \end{aligned}$$

(6)

$$\begin{aligned} \psi&= \arccos \left( \frac{r_{\mathrm {cg}}^2 + r_{\mathrm {cs}}^2 - d_\mathrm {safe}^2}{2 r_{\mathrm {cg}} r_{\mathrm {cs}}} \right) \end{aligned}$$

(7)

where $r_\mathrm {os}$ is the distance to $\mathbf {p}_{\mathrm {os}}$.

Fig. 1.

Locating the closest gap waypoint.

2.3 Obstacle Avoidance Method

While driving the robot towards $\mathbf {p}_{\mathrm {cg}}$, the TCG+ will adjust the trajectory by rotating $\mathbf {p}_{\mathrm {cg}}$ by an angle $\mathrm {\Gamma }$ temporarily in order to avoid the risk of collision with nearby obstacles. In [21], the rotation angle is computed so that the robot takes the direction of the tangent to the closest obstacle point. In case of unstructured environments, the position of this point may change rapidly resulting in an abrupt change in the robot’s trajectory. Sharp turns may also occur due to the transition from following an obstacle boundary to resuming the progress towards the goal and vise versa. The proposed approach solves these drawbacks by using all nearby obstacle points in computing the rotation angle. It also balances the transition between avoiding obstacles and approaching the goal.

The key idea of the TCG+ is the computation of the rotation angle caused by each of N obstacle points considered as a hazard while driving the robot towards $\mathbf {p}_{\mathrm {cg}}$. An obstacle point $\mathbf {p}_{i}$ is a hazard if the distance to it measured from the robot boundary is less than $D_\mathrm {s}$ and the angular distance between $\mathbf {p}_{\mathrm {cg}}$ and $\mathbf {p}_{i}$, relative to the laser scanner coordinate system is less than $\frac{\pi }{2}$, i.e.:

$$\begin{aligned} ||\mathbf {p}_{\mathrm {r}} - \mathbf {p}_{i} ||\le R + D_\mathrm {s} \, \wedge \, \mathrm {dist} (\theta _{\mathrm {cg}}, \theta _i) \le \frac{\pi }{2} \end{aligned}$$

(8)

The TCG+ method considers dividing the workspace into two subspaces; one is to the right of the robot’s direction of motion (to the right of $\overrightarrow{\mathbf {p}_\mathrm {r} \mathbf {p}_{\mathrm {cg}}}$) while the other is to the left. A hazard $\mathbf {p}_{i}$ located in any of the two subspaces causes a rotation angle ($\gamma _i$) to $\mathbf {p}_{\mathrm {cg}}$ based on its position, the position of $\mathbf {p}_{\mathrm {cg}}$, and the clearance to obstacles located on the other subspace. Assume that the subspace including $\mathbf {p}_{i}$ is denoted by ${S}_{i}$ and the other is $\hat{S_i}$. We consider the clearance to the obstacle point closest to the robot’s boundary $\hat{\mathbf {p}_c}$, among those falling in $\hat{S_i}$, since it poses the highest risk.

$$\begin{aligned} \hat{\mathbf {p}_c} = \underset{\mathbf {p}}{{\text {argmin}}} ||\mathbf {p} - \mathbf {p}_{\mathrm {r}} ||, \quad \mathbf {p} \in \hat{S_i} \end{aligned}$$

(9)

The value of $\gamma _i$ is determined so that $0.5\,||\mathbf {p}_{i} - \hat{\mathbf {p}_c} ||$ is maintained to whichever of $\mathbf {p}_{i}$ and $\hat{\mathbf {p}_c}$ is closer to the robot boundary. By this means, the clearance to obstacles is maximized as the robot moves towards $\mathbf {p}_{\mathrm {cg}}$. Moreover, oscillations occurring in narrow openings due to changing the location of the closest obstacle are reduced. If $\hat{S_i}$ doesn’t contain any hazard, $\gamma _i$ is set in such a way that the robot points parallel to the tangent of $\mathbf {p}_{i}$:

$$\begin{aligned} \gamma _i = \left\{ \begin{array}{l l} \text {sat}_{\left[ 0, \chi \right] } \left( \text {dist} \left( \zeta , \theta _\mathrm {cg} \right) \right) \, \beta - \chi \, \beta , &{} \quad \text {if}\,\, \left| \zeta - \theta _\mathrm {cg} \right| \le \pi \\ \chi \, \beta - \text {sat}_{\left[ 0, \chi \right] } \left( \text {dist} \left( \zeta , \theta _\mathrm {cg} \right) \right) \, \beta , &{} \quad \text {otherwise} \end{array} \right. \end{aligned}$$

(10)

with $\zeta$, $\beta$ and $\chi$ are given by:

$$\begin{aligned} \zeta&= \left\{ \begin{array}{l l} \theta _{i}, &{} \quad \text {if no hazard falls in}\,\, \hat{S_i}\\ \theta _{\mathrm {min}}, &{} \quad \text {otherwise} \end{array} \right. \end{aligned}$$

(11)

$$\begin{aligned} \beta&= \left\{ \begin{array}{l l} 1, &{} \quad \text {if}\,\, \zeta \ge \theta _\mathrm {cg}\\ -1, &{} \quad \text {otherwise} \end{array} \right. \end{aligned}$$

(12)

$$\begin{aligned} \chi&= \left\{ \begin{array}{l l} \frac{\pi }{2}, &{} \quad \text {if no hazard falls in}\,\, \hat{S_i}\\ \arcsin \left( \frac{\frac{1}{2} ||\mathbf {p}_{i} - \hat{\mathbf {p}_{\mathrm c}} ||}{r_{\mathrm {min}}} \right) , &{} \quad \text {otherwise} \end{array} \right. \end{aligned}$$

(13)

where $\theta _{\mathrm {min}}$ and $r_{\mathrm {min}}$ denote the angle and distance to whichever of $\mathbf {p}_{i}$ and $\hat{\mathbf {p}_{\mathrm c}}$ is closer to the robot’s boundary, respectively, and $\mathrm {\theta }_{i}$ the angle towards $\mathbf {p}_{i}$. The value of $\beta$ determines the correct direction of escaping towards the closest gap. Having a closer look at Eq. (10), it is apparent that the value of $\gamma _i$ becomes 0 if the angular distance between $\zeta$ and $\theta _\mathrm {cg}$ gets greater than $\chi$ (a leaving condition). Notice that if no hazard falls in $\hat{S_i}$ and $\mathbf {p}_i$ is the obstacle point closest to the robot, the effect of (10) is analogous to the virtual rotation angle from [21].

Hazards falling within the right subspace cause positive rotation angles, while hazards falling in the left subspace cause negative rotation angles. The average positive and negative rotation angles are computed separately. This is to assign the same weight (relative importance) to hazards located on both sides of the direction of motion, even if one side has a large number of hazards compared to the other. In this regard, we avoid the problem of movement close to obstacles (or even hitting obstacles in narrow corridors) falling on the side with fewer hazards (see [20] for more details). Let ${N}_\mathrm {pos}$ and ${N}_\mathrm {neg}$ represent the number of hazards causing positive and negative rotation angles, respectively. The average positive and negative rotation angles are then defined as follows:

$$\begin{aligned} \mathrm {\Gamma }_{\mathrm {pos}}&= \frac{\sum _{i = 1}^{N_\mathrm {pos}} \gamma _i}{N_\mathrm {pos}}, \quad {\gamma _i > 0}\end{aligned}$$

(14)

$$\begin{aligned} \mathrm {\Gamma }_{\mathrm {neg}}&= \frac{\sum _{i = 1}^{N_\mathrm {neg}} \gamma _i}{N_\mathrm {neg}}, \quad \gamma _i < 0 \end{aligned}$$

(15)

Finally, the rotation angle $\mathrm {\Gamma }$ is defined as the average of $\mathrm {\Gamma }_{\mathrm {pos}}$ and $\mathrm {\Gamma }_{\mathrm {neg}}$:

$$\begin{aligned} \mathrm {\Gamma } = \frac{\mathrm {\Gamma }_\mathrm {pos} + \mathrm {\Gamma }_\mathrm {neg}}{2} \end{aligned}$$

(16)

The location of the waypoint after rotating it by $\mathrm {\Gamma }$ is denoted by $\tilde{\mathbf {p}_\mathrm {g}}$ and computed as follows:

$$\begin{aligned} \tilde{\mathbf {p}_\mathrm {g}} = \begin{bmatrix} \cos (\mathrm {\Gamma })&\sin (\mathrm {\Gamma }) \\ -\sin (\mathrm {\Gamma })&\cos (\mathrm {\Gamma }) \end{bmatrix} \mathbf {p}_{\mathrm {cg}} \end{aligned}$$

(17)

The TCG+ method considers limiting the robot’s speed based on the distance to nearby obstacles [20, 21]:

$$\begin{aligned} v_\mathrm {cut} = \sqrt{1 - \mathrm {sat}_{[0, 1]} \left( \frac{D_{vs} - r_{c}}{D_{vs}} \right) } \,. \, v_\mathrm {max} \end{aligned}$$

(18)

where $D_{vs}$ is a parameter that determines how much the speed is limited, $v_\mathrm {max}$ the maximum linear speed of the robot, and $r_{c}$ the distance towards the obstacle point closest to the robot boundary.

For driving the robot towards $\tilde{\mathbf {p}_\mathrm {g}}$, we use the same motion commands proposed in the TCG method [21]:

$$\begin{aligned}&v = k_\mathrm {brake} \, v_\mathrm {cut}\cos (\tilde{\theta _\mathrm {g}}) \end{aligned}$$

(19)

$$\begin{aligned}&w = \mathrm {sat}_{[-w_\mathrm {max}, w_{max}]} \left( k_\mathrm {max} \, \tilde{\theta _\mathrm {g}} + \frac{v\sin (\tilde{\theta _\mathrm {g}})}{\tilde{r_\mathrm {g}}} \right) \end{aligned}$$

(20)

where $\tilde{r_\mathrm {g}}$ and $\tilde{\theta _\mathrm {g}}$ are the distance and angle towards $\tilde{\mathbf {p}_\mathrm {g}}$, respectively, and $w_\mathrm {max}$ the maximum angular speed of the robot. The value of $k_\mathrm {brake}$ in (19) is set to $\tanh \left( \tilde{r_\mathrm {g}} \right)$, if $\tilde{\mathbf {p}_\mathrm {g}} = \mathbf {p}_\mathrm {g}$ (to ensure smooth braking while reaching the goal), and to 1 otherwise. The parameter $k_\mathrm {max}$ in (20) is used to limit the angular speed of the robot. Having in mind that the maximum angular velocity in (20) corresponds to $\frac{\partial w}{\partial \tilde{\theta _\mathrm {g}}} = 0$, the value of $k_\mathrm {max}$ can be defined as follows:

$$\begin{aligned} k_\mathrm {max} = \frac{w_\mathrm {max} - \frac{1}{2} k_\mathrm {beak} v_{cut}}{\frac{\pi }{4}} \end{aligned}$$

(21)

3 Simulations

In this section, the differences in the behavior of the TCG and TCG+ methods are demonstrated using the stage robot simulator. They were implemented using the Robot Operating System (ROS) [25]. The simulated robot has a rectangular shape (0.52 m, 0.48 m) and works in a differential-driven mode. The maximum velocities were set to (0.5 m/s, 1.0 rad/s) while the safe distances ($D_s, D_{vs}$) were set to (0.7 m, 0.9 m). The sensing system adopted is a laser scanner which delivers 1100 measurements over $360^\circ$ and covering a range of 10 m.

For these simulations, two environments with many narrow and curved passages were created, mimicking unstructured environments, as shown in Fig. 2.

Fig. 2.

Simulations. (a, b) Path followed by (a) TCG and (b) TCG+ methods in scenario 1. (c, d) Path followed by (c) TCG and (d) TCG+ in scenario 2. (e, f) Translational and rotational velocities versus time for (e) TCG and (f) TCG+ in scenario 1. (g, h) Translational and rotational velocities versus time for (g) TCG and (h) TCG+ in scenario 2.

The route chosen by the TCG method in the first scenario is shown in Fig. 2a. At the starting point, the leaving condition is fulfilled since the angular distance between the closest obstacle (on the side labeled A here) and $\mathbf {p}_{\mathrm {cg}}$ (falling within gap G1 created by A and B obstacles) exceeds $\pi / 2$. In such a case, the robot navigates directly towards $\mathbf {p}_{\mathrm {cg}}$. While navigating through G1, the distance to side B obstacles gets less than the distance to side A obstacles and $\mathbf {p}_{\mathrm {cg}}$ gets inside gap G2 created by B and C obstacles. At this moment, the angular distance between the closest obstacle and $\mathbf {p}_{\mathrm {cg}}$ is less than $\pi / 2$. Therefore, the robot moves parallel to the tangent of side B obstacles until having fulfilled the leaving condition. This behavior continues until having passed all gaps. The frequent transition between following an obstacle boundary and resuming the progress towards $\mathbf {p}_{\mathrm {cg}}$, and the quick variation in the location of the closest obstacle cause rapid changes in the steering angle. This can be interpreted from the large spikes visible in the velocity profile shown in Fig. 2e. The behavior of the TCG method in the second scenario is roughly similar to that of the first one as depicted in Figs. 2c and g.

By applying the proposed TCG+ approach, the robot managed to navigate both obstacle courses much smoother than the TCG method as can be seen from Figs. 2b and d. For example, see the trajectories of both methods at the points labeled 1–5 in Figs. 2a, b, c and d. We have confirmed our visualization by plotting the recorded motion commands versus time in Figs. 2f and h.

Fig. 3.

Experiments. (a) Experimental setup for scenario 1. (b, c) Path followed by (b) TCG and (c) TCG+ in scenario 1. (d) Experimental setup for scenario 2. (e, f) Path followed by (e) TCG and (f) TCG+ in scenario 2. (g, h) Velocities versus time for (g) TCG and (h) TCG+ in scenario 1. (i, j) Velocities versus time for (i) TCG and (j) TCG+ in scenario 2.

4 Experimental Results

The simulation results have been confirmed using our rescue mobile robot GETbot, a skid-steering Pioneer 3-AT equipped with two laser scanners and an on-board computer. One laser scanner is located at the front of the robot while the other is located at the back. The front laser scanner is a Hokuyo UTM-30LX having an angular resolution of $0.25^\circ$ and a field of view of $270^\circ$ with a maximum range of 30 m. The rear scanner is a Hokuyo URG-04LX having an angular resolution of $0.35^\circ$ and a field of view of $240^\circ$ covering 5.6 m. We merged the range data acquired from both laser scanners to get a $360^\circ$ field of view. The robotic platform is rectangular ($0.52 \times 0.48$ m) with non-holonomic constraints. The maximum robot speeds ($v_\mathrm {max}$, $w_\mathrm {max}$) are 0.7 m/s and 2.4 rad/s, respectively. The safe distances ($D_s, D_{vs}$) were set to (0.7 m, 0.9).

Next, we outline two experiments carried out using GETbot where the goal was the only information provided to the robot in advance². For the first experiment, we created an environment similar to the RoboCup Rescue Arena as shown in Fig. 3a. Notice that a stable and smooth navigation in such arena reduces the possibility of wheel skidding over ramps, and therefore of great importance in rescue missions. The maximum robot velocities ($v_\mathrm {max}$, $w_\mathrm {max}$) were limited to (0.5  m/s, 1.0  rad/s), since moving fast over ramps is risky. The route chosen for the second experiment contained places where the room available to maneuver is reduced as shown in Fig. 3d. Therefore, the maximum velocities were limited to (0.4 m/s, 0.8 rad/s). Both experiments have been carried out using the implementation of the TCG and TCG+ methods.

The trajectories followed by the TCG method in experiments 1 and 2 are shown in Figs. 3b and e, respectively. The robot was able to traverse the courses of both experiments and reach the goal. However, sharp changes in the direction of motion occurred. For example, see the trajectory while traveling through the starting area (passage P1) and while leaving the passage labeled P2 in Fig. 3b. The behavior was much worse in the second experiment as the motion was oscillatory almost during the complete mission as shown in Fig. 3e. Moreover, the robot moved close to the obstacles labeled A - D in the figure. This is due to the fact that the environment is composed of very narrow passages with different obstacle shapes and sizes rather than straight walls (maze-like environment) as in the first experiment. These frequent changes in the direction of motion are a result of performing obstacle avoidance based only on the closest obstacle point. In tight passages, the location of this point quickly varies as the robot moves. Moreover, the robot was switching between moving in the direction of the tangent to the closest obstacle boundary and moving directly towards $\mathbf {p}_{\mathrm {cg}}$. The latter occurred whenever the leaving condition was satisfied. We recorded the linear and angular velocities over the course of the experiments. Figures 3g and i show these velocities plotted versus time where the large spikes, particularly in the angular velocity profile, indicate the oscillatory motion. By taking all nearby obstacles into account and by considering the distribution of obstacles to both sides of the direction of motion, TCG+ avoided this limitation while still emphasizing the closest threat on each side (see Eq. (10)). Figures 3c, f, h and j demonstrate the increased stability and smoothness of the trajectories generated by the proposed TCG+ method.

5 Evaluation

The performance of the proposed TCG+ approach is evaluated and compared to that of the TCG method based on the following metrics:

• Execution Time ($T_{tot}$). The total amount of time the robot needs to reach the goal. A low execution time is desirable for better performance.

• Accumulated Jerk ($J_{acc}$). The jerk (third time derivative of position) reflects the abrupt changes in the forces exerted by the robot actuators. Hence, smoothness can be quantified as a function of jerk. Given the translational velocity of the robot at each time step, we define the accumulated jerk metric as follows:

  $$\begin{aligned} J_{acc} = \frac{1}{T_{tot}}\int _{0}^{T_{tot}} {\left[ \ddot{v}(t) \right] }^2\, dt \end{aligned}$$

  (22)

  Having less accumulated jerk is desirable as it indicates less oscillatory, more stable, and smoother behavior.

• Average Bending Energy ($B_{avg}$). The bending energy is a measure of the energy requirement of the robot motion, and thus can be used to assess the smoothness of the trajectory. Given the curvature k at any point along a trajectory, the average bending energy is given by [26]:

  $$\begin{aligned} B_{avg} = \frac{1}{n} \int _{x_0}^{x_n} k^2 (x)\, dx \end{aligned}$$

  (23)

  where

  $$\begin{aligned} k (x) = \frac{{f}'' (x)}{\left( 1 + ({f}' (x))^2 \right) ^{\frac{3}{2}}}\, dx \end{aligned}$$

  (24)

  A low $B_{avg}$ is preferred as it is an indication of an increased smoothness.

Based on the aforementioned metrics, a performance evaluation of the TCG+ approach was performed. Table 1 shows the results obtained for the simulations and experiments presented in Sects. 3 and 4. A significant improvement in the performance of the TCG+ over the TCG can be observed in all tests conducted. We believe that this enhancement is due to the fact that the TCG+ calculates $\mathrm {\Gamma }$ based on the location of all nearby obstacles and balances the transition between avoiding obstacles and approaching the goal. It also considers the distribution of obstacles to both sides of the direction of motion.

Table 1. Performance evaluation of the TCG and TCG+ methods.

6 Conclusions

This paper presents the TCG+ navigation approach for reactive obstacle avoidance. The TCG+ adapts the earlier developed Tangential Closest Gap method by making use of all close obstacle points in determining the motion direction and by providing a smoother bridge between obstacle avoidance and goal approach. The trajectories generated by the TCG+ are smoother and more stable when compared to those generated by the TCG. Additionally, taking into account the distribution of obstacles to both robot sides reduces oscillations in narrow corridors occurring in the TCG method. Simulation and experimental results demonstrated the effectiveness of the proposed approach.