I Introduction
A boom in the study of planning for carlike robots has appeared since the DARPA Grand Challenge and Urban Challenge. This is helping to propel the advancement of autonomous driving, which is anticipated to improve the safety and efficiency of the transportation system, and offer outstanding convenience to users. While most of the current work concentrates on driving scenarios in which cars run along highways or urban roads, where the traversable area is confined by structured lane markings and barriers, few studies pay attention to obstaclerich or narrow environments, or driving through additional moving obstacles, e.g., pedestrians and other vehicles. Despite this, trajectory planning in these scenarios is of great research and development value. A typical scenario is shown in Fig.1, which occurs in an industrial estate. The complex environment and the various road participants challenge the robustness of the planner and its adaptivity to the dynamics, which continue to be important issues. Furthermore, from the perspective of popularizing autonomous driving technology, it is fundamental to satisfy the transportation requirement of daily life, which usually includes a trip from a residential district to the work place. Other lowspeed driving scenarios, including driving on a university campus, driving in an industrial estate, or parking in an open parking lot are also common.
The difficulties in planning can be viewed from two aspects: the cluttered environment, and the dynamics of the complex scenario. Arising from the navigation solution in the static environment, the most common scheme is path generation and following. Taking no account of the performance, it is workable in most lowspeed cases as the path planners handle the cluttered environment and help to avoid static obstacles efficiently. An extra obstacle avoidance module or replanning helps the car to change its route and keep its distance from obstacles. However, it requires a relatively spacious area and may leave the car trapped in the scenarios we focus on in this paper.
For narrow or cluttered environments, it is less convenient to passively avoid dynamic obstacles. Therefore, adding a time dimension to the planning is essential to cope with the dynamic environment. Additionally, planning should meet the realtime requirement for avoiding collisions. Under these conditions, to enable vehicles to travel through a complex dynamic environment safely and efficiently, we propose a realtime hierarchical planning approach, taking advantage of both the efficiency of the sampling based methods for path planning and the flexibility of optimization approaches for time scheduling. A smooth path that satisfies the kinematic constraints of the car is generated by the modified bidirectional rapidly exploring random tree (biRRT) method. Then the safe intervals (SIs) along the path are estimated and confine the searching time region for further optimization by sequential quadratic programming (SQP). The temporal planning of each pose is optimized in the sense of a customized cost function. The motions of dynamic obstacles are tracked by multiple Kalman filters. Though they are predicted to move with uniform speeds, it is practicable with the update rate of 10 HZ in the low speed scenarios. Overall, this work makes the following contributions:

We propose a hierarchical scheme for trajectory planning that by combining the biRRT path planning and temporal optimization, can perform realtime and safe driving of a car in dynamic and cluttered environment.

We propose a revised biRRT planner for fast searching that also suits car dynamics.

We propose a temporal planning method based on the idea of SIs, and build a complete system including updating and emergency/timeout recovery.

Through extensive simulation and real tests, we show the robustness and good performance of our system in various complex environments.
Ii Related work
Iia Path planning
Many autonomous vehicles have been demonstrated successfully driving through a specific route [1, 2, 3]. Behind this, various methods have been proposed to solve the planning problem with different strengths and weaknesses. Graphsearchbased planners like Hybrid A* [2] and Spatiotemporal lattice [4] search the state space represented by an occupancy grid or state lattices and yield a global optimum. However, with the dimension of space increasing, the time cost and memory consumption increase increase exponentially. Furthermore, it is critical to build the graph for unconventional road environments. Methods based on piecewise curves like polynomials [5] are suitable for a structured environment, but typically rely on prestored sampled trajectory candidates. This generateandevaluate scheme reduces the computational cost, but lacks flexibility and longhorizon prediction. Similar to the spatiotemporal lattice method, optimization methods like that in [6] can be used for spatial temporal planning. These methods minimize the customized cost so as to bring a wellrounded solution that can be collisionfree and have both low cost and high comfort. However, these methods suffer from long computation time. In contrast, sampling based methods like RRT [7] randomly sample the configuration space and construct the tree incrementally, in which case a fast solution can be provided. It avoids the discretization of the state space and requires no prior information of the environment. Thus, it ensures both flexibility in generating the path and applying it to various environments, regardless of the structures.
IiB Dynamic obstacle handling
Dynamic obstacles composed of pedestrians and vehicles bring great uncertainty to the driving environment. With the exception of several learningbased methods that make the robots avoid pedestrians in a human manner with only raw inputs [8], the strategy of modelling the motion of obstacles first and then planning dominates.
One common practice is to assume that obstacles move at a known and constant velocity (CV) within a short time horizon. However, it is crucial to consider the uncertainty of observations of obstacles and their real motion. The Bayesian occupancy filtering proposed in [9] utilizes a probabilistic grid representation of the dynamic surroundings and is further used for danger estimation as well as collision avoidance. In [10]
, by assuming constant controls of obstacles in the near future, the trajectories of the obstacles are modelled as a series of Gaussian distributions by the prediction step of the extended Kalman filter. Then possibilities of collision along the timebounded lattice are calculated and regarded as a determinant of the final trajectory. Similarly, in this paper, the moving obstacles are tracked with multiple Kalman filters; however, their motions directly influence the SIs of the nodes along the path and thus affect robot movements.
With CV assumption, methods based on the concept of velocity obstacle (VO) [11] define either collision cones or half planes [12] on the velocity space and provide solutions of collisionfree velocities. Extensions, including bicycle reciprocal collision avoidance [13], further make these methods applicable for cars. These agentbased models perform well with homogeneous moving objects but can easily get the robot stuck if the environment becomes cluttered. As the free space is usually limited compared to the size of the robot, instead of taking any collision avoidance action or frequently replanning, other methods try to incorporate the dynamic information into planning at the very beginning. In [14], the 4D search space (x,y,,time) is constructed with states defined by configurations and SI. By applying A*, it demonstrates realtime feasible planning in dynamic environments. However, it still suffers from the aforementioned limitations of A*. Instead of 4D planning, speed profile planning [15] along a preplanned path is proposed to avoid collision, which can obtain a fast solution. In [16], speed planning is performed for each path candidate and trajectory with the least cost is selected. Inspired by the ideas of safe interval and speed profile planning, we propose a hierarchical planning method that combines samplingbased path planning and temporal optimization to get realtime performance.
Iii The planning algorithm
Iiia Nomenclature
The notations used in this paper are declared in Table I. It lists the symbols that appear in the algorithms or equations that are not clearly elucidated.
Symbols  Meaning 

start pose of the car  
pose and velocity of moving obstacles  
coefficients of the curvature representation  
total length of the curve  
accumulative arc length along the curve,  
randomly generated point  
nearest node  
forward growing tree grown from the start  
backward growing tree grown from the goal  
random value in with a uniform distribution 

threshold distance between two trees  
probability of using GMM data sampling  
distance between the node and  
azimuth angle of in the coordinate of  
pose of th node, represented by  
timestamp of th node  
,  velocity and acceleration of th node 
start timestamp of the th SI of node  
number of safe intervals of node 
IiiB Framework
The framework of the proposed method is shown in Fig.2. Taking both the input and output into account four modules are presented. The algorithm relies on a perception module to provide the information of the obstacles and the car itself. Additionally, a control module translates the trajectory into control commands. More details on the path planner and temporal optimizer are shown in Fig.3. For the path planning part, we first generate a curve library offline and load it into the memory for the first plan. After receiving the sensor data and command of the goal, the modified biRRT is executed for path planning. To tackle dynamic obstacles in the environment, firstly the states of the obstacles will be modelled, which also provides information for the SI estimation of each pose. After optimizing the customized cost given the dynamic constraints and searching the boundaries defined by the SIs, suggested timestamps of each pose will be generated along with the feasible trajectory. The observations of the environment are updated at all times. If the optimizer fails to find a solution, a timer will be started and the state of the obstacles will be estimated again. If the time is up, the planner will replan the path. So, in most cases, replanning is not happening except when moving obstacles stop and block the way.
IiiC Collision checking
Collision checking is important for both the tree growing in path planning and SI estimation in temporal optimization. The most typical 2D representation is the rectangle, which is a perfect match for most types of vehicles and is conventional in the perception process (object detection and tracking). However, due to the anisotropy, it is not convenient for collision checking. In this paper, a representation of three overlapped circles that covers the footprint of the car is applied, as shown in Fig.4. For moving objects like humans that have larger aspect ratios (w/l), the threecircles representation will induce more redundant area than the footprint; therefore, these obstacles are represented by one circle. The radius of the circles in each case is shown in Eq.1.
(1) 
IiiD Path planning
We propose the modified biRRT method for path planning, which combines the speed and flexibility of RRT and also satisfies the kinematic constraints by replacing the line segments between the nodes with curves in the lookup table. In addition, the bidirectionally grown two trees stretch from the start and goal respectively, which saves time, especially in a complex environment.
The cubiccurvature curves with limited curvature are generated based on the method described in [17]. As shown in Eq.2, the curvature of the curve is a third order polynomial in arc length. Given the curvature, the function of the changes in heading angle (Eq.3) and position (Eq.4 and Eq.5) can be represented. The final curve can be represented by four parameters and we sample the end pose of the curve on the circle with radius (shown in Fig.5(a)). The radius and the angle are recorded for fast lookup. So the final representation of the curve in the lookup table is . The curve samples in the curve library are shown in Fig.5(b).
(2)  
(3)  
(4)  
(5) 
After loading the lookup table, the biRRT method will grow two trees incrementally. The algorithm can be seen in Algorithm 1, which includes sampling a node, extending the tree and connecting two trees. Some modifications have been made for it to be suitable for carlike robots. As shown in Algorithm 3, while extending the tree, we replace the line segment between and with a curve that possesses an end point close to the extending direction, and is chosen to optimally fit the line. In order to prevent repetition of nodes, each curve can only be chosen once for the same node. For trees connection, we use C Feasible SQP[18] to online calculate the connecting cubiccurvature curve if the matched two nodes satisfy several rough conditions like distance and heading changes. As the curves with are more likely to be chosen, the trees spread fast. However, it is easy to generate a loop structure and it costs much time to connect the two trees. In Algorithm 2
, in order to speed up the connection of the two trees, we introduce a sampling method that generates samples following the distribution of a Gaussian mixture model after they get close enough. As shown in Fig.
6, the centers of the Gaussians are located at the nodes of the potentially shortest path. In the following, our introduced sampling strategy will be referred to as the GMM sampling, and the pure random sampling strategy will be referred to as the Random sampling.IiiE Temporal optimization
Given a feasible path, temporal optimization can be implemented. However, it is hard to map the continuous path to a continuous duration of time. The alternate methods are that we either discretize the timeline and find the best locus on the path for each timestamp, or sample the path and find the best timestamp for each locus. For RRT, the generated path composed of nodes and edges is discretized naturally. It is natural to optimize the pathtotimeline mapping by finding the best timestamp for each node.
The idea of the SI is explicit. As demonstrated in Fig.7, given the pose of each station, it is the largest time period that ensures no collision, which means that extending the time in either direction would cause the vehicle to have a collision. Correspondingly, the complementary time intervals are the collision intervals. Here, due to the dynamic change of the surrounding environment, the SIs of successional poses show some specific patterns. For example, the line pattern in the middle can be attributed to a moving car. Taking advantage of the SI, if we set the timestamp of every node within its SIs and ensure that the distance between any two contiguous nodes is adequately small, the trajectory is collisionfree. The length of all curves is set to be smaller than the size of the car to ensure that the SI is feasible. Additionally, optimizing the timestamp at each node instead of at denselysampled locations can reduce the computational cost and save time, and, most importantly, still guarantee safety.
In this premise, if we define the velocity and acceleration of the car at each node as shown in Eq.6 and Eq.7, respectively, the temporal optimization problem can be represented (shown in Eq.8). The target of time optimization is to find the optimal timestamp of each node or an optimal , where is the length along the path. To satisfy the collision free requirement, the solution space of each timestamp is limited to its corresponding SIs. As we do not assume a reverse, should be nondecreasing and the timestamp of successive nodes should be increasing. In addition to the condition of SI, other dynamic constraints, like velocity and acceleration speed limits, should also be satisfied. For the expectation of the optimization, less time and less velocity variation will enhance the driving comfort. So the optimization object function is set to both minimize the time and acceleration speed. Here, the weights and are chosen for normalization. The problem cannot be directly solved by SQP as the inequality constraints are disjunct. It can be solved by mixedinteger nonlinear programming (MINLP) or generalized disjunctive programming (GDP) by inducing the integers for decision. However, these processes are timeconsuming.
To simplify the searching process for a SIs sequence, we construct a layered structure (shown in Fig.7). SIs of the same pose sequence are considered to be candidates in a layer. Starting with intervals with the smallest pose sequence, we grow the SI sequence till the last layer. For every valid SI in the th layer (with pose sequence ), we select children from the SIs with the pose sequence . The criteria is that the two intervals overlap for certain length of time to ensure that the car can pass successfully. Every time a childparent relationship is established, the SI of the child will be modified to also consider the limit from their parent. Only the SIs being selected are considered to be valid and can go on to find their child. When it comes to the last layer, the SI with the smaller time will be more likely to be picked. And it can go back to the parent to get the SI sequence. After getting the proper SI sequences, the final optimized solution is captured by several SQP iterations.
(6)  
(7) 
(8)  
s.t.  
Iv Experimental results
Iva Experimental setup
The proposed trajectory planner has been tested in both simulation and real environments. We constructed simulation environments in Stage[19] with different static and dynamic obstacles and implemented repeated experiments by strictly controlling the start and goal of the trajectory and also the states of the dynamic obstacles. The environment in simulations is usually simpler than in the real case, particularly with respect to dynamics. Also, sensors in the real environment receive more noise and this may affect the planning. In the sense of robustness, we tested similar scenarios in the real environment with the golf cart shown in Fig.2. It is equipped with one 16line lidar for both localization and planning. Furthermore, it is comparable in size and has similar kinematic constraints to the car in simulations. However, due to the poor repeatability of the test conditions in the real environment, it is hard to perform quantitative, massive data analysis. As the tests involve interaction between the car and the complex environment, it is not equitable to compare it with planners that can only handle static obstacles or agent based algorithms that mainly focus on the dynamic obstacles. We compared the planner with a human driver who tries to reach the goal in the shortest time while maintaining a proper driving style and complying with the same velocity limitations.
IvB Path planning
To show the effectiveness of the GMM sampling, we compare the GMM sampling and Random sampling method in the same condition. As shown in Fig.8, the goal point is (20,20,) and start point is (0,0,). The obstacles are 15 randomly generated disks with radius in the range of [0.5,2.0]. Given the same start pose and goal pose as well as the same environment, the sampling points of the GMM sampling are more concentrated on the adjacent region of the final path, while the points of random sampling spread in the space and cause a bigger tree and longer path. As the grown trees are also influenced by the pose difference between the start point and the end point, we test the planner with different sampling methods and different starting poses. Fig.8(b) shows the statistical results of the number of nodes, planning time and path cost with different start angles of 313 runs. It is clear to see the reduction of nodes for the GMM sampling methods. But this reduction of sampling nodes does not necessarily lead to a reduction in time as the method can not reduce the times of optimization, which contributes to a large proportion of the planning time. Note that planner with GMM sampling plans shorter path than with Random sampling in all cases. GMM sampling reduces the cost when there is a large heading difference between the start and goal.
As is shown in Fig.2, the planner is also tested in several representative scenes. The first scene is the vehicle turning right. In the second scene, the vehicle is turning left at a sharp corner, and there are several cars parked on the road. The third scene shows the vehicle trying to overtake a slow car to the right. There is also a pedestrian moving in the opposite direction to the left. These three scenes are very common in a residual or an industrial estate, but are quite different to the case of urban roads or highways as they are less structured and more cluttered.
]) are shown here, with the markers showing the average value of the trials and the error bars representing the 95% confidence intervals.
IvC Temporal optimization
For temporal optimization, a critical threat is the dynamic moving obstacles. As shown in Fig.10, to test the optimizer in the time domain, we design simulations of five common scenarios, including crossing, overtaking, bypassing, following and waiting. These scenarios are shown in the first row of Fig.10, with red boxes representing the test car, and blue representing the moving obstacle. Poses with a larger timestamp have higher transparency. For each scenario, the data of 10 autodriving tests and 10 human driving tests are collected.
For qualitative analysis, heat maps showing the distribution of all footsteps along the trajectory are presented. The warmer the color, the slower the car moves. The values of the pixels are normalized inside a heat map; therefore, the colors of the pixels are only meaningful in a relative sense. In the “Cross” scenario, the red car tries to turn left. Because it cannot cross in front of the blue car in advance even at its maximum speed, it moves slowly and speeds up after the blue car crosses. As there is much space for the turning, the autodriving car goes through different trajectories, while the humandriven car insists on a similar compliant route according to the driving experience in real world. In the “Overtake” scenario, the red car tries to pass a slow car (in blue), while in the “Bypass” scenario, the red car passes the blue car moving in the opposite direction. Similarly, the trajectories of the autodriving car vary while the trajectories of humandriven car show consistent patterns. The autodriving car is not programmed to obey the traffic conventions. More specifically, the lane changes to the right are performed at different locations in the “Overtake” scenario; in the “Bypass” scenario, though it has a tendency to travel to the right due to the motion of the other car, it moves more freely as there are no rigid confinements except the black line. In the “Follow” scenario, the red car has to travel through a narrow passage with the blue car moving in front, so it has to follow the blue car as there is no room to overtake it. In the “Wait” scenario, the blue car enters the narrow passage first, and the red car has to wait for the blue car to exit, otherwise the two cars will collide in the passage. From the heat map, we can see that the speeds of the red cars are uniform along the way for the “Follow” scenario, and both the autodriving and humandriving cars accelerate after passing the passage area. In the “Wait” scenario, both the autodriving and humandriving cars wait at the entrance of the passage. However, the autodriving car is more conservative and it waits farther away for the entrance to the narrow passage.
For quantitative analysis, the total time and total length of the trajectories in different scenarios are analyzed. As shown in Fig. 11, the planned trajectories of the autodriving cars have a smaller or comparative total length, which can be attributed to the free driving style, especially for the “Cross” scenario. For the time spent along the way, though the autodriving car drives through a shorter path, it shows comparative performance in only the “Cross” and “Following” scenarios. In other cases, the autodriving car takes a conservative strategy and takes more time than the humandriving car. Though the time is longer, it is acceptable for a safe trip.
IvD Real test
To prove the feasibility of the algorithm in the real environment, we build a complex environment and test it with the golf cart (shown in Fig.2) equipped with a Velodyne VLP16 lidar. As other road participants will also contribute to the success of a navigation trial, it is difficult to evaluate the effect of the algorithm in the real environment. In the test, pedestrians move around and the car is noticed or unnoticed, but in either they are acting aggressively. The speed of the car is limited to 1 m/s for safety reasons. The result of the test is shown in Fig.12. As no preliminary knowledge of the environment is provided, the path is planned based on current observation and replanning will be conducted if new observations show that the current path is not feasible. For the first row of figures, the golf cart meets a pedestrian who wants to cross the road. As the car cannot travel through safely in advance, it slows down to let the pedestrian pass by. For the second row, two pedestrians travel in the opposite direction. Because the goal is near, there is no other feasible path. So the golf cart waits for the pedestrian to leave and then reaches the end.
V Conclusions
In this paper, a hierarchical planning approach was proposed to solve the planning and obstacle avoidance problems in dynamic and cluttered environments. A modified biRRT planner was proposed to satisfy both the kinematic constraints of a car and the time and flexibility requirements in a dynamic and cluttered environment. Temporal optimization taking advantage of the idea of SI was executed to handle the dynamic environment. The planner was evaluated by both simulation and in real tests, and has shown good performance. When compared with human drivers, it showed similar moving patterns handling the narrow passage. Though it planed shorter path, it behaved less agile than human drivers in many scenarios, which can be attributed to the simple modelling of other moving objects.
References
 [1] C. Urmson, J. Anhalt, D. Bagnell, C. Baker, R. Bittner, M. Clark, J. Dolan, D. Duggins, T. Galatali, C. Geyer, et al., “Autonomous driving in urban environments: Boss and the urban challenge,” Journal of Field Robotics, vol. 25, no. 8, pp. 425–466, 2008.
 [2] M. Montemerlo, J. Becker, S. Bhat, H. Dahlkamp, D. Dolgov, S. Ettinger, D. Haehnel, T. Hilden, G. Hoffmann, B. Huhnke, et al., “Junior: The stanford entry in the urban challenge,” Journal of field Robotics, vol. 25, no. 9, pp. 569–597, 2008.
 [3] J. Ziegler, P. Bender, M. Schreiber, H. Lategahn, T. Strauss, C. Stiller, T. Dang, U. Franke, N. Appenrodt, C. G. Keller, et al., “Making bertha drive—an autonomous journey on a historic route,” IEEE Intelligent Transportation Systems Magazine, vol. 6, no. 2, pp. 8–20, 2014.
 [4] J. Ziegler and C. Stiller, “Spatiotemporal state lattices for fast trajectory planning in dynamic onroad driving scenarios,” in Intelligent Robots and Systems, 2009. IROS 2009. IEEE/RSJ International Conference on. IEEE, 2009, pp. 1879–1884.
 [5] A. Bacha, C. Bauman, R. Faruque, M. Fleming, C. Terwelp, C. Reinholtz, D. Hong, A. Wicks, T. Alberi, D. Anderson, et al., “Odin: Team victortango’s entry in the darpa urban challenge,” Journal of field Robotics, vol. 25, no. 8, pp. 467–492, 2008.
 [6] J. Ziegler, P. Bender, T. Dang, and C. Stiller, “Trajectory planning for bertha—a local, continuous method,” in Intelligent Vehicles Symposium Proceedings, 2014 IEEE. IEEE, 2014, pp. 450–457.
 [7] Y. Kuwata, J. Teo, G. Fiore, S. Karaman, E. Frazzoli, and J. P. How, “Realtime motion planning with applications to autonomous urban driving,” IEEE Transactions on Control Systems Technology, vol. 17, no. 5, pp. 1105–1118, 2009.
 [8] L. Tai, J. Zhang, M. Liu, and W. Burgard, “Sociallycompliant navigation through raw depth inputs with generative adversarial imitation learning,” arXiv preprint arXiv:1710.02543, 2017.
 [9] C. Coué, C. Pradalier, C. Laugier, T. Fraichard, and P. Bessière, “Bayesian occupancy filtering for multitarget tracking: an automotive application,” The International Journal of Robotics Research, vol. 25, no. 1, pp. 19–30, 2006.
 [10] A. Kushleyev and M. Likhachev, “Timebounded lattice for efficient planning in dynamic environments,” in Robotics and Automation, 2009. ICRA’09. IEEE International Conference on. IEEE, 2009, pp. 1662–1668.
 [11] P. Fiorini and Z. Shiller, “Motion planning in dynamic environments using velocity obstacles,” The International Journal of Robotics Research, vol. 17, no. 7, pp. 760–772, 1998.
 [12] J. van den Berg, S. J. Guy, M. C. Lin, and D. Manocha, “Reciprocal nbody collision avoidance,” 14th International Symposium of Robotic Research, ISRR 2009, pp. 3–19, 2011.
 [13] J. AlonsoMora, A. Breitenmoser, P. Beardsley, and R. Siegwart, “Reciprocal collision avoidance for multiple carlike robots,” in Robotics and Automation (ICRA), 2012 IEEE International Conference on. IEEE, 2012, pp. 360–366.
 [14] M. Phillips and M. Likhachev, “Sipp: Safe interval path planning for dynamic environments,” in Robotics and Automation (ICRA), 2011 IEEE International Conference on. IEEE, 2011, pp. 5628–5635.
 [15] C. Liu, W. Zhan, and M. Tomizuka, “Speed profile planning in dynamic environments via temporal optimization,” in Intelligent Vehicles Symposium (IV), 2017 IEEE. IEEE, 2017, pp. 154–159.
 [16] W. Lim, S. Lee, M. Sunwoo, and K. Jo, “Hierarchical trajectory planning of an autonomous car based on the integration of a sampling and an optimization method,” IEEE Transactions on Intelligent Transportation Systems, vol. 19, no. 2, pp. 613–626, 2018.
 [17] B. Nagy and A. Kelly, “Trajectory generation for carlike robots using cubic curvature polynomials,” Field and Service Robots, vol. 11, 2001.
 [18] C. T. Lawrence, J. L. Zhou, and A. L. Tits, “User’s guide for cfsqp version 2.0: Ac code for solving (large scale) constrained nonlinear (minimax) optimization problems, generating iterates satisfying all inequality constraints,” Tech. Rep., 1994.
 [19] R. Vaughan, “Massively multirobot simulation in stage,” Swarm intelligence, vol. 2, no. 24, pp. 189–208, 2008.