dscc2014-6269 - final

Proceedings of the ASME 2014 Dynamic Systems and Control Conference DSCC2014

October 22-24, 2014, San Antonio, TX, USA

DSCC2014 - 6269

UNCLASSIFIED: Distribution Statement A. Approved for public release. #24545 1

A MULTI-STAGE OPTIMIZATION FORMULATION FOR MPC-BASED OBSTACLE AVOIDANCE IN AUTONOMOUS VEHICLES USING A LIDAR SENSOR

Jiechao Liu Department of Mechanical Engineering

University of Michigan Ann Arbor, MI, 48109

[email protected]

Paramsothy Jayakumar U.S. Army RDECOM-TARDEC

Warren, MI 48397 [email protected]

Jeffrey L. Stein

Department of Mechanical Engineering University of Michigan Ann Arbor, MI, 48109

[email protected]

Tulga Ersal* Department of Mechanical Engineering

University of Michigan Ann Arbor, MI, 48109

[email protected] ABSTRACT

The dynamics of an autonomous unmanned ground vehicle (UGV) that is at least the size of a passenger vehicle are critical to consider during obstacle avoidance maneuvers to ensure vehicle safety. Methods developed so far do not take vehicle dynamics and sensor limitations into account simultaneously and systematically to guarantee the vehicle’s dynamical safety during avoidance maneuvers. To address this gap, this paper presents a model predictive control (MPC) based obstacle avoidance algorithm for high-speed, large-size UGVs that perceives the environment only through the information provided by a sensor, takes into account the sensing and control delays and the dynamic limitations of the vehicle, and provides smooth and continuous optimal solutions in terms of minimizing travel time. Specifically, information about the environment is obtained using an on-board Light Detection and Ranging (LIDAR) sensor. Ensuring the vehicle’s dynamical safety is translated into avoiding single tire lift-off. The obstacle avoidance problem is formulated as a multi-stage optimal control problem with a unique optimal solution. To solve the optimal control problem, it is transcribed into a nonlinear programming (NLP) problem using a pseudo-spectral method, and solved using the interior-point method. Sensing and control delays are explicitly taken into consideration in the formulation. Simulation results show that the algorithm is capable of generating smooth control commands to avoid obstacles while guaranteeing dynamical safety.

* Corresponding author

I. INTRODUCTION Unmanned ground vehicles (UGVs) are gaining

importance and finding increased utility in both military and commercial applications. Although earlier UGV platforms were typically exclusively small ground robots, recent efforts have targeted passenger vehicles and larger size platforms, as well. For this size of vehicles, it becomes especially important to take the dynamic limitations of the vehicle into account to guarantee dynamical safety during obstacle avoidance maneuvers. Therefore, obstacle avoidance algorithms are needed that can ensure vehicle safety even if the vehicle is operating at its dynamic limits.

Many obstacle avoidance algorithms have been developed in the literature that allow for fast, continuous, and smooth motion of UGVs among unexpected obstacles. They can be classified into four categories: graph-search based methods [1, 2], virtual potential field and navigation function based methods [3, 4], meta-heuristic based methods [5], and mathematical optimization based methods [6, 7]. Among these categories, mathematical optimization based methods are particularly attractive, because they offer a rigorous and systematic way to take vehicle dynamics and safety constraints into account, and can generate optimal control inputs.

A mathematical optimization approach can be used either in open-loop, if the environment is fully known a priori, or in closed-loop with a feedback controller for a more robust solution. Regarding the latter, the Model Predictive Control (MPC) approach is one of the most widely adopted techniques. Prior research has demonstrated successful applications of


MPC to obstacle avoidance in UGVs [6-16]. Some active safety methods leverage the MPC framework, as well, to ensure, for example, safe lane keeping or vehicle stability [17-21].

The first applications of MPC to obstacle avoidance in UGVs assumed that the controller has full knowledge about the environment. They also were not concerned with the level of fidelity that the model used by the controller needs to possess for satisfactory performance, where the performance criteria in some cases also include the dynamical safety of the vehicle, such as no tire lift-off. Previous work by the authors aimed to address this gap by developing an MPC formulation that takes into account the information about the environment as provided by the on-board LIDAR (Light Detection and Ranging) sensor [22]. They also investigated the role of model fidelity and showed that the vehicle’s dynamical safety can be guaranteed by limiting the steering angle using a high fidelity model, which then allows the MPC to work with a low fidelity model for trajectory optimization [22]. However, this investigation was done using an exhaustive search approach on a coarse mesh of the control input; hence, optimality and a smooth operation were not ensured. Also, sensor and controller delays were not considered.

The goal of this paper is to provide a novel MPC formulation for obstacle avoidance in UGVs that can achieve an optimal, smooth operation of the vehicle through the obstacle field while guaranteeing vehicle safety, minimizing travel time, and taking sensor and controller delays into account. This is done by formulating the obstacle avoidance problem into a multi-stage optimal control problem, which is then converted into a nonlinear programming (NLP) problem and solved using the interior-point method. The model fidelity question is revisited, and it is shown that a 2 degrees-of-freedom (DoF) model is sufficient for the context considered. Simulation results are given to demonstrate the performance of the developed framework under various sensing and control delays.

The rest of the paper is organized as follows. Section II presents the MPC-based obstacle avoidance formulation, including the cost function and constraints, vehicle models, and solution techniques used. Section III presents and discusses the simulation results. Conclusions are drawn in Section IV.

II. OBSTACLE AVOIDANCE FORMULATION MPC is an optimal control-based state-feedback control

technique. The feedback law is obtained by an iterative online optimization over a finite moving prediction horizon [23]. Fig.1 illustrates at a high level how MPC is utilized in this work for obstacle avoidance purposes. The three main components of the MPC-based obstacle avoidance algorithm are the vehicle model, the cost function and constraints, and the dynamic optimizer. The equations describing the vehicle dynamics, together with the cost function and constraints, are formulated into an optimal control problem. The dynamic optimizer is then used to solve this problem.

The inputs to the obstacle avoidance algorithm are the target location, obstacle information, and estimated vehicle states. Within the scope of this paper, the target location is directly specified by the user, but it could also be generated by a high-level global path planning algorithm.

The obstacle information is obtained from a model of a 2D LIDAR sensor, which provides information about range and geometrical characteristics of the closest objects to the vehicle. A 2D sensor is modeled based on the assumption that all obstacles are at least the height of where the LIDAR is mounted on the vehicle, which is in front of the vehicle. The LIDAR returns the distance to the closet obstacle boundary in each radial direction at an angular resolution of e . The angular range is [ ]0 ,180 , with the vehicle heading direction being the 90 direction. For a direction without obstacles within the detection range, the LIDAR returns the maximum detection range LIDARR . Fig. 2 shows an obstacle field with two obstacles and the output of the LIDAR for the particular vehicle pose.

The vehicle states are required to properly initialize the vehicle model used in the algorithm. In a real application, a state estimator is needed to estimate the states, since not all states can be directly measured. However, in this paper, the UGV is simulated; therefore, full state information is available and the state estimator is not needed.

FIGURE 1: SCHEMATIC OF THE MPC-BASED OBSTACLE AVOIDANCE ALGORITHM

(A) (B)

FIGURE 2: (A) A SAMPLE OBSTACLE FIELD AND THE LIDAR DETECTION DATA PLOTTED IN THE 3D SPACE; (B) THE

TOP VIEW OF LIDAR DATA AND THE SAFE AREA

VehicleModel

Cost Function& Constraints

DynamicOptimizer

Model Predictive Control

Obstacle Information Sensor Environment

MeasuredOutputs

Control Commands

Estimated States State

Estimator

AGV

Target Location

y (m)

x (m)

Obstacle

Obstacle

LIDAR detection data

Vehicle position

Vehicle heading anglex (m)

y (m

)

Safe Area

l(x,y)

GoalsT

[x(0), y(0)]

[x(Tp), y(Tp)]

[xg, yg]


The outputs of the obstacle avoidance algorithm are the control signals for the UGV. In general, both the vehicle heading angle and speed could be controlled by the algorithm. However, for simplicity, it is assumed in this paper that the vehicle speed is maintained constant, and the front wheel steering rate is considered as the only control signal.

The rest of this section describes the optimal control problem formulation and the solution procedure used in the dynamic optimizer in detail.

II.A. Optimal Control Problem Formulation The multi-stage optimal control problem to be solved at

each step of the MPC is given by Eq. (1) - (7)

Minimize 1TJ s w d= + (1)

Subject to ( ) ( ) ( )( ),t f t tx x z= (2)

( ) 00x x= (3)

( ) ( ),max 0f ft Ud d£ (4)

( ) ,maxftV V£ (5)

( ) ( )( ), 0x t y t £ (6)

0, pt Té ùÎ ê úë û (7)

Before we define the variables and explain the problem formulation in detail, let us go through the formulation at a high level. Eq. (1) specifies the cost function. Eq. (2) is the dynamic model of the vehicle represented as a set of first order ordinary differential equations (ODEs). Eq. (3) states the initial conditions of the vehicle. Eq. (4) and (5) represent the bounds on the steering angle and steering rate, respectively. Eq. (6) defines the position constraints due to the obstacles perceived by the LIDAR sensor, and Eq. (7) is the prediction horizon over which the optimal control problem is solved.

In the remainder of this subsection, this optimal control problem formulation is explained in detail.

II.A.1. Cost Function and Constraints The cost function and constraints need to be specified to

accomplish the objective of avoiding locally detected obstacles while guaranteeing the vehicle’s dynamical safety and minimizing the travel time.

Cost function. The cost function defines the soft requirement; i.e., in what sense the trajectory is optimal. In this work, the cost function is defined as

1TJ s w d= + (8)

where

( )( ) ( )( )2 2

T g p g ps x x T y y T= - + - (9)

( ) ( )2 22

0 d

pT

f fd t w t tV dé ù= +ê úë ûò (10)

Specifically, the cost function formulation includes two terms that are linearly combined using a relative weight 1w . The first term is the distance Ts between the end point of the predicted trajectory ( ), ( )p px T y Té ùê úë û and the goal ,g gx yé ùê úë û as defined in Eq. (9). A visual representation of this term is shown in Fig. 2b. Due to the constant speed assumption in this work, this term also aims to minimize the remaining travel time. The second term is a regulation term minimizing the control effort d as defined in Eq. (10), where fV is the steering rate, which is the control command to be optimized, fd is the front wheel steering angle, and 2w is a weight.

Constraints. The constraints represent the hard requirements of avoiding collision and ensuring the vehicle’s dynamical safety. These requirements are hard in the sense that their violations are not allowed under any circumstances.

To avoid collision with obstacles, the vehicle should completely lie within the safe area detected by the LIDAR as exemplified in Fig. 2b. The safe area can be defined using inequalities compacted in the following form

( ) ( )( ), 0, 0, px t y t t Té ù£ " Î ê úë û (11)

The next question is then how to specify the safe area. It is very difficult, if not impossible, to use a single function to define the safe area. Moreover, because the area is typically non-convex, multiple local optimal solutions may exist, and the optimization algorithm may converge to a different one depending on the initial solution. Ideally, however, the goal is to find the global optimal solution of the problem.

To address these challenges, the safe area is divided into sectors and triangles, assuming the edges of the obstacles are straight. Each of these regions can then be specified using a set of linear inequalities. For instance, Fig. 3 shows an example scenario with multiple obstacles and the safe area detected by the LIDAR, which is divided into sectors and triangles. As an example, Region 5 is a triangle, which can be specified using 3 linear inequalities, whereas Region 9 is a sector, which is bounded by two lines. The third boundary of Region 9 is an arc; however, because of the limit on prediction time, this arc constraint will never be active and thus is ignored.

FIGURE 3: THE SAFE AREA IS DIVIDED INTO SECTORS AND TRIANGLES

x (m)

y (m

)

123

4

5

6

78

9

1112

vehicle

10


Because the turning radius of the vehicle is limited, the vehicle needs to go through the regions sequentially. E.g., the vehicle cannot move into Region 9 without going through Regions 5, 6, 7, and 8 first. Note that Region 5 is the first region to traverse, because the vehicle heading at the current position is along the positive y-direction. Furthermore, the end of the trajectory needs to lie within a feasible sector to avoid the obstacles and move towards the goal. In Fig. 3, e.g., Regions 4, 6, and 9 are the feasible sectors. Regions 1 and 12 are infeasible, because the end of the trajectory cannot lie within these two regions due to the maximum steering angle limitation. Thus, there will be three local optimal trajectories, one for each of the three feasible regions. For each of these regions, a multi-stage optimal control problem is formulated and solved, leading to a unique solution. Solutions of those problems are compared to find the global optimal solution. The details of this solution strategy will be explained in a later section; here, the focus is on the formulation of the constraints. Referring back to Fig. 3 as an example, if the problem of having the vehicle move from Region 5 to 4 is considered, then Eq. (11) consists of two parts as follows

( ) ( ) [ ]1 1 1 10, 1,2,3, 0,i i ia x t b y t c i t T+ + £ = Î (12)

( ) ( )2 2 2 10, 1,2, ,j j j pa x t b y t c j t T Té ù+ + £ = Î ê úë û (13)

Eqs. (12) and (13) define Regions 5 and 4, respectively. In addition to the control command, which is the steering rate sequence, ( ), 0,f pt t TV é ùÎ ê úë û , the transition time 1T from Region 5 to 4 also needs to be optimized.

The second type of constraint is related to the dynamical safety of vehicle. In this study, ensuring the vehicle’s dynamical safety is translated to avoiding single tire lift-off. This is a conservative criterion used to prevent rollover [24]. This requirement could be taken directly into account as an inequality constraint to enforce a nonzero vertical load on all four tires at all times. However, vehicle models that could predict the vertical tire loads on all four tires would require a level of complexity whose computational load would be prohibitively high for the purposes of MPC. Therefore, another conservative approximation of the dynamical safety requirement is considered in this work; namely, an upper bound on the steering angle magnitude as expressed by the following inequality constraint

( ) ( ),max 0f ft Ud d£ (14)

where the maximum steering angle ,maxfd is a function of the vehicle speed 0U . There exist other factors that can affect the maximum steering angle, such as the slope of the terrain, or the location of the center of gravity (CoG) of the vehicle. However, in this study, the vehicle is assumed to move on a constant- friction flat surface. Therefore, the maximum steering angle is only a function of vehicle speed. For speeds ranging from 10 m/s to 30 m/s, the relationship between the maximum steering angle and speed is shown in Fig.4, which is obtained offline

using a 14 DoF vehicle model, which will be described in the following subsection.

II.A.2. Vehicle Models Vehicle Dynamics. Two different vehicle dynamics

models are considered in this work: a 14 DoF model to represent the plant and to generate offline the dynamical safety related look-up tables used in the MPC, and a 2 DoF model used in the MPC to predict trajectories. The schematics of the two representations are given in Fig. 5. The 14 DoF model consists of a single sprung mass connected to four unsprung masses. The suspensions between the sprung mass and unsprung masses are modeled as spring-damper systems. In the 2 DoF model, the left and right tires on each axle are lumped together. The equations for the 14 DoF model are omitted here for space limitations, but can be found in the literature [25]. This 14 DoF vehicle model correlates very well with models in TRUCKSIM and ADAMS/Car.

FIGURE 4: MAXIMUM STEERING ANGLE AS A FUNCTION OF VEHICLE LONGITUDINAL SPEED

(A) 2 DOF VEHICLE MODEL

(B) 14 DOF VEHICLE MODEL

FIGURE 5: SCHEMATICS OF THE TWO DOF VEHICLE MODEL AND THE FOURTEEN DOF VEHICLE MODEL

10 15 20 25 300

2

4

6

8

10

12

Longitudinal speed (m/s)

Max

imum

ste

erin

g an

gle

(°)

LfLr

U0

Vβ

αf

αrδf

rFyfFyr

x

y

Lf

Lr

Lt

mfl

mfr

mrr

mrl

MFRONT

U V

Wωx

ωy

ωz

ωfr

ωrl

ωfl

ωrr

Ug Vg

Wg

ωgx

ωgy

ωgz

FA

FB

δf

δf


The 2 DoF model is described by the following ODEs

( ) 0yf yrV F F M U r= + - (15)

( )yf f yr r zzr F L F L I= - (16)

ry= (17)

0 cos sinx U Vy y= - (18)

0 sin cosy U Vy y= + (19)

where yfF and yrF are tire lateral forces generated at the front axle and the rear axle, respectively, 0U is the longitudinal speed in the vehicle’s coordinate, V is the lateral speed in the vehicle’s coordinate, r is the yaw rate, y is the yaw angle, ( ),x y is the vehicle’s location in global coordinates, M is the vehicle mass, zzI is the moment of inertia, fL is the distance between the front axle and the vehicle’s CoG location and rL is the distance between the rear axle and the vehicle’s CoG location.

Tire Model. Two different tire models are considered to predict the tire lateral forces; namely, the Pacejka Magic Formula tire model [26] and the linear tire model assuming a constant cornering stiffness. For the 14 DoF model, the Pacejka tire model is used to model the longitudinal and lateral behaviors of the tires, and tire vertical behaviors are represented as linear springs without damping. The 2 DoF model is evaluated with both the pure-slip Pacejka tire model and the linear model.

The nonlinear tire model for the 2 DoF model can be summarized by the following equations

( ),yf y zf fF MF F a= (20)

( ),yr y zr rF MF F a= (21)

where zfF and zrF are vertical loads at front axle and rear axle, respectively. The tire slip angles fa and ra are given by

( )10tanf f fV L r Ua d- é ù= + -ê úë û (22)

( )10tanr rV L r Ua - é ù= -ë û (23)

The exact form of the Pacejka tire model can be found in [26]. Fig. 6a shows the relationship between the tire lateral force and the slip angle at different vertical loads described by the Pacejka tire model.

As for the linear tire model, the constant cornering stiffness ( )zC Fa⋅ ⋅ is found as the slope at the origin for different tire

vertical loads.

( )yf f zf fF C Fa a= (24)

( )yr r zr rF C Fa a= (25)

Fig. 6b shows the comparison between the linear tire model and the Pacejka tire model at a fixed tire vertical load.

The two models agree well when the slip angle is less than 5 degrees.

Longitudinal Load Transfer. Tire vertical load is required as an input to both tire models. A typical assumption with the 2 DoF model is that there is no longitudinal load transfer, so that the vertical loads on each axle are constant. However, for the purpose of this work, it is important to account for the longitudinal load transfer in the 2DoF vehicle model when the vehicle travels at high speed. Thus, the following relationships are used with the 2 DoF model to calculate the vertical loads on the front and rear axles [27] taking into account the longitudinal load transfer effects:

( ) ( )zf r CG f rF MgL M U V r h L Lé ù= - - ⋅ +ê úë û (26)

( ) ( )zr f CG f rF MgL M U V r h L Lé ù= + - ⋅ +ê úë û (27)

where CGh is the height of vehicle CoG location above the ground.

The longitudinal load transfer effects are inherently included in the 14 DoF model.

Control Input. The steering rate fV is used as the control command to be optimized. The steering rate instead of the steering angle fd is preferred to obtain a smoother control command sequence. The corresponding state equation is given as

f fd V= (28)

In summary, the 2 DoF vehicle model with the Pacejka tire model are given by Eq. (15-23, 26-28) and the 2 DoF vehicle model with the linear tire model are given by Eq. (15-19, 22-28). In a compact form, the vehicle model can be written as

( ) ( ) ( )( ), ft f t tx x V= (29)

where the states are fx y V rx y dé ù= ê úë û and the control command is fV . The limit on the steering rate is given as

( ) ,maxftV V£ (30)

(A) (B)

FIGURE 6: (A) LATERAL TIRE FORCE DESCRIBED BY THE PACEJKA TIRE MODEL (B) COMPARISON OF THE LINEAR

AND THE PACEJKA TIRE MODELS

0 -20 -40 -60 -800

5

10

15

Slip angle (°)

Late

ral f

orce

(kN

) FZ = 20 kN

FZ = 16 kN

FZ = 12 kNFZ = 8 kNFZ = 4 kN

−10 −5 0 5 10−10

−5

0

5

10

Slip angle (°)

Late

ral f

orce

(kN

)

Nonlinear Tire ModelLinear Tire Model


Model Validity. The 14 DoF vehicle model is used as the plant in the simulation, whereas the MPC uses the 2 DoF model. Thus, it is of interest to study how close the trajectory predictions with the 2 DoF are to the predictions with the 14 DoF model.

Using the steering profile in Fig. 7a as the input and considering a vehicle speed of 30 m/s, the trajectories predicted by the 2 DoF vehicle model with the linear and Pacejka tire models are compared to the trajectory of the 14 DoF vehicle model with the Pacejka tire model in Fig. 7b. The 2 DoF vehicle model with Pacejka tire model is a very good approximation to the 14 DoF vehicle model under this test condition. The prediction of the 2 DoF vehicle model with the linear tire model is not as good, because the slip angles are larger than 5 degrees and hence the differences between the linear and Pacejka tire models are significant. Thus, 2 DoF vehicle model with Pacejka magic formula tire model is used in the MPC to predict trajectories.

II.A.3. Sensing Delay and Control Delay In a real system, there may be several locations where time

delays are introduced, such as sensor data acquisition delays, sensor data transmission delays, sensor data processing delays, algorithm computation delays, command transmission delays, or actuator delays. In this work, such delays are lumped into two groups: the sensing delay and the control delay. The sensing delay means that the LIDAR data used at the current time is from dsT seconds ago. The control delay means that the plant will not execute the command computed at the current time until dcT seconds later. In this work, these delays are assumed to be constant and known and are handled by the algorithm as described below.

LIDAR data are processed to formulate the constraints on the vehicle’s position to meet the no-collision requirement. Note that the output of the LIDAR, i.e., the distance between the LIDAR and the closet obstacle boundary in each radial direction, should be used in reference to the vehicle’s pose where the scan is performed. If the algorithm is not aware of

the sensing delay, the LIDAR data will be processed with respect to the vehicle’s pose at the current time, which may result in a wrong formulation of the safe area as illustrated in Fig. 8. If the algorithm is aware of the sensing delay, the LIDAR data from dsT seconds ago can be interpreted according to the current pose of the vehicle as also illustrated in Fig. 8. The interpreted sensor data are used to construct the equations defining the safe area.

With a control delay of dcT , the control command over the horizon ( )0 0, dct t T+ is from the previous step, where 0t is the current time. The vehicle states after applying this command are predicted using the 14 DoF vehicle model for a higher accuracy, and the planning of the next control commands starts from the predicted vehicle position at 0 dct T+ instead of 0t . That is, if the control delay is not taken into consideration, the vehicle initial states for the planning are given by Eq. (31). Otherwise, the vehicle initial states are given by Eq. (32).

( )0 0tx x= (31)

( )0 0 dct Tx x= + (32)

II.A.4. Prediction Horizon A lower bound on the prediction time must be imposed as

follows to ensure that obstacle avoidance maneuver is performed early enough:

( ),min 0p p eT T U T³ + (33)

The lower bound consists of two parts. The first part corresponds to the minimum time for the vehicle to make a 90turn to avoid hitting an obstacle. It is a function of vehicle speed as depicted in Fig. 9, which is created by simulating the 14 DoF vehicle model. The second part is the execution time, which is the duration of the computed command that is implemented.

A second inequality involving the prediction horizon is introduced to ensure that all the predicted trajectories lie within the LIDAR detection range. This inequality is given below and specifies the trade-off between the vehicle longitudinal speed and LIDAR detection range:

( )0 p ds dc LIDARU T T T R+ + £ (34)

(A) (B)

FIGURE 7: (A) STEERING ANGLE PROFILE USED TO COMPARE VEHICLE RESPONSES (B) COMPARISON OF

TRAJECTORIES

FIGURE 8: THE EFFECT OF SENSING DELAY

0 2 4 6 8 10−3−2−10123

Time (s)

Stee

ring

angl

e (°

)

0 50 100 150 200 2500

50

100

150

x (m)

y (m

)

2 DoF Linear2 DoF Pacejka14 DoF Pacejka

Chassis DoF Tire Model

Vehicle current position

Vehicle position where

LIDAR data is obtainedOriginal LIDAR data

LIDAR data used if delay is not accounted for

LIDAR data used if delay is accounted for


II.A.5. Number of Stages The exact number of stages N in the multi-stage optimal

control problem corresponds to the regions the vehicle needs to traverse. Besides the steering rate sequence, 1N - time points corresponding to transition times between regions are also optimized. Fig. 10 shows the flowchart of the MPC-based obstacle avoidance algorithm.

II.B. Dynamic Optimizer The task of the dynamic optimizer is to solve the optimal

control problem formulated above. To this end, a direct method [28] called the pseudo-spectral method is used in this work to transcribe the optimal control problem into a non-linear programming (NLP) problem, which is then solved using the interior point method.

In a pseudo-spectral method, the continuous functions are approximated at a set of selected quadrature nodes. The quadrature nodes are determined by the corresponding orthogonal polynomial basis functions used for the approximation. In this work, the Gauss pseudo-spectral method is used as implemented in GPOPS-II [29, 30] to solve the

optimal control problem in this MPC-based obstacle avoidance algorithm.

To solve the NLP problem, a primal-dual interior-point algorithm with a filter line search method implemented in IPOPT [31] is used. The basic idea of the interior point method is to decompose the NLP problem with both equality and inequality constraints into a sequence of equality constrained problems by introducing a barrier function and barrier parameter. The NLP problem with only equality constraints can then be solved iteratively. The search direction is determined using the Newton-Raphson method and the step size is obtained using the backtracking line search.

III. SIMULATION RESULTS AND DISCUSSION Four sets of simulations are conducted in this section, each

with a different purpose.

The first set of simulations is used to verify the developed algorithm and to evaluate its performance under three different scenarios: no delays, uncompensated delays, and compensated delays. All three simulations are run with a vehicle speed of

0 20 m/sU = , maximum steering rate of ,max 20 /sfV = , execution horizon of 0.5 seT = , and prediction horizon of

6.5 spT = . The LIDAR range is set to 140 mLIDARR = regardless of whether there are delays or not. For the simulations with delay, the sensing and control delays are set to

0.1 sdsT = and 0.2 sdcT = , respectively.

The results for this first set of simulations are summarized in Table 1. Figs. 11 and 12 show the vehicle trajectories and steering angle profiles. Fig. 13 shows the vehicle vertical loads in Test 1 when there are no delays. It can be seen that the vehicle is free from tire lift-off when the steering angle is bounded within the maximum allowed value. Results for the delayed scenarios are similar and are thus omitted for brevity.

These results show that the developed algorithm can successfully navigate the vehicle through an example obstacle field even in the presence of delays. The algorithm may safely navigate the vehicle even without any delay compensation; however, the performance and the steering profile are closer to the no-delay scenario when the delays are compensated.

To further investigate the effect of delays two additional sets of simulations are conducted. In the first case, the sensor delay is set to zero and the control delay is varied, whereas in the second set the control delay is set to zero and the sensing delay is varied. A longer LIDAR detection range

150 mLIDARR = is used to compensate longer delays according

FIGURE 9: MINIMUM PREDICTION HORIZON

FIGURE 10: FLOWCHART OF THE MPC-BASED OBSTACLE AVOIDANCE ALGORITHM

TABLE 1: SETTINGS AND RESULTS OF THE FIRST SET OF SIMULATIONS

Sensing delay, dsT (s) 0.0 0.1 0.1

Control delay, dcT (s) 0.0 0.2 0.2 Delay compensation - No Yes

Time to target (s) 30.7 32.2 30.8

10 15 20 25 304

5

6

7

8

9

Longitudinal velocity (m/s)

Min

imum

pre

dict

ion

Tim

e (s

)

control problem

Get LIDAR detection data

Extract information from sensor data

Formulate optimal

Compare the solutions andfind the global optimal solution

Apply the calculated

Targetarrived?

Task finished

Yes

No

……Solve optimal

control problem

control commands

FeasibleOpening #1

Solve optimal control problem

FeasibleOpening #N

Optimalsolution #1

Optimalsolution #N


to Eq. (34).

The results of these two sets of simulations are summarized in Tables 2 and 3, respectively. The scenarios for which no time-to-target value is reported are the scenarios in which the algorithm cannot avoid the obstacles. Note that all those cases correspond to the uncompensated delay scenarios. When the delays are compensated, the algorithm can safely navigate the vehicle for all delay values considered. This indicates that the proposed compensation method increases the robustness of the algorithm.

With the MPC, even though the optimization problem is solved for a 6.5 second ahead prediction, only 1/13 of the solution is applied to the vehicle and a new trajectory is planned every 0.5 second using the updated vehicle state information. This prediction-correction characteristic allows the MPC to tolerate delays.

It is interesting to note that the algorithm can navigate the vehicle safely for both 1 sdsT = and 1 sdcT = scenarios even when the delays are not compensated. This is because the environmental information available from the LIDAR sensor changes depending on the vehicle position and pose, and these high delay cases just happen to result in vehicle trajectories that allow the sensor to give a better view of the local environment. Thus, a robust performance cannot be achieved without a delay compensation in the algorithm. Fig. 14 compares four trajectories, where two of them are cases of hitting the obstacle when delay is uncompensated and the other two are the corresponding simulations with compensated delay.

The fourth set of simulations is conducted to evaluate the algorithm with different vehicle speeds and sensor ranges. No delays are considered in this set of simulations. Table 4 summarizes the considered settings and obtained results and Fig. 15 compares the resulting trajectories. These results show that the developed algorithm is capable of working across different vehicle speeds and sensor ranges, as well. Ultimately, the maximum sensor range dictates the safe maximum velocity, and the algorithm can accomondate different sensor ranges successfully.

IV. SUMMARY AND CONCLUSIONS This paper presents the development of an MPC-based

framework for obstacle avoidance in large UGVs with significant vehicle dynamics. A multi-stage optimal control problem formulation is used to incorporate the data from the on-board LIDAR sensor and the dynamic limitations of the vehicle and to achieve a unique optimal solution. The resulting problem is solved using a pseudo-spectral method to transform the continuous-time optimization problem into an NLP, which is then solved using the interior-point method. The conclusion is that the developed method can yield a satisfactory performance at various sensing and control delays and vehicle

FIGURE 11: VEHICLE TRAJECTORIES FOR THE FIRST SET OF SIMULATIONS

FIGURE 12: VEHICLE STEERING ANGLE PROFILES FOR THE FIRST SET OF SIMULATIONS

FIGURE 13: VEHICLE VERTICAL LOADS FOR THE NO DELAY CASE

TABLE 2: TIME TO TARGET IN SECONDS FOR VARIOUS CONTROL DELAYS

Control delay, dcT (s) 0.0 0.2 0.4 0.6 0.8 1.0 Uncompensated delay 30.8 31.1 - - - 41.4 Compensated delay 30.8 30.8 30.8 30.9 31.0 31.1

TABLE 3: TIME TO TARGET IN SECONDS FOR VARIOUS SENSNING DELAYS

Sensing delay, dsT (s) 0.0 0.2 0.4 0.6 0.8 1.0 Uncompensated delay 30.8 30.9 32.4 - - 31.3 Compensated delay 30.8 30.8 30.9 30.9 31.0 31.1

350 450

100

200

300

400

500

600

x (m)

y (m

)

400 450150

200

250

300

350 400400

450

500

550

(a)

(b)

(a) (b)

No delaysUncompensated delaysCompensated delays

0 5 10 15 20 25 30−4

−2

0

2

4

Time (s)

Ste

erin

g an

gle

(°)

No delaysUncompensated delaysCompensated delays

0 5 10 15 20 25 300

2

4

6

8

10

12

Time (s)

Ver

tical

load

(kN

)

Front left

Front right

Rear left

Rear right


speeds. The sensing and control delays can be compensated, if they are constant and known, albeit at the expense of a reduced usable sensor range.

The limitations of this work can be summarized as follows. The vehicle is assumed to travel on a flat terrain at a constant

speed. It is of interest to consider 3D terrains and include the vehicle speed as a second controlled variable. In addition to changing the problem formulation, this would also require revisiting the model fidelity question. Furthermore, uncertainties in the model or sensor measurements are not yet considered, nor are moving obstacles. Addressing these questions is subject to future work.

V. ACKNOWLEDGMENTS The authors wish to acknowledge the financial support of

the Automotive Research Center (ARC) in accordance with Cooperative Agreement W56HZV-04-2-0001 U.S. Army Tank Automotive Research, Development and Engineering Center (TARDEC) Warren, MI.

VI. REFERENCE [1] S. M. LaValle, Planning algorithms: Cambridge

University Press, 2006. [2] B. D. Luders, S. Karaman, and J. P. How, "Robust

sampling-based motion planning with asymptotic optimality guarantees," in AIAA Guidance, Navigation, and Control Conference (GNC), Boston, MA, 2013.

[3] O. Khatib, "Real-time obstacle avoidance for manipulators and mobile robots," International Journal of Robotics Research, vol. 5, pp. 90-8, 1986.

[4] S. Shimoda, Y. Kuroda, and K. Iagnemma, "High-speed navigation of unmanned ground vehicles on uneven terrain using potential fields," Robotica, vol. 25, pp. 409-424, 2007.

[5] A. Hussein, H. Mostafa, M. Badrel-Din, O. Sultan, and A. Khamis, "Metaheuristic optimization approach to mobile robot path planning," in 1st International Conference on Engineering and Technology (ICET), New Cairo, Egypt, 2012.

[6] P. Ogren and N. E. Leonard, "A convergent dynamic window approach to obstacle avoidance," IEEE Transactions on Robotics, vol. 21, pp. 188-195, 2005.

[7] Y. Gao, T. Lin, F. Borrelli, E. Tseng, and D. Hrovat, "Predictive control of autonomous ground vehicles with obstacle avoidance on slippery roads," in Proceedings of Dynamic Systems and Control Conference (DSCC), 2010.

[8] A. Tahirovic and G. Magnani, "Passivity-based model predictive control for mobile robot navigation planning in rough terrains," in 2010 IEEE/RSJ International Conference on Intelligent Robots and Systems, 2010, pp. 307-312.

[9] A. Tahirovic and G. Magnani, "General framework for mobile robot navigation using passivity-based MPC," IEEE Transactions on Automatic Control, vol. 56, pp. 184-190, 2011.

[10] J. M. Park, D. W. Kim, Y. S. Yoon, H. J. Kim, and K. S. Yi, "Obstacle avoidance of autonomous vehicles based on model predictive control," Proceedings of the Institution

FIGURE 14: VEHICLE TRAJECTORIES FOR SIMULATIONS OF VARIOUS DELAYS

TABLE 4: SIMULATION SETTINGS AND RESULTS FOR VARIOUS VEHICLE SPEEDS

Vehicle speed, 0U (m/s) 15 20 25 30

Execution horizon, eT (s) 1.00 0.75 0.60 0.50

Execution distance (m) 15 15 15 15

Prediction horizon, pT (s) 5.9 6.75 7.8 9.0

Prediction distance (m) 89 135 195 270

LIDAR range, LIDARR (m) 95 145 205 280

Time to target (s) 61.6 44.4 34.9 29.0

FIGURE 15: VEHICLE TRAJECTORIES FOR SIMULATIONS OF VARIOUS VEHICLE SPEEDS

340 440

100

200

300

400

500

600

x (m)

y (m

)

400 430180

200

220

240

260

280

0.6s control delay uncompensated0.6s control delay compensated0.6s sensing delay uncompensated0.6s sensing delay compensated

450550

200

400

600

800

x (m)

y (m

)

500 550150

200

250

300

350

400

450 500 550450

500

550

600

650

700

(a)

(b)

(a) (b)

15 m/s

20 m/s

25 m/s

30 m/s


of Mechanical Engineers, Part D: Journal of Automobile Engineering, vol. 223, pp. 1499-1516, 2009.

[11] M. A. Abbas, J. M. Eklund, and R. Milman, "Real-time analysis for nonlinear model predictive control of autonomous vehicles," in 25th IEEE Canadian Conference on Electrical & Computer Engineering (CCECE), 2012, pp. 1-4.

[12] G. P. Bevan, H. Gollee, and J. O'Reilly, "Trajectory generation for road vehicle obstacle avoidance using convex optimization," Proceedings of the Institution of Mechanical Engineers, Part D: Journal of Automobile Engineering, vol. 224, pp. 455-473, 2010.

[13] M. Nanao and T. Ohtsuka, "Nonlinear model predictive control for vehicle collision avoidance using C/GMRES algorithm," in IEEE International Conference on Control Applications (CCA), 2010, pp. 1630-1635.

[14] J. Frasch, A. Gray, M. Zanon, H. Ferreau, S. Sager, F. Borrelli, et al., "An auto-generated nonlinear MPC algorithm for real-time obstacle avoidance of ground vehicles," in Proceedings of the European Control Conference (ECC), 2013.

[15] 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 American Control Conference (ACC), 2012, 2012, pp. 4239-4244.

[16] J. H. Jeon, R. V. Cowlagi, S. C. Peters, S. Karaman, E. Frazzoli, P. Tsiotras, K. Iagnemma, "Optimal motion planning with the half-car dynamical model for autonomous high-speed driving," in American Control Conference (ACC), Washington, DC, United states, 2013, pp. 188-193.

[17] M. Canale and L. Fagiano, "Vehicle yaw control using a fast NMPC approach," in 47th IEEE Conference on Decision and Control (CDC), 2008, pp. 5360-5365.

[18] P. Falcone, F. Borrelli, J. Asgari, H. E. Tseng, and D. Hrovat, "A model predictive control approach for combined braking and steering in autonomous vehicles," in Mediterranean Conference on Control & Automation (MED), 2007, pp. 1-6.

[19] A. Gray, M. Ali, Y. Gao, J. K. Hedrick, and F. Borrelli, "A Unified approach to threat assessment and control for automotive active safety," IEEE Transactions On Intelligent Transportation Systems, vol. 14, pp. 1490-1499, 2013.

[20] C. E. Beal and C. J. Gerdes, "Model predictive control for vehicle stabilization at the limits of handling," IEEE

Transactions On Control Systems Technology, vol. 21, no. 4, pp. 1258-1269, 2013.

[21] S. J. Anderson, S. C. Peters, T. E. Pilutti, and K. Iagnemma, "An optimal-control-based framework for trajectory planning, threat assessment, and semi-autonomous control of passenger vehicles in hazard avoidance scenarios," International Journal of Vehicle Autonomous Systems, vol. 8, pp. 190-216, 2010.

[22] J. Liu, P. Jayakumar, J. L. Overholt, J. L. Stein, and T. Ersal, "The role of model fidelity in model predictive control based hazard avoidance in unmanned ground vehicles using lidar sensors," in Dynamic Systems and Control Conference (DSCC), 2013.

[23] F. Allgöwer and A. Zheng, Nonlinear model predictive control vol. 26: Springer, 2000.

[24] B.-C. Chen and H. Peng, "Rollover warning for articulated heavy vehicles based on a time-to-rollover metric," Journal of Dynamic Systems, Measurement and Control, Transactions of the ASME, vol. 127, pp. 406-414, 2005.

[25] T. Shim and C. Ghike, "Understanding the limitations of different vehicle models for roll dynamics studies," Vehicle System Dynamics, vol. 45, pp. 191-216, 2007.

[26] H. B. Pacejka, Tire and vehicle dynamics: Elsevier, 2005. [27] A. Rucco, G. Notarstefano, and J. Hauser, "On a reduced-

order two-track car model including longitudinal and lateral load transfer," in European Control Conference (ECC), 2013, pp. 980-985.

[28] A. V. Rao, "A survey of numerical methods for optimal control," in AAS/AIAA Astrodynamics Specialist Conference, Pittsburgh, PA, United States, 2010, pp. 497-528.

[29] C. L. Darby, W. W. Hager, and A. V. Rao, "An hp-adaptive pseudospectral method for solving optimal control problems," Optimal Control Applications and Methods, vol. 32, pp. 476-502, 2011.

[30] A. V. Rao, D. A. Benson, C. Darby, M. A. Patterson, C. Francolin, I. Sanders, et al., "Algorithm 902: GPOPS, A MATLAB software for solving multiple-phase optimal control problems using the gauss pseudospectral method," ACM Transactions on Mathematical Software, vol. 37, 2010.

[31] A. Wächter and L. T. Biegler, "On the implementation of an interior-point filter line-search algorithm for large-scale nonlinear programming," Mathematical programming, vol. 106, pp. 25-57, 2006.

dscc2014-6269 - final

Documents