Fast Trajectory Planning and Robust Trajectory Tracking ... - IEEE Xplore

SPECIAL SECTION ON SECURITY ANALYTICS AND INTELLIGENCE FOR CYBER PHYSICAL SYSTEMS Received April 11, 2017, accepted May 18, 2017, date of publication June 1, 2017, date of current version June 28, 2017. Digital Object Identifier 10.1109/ACCESS.2017.2707322

Fast Trajectory Planning and Robust Trajectory Tracking for Pedestrian Avoidance YUXIAO CHEN1 , HUEI PENG1 , AND JESSY W. GRIZZLE2 , (Fellow, IEEE)

1 Department 2 Department

of Mechanical Engineering, University of Michigan, Ann Arbor, MI 48109, USA of EECS, University of Michigan, Ann Arbor, MI 48109, USA

Corresponding author: Yuxiao Chen ([email protected]) This work was supported by NSF under Contract CNS-1239037.

ABSTRACT This paper presents an integrated design method for pedestrian avoidance by considering the interaction between trajectory planning and trajectory tracking. This method aims to reduce the need for control calibration by properly considering plant uncertainties and tire force limits at the design stage. Two phases of pedestrian avoidance—trajectory planning and trajectory tracking—are designed in an integrated manner. The available tire force is distributed to the feedforward part, which is used to generate the nominal trajectory in trajectory planning phase, and to the feedback part, which is used for trajectory tracking. The trajectory planning problem is solved not by searching through a continuous spectrum of steering/braking actions, but by examining a limited set of ‘‘motion primitives,’’ or motion templates that can be adopted in sequence to avoid the pedestrian. An emergency rapid random tree (RRT) methodology is proposed to quickly identify a feasible solution. Subsequently, in order to guarantee accuracy and provide safety margin in trajectory tracking with presence of model uncertainties and exogenous disturbance, a simplified LQR-based funnel algorithm is proposed. Simulation results provide insight into how pedestrian collisions can be avoided under given initial vehicle and pedestrian states. INDEX TERMS Vehicle safety, motion planning, motion control, rapid random tree, LQR funnel.

I. INTRODUCTION

While accidents involving pedestrians constitute a relatively small portion of motor vehicle crashes in the U.S., they are responsible for a very large percentage of casualties. In 2015, 5376 pedestrians were killed in motor vehicle crashes in the U.S., making up about 14% of total fatality [1]. In China, the ratio is even higher at 25.4% [2]. Both automakers and administrative organizations have made great efforts to mitigate this problem. For example, Toyota and Ford have both demonstrated pedestrian avoidance functions using active steering and braking [3], [4]. The Euro New Car Assessment Program (NCAP) started to rate Active Emergency Braking (AEB) systems [5]. All systems evaluated to date have focused only on braking. In the future, systems using both braking and steering are likely to be introduced. The problem of pedestrian avoidance, or more generally obstacle avoidance, consists of two tasks: trajectory planning and trajectory tracking. The trajectory planning problem for obstacle avoidance has been studied extensively for over 20 years. Three popular methods are cell decomposition, artificial potential field, and roadmap based approaches.

9304

A typical cell decomposition method is the vector field histogram method proposed by Borenstein et al., which detects and records unknown obstacles in 2-dimensional Cartesian cells [6]. Fulgenzi et al. and Rachel Bis et al. then introduced the concept of velocity obstacles that enables cell decomposition methods to deal with moving objects [7], [8]. The artificial potential field method was proposed by Khatib et al. to drive robots with virtual force derived from the gradient of a potential field [9], [10]. Roadmap method, on the other hand, is more suitable for trajectory planning with nonholonomic constraints and dynamic constraints, since the feasibility of trajectory can be guaranteed while constructing the roadmap. In particular, trajectory planning problems with dynamic constraint are often referred to as kinodynamic trajectory planning. The method for holonomic trajectory planning such as Probabilistic Roadmap(PRM), Rapid Random Tree(RRT) has been extended to kinodynamic programming via steering function [11], [12], or motion primitive [13]–[15], which use a set of pre-computed trajectories as building blocks and piece them together to get desired trajectories. While first used on unmanned aerial vehicles, this method has been applied on

2169-3536 2017 IEEE. Translations and content mining are permitted for academic research only. Personal use is also permitted, but republication/redistribution requires IEEE permission. See http://www.ieee.org/publications_standards/publications/rights/index.html for more information.

VOLUME 5, 2017

Y. Chen et al.: Fast Trajectory Planning and Robust Trajectory Tracking for Pedestrian Avoidance

ground vehicle trajectory planning [16], [17]. Though there has been a lot of effort improving the algorithm, the computation efficiency remains a problem. Another type of trajectory planning method is the local method that improves the performance by gradient type approach, which includes shooting method [18], direct collocation [19] and the Model Predictive Control (MPC). With the development of optimization, various MPC approaches have been developed, including Mixed integer Programming based MPC [20], nonlinear programming based MPC [21], and MPC combined with potential field [22]. However the local method suffers when the feasible region is nonconvex and the convergence is not guaranteed. The problem of pedestrian avoidance requires kinodynamic planning, and fast computation is of great importance. An early decision may be better than an optimal but late one. On the other hand, in such a safety critical application, the accuracy of trajectory tracking under uncertainty and disturbance is also critical. The goal of this research is to develop a method that can quickly identify a feasible path for pedestrian avoidance, while guarantee its feasibility. The proposed treats trajectory planning and trajectory tracking in an integrated way. An emergency RRT algorithm with motion primitives is proposed to perform fast trajectory planning; and a LQR funnel algorithm is proposed to track the trajectory with bound on closed loop error. The bound is then used as a safety margin in the trajectory planning phase to ensure safety with the presence of exogenous disturbance and model uncertainty. For the remainder of the paper, Section II explains the dynamic models. Section III introduces basic assumptions on the environment and formulates the pedestrian avoidance problem. Sections IV and V present the motion primitive construction and the emergency RRT method that allows fast online trajectory planning. Section VI presents the funnel algorithm that obtains the bound of trajectory tracking error. Results and the discussion are presented in Sections VII and finally conclusion is drawn in Section VIII. II. VEHICLE MODELS

Two ground vehicle dynamic models are used in this paper. The first one is a simple linear lateral-yaw model for motion primitive design and the derivation of dynamic constraints. The second one is the commercial software Carsim, which is widely used in industry for simulations and validation. The lateral-yaw model, also known as the bicycle model or the single-track model, as shown in Figure 1 is governed by the following set of differential equations. m˙vy = Fyf + Fyr − mrvx Iz ψ¨ = Iz r˙ = Mz = Fyf a − Fyr b,

(1) (2)

where vx and vy are the vehicle longitudinal and lateral speed respectively; Fyf and Fyr are the lateral forces on the front and rear axles; ψ is the yaw angle of the vehicle; and yaw rate, its derivative, is denoted as r. a and b represent the distance VOLUME 5, 2017

from the front and rear axles to the CG, respectively, and m and Iz are the mass and moment of inertia of the vehicle. Here we assume that the steering angle δf is small enough that sin δ ≈ δf cos δf ≈ 1.

(3)

In the pedestrian avoidance situation discussed in this paper, the steering angle never exceeds 5 degrees, which makes this assumption valid. Assuming linear tire force generation, the following equations determine the lateral forces on each axle. vy + ar (4) Fyf = Cαf αf = Cαf δf − vx br − vy Fyr = Cαr αr = Cαr , (5) vx where Cαi is the cornering stiffness of the front or rear axles and αi is the sideslip angle of the axles (i = f , r). vx is the longitudinal speed. The dynamic model is linear with constant vx .   bCαr − aCαf Cαf + Cαr − v − x   vy v˙ y mvx mvx  =  bCαr − aCαf a2 Cαf + b2 Cαr  r r˙ − Iz vx Iz vx  C  αf  m  (6) +  aC  δf αf Iz Written in compact form, the dynamic equations are represented as: ζ˙ = f (ζ, u),

(7)

T where ζ = vy r and u = δf . A lateral-yaw model with yaw angle and lateral displacement as part of the state vector can be obtained based on the original model, plus two additional equations: ψ˙ = r y˙ = vx ψ + vy

(8)

Here we are assuming that the heading angle |ψ| 1. This assumption is valid since the case we are considering is pedestrian avoidance on a straight lane. This model is denoted as ξ˙ = f¯ (ξ, u) , (9) T where ξ = y ψ vy r , u = δf . Carsim is used in the verification of the proposed method. Carsim is a widely used commercial software for vehicle dynamic simulations. It has 41 DOFs and captures key nonlinearities through look-up tables or gain-scheduled nonlinear equations, and can be accurately tuned with experimental data. 9305


III. ASSUMPTIONS AND PROBLEM FORMULATION

Two basic assumptions underlie this work: (1) the pedestrian’s speed is bounded and the bound is known, and (2) the friction limit of the road is known. To the authors’ best knowledge, no widely accepted model exists to describe the behavior of pedestrians. Some earlier studies assumed that information about the moving obstacle is completely known to the controller [20], [22], which was not realistic for pedestrian avoidance problems. Social force model has been used to predict the pedestrian motion, but the accuracy is not sufficient for high speed pedestrian avoidance [23]. Stochastic model has also been applied to predict the future motion of pedestrians, including categorizing the motion of the pedestrian into stages [24], modelling the intention of pedestrian as hidden state for hidden Markov chains [25]. For safety’s sake, such a stochastic model should not be used because it would be viewed as ‘‘betting on safety’’ and may have legal/liability implications. In this paper, therefore, a ‘‘worst-case’’ approach is adapted, in which we assume that the upper bound of the pedestrian’s speed is known, with movement possible in any direction: vped ≤ vmax . (10)

FIGURE 1. Lateral-yaw model of Ackerman-steering vehicles.

This assumption projects the future location of the obstacle into a circle, with its diameter growing linearly over time. The obstacle set is defined explicitly as q 2 2 Xd = (x, y) | x − xp + y − yp ≤ vmax t , (11) where xp , yp is the initial position of the pedestrian, and t is the time duration since the beginning of path planning. In addition to the requirement that the vehicle never hit the pedestrian, it must also never hit the lane boundaries. The width of the lane is set at 3.6 meters, vmax is set to 1.42m/s, which is based on the statistics from [26]. Another important assumption in our study is that the friction limit of the road, or at least a lower bound, is known. Thus, the dynamic capability of the vehicle is known. Given a vehicle initial condition ξ0 and pedestrian obstacle Xd , the goal is to find a dynamic trajectory that avoids entering the obstacle set Xd before either of the following two things occur: 1. the vehicle’s longitudinal position passes the longitudinal position of the pedestrian; 2. the vehicle comes to a full stop. From the perspective of obstacle avoidance, all feasible solutions satisfying the above requirements are acceptable. Remark: Other types of moving obstacles such as vehicles can also be handled with this framework, but there should be more information about the future motion, since the assumption in (10) may be too conservative for vehicles. IV. MOTION PRIMITIVE LIBRARY

We base our trajectory planning method on motion primitives. Motion primitives are trajectory segments of the dynamic 9306

FIGURE 2. Line search for steering maneuver construction.

system, with predefined input and state vectors. Emilio et al. proposed the trim and maneuver idea in [15], where trims are trajectories that operate at an equilibrium, and maneuvers are the transitions between two equilibrium points. In the vehicle case, a trim is a segment of trajectory that has fixed longitudinal speed, sideslip speed and yaw rate vx vy r . A trim can last for an arbitrary amount of time. A maneuver, however, has a fixed time span, depending on the starting/ending equilibria. For simplicity of the algorithm, we enforce the durations of all motion primitives to be multiples of unit duration Ts , and all trims are assumed to last for one unit duration Ts . A. TRIM DESIGN

Trim design involves selecting equilibrium points of the dynamic equations. Since the 3 states: vx , vy , r are not independent, we select the two states critical to ground vehicle motion vx and r to determine the trims. More precisely, in dynamic equation (7), picking a longitudinal speed vx and yaw rate r, the input steering angle δf and the steady state VOLUME 5, 2017


sideslip speed vy can be solved by setting : vy 0 A + Bδf = , r 0

(12)

where r is fixed, δf and vy are solved Eq. (12). The number of trims is a design choice, and the selection should be made carefully, as the size of the trim library determines the size of the motion primitive library. A large motion primitive library can generate more exquisite trajectories, but the search process for an optimal trajectory is then likely to take more time. Although in theory the trim equilibrium can be chosen arbitrarily, a wise choice may lead to better performance of the trajectory planning algorithm. In an emergency situation application, the following criteria should be considered when choosing trim equilibria: a. Trim library should include trims with a wide range of yaw rate to ensure that swerving hard is included as an option. b. The number of trims should be limited to ensure the real-time performance. c. The duration of maneuvers linking different trims should be short. The reason will be explained in the next section. The trim table is selected based on the vehicle model. The exact value can and should adapt to the road and vehicle conditions if this method is to be deployed on a test/production vehicle. For example, on slippery roads or with heavier vehicles, the magnitude of the extreme yaw rates should reduce to what the vehicle/road is capable of executing. The number of trims can also be adjusted based on the available computation power and memory on-board. B. MANEUVER DESIGN

The primary requirement for a maneuver is that the starting and ending points should have the same states (vx , vy , r) as the two trims it connects respectively. It should also satisfy dynamic constraints such as tire force limit. When the vehicle is executing a maneuver, it cannot abort in the middle, because any middle point is not a trim equilibrium, and no motion primitive can follow the unfinished maneuver. For this reason, the duration of a maneuver should be as short as possible. To avoid maneuvers with long duration, not all pairs of trims are directly connected by a maneuver. Maneuvers connect only (i) trims with the same longitudinal speed and different yaw rates, and (ii) trims with the same yaw rate and adjacent longitudinal speed. The first type of maneuvers is called steering maneuvers; the second type is called braking maneuvers (although acceleration may also be used). Braking maneuvers are easy to construct since it the longitudinal dynamic is simply a double integrator. We then show the construction process of steering maneuver. The goal of constructing steering maneuvers is to get an input and state sequence of minimum duration that satisfies the above mentioned constraints. For a fixed horizon, an optimization problem can be formulated to test whether an input sequence satisfying all constraints exists. Then a line VOLUME 5, 2017

search is performed that gradually increases the horizon of the optimization, until a feasible solution is found. The optimization problem is formulated with linear dynamics and constraints, with a cost function in quadratic form. The goal of optimization is to test whether a solution exists for the current horizon, rather than to minimize the cost function. The constraints are: a. Tire force limit: The lateral force on each axle cannot exceed the friction limit. b. Steering rate limit: The steering rate is bounded, based on the specific steer by wire hardware specification. δ˙f ≤ δ˙max (13) c. Reaching the goal state: The maneuver state trajectory should start from the equilibrium of the starting trim and land on the equilibrium of the target trim at the end of the maneuver. The cost function penalizes vehicle sideslip and steering input, which represents the severity of the maneuver. It is set up to pick one solution among all feasible solutions of the same duration. The continuous-time dynamic shown in Eq. (6) is discretized with the sampling time ts = Ts /Nd following standard discretization procedure for linear systems, where Nd is an integer representing the ratio between the unit duration of motion primitive and the sampling time of the discrete lateral-yaw model of the vehicle. The discrete-time dynamic equation is represented as: ζ i+1 = g ζ i , ui , i ≥ 0. (14) The line search for steering maneuver input sequence is performed by iterating the optimization process with increasing horizon until a feasible solution is found, as shown below. J is the cost function, defined as J=

nNd X

w1 viy + w2 δfi .

(15)

i=1

Integer n is the number of unit durations. It is initialized to be 1, and keeps increasing until a feasible solution is found. h1 represents the tire force constraint, h2 represents the steering rate constraint, and h3 represents the final state equality constraint:  ar + vy b  Cαf δf − ≤ µmg vx a+b h1 :  Cαf −br + vy ≤ µmg a vx a+b ( d vnN = v1y y h2 : 1δf ≤ δ˙max ts , h3 : nN (16) r d = r 1. where µ is the friction limit; v1y and r 1 are the lateral velocity and yaw rate of the succeeding trim. Using the procedure described for trim table selection and maneuver design, the process of building the motion primitive library is systematic and automated. 9307


FIGURE 3. The Original RRT process [27].

V. MOTION PRIMITIVE TRAJECTORY PLANNING A. MOTION PRIMITIVE TREE

FIGURE 4. The Proposed Emergency RRT process.

The trajectory planning phase is associated with a tree structure. Every node in the tree represents vehicle state in the configuration space, and every edge is built with motion primitives. To be clear, the in the motion state considered primitive construction is vx vy r , while the vehicle state in the configuration space is X Y vx vy r ψ . The position states(X , Y , ψ) is calculated while propagating the tree with motion primitives. All nodes in the same logic layer on the tree must also share the same time index, which defines the depth of the layer. Node (i) .T = kTs ,

∀Node in layer k

(17)

Since all trims last for Ts , the node following a trim naturally satisfies Eq. (17). The duration of a maneuver is a multiple of Ts . Suppose the duration of one maneuver is m times Ts . When m > 1, the maneuver is cut into m segments, each lasting for Ts . The middle point of a maneuver can only be followed by the next segment of the maneuver until the maneuver is fully executed. B. EXPANDING MOTION PRIMITIVE TREE WITH RRT 1) ORIGINAL RRT

The RRT concept was originally proposed by LaValle and Kuffner [27]. This algorithm is probabilistically complete, that is, as iteration goes to infinity, the probability of finding an existing solution approaches 1. In Figure 3, T denotes the rapid random tree; xinit is the original state of the vehicle; xrand is a random state sampled in the configuration space; xnear is the nearest existing node of the tree to xrand ; uis the input trajectory calculated using heuristic path planning algorithm and xnew is the new node added to the tree. For more detail, please see [27]. 2) EMERGENCY RRT

The pedestrian avoidance problem requires modification to the original RRT algorithm because a. the velocity in the longitudinal degree of freedom is significantly larger than those in other dimensions; 9308

b. the problem is bounded by the lane edges, which greatly reduces the region to be explored; c. the problem horizon is short—typically between 1s and 3s, and computation should be fast enough for real-time implementation; d. both the position and velocity states are critical to the success of avoiding the pedestrian. To accommodate these features, based on the motion primitive library constructed, the original RRT method was modified in the following ways: a: FINITE NUMBER OF INPUTS

The input is selected from the motion primitive library. b: KEEP ALL CHILD NODES

Since the input choice is limited, exploring all the child nodes is fast. Comparing to the original RRT method, which usually uses optimization or shooting method to grow the child nodes, exploring all child nodes expands the tree faster and help find a feasible solution faster. c: EXTREME NODE FIRST

Nodes with extreme values of states (X , Y and ψ) are expanded first. In an obstacle avoidance situation, extreme nodes push closer to the lane boundary and tire force limits, thus have higher priority. d: RANDOMLY GROW NON-EXTREME NODES

After growing all the extreme nodes, non-extreme nodes are randomly chosen to expand. If a solution that can be identified as a combination of motion primitives exists, eventually it will be found. The process of the proposed emergency RRT is summarized in Figure 4. We note the following: • We assume that the initial state is at a trim point. If not, then we first decelerate the vehicle to reach the nearest trim. xinit is the initial state of the VOLUME 5, 2017


3) SELECTING THE OPTIMAL NODE AMONG THE FEASIBLE SET

FIGURE 5. Layer area approximation (green shadowed area).

•

vehicle(X ,Y ,ψ,vx ,vy ,r). Xd is the danger zone surrounding the pedestrian, as defined in Eq. (11). Integer N represents the number of layers of RRT, which is determined by the trajectory planning horizon as N = T /Ts , where T is the planning horizon. T is selected based on the initial location of the pedestrian and the vehicle speed. T = max{T1 , T2 },

•

•

vx lx , T1 = , T2 = vx amin

(18)

where T1 is the time needed for the vehicle to pass the pedestrian at the current speed, lx denotes the longitudinal distance from vehicle to the pedestrian, and T2 is the time needed for the vehicle to come to a full stop, amin denotes the minimum acceleration(maximum braking). M is the iteration time of RRT. As M grows, the chance of finding a solution grows. Function Get_layer_area() calculates a rough approximation of reachable area of the vehicle at a certain layer based on the algebraic and dynamic constraints, which is illustrated in Figure 5. Function Lowest_density() returns the layer with lowest node density, calculated as number of nodes in this layer divided by area of layer. ρlayer =

Number of nodes within layer . Area of layer

(19)

Edge is defined as the set of extreme nodes of a layer, including nodes with minimum and maximum of X and Y coordinates, heading angle ψ, and longitudinal speed vx . • Function Expand_node() executes all motion primitives that can follow one node and adds collision-free child nodes to the tree. • Function Random_node() randomly generates a point at the possible area of the chosen layer, then selects the node that lies closest to that point, where Cartesian distance is used to measure the distance. • The process of randomly selecting non-extreme nodes favors nodes far away from other nodes. It does not give every node a fair chance, but rather gives the whole area a fair chance. In conclusion, this algorithm starts by expanding all extreme nodes of each layer. At each iteration, it calculates the node density of all layers. Then it randomly selects a node from the layer before the layer of lowest density and expands that node. •

VOLUME 5, 2017

In order to pick one node out of all feasible nodes, a measurement of optimality needs to be defined. Note that every node represents a trajectory from the initial condition, therefore the optimality of a node is in fact the optimality of its corresponding trajectory. The cost for each node consists of two parts: the final cost and the cumulative cost. The final cost function punishes the lateral displacement and yaw angle of the vehicle at the terminal point. These two states indicate how misaligned the vehicle is. Furthermore, the final cost discourages the vehicle to slow down, since it is not preferred to stop the vehicle if not necessary. Jf = wy y (T ) + wψ ψ (T ) + wv vx (T )

(20)

The cumulative cost penalizes the maximum longitudinal acceleration and lateral speed during each time step. These two variables indicate the severity of the motion primitive: Jmp (k) = wv v

max (k) + wa amax (k) .

The total cost of a node is calculated as X Jn = Jf + Jmp .

(21)

(22)

k

If no feasible solution is found, then the emergency RRT algorithm declares failure, and the control would be to brake as hard as possible to minimize impact velocity. VI. TRAJECTORY TRACKING WITH SAFETY MARGIN A. FORCE DISTRIBUTION BETWEEN FEEDFORWARD AND FEEDBACK

In a safety critical situation, it is important to track the planned trajectory with guaranteed accuracy. In the literature, trajectory planning and trajectory tracking are sometimes handled separately. However, the two problems are actually coupled, since higher tire forces in principle generate more aggressive trajectories, and stronger feedback control results in a smaller tracking error, but the sum of the two is limited by the friction limit. Thus, given the limit of total tire force, the distribution of the total available tire force to the feedforward and feedback controls should be considered a priori instead of being an after-thought, or ‘‘prove by simulations.’’ We follow the procedure described below. Figure 6 shows longitudinal tire force as a function of the tire slip ratio. Lateral tire force changes with the tire slip angle in a similar way. Fz denotes the vertical load of the tire. We denote the nominal input force of the nominal trajectory generated in trajectory planning phase as feedforward force, and the input force of trajectory tracking control as feedback force. The sum of feedback and feedforward force should be limited such that the total tire force does not exceed the friction limit. Since the trajectories consist of motion primitives and all motion primitives are predefined, the maximum feedforward tire force can be pre-selected and enforced as 9309


As our work involves only a linear time-invariant (LTI) system, a simplified funnel method is proposed, one which is much faster to compute. In the trajectory tracking phase, two feedback controllers work simultaneously: longitudinal tracking and lateral tracking. Longitudinal tracking is relatively simple because of the simple dynamics. Thus only the lateral feedback design is discussed in this paper; the longitudinal feedback design process is similar, it is a state feedback controller designed with LQR in the following form: T 1ax = Kx 1x 1vx , (23) FIGURE 6. Force distribution between feedback and feedforward.

where 1ax is the feedback acceleration input, 1x and 1vx are the longitudinal position and velocity error and Kx is the feedback gain. 1) TRAJECTORY TRACKING PROBLEM FORMULATION

The lateral tracking problem is formulated as a disturbance rejection problem with limited feedback control force. Recall that the motion primitives are designed based on a linear lateral-yaw model with yaw angle, yaw rate, lateral displacement and lateral speed as the four states. The simplified model ignores disturbances such as crosswind and road bank angle. Let ξ0 and u0 denote the nominal state and input trajectory of a motion primitive, then according to Eq.(9), we have: ξ˙0 = f¯0 (ξ0 , u0 ) .

(24)

The actual dynamic equation is denoted as ξ˙0 + 1ξ˙ = f¯1 (ξ0 + 1ξ, u + 1u, d) ,

FIGURE 7. Process for the line search for ρ, α.

constraint in the construction of motion primitives. For example, in this paper, the constraint for feedforward control force is set to be 0.7 Fz , and the feedback tire force limit is set to be 0.1 Fz . Thus, the total tire force will not exceed 0.8 Fz . On a dry road surface, the tire can generate about 0.9 Fz , which leaves a small margin of safety. In general, it should be emphasized that the force distribution should adapt to motion primitives and road conditions. B. FEEDBACK DESIGN AND FUNNEL CALCULATION

In order to ensure the accuracy of trajectory tracking, given the available force for feedback control, a funnel algorithm is developed. A funnel around the nominal trajectory gives a bound on the feedback error, which is used as a safety margin in the trajectory planning phase. The LQR funnel idea was first proposed by Tedrake et al. with Sum Of Squares (SOS) based funnels in [28]–[30]. The SOS based funnel can be used for both linear time varying and time-invariant systems. 9310

(25)

where d is the disturbance input, 1ξ denotes the error between the actual state trajectory and the nominal trajectory, and 1u denotes the feedback control input. Although the linear lateral-yaw model is a good approximation of the lateral dynamic, modeling uncertainty still exists. We write the error dynamic in the following form: ξ˙0 = A0 ξ0 + B0 u0 ξ˙0 + 1ξ˙ = (A0 + 1A) (ξ0 + 1ξ ) + (B0 + 1B) (u0 + 1u) + B1 d,

(26)

where A0 , B0 represents the nominal dynamics in the linear model and 1A and 1B are the model uncertainties. It is assumed that k1Ak ≤ MA ,

k1Bk ≤ MB , kB1 dk ≤ Md ,

(27)

where k·k denotes the induced matrix norm, which is defined as kMxk∞ kM k = sup . (28) x6=0 kxk∞ Ignoring the higher order terms, the error dynamics become ¯ 1ξ˙ = A0 1ξ + B0 1u + d, d¯ = 1Aξ0 + 1Bu0 + B1 d.

(29) VOLUME 5, 2017


The quadratic programming in Eq. (34) gives a conservative approximation of the region of attraction. Using SOS(Sum of Square) programming [30], a better approximation can be obtained through the following SOS programming: maximize γ ,

FIGURE 8. The feedback funnel for dmax = [0 0 0.035 0.01]T .

The new disturbance variable d¯ is bounded: d¯ ∈ D := d| kdk ≤ MA kξ0 k∞ + MB ku0 k∞ + Md (30) where MA and MB are the uncertainty bounds of A0 and B0 in the matrix norm, respectively; Md is the bound of the exogenous disturbance d. kξ0 k∞ and ku0 k∞ are the maximum state and input norm along the nominal trajectory. 2) FUNNEL COMPUTATION

For an LTI system, designing a feedback control using the LQR technique is simple. After solving the Riccati Equation, a feedback gain matrix K and cost-to-go matrix P can be obtained: PA0 + AT P − PB0 R−1 BT0 P + Q = 0 K = −R−1 BT0 P 1u = K 1ξ,

(31)

Then a quadratic Lyapunov function is formulated with the cost-to-go matrix: V = 1ξ T P1ξ.

(32)

For simplicity, in the remainder of this paper we denote the sublevel set 1ξ |1ξ T P1ξ ≤ c of the Lyapunov function as {V ≤ c} for some constant c. Proposition 1: If ∃ρ > 0, PA¯ + A¯ T P + P + M ρ P ≺ 0, where M=sup d¯ T Pd¯ , then all states outside the sublevel ¯ d∈D

set {V ≤ ρ} will eventually converge into the set assuming infinite input is available. Proof: see A.1 in Appendix. Since the constraint for ρ is monotone (for any ρ satisfying the condition, a larger ρ will also satisfy the condition), therefore the minimum ρ can be obtained by performing a line search. The line search requires only checking the negative definiteness of a matrix, and is fast to compute. One important limit for feedback control is the available force to use. umin ≤ K 1ξ ≤ umax (33) Eq. (33) is a linear constraint that results in a polytope of 1ξ . A straightforward way to find the largest sublevel set within the polytopic constraint is through quadratic programming. maximize 1ξ T P1ξ, 1ξ

subject to umax ≥ K 1ξ ≥ umin VOLUME 5, 2017

(34)

! 1ξ T PA + AT P 1ξ + 1ξ T PBumax subject to − T +uTmax BT P1ξ + 2d P1ξ + λ1 (umax − K 1ξ ) + λ2 γ − 1ξ T P1ξ T + λ3 M − d Pd SOS, λ1 , λ2 , λ3 SOS.

(35)

This SOS programming extends the region of attraction to the region where K 1ξ ≥ umax . That is, even when the feedback control force is saturated, the Lyapunov derivative is still negative in certain regions. λ1 , λ2 and λ3 are the SOS multipliers for the algebraic set using Parrilo’s hierarchy [31]. By adding the multipliers, the SOS condition for the polynomial is enforced only within an algebraic set. Denote γ := 1ξ T P1ξ . If γ > ρ, the region of attraction is the sublevel set {V ≤ γ }, and the error will eventually converge to the final sublevel set {V ≤ ρ}. This ‘‘robust stability’’ attribute against feedback saturation is a result of asymptotic convergence: the error will stay within the region of attraction but the convergence time is not guaranteed. A stronger argument can be obtained with convergence time guaranteed. However, the size of the final set will be larger. Proposition 2: Given the set of D and the region of attraction characterized by {V ≤ γ }, ∃ρ > 0, α > 0, ρ < γ such that the following are satisfied: M PA¯ + A¯ T P + P + P + αP ≺ 0 (36) ρ (37) (γ − ρ) /t < αρ, n o T where M = sup d Pd , then all states in {γ ≥ V > ρ} will d∈D

converge to the set {V ≤ ρ} within time t. Proof: see A.2 in Appendix. The search for α and ρ can start with the line search for ρ by setting α = 0, which is explained earlier. Then, gradually increase ρ and search for the largest α possible by checking the matrix negative definiteness in Eq. (36), until the condition shown in (37) is met. Where 1ρ0 and resolution are pre-set constants. The whole funnel computation process is illustrated in Figure 7. A sample computed funnel is shown in Figure 8. C. OFFLINE COMPUTATION

The funnel algorithm guarantees that all states within the funnel at the beginning will never leave the funnel during the execution of the trajectory, which gives a region of attraction for the closed-loop system. This makes the trajectory planning results for one initial condition able to be applied 9311


TABLE 1. Model parameters.

FIGURE 9. Lateral speed during the maneuver.

TABLE 2. The trim library.

FIGURE 10. Yaw rate during the maneuver.

to any initial conditions inside the region of attraction of the funnel. This enables offline computations, which further accelerates the computation for trajectory planning, and give some insight to the problem of pedestrian avoidance. The initial condition of trajectory planning is determined by six states. In addition to longitudinal speed and yaw rate determined by the trim state of the vehicle, the other four states are longitudinal distance lx , lateral position of the vehicle Y , yaw angle ψ and the lateral position of the pedestrian. A grid of initial conditions are selected, whose grid size is determined by the size of the region of attraction. The trajectory planning result for one grid point is applicable to all initial condition inside the region of attraction. The result of offline computation is shown in Section VII.D. VII. RESULTS A. MOTION PRIMITIVE CONSTRUCTION

As discussed in Section IV, trims are constructed by selecting the equilibrium states, and maneuvers are constructed by performing a series of quadratic programming. TABLE 1 shows the parameter of the D-class sedan model used in this paper, which is obtained from Carsim. These parameters determine the linear lateral-yaw model. Based on the criteria listed in Section IV.A, a set of trim equilibria is selected as shown in TABLE 2. Although at a lower speed the yaw rate can be pushed further, in light of the lane boundaries, having a large yaw rate is not viable. The step between different longitudinal velocities is chosen such that the braking maneuvers are of appropriate duration. Such a structure with the same yaw rates at different speeds also helps to simplify the construction of the maneuver. 9312

FIGURE 11. Steering angle during the maneuver.

The maneuver that links two trims with the same longitudinal speed at 20m/s with different yaw rates being 0.3rad/s and −0.3rad/s is selected as an example of a maneuver construction below: Trim1: vx = 20m/s, r = −0.3rad/s Trim2: vx = 20m/s, r = 0.3rad/s. The maneuver lasts for one unit duration Ts , which is 0.5s. The trajectory of the states and input are calculated with quadratic programming, as described in Figure 2 Figure 9 and Figure 10 show the trajectory of the states during the maneuver. The states at the starting and ending points of the maneuver are the equilibria of trim 1 and trim 2, respectively. Figure 11 shows the steering angle input of the maneuver. It is clear that the steering angle is constrained by the maximum change rate stated in (13), which is set as δ˙max = 40 deg /s ≈ 0.7rad/s. B. TRAJECTORY PLANNING RESULTS

Two sample scenarios are chosen to demonstrate the results obtained from the proposed trajectory planning algorithm. One is a swerving case; the other is a braking case. 1) SWERVING CASE

In the swerving case, the pedestrian is assumed to suddenly appear 10 meters in front of the vehicle, 2.2 meters left of VOLUME 5, 2017


FIGURE 12. Trajectory planning results for a swerving case.

FIGURE 14. Carsim simulation trajectory without feedback control.

FIGURE 13. Trajectory planning result for a braking case. FIGURE 15. Carsim simulation trajectory with feedback control.

the center of the lane (40cm to the lane edge). The vehicle was driving at 12.5m/s in the center of the lane. It is not possible to stop the vehicle in front of the pedestrian. The RRT found a trajectory that swerves to the left to avoid the collision. As shown in Figure 12, the circular danger zone grows over time. The color of the rectangles and the circles are indices of time. The red stars in the figure represent the nodes that have been expanded; that is, all of their child nodes are explored. The green triangles represent the nodes that are not yet expanded; the blue dots are the nodes that are selected as the nominal trajectory. 2) BRAKING CASE

In the braking case, the pedestrian suddenly appears 20 meters in front of the vehicle in the middle of the lane. Since the problem horizon is long, the danger zone grows so big that it blocks the whole lane width. Assuming that the vehicle is not allowed to leave the lane (which is an assumption that can be removed if lane change is allowed), there is not enough room to swerve to avoid a collision. The RRT finds a trajectory that use braking to stop the vehicle in time without hitting the pedestrian. C. TRAJECTORY TRACKING RESULT

The simulation with feedback controller uses Carsim as the vehicle model, with the trajectory planning results as the nominal trajectory. The vehicle is misaligned initially with ψ0 = 0.03rad, showing the effect of possible sensor error. Figure 14 shows the Carsim simulation trajectory without the feedback control. The initial yaw angle misalignment at ψ0 = 0.03rad creates a significant difference between the actual trajectory and the nominal one. Figure 15 shows the Carsim simulation trajectory with feedback control. The trajectory tracking error is significantly reduced. Figure 16 shows the error between the trajectory in Carsim and the desired trajectory. Compared to the simulation VOLUME 5, 2017

FIGURE 16. Feedback error of trajectory tracking.

without feedback, the closed-loop error is bounded. Though the feedback control is designed with a linearized model, the funnel algorithm is able to capture the modelling uncertainty and predict the error bound. D. OFFLINE COMPUTATION RESULTS

Figure 17 shows a set of sample offline computation results. Each green dot indicates that if a pedestrian suddenly appears at that position, the trajectory planning algorithm finds a solution. In other words, the green color indicates the safe region. In contrast, the red area is the area where trajectory planning fails to find a collision-free solution. Anywhere beyond 19 meters, which is the minimum distance for the vehicle to stop, is marked green, meaning that the vehicle can always avoid collision by braking. Now consider a typical sensor for pedestrian detection, a stereo camera used by a BOSCH driver assistance system [32]. It has a detection area with ±45 deg horizontal azimuth angle, and a maximum detection range of 50 meters [32]. The two dashed lines showed the azimuth angle of the camera. In reality, a pedestrian is expected to 9313


FIGURE 17. Offline RRT computation.

FIGURE 19. Offline computation results with lower velocity.

FIGURE 20. Solution involving lane change.

FIGURE 18. Offline results for y = 0.9m.

appear at the lane edge rather than in the middle of the lane. The offline computation results indicate that when the pedestrian is detected far away, the emergency RRT always finds a solution with braking; when the pedestrian appears close to the vehicle, the emergency RRT provides a swerving solution. In some cases, the vehicle needs to begin swerving before the pedestrian enters the lane. The offline computation results also show how the initial condition of the vehicle influences the existence of feasible solutions. Figure 18 shows that when the vehicle is not driving along the lane center, the infeasible area will lean to the opposite side of where the vehicle is driving. What this figure indicates is that assuming the controlled vehicle is passing a bus, where there is a chance a pedestrian might suddenly dash out from behind the bus, then it is wise to drive a short distance away from the bus. Figure 19 shows the RRT result at a lower velocity (10m/s). As expected, the safe region grows since the vehicle needs a shorter distance to stop. However, before the stopping distance, the lower velocity does not make the region where the vehicle can swerve to avoid the pedestrian grow. In other words, a pedestrian suddenly appearing close to the vehicle is still going to be hard to avoid. E. EFFECT OF LANE CHANGE

The proposed method can naturally generate solution involving lane change if the sensor detects that the adjacent lane is free. As shown in figure 20, simply by changing the lane 9314

FIGURE 21. Lane change with vehicle on adjacent lane.

edge constraint, emergency RRT generates solution using lane change in cases where no solution is found if lane change is forbidden. When there are other vehicles using the adjacent lane, the proposed method treats the adjacent vehicles as moving obstacles with fixed velocity, and finds a trajectory without collision. Note that the uncertainty of future motion of the adjacent vehicles can be considered under this framework, but it is omitted for simplicity. Figure 21 shows a scenario of lane change with another vehicle driving on the adjacent lane. The box filled with color shows the future position of the adjacent vehicle. F. WORST CASE PHILOSOPHY AND REAL-TIME IMPLEMENTATION

The offline computation results shown in this section is not very encouraging, which is mainly because of the worst case assumption, which greatly increased the area of danger and limited the trajectory of the vehicle. In reality, a pedestrian walking at one direction is not likely to change direction drastically. According to the result in [33], we further impose constraint on the acceleration of the pedestrian to be 0.21g. This changes the region of possible future position of the pedestrian. Figure 22 shows the pedestrian walking region with acceleration limit. The quiver shows the current velocity. As time grows, the shape becomes more similar to a circle. Figure 23 demonstrates the simulation with limited acceleration assumption on the pedestrian. The pedestrian is VOLUME 5, 2017


TABLE 3. Computation time of the trajectory planning methods.

FIGURE 22. Pedestrian walking region with acceleration limit.

motion of the pedestrian, a better solution may be found. However, because of the worst-case philosophy, any solution found is guaranteed to be safe regardless of the pedestrian’s future motion as long as the motion satisfies the assumption. G. COMPUTATION ASSESSMENT

The computation time of trajectory planning and funnel computation are assessed and compared to existing algorithms. All computation is done on a MATLAB platform installed on a laptop equipped with Pentium i7 processor @ 2.7GHz and 16GB RAM. FIGURE 23. Simulation of real-time implementation (swerving cases and braking cases).

FIGURE 24. Real-time simulation with static pedestrian.

discovered on the road side, and 8 different directions of walking are simulated. Circles with different color shows different motion of the pedestrian, and the corresponding vehicle trajectories are shown with the same color. Without the acceleration constraint, since the uncertainty grows with time and blocks the road, the Emergency RRT only gives a solution using braking when the vehicle is at the origin. When pedestrian acceleration constraint is added, we simulates the real-time situation by letting RRT run at every time step of motion primitive, i.e., The RRT will generate a new solution every Ts . According to the motion of the pedestrian, in 6 out of 8 cases, a swerving solution is given instead; braking is needed only when the pedestrian is walking towards the lane center. Figure 24 shows the simulation with a static pedestrian. There was no solution when the vehicle was at the origin. As a default solution, the vehicle slowed down. Since the pedestrian was not moving into the lane, the RRT then found a solution in the second time step and swerved to avoid the collision. This indicates that the solution given by RRT is the worstcase solution. In real-time implementation, based on the VOLUME 5, 2017

1) TRAJECTORY PLANNING COMPUTATION ASSESSMENT

The motion primitive based method is compared to another kinodynamic trajectory planning method, the MPC method. The MPC was based on nonlinear programming, which is realized by the MATLAB toolbox IPOPT. IPOPT can solve nonlinear programming using the interior point method. The time step for MPC was set to be Ts = 0.2s . For motion primitives, two time step settings are tested. The reason is that within one time step, the control input of a MPC generated trajectory is fixed. Therefore, the time step cannot be too big; otherwise, the trajectory will be too coarse. On the other hand, during each time step of a motion primitive, the input is changing and is already optimized offline, thus enabling the motion primitive method to use a longer time step. Both cases mentioned previously are chosen as test scenarios. As shown in TABLE 3, when Ts = 0.2s, the planning horizons are 1.2s and 2.6s for both the MP and MPC method: the motion primitive method takes a shorter time to compute. When the MP method uses Ts = 0.5s, the planning horizon becomes 1.5s and 3.0s in the two cases, which are multiples of Ts , and the computation is even faster. The major cost for the nonlinear MPC method is to calculate and invert the Hessian matrix, which grows cubically with the planning horizon. The complexity of the motion primitive method, on the other hand, grows linearly with the horizon. 2) FUNNEL COMPUTATION ASSESSMENT

The method for funnel computation proposed in this paper is applicable to LTI systems only. It is compared to the SOS based method, which is applicable to both LTI and LTV systems. The SOS programming is executed with SOSTOOLS on MATLAB, with Sedumi being the tool to solve semidefinite programming. The MATLAB Cholesky function was used to check matrix positive definiteness for the LTI funnel computation. 9315


For any point outside the sublevel set {V ≤ ρ}, the derivative of the Lyapunov function is always negative; therefore, it will eventually converge into the set.

TABLE 4. Assessment of the funnel computation.

B. PROOF OF PROPOSITION 2

Starting with the derivative of the Lyapunov function: V (t) − V (0) ! Z t ¯ 1ξ T PA1ξ + 1ξ T A¯ T P1ξ dt = T +d P1ξ + 1ξ T Pd 0 M T ¯ ¯ PA + A P + P + P + αP ≺ 0 ρ ∀1ξ ∈ {V ≥ ρ} , ⇒ V (0) − V (t) ≥ 1ξ (t)T P1ξ (t) α = αρt

(42)

(43)

Since

As shown in TABLE 4, the computation cost for the funnel is negligible. Comparing the computed results with the SOS programming results, no difference appears in Case 2, and a very small 5% difference in Case 1. VIII. CONCLUSION

An emergency RRT method is proposed that quickly search for a dynamically feasible trajectory that avoids the pedestrian by braking and steering. Together with the funnel algorithm proposed, the vehicle is guaranteed to avoid collision under the worst case. The simulation result indicates that when there is no accurate model of the pedestrians’ motion, braking is the most efficient way to ensure safety; when there exists model of the pedestrians’ motion, such as knowing the velocity and direction of walking, as shown in Section VII.F, steering solutions exist in many scenarios. APPENDIX A. PROOF OF PROPOSITION 1

The proof is based on the Lyapunov function and level sets. Taking the derivative of V , and substituting A + BK with A¯ T ¯ V˙ = 1ξ T PA1ξ + 1ξ T A¯ T P1ξ + d P1ξ T + 1ξ T Pd (38)

Note that (1ξ − D)T P (1ξ − D) = DT PD + 1ξ T P1ξ − DT P1ξ − 1ξ T PD ≥ 0 ⇒ DT P1ξ + 1ξ T PD ≤ DT PD + 1ξ T P1ξ.

(39)

Since D is bounded, an upper limit is defined as DT PD ≤ M .

(40)

Substituting (39) and (40) into (38): ¯ V˙ ≤ 1ξ T PA1ξ + 1ξ T A¯ T P1ξ + M + 1ξ T P1ξ n o ∀1ξ ∈ 1ξ |1ξ T P1ξ ≥ ρ , M T T ˙ ¯ ¯ (41) V ≤ 1ξ PA + A P + P + P 1ξ ≤ 0 ρ 9316

(γ − ρ) /t < αρ

(44)

V (t) ≤ V (0) − αρt ≤ V (0) − (γ − ρ) ≤ ρ

(45)

then

which means any initial error inside the region of attraction converges to the final sublevel set {V ≤ ρ} within time t. ACKNOWLEDGMENT

It is part of the Cyber-Physical System project participated in by the University of Michigan; University of California, Los Angeles; Texas A&M University; and Carnegie Mellon University. REFERENCES [1] NHTSA. (2015). NHTSA’s National Center for Statistics and Analysis, accessed on Aug. 28, 2014. [Online]. Available: http://wwwnrd.nhtsa.dot.gov/Pubs/811748.pdf [2] 2012 Annual Report of Chinese Road Traffic Accidents Statistics, Nat. Bureau Statist. China, Beijing, China, 2012. [3] ESQ—Toyota Develops New Pedestrian Safety Technology, Toyota, Aichi, Japan, 2013. [4] Ford Develops Test Car That Automatically Steers Around Stopped or Slowing Vehicles or Pedestrians | Ford of Europe | Ford Media Center, Ford, Dearborn, MI, USA, 2013. [5] Autonomous Emergency Braking | Euro NCAP—For Safer Cars Crash Test Safety Rating, Euro NCAP, 2016. [6] J. Borenstein and Y. Koren, ‘‘The vector field histogram-fast obstacle avoidance for mobile robots,’’ IEEE Trans. Robot. Autom., vol. 7, no. 3, pp. 278–288, Jun. 1991. [7] C. Fulgenzi, A. Spalanzani, and C. Laugier, ‘‘Dynamic obstacle avoidance in uncertain environment combining PVOs and occupancy grid,’’ in Proc. IEEE Int. Conf. Robot. Autom., Apr. 2007, pp. 1610–1616. [8] R. Bis, H. Peng, and G. Ulsoy, ‘‘Velocity occupancy space: Robot navigation and moving obstacle avoidance with sensor uncertainty,’’ in Proc. ASME Dyn. Syst. Control Conf., vol. 1. 2009, pp. 363–370. [9] O. Khatib, ‘‘Real-time obstacle avoidance for manipulators and mobile robots,’’ Int. J. Robot. Res., vol. 5, no. 1, pp. 90–98, 1986. [10] S. Shimoda, Y. Kuroda, and K. Iagnemma, ‘‘Potential field navigation of high speed unmanned ground vehicles on uneven terrain,’’ in Proc. IEEE Int. Conf. ICRA, Apr. 2005, pp. 2828–2833. [11] S. Karaman, M. R. Walter, A. Perez, E. Frazzoli, and S. Teller, ‘‘Anytime motion planning using the RRT∗ ,’’ in Proc. IEEE Int. Conf. Robot. Autom., May 2011, pp. 1478–1483. [12] J. H. Jeon et al., ‘‘Optimal motion planning with the half-car dynamical model for autonomous high-speed driving,’’ in Proc. Amer. Control Conf., Jun. 2013, pp. 188–193. VOLUME 5, 2017


[13] E. Frazz, M. A. Dahleh, and E. Feron, ‘‘Maneuver-based motion planning for nonlinear systems with symmetries,’’ IEEE Trans. Robot., vol. 21, no. 6, pp. 1077–1091, Dec. 2005. [14] E. Frazzoli, M. A. Dahleh, and E. Feron, ‘‘Real-time motion planning for agile autonomous vehicles,’’ J. Guid., Control, Dyn., vol. 25, no. 1, pp. 116–129, 2002. [15] E. Frazzoli, M. A. Dahleh, and E. Feron, ‘‘Robust hybrid control for autonomous vehicle motion planning,’’ in Proc. 39th IEEE Conf. Decision Control, vol. 1. Dec. 2000, pp. 821–826. [16] A. Gray, Y. Gao, T. Lin, J. K. Hedrick, H. E. Tseng, and F. Borrelli, ‘‘Predictive control for agile semi-autonomous ground vehicles using motion primitives,’’ in Proc. Amer. Control Conf. (ACC), Jun. 2012, pp. 4239–4244. [17] M. G. Plessen, D. Bernardini, H. Esen, and A. Bemporad, ‘‘Spatial-based predictive control and geometric corridor planning for adaptive cruise control coupled with obstacle avoidance,’’ IEEE Trans. Control Syst. Technol., pp. 1–13, 2017. [18] H. G. Bock and K.-J. Plitt, ‘‘A multiple shooting algorithm for direct solution of optimal control problems,’’ in Proc. IFAC World Congr., vol. 9. 1984, pp. 242–247. [19] D. A. Benson, G. T. Huntington, T. P. Thorvaldsen, and A. V. Rao, ‘‘Direct trajectory optimization and costate estimation via an orthogonal collocation method,’’ J. Guid., Control, Dyn., vol. 29, no. 6, pp. 1435–1440, Nov. 2006. [20] T. Schouwenaars, B. De Moor, E. Feron, and J. How, ‘‘Mixed integer programming for multi-vehicle path planning,’’ in Proc. Eur. Control Conf., Sep. 2001, pp. 2603–2608. [21] Y. Gao, A. Gray, J. Frasch, T. Lin, and E. Tseng, ‘‘Spatial predictive control for agile semi-autonomous ground vehicles,’’ in Proc. 11th Int. Symp. Adv. Veh. Control, 2012, pp. 1–6. [22] J. Ji, A. Khajepour, W. W. Melek, and Y. Huang, ‘‘Path planning and tracking for vehicle collision avoidance based on model predictive control with multiconstraints,’’ IEEE Trans. Veh. Technol., vol. 66, no. 2, pp. 952–964, Feb. 2017. [23] G. Ferrer and A. Sanfeliu, ‘‘Proactive kinodynamic planning using the extended social force model and human motion prediction in urban environments,’’ in Proc. IEEE/RSJ Int. Conf. Intell. Robots Syst., Sep. 2014, pp. 1730–1735. [24] C. F. Wakim, S. Capperon, and J. Oksman, ‘‘A Markovian model of pedestrian behavior,’’ in Proc. IEEE Int. Conf. Syst., Man Cybern., vol. 4. Oct. 2004, pp. 4028–4033. [25] T. Bandyopadhyay, C. Z. Jie, D. Hsu, M. H. Ang, Jr., D. Rus, and E. Frazzoli, Intention-Aware Pedestrian Avoidance. Cham, Switzerland: Springer, 2013, pp. 963–977. [26] J. Montufar, J. Arango, M. Porter, and S. Nakagawa, ‘‘Pedestrians’ normal walking speed and speed when crossing a street,’’ Transp. Res. Rec., J. Transp. Res. Board, pp. 90–97, Jan. 2007. [27] S. M. LaValle and J. J. Kuffner, ‘‘Randomized kinodynamic planning,’’ Int. J. Robot. Res., vol. 20, no. 5, pp. 378–400, 2001. [28] R. Tedrake, ‘‘LQR-Trees: Feedback motion planning on sparse randomized trees,’’ in Proc. Artif. Intell., 2009, p. 8. [29] R. Tedrake, I. R. Manchester, M. Tobenkin, and J. W. Roberts, ‘‘LQR-trees: Feedback motion planning via sums-of-squares verification,’’ Int. J. Robot. Res., vol. 29, no. 8, pp. 1038–1052, 2010. [30] A. Majumdar and R. Tedrake, ‘‘Robust online motion planning with regions of finite time invariance,’’ in Algorithmic Foundations of Robotics X. Berlin, Germany: 2013, pp. 543–558. [31] P. A. Parrilo, ‘‘Semidefinite programming relaxations for semialgebraic problems,’’ Math. Program., vol. 96, pp. 293–320, May 2003.

VOLUME 5, 2017

[32] Driver Assistance Systems—Road Sign Recognition—Stereo Video Camera, Multi Purpose Camera, BOSCH, Stuttgart, Germany. [33] T. Fugger, B. Randles, A. Stein, W. Whiting, and B. Gallagher, ‘‘Analysis of pedestrian gait and perception-reaction at signal-controlled crosswalk intersections,’’ Transp. Res. Rec., J. Transp. Res. Board, vol. 1705, pp. 20–25, Jan. 2000.

YUXIAO CHEN received the bachelor’s degree in mechanical engineering from Tsinghua University, Beijing, China, in 2013. He is currently pursuing the Ph.D. degree with the University of Michigan. His research interests are control theory, cyber physical system, and particularly the application in vehicle control.

HUEI PENG received the Ph.D. degree in mechanical engineering from the University of California at Berkeley, Berkeley, in 1992. He is currently a Professor with the Department of Mechanical Engineering, University of Michigan. His research interests include adaptive control and optimal control, with an emphasis on their applications to vehicular and transportation systems. His current research focuses include the design and control of electrified vehicles, and connected/automated vehicles. He has been an active member of the Society of Automotive Engineers (SAE) and the American Society of Mechanical Engineers. He is both an SAE Fellow and an ASME Fellow. He is a ChangJiang Scholar at the Tsinghua University of China.

JESSY W. GRIZZLE (F’97) received the Ph.D. in electrical engineering from The University of Texas at Austin in 1983. He is currently a Professor of Electrical Engineering and Computer Science with the University of Michigan, where he holds the titles of the Elmer Gilbert Distinguished University Professor and the Jerry and Carol Levin Professor of Engineering. He jointly holds 16 patents dealing with emissions reduction in passenger vehicles through improved control system design. He is a fellow of the IFAC. He received the Paper of the Year Award from the IEEE Vehicular Technology Society in 1993, the George S. Axelby Award in 2002, the Control Systems Technology Award in 2003, the Bode Prize in 2012, and the IEEE TRANSACTIONS on CONTROL SYSTEMS TECHNOLOGY Outstanding Paper Award in 2014. His work on bipedal locomotion has been the object of numerous plenary lectures and has been featured on CNN, ESPN, Discovery Channel, The Economist, Wired Magazine, Discover Magazine, Scientific American, and Popular Mechanics.

9317