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 humandriven 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 egopose [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 egopose uncertainty is of high priority, which is confirmed by the great number of research dedicated to this problem (e.g. [2, 4]).
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 statespace 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 realworld 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 egopose 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 egopose, 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 egopose probability source the system can work with any other type of egopose 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 realtime 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 egopose 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 EgoPose 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 egopose; 
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:
(1) 
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 egopose 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 worstcase scenario. If the collision probability for the worstcase 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
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 realtime 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.
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:
(2) 
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:
(3) 
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:
(4) 
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 bruteforce 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 .
Ix Experimental Results
The experiments were performed for collision avoidance with static obstacles. The carlike robot was used as an experimental vehicle.
In Fig. 5 on the left, the system stepbystep 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 in the center shows the dependence of collision probability on speed limit over time. Data are recorded from real passage. Along the Xaxis the route passage time is shown. The Yaxis 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 200th 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 egopose 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 realtime calculation of safe speed limit. The main contribution is the method for collision probability estimation that takes into account the egopose 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 worstcase 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 egopose, 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.
References
 [1] (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] (2015) Multiobjective path planning: localization constraints and collision probability. IEEE Transactions on Robotics 31 (3), pp. 562–577. Cited by: §I.
 [3] (2005) Monte carlo road safety reasoning. In IEEE Proceedings. Intelligent Vehicles Symposium, 2005., pp. 319–324. Cited by: §I.
 [4] (2011) Probabilistic collision checking with chance constraints. IEEE Transactions on Robotics 27 (4), pp. 809–815. Cited by: §I.
 [5] (2016) Cost and benefit estimates of partiallyautomated vehicle collision avoidance technologies. Accident Analysis & Prevention 95, pp. 104–115. Cited by: §I.
 [6] (2014) Risk assessment for collision avoidance systems. In 17th International IEEE Conference on Intelligent Transportation Systems (ITSC), pp. 386–391. Cited by: §I.
 [7] (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] (2018) Monte carlo motion planning for robot trajectory optimization under uncertainty. In Robotics Research, pp. 343–361. Cited by: §I.
 [9] (2000) RRTconnect: an efficient approach to singlequery path planning. In ICRA, Vol. 2. Cited by: §I.
 [10] (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] (2018) An adhoc samplingbased planner for onroad automated driving. In 2018 21st International Conference on Intelligent Transportation Systems (ITSC), pp. 2371–2376. Cited by: §I.
 [12] (2018) How safe is safe enough for selfdriving vehicles?. Risk analysis. Cited by: §I.
 [13] (2014) Incremental samplingbased algorithm for riskaware planning under motion uncertainty. In Robotics and Automation (ICRA), 2014 IEEE International Conference on, pp. 2051–2058. Cited by: §I.
 [14] (2018) Cmpdm: continuouslyparameterized riskaware 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] (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] (2016) Evaluating trajectory collision probability through adaptive importance sampling for safe motion planning. arXiv preprint arXiv:1609.05399. Cited by: §I.
 [17] (2016) An integrated approach to maneuverbased trajectory prediction and criticality assessment in arbitrary road environments. IEEE Transactions on Intelligent Transportation Systems 17 (10), pp. 2751–2766. Cited by: §I.
 [18] (2011) LQGmp: 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] (2011) Closedloop belief space planning for linear, gaussian systems. In Robotics and Automation (ICRA), 2011 IEEE International Conference on, pp. 2152–2159. Cited by: §I.
 [20] (2019) Crash mitigation in motion planning for autonomous vehicles. IEEE Transactions on Intelligent Transportation Systems 20 (9), pp. 3313–3323. Cited by: §I.