^{1}

^{*}

^{2}

^{1}

^{1}

^{2}

This article is an open access article distributed under the terms and conditions of the Creative Commons Attribution license (http://creativecommons.org/licenses/by/3.0/).

The goal of this paper is to solve the problem of dynamic obstacle avoidance for a mobile platform by using the stochastic optimal control framework to compute paths that are optimal in terms of safety and energy efficiency under constraints. We propose a three-dimensional extension of the Bayesian Occupancy Filter (BOF) (Coué

Autonomous vehicles have nowadays become popular for applications such as surveillance and passenger transport. In both cases the safety and efficiency of these systems depends on the ability of the autonomous navigation system to deal with unpredictable dynamic changes in the environment. Autonomous navigation systems have been studied extensively in the literature [

The localisation stage can be greatly improved by using Global Positioning System (GPS) or its enhanced version: the Differential GPS (DGPS) [

For autonomous vehicles to achieve safe operation, understanding safe operation as an obstacle avoidance task that preserves the integrity of the robot and the other objects or people in a dynamic environment, a combination of a priory map with sensory information that comes from sensor fusion [

In addition to localisation, a robust autonomous mobile platform requires an obstacle avoidance system. Such system ensures that a vehicle navigates safely around the obstacles while trying to reach its goal. Obstacle avoidance can be divided into global and local approaches. While the former approach assumes a complete model of the environment, such as the potential field methods, the local methods require only partial observability of the environment at the cost of guaranteeing only local optimality. However, computational cost is much lower for local methods and they can be often implemented in the form of reactive controllers. These reactive methods take control of the robot when an obstacle is detected to prevent collision. They use the nearest portion of the environment modelled using the current sensor observation. Some representative examples are Vector Field Histogram (VFH+) [

One of the major drawbacks of the reactive methods is that they do not take into account the dynamic changes of the environment and assume that all obstacles are static. Therefore, they can not predict their motion. This is, however, an unrealistic assumption especially when the vehicle deals with a high uncertainty over the position, shape and velocity of the obstacles and it is still a challenge for real world applications [

This paper, which extends our previous work recently sketched in [

The rest of the paper is organized as follows: Section 2 shows the proposed method; Section 3 describes the experiments and the actual results; and finally, in Section 4 we conclude and discuss the future works.

In this section we propose an extension of the method for avoiding dynamic obstacles in three dimensions while minimising the energy consumption. To tackle this problem, we obtain a probabilistic model of the dynamic environment, then we use this model to predict the motion of the obstacles inside our perception stage and finally, we use optimal path planning using approximate inference to improve the planning stage based on an energy consumption model.

We use the Bayesian Occupancy Filter (BOF) [

The Bayesian Occupancy Filter can be implemented as a loop of a prediction and estimation steps. The authors of [^{t}^{t}^{−1}, ^{t}^{−1}) is then the transition probability defined by the vehicle dynamics. The standard BOF framework has several issues with the velocity estimation. Firstly, this framework assumes that the velocity of each grid cell is constant [_{x}_{y}

We have improved the system in terms of efficiency, by adding a stage to detect the relative velocities of the obstacles without assuming constant velocity objects or discretised velocities.

Firstly, we capture a frame using the laser data. This frame has the form of a zenith image of the detected obstacles. We then estimate the movement of the obstacles between two frames by computing the pyramidal implementation of Lukas Kanade optical flow algorithm [

The output of our probabilistic model is illustrated in

In order to compare our method to the classical obstacle avoidance algorithms in terms of energy, we only take into account the power demand of the robot's motors. We assume that the rest of the equipment has constant energy consumption and therefore, it cannot be improved any further. We also assume that the power demands of the robot's motors are based on overcoming inertia, road grade, tyre friction, and aerodynamic loss. This road-load methodology was mainly introduced by [^{2}, ^{2}), _{G}_{R}^{3}), _{D}_{F}^{2} (≈ 1 m^{2} in our case).

The robot speed that we use to obtain the power demand is provided by the kinematic model of the robot based on the angular speed of the wheels for each time step (100 ms in our case). According to this, we assume that the robot is moving with linear speed between execution steps. On the other hand, our planning and almost all the classical algorithms do not allow the robot to describe sharp turns or spin.

The stochastic optimal control has been successfully used to solve optimisation problems in robotics [_{t}_{x}_{y}_{x}_{y}_{t}_{t}_{t}_{t}_{t}_{t}_{t}_{t}_{0}_{T}_{x}_{t}_{u}_{t}_{t}_{t}_{t}_{t}_{t}_{t}_{t}_{0:T}, _{0:T})). The costs _{0:T}, _{0:T}) play the role of the neg-log-probability of _{t}_{t}_{0:T}∣_{0:T} = 1) over the state trajectories to solve the path planning problem using the Gaussian message passing algorithm. This involves combining the forward (_{xt−1}_{→xt}), backward ((_{t}_{xt+1→xt}) and cost messages ((_{t}_{wt→xt}(_{t}

The cost function in AICO is defined by task variables. Each task variable defines a task space and a squared metric is used to compute the cost inferred in this space. However, inference-based path planning with the linearised motion model and the holonomic constraint is difficult and suffers from problems with local minima due to the velocity constraints. The reasoning behind this is that the Gaussian distribution over the state space can potentially assign probability mass to states that do not satisfy the holonomic constraint, which either causes sideways slipping in the model or if we constrain the Gaussian itself the distribution becomes degenerate. For this reason, we have excluded the orientation from the state and we have added an additional cost term to penalise for angular velocity instead. We assume that arbitrary angular velocities can be executed but optimise for low angular velocities. This reduces the complexity of the state space by turning the hard holonomic constraint into a soft constraint.

In order to achieve robustness and safety of our vehicle, we use a method that yields free paths that tend to maximise the clearance between the vehicle and the obstacles based on a Voronoi graph. The aim of the global planning is to keep the vehicle at a safe distance from the surrounding obstacles. We compute the initial path using graph search on the Voronoi graph. This path is then used as AICO initialisation and it helps to deal with local minima.

In this section we describe the implementation of our system and the experimental results. The results have been obtained from the real Seekur Jr. platform and the simulator provided by Mobilerobots.

The results have been evaluated in two stages: firstly we evaluate the gain of using the probabilistic model of the environment inside the perception stage of the classical algorithms (VFH+, CVM, LCM and BCM). Secondly, we compare our whole dynamic obstacle avoidance system with the classical algorithms.

We have tested our system in an outdoor environment in the South Parking of the Polytechnic School at the University of Alcalá (UAH). The overall area of the environment is approximately 70 × 70 ^{2} (

The robot used in the experimentation was a Seekur Jr. (

We used four classical algorithms on the real platform and analysed the effect of using our proposed probabilistic model. For simplicity, we only take into account the

Parallel: For this test, the robot is located in the middle of a 5 meters wide corridor (

Perpendicular: For this test the robot is located at the crossroads (

For all of these experiments, we have analysed the following parameters:

Path curvature: The assumption is that the smoother the path, the lower the energy consumption.

Acceleration (^{−2}) ignoring energy regenerated from breaking.

Velocity (^{−1}).

Time (

Energy (

As an example, the top part of the

The aim of this experiment is to show that our system computes safe paths, reaching the goal configuration optimally with respect to energy consumption. Here we show that the performance of the reactive methods can be further improved by optimising the motion with respect to energy. The inference based planner described in Section 2.3 is initialized using the path computed from the Voronoi graph of the environment including only static obstacles. Then AICO computes the initial optimal path from start to goal positions (

We have used our probabilistic model of the environment to detect the position and velocity of dynamic obstacles in the robot coordinate frame which we have then mapped into the global coordinate frame. We use this information to predict the movement of these obstacles. AICO is then used to compute the optimal trajectory around the initial path while using the probabilistic model predictions about the dynamic obstacles. A set of task variables has been used to define the optimality: position, power demand (

Only small changes in the environment between two time steps are expected. In such situations AICO requires only small number of iterations to converge which makes re-planning computationally affordable. The replanning time (3 s on average) is however too long to be use as a reactive controller.

We have applied our proposed method to perform the task while avoiding the obstacles and minimising energy consumption. AICO solves the finite horizon optimisation problems which means that the duration of the trajectory needs to be specified a priori. It is not within the scope of this paper to optimise for time, we have therefore set the trajectory duration to 20 s for parallel scenario and 35 s for perpendicular scenario, which are the respective average durations as computed using the reactive methods with our probabilistic model.

In this paper, we have presented a method for avoiding dynamic obstacles, taking into account not only the integrity of the system, but also the minimization of the energy consumption. We have proposed a probabilistic model of the dynamic environment using: (1) an extension of the Bayesian Occupancy Filter proposed by [

We have used the probabilistic model of the environment inside the perception stage of the four classical methods: VFH+, BCM, CVM and LCM. Just by using a single level of the occupancy grid we were already able to improve the perception stage of these avoidance systems. The results have shown an improvement in energy consumption up to 45.5%. We have also shown that the resulting trajectories as well as the velocity and acceleration profiles are much smoother when using our method.

We have implemented a Voronoi graph based global path planner which serves as an initialisation method for our inference based local planner: AICO. Within AICO we combine multiple task variables to obtain the optimal path based on obstacle clearance and energy consumption. This method shows further qualitative improvement against the reactive methods with an improvement in energy consumption of 10.91% and 16.51% respect to the best results of reactive methods in perpendicular and parallel scenarios respectively. The computational cost of using AICO is currently too high for it to be used in real-time planning applications.

In near future, we intend to improve the accuracy of the probabilistic model by refining the discretisation. We also intend to improve the calculation of the probabilistic model to speed up the algorithm to accommodate for higher number of cells. The planning algorithm can be improved by solving the inference at multiple scales and by exploiting parallel computation. Furthermore, we will address issues of coverage using topology based properties such as winding numbers [

This work has been funded by TIN2011-29824-C02-01 and TIN2011-29824-C02-02 (ABSYNTHE project) from the “Ministerio de Economía y Competitividad” and a grant of the Fundación Caja Madrid.

Discretisation levels.

Probabilistic model of the dynamic environment.

Simulated environment and sensing results.

Graphical model of AICO in configuration and task space.

Two examples of optimal paths computed using AICO in a static environment.

Test environment. (

Seekur Jr. used in the experimentation.

Path using VFH+ and AICO: parallel scenario.

Path using VFH+ and AICO: perpendicular scenario.

Velocities and accelerations using LCM and AICO: parallel scenario.

Velocities and accelerations using LCM and AICO: perpendicular scenario.

Energy consumption using VFH+ algorithm.

Summary of results for energy consumption. AICO is being compared with the best reactive method for the given scenario.

Scenario | Perception stage | Total Energy Consumption (J) | Reduction of Consumption (%) | |
---|---|---|---|---|

VFH+ | Parallel | Raw laser data | 0.21404 | –18.56% |

Probabilistic model | ||||

Perpendicular | Raw laser data | 0.12495 | –29.26% | |

Probabilistic model | 0.088383 | |||

CVM | Parallel | Raw laser data | 0.22615 | –7.52% |

Probabilistic model | 0.20914 | |||

Perpendicular | Raw laser data | 0.10458 | –4.39% | |

Probabilistic model | 0.099981 | |||

LCM | Parallel | Raw laser data | 0.20125 | +2.89% |

Probabilistic model | 0.20709 | |||

Perpendicular | Raw laser data | 0.09097 | −8.1295% | |

Probabilistic model | ||||

BCM | Parallel | Raw laser data | 0.20546 | –7.29% |

Probabilistic model | 0.19047 | |||

Perpendicular | Raw laser data | 0.16599 | −45.5% | |

Probabilistic model | 0.090454 | |||

AICO | Parallel | Probabilistic model | −16.51% | |

Perpendicular | Probabilistic model | −10.91% |