Predictive Probability Path Planning Model For Dynamic Environments

07/29/2020 ∙ by Sourav Dutta, et al. ∙ Southern Illinois University University at Albany 0

Path planning in dynamic environments is essential to high-risk applications such as unmanned aerial vehicles, self-driving cars, and autonomous underwater vehicles. In this paper, we generate collision-free trajectories for a robot within any given environment with temporal and spatial uncertainties caused due to randomly moving obstacles. We use two Poisson distributions to model the movements of obstacles across the generated trajectory of a robot in both space and time to determine the probability of collision with an obstacle. Measures are taken to avoid an obstacle by intelligently manipulating the speed of the robot at space-time intervals where a larger number of obstacles intersect the trajectory of the robot. Our method potentially reduces the use of computationally expensive collision detection libraries. Based on our experiments, there has been a significant improvement over existing methods in terms of safety, accuracy, execution time and computational cost. Our results show a high level of accuracy between the predicted and actual number of collisions with moving obstacles.



There are no comments yet.


page 6

page 7

This week in AI

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

I Introduction

Motion planning is a term used in robotics for finding valid configurations that move a robot from source to destination. An open problem in motion planning is planning in narrow passages and the presence of dynamic obstacles. Methodologies of motion planning find applications in self-driving cars initiatives, cooperative robot swarm scenarios, and efficient mobility of robots in tightly crowded areas while performing important tasks.

Fig. 1: Process Description

In robot path planning, spatial uncertainty relates to an area or region of the environment which may or may not be safe to navigate due to complex relationships with moving obstacles leading to a high frequency of collision. Temporal uncertainty defines the time interval during which it becomes uncertain if an identified region is safe to navigate. In the presence of these uncertainties, it is challenging to generate a successful path for a robot, due to a high probability of collision with moving obstacles. In robotics, a lot of literature exists to model and alleviate temporal [zhang2017multirobot, lahijanian2016iterative] and spatial [boularias2015grounding, belter2016improving] uncertainties, however, this is still an open research problem. There have been various studies that define spatial and temporal uncertainties over crash data for road safety to prevent motor vehicle accidents [lord2008effects, theofilatos2016predicting, fawcett2017novel].

Our method, as demonstrated in Figure 1

, has an offline training and an online planning phase. The online training phase plans a static trajectory for the robot and trains the Poisson processes. The online planning phase estimates the collision probabilities and adjusts the speed of the robot to avoid any collisions. The

contributions of this paper include:

  • the development of two Poisson random variables that can generate probability mass functions in space and time and predict the probability of collision at any given point of space, or calculate the inter-arrival time of the collision at any given point of time.

  • a cost-efficient non-homogeneous Poisson process for effective collision detection in dynamic environments.

  • the creation of a dynamic collision-avoidance framework that can be used with any path planning algorithm.

Based on our experiments, we show that our algorithm effectively finds a safe path in a dynamic environment with a high level of accuracy without expensive re-planning.

Ii Related Work

Collision detection and avoidance in the presence of moving obstacles has received a lot of attention recently. Attempts have been made to solve this problem using a geometric approximation method, an online planning strategy and real-time perception of the environment [goerzen2010survey, yegenoglu1988online, ferguson2015real].

Ii-a Geometric Algorithms for fast collision detection

Researches in creating collision detection libraries most commonly use geometric approximations such as bounding volume hierarchies, inner volume approximation, and workspace certificates. Bounding volume hierarchies, which are used in the collision detection algorithm (Sphere[hubbard1996approximating], Axis Aligned Bounding Box [tu2009research], Discrete Orientation Polytopes [klosowski1998efficient]), follow two main steps: decomposing object into regions and bounding those regions with geometric shape primitives. Inner volume approximations pack the geometric shape primitives inside given objects to provide good coverage of the space. The work in [weller2009inner] proposed an irregular sphere packing method or Stolpner et. al. [stolpner2011medial] provided an approach of generating an inner sphere tree by approximation the medial axis. Unlike previous approximation methods, Ghosh [ghoshfast] approximated both obstacle and free workspace with a set of geometric shape primitives and its topology to improve the performance of collision detection. However one of the drawbacks of most of these approximation methods is that they usually use a single type of geometric shape primitives. Thus other approaches rely on the concept of the workspace safety certificate. A workspace safety certificate is regions that are guaranteed to be collision free thus ensure the validity of robot configurations or path segments. Lacevic et. al. [lacevic2016burs] proposed an approach to approximate C-free certificates for efficient path planning. Additionally, Yang et. al. [yang2004adapting] utilized a set of a sphere in the free space to improve the sampling distribution. Other widely used approaches to collision detection are Adaptive Monte Carlo localization on the motion, location and sensors of each agent involved in the collision [broadhurst2005monte, siagian2007biologically, cobano2011path], this method proved to be computationally expensive. In [hennes2012multi], Hennes et al. presented a collision-avoidance approach in the presence of moving obstacles, called collision avoidance with localization uncertainty to alleviate the use of Monte-Carlo localization. They used local communication to share the robots’ state information to ensure smooth collision-free motion. In the following sections, we discuss the different approaches taken in existing texts to handle moving obstacles to avoid collisions.

Ii-B Dynamic collision avoidance using Temporal Logic specification

In [maly2013iterative], Maly et al. presented a framework for iterative temporal motion planning in dynamic environments for a hybrid system with complex and nonlinear dynamics given a temporal logic specification in a partially unknown environment. They also defined a scheme to measure the closeness of satisfaction of a temporal logic specification to plan the desired trajectory. Additionally, Agha-Mohammadi et al. [agha2018slap] proposed a rollout-policy-based algorithm for online replanning in belief space to enable Simultaneous localization and planning (SLAP). The method is able to handle the presence of changes in the environment and large deviations in the robot’s pose by using lazy evaluation of the generated feedback tree and replan accordingly. Hoxha el at. [hoxha2016planning] proposed a framework for online planning for robotic systems in dynamic environments by utilizing Metric Temporal Logic specifications to inspect and modify available plans to avoid obstacles and satisfy specifications in a dynamic environment.

Ii-C Poisson Process for collision event counting

The Poisson process model has been widely used in motion planning fields for collision detection. A closely related work to this paper [hess2013poisson] used a Poisson-driven dirt distribution map for identifying static objects for a cleaning robot. The environment was tessellated into occupancy grids and each of the cells was observed to learn the rate at which the floor gets dirty. A homogeneous Poisson process was constructed for each cell of the grid with the polluting events as a random variable. Given a cleaning operation had taken place at time , the dirt level at time was predicted by the expected value of polluting events. In this paper, a similar approach has been used to identify the temporal distribution of moving obstacles, instead of static polluting agents. Therefore our approach requires an intelligent inclusion of spatial random and temporal distribution. Furthermore, research in [mahmud2016poisson] used the Poisson process, specifically the Log-Gaussian Cox Process, for modeling the inter-activity time to predict the starting time of the next unobserved activity in a video. Using the Poisson process to model the obstacles in the environments, Karaman et al. [karaman2012high] derived lower and upper bounds on the critical speed in which a vehicle could safely traverse those environments. The work in [molina2019go] proposed an exploration approach for mobile robots that builds and refines a spatio-temporal model of pedestrian motion by using the uncertainty of a Poisson process built from past observations to infer the locations and intervals to explore at future times. Moreover, Jovan et. al. [jovan2016poisson] used periodic Poisson processes to count the number of activities performed by a human in a time of different rooms of a building to maximize human-robot interactions. To address the problem of mapping human flows with mobile robots in an indoor environment, Jumel et. al. [jumel2017mapping] used a spatial Poisson process to estimate the human presence probability in each region of that environment.

Ii-D Non-homogeneous Poisson Process

In [lin2017sampling], Lin et al. presented a sampling-based motion planning strategy for UAV collision avoidance. They used a closed-loop RRT-based model(Rapidly exploring Random Tree sampling method) with a simplified node connection strategy followed by using intermediate waypoints to create candidate trajectories or by using reachable sets for collision prediction. These intermediate waypoints are defined as milestones or stopping points along the path of travel for the robot where the course of trajectory is subject to change.

Moreover, to address planning problems for human-robot interaction in social environments, Tipaldi el at. [tipaldi2011planning] learned a non-homogenous spatial Poisson process whose rate function encodes the occurrence probability of human activities in space and time. Although this method can be considered suitable for predicting collision with moving obstacles, it contains no information about cells or rarely used and unobserved places in an environment.

While most of the literature discussed above depend on dynamic replanning even after some offline obstacle prediction, in this paper, we use Poisson distribution map to keep track of the number of collisions occurring over distance and time and predict future collision ahead of time and avoid it without any expensive dynamic replanning at the time of traversal when the robot is traveling at a certain speed.

Iii Methodology

A Poisson distribution is a discrete probability distribution that expresses the probability of a given number of events occurring in a fixed interval of time or space if these events occur with a known constant rate (known as a parameter of the distribution) and their occurrences are independent of the time since the last event. A discrete random variable

is said to be a Poisson random variable with parameter , represented as , if its range is {0,1,2,3,…}, and its probability mass function is given by:


The rate of collision with static obstacles is independent of time but dependent on space. Therefore, they display spatial properties. In this paper, we show that the rate of collision for dynamic obstacles can be dependent on both time and space due to its inherent properties, i.e, constantly changing locations at each timestamp. We implement two Poisson random variables to calculate the rate of collisions over unit distance and unit time. One of the novelties of this algorithm is that both the Poisson random variables are estimated during an offline training phase for an environment with dynamic obstacles. The trajectory generation is also offline, and no replanning strategy has been applied.

Iii-a Modeling the environment

We model the environment comprising of moving obstacles in an infinite Euclidean space with a robot in a finite space boundary. The robot plans its trajectory in a configuration space (C-Space) that comprises of the feasible configuration of the robot defined over vector space. We have modeled the obstacles in relation to the coordinates and no information about the velocity, orientation or trajectory is known beforehand. The obstacles can arrive at random anywhere inside or outside the environment.

During the training phase, collision counts are determined by the number of times the obstacles appear on the trajectory of the host robot. However, the robot knows about static obstacles in the environment, which it initially plans with. Figure 2 shows an environment with 2 static and 20 randomly placed moving obstacles.

Fig. 2: Environment with 2 static and 20 random moving obstacles (marked as floating dots without any edges)

Iii-B Spatio-temporal Poisson Process

A problem with fitting a Poisson Process over a trajectory generated by a path planning algorithm is that the traversal time is independent of the path generation. Therefore, it becomes impossible to observe the number of collisions at the time of path generation. We can, however, generate Poisson parameters over space and over time separately. Our Poisson random variable over space has a parameter estimated over a fixed interval of space is 1-meter distance. A Poisson random variable over time has a parameter estimated over a fixed interval of time of 1 second.

Individually they don’t produce a safe path to travel unless we choose to re-plan the path, or know exactly when or where the last collision event occurred, and then calculate the arrival time for the next collision event. That involves a dynamically trained process, which is expensive and may compromise the robot’s safety by making it vulnerable to collisions during the training process. However, in order to train a Poisson process offline, we can observe the number of times an obstacle appears on the trajectory of the robot when it is yet to start its traversal of the planned trajectory.

It is not necessary that the dynamic obstacles will be uniformly distributed within the environment. Therefore, the need for a spatial Poisson random variable can be justified in order to focus our observations on specific parts of the trajectories where the probability of collision is high. It can be seen intuitively that in a real-world scenario, dynamic obstacles can only be observed at points of intersection, merger or crossing of multiple roads.

In our method, we have therefore combined two Poisson random variables, one over space and one over time. The spatial variable represents the number of collisions over unit distance and is observed only over the planned trajectory, irrespective of the rest of the environment.


We make observations (like snapshots) of the trajectory at different timestamps and count the total number of collisions (C) over total distance (D) from start to goal. The parameter for the spatial random variable is calculated as in Equation 2, where is the number of observation timestamps:

The temporal Poisson random variable is used to determine the time-period between each consecutive collisions on the robot’s trajectory. We fit a Poisson process on the collision counts over each edge connecting two waypoints and determine the inter-arrival time of the collision events. When the robot arrives at a waypoint, it has to determine the time it takes to travel the edge connecting the next waypoint at its current speed. If the time to travel that edge is lesser than the arrival time of the next collision event, then the robot is allowed to pass, else, its speed is decreased and the same probability calculations are repeated. The probability calculations for the temporal random variable has been discussed in section III-D.

Iii-C Estimation of Poisson Parameters

The Poisson parameters for either of the two Poisson random variables were calculated using maximum likelihood estimation (MLE). From the probability mass function in Equation 1, we get the likelihood function as Equation 3, where n is the number of desired observations, and is the number of collisions at that timestamp.


Taking log on both sides we have the log-likelihood function:


Taking derivative with respect to :


Setting Equation 5 to zero, we have:


Equation 6 gives the MLE of a Poisson parameter, which is equal to the average of all observations of collision counts (also known as the sample mean).

Iii-D Estimation of the inter-arrival time

The inter-arrival time has an exponential distribution. The probability density function of the distribution is given as follows:


Therefore the cumulative density function is given as:


Therefore, the probability of the collision event occurring before the robot reaches the next waypoint is determined using (eq. 9)


and subsequently by (eq. 10), where T is the arrival time of the next collision event, t is the time it takes for the robot to travel through the edge, and is the parameter of the Poisson distribution.


The more reduction in speed that occurs, the greater will be the traversal time for an edge since our distance between two waypoints is fixed. As can be seen from Equation 10, the probability reduces with a decreased speed, hence a collision can be avoided.

0:  env is a environment where

is the degrees of freedom of the robot.

0:  Start and Goal are the start and goal points of the robot in env.
0:   is the allocated number of timestamps for observation.
  while  do
  end while
Algorithm 1 Offline Training

Iii-E Algorithms

Algorithm 1 shows an overview of the offline training process. Initially, a trajectory has been generated using an RRT* algorithm as shown in Figures 2(b) and 2(a). This framework can be used with any offline motion planning algorithm without the loss of generality. Once the trajectory has been generated, it then identifies individual waypoints in the C-Space that are part of the trajectory (Step 1 of Algorithm 1) and connects them using a straight line local planner (Step 2 of Algorithm 1). It then observes the edges for collision (Steps 4-7 of Algorithm 1). During the observation period, two types of events are recorded – one for collision count over the entire trajectory at different timestamps, another for collision count over each edge over a period of time. The algorithm then models them using two Poisson random variables: i.) for the rate of collision per unit distance () (Step 9 of Algorithm 1), ii.) for the rate of collision per unit time for each edge connecting two waypoints () (Step 10 of Algorithm 1).

(a) RRT* Tree Generation
(b) Resultant Trajectory Formed
Fig. 3: Trajectory Generation Using RRT*
0:   determined in Algorithm 1.
0:   is the Poisson parameter for the spatial random variable.
0:   is the Poisson parameter for the temporal random variable for each in edges.
3:  for i=1 to count(edges) do
7:     if  then
8:        while  do
9:           decrease
12:        end while
13:     end if
14:     traverse the edge at
16:  end for
Algorithm 2 Online Planning

Algorithm 2 shows an overview of the online planning process. Initially, it calculates the probability of collision over each edge based on the length of the edge. It also calculates the probability of the arrival time of the next obstacle through that edge being greater than the time taken by the robot to traverse that distance. If both of these probabilities are less than a predefined threshold, the robot is allowed to move to the next waypoint by traversing that edge; otherwise, the speed of the robot is decreased (Step 9 of Algorithm 2). Since the distance between any two consecutive waypoints is fixed, we only manipulate the time it takes for the robot to travel that distance, by manipulating the speed of the robot. This process is repeated (loop in Step 8 of Algorithm 2) until a parameter () is found for which the probability of collision is low and it is safe for the robot to move to the next waypoint.

Moving obstacles Possible collisions Collisions avoided Percentage Accuracy 5 18 17 94.44 10 28 26 92.85 15 41 35 77.78 20 60 60 100 25 101 95 95 30 109 103 98.09 35 129 122 94.57 40 103 97 94.18 45 78 71 91.02 50 133 124 93.23
TABLE I: Predictive Collision Detection Run With Different Numbers of Moving obstacles and 10 Observation timestamps
Moving obstacles Possible collisions Collisions avoided Percentage Accuracy 5 8 7 87.5 10 35 32 91.43 15 60 56 93.33 20 61 55 90.16 25 84 77 91.67 30 93 86 92.47 35 179 162 90.50 40 146 135 92.47 45 144 137 95.14 50 164 156 95.12
TABLE II: Predictive Collision Detection Run With Different Numbers of Moving obstacles and 20 Observation timestamps
(a) Closest distance of obstacles from robot with 10 observation timestamps
(b) Collisions with and without speed adjustment with 10 observation timestamps
(c) Closest distance of obstacles from robot with 20 observation timestamps
(d) Collisions with and without speed adjustment with 20 observation timestamps
Fig. 4: Simulation experiments with 10 and 20 observations
Fig. 5: Simulation experiment on an environment with 10 random moving obstacles (marked as floating dots without any edges)

Iii-F Complexity analysis

The worst case time complexity for the Algorithm 1 is where m is the number of edges on the trajectory and n is the number of observations. The worst case time complexity for the Algorithm 2 is where o is the number of times we have to decrease the speed of the robot. Given a large value of m, we can simplify the time complexity to be .

Iv Experiments

In order to demonstrate the proficiency of the approach, we compared it with other existing algorithms that solve the same problem. The evaluation has been done in stages. A proof of concept involving simulation experiments has been performed. The simulation experiments were done using a python simulation environment built using pygame [pygame]. The success of this proof of concept leads to real-world testing.

Iv-a Experimental Setup

In order to simulate a 3D environment, 3D plotting libraries were used in python. The moving obstacles were generated at random coordinates to make their trajectories unpredictable. The real-world testing used crazyflie 2.1 drones from bitcraze [bitcraze] in a controlled setting utilizing controlled moving obstacles. The moving obstacles were other crazyflie drones that were following a fixed trajectory. All the drones in the environment were communicating with the bitcraze Loco Positioning System (LPS).

A typical real-world controlled experiment would involve running a Ubuntu 18.04 operating system on a Dell Optiplex computer with a Crazy-radio antenna (also from bitcraze) receiving signals from 8 LPS-anchor tags at each corner of the 3D environment to estimate the position of the drones in the environment. The environmental setup is shown in Figure 7.

Iv-B Experimental Results

Fig. 6: Simulated representation of real-world experiment on an environment with 5 moving obstacles (marked as floating dots without any edges) with predefined trajectories and different speeds

Iv-B1 Simulation experiments

We run 10 experiments each with increments of 5 obstacles. We also recorded the results from 10 and 20 observations timestamps for the training algorithm. Table II and II lists the results from the experiments run on different environments with varying numbers of moving obstacles with 10 and 20 observations timestamps. As we can see, the number of possible collisions without any speed adjustment (control experiment) increases as the number of moving obstacles increases, but the number of avoided obstacles also improved. This has happened because, with more obstacles in the environment, the Poisson processes were trained better than with a lower number of obstacles. Figure 3(a) and 3(c) show the plot of closest distances of any obstacle to the robot with respect to the number of moving obstacles. Figure 3(b) and 3(d) plot the number of collisions with and without the collision avoidance algorithm with 10 and 20 observation timestamps, respectively. It also shows the margin of error for each experiment. The robot had no knowledge of the total number of collisions; it has been determined for recording the accuracy of the algorithm only after its execution has completed. Figure 5 shows the step by step execution of our algorithm on a simulated environment with 20 moving obstacles generated randomly.

The average closest distance when the robot’s maximum speed is was when only 10 observation timestamps were used to train the algorithm with 5 to 50 moving obstacles. When compared to the closed-loop RRT algorithm presented in [lin2017sampling] that uses up to 2 moving obstacles, the average closest distances were for speed of . Our environment is of size cubic meters, thus, we scale the average distance between the robot and the moving obstacle, to make a fair comparison. The environment size used in [lin2017sampling] was approximately cubic meters. We scale the average closest distance between the robot and the moving obstacle from our algorithm by a factor of . In a scaled environment of similar size as [lin2017sampling], we would get an average closest distance of meters for speeds of . Thus our algorithm performs better with the larger closest distance.

Iv-B2 Real-world experiments

In the real-world experiments, we consider a 0.3 meters proximity between host drone (drone running the predictive collision algorithm) and moving obstacle as a collision. This distance is optimum because at this distance the obstacle drone can be avoided and the host drone can continue through its trajectory even if it has failed to predict a collision. A test run is considered to be successful if it has less than 20% of unpredicted collisions. With up to two moving obstacles, we found that the trajectory of the host drone was successfully traversed without colliding with the moving obstacles. Refer to the attached video for details. A set of control experiments were conducted to test whether the host drone can traverse without the use of our algorithm. In all the cases, the host and the obstacle drones collided and the experiment failed, without the use of predictive collision avoidance. This necessitates the use of our algorithm for dynamic obstacle avoidance. Figure 6 shows the step by step execution of our algorithm on a simulated representation of a real-world environment with 5 moving obstacles, where, both the robot and the obstacles had a predefined trajectory. The robot (in blue) follows an uprising spiral trajectory while the obstacles (in orange) follow a straight line parallel to the X-axis with different speeds, and the robot slows down due to our algorithm.

Fig. 7: Real world experimental setup

V Conclusion

We have developed a dynamic collision detection and avoidance framework that is compatible with most of the motion planning strategies. We have tested with a popular algorithm (RRT*) and achieved great results in terms of accurate prediction of collisions. We have shown that by using our framework, there is no necessity for dynamic-replanning for the trajectory. We have used a static trajectory, as shown in the Algorithm 1. As future work, we would like to study if Poisson processes trained offline can be used to generate static trajectories in dynamic environments by calculating collision probabilities during local planning.