TRAJECTORY PLANNING FOR AUTOMATED YIELDING MANEUVERS, Mattias Brännström, Erik Coelingh, and Jonas Fredriksson Funded by: FFI strategic vehicle research and innovation ITRL conference
Why automated maneuvers? Safety and convenience Your life Your time Your choice Page 2 / 16
Why trajectory planning? An automated vehicle must know what to do. Page 3 / 16
Agenda Problem description Contribution Short introduction to convex optimization and model predictive control Trajectory planning algorithm Summary Questions Page 4 / 16
Problem description If the ego vehicle should perform an automated yielding maneuver, determine: 1. in which gap between some traffic participants or objects and at what time instance the maneuver should be performed, and 2. calculate a feasible maneuver (if such exists) in terms of a longitudinal and a lateral trajectory, i.e., the control signals, which allow the ego vehicle to position itself in the selected gap at the desired time instance. Furthermore, the maneuver should be planned such that the ego vehicle maintains safety margins to all surrounding traffic participants and objects, respects traffic rules and regulations, as well as satisfies physical and design limitations. Page 5 / 16
What is the challenge? To develop a trajectory planning algorithm which is applicable to passenger vehicle ADAS or highly automated vehicles, the algorithm must have the ability to deal with the demands of limited computational resources, planning in a dynamic and uncertain environment, generating safe collision-free trajectories, satisfying physical and design limitations, as well as abiding traffic rules and regulations. Page 6 / 16
Contribution Trajectory planning algorithms for automated yielding maneuvers formulated as convex Quadratic Programs (QP) within the Model Predictive Control (MPC) framework which provides a structured approach to express system objectives and constraints to allow for reliable, predictable, and robust, real-time implementation without the assumption of a reference trajectory. Page 7 / 16
Convex QP min trajectory cost function, subject to vehicle dynamics, physical and design constraints, collision avoidance constraints. min w subject to J w = 1 2 wt Hw + d T w H in w k in, H eq w = k eq. Page 8 / 16
MPC 1. Measure the state x at the current time instance t. 2. Solve the optimal control QP problem to obtain the control sequence U. 3. Apply the first element of U to the system. 4. Repeat Step 1-3 at time instance t+1. x(t) x(t + 1) u t = u (t) u(t + 1) = u (t + 1) Page 9 / 16 t-1 t t+1 t+n t+n+1 Time
Trajectory planning for automated yielding maneuvers Problem: Find a longitudinal and a lateral trajectory, i.e. the control signals, which allows the ego vehicle, E, to perform a safe and smooth yielding maneuver e.g. a lane change maneuver. Solution: Loosely coupled longitudinal and lateral optimal control problems in the form of convex QPs which are solved within the MPC framework. Page 10 / 16
Trajectory planning algorithm I. Determine an appropriate inter-vehicle traffic gap in the target lane and a time instance to initialize the lateral motion of the maneuver. II. III. IV. Determine the longitudinal safety corridor. Determine the longitudinal trajectory (QP optimization). Determine the lateral safety corridor. V. Determine the lateral trajectory (QP optimization). Page 11 / 16
Simulation results Simulation scenario: decelerate into the lane change gap. Page 12 / 16
Real-time experiments Page 13 / 16
Real-time experiments Page 14 / 16
Summary min trajectory cost function, subject to vehicle dynamics, physical and design constraints, collision avoidance constraints. Page 15 / 16
Further information julia.nilsson@volvocars.com J. Nilsson, M. Brännström, E. Coelingh, and J. Fredriksson, Longitudinal and Lateral Control for Automated Lane Change Maneuvers, American Control Conference, pp. 1399-1404, July, 2015, Chicago, IL, USA. J. Nilsson, M. Brännström, J. Fredriksson, and E. Coelingh, Longitudinal and Lateral Control for, IEEE Transactions on Intelligent Transportation Systems, vol. 17, no. 5, pp. 1404-1414, May, 2016. J. Nilsson, M. Brännström, J. Fredriksson, and E. Coelingh, Lane Change Maneuvers for Automated Vehicles, Accepted for publication in IEEE Transactions on Intelligent Transportation Systems, 2016. J. Nilsson, J. Silvlin, M. Brännström, E. Coelingh, and J. Fredriksson, If, When, and How to Perform Lane Change Maneuvers on Highways, Accepted for publication in IEEE Intelligent Transportation Systems Magazine, 2016. J. Nilsson, Automated Driving Maneuvers -Trajectory Planning via Convex Optimization in the Model Predictive Control Framework, PhD Thesis, ISBN 978-91-7597-450-7, Doktorsavhandlingar vid Chalmers Tekniska Högskola, Ny serie nr 4131, ISSN 0346-718X, Chalmers University of Technology, October, 2016, Gothenburg, Sweden. Automated Driving Maneuvers Page 16 / 16
Questions
I. Traffic gap and initiation time selection 1. Approximate the reachable set of E by e.g. a set of ACC trajectories T T 2. For each inter-vehicle traffic gap and discrete time instance to initialize the lateral motion of the maneuver, determine if there exists a trajectory within the set, T, which allows E to be safely positioned in its current lane, enter the lane change region of the inter-vehicle traffic gap at the specified time instance, remain in that region for n min, and thereafter safely be positioned in the target lane until the end of the prediction horizon T F T T 3. Select the gap and time instance for which the trajectory, T F T, minimizes J x = N 2 2 2 k=1 ϑ v xk v xdesk + κ axk + φ axk Page 18 / 27
II. Longitudinal safety corridor x maxk = x 1k s d, k = 0,, N Peri, x maxk = min x 1k, x 2k s d, k = N Peri,, N Post, x maxk = x 2k s d, k = N Post,, N. N Post = N Peri + n min x 3 x 4 x 1 x 2 Page 19 / 27
II. Longitudinal safety corridor x mink = x 3k + s d, k = 0,, N Peri, x mink = max x 3k, x 4k + s d, k = N Peri,, N Post, x mink = x 4k + s d, k = N Post,, N. x 3 x 4 x 1 x 2 Page 20 / 27
III. Longitudinal trajectory planning Longitudinal motion captured by a double integrator t 2 x k+1 = x k + v xk t s + a s xk, k = 0,, N 1, v x k+1 = v xk + a x k t s, k = 0,, N 1, which is subjected to the following set of constraints 2 x mink x k x maxk, k = 1,, N, v xmink v xk v xmaxk, k = 1,, N, a xmink a xk a xmaxk, k = 1,, N, a xmink a xk a xmaxk, k = 1,, N. Page 21 / 27
III. Longitudinal trajectory planning Trajectory planning problem is solved as a standard quadratic program subject to min w J x = 1 2 wt Hw, H eq w = K eq, H in w K in, where w = x k, v xk, a xk and J x = N k=1 ϑ v xk v xdesk 2 + κ axk 2 + φ axk 2. Page 22 / 27
IV. Lateral safety corridor y maxk = L li k, k = 0,, N Peri, y maxk = L li+1 k, k = N Peri,, N, y mink = L ri k, k = 0,, N Post, y mink = L ri+1 k, k = N Post,, N. L li+1 L li / L ri+1 L ri Page 23 / 27
V. Lateral trajectory planning Lateral motion captured by a double integrator t 2 y k+1 = y k + v yk t s + a s yk, k = 0,, N 1, v yk+1 = v yk + a yk t s, k = 0,, N 1, which is subjected to the following set of constraints 2 y mink y k y maxk, k = 1,, N, v ymink v yk v ymaxk, k = 1,, N, 0.17v xk v yk 0.17v xk a ymink a yk a ymaxk, k = 1,, N, a ymink a yk a ymaxk, k = 1,, N. Page 24 / 27
V. Lateral trajectory planning Trajectory planning problem is solved as a standard quadratic program subject to min w J y = 1 2 wt Hw H eq w = K eq, where w = y k, v yk, a yk and H in w K in, J y = N k=1 ϕ v yk 2 + ψ ayk 2 + θ ayk 2.
What about dense traffic situations? Allow the algorithm to plan trajectories which account for motion dependent safety critical zones of miscellaneous shape. Page 26 / 27
What about dense traffic situations? Page 27 / 27