- Email: [email protected]
SVM based Intention Inference and Motion Planning at Uncontrolled Intersection
10th IFAC Symposium on Intelligent Autonomous Vehicles 10th IFAC Symposium on Autonomous Vehicles 10th IFACPoland, Symposium on Intelligent Intelligent Autonomous Vehicles Available online at www.sciencedirect.com Gdansk, July 3-5, 2019 10th IFACPoland, Symposium on Intelligent Autonomous Vehicles Gdansk, Poland, July 3-5, 3-5, 2019 Gdansk, July 2019 Gdansk, Poland, July 3-5, 2019
ScienceDirect
IFAC PapersOnLine 52-8 (2019) 356–361
SVM based Intention Inference and Motion Planning SVM based Intention Inference and Motion Planning Inference at Uncontrolled Intersection SVM based Intention and Motion Planning at Uncontrolled Intersection at Uncontrolled Intersection Yonghwan Jeong*, Kyongsu Yi**, and Sungmin Park*** Yonghwan Jeong*, Yonghwan Jeong*, Kyongsu Kyongsu Yi**, and and Sungmin Sungmin Park*** Park*** Yi**, Yonghwan Jeong*, Kyongsu Yi**, and Sungmin Park*** *Mechanical Engineering Department, Seoul National University *Mechanical Engineering Department, Seoul *Mechanical Engineering Department, Seoul National National University University Seoul, Korea,(e-mail: [email protected]) *Mechanical Engineering Department, Seoul National University Seoul, Korea,(e-mail: [email protected]) **Mechanical Engineering Department, Seoul National University Seoul, Korea,(e-mail: [email protected]) **Mechanical Department, Seoul **Mechanical Engineering Engineering Department, Seoul National National University University Seoul, Korea,(e-mail: [email protected]) **Mechanical Engineering Department, Seoul National University Seoul, Korea,(e-mail: [email protected]) Seoul, Center, Korea,(e-mail: [email protected]) ***R&D Hyundai Motor Company Seoul, Center, Korea,(e-mail: [email protected]) ***R&D Motor Company ***R&D Korea,(e-mail: Center, Hyundai Hwaseong, [email protected]) ***R&D Center, Hyundai Motor Company Hwaseong, [email protected]) Korea,(e-mail: Hwaseong, [email protected]) Hwaseong, Korea,(e-mail: [email protected])
Abstract: This paper presents a support vector machine (SVM) based intention inference and motion Abstract: This aa support machine (SVM) based inference and motion Abstract: This paper paper presents support vector machine (SVM)intersection. based intention intention inference andvehicles motion planning algorithm for presents autonomous drivingvector through uncontrolled Intention of target Abstract: This paper presents a support vector machine (SVM)intersection. based intention inference andvehicles motion planning algorithm for autonomous driving through uncontrolled Intention of target planning algorithm for autonomous driving through uncontrolled intersection. Intention of target vehicles is inferred using SVM with intersection map to predict the future state of targets. A cross point, which planning algorithm for autonomous driving through uncontrolled intersection. Intention of target vehicles is using SVM intersection map the future of A which is inferred inferred usingcollision SVM with with intersection map to to predict predict thepredicted future state state of targets. targets. A cross cross point, point, which has a highest probability, is estimated using target state considering prediction is inferred usingcollision SVM with intersection map to predict thepredicted future state of targets. A cross point, which has a highest collision probability, is estimated using predicted target state considering prediction has a highest probability, is estimated using target state considering prediction uncertainty. Longitudinal acceleration is determined using model predictive control approach considering has a highest collision probability, is estimated using predicted target state considering prediction uncertainty. Longitudinal acceleration is determined determined using model predictive predictive controland approach considering uncertainty. is model control approach the predictedLongitudinal cross point.acceleration The proposed algorithm using is validated via simulation vehicleconsidering tests. The uncertainty. Longitudinal acceleration is determined using model predictive controland approach considering the predicted cross point. The proposed algorithm is validated via simulation and vehicle tests. The The the predicted cross point. The proposed algorithm is validated via simulation vehicle tests. results show the accurate intention inference and human-like motion planning at uncontrolled intersection the predicted cross point.intention The proposed algorithm is validated viaplanning simulation and vehicleintersection tests. The results show the accurate inference and human-like motion at uncontrolled results show the accurate intention inference and human-like motion planning at uncontrolled intersection scenarios. results show the accurate intention inference and human-like motion planning at uncontrolled intersection scenarios. scenarios. scenarios. © 2019, IFAC (Internationalvehicles, Federation of Automatic Control) Hosting by Elsevier Ltd. All rights reserved. Keywords: Autonomous Machine learning, Support Vector Machine, Intention Inference, Keywords: Autonomous vehicles, Machine learning, Vector Keywords: Autonomous vehicles, Control. MachineUncontrolled learning, Support Support Vector Machine, Machine, Intention Intention Inference, Inference, Motion Planning, Model Predictive Intersection. Keywords: Autonomous Predictive vehicles, MachineUncontrolled learning, Support Vector Machine, Intention Inference, Motion Motion Planning, Planning, Model Model Predictive Control. Control. Uncontrolled Intersection. Intersection. Motion Planning, Model Predictive Control. Uncontrolled Intersection. 1. INTRODUCTION 1. 1. INTRODUCTION INTRODUCTION INTRODUCTION For the last decade, 1.lots of research has been carried out for of for research has been beendriving carried out for for For the last decade, lots of research has carriedsuch out each element technology autonomous as For the last decade, lots of for research has beendriving carriedsuch out for each element technology autonomous as perception, decision, and control (Bengler et al. [2014]). each elementdecision, technology for autonomous driving such as decision, and control (Bengler et being al. [2014]). [2014]). perception, control (Bengler al. Some achievement ofand these studies are nowet massperception, decision, and control (Bengler et al. [2014]). Some achievement of these studies are now beingdriving massproduced in the name of partially autonomous Some achievement of these studies are now beingdriving massproduced in of autonomous produced in the the name name of partially partially autonomous systems (PADS). places, where accidentsdriving occur produced in the However, name ofthe partially autonomous driving However, the places, where accidents occur systems (PADS). theand places, where accidents occur frequently such asHowever, crosswalks intersections of the urban systems (PADS). However, the places, where accidents occur frequently such as crosswalks and intersections of the urban frequently such asnot crosswalks and intersections of the driver urban environment, are addressed by the commercialized frequently such as crosswalks and intersections of the urban environment, are addressed by driver environment, are not not addressed by the the commercialized commercialized driver assistance system. Especially uncontrolled intersection is one environment, are not addressed by the commercialized driver assistance system. Especially uncontrolled intersection is one assistance system. Especially uncontrolled intersection is one of the most dangerous place in urban environment because assistance system. Especially uncontrolled intersectionbecause is one of the most dangerous place in urban environment because of the most dangerous place in urban environment there ismost no traffic signals to controlenvironment and guidebecause traffic of the is place in there no dangerous traffic signals to urban controlcontinuous and guideresearch traffic participants. To solve these problems, there is no To traffic to controlcontinuous and guideresearch traffic participants. solvesignals these problems, has been conducted autonomous in anresearch urban participants. To solvefor these problems, driving continuous has been conducted for autonomous driving in an has been conducted for autonomous an urban urban environment to increase safety of alldriving traffic in participants has been conducted for autonomous driving in an urban environment to [2008]). increase In safety of studies, all traffic participants (Bougler et al. these two issues are environment to increase safety of all traffic participants (Bougler et for al. [2008]). In these studies,attwouncontrolled issues are addressed autonomous driving (Bougler et for al. [2008]). In these studies,attwouncontrolled issues are addressed driving intersections. The autonomous first issue is inferring theatintention of the addressed for autonomous driving uncontrolled intersections. The whether first issuethe is inferring the intention of the target vehicles other vehicle is making intersections. The first issuethe is inferring the intention of the target vehicles other The vehicle is issue making concessions and whether where to the proceed. second is target vehicles whether other vehicle is making concessions and towhere to subject proceed.vehicle The second issue is motion planning plan the behavior so that concessions and towhere to subject proceed.vehicle The second issue is motion planning plan the behavior so that the vehicle can pass through the uncontrolled intersection motion planning to plan the subject vehicle behavior so that the vehicle can pass through the uncontrolled intersection the vehicle can pass uncontrolled intersection over the minimal riskthrough of thethecollision according to the the vehicle can pass uncontrolled intersection over the ofminimal riskthrough of thethe collision according to the intention other vehicles. over the ofminimal risk of the collision according to the intention other vehicles. intention of other vehicles. First, a various approaches for intention inference have been First, a out various approaches intention inference have been carried in the literature for from model-based approaches to First, a out various approaches for intention inference have been carried in the literature from model-based approaches to machine learning based approaches. For model-based carried outlearning in the literature from model-based to machine approaches. For approaches model-based approach, Interactingbased Multiple Model (IMM) filter based machine learning based approaches. For model-based approach, Interacting Model (IMM) based intention inference hasMultiple been used widely from filter aviation to approach, inference Interacting Model (IMM) based intention hasMultiple been used widely from filter aviation to intention inference has been used widely from aviation to
motor industry. Toledo-Moreo and Zamora-Izquierdo [2009] motor industry. Toledo-Moreo andalgorithm Zamora-Izquierdo [2009] proposed the intention inference for lane change motor industry. Toledo-Moreo andalgorithm Zamora-Izquierdo [2009] proposed the intention inference for lane change proposed the intention inference algorithm for lane change prediction the by using GPS/IMU measurement IMM filter. proposed intention inference algorithm based for lane change prediction by using GPS/IMU measurement based IMM filter. prediction by Liebner using GPS/IMU measurement IMMdriver filter. Furthermore, et al. [2012] used the based intelligent prediction by using GPS/IMU measurement based IMM filter. Furthermore, Liebner et al. [2012] used the intelligent driver model to infer the subject vehicle driver's intention to utilize Furthermore, Liebner et al. [2012] used the intelligent driver model to infer the subject vehicle intention to utilize for intersection assist system. Fordriver's machine learning based model to infer the subject vehicle driver's intention to utilize for intersection assist system. For machine learning based approach, S. Lefèvre et al. [2011] proposed the Dynamic for intersection assist et system. For machine learning based approach, S. Lefèvre al. [2011] Dynamic Bayesian Network (DBN) basedproposed driver's the manoeuvre approach, S. Lefèvre et al. [2011] proposed the Dynamic Bayesian Network considering (DBN) based driver's observation. manoeuvre inference algorithm probabilistic Bayesian algorithm Network considering (DBN) based driver's observation. manoeuvre inference probabilistic Kumagai et al. [2003] used simple DBN to observation. predict the inference algorithm considering probabilistic Kumagai et al. [2003] used simple DBN to predict the Kumagai etto al. simple DBNbased to predict the probability stop[2003] at stopused line. A classifier on Support Kumagai etto al. [2003] used simple DBNbased to predict the probability stop at stop line. A classifier on Support Vector Machine (SVM) and Hidden Markov Model (HMM) probability to stop at stop line. A classifier based on Support Vector Machine and Hidden Markov Model (HMM) to classify driver(SVM) behaviours at intersections proposed by Vector Machine (SVM) and Hidden Markov Model (HMM) to classify driver behaviours at intersections proposed by Aoude et al. [2012]. In recent years, communication based to classify behaviours atyears, intersections proposed by Aoude et al.driver [2012]. In recent communication based approaches base been attempted to develop collision Aoude et al. [2012]. In recent years, communication based approaches been attempted to the develop collision avoidance atbase intersection to share safety-related approaches at baseintersection been attempted to the develop collision avoidance to share safety-related information using V2V by Hafner et al. [2011]. avoidance at intersection to share the safety-related information using V2V by Hafner et al. [2011]. information using V2V by Hafner et al. [2011]. Several approaches have been developed to deal with the Several approaches have been developed to with Several approaches havethe been developeddesired to deal dealmotion with the the second issue, planning appropriate to Several approaches have been developed to dealmotion with the second issue, planning the appropriate desired to avoid collision and pass intersection quickly. The hierarchical secondcollision issue, and planning the appropriate desired motion to avoid passwhich intersection quickly. The and hierarchical planning framework, combining task motion avoid collision and pass intersection quickly. The hierarchical planning framework, which combining task and motion planning, generates trajectories in several intersection planning framework, which combining task and motion planning, generates trajectories in several intersection scenarios (Chen et al. [2016]). However, this study deal with planning, generates trajectories in several intersection scenarios (Chen et al. [2016]). However, this studyalldeal with uncertainties of the target vehicles by considering possible scenarios (Chen et al. [2016]). However, this study deal with uncertainties of the target vehicles by considering all possible situations notof predict the intention of the target. Schildbach et uncertainties the target vehicles by considering all possible situations intention the target. et al. [2016] not usespredict robustthe MPC to findof safe gaps inSchildbach the crossing situations not predict the intention of the target. Schildbach et al. uses to in crossing al. [2016] [2016] uses robust robust MPC to find find safe gaps in the the crossing traffic concerning the MPC actuator limitsafe andgaps safety constraints. al. [2016] uses robust MPC to find safe gaps in the crossing traffic concerning limitto and safety constraints. Similar approach the areactuator applied automated yielding traffic concerning theare actuator limitto and safety constraints. Similar approach applied automated yielding manoeuvres such as lane change, intersection, and Similar approach are appliedchange, to automated yielding manoeuvres such et asal. lane intersection, and roundabout (Nilsson [2016]). manoeuvres(Nilsson such etasal. lane change, intersection, and roundabout [2016]). roundabout (Nilsson et al. [2016]).
2405-8963 © 2019, IFAC (International Federation of Automatic Control) Hosting by Elsevier Ltd. All rights reserved. Copyright 2019 responsibility IFAC Peer review©under of International Federation of Automatic Control. Copyright © 2019 2019 IFAC IFAC Copyright © 10.1016/j.ifacol.2019.08.113 Copyright © 2019 IFAC
2019 IFAC IAV Gdansk, Poland, July 3-5, 2019
Yonghwan Jeong et al. / IFAC PapersOnLine 52-8 (2019) 356–361
357
This paper focused on designing a SVM based intention inference and motion-planning algorithm at uncontrolled intersection. The proposed algorithm consist of target intention inference and longitudinal motion planning as shown in Fig. 1. SVM classifier with intersection map is used to infer the intention of the targets before entering the intersection only using longitudinal motion of target vehicles measured by LiDAR sensors. Based on the inferred intention of targets, longitudinal motion of the subject vehicle is determined using MPC approach. In this stage, we predict the future motion of vehicles and estimate the cross point. Then, we optimize the motion of subject vehicle to secure the safety clearance while preventing the unnecessary deceleration. The simulation has been conducted to show the effectiveness of the proposed algorithm. The proposed algorithm implemented to the test vehicle and validated via vehicle tests. Fig. 2. Example result of intersection map building 2. SVM BASED TARGET INTENTION INFERENCE 2.2 Support Vector Machine Training
Target intention inference is classification problem of human driving behavior using limited information. Among various classification techniques, SVM is appropriate to classify driving behavior at uncontrolled intersection, because the lateral motion of intersection approaching vehicle can be classified as discrete intentions such as going straight and turning. In addition, since the direction that can be taken in each lane is specified by traffic rules, the classification problem can be simplified as binary classification.
Collecting learning data is a crucial step for machine learning based approach. The training data can be obtained in a various methods such as computer simulation, hardware in the loop simulation (HILS), and driving data in real traffic. Among the methods for data generation, computer simulation with random parameter has been used to reduce the effort to acquiring training data. When generating the data set for training SVM classifier, we assumed that the driver try to limit the lateral acceleration while turning manoeuvre by decelerating before entering the intersection. In order to reflect the various characteristics of driver at intersection approach, we applied the normal distribution based simulation parameters to generate the realistic training data. Initial speed vini, maximum velocity vmax, maximum lateral acceleration alat,max are randomly generated and defined as follows:
2.1 Intersection Map Building As mentioned before, structured intersection map is required to simplify classification problem by providing available manoeuvres at each lanes. In this study, intersection map for target intention inference only includes the location of lane, enter/exit node, and connection relationships between nodes. The constructed intersection map is depicted in Fig. 2. As shown in Fig.2, centerline of the road and lane makers are marked as yellow solid line and black dotted line, respectively. Enter and exit node of intersection are marked as red and blue circle with index number respectively. Connection relationship between each nodes is defined as connectivity matrix Ccon. If i-th enter node and j-th exit node is connected, (i,j) component of is defined as 0, 1, 2, 3 for non-connected, straight, left turn, and right turn case respectively. Example connection relationships of entering node 1 and 2 are illustrated in Fig.2 as magenta and cyan solid lines.
Moving Obs. Detection
Localization
Route/Task Planning Intersection Map
Target State Global Position Target Task
Target Intention Inference - SVM Training (offline process) - SVM based Target Intention Inference (online process)
Longitudinal Motion Planning - Vehicle Motion Prediction - Cross Point Region Estimation - MPC based Velocity Planning
Desired Ax
vmax 50km / h N 0, (7.5km / h) 2 vini 50km / h N 0, (10km / h) 2 alat ,max 2m / s 2 N 0, (0.25m / s 2 ) 2 3m / s ades 1.5m / s 2
(1)
2
Total 300 sets of data is generated for straight, left turn, and right turn respectively. For lateral controller, a steering controller embedded in Carsim is used to tracking the path, which is defined in intersection map. Training data set for SVM is generated as shown in Fig. 3.
Vehicle
Desired SWA
Lane Info
Fig. 1. Overall architecture of the autonomous driving algorithm for uncontrolled intersection
Fig. 3. Training data set for SVM algorithm 357
2019 IFAC IAV 358 Gdansk, Poland, July 3-5, 2019
Yonghwan Jeong et al. / IFAC PapersOnLine 52-8 (2019) 356–361
3. MOTION PLANNING AT UNCONTROLLED INTERSECTION
Therefore, a simple path tracking controller based on Ackermann steering angle is used to determine a steering angle input in Rajamani [2011]. Longitudinal motion of the target vehicles entering intersection are modelled under several assumptions. Drivers, who want to cross the intersection straight, normally control the vehicle to adhere to speed limit. Other drivers, who want to left or right turn, generally decelerate the vehicle to lower lateral acceleration for ride comfort. Under there assumptions, desired velocity can be determined as
In this section, intersection map, intention, position, and longitudinal velocity of target vehicle are used to plan the desired motion at uncontrolled intersection. The intersection map is the same one, which is defined for target intention inference at section 2, and the intention is the result from the SVM based target intention inference algorithm. The position and velocity of the target vehicles are acquired form LiDAR, which is easily estimative values. This state definition is necessary to increase the easiness of implementation to test platform. To minimize the effect of the perception algorithm to motion planning, variables difficult to estimate are excluded in this study. The algorithmic flow of the proposed motion-planning algorithm part is illustrated in Fig. 4.
vdes (k ) max vmax
Based on the information from the perception and intention inference algorithm, cross point between subject and target vehicles has been predicted. In this paper, ‘cross point’ means a place and timing which has a highest collision probability. Therefore, cross point is defined as a region, not a single point. To predict the cross point, future motion of the subject and target vehicles should be predicted upon consideration of the sensor and prediction uncertainty. First, to predict the state of the vehicles, the particle model has been used as a vehicle model. The vehicle model for state prediction is expressed in discrete from as
An example of the prediction results of subject and single target vehicle case is illustrated in Fig 5. The sampling time of the prediction is 0.1s and the moment where the end of prediction is assigned when vehicle exit the intersection. As can be seen in Fig 5, the future trajectory of the subject and target vehicle are represented as a blue and red each marked at intervals of 1 second. For easy analysis, all prediction uncertainty are integrated in the predicted states of target. Target vehicle, which is indicated as a red car, has a two shadows at each step. These two shadows shows the ±3σ position of each step which indicates that the probability of the target vehicle exist between two shadows is almost 99%. Therefore, cross point can be predicted by monitoring an overlapping between predicted trajectory of subject vehicle and region can be determined by uncertainty zone. In this example case, the nearest moment between vehicles is described as a green car in Fig 5.
px (k 1) px (k ) v(k ) cos( (k ))dt p y (k 1) p y (k ) v(k ) sin( (k ))dt
(2)
v(k ) tan( (k ))dt L v(k 1) v(k ) a(k )dt
where px, py, ψ and vx denotes a longitudinal, lateral position, yaw angle and velocity of the vehicle. δ, L and ax denotes steering angle, wheelbase and longitudinal acceleration to vehicle model. dt is sampling time of the prediction.
After the center of cross point is defined, a size of region should be determined using propagated uncertainty of target vehicle position. In Fig. 6, clearance between subject vehicle and mean value of the predicted target vehicle is illustrated as blue line, and clearance between subject vehicle and ±3σ position is illustrated as red line. The region of the cross point
The input of the vehicle model should be properly determined to propagate the state of the vehicle with an appropriate uncertainty. In this paper, the intention of the target vehicles are pre-defined using the SVM based intention inference algorithm, which means that the future trajectory of targets are already estimated based on the trajectory information of the intersection map with inferred intention. Cross Point Prediction
Motion Planning
Target State
Target Intention
Subject Vehicle State Prediction
Desired Path
Rough Map
Target Vehicle State Prediction
Desired Acceleration Cross Point (Collision Region) Estimation
(3)
where, σv is the variance of uncertainty of the desired velocity of the vehicle. If we assume that the driver has a nominal drive pattern, vmax and alat,norm can be defined. First, 50 km/h, which is an urban road speed limit in South Korea, is used as vmax. For alat,norm, 1m/s2 is used as a nominal lateral acceleration. σv is assumed as a normal distribution with standard deviation 0.25m/s at each step when we use a sampling time as 0.1s. This uncertainty has been propagated using process update of Kalman filter.
3.1 Cross Point Prediction
(k 1) (k )
alat , norm v
Longitudinal Motion Optimization
Fig. 5. Example of vehicle state prediction and cross point estimation
Fig. 4. Block diagram for longitudinal motion planning part 358
2019 IFAC IAV Gdansk, Poland, July 3-5, 2019
Yonghwan Jeong et al. / IFAC PapersOnLine 52-8 (2019) 356–361
can be determined based on predicted clearance as follows:
scross
minimum among the all vehicles entering the intersection, ego vehicle accelerate to cross the intersection first by planning a desired trajectory above cross point. In other cases, ego vehicle decelerate until crossing priority of ego vehicle become first.
x target (tcp ) x target (tcp )
tcross
scross vego (tcp )
(4)
3.3 Optimal Longitudinal Motion Planning
where, tcp is the nearest moment between subject and target vehicle. x+target and x-target respectively denotes a +3σ and -3σ clearance. In addition, vego is the predicted velocity profile of the subject vehicle. Using this predicted information, a size of cross point region can be determined as equation (4).
The optimal longitudinal motion planning of the subject vehicle is designed using model predictive control (MPC) method. The MPC problem is designed based on the predicted state of vehicles and cross point. The region of cross point and crossing priority are used to determine position constraint, which must be satisfied to avoid collision with participants of intersection. Then, vehicle dynamics and velocity, acceleration constraint are also considered to satisfy the guarantee the comfort.
3.2 Crossing Priority Decision In previous section, the cross point has been defined based on ego and target vehicle state prediction. To avoid collision, ego vehicle should not violate the region of the cross point. In other words, it is necessary to determine whether to accelerate or decelerate the ego vehicle. In this paper, time to intersection cross (TTIC) is defined to determine crossing priority. TTIC means the estimate time to cross the intersection. TTIC is determined as follows: n 1
TTIC (t )
d remain (k 1) d remain (k )
k 1 0.5 (v pred ( k 1) v pred ( k ))
Since the purpose of the longitudinal motion-planning algorithm for uncontrolled intersection is to determine desired velocity for collision avoidance at intersection, only longitudinal dynamics of equation (2) is considered in simple form. State and input for longitudinal dynamics is defined as
state : X p v input :u a
(5)
where, dremain and vpred denotes the remaining distance to exit of the intersection and predicted velocity profile, respectively. Using the equation (5) with dremain and vpred of the ego and nominal value of predicted target vehicle state, TTIC of ego and target vehicle can be determined. After that, crossing priority is decided using the TTIC of all participant vehicles entering the intersection. The criteria of crossing priority decision is summarized
cross first cross later
if min(TTIC ) TTICego else
359
(7)
where, p, v, and a denote subject vehicle’s distance on path coordinate, longitudinal velocity, and longitudinal acceleration input, respectively. Using the simple longitudinal dynamics, the set of constraints for MPC problem is defined as
X (0) X 0 amin u (k ) amax vmin v(k ) vmax
(6)
pmin p(k ) pmax
(8)
X (k 1) AX (k ) Bu (k )
Equation (6) means that if TTIC of the ego vehicle is the
1 dt 0 where A B 0 1 dt The first constraint is the most basic condition for MPC problem, which ensures the optimal solution of the MPC matches current vehicle state at initial state. The second and third one limit the longitudinal acceleration velocity in order to allow for smooth and comfortable manoeuvres within physical limit and obey the speed limit. The fourth one guarantees that vehicle remains within the safety. Different from other constrains, fourth one changes each step based on the results of the cross point prediction and cross priority decision. A plane of the predicted station and prediction time in Fig. 6 is divided in two region, upper region and lower region is correspond to cross first and later respectively. Each region is used as a fourth constraint in (8). The fifth one is the dynamic constraint of simple longitudinal model. The optimal longitudinal motion planning problem can be written as a standard QP. The states are considered as an
Fig. 6. Cross point and collision region estimation 359
2019 IFAC IAV 360 Gdansk, Poland, July 3-5, 2019
Yonghwan Jeong et al. / IFAC PapersOnLine 52-8 (2019) 356–361
error between initial guess and future state to make the QP as a convex optimization problem. Therefore, the cost function J is defined as
J
position. As shown in Fig. 7, the reference and proposed algorithm show the almost same velocity history. However, conventional algorithm over-estimates the collision and generates unnecessary deceleration which makes that the intersection cross time was delayed by more than 2 seconds. Therefore, appropriate intention inference algorithm is essential to cross the intersection human-like.
n
X (k )T QX (k ) Ru(k )2
(9)
k 1
where, ΔX(k) = Xguess(k) - X(k). Xguess(k) is the initial guess of the states of subject vehicle. In this paper, predicted position of the subject vehicle is used as an initial guess.
5. VEHICLE TEST RESULTS The proposed motion-planning algorithm is evaluated using the test vehicle as shown in Fig. 8. The test platform is based on Hyundai IONIQ Electric with six LiDAR, front camera, around view, and GPS/IMU. LabVIEW/MATLAB based software architecture and Micro-autobox II has been used to process the measurements of environment sensors and execute the proposed algorithm in real time. The design parameters for the longitudinal motion planning optimization problems are same with simulation but maximum velocity vmax is only changed to 30km/h.
4. SIMULATION RESULTS To verify the motivation of the proposed algorithm, three kinds of different algorithm has been compared. The same MPC based longitudinal motion planning algorithm is used to determine the desired acceleration and has been implemented as a QP optimization problem in CVXGEN interfaced with MATLAB (Mattingley and Boyd [2012]). However, different decision algorithms are used to verify the significance of the intention inference. First algorithm, which is called ‘reference algorithm’, assumes that future trajectory and intention of other vehicles are perfectly known. Next, the second one, ‘conventional algorithm’, does not estimate the intention of the other vehicles but assumes that all vehicles entering the intersection must cross at the intersection. Finally, the proposed target intention inference algorithm is used to infer the intention before entering the intersection.
Among the numerous scenarios of the uncontrolled intersection, we selected left turn across path scenario where subject vehicle encountered an oncoming vehicle in uncontrolled intersection, which is illustrated in Fig. 9. The trajectory history of subject vehicle and target vehicle is marked as blue and red car. The vehicle test results of the proposed algorithm is described in Fig. 10. The subject vehicle decelerated to pass the oncoming vehicle first and then turned left with enough clearance. The history of priority, subject acceleration, subject velocity, target intention, clearance, and target velocity are described in Fig. 10. In this study, if vehicle decide to pass the intersection first, the priority is 1 and if not, the priority is 2. Target intention 1, 2, and 3 means going straight, turning right, and left each. As shown in Fig. 10, target oncoming vehicle is detected 20 meters ahead at 18 seconds and tracked total 4 seconds duration. Among that duration, the target has a constant speed, because the driver of the target expected that the slower subject vehicle will yield the target. This procedure of the driver is appeared similarly in the test results. After detecting the target, SVM based predictor predict the target will go straight. Based on the inferred target intention and TTIC, the cross priority of subject is changed from 1 to 2 and decelerated mildly to pass the target first. All results were derived within constraints and the clearance of each vehicle is maintained above the safety margin.
The comparison of three algorithms is evaluated in simulated intersection crossing situations in which two vehicles enter the intersection facing each other and made a left turn. Which means that these two vehicles do not collide. The constraints for velocity and acceleration are set as nominal range of urban road conditions. vmin of 0 km/h, vmax of 50 km/h, amin of -3 m/s2, and amax of 1 m/s2, has been used as constraints. The results of simulation are described in Fig. 7. Reference, conventional, and proposed algorithm are coloured as red, green, and blue respectively. The trajectory of these vehicles are marked at 2-second intervals after departing from same
IBEO Lux LiDAR
Fig. 7. Simulation based effectiveness analysis of proposed algorithm for target intention inference algorithm
Around View Monitor
Ublox M8L GPS/IMU
Fig. 8. Configuration of Test Vehicle 360
Front Camera
2019 IFAC IAV Gdansk, Poland, July 3-5, 2019
Yonghwan Jeong et al. / IFAC PapersOnLine 52-8 (2019) 356–361
5. CONCLUSIONS
361
funded By the Ministry of Trade, Industry & Energy (MOTIE, Korea), and The Ministry of Land, Infrastructure, and Transport (MOLIT, KOREA) [Project ID: 18TLRPB146733-01, Project Name: Connected and Automated Public Transport Innovation (National R&D Project)].
The proposed target intention inference and longitudinal motion planning at uncontrolled intersection for autonomous vehicle has been developed as part of the urban autonomous driving technology. The target intention inference algorithm precisely predicts object vehicle’s intention such as going straight, and turning using SVM. Based on the inferred intention of target, future trajectory of target and cross point is predicted. After that, linear MPC based longitudinal motion planning algorithm find the optimal motion. The performance of the proposed algorithm is evaluated via simulation and vehicle test. This algorithm will be tested multi-target scenarios to validate the robust performance.
REFERENCES Aoude, G.S., Desaraju, V.R., Stephens, L.H. and How, J.P. (2012). Driver behavior classification at intersections and validation on large naturalistic data set. IEEE Transactions on Intelligent Transportation Systems, 13(2), 724-736. Bengler, K., Dietmayer, K., Farber, B., Maurer, M., Stiller, C., and Winner, H. (2014). Three decades of driver assistance systems: Review and future perspectives. IEEE Intelligent Transportation Systems Magazine, 6(4), 6-22. Bougler, B., Cody, D., and Nowakowski, C. (2008). California intersection decision support: a drivercentered approach to left-turn collision avoidance system design. UC Berkeley: California Partners for Advanced Transportation Technology. Retrieved from https://escholarship.org/uc/item/5nz512bt. chaandar Ravichandar, H., and Dani. A. (2015). Human intention inference through interacting multiple model filtering. In IEEE International Conference on Multisensor Fusion and Integration for Intelligent Systems (MFI), 220-225. Chen, C., Rickert, M. and Knoll, A. (2016). Combining task and motion planning for intersection assistance systems. In IEEE Intelligent Vehicles Symposium (IV), 1242-1247. Hafner, M.R., Cunningham, D., Caminiti, L. and Del Vecchio, D. (2011). Automated vehicle-to-vehicle collision avoidance at intersections. In Proceedings of world congress on intelligent transport systems. Kumagai, T., Sakaguchi, Y., Okuwa, M. and Akamatsu, M. (2003). Prediction of driving behavior through probabilistic inference. In Proc. 8th Intl. Conf. Engineering Applications of Neural Networks, 117-123. Lefèvre, S., Laugier, C. and Ibañez-Guzmán, J. (2011). Exploiting map information for driver intention estimation at road intersections. In IEEE Intelligent Vehicles Symposium (IV), 583-588. Liebner, M., Baumann, M., Klanner, F. and Stiller, C. (2012). Driver intent inference at urban intersections using the intelligent driver model. In IEEE Intelligent Vehicles Symposium (IV), 1162-1167. Mattingley, J. and Boyd, S. (2012). CVXGEN: A code generator for embedded convex optimization. Optimization and Engineering, 13(1), 1-27. Nilsson, J., Brännström, M., Fredriksson, J. and Coelingh, E. (2016). Longitudinal and lateral control for automated yielding maneuvers. IEEE Transactions on Intelligent Transportation Systems, 17(5), 1404-1414. Rajamani, R. (2011). Vehicle dynamics and control. Springer Science & Business Media. Schildbach, G., Soppert, M. and Borrelli, F. (2016). A collision avoidance system at intersections using robust model predictive control. In IEEE Intelligent Vehicles Symposium (IV), 233-238.
ACKNOWLEDGMENT This work was supported by Hyundai Motor Company and the Technology Innovation Program (10079730, Development and Evaluation of Automated Driving Systems for Motorway and City Road and driving environment)
Fig. 9. Trajectory history of subject and target vehicle
Fig. 10. Summary of vehicle test results 361
ScienceDirect
IFAC PapersOnLine 52-8 (2019) 356–361
SVM based Intention Inference and Motion Planning SVM based Intention Inference and Motion Planning Inference at Uncontrolled Intersection SVM based Intention and Motion Planning at Uncontrolled Intersection at Uncontrolled Intersection Yonghwan Jeong*, Kyongsu Yi**, and Sungmin Park*** Yonghwan Jeong*, Yonghwan Jeong*, Kyongsu Kyongsu Yi**, and and Sungmin Sungmin Park*** Park*** Yi**, Yonghwan Jeong*, Kyongsu Yi**, and Sungmin Park*** *Mechanical Engineering Department, Seoul National University *Mechanical Engineering Department, Seoul *Mechanical Engineering Department, Seoul National National University University Seoul, Korea,(e-mail: [email protected]) *Mechanical Engineering Department, Seoul National University Seoul, Korea,(e-mail: [email protected]) **Mechanical Engineering Department, Seoul National University Seoul, Korea,(e-mail: [email protected]) **Mechanical Department, Seoul **Mechanical Engineering Engineering Department, Seoul National National University University Seoul, Korea,(e-mail: [email protected]) **Mechanical Engineering Department, Seoul National University Seoul, Korea,(e-mail: [email protected]) Seoul, Center, Korea,(e-mail: [email protected]) ***R&D Hyundai Motor Company Seoul, Center, Korea,(e-mail: [email protected]) ***R&D Motor Company ***R&D Korea,(e-mail: Center, Hyundai Hwaseong, [email protected]) ***R&D Center, Hyundai Motor Company Hwaseong, [email protected]) Korea,(e-mail: Hwaseong, [email protected]) Hwaseong, Korea,(e-mail: [email protected])
Abstract: This paper presents a support vector machine (SVM) based intention inference and motion Abstract: This aa support machine (SVM) based inference and motion Abstract: This paper paper presents support vector machine (SVM)intersection. based intention intention inference andvehicles motion planning algorithm for presents autonomous drivingvector through uncontrolled Intention of target Abstract: This paper presents a support vector machine (SVM)intersection. based intention inference andvehicles motion planning algorithm for autonomous driving through uncontrolled Intention of target planning algorithm for autonomous driving through uncontrolled intersection. Intention of target vehicles is inferred using SVM with intersection map to predict the future state of targets. A cross point, which planning algorithm for autonomous driving through uncontrolled intersection. Intention of target vehicles is using SVM intersection map the future of A which is inferred inferred usingcollision SVM with with intersection map to to predict predict thepredicted future state state of targets. targets. A cross cross point, point, which has a highest probability, is estimated using target state considering prediction is inferred usingcollision SVM with intersection map to predict thepredicted future state of targets. A cross point, which has a highest collision probability, is estimated using predicted target state considering prediction has a highest probability, is estimated using target state considering prediction uncertainty. Longitudinal acceleration is determined using model predictive control approach considering has a highest collision probability, is estimated using predicted target state considering prediction uncertainty. Longitudinal acceleration is determined determined using model predictive predictive controland approach considering uncertainty. is model control approach the predictedLongitudinal cross point.acceleration The proposed algorithm using is validated via simulation vehicleconsidering tests. The uncertainty. Longitudinal acceleration is determined using model predictive controland approach considering the predicted cross point. The proposed algorithm is validated via simulation and vehicle tests. The The the predicted cross point. The proposed algorithm is validated via simulation vehicle tests. results show the accurate intention inference and human-like motion planning at uncontrolled intersection the predicted cross point.intention The proposed algorithm is validated viaplanning simulation and vehicleintersection tests. The results show the accurate inference and human-like motion at uncontrolled results show the accurate intention inference and human-like motion planning at uncontrolled intersection scenarios. results show the accurate intention inference and human-like motion planning at uncontrolled intersection scenarios. scenarios. scenarios. © 2019, IFAC (Internationalvehicles, Federation of Automatic Control) Hosting by Elsevier Ltd. All rights reserved. Keywords: Autonomous Machine learning, Support Vector Machine, Intention Inference, Keywords: Autonomous vehicles, Machine learning, Vector Keywords: Autonomous vehicles, Control. MachineUncontrolled learning, Support Support Vector Machine, Machine, Intention Intention Inference, Inference, Motion Planning, Model Predictive Intersection. Keywords: Autonomous Predictive vehicles, MachineUncontrolled learning, Support Vector Machine, Intention Inference, Motion Motion Planning, Planning, Model Model Predictive Control. Control. Uncontrolled Intersection. Intersection. Motion Planning, Model Predictive Control. Uncontrolled Intersection. 1. INTRODUCTION 1. 1. INTRODUCTION INTRODUCTION INTRODUCTION For the last decade, 1.lots of research has been carried out for of for research has been beendriving carried out for for For the last decade, lots of research has carriedsuch out each element technology autonomous as For the last decade, lots of for research has beendriving carriedsuch out for each element technology autonomous as perception, decision, and control (Bengler et al. [2014]). each elementdecision, technology for autonomous driving such as decision, and control (Bengler et being al. [2014]). [2014]). perception, control (Bengler al. Some achievement ofand these studies are nowet massperception, decision, and control (Bengler et al. [2014]). Some achievement of these studies are now beingdriving massproduced in the name of partially autonomous Some achievement of these studies are now beingdriving massproduced in of autonomous produced in the the name name of partially partially autonomous systems (PADS). places, where accidentsdriving occur produced in the However, name ofthe partially autonomous driving However, the places, where accidents occur systems (PADS). theand places, where accidents occur frequently such asHowever, crosswalks intersections of the urban systems (PADS). However, the places, where accidents occur frequently such as crosswalks and intersections of the urban frequently such asnot crosswalks and intersections of the driver urban environment, are addressed by the commercialized frequently such as crosswalks and intersections of the urban environment, are addressed by driver environment, are not not addressed by the the commercialized commercialized driver assistance system. Especially uncontrolled intersection is one environment, are not addressed by the commercialized driver assistance system. Especially uncontrolled intersection is one assistance system. Especially uncontrolled intersection is one of the most dangerous place in urban environment because assistance system. Especially uncontrolled intersectionbecause is one of the most dangerous place in urban environment because of the most dangerous place in urban environment there ismost no traffic signals to controlenvironment and guidebecause traffic of the is place in there no dangerous traffic signals to urban controlcontinuous and guideresearch traffic participants. To solve these problems, there is no To traffic to controlcontinuous and guideresearch traffic participants. solvesignals these problems, has been conducted autonomous in anresearch urban participants. To solvefor these problems, driving continuous has been conducted for autonomous driving in an has been conducted for autonomous an urban urban environment to increase safety of alldriving traffic in participants has been conducted for autonomous driving in an urban environment to [2008]). increase In safety of studies, all traffic participants (Bougler et al. these two issues are environment to increase safety of all traffic participants (Bougler et for al. [2008]). In these studies,attwouncontrolled issues are addressed autonomous driving (Bougler et for al. [2008]). In these studies,attwouncontrolled issues are addressed driving intersections. The autonomous first issue is inferring theatintention of the addressed for autonomous driving uncontrolled intersections. The whether first issuethe is inferring the intention of the target vehicles other vehicle is making intersections. The first issuethe is inferring the intention of the target vehicles other The vehicle is issue making concessions and whether where to the proceed. second is target vehicles whether other vehicle is making concessions and towhere to subject proceed.vehicle The second issue is motion planning plan the behavior so that concessions and towhere to subject proceed.vehicle The second issue is motion planning plan the behavior so that the vehicle can pass through the uncontrolled intersection motion planning to plan the subject vehicle behavior so that the vehicle can pass through the uncontrolled intersection the vehicle can pass uncontrolled intersection over the minimal riskthrough of thethecollision according to the the vehicle can pass uncontrolled intersection over the ofminimal riskthrough of thethe collision according to the intention other vehicles. over the ofminimal risk of the collision according to the intention other vehicles. intention of other vehicles. First, a various approaches for intention inference have been First, a out various approaches intention inference have been carried in the literature for from model-based approaches to First, a out various approaches for intention inference have been carried in the literature from model-based approaches to machine learning based approaches. For model-based carried outlearning in the literature from model-based to machine approaches. For approaches model-based approach, Interactingbased Multiple Model (IMM) filter based machine learning based approaches. For model-based approach, Interacting Model (IMM) based intention inference hasMultiple been used widely from filter aviation to approach, inference Interacting Model (IMM) based intention hasMultiple been used widely from filter aviation to intention inference has been used widely from aviation to
motor industry. Toledo-Moreo and Zamora-Izquierdo [2009] motor industry. Toledo-Moreo andalgorithm Zamora-Izquierdo [2009] proposed the intention inference for lane change motor industry. Toledo-Moreo andalgorithm Zamora-Izquierdo [2009] proposed the intention inference for lane change proposed the intention inference algorithm for lane change prediction the by using GPS/IMU measurement IMM filter. proposed intention inference algorithm based for lane change prediction by using GPS/IMU measurement based IMM filter. prediction by Liebner using GPS/IMU measurement IMMdriver filter. Furthermore, et al. [2012] used the based intelligent prediction by using GPS/IMU measurement based IMM filter. Furthermore, Liebner et al. [2012] used the intelligent driver model to infer the subject vehicle driver's intention to utilize Furthermore, Liebner et al. [2012] used the intelligent driver model to infer the subject vehicle intention to utilize for intersection assist system. Fordriver's machine learning based model to infer the subject vehicle driver's intention to utilize for intersection assist system. For machine learning based approach, S. Lefèvre et al. [2011] proposed the Dynamic for intersection assist et system. For machine learning based approach, S. Lefèvre al. [2011] Dynamic Bayesian Network (DBN) basedproposed driver's the manoeuvre approach, S. Lefèvre et al. [2011] proposed the Dynamic Bayesian Network considering (DBN) based driver's observation. manoeuvre inference algorithm probabilistic Bayesian algorithm Network considering (DBN) based driver's observation. manoeuvre inference probabilistic Kumagai et al. [2003] used simple DBN to observation. predict the inference algorithm considering probabilistic Kumagai et al. [2003] used simple DBN to predict the Kumagai etto al. simple DBNbased to predict the probability stop[2003] at stopused line. A classifier on Support Kumagai etto al. [2003] used simple DBNbased to predict the probability stop at stop line. A classifier on Support Vector Machine (SVM) and Hidden Markov Model (HMM) probability to stop at stop line. A classifier based on Support Vector Machine and Hidden Markov Model (HMM) to classify driver(SVM) behaviours at intersections proposed by Vector Machine (SVM) and Hidden Markov Model (HMM) to classify driver behaviours at intersections proposed by Aoude et al. [2012]. In recent years, communication based to classify behaviours atyears, intersections proposed by Aoude et al.driver [2012]. In recent communication based approaches base been attempted to develop collision Aoude et al. [2012]. In recent years, communication based approaches been attempted to the develop collision avoidance atbase intersection to share safety-related approaches at baseintersection been attempted to the develop collision avoidance to share safety-related information using V2V by Hafner et al. [2011]. avoidance at intersection to share the safety-related information using V2V by Hafner et al. [2011]. information using V2V by Hafner et al. [2011]. Several approaches have been developed to deal with the Several approaches have been developed to with Several approaches havethe been developeddesired to deal dealmotion with the the second issue, planning appropriate to Several approaches have been developed to dealmotion with the second issue, planning the appropriate desired to avoid collision and pass intersection quickly. The hierarchical secondcollision issue, and planning the appropriate desired motion to avoid passwhich intersection quickly. The and hierarchical planning framework, combining task motion avoid collision and pass intersection quickly. The hierarchical planning framework, which combining task and motion planning, generates trajectories in several intersection planning framework, which combining task and motion planning, generates trajectories in several intersection scenarios (Chen et al. [2016]). However, this study deal with planning, generates trajectories in several intersection scenarios (Chen et al. [2016]). However, this studyalldeal with uncertainties of the target vehicles by considering possible scenarios (Chen et al. [2016]). However, this study deal with uncertainties of the target vehicles by considering all possible situations notof predict the intention of the target. Schildbach et uncertainties the target vehicles by considering all possible situations intention the target. et al. [2016] not usespredict robustthe MPC to findof safe gaps inSchildbach the crossing situations not predict the intention of the target. Schildbach et al. uses to in crossing al. [2016] [2016] uses robust robust MPC to find find safe gaps in the the crossing traffic concerning the MPC actuator limitsafe andgaps safety constraints. al. [2016] uses robust MPC to find safe gaps in the crossing traffic concerning limitto and safety constraints. Similar approach the areactuator applied automated yielding traffic concerning theare actuator limitto and safety constraints. Similar approach applied automated yielding manoeuvres such as lane change, intersection, and Similar approach are appliedchange, to automated yielding manoeuvres such et asal. lane intersection, and roundabout (Nilsson [2016]). manoeuvres(Nilsson such etasal. lane change, intersection, and roundabout [2016]). roundabout (Nilsson et al. [2016]).
2405-8963 © 2019, IFAC (International Federation of Automatic Control) Hosting by Elsevier Ltd. All rights reserved. Copyright 2019 responsibility IFAC Peer review©under of International Federation of Automatic Control. Copyright © 2019 2019 IFAC IFAC Copyright © 10.1016/j.ifacol.2019.08.113 Copyright © 2019 IFAC
2019 IFAC IAV Gdansk, Poland, July 3-5, 2019
Yonghwan Jeong et al. / IFAC PapersOnLine 52-8 (2019) 356–361
357
This paper focused on designing a SVM based intention inference and motion-planning algorithm at uncontrolled intersection. The proposed algorithm consist of target intention inference and longitudinal motion planning as shown in Fig. 1. SVM classifier with intersection map is used to infer the intention of the targets before entering the intersection only using longitudinal motion of target vehicles measured by LiDAR sensors. Based on the inferred intention of targets, longitudinal motion of the subject vehicle is determined using MPC approach. In this stage, we predict the future motion of vehicles and estimate the cross point. Then, we optimize the motion of subject vehicle to secure the safety clearance while preventing the unnecessary deceleration. The simulation has been conducted to show the effectiveness of the proposed algorithm. The proposed algorithm implemented to the test vehicle and validated via vehicle tests. Fig. 2. Example result of intersection map building 2. SVM BASED TARGET INTENTION INFERENCE 2.2 Support Vector Machine Training
Target intention inference is classification problem of human driving behavior using limited information. Among various classification techniques, SVM is appropriate to classify driving behavior at uncontrolled intersection, because the lateral motion of intersection approaching vehicle can be classified as discrete intentions such as going straight and turning. In addition, since the direction that can be taken in each lane is specified by traffic rules, the classification problem can be simplified as binary classification.
Collecting learning data is a crucial step for machine learning based approach. The training data can be obtained in a various methods such as computer simulation, hardware in the loop simulation (HILS), and driving data in real traffic. Among the methods for data generation, computer simulation with random parameter has been used to reduce the effort to acquiring training data. When generating the data set for training SVM classifier, we assumed that the driver try to limit the lateral acceleration while turning manoeuvre by decelerating before entering the intersection. In order to reflect the various characteristics of driver at intersection approach, we applied the normal distribution based simulation parameters to generate the realistic training data. Initial speed vini, maximum velocity vmax, maximum lateral acceleration alat,max are randomly generated and defined as follows:
2.1 Intersection Map Building As mentioned before, structured intersection map is required to simplify classification problem by providing available manoeuvres at each lanes. In this study, intersection map for target intention inference only includes the location of lane, enter/exit node, and connection relationships between nodes. The constructed intersection map is depicted in Fig. 2. As shown in Fig.2, centerline of the road and lane makers are marked as yellow solid line and black dotted line, respectively. Enter and exit node of intersection are marked as red and blue circle with index number respectively. Connection relationship between each nodes is defined as connectivity matrix Ccon. If i-th enter node and j-th exit node is connected, (i,j) component of is defined as 0, 1, 2, 3 for non-connected, straight, left turn, and right turn case respectively. Example connection relationships of entering node 1 and 2 are illustrated in Fig.2 as magenta and cyan solid lines.
Moving Obs. Detection
Localization
Route/Task Planning Intersection Map
Target State Global Position Target Task
Target Intention Inference - SVM Training (offline process) - SVM based Target Intention Inference (online process)
Longitudinal Motion Planning - Vehicle Motion Prediction - Cross Point Region Estimation - MPC based Velocity Planning
Desired Ax
vmax 50km / h N 0, (7.5km / h) 2 vini 50km / h N 0, (10km / h) 2 alat ,max 2m / s 2 N 0, (0.25m / s 2 ) 2 3m / s ades 1.5m / s 2
(1)
2
Total 300 sets of data is generated for straight, left turn, and right turn respectively. For lateral controller, a steering controller embedded in Carsim is used to tracking the path, which is defined in intersection map. Training data set for SVM is generated as shown in Fig. 3.
Vehicle
Desired SWA
Lane Info
Fig. 1. Overall architecture of the autonomous driving algorithm for uncontrolled intersection
Fig. 3. Training data set for SVM algorithm 357
2019 IFAC IAV 358 Gdansk, Poland, July 3-5, 2019
Yonghwan Jeong et al. / IFAC PapersOnLine 52-8 (2019) 356–361
3. MOTION PLANNING AT UNCONTROLLED INTERSECTION
Therefore, a simple path tracking controller based on Ackermann steering angle is used to determine a steering angle input in Rajamani [2011]. Longitudinal motion of the target vehicles entering intersection are modelled under several assumptions. Drivers, who want to cross the intersection straight, normally control the vehicle to adhere to speed limit. Other drivers, who want to left or right turn, generally decelerate the vehicle to lower lateral acceleration for ride comfort. Under there assumptions, desired velocity can be determined as
In this section, intersection map, intention, position, and longitudinal velocity of target vehicle are used to plan the desired motion at uncontrolled intersection. The intersection map is the same one, which is defined for target intention inference at section 2, and the intention is the result from the SVM based target intention inference algorithm. The position and velocity of the target vehicles are acquired form LiDAR, which is easily estimative values. This state definition is necessary to increase the easiness of implementation to test platform. To minimize the effect of the perception algorithm to motion planning, variables difficult to estimate are excluded in this study. The algorithmic flow of the proposed motion-planning algorithm part is illustrated in Fig. 4.
vdes (k ) max vmax
Based on the information from the perception and intention inference algorithm, cross point between subject and target vehicles has been predicted. In this paper, ‘cross point’ means a place and timing which has a highest collision probability. Therefore, cross point is defined as a region, not a single point. To predict the cross point, future motion of the subject and target vehicles should be predicted upon consideration of the sensor and prediction uncertainty. First, to predict the state of the vehicles, the particle model has been used as a vehicle model. The vehicle model for state prediction is expressed in discrete from as
An example of the prediction results of subject and single target vehicle case is illustrated in Fig 5. The sampling time of the prediction is 0.1s and the moment where the end of prediction is assigned when vehicle exit the intersection. As can be seen in Fig 5, the future trajectory of the subject and target vehicle are represented as a blue and red each marked at intervals of 1 second. For easy analysis, all prediction uncertainty are integrated in the predicted states of target. Target vehicle, which is indicated as a red car, has a two shadows at each step. These two shadows shows the ±3σ position of each step which indicates that the probability of the target vehicle exist between two shadows is almost 99%. Therefore, cross point can be predicted by monitoring an overlapping between predicted trajectory of subject vehicle and region can be determined by uncertainty zone. In this example case, the nearest moment between vehicles is described as a green car in Fig 5.
px (k 1) px (k ) v(k ) cos( (k ))dt p y (k 1) p y (k ) v(k ) sin( (k ))dt
(2)
v(k ) tan( (k ))dt L v(k 1) v(k ) a(k )dt
where px, py, ψ and vx denotes a longitudinal, lateral position, yaw angle and velocity of the vehicle. δ, L and ax denotes steering angle, wheelbase and longitudinal acceleration to vehicle model. dt is sampling time of the prediction.
After the center of cross point is defined, a size of region should be determined using propagated uncertainty of target vehicle position. In Fig. 6, clearance between subject vehicle and mean value of the predicted target vehicle is illustrated as blue line, and clearance between subject vehicle and ±3σ position is illustrated as red line. The region of the cross point
The input of the vehicle model should be properly determined to propagate the state of the vehicle with an appropriate uncertainty. In this paper, the intention of the target vehicles are pre-defined using the SVM based intention inference algorithm, which means that the future trajectory of targets are already estimated based on the trajectory information of the intersection map with inferred intention. Cross Point Prediction
Motion Planning
Target State
Target Intention
Subject Vehicle State Prediction
Desired Path
Rough Map
Target Vehicle State Prediction
Desired Acceleration Cross Point (Collision Region) Estimation
(3)
where, σv is the variance of uncertainty of the desired velocity of the vehicle. If we assume that the driver has a nominal drive pattern, vmax and alat,norm can be defined. First, 50 km/h, which is an urban road speed limit in South Korea, is used as vmax. For alat,norm, 1m/s2 is used as a nominal lateral acceleration. σv is assumed as a normal distribution with standard deviation 0.25m/s at each step when we use a sampling time as 0.1s. This uncertainty has been propagated using process update of Kalman filter.
3.1 Cross Point Prediction
(k 1) (k )
alat , norm v
Longitudinal Motion Optimization
Fig. 5. Example of vehicle state prediction and cross point estimation
Fig. 4. Block diagram for longitudinal motion planning part 358
2019 IFAC IAV Gdansk, Poland, July 3-5, 2019
Yonghwan Jeong et al. / IFAC PapersOnLine 52-8 (2019) 356–361
can be determined based on predicted clearance as follows:
scross
minimum among the all vehicles entering the intersection, ego vehicle accelerate to cross the intersection first by planning a desired trajectory above cross point. In other cases, ego vehicle decelerate until crossing priority of ego vehicle become first.
x target (tcp ) x target (tcp )
tcross
scross vego (tcp )
(4)
3.3 Optimal Longitudinal Motion Planning
where, tcp is the nearest moment between subject and target vehicle. x+target and x-target respectively denotes a +3σ and -3σ clearance. In addition, vego is the predicted velocity profile of the subject vehicle. Using this predicted information, a size of cross point region can be determined as equation (4).
The optimal longitudinal motion planning of the subject vehicle is designed using model predictive control (MPC) method. The MPC problem is designed based on the predicted state of vehicles and cross point. The region of cross point and crossing priority are used to determine position constraint, which must be satisfied to avoid collision with participants of intersection. Then, vehicle dynamics and velocity, acceleration constraint are also considered to satisfy the guarantee the comfort.
3.2 Crossing Priority Decision In previous section, the cross point has been defined based on ego and target vehicle state prediction. To avoid collision, ego vehicle should not violate the region of the cross point. In other words, it is necessary to determine whether to accelerate or decelerate the ego vehicle. In this paper, time to intersection cross (TTIC) is defined to determine crossing priority. TTIC means the estimate time to cross the intersection. TTIC is determined as follows: n 1
TTIC (t )
d remain (k 1) d remain (k )
k 1 0.5 (v pred ( k 1) v pred ( k ))
Since the purpose of the longitudinal motion-planning algorithm for uncontrolled intersection is to determine desired velocity for collision avoidance at intersection, only longitudinal dynamics of equation (2) is considered in simple form. State and input for longitudinal dynamics is defined as
state : X p v input :u a
(5)
where, dremain and vpred denotes the remaining distance to exit of the intersection and predicted velocity profile, respectively. Using the equation (5) with dremain and vpred of the ego and nominal value of predicted target vehicle state, TTIC of ego and target vehicle can be determined. After that, crossing priority is decided using the TTIC of all participant vehicles entering the intersection. The criteria of crossing priority decision is summarized
cross first cross later
if min(TTIC ) TTICego else
359
(7)
where, p, v, and a denote subject vehicle’s distance on path coordinate, longitudinal velocity, and longitudinal acceleration input, respectively. Using the simple longitudinal dynamics, the set of constraints for MPC problem is defined as
X (0) X 0 amin u (k ) amax vmin v(k ) vmax
(6)
pmin p(k ) pmax
(8)
X (k 1) AX (k ) Bu (k )
Equation (6) means that if TTIC of the ego vehicle is the
1 dt 0 where A B 0 1 dt The first constraint is the most basic condition for MPC problem, which ensures the optimal solution of the MPC matches current vehicle state at initial state. The second and third one limit the longitudinal acceleration velocity in order to allow for smooth and comfortable manoeuvres within physical limit and obey the speed limit. The fourth one guarantees that vehicle remains within the safety. Different from other constrains, fourth one changes each step based on the results of the cross point prediction and cross priority decision. A plane of the predicted station and prediction time in Fig. 6 is divided in two region, upper region and lower region is correspond to cross first and later respectively. Each region is used as a fourth constraint in (8). The fifth one is the dynamic constraint of simple longitudinal model. The optimal longitudinal motion planning problem can be written as a standard QP. The states are considered as an
Fig. 6. Cross point and collision region estimation 359
2019 IFAC IAV 360 Gdansk, Poland, July 3-5, 2019
Yonghwan Jeong et al. / IFAC PapersOnLine 52-8 (2019) 356–361
error between initial guess and future state to make the QP as a convex optimization problem. Therefore, the cost function J is defined as
J
position. As shown in Fig. 7, the reference and proposed algorithm show the almost same velocity history. However, conventional algorithm over-estimates the collision and generates unnecessary deceleration which makes that the intersection cross time was delayed by more than 2 seconds. Therefore, appropriate intention inference algorithm is essential to cross the intersection human-like.
n
X (k )T QX (k ) Ru(k )2
(9)
k 1
where, ΔX(k) = Xguess(k) - X(k). Xguess(k) is the initial guess of the states of subject vehicle. In this paper, predicted position of the subject vehicle is used as an initial guess.
5. VEHICLE TEST RESULTS The proposed motion-planning algorithm is evaluated using the test vehicle as shown in Fig. 8. The test platform is based on Hyundai IONIQ Electric with six LiDAR, front camera, around view, and GPS/IMU. LabVIEW/MATLAB based software architecture and Micro-autobox II has been used to process the measurements of environment sensors and execute the proposed algorithm in real time. The design parameters for the longitudinal motion planning optimization problems are same with simulation but maximum velocity vmax is only changed to 30km/h.
4. SIMULATION RESULTS To verify the motivation of the proposed algorithm, three kinds of different algorithm has been compared. The same MPC based longitudinal motion planning algorithm is used to determine the desired acceleration and has been implemented as a QP optimization problem in CVXGEN interfaced with MATLAB (Mattingley and Boyd [2012]). However, different decision algorithms are used to verify the significance of the intention inference. First algorithm, which is called ‘reference algorithm’, assumes that future trajectory and intention of other vehicles are perfectly known. Next, the second one, ‘conventional algorithm’, does not estimate the intention of the other vehicles but assumes that all vehicles entering the intersection must cross at the intersection. Finally, the proposed target intention inference algorithm is used to infer the intention before entering the intersection.
Among the numerous scenarios of the uncontrolled intersection, we selected left turn across path scenario where subject vehicle encountered an oncoming vehicle in uncontrolled intersection, which is illustrated in Fig. 9. The trajectory history of subject vehicle and target vehicle is marked as blue and red car. The vehicle test results of the proposed algorithm is described in Fig. 10. The subject vehicle decelerated to pass the oncoming vehicle first and then turned left with enough clearance. The history of priority, subject acceleration, subject velocity, target intention, clearance, and target velocity are described in Fig. 10. In this study, if vehicle decide to pass the intersection first, the priority is 1 and if not, the priority is 2. Target intention 1, 2, and 3 means going straight, turning right, and left each. As shown in Fig. 10, target oncoming vehicle is detected 20 meters ahead at 18 seconds and tracked total 4 seconds duration. Among that duration, the target has a constant speed, because the driver of the target expected that the slower subject vehicle will yield the target. This procedure of the driver is appeared similarly in the test results. After detecting the target, SVM based predictor predict the target will go straight. Based on the inferred target intention and TTIC, the cross priority of subject is changed from 1 to 2 and decelerated mildly to pass the target first. All results were derived within constraints and the clearance of each vehicle is maintained above the safety margin.
The comparison of three algorithms is evaluated in simulated intersection crossing situations in which two vehicles enter the intersection facing each other and made a left turn. Which means that these two vehicles do not collide. The constraints for velocity and acceleration are set as nominal range of urban road conditions. vmin of 0 km/h, vmax of 50 km/h, amin of -3 m/s2, and amax of 1 m/s2, has been used as constraints. The results of simulation are described in Fig. 7. Reference, conventional, and proposed algorithm are coloured as red, green, and blue respectively. The trajectory of these vehicles are marked at 2-second intervals after departing from same
IBEO Lux LiDAR
Fig. 7. Simulation based effectiveness analysis of proposed algorithm for target intention inference algorithm
Around View Monitor
Ublox M8L GPS/IMU
Fig. 8. Configuration of Test Vehicle 360
Front Camera
2019 IFAC IAV Gdansk, Poland, July 3-5, 2019
Yonghwan Jeong et al. / IFAC PapersOnLine 52-8 (2019) 356–361
5. CONCLUSIONS
361
funded By the Ministry of Trade, Industry & Energy (MOTIE, Korea), and The Ministry of Land, Infrastructure, and Transport (MOLIT, KOREA) [Project ID: 18TLRPB146733-01, Project Name: Connected and Automated Public Transport Innovation (National R&D Project)].
The proposed target intention inference and longitudinal motion planning at uncontrolled intersection for autonomous vehicle has been developed as part of the urban autonomous driving technology. The target intention inference algorithm precisely predicts object vehicle’s intention such as going straight, and turning using SVM. Based on the inferred intention of target, future trajectory of target and cross point is predicted. After that, linear MPC based longitudinal motion planning algorithm find the optimal motion. The performance of the proposed algorithm is evaluated via simulation and vehicle test. This algorithm will be tested multi-target scenarios to validate the robust performance.
REFERENCES Aoude, G.S., Desaraju, V.R., Stephens, L.H. and How, J.P. (2012). Driver behavior classification at intersections and validation on large naturalistic data set. IEEE Transactions on Intelligent Transportation Systems, 13(2), 724-736. Bengler, K., Dietmayer, K., Farber, B., Maurer, M., Stiller, C., and Winner, H. (2014). Three decades of driver assistance systems: Review and future perspectives. IEEE Intelligent Transportation Systems Magazine, 6(4), 6-22. Bougler, B., Cody, D., and Nowakowski, C. (2008). California intersection decision support: a drivercentered approach to left-turn collision avoidance system design. UC Berkeley: California Partners for Advanced Transportation Technology. Retrieved from https://escholarship.org/uc/item/5nz512bt. chaandar Ravichandar, H., and Dani. A. (2015). Human intention inference through interacting multiple model filtering. In IEEE International Conference on Multisensor Fusion and Integration for Intelligent Systems (MFI), 220-225. Chen, C., Rickert, M. and Knoll, A. (2016). Combining task and motion planning for intersection assistance systems. In IEEE Intelligent Vehicles Symposium (IV), 1242-1247. Hafner, M.R., Cunningham, D., Caminiti, L. and Del Vecchio, D. (2011). Automated vehicle-to-vehicle collision avoidance at intersections. In Proceedings of world congress on intelligent transport systems. Kumagai, T., Sakaguchi, Y., Okuwa, M. and Akamatsu, M. (2003). Prediction of driving behavior through probabilistic inference. In Proc. 8th Intl. Conf. Engineering Applications of Neural Networks, 117-123. Lefèvre, S., Laugier, C. and Ibañez-Guzmán, J. (2011). Exploiting map information for driver intention estimation at road intersections. In IEEE Intelligent Vehicles Symposium (IV), 583-588. Liebner, M., Baumann, M., Klanner, F. and Stiller, C. (2012). Driver intent inference at urban intersections using the intelligent driver model. In IEEE Intelligent Vehicles Symposium (IV), 1162-1167. Mattingley, J. and Boyd, S. (2012). CVXGEN: A code generator for embedded convex optimization. Optimization and Engineering, 13(1), 1-27. Nilsson, J., Brännström, M., Fredriksson, J. and Coelingh, E. (2016). Longitudinal and lateral control for automated yielding maneuvers. IEEE Transactions on Intelligent Transportation Systems, 17(5), 1404-1414. Rajamani, R. (2011). Vehicle dynamics and control. Springer Science & Business Media. Schildbach, G., Soppert, M. and Borrelli, F. (2016). A collision avoidance system at intersections using robust model predictive control. In IEEE Intelligent Vehicles Symposium (IV), 233-238.
ACKNOWLEDGMENT This work was supported by Hyundai Motor Company and the Technology Innovation Program (10079730, Development and Evaluation of Automated Driving Systems for Motorway and City Road and driving environment)
Fig. 9. Trajectory history of subject and target vehicle
Fig. 10. Summary of vehicle test results 361