Safe Speed Control and Collision Probability Estimation Under Ego-Pose Uncertainty for Autonomous Vehicle

In order for autonomous vehicles to become a part of the Intelligent Transportation Ecosystem, they are required to guarantee a particular level of safety. For that to happen a safe vehicle control algorithms need to be developed, which include assessing the probability of a collision while driving along a given trajectory and selecting control signals that minimize this probability. In this paper, we propose a speed control system that estimates a collision probability taking into account static and dynamic obstacles as well as ego-pose uncertainty and chooses the maximum safe speed. For that, the planned trajectory is converted by the control system into control signals that form input for the dynamic vehicle model. The model predicts a real vehicle path. The predicted trajectory is generated for each particle – a weighted by a probability hypothesis of the localization system about the vehicle pose. Based on the predicted particles' trajectories, the probability of collision is calculated, and a decision is made on the maximum safe speed. The proposed algorithm was validated on the real autonomous vehicle. The experimental results demonstrate that the proposed speed control system reduces the vehicle speed to a safe value when performing maneuvers and driving through narrow openings. Therefore the observed behavior of the system is mimicking a human driver behavior when driving in difficult and ambiguous traffic situations.


page 1

page 5


DL-IAPS and PJSO: A Path/Speed Decoupled Trajectory Optimization and its Application in Autonomous Driving

This paper presents a free space trajectory optimization algorithm of au...

Steering Action-aware Adaptive Cruise Control for Teleoperated Driving

In this paper, a steering action-aware Adaptive Cruise Control (ACC) app...

Quantitative Risk Indices for Autonomous Vehicle Training Systems

The development of Autonomous Vehicles (AV) presents an opportunity to s...

Parallel Multi-Hypothesis Algorithm for Criticality Estimation in Traffic and Collision Avoidance

Due to the current developments towards autonomous driving and vehicle a...

Input-to-State Safety with Input Delay in Longitudinal Vehicle Control

Safe longitudinal control is discussed for a connected automated truck t...

Correct by Design Coordination of Autonomous Driving Systems

The paper proposes a method for the correct by design coordination of au...

An Online Evolving Framework for Modeling the Safe Autonomous Vehicle Control System via Online Recognition of Latent Risks

An online evolving framework is proposed to support modeling the safe Au...

I Introduction

The active development of the autonomous vehicles brings closer their integration into the urban transportation system. One of the main advantages of autonomous vehicles over traditional human-driven cars is an increased driving safety [5, 12, 20] or, in other words, a decrease in the number of traffic accidents. However, in reality, there are many technical and scientific problems, to overcome to guarantee the safety of autonomous vehicles. One of these problems is the safe vehicle control, which comprises assessing the collision probability while driving along a given trajectory and selecting control signals that minimize this probability [10].

Many existing approaches to motion planning and the collision probability estimation assume the precisely determined vehicle ego-pose [7, 9]. However, when working in a real environment, it is crucial to take into account the pose uncertainty caused by unpredictable deviations in vehicle motion, inaccuracies in sensor measurements, changing environment, and other uncertainty factors. Thus, the task of assessing the safety of the motion trajectory under conditions of ego-pose uncertainty is of high priority, which is confirmed by the great number of research dedicated to this problem (e.g. [2, 4]).

Fig. 1: Approximation of the space distribution of the future trajectories (A), method to avoid a collision with obstacles from static occupancy grid map (B), method to avoid a collision with dynamic obstacles (C).

A common approach to motion planning and collision probability estimation is Monte Carlo Motion Planning. It estimates collision probability by repeatedly simulating the vehicle movement along the desired trajectory [8, 16, 3]. The probability is calculated as the ratio of the number of simulations in which the fact of a collision was recorded to the total number of simulations. This approach requires a large number of experiments to obtain a reliable estimate of the probability, and, as a result, is computationally complex.

An alternative approach is based on the prior vehicle state-space probability distribution propagation along the predefined trajectory 

[13, 18, 19, 6]. In [15]

, the authors propose an extension to this approach, taking into account that the distribution of states at each moment depends on whether the distributions at previous time moments were free from collisions. Accounting is carried out through truncating the distribution at each step of the algorithm – discarding hypotheses for which a collision is observed, and approximating the truncated distribution by Gaussian one. Such an approach makes it possible to propagate the truncated probability distribution forward along the trajectory and, as a result, makes it possible to calculate the collision probability more precisely. The disadvantage of all algorithms based on the propagation of prior distribution is the approximation of sensors measurement errors and the uncertainty of vehicle motion by the normal distribution. Such an approximation allows one to calculate the collision probability analytically, however, it is rarely observed in real-world problems.

Some works consider the problem of trajectory safety assessment, not only taking into account static obstacles but also considering possible trajectories of other road users [17, 14, 1, 11].

Even though the tasks of trajectory planning and the collision probability estimation are widely studied, most existing approaches do not take into account the uncertainty of vehicle ego-pose at the time of planning. The vehicle’s current position is considered precisely known, and future uncertainty is usually modeled by a normal distribution centered in the points of the desired trajectory, which does not reflect a real motion uncertainty.

A vehicle speed control system is proposed in this work. It controls the speed to achieve an acceptable value of the collision probability which is imposed by the probability threshold function. To assess the safety of the vehicle trajectory under conditions of pose uncertainty the proposed system accepts as an input an arbitrary probability distribution of vehicle ego-pose, which also distinguishes it from existing approaches. Unlike other approaches Monte Carlo method is not used directly to assess the safety of the trajectory, but only used to estimate the current probability distribution of the vehicle position for the evaluation of proposed system. Although the Monte Carlo localization method is used as an ego-pose probability source the system can work with any other type of ego-pose probability distribution (e.g. Kalman filter). To assess safety, only a part of the future trajectory, predicted by a vehicle dynamic model, is used. This significantly reduces computational load and allows the proposed approach to be used in real-time systems.

There are two main contributions presented in this paper. The first contribution is the novel safe speed control system which explicitly accounts for the vehicle ego-pose uncertainty. The second one is the method for collision probability estimation. The proposed method distinguishes uncertainty handling for static obstacles specified in the global reference frame and for dynamic obstacles which are detected in the vehicle local reference frame.

Ii Problem Statement

In this paper, we propose the Safe Speed Control System Under Ego-Pose Uncertainty for Autonomous Vehicle. The system has the following inputs:

  • estimated vehicle pose in map reference frame at the current moment of time ;

  • probability distribution of vehicle pose
    , where is the probability of to be an ego-pose;

  • current vehicle speed ;

  • reference trajectory – the sequence of waypoints i.e. global trajectory;

  • static occupancy grid map as a binary image;

  • a set of detected dynamic obstacles , where each obstacle is a polyline that circumscribes the projection of an obstacle to the road plane in the reference frame attached to the vehicle.

The output of the system is the maximum safe speed , such that:


where is the maximum possible speed (restricted by road properties), – collision probability threshold function, (further referred as to ) – conditional vehicle collision probability for given within the prediction horizon [, ], – prediction duration, is a speed limit for prediction horizon – the maximum allowed speed on the predicted trajectory.

Simply put, the objective of equation (1) is to determine which is the highest speed limit among considered speed limits that is considered to be safe, i.e. conditional collision probability is less than collision probability threshold determined by function within prediction horizon.

Iii Key Assumptions

In order to estimate collision probability within prediction horizon, it is necessary to analyze the space of all possible vehicle future trajectories. The variability of future trajectories firstly depends on ego-pose uncertainty determined in the map reference frame at the current moment of time, and secondly on how this uncertainty will change over time. Possible future changes of estimated pose will lead to variations of future trajectory shape due to the shift of new pose estimation relative to the reference trajectory. Such changes are impossible to predict. We assume that they may occur randomly at a random moment, then the best approximation of the spatial distribution of the future trajectories is shown in Fig. 

1 (A). In this figure, green lines are the copies of the predicted trajectory for current estimated vehicle pose . Each of them is transferred to pose of every hypothesis , in other words, they capture the future motion of pose distribution in case of absence of new sensory data. Red lines are possible trajectories provided that a random hypothesis became the new estimated pose at a random moment of time.

Since it is computationally intractable to generate so many predictions of trajectories with sufficient discretization, several simplifications are proposed.

In order to avoid a collision with static obstacles we can predict trajectory for estimated vehicle pose and transfer it to pose of each hypothesis without varying its shape (same as green lines from Fig. 1 A). This approach allows us to assess collision probability within prediction horizon in case of a complete lack of new data to reestimate pose distribution, which represents the worst-case scenario. If the collision probability for the worst-case scenario does not exceed the threshold value, then it will not exceed the threshold in any other case. Schematically, this approach is illustrated in Fig. 1 (B).

With dynamic obstacles, this approach is not suitable, since their coordinates are measured relative to the vehicle and for each hypothesis, the relative position of the obstacles will be the same (provided that the error of the obstacle detector is neglected). To estimate the probability, we can consider the case in which the variation in the relative profiles of predicted trajectories will be maximum. This case arises if the trajectories will be predicted for each hypothesis separately, taking it as the estimated vehicle pose. Schematically, this approach is illustrated in Fig. 1 (C).

Iv Safe Speed Control System Structure

Fig. 2: Vehicle Safe Speed Control System flowchart.

The system structure is shown on Fig. 2. Safe speed is recalculated periodically. The calculation is performed in cycle, where at each cycle iteration collision probability is estimated for given . The collision probability is determined by two distinct probabilities: the probability of collision with static and with dynamic obstacles (both described in the respective sections).

On a lower level, main operations inside the Safe Speed Control System are:

  • Trajectory prediction, described in Trajectory Prediction Module section.

  • Checking intersection of trajectories with static obstacles .

  • Checking intersection of trajectories with dynamic obstacles .

  • Collision probability estimation based on information about collision for every trajectory. For static obstacles it described in section Collision Probability Estimation With Static Obstacles. With dynamic obstacles, it works in a similar way.

  • Safe speed estimation, i.e. determination of optimal speed limit based on the calculated collision probabilities for various speed limits (according to the equation (1)). Described in details in Safe Speed Estimation section.

V Trajectory Prediction Module

Trajectory Prediction Module calculates the future trajectory in real-time based on the dynamic model of the vehicle. The inputs are vehicle current state (pose , speed and etc.), reference trajectory , speed limit , prediction duration . The output is predicted trajectory. The module structure is shown in Fig. 3.

Fig. 3: Trajectory Prediction Module flowchart.

Trajectory Prediction Module combines the vehicle motion control system (CS) consisting of trajectory and speed controllers and a dynamic vehicle model. CS used in this module is fully identical to the one that controls the vehicle. This guarantees the complete identity of the control signals generated by both control systems.

In case of collision probability calculation with static obstacles, a trajectory prediction is calculated for estimated vehicle pose only. In case of dynamic obstacles, future trajectories are generated for each hypothesis separately, taking its pose as the estimated vehicle pose.

Vi Collision Probability Estimation With Static Obstacles

The collision probability with static obstacles is estimated as follows:

  • Trajectory Prediction Module calculates the future trajectory for the estimated vehicle pose (B in Fig. 5).

  • The trajectory is transferred to each hypothesis so that its beginning coincides with the current hypothesis position, and rotates by the relative yaw angle. Thus, trajectory of each hypothesis is obtained (C in Fig. 5).

  • Every trajectory is checked for collisions with static occupancy grid map . A trajectory is considered safe if at every its point vehicle does not intersect with static obstacles (D in Fig. 5).

  • The probability of collision with static obstacles is calculated:


where is an integral over pose distribution. The Monte Carlo method approximates this space with a discrete set of particles, so the integral is replaced by the sum over the set of particles; is the number of trajectories, each corresponds to the -th particle; is the probability of -th particle pose to be the current vehicle pose; is the probability of a collision between vehicle projection and static obstacles along the trajectory, given that -th particle pose is the current vehicle pose:


Vii Collision Probability Estimation With Dynamic Obstacles

The collision probability with dynamic obstacles is calculated in a similar way, except that for each hypothesis pose the trajectory prediction module predicts trajectory separately (assuming to be the vehicle pose). After that, it is the relative profile of each trajectory in the vehicle reference frame that is checked for the presence of collisions with obstacles (also defined in the vehicle reference frame).

Viii Safe Speed Estimation

Total collision probability with both dynamic and static obstacles can be calculated as follows:


The resulting probability is used to calculate the maximum safe speed . The dependence of on speed limit for several arbitrary test cases is shown in Fig. 4. We assume that function increases monotonically. According to experimental results, it holds in most cases however slight deviations are possible. If we neglect these deviations, then instead of a brute-force search, the calculation of the safe speed can be performed by quick search algorithms, e.g. binary search, i.e. at each iteration of the algorithm, the obtained collision probability for the current speed limit is compared with the value of the probability threshold function for the same . After this, the collision probability is recalculated for the new speed limit value set according to the binary search algorithm from the range .

Fig. 4: Dependence of collision probability on speed limit

Ix Experimental Results

The experiments were performed for collision avoidance with static obstacles. The car-like robot was used as an experimental vehicle.

Fig. 5: Step-by-step visualization of the safe speed control system operation (left). Vehicle trajectory with speed profile obtained by the speed control system (right).

In Fig. 5 on the left, the system step-by-step operation is visualized. Here, blue indicates the estimated vehicle pose and predicted trajectory, dark green indicates particles – pose hypotheses, light green lines – future trajectories for each particle. Trajectories ending with a red triangle have collisions.

In Fig. 5 on the right, the vehicle trajectory obtained as a result of real passage is shown. The trajectory color at each point corresponds to the instantaneous speed of the vehicle. Red color corresponds to zero speed, green - to the maximum possible speed . The discontinuities observed in the trajectory arise due to corrections of estimated vehicle pose by the localization system. It can be seen from the figure that the speed was severely limited in places where the pose estimation was not accurate, the movement occurred close to the walls or in front of difficult sections of the route such as turns or narrow entrances. After clarifying vehicle estimated pose or after passing difficult sections, the speed was recovered.

Fig. 6: Dependence of collision probability on speed limit over time.
Fig. 7: Types of collision probability threshold function and corresponding maximum safe speed values for few charts (constant function on the left, decreasing linear function in the center, decreasing exponential function on the right).

Fig. 6 in the center shows the dependence of collision probability on speed limit over time. Data are recorded from real passage. Along the X-axis the route passage time is shown. The Y-axis represents the sequence of speed limit values from 0 to the maximum possible on this route – m/s. The color (according to the scale on the left) shows the probability of a collision within the prediction horizon for the entire range of speed limits. Since deviations from the monotonic increase in the function are insignificant, it is possible to use fast search algorithms on ordered data to determine the maximum safe speed . Also probability threshold function must be monotonically decreasing or constant. It is noticeable that varies greatly over time. Function rapid changes occur mainly at the moments of localization resampling (resampling of particles in the Monte Carlo localization algorithm). To avoid unwanted jerks (alternating high accelerations and braking) of the vehicle, several probability threshold functions were analyzed.

The lower part of Fig. 6 shows 3 dependences of the collision probability on the speed limit over time from the same passage, where black indicates forbidden speed limits, the collision probability of which exceeds the threshold according to function. For clarity, the same functions , as well as the corresponding maximum safe speeds for typical dependencies are shown in Fig. 7. Back to Fig. 6, it is noticeable that when using the decreasing functions or , the maximum safe speed limits change much smoother than when applying the constant threshold function , motion becomes less jerky. Moreover, the decreasing nature of the collision probability threshold function is justified by the fact that on the high speeds even small threshold probability cannot be neglected which means the threshold value should decrease with the increase of speed.

In the central graph in Fig. 6 after the 200-th second, a region is noticeable where, at low speeds, the collision probability is close to zero, but when the speed increases it abruptly becomes close to 1. The situations describing these cases are shown in Fig. 6 in the upper right and upper center images, respectively. We can see that the low probability at low speeds is explained by the high accuracy of localization in this section of the circular motion. A high probability is caused by the fact that the trajectory control system is not capable of completing a maneuver of a turn at such speeds which leads to a collision. In other areas, the increased probability of a collision is due, for the most part, to high ego-pose uncertainty, as in the upper left image of Fig. 6.

X Conclusion

In this paper we proposed the safe speed control system for autonomous vehicle, based on real-time calculation of safe speed limit. The main contribution is the method for collision probability estimation that takes into account the ego-pose uncertainty for calculation of collision probability with both static and dynamic obstacles. Experiments showed that the proposed system allows avoiding collisions by decreasing vehicle speed.

The proposed approximations for taking into account the uncertainty of future trajectories made it possible to obtain an upper bound of the collision probability as the worst-case scenario with a minimum number of computational operations. Also, the use of the discovered property about the monotonically increasing function of the dependence of the collision probability on the speed limit allows us to reduce the computational complexity by an order of magnitude. The proposed method allows not only to prevent a collision with a specific obstacle but can also be used to control the quality of the localization system, i.e. with a high uncertainty of ego-pose, the number of predicted collisions with arbitrary obstacles increases, as a result of which vehicle will slowed down or even stopped until the pose estimation will become more accurate.

In the future, the proposed method of collision probability estimation can be expanded by taking into account the error model of dynamic obstacle detector. It is also possible to take into account prediction of trajectories of dynamic obstacles.


  • [1] S. Annell, A. Gratner, and L. Svensson (2016) Probabilistic collision estimation system for autonomous vehicles. In 2016 IEEE 19th International Conference on Intelligent Transportation Systems (ITSC), pp. 473–478. Cited by: §I.
  • [2] S. D. Bopardikar, B. Englot, and A. Speranzon (2015) Multiobjective path planning: localization constraints and collision probability. IEEE Transactions on Robotics 31 (3), pp. 562–577. Cited by: §I.
  • [3] A. Broadhurst, S. Baker, and T. Kanade (2005) Monte carlo road safety reasoning. In IEEE Proceedings. Intelligent Vehicles Symposium, 2005., pp. 319–324. Cited by: §I.
  • [4] N. E. Du Toit and J. W. Burdick (2011) Probabilistic collision checking with chance constraints. IEEE Transactions on Robotics 27 (4), pp. 809–815. Cited by: §I.
  • [5] C. D. Harper, C. T. Hendrickson, and C. Samaras (2016) Cost and benefit estimates of partially-automated vehicle collision avoidance technologies. Accident Analysis & Prevention 95, pp. 104–115. Cited by: §I.
  • [6] A. Houénou, P. Bonnifait, and V. Cherfaoui (2014) Risk assessment for collision avoidance systems. In 17th International IEEE Conference on Intelligent Transportation Systems (ITSC), pp. 386–391. Cited by: §I.
  • [7] X. Hu, L. Chen, B. Tang, D. Cao, and H. He (2018) Dynamic path planning for autonomous driving on various roads with avoidance of static and moving obstacles. Mechanical Systems and Signal Processing 100, pp. 482–500. Cited by: §I.
  • [8] L. Janson, E. Schmerling, and M. Pavone (2018) Monte carlo motion planning for robot trajectory optimization under uncertainty. In Robotics Research, pp. 343–361. Cited by: §I.
  • [9] J. J. Kuffner Jr and S. M. LaValle (2000) RRT-connect: an efficient approach to single-query path planning. In ICRA, Vol. 2. Cited by: §I.
  • [10] A. Lambert, D. Gruyer, and G. Saint Pierre (2008) A fast monte carlo algorithm for collision probability estimation. In Control, Automation, Robotics and Vision, 2008. ICARCV 2008. 10th International Conference on, pp. 406–411. Cited by: §I.
  • [11] C. Lienke, M. Keller, K. Glander, and T. Bertram (2018) An ad-hoc sampling-based planner for on-road automated driving. In 2018 21st International Conference on Intelligent Transportation Systems (ITSC), pp. 2371–2376. Cited by: §I.
  • [12] P. Liu, R. Yang, and Z. Xu (2018) How safe is safe enough for self-driving vehicles?. Risk analysis. Cited by: §I.
  • [13] W. Liu and M. H. Ang (2014) Incremental sampling-based algorithm for risk-aware planning under motion uncertainty. In Robotics and Automation (ICRA), 2014 IEEE International Conference on, pp. 2051–2058. Cited by: §I.
  • [14] D. Mehta, G. Ferrer, and E. Olson (2018) C-mpdm: continuously-parameterized risk-aware mpdm by quickly discovering contextual policies. In 2018 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS), pp. 7547–7554. Cited by: §I.
  • [15] S. Patil, J. Van Den Berg, and R. Alterovitz (2012) Estimating probability of collision for safe motion planning under gaussian motion and sensing uncertainty. In Robotics and Automation (ICRA), 2012 IEEE International Conference on, pp. 3238–3244. Cited by: §I.
  • [16] E. Schmerling and M. Pavone (2016) Evaluating trajectory collision probability through adaptive importance sampling for safe motion planning. arXiv preprint arXiv:1609.05399. Cited by: §I.
  • [17] M. Schreier, V. Willert, and J. Adamy (2016) An integrated approach to maneuver-based trajectory prediction and criticality assessment in arbitrary road environments. IEEE Transactions on Intelligent Transportation Systems 17 (10), pp. 2751–2766. Cited by: §I.
  • [18] J. Van Den Berg, P. Abbeel, and K. Goldberg (2011) LQG-mp: optimized path planning for robots with motion uncertainty and imperfect state information. The International Journal of Robotics Research 30 (7), pp. 895–913. Cited by: §I.
  • [19] M. P. Vitus and C. J. Tomlin (2011) Closed-loop belief space planning for linear, gaussian systems. In Robotics and Automation (ICRA), 2011 IEEE International Conference on, pp. 2152–2159. Cited by: §I.
  • [20] H. Wang, Y. Huang, A. Khajepour, Y. Zhang, Y. Rasekhipour, and D. Cao (2019) Crash mitigation in motion planning for autonomous vehicles. IEEE Transactions on Intelligent Transportation Systems 20 (9), pp. 3313–3323. Cited by: §I.