Detection and Motion Planning for Roadside Parked Vehicles at Long Distance

Transcription

1 2015 IEEE Intelligent Vehicles Symposium (IV) June 28 - July 1, COEX, Seoul, Korea Detection and Motion Planning for Roadside Parked Vehicles at Long Distance Xue Mei, Naoki Nagasaka, Bunyo Okumura, and Danil Prokhorov Toyota Research Institute North America, Ann Arbor, MI, USA {xue.mei, naoki.nagasaka, bunyo.okumura, danil.prokhorov}@tema.toyota.com Abstract Reliable long distance obstacle detection and motion planning is a key issue for modern intelligent vehicles, since it can help to make the decision early and design proper driving trajectory to avoid discomfort for the passengers caused by hard brake or sudden large lateral movement. Specifically, when there is vehicle parked on the roadside, we need to detect its position and pass it safely with proper distance without causing much disruption during driving. In this paper, we propose a method to detect roadside parked vehicles robustly and design a trajectory with proper lateral offset from the lane center for the host vehicle to safely pass by it. To successfully detect the roadside parked vehicles, we fuse the output from a long range lidar and radar. We pre-compute multiple path candidates with different lateral offset, and the path planner selects the most proper one based on the distance of the parked vehicle to the lane center. To deal with false alarms and missing detections, we apply temporal filtering to the detection output and history of the decision making. The speed control is carefully designed to ensure that the host vehicle passes the parked vehicle with a safe and comfortable speed. The implemented system was evaluated in numerous scenarios with vehicles parked on the roadside. The results show that the system effectively commands the host vehicle to pass by the parked vehicle safely and comfortably with proper distance and smooth trajectory. I. INTRODUCTION A special scenario within the area of research for intelligent vehicles is roadside parked vehicle detection and avoidance, which is not only useful for highly automated vehicles, but also in vehicle safety system to detect and plan the motion to safely pass the obstacle. Fig. 1 shows an image with a parked vehicle on the roadside on the route of the host vehicle. The white car parked close to the road boundary on the right and can raise safety concerns if we pass it without slightly moving to the left and reducing the speed. Successfully detecting roadside parked vehicle and planning a smooth trajectory to pass is a difficult task. Firstly, the 3d points output from lidar installed on the vehicle is very sparse at long distance. No data association can be made with a few points from the vehicle and hence the speed is difficult to estimate from lidar sensor. While the radar sensor is good at speed estimation, it has insufficient lateral accuracy to locate the vehicle. Secondly, the planner has to deal with missing detections and false alarms from detection results which can cause unstable and inconsistent motion planning. In this paper, we describe the detection and motion planning system developed to address this challenge, i.e., the part of the system that takes a sensor and map based representation of the situation as input, detect the roadside parked vehicle and generates a motion trajectory that the vehicle is supposed to follow. The system is designed to follow human intentions to pass the parked vehicle with Fig. 1. An image with parked vehicle on the roadside. We need to estimate the position of the parked vehicle and make decision on the motion planner to pass it with proper speed and safe distance. proper speed and safe distance. A flowchart to illustrate the proposed vehicle detection and motion planning system is presented in Fig. 2. First, the 3d point clouds from lidar are clustered and segmented based on the Euclidean distance of the points. A convex hull is constructed for each point cluster and is considered a potential obstacle. Second, combined with radar obstacle detection, the obstacles are associated with closest radar observations within a certain distance range and assigned the speed from the associated radar observations. The obstacles that have no association with radar observation or higher speed than a threshold are identified as noise or moving obstacles and removed for the purpose of this task. Third, the distance between the obstacle and the lane center is calculated from the road structure in the map. All obstacles that are not within a certain distance to the road boundary will not have impact on the host vehicle s driving route and be left out from consideration. For the rest of the parked vehicle candidates, we select the most proper path from a finite set of pre-computed paths based on the lateral distance to the obstacles to safely pass the parked vehicle. Temporal filtering is applied to remove false alarms and missing detections. Finally, the speed control is employed to ensure that the host vehicle passes the obstacle with a comfortable speed. To assure reasonably accurate localization in the presence of missing or unclear lane markings, our method relies on proprietary localization algorithms which utilize maps, GPS and other inputs /15/$ IEEE 412

2 Sensor data Validation with map Path selection Point clustering Distance calculation Temporal filtering Obstacle detection Cross check with radar Speed control Fig. 2. A flowchart to illustrate the proposed detection and motion planning system. Our contribution is four-fold: 1) we utilize both long range lidar and radar sensors in a complementary way for obstacle detection. This detection algorithm is not only able to take advantage of accurate positioning from the lidar sensor, but also obtains complementary speed estimation from radar sensor; 2) desired paths with different lateral offset are pre-computed and dynamically selected based on the position of the parked vehicle. This solution creates a smooth, consistent, and noise-insensitive trajectory for safe maneuver and planning; 3) to deal with false alarms and missing detections occurring in the sensor observation stream, we employ a temporal filtering scheme; and 4) speed control helps the host vehicle to pass the parked vehicle comfortably, imitating the behavior of a careful human driver. The rest of the paper is organized as follows. In the next section related work is summarized. The parked vehicle detection algorithm is described in Section III. Section IV details the lateral motion planning with different offset types. Section V introduces temporal filtering and Section VI explains speed control. Experimental results are reported in Section VII, followed by conclusion in Section VIII. II. RELATED WORK Detection and tracking of surrounding vehicles is a core task in the field of intelligent vehicles. Multi-sensor fusion can provide complementary information for robust moving object detection. In [8], two sensor fusion architectures are described for pedestrian detection in urban scenarios using information from a lidar and a single camera. Several features for each sensor measurements and classification algorithms for better accuracy are exploited. Stiller et al. [10] use radar, lidar, and stereo vision for obstacle detection and tracking. Planning a path to avoid obstacles in safe and comfortable way is another core task for intelligent vehicles. The path planning methods can be largely divided into two different categories: selecting best path from a finite set of available path candidates [4], [11], [5] and calculating path dynamically by solving an optimization problem [13]. The methods in the former category rely on a finite set of pre-computed paths and a selection from this set based on the vehicle s surrounding environment. In [4], it presents an effective algorithm for state space sampling utilizing a model-based trajectory generation approach. Given the road networks that limit the space of acceptable motions, a state space sampling strategy is effective. In [5], it proposes path planning method based on the state lattice framework [7]. The method computes a finite set of path candidates, and selects the best path from candidates by using dynamic programming. Computational cost depends on the number of candidates, i.e., variations of maneuvers. In order to improve smoothness, a post-optimization can be applied for the best candidate path [12]. There are also path planning methods that find the best trajectory from an optimization function. In [9], it defines potential field for velocity depending on obstacles, and then computes optimal path depending on the velocity potential field. The method handles both static and moving obstacles in dynamic environments simultaneously. In [13], it creates a trajectory by minimizing an objective function with imposed constraints for local continuity and smoothness. It also takes into account both dynamic and static obstacles which are modelled in the form of polygons. However, it is difficult for optimization methods to provide a stable trajectory output because of objective function specifics. In path planning, it is important to provide a stable and smooth trajectory output robust against perception uncertainties. Perception is greatly challenged to detect obstacles reliably at long distance, even with sensor fusion methods. In addition, sometimes intelligent vehicle needs to pass obstacles within a short lateral distance because of a narrow driving corridor. For such cases, human occupants may feel uneasy because of nature of driving. Radar can provide speed estimates of obstacles, but it usually lacks horizontal accuracy. Whereas lidar can provide accurate position of obstacles, the speed of obstacles is hard to estimate, especially at long distance. Motivated by the success of sensor fusion for the object detection [3], [1], [6], we propose a roadside parked vehicle detection method by taking advantage of both lidar and radar sensors. For the motion planning of the obstacle avoidance after it is detected, we propose a path planning method based on searching the best path from a finite set of path candidates. The method provides both a smooth and stable trajectory robust to uncertain perception. We also propose a speed control method in order to provide comfortable driving when the vehicle is passing nearby obstacles. III. PARKED VEHICLE DETECTION In this section, we describe the parked vehicle detection algorithm using both lidar and radar sensor data as input. The lidar sensor is IBEO LUX which has 4 laser scan levels with detection range up to 200 meters. The radar sensor is BOSCH LRR3 which has a detection range of 0.5 up to 250 meters with a field of view of 30. Both lidar and radar are calibrated in the vehicle coordinate system. Fig. 3 shows the examples of lidar data points before segmentation (left) and after 413

3 Fig. 3. Examples of lidar data points. Left image shows the raw lidar data points output from sensor, and right image shows the segmented data points after removing ground points. Fig. 4. An illustrative example of the constructed graph. Each node denotes a 3d point and an edge is added to certain pairs of the points. Different color denotes the points from different layer. Each horizontal scanning angle from each layer has up to 3 points with different range. segmentation (right). There are 4 different colors of the data points in the left image indicating points from different laser scan levels. Whereas in the right image, different colors are assigned to each point cluster identifying different obstacles. Ground points are filtered out automatically by using the ground flag output in raw sensor data. To segment the 3d point clouds to clusters, we use an efficient graph based segmentation algorithm described in [2]. The algorithm is sufficiently fast, as it runs in time nearly linear in the number of the 3d points. The algorithm requires a construction of a graphical model between 3d points. This graph is built by assigning each node to a 3d point and adding an edge between certain pairs of the points. The weight of each edge is defined by the Euclidean distance of the two points. The points are connected by an edge if they are: 1) from the same layer with adjacent horizontal scanning angle; or 2) from the adjacent layer with the same horizontal scanning angle. An illustrative example of the constructed graph is shown in Fig. 4. Different color denotes the points from different layer. Each horizontal scanning angle from each layer can have up to 3 points with different range as the receiver can record up to 3 peaks. The points are segmented to different clusters by iteratively merging similar clusters. Lidar is able to output accurate position of the obstacle with the error up to the resolution of the scanning angle, but the speed of the obstacle cannot be inferred reliably due to sparsity of points at long distance which makes it very difficult to do temporal association. Aided by speed information provided by radar, we are able to detect the parked vehicle robustly. We associate the lidar point cluster with the radar observation if the distance is within a threshold. Fig. 5 shows the obstacle detection results (points in cyan) and the corresponding radar observation (circle in red). IV. LATERAL MOTION PLANNING To develop a lateral motion planner to maintain safe clearance to surrounding obstacles, we propose a two-step approach by making use of the road structure. First, path Fig. 5. Obstacle detection results (points in cyan) and radar observation (circle in red). candidates are pre-computed based on the road structures such as lane markings and curbs. Second, the paths are evaluated based on the position and distance of the surrounding obstacles that will have impact to the driving route of the host vehicle. The obstacle size estimation is difficult since lidar observations are sparse, especially at long distances. Therefore, only the observed points are used to construct the driving path. As the host vehicle is getting closer to the obstacle, the points become denser potentially allowing us to have a good estimation of the obstacle size. A. Pre-computation of path candidates Rough information about road structure can be obtained from RNDF (Route Network Definition File). With RNDF, road and map data as inputs, we generate smooth path candidates by solving an optimization objective function that takes curvature into account. The default driving path is the 414

4 3 rd left 2 nd left 1 st left center Lane boundary Fig st right Lane boundary 2 nd right 3 rd right Path candidates with different lateral offsets in a single lane. lane center. When the host vehicle is driving a path, the center of the host vehicle follows that path. There are 2k +1 candidate paths in a single lane: k candidate paths to the left of the lane center and another k candidate paths to the right. The leftmost and rightmost candidate paths are overlapping with the lane markings. Fig. 6 shows an example of 7 (k = 3) candidate paths in a single lane, with 3 paths on each side of the center path. These candidate paths cover a range of possible routes that the host vehicle will take to safely pass the obstacles on the roadside or to keep a proper distance to the vehicles on either side of the adjacent lanes. B. Evaluation of path candidates Before evaluating path candidates, the host vehicle needs to know which obstacles will interfere with the current driving path laterally and leave out those that will not. For example, vehicles driving in front of the host vehicle in the same lane will not affect it laterally as the host vehicle has to drive behind them along the current path unless a lane change is needed. An obstacle is not involved in the evaluation of path candidates if it meets at least one of the following conditions: 1) its distance along the current driving path is larger than a threshold; 2) it is moving in the same direction as the current driving path and its distance along the path is larger than the stop distance of the host vehicle. The stop distance is defined as the distance from the host vehicle to the location where the vehicle is supposed to stop, which is caused by the presence of an obstacle in front, a traffic sign, etc.; 3) its relative velocity is positive, i.e., the vehicle is moving away from us; 4) it is driving in the same lane as the host vehicle and its velocity is larger than some small threshold, i.e., it is neither stopping nor moving slowly. After ignoring irrelevant obstacles that satisfy either one of the four conditions above, the host vehicle makes decision of the best path based on the distance of the remaining path candidates to the surrounding obstacles. Basically, the host vehicle will keep the center lane path as much as possible unless it is forced to take other paths with different lateral offsets based on the position of the obstacles. The best path candidate is selected such that the host vehicle passes the obstacle with enough clearance and keeps as close as possible to the center lane. There are also some exceptions that will keep the host vehicle from taking lateral move even when there is obstacle on the roadside. For example, when another vehicle is trying to pass the host vehicle from the left of the current driving lane, we do not take a path with a left lateral offset even if an obstacle is detected to the right on the roadside. Instead we keep driving at the center of the obstacle Path 7 Path 6 Path 5 Path 4 Path 3 Path 2 Path 1 Fig. 7. Obstacle avoidance without lane change. There is an obstacle on the roadside that obstructs a part of the lane. Path candidates 1 and 2 will cause the host vehicle to collide with the obstacle and are ruled out first. There is not enough clearance for the host vehicle to pass the obstacle comfortably while taking path candidates 3 and 4. Path candidate 5 is chosen as the result. lane while gradually reducing speed and preparing to stop if the required lateral offset is not feasible before passing the obstacle on the right. If the host vehicle is not able to find any path which can maintain enough clearance to pass the obstacle, it will keep the current path while decelerating as it is getting closer to the roadside obstacle. It makes sense to stop the host vehicle behind the obstacle if the lateral clearance is too small. Fig. 7 shows an example of the path selection from a finite set of the path candidates. There is an obstacle on the roadside that obstructs a part of the lane. Path candidates 1 and 2 will cause the host vehicle to collide with the obstacle and are ruled out first. There is not enough clearance for the host vehicle to pass the obstacle comfortably while taking path candidates 3 and 4. Path candidate 5 is chosen as the result. V. TEMPORAL FILTERING FOR LATERAL MOVE Once the parked vehicle is detected, the detection result is sent to the planner to command the motion of the host vehicle. At each frame, the planner will evaluate the current input from the detection module along with the input history and make decisions for lateral movement. If the parked roadside vehicle were confidently detected over a sufficiently long sequence of frames, the planner would command the host vehicle to make its lateral move as soon as the parked vehicle is observed and move back to lane center as soon as such a vehicle is no longer present. However, the detection results might not always be reliable due to noise or position uncertainty of the host vehicle or the road terrain variations. If we act too quickly, i.e., as soon as the parked vehicle is first detected, the planner may be prone to false alarms. If we wait for a long time for the detection confirmation, we may have to resort to uncomfortable lateral and longitudinal movement. In this section, we design a temporal filtering scheme to make the decision for lateral movement avoiding the sudden lateral movements back and forth based on the history of the detection results. The number of frames for the vehicle 415

5 detection confirmation is set to 3 in our experiments as an acceptable trade-off for detection and motion planning. The preferred path ψ pref is chosen based on the detection result at the current frame which serves as the input to the temporal filtering algorithm. The final output path ψ out is calculated from the history of the previous paths. The previous path ψ prev denotes the output of the path from a set of path candidates in the previous frame. If the preferred path ψ pref and the previous path ψ prev are not equal, a lateral move is in order. To deal with false alarms and missing detections, we introduce two counters: observation counter n obv and decision counter n dec. When a lateral move is initiated, the observation counter n obv is set to a high value k obv which would gradually decrease to zero when no obstacle is observed in the following frames. The decision counter n dec is initialized to a small value and changes its value depending on the observation counter. It is responsible for setting the output path ψ out of the current frame. Only if n dec is greater than a pre-defined threshold th ndec, the output ψ out will be the same as the preferred lateral path ψ pref, and the lateral move command will be sent to the controller. It will take a few frames for n dec to exceed the threshold th ndec which prevents the planner from acting too quickly upon obstacle detection. It will also take a few frames for n obv to decrease to zero when the obstacle detection is not persistent, making it robust to missing detections. We introduce intermediate path variable ψ n to keep track of the path based on detections only. This logic is described in Algorithm 1. Algorithm 1 Temporal Filtering algorithm Input: The preferred path ψ pref, the previous path ψ prev. Output: The output path ψ out. 1: ψ n = ψ pref ; 2: if ψ pref ψ prev then 3: n obv = k obv ; /* initialize observation counter */ 4: /* initialize decision counter */ 5: if ψ n ψ prevn then 6: n dec = 1; 7: else n dec = min(n dec + 1, k dec ); 8: else 9: if ψ n ψ prevn then 10: n obv = max(n obv 1, 0); 11: if n obv > 0 then 12: n dec = min(n dec + 1, k dec ); 13: else n dec = max(n dec 1, 0); 14: if ψ n > ψ prevn then 15: n obv = n dec = 0; 16: ψ n = ψ prevn ; 17: /* Assign the output path ψ out and reset counters */ 18: if n dec th ndec then 19: ψ out = ψ n ; n obv = n dec = 0; 20: else ψ out = ψ prev ; 21: ψ prevn = ψ n. When the host vehicle drives with a lateral offset and pass the roadside parked vehicle, it will move back to the center line of the lane if another roadside obstacle is not observed for n no consecutive frames. VI. SPEED CONTROL Speed control helps the host vehicle pass the parked vehicle with comfortable and safe speed that imitates the behavior of a careful human driver. When a driver approaches the parked vehicle on the roadside, he would decelerate and pass the vehicle with reduced speed. This carefulness is warranted because someone in the parked vehicle might open the door while the host vehicle is passing by. After selecting the best lateral offset, our planner determines if deceleration is necessary while passing a parked vehicle. If the lateral distance between the best offset path and the parked vehicle is smaller than a threshold, the host vehicle decelerates until the speed is within a limit. While the planner adjusts a gap between the host and the parked vehicle as much as possible, sufficient gap cannot be ensured in some situations, e.g., a parked vehicle protrudes into the lane from the roadside, or there is another vehicle on the other side of road. Once determined that a deceleration is necessary, planner plans for a comfortable level of deceleration at the point of passing the parked vehicle. The deceleration levels are calculated based on actual road experiments. VII. EXPERIMENTS We tested the proposed method in numerous scenarios with vehicles parked on the roadside. The experimental results demonstrate effectiveness of our proposed method to detect roadside obstacles and safely pass them with comfortable speed. Two examples of the roadside parked vehicle detection and avoidance are shown in Fig. 8 and Fig. 9. In Fig. 8 (a), the parked vehicle on the left side of the one-lane road is detected at 110 meters away. There are only a few reflections from a parked vehicle at long distance. The reflections are shown in cyan color and highlighted by a red circle (pointed by an arrow). A path with lateral offset to the right to pass the parked vehicle is shown in green in Fig. 8 (b). In Fig. 8 (c), the host vehicle moves along the path and is getting closer to the parked vehicle. The host vehicle which is about to pass the stationary vehicle and that just after passing the vehicle are shown in Fig. 8 (d) and (e), respectively. Similarly in Fig. 9, the parked vehicle is on the right side of the road on a two-lane road. The host vehicle was able to successfully detect the parked vehicle and planned the lateral move to avoid it. In Fig. 9 (c), multiple obstacles are detected and shown in cyan. Only an obstacle that has an impact to the current driving path is taken into account for the path candidate selection and shown in the red circle. In Fig. 9 (e), the host vehicle has passed the parked vehicle, and none of the detected obstacles has any impact to the driving path. It should be noted that our proposed system is not immune to some failure modes. From the perception side, the sensors may lose detection of the parked vehicle due to their limited 416

6 field of view, especially when the road is curved or has a significant slope. From the planning side, an incoming vehicle in the adjacent lane suddenly appearing or a vehicle coming fast from behind in the next lane while the lateral maneuver is underway may also create problems. Future research is expected to address these issues. VIII. CONCLUSION In this paper, we have presented a robust detection and motion planning method for roadside parked vehicle avoidance. The method is able to effectively detect the roadside parked vehicle by appropriately fusing the information from lidar, radar and map. The best path is selected from a finite set of pre-computed path candidates with different lateral offset to ensure a smooth and robust trajectory to pass the parked vehicle. Temporal filtering is also developed to handle missing detections and false alarms, and speed control is applied to ensure safe and comfortable passing. The experimental results demonstrate that the proposed method is capable of taking advantage of multiple sensors reliably, assuring safety and comfort of driving when dealing with obstacles. Acknowledgement: We would like to thank Masahiro Harada for discussion and comments about the paper. REFERENCES [1] H. Cho, S. Y. Woo Seo, B. V. K. V. Kumar, and R. R. Rajkumar. A multi-sensor fusion system for moving object detection and tracking in urban driving environments. IEEE International Conference on Robotics and Automation (ICRA), , [2] P. F. Felzenszwalb, D. P. Huttenlocher. Efficient Graph-Based Image Segmentation. International Journal of Computer Vision, Vol. 59 no. 2, [3] D. Gohring, M. Wang, M. Schnurmacher, and T. Ganjineh. Radar/lidar sensor fusion for car-following on highways. IEEE International Conference on Robotics and Automation (ICRA), , [4] T. M. Howard, C. J. Green, A. Kelly, and D. Ferguson. State space sampling of feasible motions for high-performance mobile robot navigation in complex environments. Journal of Field Robotics, 25(6 7), , [5] M. McNaughton, C. Urmson, J. M. Dolan, and J.-W. Lee. Motion planning for autonomous driving with a conformal spatiotemporal lattice. IEEE International Conference on Robotics and Automation (ICRA), , [6] D. Nuss, S. Manuel, and D. Klaus. Consistent environmental modeling by use of occupancy grid maps, digital road maps, and multi-object tracking. IEEE Intelligent Vehicles Symposium, , [7] M. Pitvoraiko and A. Kelly. Efficient constrained path planning via search in state lattices. International Symposium on Artificial Intelligence, Robotics, and Automation in Space, [8] C. Premebida, O. Ludwig, and U. Nunes. LIDAR and Vision-Based Pedestrian Detection System, Journal of Field Robotics, 26(9), , [9] N. Shibata, S. Sugiyama and T. Wada. Collision Avoidance Control with Steering using Velocity Potential Field. IEEE Intelligent Vehicles Symposium, , [10] C. Stiller, J. Hipp, and A. E. C. Rossig. Multisensor obstacle detection and tracking. Image and Vision Computing, 18, , [11] M. Werling, J. Ziegler, S. Kammel, S. Thrun. Optimal trajectories for dynamic street scenarios in a frenet frame. IEEE International Conference on Robotics and Automation (ICRA), , [12] W. Xu, J. Wei, J. M. Dolan, H. Zhao, and H. Zha. A Real-Time Motion Planner with Trajectory Optimization for Autonomous Vehicles. IEEE International Conference on Robotics and Automation (ICRA), , [13] J. Ziegler, P. Bender, T. Dang, and C. Stiller. Trajectory Planning for BERTHA - a Local, Continuous Method. IEEE Intelligent Vehicles Symposium, ,

7 (a) (b) (c) (d) (e) Fig. 8. An example of the parked vehicle avoidance is demonstrated. The host vehicle is driving on a one-lane road with a vehicle parked on the left side of the road. (a) Parked vehicle is detected on the left side of the road. (b) A path with lateral offset to the right is selected. (c) The host vehicle is approaching the parked vehicle. (d) The host vehicle is about to pass the parked vehicle. (e) The host vehicle has passed the parked vehicle. (a) (b) (c) (d) (e) Fig. 9. An example of the parked vehicle avoidance is demonstrated. The host vehicle is driving on a two-lane road with a vehicle parked on the right side of the road. (a) Parked vehicle is detected on the right side of the road. (b) A path with lateral offset to the left is selected. (c) The host vehicle is approaching the parked vehicle. (d) The host vehicle is about to pass the parked vehicle. (e) The host vehicle has passed the parked vehicle. 418