1 INTRODUCTION 2 PROBLEM DEFINITION

Transcription

1 Autonomous cruise control with cut-in target vehicle detection Ashwin Carvalho, Alek Williams, Stéphanie Lefèvre & Francesco Borrelli Department of Mechanical Engineering University of California Berkeley, CA, USA. ABSTRACT: This paper presents a longitudinal control algorithm for autonomous vehicles on highways. The focus is on identifying target vehicles in the scene which are relevant from a control perspective. These not only include vehicles in the ego vehicle s lane, but also those in neighboring lanes that are likely to cut in. A learning-based framework is used to estimate the lane change probability of such vehicles and identify relevant targets. The ability of the proposed approach to detect target vehicle cut-ins early and yield smoother control actions is demonstrated using data collected from a passenger vehicle in real traffic on a highway. 1 INTRODUCTION Autonomous or Adaptive Cruise Control (ACC) systems are present in many commercial vehicles today. By controlling the speed of the ego vehicle to match the speed limit and adapting the speed to that of the car in front, these systems help prevent rear-end collisions and relieve the driver of a tedious task. Moreover, they can easily be combined with autonomous lane-keeping systems to provide full autonomy on highways. Typically, ACC systems focus on one target vehicle at a time, namely the one currently in front of the ego vehicle. In this paper, we propose an improved algorithm which can handle other vehicles cutting in front of the ego vehicle. 2 PROBLEM DEFINITION Commercial ACC systems use a combination of onboard radar and camera to identify a Primary Target (PT) - the car in front of the ego vehicle in its own lane. The relative distance and speed of the PT are then passed to the control algorithm, which determines an acceleration command for the ego vehicle such that a safe following distance is maintained. If a PT is not detected, the controller tracks a desired speed such as the road speed limit. A challenging scenario for current ACC systems is to deal with target vehicles in neighboring lanes cutting in to the lane of the ego vehicle. The authors in Müller et al. (2011) recognize the vehicle cutting in as the PT when the Time to Lane-Crossing (TLC) of the target vehicle is within a threshold. In Moon et al. (2010), a fuzzy logic approach is used to compute the probability of a target vehicle entering into the ego vehicle s lane. These approaches are rule-based and not robust to noisy measurements. Moreover, they assume the existence of a single PT. In the case of multiple target vehicles, existing approaches choose the most relevant one based on some heuristics. The contributions of this paper are as follows: Given the relative positions of surrounding vehicles from the sensor fusion system, and the road geometry from the camera, we propose a learning-based approach to estimate the probability of lane change for each vehicle in the scene. Target vehicles which are relevant for ACC (possibly more than one) are then identified in the ego vehicle s lane and adjacent lanes. We present a multi-target autonomous cruise controller. The reference acceleration is generated via a Learning-by-Demonstration (LbD) algorithm using the most relevant target vehicle (Lefèvre et al. 2016). The control strategy is based on robust Model Predictive Control (MPC), where the safety constraints are imposed for all relevant vehicles. This avoids the need for rules to decide the most safety-critical target vehicle. The effectiveness of the proposed approach is demonstrated using data collected from our test vehicle. The sensor fusion and control algorithms are capable of running in real-time on embedded rapid control prototyping platforms. A schematic of the system architecture is shown in Figure 1. The Sensor Fusion module combines the measurements from the sensors such as radar, camera, lidar and GPS/INS to provide estimates of

2 the open-source solver ECOS for solving the optimization problem (Domahidi et al. 2013). A fused measurement of each associated TV s states is obtained by a convex combination of the measurements from each sensor. Additionally, a Kalman filter is used to estimate the relative positions and velocities of the TVs with respect to the EV. Figure 1: System architecture (described in Section 2). This paper focuses on the integration of the Sensor Fusion, Relevant Target Identification and Controller modules for ACC. the relative positions and velocities of target vehicles (TVs) in front of the ego vehicle (EV). The Relevant Target Identification module computes lane change probabilities of the detected TVs and identifies relevant TVs for ACC. These are denoted as Relevant Targets (RTs) as opposed to a single PT in the case of conventional ACC systems. Based on the above information, the Controller computes the desired acceleration command for the Vehicle. The remainder of the paper is organized as follows. Section 3 discusses the sensor fusion approach. The identification of RTs and the control algorithm are presented in Sections 4 and 5, respectively. Experimental results are discussed in Section 6 and concluding remarks are made in Section 7. 3 SENSOR FUSION AND TRACKING The experimental vehicle used for this application is equipped with a radar, camera and lidar. Each sensor reports the relative positions and velocities of the TVs in front of the EV with respect to its body fixed coordinate frame. The main challenge of sensor fusion is data association, that is, deciding which measurements from the three sensors originate from the same TV. In our case, the individual sensors perform their own tracking, so we use a globally optimal probabilistic track-to-track association algorithm to determine which tracks pertain to the same realworld object. Based on the work of Deb et al. (1997), we formulate the problem as a variation of the 3-dimensional assignment problem. Measurements from each of the three sensor tracks are grouped into triples to represent the hypothesis that those measurements have a common origin. The output of the algorithm is the optimal assignment which minimizes a chosen cost function. The cost of a triple is defined to be the negative log of the ratio of the probability that all three measurements in the triple pertain to the same object to the probability that all three measurements are spurious. The cost function is then the sum of the costs of all possible triples. The resulting optimization problem is a Mixed Integer Linear Program (MILP) which can be solved online. We use 4 IDENTIFICATION OF RELEVANT TARGETS The main focus of this paper is to account for TVs cutting in to the path of the EV. This section presents a learning-based approach to infer the lane changing intent of vehicles in the neighboring lanes. Based on the work of Lefèvre et al. (2014), we model the lane change decision making process for a given TV as a Hidden Markov Model (HMM). We define the following variables: m t M = {LK, LCL, LCR} is the hidden mode or latent variable at time instant t (LK = lane keeping, LCL = lane change left, LCR = lane change right). z t is the observed variable at time instant t. In our case, z t = e yt, where e y is the lateral position of the TV with respect to the centerline of the nearest lane. The variable e y is estimated using a combination of the position of the TV relative to the EV obtained from the sensor fusion system and the road geometry information provided by the camera on the EV. Remark 1. Additional features such as the relative orientation of the TV with respect to its lane can be used to improve the performance of the lane change intention estimation. The joint probability distribution of the modes m 0:t = {m 0,..., m t } and the observations z 1:t = {z 1,..., z t } is given by P (m 0:t,z 1:t ) = P (m 0 ) t P (m k m k 1 )P (z k m k ), k=1 (1) where the emission probability density function P (z k m k ) is modeled as a Gaussian distribution. The parameters which characterize the prior and transition probability mass functions P (m 0 ) and P (m k m k 1 ), respectively, and the means and covariances of the Gaussian emission density function are learned from data collected from our test vehicle using the Expectation-Maximization (EM) algorithm and the Bayesian Information Criterion (Lefèvre et al. 2014). During online operation, at time t, inference on the HMM gives us a probability distribution over the hidden mode m t for each TV conditioned on the history

3 of observations z 1:t. That is, P (m t = i z 1:t ) can be recursively computed as P (m t = i z 1:t ) P (z t m t = i) P (m t 1 = j z 1:t 1 )P (m t = i m t 1 = j), (2) j M initialized with the prior distribution P (m 0 ). The most likely mode m ML t at time t is defined as m ML t = arg max i M P (m t = i z 1:t ). (3) Among all TVs in front of the EV, we define the RTs as the TVs in the EV s lane, the TVs in the adjacent left lane whose most likely mode m ML t = LCR, the TVs in the adjacent right lane whose most likely mode m ML t = LCL. The relative distances and velocities of the RTs are then passed to the controller described below. In the absence of a vehicle satisfying the above criteria, a free-flow mode is triggered wherein the controller tracks the road speed limit or a user-defined reference speed as in conventional cruise control systems. 5 CONTROLLER The control strategy is based on combining LbD with MPC, previously presented in Lefèvre et al. (2016). The idea is to leverage the non-parametric nature of LbD and the safety guarantees provided by MPC. We extend our methodology to account for multiple RTs. A review of the control design is presented below. The details can be found in Lefèvre et al. (2016). a t is the acceleration of the EV at time instant t. The joint distribution of the modes m 0:t = {m 0,..., m t }, observations z 1:t = {z 1,..., z t } and accelerations a 1:t = {a 1,..., a t } is given by P (m 0:t, z 1:t, a 1:t ) = t P (m 0 ) P (m k m k 1 )P (z k, a k m k ). (4) k=1 where the emission density function P (z k, a k m k = i) is modeled as a Gaussian N (µ i, Σ i ). The parameters that characterize the distributions in (4) are learned from data collected from a single driver. This allows us to personalize the driving behavior of the autonomous vehicle (Lefèvre et al. 2016). During online operation, the reference acceleration a ref t at time t is computed using Gaussian Mixture Regression (GMR) as a ref t = E[a t z 1:t ] = M P (m t = i z 1:t )[µ a i + Σ az i (Σ zz i ) 1 (z t µ z i )], (5) i=1 where P (m t = i z 1:t ) is computed by the recursive algorithm in (2) and [ ] [ ] µ z µ i = i Σ zz µ a, Σ i = i Σ za i i Σ az i Σ aa. (6) i A sequence of reference accelerations a ref t:t+n over a desired time horizon N can be obtained by iteratively simulating a ref through the vehicle dynamics (described later in Section 5.2.3) and running (5) using the simulated features. In order to compute the predicted values of the features, the most relevant TV is assumed to move at a constant velocity over the horizon N. 5.1 Learning-by-Demonstration (LbD) The goal of LbD is to generate a reference acceleration for the MPC-based controller for the given driving situation. We use the frameworks of HMMs and Gaussian Mixture Regression (GMR) in this work. Similar to Section 4, the car-following behavior of the driver is modeled as a HMM. We define the following variables: m t {1,..., M} is the hidden mode at time instant t, where M is the number of hidden modes. z t = [d r t, v r t, v t ] is the vector of observations at time t, where d r and v r are the relative distance and relative velocity of the most relevant TV, respectively, and v is the velocity of the EV. The determination of the most relevant target among the RTs is discussed later in Section Model Predictive Control (MPC) The reference acceleration is tracked by a robust MPC-based controller where the future accelerations of the RTs are assumed to be bounded disturbances. The control inputs are chosen such that the collision avoidance constraints are satisfied for all possible values of this disturbance. The formulation of the robust safety constraints for the case of a single PT is introduced in Lefèvre et al. (2016) and summarized below. The approach is extended to handle all RTs present at the current time step Robust safety constraints In order to ensure that the EV avoids rear-end collisions with the RTs for all possible future accelerations of the RTs, we take a worst-case approach. That is, the RTs are assumed to apply maximum braking

4 starting from their current positions and velocities till they come to a halt. In Lefèvre et al. (2016), we show that the resulting safety constraints on the position and velocity of the EV over the prediction horizon for a given RT can be written as d p t+k d t+k d safe (k = 1,..., N 1) (7) d p t+n d t+n + v2 t+n 2a min ( vp t+n )2 a p min d safe, (8) where d p t+1:t+n and vp t+1:t+n are the predicted positions and velocities of the RT assuming a maximum braking of a p min. The variable a min is the maximum deceleration that be commanded by the controller on the EV and d safe is the minimum relative safety distance. Note that the constraints (7) and (8) are imposed for all RTs present at time t Safety margin Equation (8) introduces the notion of a safety margin. Intuitively, the LHS of (8) is the distance between the EV and the RT when they stop after applying maximum braking. For a given RT, we define the normalized braking distance (NBD) at time t as follows: NBD t = 1 d safe ( d p t d t + v2 t (vp t ) 2 2a min a p min ). (9) We use this metric to determine the most relevant TV for the LbD approach presented in Section 5.1. That is, among the detected RTs, the vehicle with the smallest NBD is chosen as the most relevant TV. This metric is also used to compare the performance of our algorithm against existing approaches in the next section Longitudinal dynamics model The following linear difference equations are used to describe the longitudinal motion of the EV for the MPC design: [ ] dt+1 ξ t+1 := v t+1 [ dt + T = s v t + 1T ] 2 2 s a t =: f(ξ v t + T s a t, a t ), (10) t where ξ t and a t denote the EV state and control input at time t, respectively, and T s is the sampling time Online optimization problem In MPC, at each time step, a constrained finite horizon optimal control problem is solved. The first control input from the optimal input sequence is implemented, and the process is repeated at the next time step with the new measurements. At time t, the optimization problem to be solved online is given by min a t,ɛ N 1 k=0 ( vt+k+1 v ref 2 Q + a t+k a ref t+k 2 R+ a t+k 2 P ) + Sɛ (11a) s.t. ξ t+k+1 = f(ξ t+k, a t+k ) (11b) d p i t+k+1 d t+k+1 d safe ɛ (i = 1,..., N RT ) (11c) a t+k = a t+k a t+k 1 a min a t+k a max a min a t+k a max (k = 0,..., N 1) d p i t+n d t+n + v2 t+n ( vpi 2a min t+n )2 a p min d safe ɛ (11d) (11e) (11f) (i = 1,..., N RT ) (11g) ξ t = ξ(t), a t 1 = a(t 1), ɛ 0, (11h) where the expression z 2 M = zt Mz for a variable z and a t denotes the control input sequence {a t,..., a t+n 1 } to be optimized over. The reference speed is denoted by v ref and the tracking error is penalized with a small value in Q. Actuator magnitude and slew rate limits are enforced in (11e) and (11f), respectively. The safety constraints (11c) and (11g) are imposed as soft constraints with a high penalty S on the slack variable ɛ. The subscript i in (11c) and (11g) denotes the i th vehicle out of the N RT RTs present at time t. The optimization problem (11) is a standard nonlinear program which we solve using NPSOL (Gill et al. 1986). 6 EXPERIMENTS The proposed ACC system was tested using sensory data collected from our experimental vehicle. Note that the system is capable of running in real-time on embedded rapid control prototyping platforms (see Lefèvre et al. (2016) for details). In this work, for a fair comparison of the proposed strategy with existing approaches, we simulate the effect of the control input on the motion of the EV but use real data to reconstruct the environment. It is assumed that the EV has no influence on TVs that cut-in to its path. The evaluation process is explained in more detail below. 6.1 Data collection and scene reconstruction We collected manual driving data from our on-board vehicle sensors (radar, camera, lidar, GPS/INS) on highways. In order to accurately reconstruct the evolution of the environment, the inertial motion of the EV is estimated using the GPS/INS measurements. Combining this with the relative motion of the TVs perceived by the outward looking sensors allows us to estimate the inertial motion of the TVs. It is then straightforward to compute the apparent relative motion of the TVs with respect to the EV during a simulation involving the proposed controller. The TV data

5 is passed to the sensor fusion and RT identification algorithms. The resulting RT data is used as an input to the MPC-based controller. Finally, the EV motion is simulated using (10), hence closing the loop. We focus specifically on short segments of the datasets (around s each) which involve a cut-in maneuver by a TV in the neighboring lane. The segment start time is chosen to be a few seconds before the TV initiates the lane change and the end time is when the TV completes the lane change. 6.2 Evaluation methodology We compare the performance of the proposed ACC approach (denoted as ACC 1) with the existing ACC system implemented on our experimental vehicle (denoted as ACC 2). Note that the existing system uses a combination of radar and camera without any lane change intent estimation to identify a single PT. The PT identification is based on whether a candidate TV lies in a virtual lane constructed on the basis of the longitudinal velocity and yaw rate of the EV. The main difference between ACC 1 and ACC 2 lies in the identification of the RTs or the PT. For a fair evaluation, the same control strategy (presented in Section 5) is used for both approaches. For each segment of the dataset involving a simulation of either ACC 1 or ACC 2, we compute the following metrics: 1. Absolute values of the mean and maximum deceleration, µ a and p a, respectively. 2. Absolute values of the mean and maximum negative jerk, µ j and p j, respectively. 3. Minimum normalized braking distance, NBD, as defined in (9), computed for the most relevant TV. 4. Maximum inverse time-to-collision, TTC i, defined as the ratio of the relative speed of the EV with respect to the most relevant RT and the relative distance. Metrics (1) and (2) characterize the comfort of the system, where lower values correspond to a smoother behavior. Metrics (3) and (4) characterize the safety. A higher value of the NBD is desired while a lower value of TTC i implies lower collision risk. 6.3 Results For a given approach (ACC 1 or ACC 2), the mean and standard deviation of the quantities described in Section 6.2 were computed for 28 segments from 4 datasets and are tabulated in Table 1. We see that ACC 1 yields lower values of the mean and maximum deceleration as compared to ACC 2 resulting in a more comfortable experience for the driver. The Metric Unit ACC 1 ACC 2 µ a m/s ± ± 0.91 p a m/s ± ± 1.17 µ j m/s ± ± 1.68 p j m/s ± ± 1.29 NBD ± ± 3.43 TTC i s ± ± 1.24 Table 1: Means and standard deviations of the metrics defined in Section 6.2 for the proposed approach (ACC 1) and the existing approach (ACC 2) computed from 28 simulations. Acceleration [m/s 2 ] ACC1 ACC Time [s] Figure 2: Comparison of the closed-loop acceleration profiles during a simulation of the scenario described in Section 6.3 of the proposed approach (ACC 1, solid line) and the existing approach (ACC 2, dashed line). ACC 1 results in a smoother acceleration command that ACC 2. difference in the mean and peak negative jerk is less marked. This can be attributed to the fact that only aggressive cut-in maneuvers were selected for the analysis which required a sudden change in the acceleration commanded by the controller. The average values of the NBD and TTC i show that ACC 1 yields a safer behavior than ACC 2, validating the consideration of TVs lane change intentions. We further analyze an interesting and challenging situation recorded in one of the datasets where two TVs changed lanes in front of the EV within a short span of time. At time t = 5.4 s (from an arbitrary t = 0), the first TV (TV 1) crosses the right boundary of the EV s lane, followed by the second TV (TV 2) at time t = 9.2 s. The small relative distance and lower speeds of the TVs necessitates a braking maneuver by the EV. Figure 2 shows a comparison of the acceleration profiles for ACC 1 and ACC 2 when this scenario is reconstructed in simulation. It is seen that due to the lane change estimation of TV 1, ACC 1 starts braking at t = 4.3 s, 1.2 s before ACC 2, resulting in a smoother acceleration profile. Later, when TV 2 cuts in, ACC 1 responds at t = 8.1 s, 1.8 s before ACC 2. The deceleration command saturates in the case of ACC 2 but not in the case of ACC 1. A comparison of the metrics defined in Section 6.2 for both approaches is shown in Table 2.

6 Metric Unit ACC 1 ACC 2 µ a m/s p a m/s µ j m/s p j m/s NBD TTC i s Table 2: Comparison of the metrics defined in Section 6.2 for ACC 1 and ACC 2 during a simulation of the scenario described in Section 6.3. Most Likely Intent Lateral Position [m] RT Flag LCR LCL LK ACC1 ACC Time [s] Figure 3: Results of the relevant target identification for a TV cut-in scenario from one of the datasets (described in Section 6.4). ACC 1 identifies the RT 1.85 s before ACC Relevant target identification performance Finally, we compare the performance of the proposed RT identification methodology presented in Section 4 (ACC 1) against the existing approach (ACC 2). The metric used is the time difference between the two approaches recognizing a cutting-in TV as a RT or PT. It is observed that our method identifies a RT 1.28 ± 0.26 s before the existing approach. The trajectory of a TV s lateral position e y while it performs a cut-in maneuver from one of the datasets is shown in top plot of Figure 3. The middle plot shows the estimated most likely mode (defined in Section 4). We see that the estimator switches to the lane change right (LCR) mode 1.4 s before the estimated center of mass of the TV crosses the lane boundary. The bottom plot of Figure 3 depicts the binary variable indicating whether or not the TV has been identified as a RT for both methods ACC 1 and ACC 2. It is seen that ACC 1 identifies the RT 1.85 s before ACC 2 in this scenario. 7 CONCLUSIONS This paper presents an approach for detecting target vehicle cut-ins for ACC systems. Data from multiple sensors (radar, camera and lidar) are fused to estimate target vehicles positions in their lanes. A learning-based lane change intention estimation algorithm identifies target vehicles which may change lanes into the path of the ego vehicle. A control strategy based on robust MPC is presented which considers safety with respect to all relevant target vehicles without the need for heuristics to select a single primary target. The proposed methodology is shown to improve the performance of the ACC system as compared to an existing approach, based on several metrics that consider comfort and safety. ACKNOWLEDGMENTS The authors would like to thank the Hyundai Motor Company for their support. REFERENCES Deb, S., M. Yeddanapudi, K. Pattipati, & Y. Bar-Shalom (1997). A generalized S-D assignment algorithm for multisensor-multitarget state estimation. IEEE Transactions on Aerospace and Electronic Systems 33(2), Domahidi, A., E. Chu, & S. Boyd (2013). ECOS: An SOCP solver for embedded systems. In European Control Conference (ECC), pp Gill, P. E., W. Murray, M. A. Saunders, & M. H. Wright (1986). User s guide for NPSOL (version 4.0): A Fortran package for nonlinear programming. No. SOL Stanford University Systems Optimization Lab. Lefèvre, S., A. Carvalho, & F. Borrelli (2016). A learningbased framework for velocity control in autonomous driving. IEEE Transactions on Automation Science and Engineering. Lefèvre, S., Y. Gao, D. Vasquez, E. Tseng, R. Bajcsy, & F. Borrelli (2014). Lane keeping assistance with learning-based driver model and model predictive control. In Proc. 12th International Symposium on Advanced Vehicle Control. Moon, S., H.-J. Kang, & K. Yi (2010). Multi-vehicle target selection for adaptive cruise control. Vehicle System Dynamics 48(11), Müller, D., J. Pauli, M. Meuter, L. Ghosh, & S. Müller- Schneiders (2011). A generic video and radar data fusion system for improved target selection. In 2011 IEEE Intelligent Vehicles Symposium (IV), pp