Path Planning in Dynamic Environments Using Time-Warped Grids and a Parallel Implementation

This paper proposes a solution to the problem of smooth path planning for mobile robots in dynamic and unknown environments. A novel concept of Time-Warped Grid is introduced to predict the pose of obstacles in the environment and avoid collisions. The algorithm is implemented using C/C++ and the CUDA programming environment, and combines stochastic estimation (Kalman filter), harmonic potential fields and a rubber band model, and it translates naturally into the parallel paradigm of GPU programming. In simple terms, time-warped grids are progressively wider orbits around the mobile robot. Those orbits represent the variable time intervals estimated by the robot to reach detected obstacles. The proposed method was tested using several simulation scenarios for the Pioneer P3-DX robot, which demonstrated the robustness of the algorithm by finding the optimum path in terms of smoothness, distance, and collision-free, in both static or dynamic environments, and with large number of obstacles.

READ FULL TEXT VIEW PDF
POST COMMENT

Comments

There are no comments yet.

Authors

page 1

page 2

page 3

page 4

06/27/2019

A Constant-Factor Approximation Algorithm for Online Coverage Path Planning with Energy Constraint

In this paper, we study the problem of coverage planning by a mobile rob...
10/19/2017

RRT* Combined with GVO for Real-time Nonholonomic Robot Navigation in Dynamic Environment

Challenges persist in nonholonomic robot navigation for dynamic environm...
06/22/2018

GONet++: Traversability Estimation via Dynamic Scene View Synthesis

Robots that interact with a dynamic environment, such as social robots a...
08/23/2020

Mobile Robot Path Planning in Static Environments using Particle Swarm Optimization

Motion planning is a key element of robotics since it empowers a robot t...
01/30/2019

Walking Posture Adaptation for Legged Robot Navigation in Confined Spaces

Legged robots have the ability to adapt their walking posture to navigat...
06/13/2020

Decentralized decision making and navigation strategy for tracking intruders in a cluttered area by a group of mobile robots

In the current era of the industrial revolution, mobile robots are playi...
This week in AI

Get the week's most popular data science and artificial intelligence research sent straight to your inbox every Saturday.

1 Introduction

The problem of path planning of mobile robots consists of the trajectory planning of a mobile robot from a start state to a final state. The objective of such start-to-goal path planning is to find the best suitable path between two points while avoiding collisions with both static and moving obstacles. In this case, the suitability of the “best” path is determined by a function representing the occupancy of the space, while the selected robot trajectory corresponds to the optimal path on the surface of this occupancy function according to some criteria (e.g. smoothness, length, etc.).

In such problems, the trade off between performing quick, shallow searches for sub-optimal solutions and performing deep searches for the price of heavy computation has always been a practical and important issue. The application of parallel algorithms however, can minimize or even eliminate this issue by enabling deeper searches without losing accuracy or real-time applicability. In addition to saving time, parallel computation can lead to extra advantages [Barney10] such as solving the same problems at larger scales, providing redundancy of control, overcoming memory constraints, and performing remote processing (e.g. tele-operation of robots).

However, these advantages do not come with a low price tag. For effective use of parallel computing, the computational problem must be partitioned into discrete parts, or tasks, to be executed simultaneously. This partitioning into discrete parts is carried out by identifying independent and self-contained sub tasks where the overall result can be readily extracted from the sum of the individual results. As it will be explained in further details later, this can be a daunting operation.

In this paper, a solution to the problem of smooth path planning for mobile robots in dynamic and unknown environments is proposed. A novel concept of time-warped grid is introduced to predict the pose of obstacles in the environment while avoiding collisions with those obstacles. The concept combines stochastic estimation (Kalman filter), harmonic potential fields and a rubber band model, and it translates naturally into the parallel paradigm of GPU programming. The intuition behind the proposed method for dealing with dynamic environments is derived from the analogy of pedestrians “jaywalking” or crossing streets on which vehicles are driving. When jaywalking, pedestrians anticipate where the vehicles will be at future times and adjust their path accordingly. If a person were to consider only a snapshot of the street and assume a fixed position for each vehicle, then he/she would be in a significant risk of being run over if tried to cross the street. Similarly in this work, the robot predicts the future configuration of moving obstacles by using a combination of Kalman filter and time-warped grids. Consequently, a path is plotted for the future positions of the obstacles, rather than the current ones.

In simple terms, time-warped grids are progressively wider orbits around the mobile robot. Those orbits represent the variable time intervals estimated by the robot to reach detected obstacles. The main idea of time-warped grid is to acknowledge that the further the obstacle is, the more delayed is its impact on the path. The use of time-warped grid allows the system to address at the same time, the problems of convergence, speed, and moving obstacles in the calculation of a smooth path for the mobile robot without any prior knowledge regarding the environment. All assumptions made by the system derive from a laser sensor mounted on the robot and a localization system (e.g. vision-based landmark localization [DeSKak02]) that provides distance to the goal, even when it lies outside the range of the robot sensors in large environments.

As the experiments performed demonstrate, the path obtained by the predictive aspect of our method is not only short, but it also contains no loops, no sharp turns, and no changes of speed of the robot, making it ideal for carrying of delicate materials or for wheelchair navigation. Our experiments also demonstrate the robustness of the method, which can always find an optimum path – i.e in terms of smoothness, distance, and collision-free – either in static or dynamic environments, even with a very large number of obstacles.

2 Related Work

Unlike coverage path planning where the emphasis is in sweeping out the space sensed by the robot – e.g. floor cleaning, lawn mowing, harvesting, etc. [Cho01], the objective of start-goal path planning is to find suitable paths between two points while avoiding collision with static or moving obstacles. The ability to generate an optimal path from an initial point to a final destination in real-time is still one of the key challenges in Robotics. The area is becoming even more influential with the near deployment of self-driving autonomous vehicles [Bast16].

Different techniques have been proposed in this domain, where the challenges become increasingly difficult with the size of the environment and the number of moving obstacles. From the early algorithms, the focus often turned into finding cost-minimal paths through the robot environment, and the use of maps became so attached to the problem that many confuse start-goal and map-based as synonymous. Indeed, most of the approaches in the literature rely on some sort of map or grid, and the large size of the environment and consequent number of grids rendered many of these methods to off-line use only. Moreover, the complexity and uncertainty of the path planning problem increase greatly in dynamic environments due to the change of the entire information in the environment along with the movement of obstacles.

An early and important algorithm to address the path planning problem in dynamic environments is known as diffusion process over grid-based maps, first presented by Steels [Steels88], and further developed by Schmidt et al. [SchAza92]

. In diffusion method, the environment is discretized into a grid of cells, where the cell representing the goal applies an attracting force in succession to the neighbouring free cells all the way to the cell occupied by the robot. These attractive forces are modeled by fluid diffusion equations, hence the name of the method. The robot proceeds towards the cell with the highest level of force computed by a gradient-based iterative algorithm. The diffusion method can be implemented as a two layer cellular neural network

[Sie94], and use of an unsteady diffusion equation model [SchAza92] makes it applicable to time-varying environments. More recently, Vázquez-Otero et al. [VazMun12] used reaction-diffusion dynamic models based on biological processes with advantages of smoother trajectories and tolerance to noisy data.

Diffusion-based methods allow on-line path planning and result in short and collision-free paths, without suffering from problems with local minima. However, the algorithm is global and require the prior knowledge of the robot’s environment, thus cannot deal with more advanced navigation tasks such as exploration of unknown environment or multi-robot navigation. Moreover, the diffusion equation parameters are difficult to tune, and the implementation is highly computationally demanding, making the algorithm less favorable for real-time or large environment applications.

Borenstein and Koren [BorKor91]

presented an efficient algorithm for path planning called the vector field histogram (VFH) method, further improved by Ulrich and Borenstein

[UlrBor98] [UlrBor00] by considering the robot size and choosing a safe and efficient steering direction. This approach uses a grid-based occupancy mapping, where occupancy information is described by a histogram representation of the robot’s environment. The free space and obstacles are localized by their angle as well as their distance relative to the robot. The final steering direction for the robot is selected (based on specified thresholds and proximity to obstacles) from the candidates closest to the goal direction. Due to the statistical nature of the algorithm, the VFH method is very robust to uncertainties in sensor readings and dynamic models, making it ideal for unknown and time-varying environments. While the method is computationally efficient, it does not result in globally optimal paths (since it is a local path planner) and is also prone to dead-end situations.

The dynamic window approach incorporating the motion dynamics of the robot was proposed in [FoxThr97], and further generalized in [BroKha99]. In this method, the control of the robot is carried out directly in the space of translational and rotational velocities, and the search space for admissible velocities is reduced in three steps over three windows given by: i) approximating trajectories by finite sequences of circular arcs, ii) considering only the next steering command, and iii) restricting the space to the velocities that can be reached within a short time interval. The final path for the robot is generated by maximizing an objective function formed by the intersection of the three windows. Using a dynamic window approach, the acceleration capabilities of the robot are considered, and obstacle avoidance can be performed at high velocities. However, the algorithm is still prone to local minima problems, since it only considers the goal heading without integrating the information about the free space.

Potential field methods, as proposed by [Khatib86], determine the heading direction of the robot by representing the robot environment as a potential field, where the goal applies an attractive force and obstacles assert repulsive forces to the field. The robot trajectory is calculated by superposition of the two fields and following the low potential along the field according to a fastest descent optimization. Methods based on potential fields are computationally efficient and suitable for path planning with real-time conditions. However, they often fail to find trajectories in congested environments, and can also result in oscillatory motions in narrow passages [KorBor91].

Another problem in start-goal planning is regarding the convergence to a solution. In potential fields [HwaAhu92] and the A* algorithm [HarRap68], for example, a guarantee that the system will find a solution either can not be provided at all (potential fields) [Latombe12]

, or it can be provided only if the heuristic is guaranteed to always be optimistic (A*) – i.e. the true cost of a path is at least as large as the estimated cost. For the first case, Kim and Khosla

[KimKho92] proposed instead a harmonic potential function that eliminates the possibility of local minima in the potential fields, which prevented it from finding a path to the goal. In the latter case of A* algorithm, the alternative is to make the heuristic more optimistic, which increases the method’s computational complexity, making it less likely to run in real-time.

As mentioned above, some of these methods require prior knowledge about the environment in a static setting. Violation of this requirement, i.e. existence of moving obstacles, leads to sudden changes and oscillations in the robot path, which can be aggravated by the sensitivity and inaccuracy of the robot sensors. While these consequences may be acceptable for a mobile robot, applications involving autonomous wheelchair navigation can become quite uncomfortable for human passengers. In [HonDes10], it was proposed a robust method using a rubber band model to smoothen the path and reduce the number of sharp angles obtained from the use of harmonic potential fields alone. While that approach worked well for static environments, it did not address the case of moving obstacles.

In order to address the computational complexity of these methods, many researchers have recently developed parallel implementation of path planning algorithms on Graphics Processing Units (GPUs). In [Ble08], the authors proposed a method for globally optimum path planning using a combination of the A* and Dijkstra’s algorithms [Dijkstra59]. The two algorithms were modified to take advantage of data parallelism of GPUs, which led to an implementation of edge lists using adjacency tables to reach a remarkable speed-up when compared to traditional C++ implementations.

Also exploiting the nature of these algorithms and the parallel paradigm of GPU computation, Kider et al. [KidSaf10] proposed a randomized version of A*, called R*GPU search. Their main contributions are certainly the smaller memory requirements when compared to the original A*, the avoidance of local minima by the use of randomly selected subgoals, and the scalability of the method to high-dimensional planning problems.

However, even when computational complexity is not an issue, the major drawback of any A* based method remains in the difficulty in coping with dynamic environments. That is, A* algorithm relies on the optimism of the heuristics. Since those heuristics derive from the values of the map cells, potential changes over time in these values or changes in topology of the graphs due to moving obstacles lead to inversions of those heuristics, and hence to loops and/or failure in converging to the goal. Another problem of the A* algorithm can be seen in the proposed approach in [Russell16]. The main drawback in this approach is that the A* uses uniform grid representation which requires large amount of memory for regions that may never be traversed or may not contain any obstacles, affecting the efficiency of the method. This drawback can also happen in the dynamic version of the A* algorithm called D* [Ste94], even though it indeed generates optimal trajectories in unknown environments.

In this paper, we address at the same time the problems of convergence, speed, and highly dynamic environments in the calculation of a smooth path for a mobile robot without any prior knowledge regarding the environment (static or dynamic). All assumptions made by the system derive from a laser sensor mounted on the robot and a localization system (here assumed to be a vision-based, landmark-based localization [DeSKak02]) which needs to provide only location of the goal, even when it lies outside the range of the robot sensors in large environments.

3 Proposed Method

The proposed method for the path planning problem tackles the limitations of other systems – namely the problems of convergence, fast processing, moving obstacles and sharp paths – by combining a few concepts. First, we rely on harmonic potentials to guarantee the calculation of a path if one exists111A path may not exist only if obstacles completely block the robot’s path to the goal.. As it was pointed out earlier, the path produced by harmonic potentials can present sharp turns, which for many applications, such as autonomous wheelchair navigation, can produce an uncomfortable experience for the passenger. So, we re-introduce the idea of a rubber band model [HonDes10] to smoothen the path created by the harmonic potentials. Since moving obstacles can also lead to unexpected changes in path, we propose the use of Kalman filter for stochastic estimation of the positions of the obstacles. While Kalman filter in itself has been widely used in the past, our main contribution here is in the combination of Kalman filter and a novel idea of time-warped grid, which will be explained later.

As the experiments performed will demonstrate, the path obtained by the predictive aspect of our method is not only the shortest possible path, but it also contains no loops222Unless a path momentarily does not exist due to a large number of obstacles (see Section 4)., no sharp turns, and no changes of speed of the robot, making it ideal for carrying of delicate materials or for wheelchair navigation. Since our method is also relatively computationally intensive, we resort to an efficient parallel implementation using GPUs over the time-warped grids.

3.1 Harmonic Potential Fields

In grid-based maps, the idea is to represent the environment as a 2D grid. Such grid is basically the projection of all objects in the environment – in our case detected using a laser range sensor and a vision system – onto the ground plane. When potential fields are applied on top of such grids, obstacles are described by high potentials or hills that must be avoided, and start and goal points are the highest and smallest potentials, respectively. The path towards the destination is defined along the valleys of the potential field. Unfortunately, due to interaction between multiple objects, valleys are not unique in their potential values. Moreover, besides the possibility of multiple valleys, potential fields can also present local minima. These two conditions can cause the robot to fail to find a path to the goal. However, these same problems disappear when we use harmonic potential fields instead [ConGru93, Robert_08_harmonic].

Harmonic functions satisfy the min-max principle, and hence, spontaneous creation of local minima within the space is impossible. This principle is satisfied when the Laplace’s equation constraint on the functions is true. In other words, a harmonic function on a domain is a function that satisfies:

This same function can be discretized and the numerical solution of Laplace’s equation becomes [ConGru93]:

(1)

where represents the discrete sample of at coordinates of the grid, and denotes the iteration number. At each iteration, a grid cell of is updated with the average value of its neighbors. On a sequential computer, this solution is usually implemented as follows:

(2)

That is, the next values of the top and left neighbors of the current cell are updated and used in the calculation of that same cell. This speed up of the algorithm allows for the values of the next iteration to quickly propagate through the grid. However, it also slightly distorts the real value of the harmonic potentials [HonDes10].

In order to explain the proposed method, a few basic elements need to be revisited. First, a goal is a grid cell with the lowest harmonic value (). This value is fixed and it will never be affected by its neighboring values. An obstacle is any cluster of cells blocking a potential path towards the goal. Its value is maximum () and is also never affected by its neighbors. However, its position may change – e.g. in dynamic environments. Free space is any grid cell that does not contain an obstacle or the goal. The value of the harmonic potential in the free space is initialized with and is updated at each iteration.

A path to the goal is given by an index matrix, , which for every coordinate contains the index of the neighbor with the lowest harmonic potential. That is,

(3)

3.2 Rubber Band Model

As proposed in [HonDes10], the rubber band model is employed to optimize the path obtained by the harmonic potential fields. This idea of rubber band was previously introduced in [Hilgert_03], but mostly to define obstacle contours. Here, we combine the idea of rubber band model and harmonic potentials to define the path as a smoothed linked list of grid cells. The two immediately adjacent cells in the link, i.e. the previous and the next cells along the link from the current cell, exert internal forces on that same cell. Figure 1 illustrates this idea for the cell and its previous and next cells in the path, and , respectively.

resultant force

Figure 1: Tensions exerted by consecutive cells along the path.

Every cell in the path is affected by two kinds of forces: the internal tension (rubber band) forces , and the potential force . The position of a cell in the path is given by the pair that leads to the resultant forces to be minimum. That is:

(4)
(5)

where represents the direction toward the pair in Equation (4), used to move the position of the current cell in the path at each iteration.

Let us assume that the current coordinates of the cell is , and the coordinates of the two neighbors are and , respectively. The resultant of the forces on the cell , as shown by Figure 2, provides the direction and intensity with which the path should be moved in order for the forces to reach equilibrium.

The last component of these calculations is the force derived from the harmonic potential. This force is calculated using:

(6)

where represents the harmonic potential at the current position of the cell in the path given by Equation (1), and represents the harmonic potential of the candidate position to which the cell is being dragged. At each step, the robot moves towards the next optimized position in the path.

Figure 2 summarizes the idea of the harmonic potentials and the internal tension (elastic) forces of the model. In the figure, red lines represent obstacles (walls) and the black area is the desired destination of the robot. The darker the color in free space, the lower the harmonic potential value. Figure 2 also shows how the path obtained from the simple application of harmonic fields (full/blue path) compares to the one being optimized by the rubber band model (dotted/purple path).

moving direction
Figure 2: Here, it is shown the resultant forces on cell , as well as the harmonic potential path (full/blue line) being optimized by the rubber band model (dotted/purple line).

3.3 Kalman Filter

A Kalman filter addresses the general problem of estimating the state of a discrete-time process, which in our case represents the position of a moving obstacle. This state evolves with the step , governed by the following linear stochastic equation [WelBis97]:

(7)

where , the state matrix, relates the state at time step to the state at ; , the input matrix, relates the control input to the state

, and the random variable

represents a process noise. Due to sensor limitations, the state of the system is observed by according to a measurement model given by:

(8)

where the matrix H is the observation matrix, and the random variable

represents the uncertainty of the observation given the current state. Also, all random variables in the Kalman filter are assumed to be independent, normal distributions. That is,

for , for , , and .

The Kalman filter is an iterative method that alternates between two phases: prediction and update. Since in our case, moving obstacles are assumed to move without requiring any input , at each iteration, the Kalman filter predicts the system’s next state through Equations (9) and (10):

(9)
(10)

where is the estimation of the state based on the observations up to time

, and the estimate variance

is the mean squared error in the estimate .

Then during the update phase, the state of the system is refined by:

(11)
(12)

where the Kalman gain, is defined as

(13)

The integration of Kalman filter and the time-warped grid will be discussed in Section 3.5.

3.4 Time-Warped Grid

In a static environment, a path computed by combining the harmonic potentials and the rubber band model above is guaranteed to always exist and to be smooth. However, if obstacles are allowed to freely move in the environment, that same path must be constantly updated using the robot sensors (in our case the laser range sensor and the vision system). In that case, newly sensed positions of the obstacles will lead to new harmonic potentials and hence new smoothened paths to the goal from the rubber band model. This behavior is undesired, since it can force the robot to follow inefficient paths like loops, paths that sharply move the robot away from the goal, and/or paths that bring the robot dangerously close to obstacles. By using time-warped grid and Kalman filter to estimate obstacle velocities, our system is able to estimate future positions of the obstacles and calculate optimum paths despite of moving obstacles.

The idea of time-warped grid was inspired in general relativity, where the fabric of space-time is warped by large gravitational forces. Here, instead of gravitational forces, we use the velocities of the robot and of the obstacles to warp the grid in the environment map. The motivation for that, as we will explain in greater detail next, comes from the consequent assignment of a parallel processor for the computation of the potentials at each cell of the grid – and hence the path of the robot to the goal. Since the grid is warped by the velocities of all moving obstacles in the grid, the path itself becomes a function of those velocities, leading to a path plotted for the future position of those obstacles, rather than the current ones.

First, imagine a grid warped by the velocity of the robot alone. Such grid, depicted in Figure 3, has enlarged squares in front of the robot, i.e. in the direction of motion. If we think of these squares as the space traveled in one unit of time, it becomes obvious that the grids must be larger in front of the robot, smaller behind it, and the same on each side. In fact, in the direction of the motion of the robot, the warped grids stretch like ellipses centered around the position of the robot. Each of such ellipse can be numbered, representing the degree of warping. In order to estimate the future positions of moving obstacles, the robot should find the warps containing moving obstacles and label the obstacle with the corresponding warp number. As a result, if an obstacle moves towards the robot, its label decreases.

main robot

moving obstacles
Figure 3: Time-Warped Grid created by concentric ellipses around the main robot.

The time-warp of a moving obstacle with position of is derived using the equation for a standard ellipse which has been rotated through an angle :

(14)

where are the major and minor axes of the ellipse respectively, and is the orientation of the main robot. The centers of ellipses are calculated based on the position of the main robot , so the ellipses are larger and longer as they get farther from the robot, that is:

The elliptical grids are set for this application such that . Therefore, the time-warp number of a moving obstacle is calculated as the length of the major axis of the ellipse on which the moving obstacle is located:

(15)

After calculating the time-warps, Kalman filter estimates the future position of the moving obstacles for the next ’th step, where is the warp number of the grid occupied by the detected obstacle. Needless to say, Kalman filter algorithm should be initialized with respect to the velocity of the robot and the amount of noise in sensor readings.

The main idea of time-warped grid is to acknowledge that the further the obstacle is, the more delayed should be its impact in the path unless the obstacle also moves with great velocity towards the path. In summary:

(16)

where is the ratio of the velocity of the robot to the moving obstacle and is the warp number i.e. the corresponding ellipse number.

(17)

This estimated position of the obstacle is considered as the possible collision point with the robot and marked on the grid as if it were a fixed obstacle by increasing the corresponding harmonic value. Therefore, this obstacle would only affect the path of the robot if its future position lies in the vicinity of the robot path. Since the further the distance of the robot to the warped grid, the worst is Kalman filter predictions – i.e. the further in the future is the Kalman filter prediction, the less accurate it is – the assignment of “occupied” grids (high harmonic values) uses a Gaussian function. In other words, not only the estimated positions of the obstacles in the future are marked as occupied, but also their neighboring points. This Gaussian function is defined based on the calculated uncertainty given by Kalman filter at each step (Equation (12)) and a desired “safety” distance to obstacles.

At each iteration of the algorithm, the grid is cleared from harmonic potentials, and new values are updated onto the grid. This approach is justified since the estimations from the Kalman filter do not change drastically from one iteration to the next, and the values evolve slowly anyways. Also, at each iteration, the predicted harmonic potentials are used together with the rubber band model to determine a path for the robot to follow. It should be mentioned here that the robot does not have any prior knowledge about the environment, except for the position of the goal.

It should be clear to the reader by now, that the time-warped grid method reduces and simplifies many calculations. For one, it eliminates the need to take the directions of the movement and the absolute value of the distance between the robot and the moving obstacles into consideration for the calculation of the path. In fact, the degree of warping assigned to the grids encodes both that direction and the time factors required for the calculations of the path.

In the next section, it will be further detailed the use of the time-warped grid combined with the Kalman filtering.

3.5 Integration of Kalman Filter with Time-Warps

As mentioned before, in this work the estimation of the position of moving obstacles in the future is done by Kalman filter. It is assumed that the current position of the obstacle is not known accurately due to noise and other errors in the vision-based localization system. In fact, the motion dynamics of the obstacles and their associated amount of noise are modeled in the implemented Kalman filter. The estimated positions of the obstacles are based on the warp number, the dynamic model and the previous positions of the obstacle using the iterative process described by Equations (7 - 13).

For the experiments presented in Section 4, the moving obstacles are simulated by the MobileSim software and the Aria API [Whitbrook09], and they wander inside of a two-dimensional map with a mostly constant velocity333Interactions between moving obstacles can lead to change of velocity and/or direction of motion by one or all obstacles involved.. Therefore, the state contains and positions, and and velocities, and matrix in Equation (7) is formed as below:

There is no input for this application so matrix is zero in Equation (7). The vision-based localization system provides measurements only from and position, not the velocities. Therefore, the matrix in Equation (8) becomes:

Since the simulated moving obstacles usually have only linear motion (they turn only when they get close to walls and other obstacles), the process noise () is assumed to be very small. On the other hand, observations can be noisy and since we have no knowledge about the exact position of the obstacles, the measurement noise () should be adequately set.

As we mentioned before, at each step, the Kalman filter uses the computed a-posteriori error covariance estimate () to adjust the uncertainty for the neighbors of the estimated position. In other words, the more uncertainty returned by the Kalman filter, the larger is the marked area on the grid around the estimated position of the obstacle.

Figure 4 illustrates how the estimated future positions based on the time-warped grid and Kalman filtering affect the planned path by harmonic potential fields and the rubber band model, in order to avoid collisions. The window in Figure 4(a) is the output of MobileSim with the actual robots, the laser range sensor information, and the walls of the environment. The window in Figure 4(b) is created by the main program using OpenGL to present the internal representation of the environment based on the sensed information. As the reader can notice, Figure 4(b) presents some small blue dots, besides the larger red dots. The first are the estimated and noisy observations made by the laser sensor and the vision-based localization system, while the latter are the predicted future positions of the moving obstacles by time-warps and Kalman filter. The detected static obstacles are marked with red lines. The solid black line represents the traversed path by the robot, while the dotted purple line shows the planned path by harmonic potentials and rubber band optimization. The goal position is represented by the darkest area on the map, indicating the lowest harmonic potential value.

main robot

moving obstacles
(a)

goal

noisy observation

planned path

estimated future position

of the moving obstacle

static obstacle

traversed path
(b)
Figure 4: The effect of the moving obstacles on the planned path: (a) actual environment (b) program’s internal representation, which shows the estimated and noisy observations made by the laser sensor and the vision-based localization system (small blue dots), the predicted future positions of the moving obstacles by time-warps and Kalman filter (larger red dots), the detected static obstacles (red lines), the planned path (purple dashed line), the traversed path (black solid line), and the goal position (darkest area indicating the lowest harmonic potential value).

3.6 GPU-Based Parallel Implementation

All the above mentioned algorithms are highly computationally intensive especially when the map is large and/or there are many moving obstacles in the environment. The problem of using harmonic fields is that it requires repeated updates of the potential values at every cell of the grid. These updates are in turn a function of the potential of the neighboring cells, which leads to a recursive and quite-time consuming algorithm. Furthermore, the algorithm for optimizing the path using the rubber band model needs to calculate different forces at every cells of the grid in all iterations which is a heavy process. Estimating the future positions of moving obstacles requires another real-time process using time-warps and Kalman filter for each moving obstacle. An effective way to speed up these time consuming algorithms is through the use of parallel computation. Even more pertinent to our problem, since a fine and detailed grid of the environment may require millions of cells, we propose the parallelization through the use of General Purpose Graphics Processor Units (GPGPUs). Due to the use of grid-based maps, all these algorithms translate quite nicely into the parallel paradigm of GPU computing.

In that sense, several CUDA programs, namely kernel functions, are implemented to carry out the calculation of grid cells. The environment is divided into 10 cm by 10 cm cells, and each cell contains the input data for a GPU kernel called by a number of threads equal to the number of grid cells. In other words, each GPU thread is responsible for its own cell of the grid – e.g. for a 25.6 meter by 25.6 meter area, 256 256 threads (forming 16 by 16 thread blocks) are created to process the data in the grids. To this end, four GPU kernels are called in order to: 1) calculate the warp of each cell; 2) if there is a moving obstacle in the cell, run the Kalman filter algorithm to estimate the future position of the obstacle; 3) calculate the harmonic potential value of each cell; and 4) optimize the path using rubber band model. Each kernel function is multiply instantiated by the CUDA platform and the calculations of the various grid cells is performed in parallel by the GPU.

Using different thread blocks provides the advantage of using GPU shared memory. The calculated harmonic values reside in the GPU global memory and are accessed by different threads. Since the neighbor cells have some shared harmonic values, by loading the harmonic values into the shared memory, the number of global memory accesses are reduced, which has a significant effect on the speed of the algorithm. An example of an environment divided into grid cells and their corresponding threads and thread blocks are shown in Figure 5. The figure illustrates this advantage of thread blocks and their shared memory.

Moreover, using GPUs as proposed enables the robot to handle many (if not all) moving obstacles at the same time. Some test scenarios with large number of obstacles can be found in Section 4. Final control of the robot and wandering the moving obstacles inside of the map are done by CPU.

Environment

map

Thread block

GPU thread
Figure 5: Mapping the environment to a GPU. Each grid cell is assigned to a GPU thread. The thick lines show the boarder of the thread blocks.

The proposed path planning procedure is outlined in Algorithm 1.

1:Initialize the 2D map and harmonic potential values
2:procedure Map Update
3:      Read laser range sensor and vision-based localization data
4:      Detect static and dynamic obstacles
5:      for each object do in parallel
6:            if Static then
7:                 Update the harmonic potential value:
8:            else if Dynamic then
9:                 Calculate the time-warp number of the moving obstacle via Eq. 15
10:                 Estimate the future position using Kalman filter via Eqs. 7-13
11:                 Update the the harmonic potential value:                   
12:procedure Path Planning
13:      for each cell in the map do in parallel
14:            for k = 1 to 100 do
15:                 Calculate harmonic potentials via Eq. 2                   
16:      for each cell in the map do in parallel
17:            Update the index matrix with the position of the neighbor with lowest harmonic potential value via Eq. 3       
18:      for each cell in the index matrix do in parallel
19:            Calculate tension force and potential force via Eq. 6
20:            Update the index matrix with the position of the neighbor with the minimum resultant force (rubber band optimization via Eqs. 4 and 5)       
21:      Generate the current path based on the optimized index matrix starting from the current position of the robot
22:      Move the robot to the next cell on the current path
Algorithm 1 Dynamic Path Planning via Time-Warped Grids

4 Experimental Results

Various test scenarios were performed using different maps and conditions of the environments, as well as the number of moving obstacles. All tests were performed for indoor navigation using a robot simulator for the Pioneer P3-DX robots (MobileSim). It is important to mention that all test scenarios were conducted using the NVIDIA GeForce GTX480 GPU and the CUDA programming environment.

For Figures 6-8, the left window is the output from MobileSim, while the right window was created by the main program using OpenGL/CUDA to depict the internal representation of the sensed information form the environment, including the harmonic potentials, estimated position of obstacles, etc. Also in the left window of Figures 6 and 7, the dashed green lines illustrate the movement of obstacles, and the solid black lines in the right window show the path traversed by the mobile robot. The red squares show the estimated position of the moving obstacles in future, which are updated in real time, while the red lines demonstrate the static obstacles sensed by the laser sensor of the robot. The reader should keep in mind that these figures display simply a snapshot of the environment and the final traversed path at the end of the tests. However, the moving obstacles may have caused the path to shift, which cannot be easily seen by these snapshots. Simulation videos of the the performance of the proposed path planning algorithm can be found in the accompanying video for the paper, available at http://vigir.missouri.edu/Time-Warp-Grid/.

Figures 6 and 7 show some of these scenarios for different number of moving obstacles. The robot was able to find a smooth and short path while avoiding collisions in the majority of the scenarios (Table 1).

(a) 1 moving obstacle, map 1.
(b) 2 moving obstacles, map 1.
(c) 4 moving obstacles, map 2.
Figure 6: Path planning results for 1, 2 and 4 moving obstacles (Left: actual environment, Right: program’s internal representation).
(a) 8 moving obstacles, map 2.
(b) 12 moving obstacles, map 1.
(c) 16 moving obstacles, map 3.
Figure 7: Path Planning results for 8, 12 and 16 moving obstacles (Left: actual environment, Right: program’s internal representation).

Our first evaluations were exactly regarding these three metrics: success in avoiding collisions; length; and smoothness of the path. The proposed approach was tested over 400 times in different environments, all with size of meters. The percentage of success for different numbers of moving obstacles is presented in Table 1. A very strict criterion was used to determine success or failure: if the robot collided with any obstacle before reaching the destination, or if any moving obstacle was forced to change direction because of the robot, the test was considered a failed attempt. It is important to mention that with the proposed algorithm, the robot can never stop and must always move with a constant velocity.

Besides success rate, the algorithm was also tested for shortness of the path found. Table 2 reports the average length of the path for different maps and different number of obstacles. It would be reasonable to expect the length of the path to grow with the number of moving obstacles in the environment. However, this is not always the case. Sometimes, random movement of obstacles cause the path to change drastically and the final length increases even for a small number of obstacles.

The last property of the path to be evaluated was regarding its smoothness. Here, it should be mentioned that a path is usually regarded as smooth if it does not intersect itself and its tangent at each point varies continuously [TayMan83]. Based on this definition, it can be stated that all paths found by the algorithm were smooth, as it is shown by Figures 6 and 7.

However, due to the target application of the proposed algorithm for power wheelchairs, we also evaluated the algorithm for large turning angles. So, in order to further quantify the smoothness of the optimized path in a dynamic environment, we computed the histogram of turning angles performed during navigation. Figure 10 shows the histograms for different numbers of moving obstacles. As these plots indicate, the turning angles are mostly concentrated on the small angular values. Moreover, even though increasing the number of moving obstacles leads to larger turning angles, these are very rare and likely due to some “deadlocked” situations.

(a) (b)
(c) (d)
Figure 8:

(a, b) A scene when there is no planned path at the moment. (c, d) The algorithm can plan the path quickly.

Figure 9: A loop in the path caused by a blocked situation.

Indeed, in scenarios with a very large number of moving obstacles, it might happen that these obstacles completely block the path of the robot towards the goal. One such scenario is presented in Figure 8 (a)-(b). However, even in those extreme cases, after a while, the robot is able to re-trace a path towards the goal as the obstacles move again away from the robot path (Figure 8 (c)-(d)).

Number of moving obstacles 1 2 4 8 12 16
Percentage of success 95.0 % 95.0 % 92.5 % 87.5 % 80.0 % 75.0 %
Table 1: Percentage of success in path planning for different scenarios (out of 400 simulations).
Number of moving obstacles 1 2 4 8 12 16
Map #1 4610 5400 5270 4970 5955 7085
Map #2 6705 6955 7665 6475 6415 6965
Map #3 4850 4680 4725 6800 7380 8215
Map #4 4870 4875 5045 5090 5660 6885
Table 2: Average length of the traversed path (cm).

Here, we should mention that due to the limitation of the algorithm of not controlling the velocity of the robot, if the blocking situation persists for long enough, the robot has to turn around until it finds a new path, causing loops and sharp turns in the path (Figure 9). These deadlock situations occur when the moving obstacles are aligned in a linear formation in a very dense area, or when multiple moving obstacles try to negotiate a narrow passage, e.g. a doorway. Deadlock situations can be avoided by adapting the algorithm to control the velocity of the mobile robot, which is beyond the scope of this work and will be the subject of future work.

Figure 10: Distribution of turning angles (in degree) for 20 repetitions of the algorithm for 1, 2, 4, 8, and 16 moving objects.

5 Conclusion

This paper presented a unique smooth path planning approach for dynamic and unknown environments for mobile robots. The algorithm relies on: harmonic potential fields to build a path; rubber band model to smoothen the path; and an integration of Kalman Filter and a new idea of Time-Warped Grid to estimate the position of moving obstacles and avoid collisions. The concept of time-warped grid reduces and simplifies many processes by eliminating the need to take the directions of movement and the absolute value of the distance between the robot and the moving obstacles into consideration during the estimation process. The proposed method was tested exhaustively using several simulation scenarios for the Pioneer P3-DX robot. The implementation of the algorithm was carried out using C/C++ and CUDA programming for real-time performance. As the simulations demonstrated, our approach is robust and is able to find the optimum path – i.e in terms of smoothness, distance, and collision-free – either in static or dynamic environments, with very high degree of success even with a very large number of moving obstacles.

6 Future Work

In the course of this research project we have identified many directions for future work. Some are particular to our proposed approach, and some are broader in scope. The vision-based landmark localization system could be replaced by an algorithm to distinguish dynamic and static obstacles using only a laser sensor, which may result in a simpler system. Furthermore, using embedded GPUs like NVIDIA Jetson enables us to run the algorithm on the robot itself, rather than having an external server and consequent communications.

The dynamic obstacles in this work are assumed to have linear motion (they turn only when they get close to walls and other obstacles) and move with a constant velocity, resulting in a simple state matrix and dynamics used by the Kalman filter algorithm. In case of more complicated behavior for the moving obstacles, the Kalman filter should be built using the associated dynamic equations. Machine learning techniques such as neural networks can also be employed to learn the model and estimate the future positions of the moving obstacles and collision points.

Finally, an algorithm that could adopt the velocity of the mobile robot – or even stop it – should be investigated to solve the rare, but existing cases of momentary deadlocks.