A model predictive control approach combined

Research Article

A model predictive control approach combined unscented Kalman filter vehicle state estimation in intelligent vehicle trajectory tracking

Advances in Mechanical Engineering 2015, Vol. 7(5) 1–14 Ó The Author(s) 2015 DOI: 10.1177/1687814015578361 aime.sagepub.com

Hongxiao Yu1, Jianmin Duan1, Saied Taheri2, Huan Cheng1 and Zhiquan Qi3

Abstract Trajectory tracking and state estimation are significant in the motion planning and intelligent vehicle control. This article focuses on the model predictive control approach for the trajectory tracking of the intelligent vehicles and state estimation of the nonlinear vehicle system. The constraints of the system states are considered when applying the model predictive control method to the practical problem, while 4-degree-of-freedom vehicle model and unscented Kalman filter are proposed to estimate the vehicle states. The estimated states of the vehicle are used to provide model predictive control with real-time control and judge vehicle stability. Furthermore, in order to decrease the cost of solving the nonlinear optimization, the linear time-varying model predictive control is used at each time step. The effectiveness of the proposed vehicle state estimation and model predictive control method is tested by driving simulator. The results of simulations and experiments show that great and robust performance is achieved for trajectory tracking and state estimation in different scenarios. Keywords Intelligent vehicle, trajectory tracking, unscented Kalman filter, state estimation, model predictive control

Date received: 26 November 2014; accepted: 16 February 2015 Academic Editor: Yangmin Li

Introduction Recently, with the development of the intelligent transportation technology, intelligent vehicles have stepped into our daily lives. Google has already tested autonomous cars successfully. Additionally, several automotive companies such as Volvo, Ford, and Tesla have developed new intelligent vehicles with claims that autonomous cars will become available for limited markets by 2030. Another application of autonomous vehicles which has found much interest among researchers is the wheeled robot technology, for various applications such as self-driving in the hazardous environments.1 The automated algorithms of vehicle control

play a significant role in the self-driving systems. As is well known, the control performance of the intelligent

1

Beijing Key Laboratory of Traffic Engineering, College of Transportation, Beijing University of Technology, Beijing, China 2 Department of Mechanical Engineering, Center for Tire Research (CenTiRe), Virginia Polytechnic Institute and State University, Blacksburg, VA, USA 3 School of Mechanical Engineering, Beijing Institute of Technology, Beijing, China Corresponding author: Hongxiao Yu, Beijing Key Laboratory of Traffic Engineering, College of Transportation, Beijing University of Technology, Beijing 100124, China. Email: [email protected]

Creative Commons CC-BY: This article is distributed under the terms of the Creative Commons Attribution 3.0 License (http://www.creativecommons.org/licenses/by/3.0/) which permits any use, reproduction and distribution of the work without further permission provided the original work is attributed as specified on the SAGE and Open Access pages (http://www.uk.sagepub.com/aboutus/ openaccess.htm). Downloaded from ade.sagepub.com by guest on September 29, 2016

2

Advances in Mechanical Engineering

vehicles depends not only on the robust control algorithm but also on the accurate estimation of vehicle state. However, some of the key states are hard to measure directly. Therefore, an online estimation method is necessary to be proposed in the self-driving control. In the past, Wenzel et al.2 and Baffet et al.3 proposed to use the dual extended Kalman filter (DEKF) for vehicle state estimation and vehicle parameter estimation, which made use of two Kalman filters (KFs) running in parallel. Best et al.4 considered a real-time state estimation of vehicle handling dynamics by the extended adaptive KF. Chu et al.5 addressed fuzzy logic and KF for the longitudinal vehicle velocity observer. However, for the online and real-time estimation, KF has trouble in handling the nonlinear system estimation; the extended Kalman filter (EKF) needs first-order linearizaion of the nonlinear system, which would lose vehicle nonlinear dynamic characteristics. Ray6 estimated the vehicle dynamic states and the lateral tire forces using the 9-degree-of-freedom (DOF) vehicle model. Imsland et al.7 and Zhao et al.8 explored the nonlinear observers to estimate longitudinal and lateral vehicle velocities. Shraim et al.9 and Ouladsine et al.10 estimated velocity and side slip angle by developing a sliding mode observer. However, if the state is unobservable, the state observer would fail. At the same time, there are numerous dynamics and kinematics constraints of vehicle in the self-driving systems. The model predictive control (MPC) method can handle constraints in a systematic way. It is one of the best ways to process the required system constraints in a wide operating region and close to the boundary of the states and the inputs. Gray et al.11 presented an active safety system for avoiding obstacles and preventing road departures. The nonlinear model predictive controller (NMPC) was designed with the goal of using the minimum control intervention to keep the driver safe. Gao et al.12 and Gray et al.13 proposed a

hierarchical control framework used for autonomous and semi-autonomous guidance of ground vehicles; the planned trajectory was tracked by the NMPC method. Abbas14 addressed the MPC controller to provide satisfactory online obstacle avoidance and tracking performance by driving simulator with CarSimÒ. Moreover, NMPC is difficult to develop in real time, which needs to solve the optimization in each control step. In order to reduce the computational burden, a linearization of the nonlinear plant model in MPC controller was developed to figure out the problem,15–18 and using parallel advances to improve the real-time MPC,19–21 which can improve the controller computational burden. In the aforementioned literature, the estimation of the system states is not considered in the control system design process. In this article, the work is presented to investigate the MPC method in the trajectory tracking with the nonlinear state estimation of the vehicle. For state estimation, using the 4-DOF vehicle model combined with the unscented Kalman filter (UKF), the states are estimated to satisfy controllers needed. UKF uses a deterministic sampling approach to capture the mean and covariance estimates with a minimal set of sample points instead of Jacobian matrices. The linear approximators would lose the vehicle and tire nonlinear characteristic in the state estimation. Furthermore, a NMPC tracking problem is transferred to linear timevarying model predictive control (LTV-MPC) in order to do a self-driving tracking control combined with the kinematic model of the vehicle. The proposed method can handle the tracking problem’s state and input constraints and decrease the computational burden in realtime control. The structure of the proposed approach is shown in Figure 1. First, the UKF combined with 4DOF dynamics vehicle model is developed to estimate the vehicle motion states such as longitudinal velocity, yaw rate, vehicle position, and lateral acceleration, which are passed to the MPC controller in real-time

Figure 1. Structure of the proposed approach.

Downloaded from ade.sagepub.com by guest on September 29, 2016

Yu et al.

3   €  hr2 u m v_ + ru + hu = Fxfl dfl + Fxfr dxfr + Fyfl + Fyrl + Fyrr + Fyfr Iz r_  Ixz p_  mhðu  rvÞu = aFxfl dfl     + aFxfr dfr + a Fyfl + Fyfr  b Fyrl + Fyrr   Fxfl  Fyfl dfl  Fxfr + Fyfr dfr t1 + 2 ðFxrl  Fxrr Þt2 + Mz1 + Mz2 + Mz3 + Mz4 + 2   € + ms hðv_ + ruÞ  Ixz r_ Ix  mh2 u     2  ms h + Iy  Iz r2 u + ku1 + ku2 u_   + cu1 + cu2  ms gh u = 0

Figure 2. Vehicle model showing 4 degrees of freedom: longitudinal, lateral, yaw, and roll.

while monitoring vehicle stability. Second, all of the estimation state is provided for system plant to predict the vehicle motion, solve the optimization with vehicle dynamics and kinematics constraints in each control step, and output the control variables to track a given trajectory. This article is organized as follows: in section ‘‘Vehicle modeling,’’ the 4-DOF model of the fourwheel vehicle is developed and used for the state estimation, while the vehicle kinematic model is used for trajectory tracking. In section ‘‘State estimation and MPC tracking,’’ the UKF method is used to estimate the state of the vehicle, which is needed for MPC. In addition, LTV-MPC formulation is used to track the reference trajectory and explain how to choose the cost function and build a linearization of a nonlinear plant. Simulations and experimental results are discussed in section ‘‘Simulation results.’’ Conclusions are presented in section ‘‘Conclusion.’’

Vehicle modeling Assuming the vehicle is driving on the plane and ignoring the pitch and vertical movement of vehicle, Figure 2 shows a 4-DOF vehicle model which describes the dynamics of the vehicle with four wheels. The DOFs are longitudinal, lateral, yaw, and roll, respectively. The model is utilized for online estimation of dynamic states of the vehicle. The vehicle dynamics equations are shown as follows mðu_  rv  hu_r  2hru_ Þ = Fxfl + Fxfr + Fxrl + Fxrr  Fyfl dfl  Fyfr dfr

ð1Þ

ð3Þ

ð4Þ

where a is the distance from the front axle to CG, b is the distance from the rear axle to CG, r is the yaw rate, kui is the damping coefficient at the front and rear axles, h is the height of the roll center, cui is the roll stiffness, m is the vehicle mass, and ms is the vehicle sprung mass. The slip angle can be determined by kinematic relationships given as   v + ra afl = dfl  arctan u + r t21   v + ra afr = dfr  arctan u  r t21   v  rb arl = arctan u + r t22   v  rb arr = arctan u  r t22

ð5Þ ð6Þ ð7Þ ð8Þ

The longitudinal and lateral velocities of the vehicles u and v, respectively, wheel speed v and yaw rate r are used to calculate tire slip ratio, as shown in equations (9)–(14). When the peripheral wheel speed is smaller than the wheel center speed sij = 1 

Four-DOF vehicle model

ð2Þ

vij R , vij

i = f , r; j = l, r

ð9Þ

When the peripheral wheel speed is larger than the wheel center speed sij = 1 

vij , vij R

i = f , r; j = l, r

ð10Þ

where vij are shown as follows rffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffi  t1 2 vfl = u + r + ðv + raÞ2 2 rffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffi  t1 2 vfr = u  r + ðv + raÞ2 2

Downloaded from ade.sagepub.com by guest on September 29, 2016

ð11Þ ð12Þ

4

Advances in Mechanical Engineering rffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffi  t2 2 vrl = u  r + ðv  rbÞ2 2 rffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffi  t2 2 vrr = u + r + ðv  rbÞ2 2

ð13Þ ð14Þ

The wheel load transfer Fzij on each of four wheels are computed as Fzij = Fzsij 6 Fzsfj =

hg max hg may 7 , 2L 2t1

i = f , r; j = l, r

bmg amg , Fzrj = , 2ð a + bÞ 2ð a + bÞ

j = l, r

ð15Þ ð16Þ

The signs + and  are chosen for each specific wheels, and ax and ay are the estimated longitudinal and lateral accelerations, respectively. The equations of motion can be presented by the following geometric expression X_ = u cosðcÞ  u tanðbÞsinðcÞ

ð17Þ

Y_ = u sinðcÞ + u tanðbÞcosðcÞ u b = arctan v

ð18Þ

Figure 3. Coordinate system of the vehicle kinematic model.

2

0 0

2

ð19Þ

cos ur 6 sin u r x~ + 6 4 tan u

where b is the body slip angle; c is the yaw angle; and X and Y are the global x coordinates and global y coordinates, respectively.

r

l

0 0 vr l cos2 dr

3

0 ð23Þ

7 7~u = A~x + B~u 5

As presented in section ‘‘Trajectory tracking,’’ the model prediction control is used in the discrete-time model. Thus, the continuous system needs to transfer to a discrete system. The sampling period is set as T . Euler’s approximation is proposed to obtain the discrete-time model, which is modified by

Vehicle kinematic model The kinematic model can be described as 2 3 2 3 cos u x_ 6 y_ 7 = u6 sin u 7 6 7 6 7 6 u_ 7 6 tan d 7

3 vr sin ur 7 vr cos ur 5

0 0 6 x~_ = x_  x_ r = 4 0 0

ð20Þ

~xðk + 1Þ = Ak, t ~xðk Þ + Bk, t ~uðk Þ

ð24Þ

l

where Ak, t = I + AT, Bk, t = BT, and T is the sample time

The compact form as x_ = f ðx, iÞ

ð21Þ

where (x, y) is the position of the vehicle rear axle, u is the yaw angle of vehicle, l is the wheelbase, and i(u, u) is the control input. Figure 3 shows the coordinate system for the vehicle kinematic model. The reference trajectory is defined as follows x_ r = f ðxr , ur Þ

ð22Þ

where xr is the reference state and ur is the reference input. The tracking objective is to find a control method to satisfy

2

1 Ak, t = 4 0 0

2 3 3 T cos ur 0 0 vr sin ur T 6 sin ur 7 0 1 vr cos ur T 5, Bk, t = 4 TT tan ur Tvr 5 0 1 l cos2 dr l ð25Þ

State estimation and MPC tracking UKF for vehicle state estimation Julier and colleagues22,23 proposed to use UKF. Compared with EKF linearizing using Jacobian matrices, the UKF uses a deterministic sampling approach to obtain the mean and covariance estimates with a minimal set of sigma points. The UKF is more

Downloaded from ade.sagepub.com by guest on September 29, 2016

Yu et al.

5

powerful than EKF on the nonlinear system in various field applications such as railways, ships, aircrafts, solar probes, and so on.24–28 Prediction step. We compute a collection of sigma points and form a set of 2n + 1 sigma points. The columns of x k1 are computed by equations (26)–(28) ðxk1 Þ0 = ^ xk1 pffiffiffiffiffiffiffiffiffiffiffiffipffiffiffiffiffiffiffiffiffiffi ðx k1 Þi = ^ xk1 + n + l Pk1 ðx k1 Þi + 1 = ^ xk1 

pffiffiffiffiffiffiffiffiffiffiffiffipffiffiffiffiffiffiffiffiffiffi n+l Pk1 , i

ð26Þ i

ð27Þ

i = 1, . . . , n

ðx k1 Þi + 1 = ^xk1 

  ^ k Þi = f ðxk1 Þi , ðx

i = 0, . . . , 2n

ð29Þ

where f is the differential equation defined in equations  (1)–(4) for our case. Compute the predicted mean ^xk and the predicted covariance Pk  ^ xk

=

2n X

   ^yik = h x k i ,

Pk =

   T ^ k Þi ^ ^ k Þi ^ Wi ð x xk ð x xk + Qk1

mk =

2n X

  T ^ik  mk ^yik  mk + Rk Wm i y

ð40Þ

  T i i Wci y^k  mk ^yk  mk + Rk

ð41Þ

i=0

Sk =

2n X i=0

Ck =

      T Wci (x  k i xk ) (x k i xk )

l n+l   + 1  a2 + b0

ð32Þ ð33Þ

Wm i =

1 , 2 ð n + lÞ

i = 1, . . . , 2n

ð34Þ

Wci =

1 , 2 ð n + lÞ

i = 1, . . . , 2n

ð35Þ

0

b is a parameter used to incorporate any prior knowledge about the distribution of x.

where R is the measurement noise covariance matrix. Kk is the filter Kalman gain, and xk is the filter state mean, and Pk is covariance and yk is conditional to the measurement.

xk = x k

ð44Þ ð45Þ

xS ðtÞ =

T u v c r u u_ X Y Fxfl , Fyfl , Fxfr , Fyfr , Fxrl , Fyrl , Fxrr , Fyrr ð46Þ U ðt Þ = d

T yðtÞ = ax , ay , r, u

ð36Þ i

+ Kk ðyk  mk Þ

ð43Þ

Estimation of vehicle states based on UKF. The estimation model of the vehicle dynamics includes the four equations of motion (1)–(4). The state vector xS (t) consist of longitudinal and lateral velocities (u and v), yaw angle _ (c), yaw rate (r), roll angle, and roll rate (u and u, respectively), and the tire longitudinal and lateral force vectors (Fxfl , Fyfl , Fxfr , Fyfr , Fxrl , Fyrl , Fxrr , Fyrr ). Input signal is the front wheel angle d, while the measurement elements are longitudinal and lateral accelerations (ax _ with the white and ay ), yaw rate (r), and roll rate(u) noise from CarSim motion sensor. The state vector is shown as follows

Update step. First, compute the sigma points ðxk1 Þ0 = ^ xk1 pffiffiffiffiffiffiffiffiffiffiffiffipffiffiffiffiffiffiffiffiffiffi ðx k1 Þi = ^ xk1 + n + l Pk1

ð42Þ

i=0

ð31Þ

where Qk1 is the process error covariance matrix, and c the weights Wm i and Wi are defined as

l n+l

2n X

T Pk = P k  Kk Sk Kk

i=0

Wm 0 =

ð39Þ

Finally, compute the predicted mean mk , the predicted covariance of the measurement Sk , and the crosscovariance of the state and measurement Ck

ð30Þ

 c

Wc0 =

i = 0, . . . , 2n

Kk = Ck S1 k ^ k Þi Wm i ðx

i = 1, . . . , n

Then, propagate sigma points through the measurement model

i=0 2n X

i

ð38Þ

ð28Þ

where l = a2 (n + k)  n is a scaling parameter, and a and k determine the spread of the sigma points around the mean. Once x k1 is computed, we substitute the sigma points into the dynamic model

pffiffiffiffiffiffiffiffiffiffiffiffipffiffiffiffiffiffiffiffiffiffi n+l Pk1 ,

ð37Þ

ð47Þ ð48Þ

The nonlinear differential equation is the 16th-order estimation model

Downloaded from ade.sagepub.com by guest on September 29, 2016

6

Advances in Mechanical Engineering ^x_ s ðtÞ = f ð^x, U Þ + w

ð49Þ

^yðtÞ = hð^x, U Þ + v

ð50Þ

where ^x_ s and ^y(t) are the estimation state values, w is the process noise, and v is the measurement noise vector (random zero-mean white noise). Q(t) = E(wwT ) is the process-noise covariance matrix and R(t) = E(vvT ) is measurement-noise covariance matrix. The UKF method mentioned above is proposed to use the estimate states with equations (1)–(4).

Trajectory tracking The tracking problem is described in the section ‘‘Vehicle modeling.’’ Trajectory tracking should obtain the vehicle accuracy states in real time. In our case, the longitudinal velocity, yaw angle, and position of the vehicle would be used, which can be estimated by UKF. The control system is presented as above equations (22)–(25). The cost function is used to track the desired trajectory quickly and smoothly. Therefore, the state deviation and control variables are needed for optimization. The cost function in our proposed is given as J ðk Þ =

Mp X

~xT ðk + ijtÞQm~xðk + ijtÞ

decrease the computational time, and a new cost function is explored as the reference29

+

umin  uðk + ijtÞ  umax

minfJ ðk Þg

ð53Þ

subject to umin  uðk + ijtÞ  umax x_ = f ðx, uÞ

ð54Þ

xðkjtÞ = x0

where i 2 ½0, Mp  1 and x0 is the initial condition of the state. The nonlinear optimization problem can be solved by the NMPC, but NMPC will take computational effort much higher than linear MPC. It is hard for us to solve the programming function as real time. Moreover, equation (52) cannot limit the increment of the control in each step time, and the mutations cannot be inhibited to affect the continuity of the system. Therefore, the LTV-MPC is proposed in this section to

ð55Þ DU ðk

+ ijtÞ2R

+ re2

where z(k + ijt) is the tracking state of vehicle; zref (k + ijt) is the reference states for the output tracking variable; Mp is the prediction horizon; Mc is the control horizon; Qm , Rm , and r are the weighting matrices of appropriate dimensions; and e is a slack variable. The vehicle position and yaw angle are provided from UKF estimator, which is the kinematic model states as shown in equation (20). The lateral acceleration and yaw rate are used to evaluate the vehicle stability. The time-varying system is explored by the linearization method to transform the optimization problem in every sampling time. Since the function is convex, it is easy to solve the quadratic programming (QP) problems to obtain the optimization solutions. The new state is modified by equation (56), which includes the error of input as one more state  jðkjtÞ =

~xðkjtÞ ~uðk  1jtÞ

ð56Þ

The new state space equation can be described by the following LTV system ~ k, t jðkjtÞ + B ~ k, t DU ðkjtÞ jðk + 1jtÞ = A

ð57Þ

~ k, t jðkjtÞ zðkjtÞ = C

ð58Þ

ð52Þ

The nonlinear optimization problem can be presented as follows

M c 1 X i=1

+~ uT ðk + i  1jtÞRm ~ uðk + i  1jtÞ

where Mp is the prediction horizon and Qm and Rm are the weighting matrices for error state and control variables, respectively. At the same time, the bound of input is given as

zðk + ijtÞ  zref ðk + ijtÞ2Q

i=1

ð51Þ

i=1

Mp X

J ðk Þ =

where ~ k, t = A



Ak, t 0m 3 n

 Bk, t Bk, t ~ , Bk, t = Im Im

n is the dimension of state, and m is the dimension of control. For the sake of simplicity system, we assume ~ t, t and B ~ k, t = A ~ k, t = B ~ t, t . The state and system output A can be calculated by   p ~M ~ Mp ~ j t + Mp jt = A t, t j ðtjtÞ + At, t Bt DuðtjtÞ p Mc 1 ~ ~M +  +A Bt Duðt + Mc jtÞ t, t   p ~ t, t A ~ ~ Mp 1 B ~M ~ t DuðtjtÞ z t + Np jt = C t, t jðkjtÞ + Ct, t At, t p Mc 1 ~ ~M ~ t, t A Bt Duðt + Mc jtÞ +  +C t, t

In the prediction horizon Mp , and control horizon Mc , the system prediction output equation is given as Y ðtÞ = FðtÞjðtjtÞ + YðtÞDU ðtjtÞ

Downloaded from ade.sagepub.com by guest on September 29, 2016

ð59Þ

Yu et al.

7

where Y (t)Y(t)DU (tjt) is given as 2

jðt + 1jtÞ

3

6 jðt + 2jtÞ 7 7 6 7 6 7 6  7 6 Y ðt Þ = 6 7 6 jðt + Mc jtÞ 7 7 6 7 6  5 4   j t + Mp jt 2~ ~ 3 Ct, t At, t 6 ~ ~2 7 6 Ct, t At, t 7 7 6 6  7 7 6 F ðt Þ = 6 7 c 7 ~M ~ t, t A 6C t, t 7 6 7 6 4  5 M ~ t, tp ~ t, t A C Y ðt Þ = 2 ~ ~ Ct, t Bt, t 0 6 ~ ~ ~ ~ t, t B ~ t, t C 6 Ct, t At, t Bt, t 6 6   6 6 c ~ ~M ~ ~ ~ t, t A ~ t, t 6 C Ct, t At,Mtc 1 B t, t Bt, t 6 6 4   M 1 M p ~ t, t B ~ t, t A ~ t, tp 2 B ~ t, t A ~ t, t C ~ t, t C 3 2 DuðtjtÞ 7 6 6 Duðt + 1jtÞ 7 7 DU ðtÞ = 6 7 6  5 4 Duðt + Mc jtÞ

Figure 4. Driving simulator.

J ðk Þ = 0

0

0

0

0

0





 ~ ~ ~ t, t Ct, t At, t B

3













p Mc 1 ~ ~ t, t A ~M C Bt, t t, t



7 7 7 7 7 7 7 7 7 5

ð60Þ

The amplitude of constraints in the control incremental input can be rewritten as umin  ur ðt + k Þ  Duðt + k Þ  umax  ur (t + k) Dumin (t + k)  u(t + k)  Dumax (t + k)  268  d  268  0:478  Dd  0:478   0:2 0:2  ulim  26 26   0:05 0:05  Dulim  0:47 0:47

ð61Þ

Mp X i=1

+

where Ht = 2(Y(t)T Qm Y(t) + Rm ), Gt = 2Y(t)T Qm F(t)e(t), e(t) is the error of the tracking, Qm = diag(Q, Q, . . . Q), and Rm = diag(R, R, . . . , R).

Simulation results In this section, there are two scenarios to validate the behavior of the proposed state estimation and pathtracking. The first scenario is motion along the straight line tracking in different target longitudinal velocities. The states of the vehicle are also estimated in the driving task. Next, we consider the reference trajectory as an ellipse to validate the lateral tracking capability. In order to evaluate the MPC tracking ability of algorithms and accuracy of estimation, the different velocities and environments are proposed in the simulation. Driving simulator shown in Figure 4 with CarSim is used in all the simulations as reference and set motion sensor as measurement elements in the CarSim. The speed controller used the CarSim driver model. The parameters of the vehicle are shown in Table 1.

Figure 5(a) shows the straight line tracking scenario. The original position of the vehicle is (0, 0), and the reference path original point is (5, 5) as shown in Figure 5(a). The function of the reference path is given as

k zðk + ijtÞ  zref ðk + ijtÞ k2Q

M c 1 X

ð63Þ

Straight line tracking scenario

Model predictive controller designed J ðk Þ =

1 DU ðtÞT Ht DU ðtÞ + GtT DU ðtÞ 2

ð62Þ DU ðk + ijtÞ2R + re2

i=1

Now, it is possible to recast the optimization problem in a usual quadratic problem from

yðtÞ = xðtÞ = ut uðtÞ = p4

ð64Þ

where u is the desired longitudinal velocity, and the constraint of the system is the same as (61). Sample time is T = 0:05 s, prediction horizon Mp = 50, and control horizon Mc = 25. The initial velocity is set as the target

Downloaded from ade.sagepub.com by guest on September 29, 2016

8

Advances in Mechanical Engineering

Table 1. Parameters of vehicle. Symbol

Description

Value

m Iz Ix Iy Ixz l a b t1 =t2 cu1 =cu2 ku1 =ku2 h1 h2 h

Total mass of the vehicle Yaw inertia moment Roll inertia moment Pitch inertia moment Inertia product around x–z plane Wheelbase Distance between front axle Distance between rear axle Front/rear track width Front/rear roll stiffness Front/rear roll damping Front height of virtual roll axis Rear height of virtual roll axis Height of CG

1231 kg 2031.4 kg/m2 288 kg/m2 2031.4 kg/m2 0 kg/m2 2.6 m 1.04 m 1.56 m 1.481 m 980 N m/° 3000 N m/°/s 0.35 m 0.37 m 0.54 m

Different Velocity Tracking Deviation

Different Velocity Tracking

8

80 10km/h 20km/h 30km/h 40km/h Reference Path Start

70

60

6 5

Y/m

Y/m

50

40

4

30

3

20

2

10

1

0

10km/h 20km/h 30km/h 40km/h

7

0

10

20

30

40 X/m

50

60

70

80

0

0

5

10

15

(a)

20 t/s

25

30

35

40

(b)

Figure 5. (a) Different velocity line tracking and (b) different velocity tracking deviation.

velocity (10, 20, 30, and 40 km/h). The vehicle initial heading angle is set to 0 rad. Figure 5(b) describes the tracking deviation in different target longitudinal velocities. The larger initial velocity leads to the larger tracking error. A 40 km/h tracking error is larger than 6 m from 0 to 3 s. Figures 6–8 show the estimation of the vehicle state in which accuracies are achieved by UKF method. Figure 6(a) shows the result of different longitudinal speeds of the vehicle. Figure 6(b) shows the MPC controller output desired speeds. Figures 7 and 8 give us the lateral acceleration and yaw rate in different target longitudinal velocities, respectively. All of the yaw rate is in the reasonable range. However, compared with other tracking speeds, the lateral acceleration of 40 km/h tracking is larger than 0.4 g from 0 to around 3 s, the tire would not work in the linear range, and the vehicle would

lose stability. So, the error of tracking is large in the 40 km/h velocity. The tracking model is vehicle kinematic model, which would lose dynamic vehicle characters in the high velocity. In addition, all these estimated states of the vehicle would be provided to the MPC controller for tracking and identifying the vehicle stability. The stability of vehicle and comfort of passengers would be lost in the large lateral acceleration. The estimated states of the vehicle not only provide the control state but also can help us analyze the vehicle stability and change the control method in the different cases.

Ellipse tracking scenario In this part, the ellipse scenario is proposed to track. The reference path is shown by the parameter equation form

Downloaded from ade.sagepub.com by guest on September 29, 2016

Yu et al.

9

MPC Output Desire Speed 45

40

40

35

35

30

30

Speed /km/h

Longtudinal Velocity/km/h

Longtudinal Velocity Estimation 45

25

25

20

20

15

15

10

10

5

0

5

10

15

20 t/s

25

30

35

5

40

10km/h 20km/h 30km/h 40km/h

0

5

10

15

(a)

20 t/s

25

30

35

40

(b)

Figure 6. (a) Longitudinal velocity estimation and (b) MPC output desired speed.

D10km/h Lateral Acceleration Estimation

D20km/h Lateral Accleration Estimation

3

3

Real Estimation

2

Real Estimation

2

1

Lateral Accleration m/s^2

Lateral Acceleration m/s2

1

0

-1

-2

-3

-2

-3

-5

0

5

10

15

20 t/s

25

30

35

-6

40

5

10

15

20 t/s

25

(a)

(b) D40km/h Lateral Accleration Estimation

30

35

40

3 Real Estimation

2

Real Estimation

2

1

D40km/h Lateral Accleration m/s^2

1

0

-1

-2

-3

0

-1

-2

-3

-4

-4

-5

-5

-6

0

D30km/h Lateral Accleration Estimation 3

Lateral Accleration m/s^2

-1

-4

-4

-5

0

0

5

10

15

20 t/s

25

30

35

40

-6

0

5

10

(c)

15

20 t/s

25

30

35

40

(d)

Figure 7. (a) 10 km/h lateral acceleration estimation, (b) 20 km/h lateral acceleration estimation, (c) 30 km/h lateral acceleration estimation, and (d) 40 km/h lateral acceleration estimation.

Downloaded from ade.sagepub.com by guest on September 29, 2016

10

Advances in Mechanical Engineering

D10km/h Yaw Rate Estimation

D20km/h Latral Velocity Estimation

0.2

0.2

0.1

0.1

Real Estimation

0

-0.1

Yaw Rate/rad/s

Yaw Rate/rad/s

0

-0.1

-0.2

-0.2

-0.3

-0.3 -0.4

-0.4

-0.5

-0.5

-0.6

0

5

10

15

20 t/s

25

30

35

40

0

5

10

15

20 t/s

25

30

(a)

(b)

D30km/h Yaw Rate Estimation

D40km/h Yaw Rate Estimation

0.2

35

0.2 Real Estimation

0.1

0.1

0

0

Yaw Rate/rad/s

Yaw Rate/rad/s

Real Estimation

-0.1

-0.2

-0.1

-0.2

-0.3

-0.3

-0.4

-0.4

-0.5

40

0

5

10

15

20 t/s

25

30

35

40

-0.5

0

5

10

(c)

15

20 t/s

25

30

35

40

(d)

Figure 8. (a) 10 km/h yaw rate estimation, (b) 20 km/h yaw rate estimation, (c) 30 km/h yaw rate estimation, and (d) 40 km/h yaw rate estimation.

8 > < xðtÞ = 24 sin uðtÞ yðtÞ = 40  24 cos uðtÞ > : uðtÞ = 0:4ut

ð65Þ

where u is the desired longitudinal velocity. The initial velocity is set to 0. The target velocity is chosen as 10.8, 18, and 36 km/h. The initial time is ti = 0; the end time is te = 40 s. The initial state is x(ti ) = x0 = 0, y(ti ) = y0 = 0. The constraints of the control are obtained the same as equation (61). Sample time is T = 0:05 s, prediction horizon Mp = 50, and control horizon Mc = 25. Figure 9(a) and (b) shows the result of the tracking and the deviation of the tracking, respectively. Figure 9(c) shows the MPC controller output desired speeds. The desired longitudinal velocities 10.8 and 18 km/h can handle the tracking rapidly, and the system deviation tend to zero, while the desired longitudinal velocity 36 km/h is difficult to follow the

reference path. The biggest deviation comes from 5 to 10 s and from 20 to 35 s. Figure 10(a) shows the longitudinal velocity of vehicle output and estimation. Figure 10(b)–(d) shows the yaw angle real states and estimations, for the 36 km/h yaw angle is too larger to keep safe. Figures 11 and 12 are the lateral acceleration and yaw rate of the vehicle real states and estimations. The results of estimation are really close to the real value. The desired velocity 10.8 and 18 km/h lateral acceleration value is smaller than 0.4 g during the path-tracking as shown in Figure 11(a) and (b). The yaw rate value of 10.8 and 18 km/h is not changed drastically with the path-tracking. In the 36 km/h situation, the lateral acceleration value is changed drastically and is over than 0.7 g from 5 to 10 s. In the 12–17 s and 28–37 s, the accelerations are also larger than 0.4 g (Figure 11(c)). At the same time, the yaw rates are changed a lot (Figure 12(c)).

Downloaded from ade.sagepub.com by guest on September 29, 2016

Yu et al.

11

Different Velocity ellipse Tracking

Different Velocity Tracking Deviation

80

20

Reference D10.8km/h D18km/h D36km/h

70 60

D10.8km/h D18km/h 36km/h

18 16 14 12

Distance/m

Y/m

50 40

10 8

30 6

20

4 2

10

0

0 -40

-30

-20

0

5

10

15

X/m

20 t/s

(a)

(b)

-10

0

10

20

30

25

30

35

40

MPC Output Desire Speed 40

35

Speed

30

25

20

15

10

0

5

10

15

20 t/s

25

30

35

40

(c) Figure 9. (a) Ellipse tracking, (b) tracking deviation, and (c) MPC desired speeds.

The cases of these situations are hard to track the trajectory for the controller. The stability of the vehicle would be lost during the path-tracking.

Conclusion In this article, a nonlinear vehicle state estimation using UKF method and trajectory tracking by the MPC approach are proposed, while a LTV-MPC controller is addressed to decrease the computing burden in the realtime control. The capabilities of the method have been simulated in two different scenarios by driving simulator. The first one is the straight line tracking scenario in different target velocities. The other one is the ellipse tracking scenario with different desired velocities. These show good performances for tracking the reference path. The MPC controller shows a high-accuracy solution to

track the reference path in the low-velocity case. The state estimation can provide all the vehicle state information for MPC controller such as the position of the vehicle and the yaw angle, which are used for controller, and the lateral acceleration and the yaw rate of the vehicle, which are used to evaluate vehicle stability and keep safe for tracking. In the simulation result, the vehicle lost stability at the large lateral acceleration and yaw rate ratio in the trajectory tracking. The trajectory tracking problem does not only consider the tracking error but also needs to match the stability of the vehicle. Moreover, the future improvement will aim to propose a method to solve the high-speed tracking control and the nonlinear state estimation with complex tire model. It is not easy to estimate the tire forces and dynamic friction coefficients for the MPC controller, so the controller is limited to accurately tracking in the

Downloaded from ade.sagepub.com by guest on September 29, 2016

12

Advances in Mechanical Engineering

Longtudinal Velocity Estimation

D10.8km/h Yaw Angle

70

0

Real D10.8km/h D18km/h D36km/h

Estimation Real

-0.5 -1

50

-1.5 -2

40

Yaw rad

Longtudinal Velocity/km/h

60

30

-2.5 -3 -3.5

20

-4

10

0

-4.5 -5

0

5

10

15

20 t/s

25

30

35

40

0

5

10

15

20 25 Time (seconds)

(a)

40

(b) 0

Estimation Real

-1 -2

-4

-3

-6

-4

-8

-5

-10

-6

-12

-7

-14

-16

0

5

10

15

20 25 Time (seconds)

30

35

Estimation Real

-2

Yaw rad

Yaw rad

35

D 36km/h Yaw angle

D18km/h Yaw Angle 0

-8

30

40

0

5

10

15

20 25 Time (seconds)

(c)

30

35

40

(d)

Figure 10. (a) Longitudinal velocity estimation, (b) 10.8 km/h yaw angle, (c) 10.8 km/h yaw angle, and (d) 36 km/h yaw angle.

10.8km/h Lateral accleration Estimation

18km/h Lateral accleration Estimation

1

1.5 Real Estimation

0.5

0.5

18km/h Lateral accleration

0

10.8km/h Lateral accleration

Real Estimation

1

-0.5 -1 -1.5 -2 -2.5

0 -0.5 -1 -1.5 -2 -2.5

-3

-3

-3.5

-3.5

0

5

10

15

20 t/s

25

30

(a)

35

40

0

5

10

15

20 t/s

25

30

35

40

(b)

36km/h Lateral accleration Estimation 2 Real Estimation

36km/h Lateral accleration

0

-2

-4

-6

-8

-10

0

5

10

15

20 t/s

25

30

35

40

(c) Figure 11. (a) 10.8 km/h lateral acceleration, (b) 18 km/h lateral acceleration, and (c) 36 km/h lateral acceleration. Downloaded from ade.sagepub.com by guest on September 29, 2016

Yu et al.

13

D10.8km/h Yaw Rate Estimation

D18km/h Yaw Rate Estimation

0.1

0.1 Real Estimation

0 -0.1

-0.1

-0.2

Yaw Rate/rad/s

Yaw Rate/rad/s

Real Estimation

0

-0.3 -0.4

-0.2

-0.3

-0.4

-0.5

-0.5

-0.6

0

5

10

15

20 t/s

25

30

35

-0.6

40

0

5

10

15

20 t/s

(a)

25

30

35

40

(b) D36km/h Yaw Rate Estimation 0 Real Estimation

-0.1

Yaw Rate/rad/s

-0.2

-0.3

-0.4

-0.5

-0.6

-0.7

0

5

10

15

20 t/s

25

30

35

40

(c) Figure 12. (a) 10.8 km/h yaw rate estimation, (b) 18 km/h yaw rate estimation, and (c) 36 km/h yaw rate estimation.

different surfaces. The mass point model and simple vehicle dynamic model would be applied in the tracking MPC to improve the high-speed tracking and adapt in different surfaces and changed abruptly surfaces. Declaration of conflicting interests The authors declare that there is no conflict of interest.

Funding This work was supported by the National Natural Science Foundation of China (NSFC grant no. 51005019) and the Foundation of the Beijing Institute of Technology (no. 20130342036).

References 1. Spenko M, Kuroda Y, Dubowsky S, et al. Hazard avoidance for high-speed mobile robots in rough terrain. J Field Robot 2006; 23: 311–331.

2. Wenzel TA, Burnham KJ, Blundell MV, et al. Dual extended Kalman filter for vehicle state and parameter estimation. Vehicle Syst Dyn 2006; 44: 153–171. 3. Baffet G, Charara A, Lechner D, et al. Experimental evaluation of observers for tire–road forces, sideslip angle and wheel cornering stiffness. Vehicle Syst Dyn 2008; 46: 501–520. 4. Best MC, Gordon T and Dixon P. An extended adaptive Kalman filter for real-time state estimation of vehicle handling dynamics. Vehicle Syst Dyn 2000; 34: 57–75. 5. Chu L, Chao L, Zhang Y, et al. Design of longitudinal vehicle velocity observer using fuzzy logic and Kalman filter. In: International conference on electronic and mechanical engineering and information technology (EMEIT), Harbin, China, 12–14 August 2011, vol. 6, pp.3225–3228. New York: IEEE. 6. Ray LR. Nonlinear state and tire force estimation for advanced vehicle control. IEEE Trans Contr Syst Technol 1995; 3: 117–124.

Downloaded from ade.sagepub.com by guest on September 29, 2016

14

Advances in Mechanical Engineering

7. Imsland L, Johansen TA, Fossen TI, et al. Vehicle velocity estimation using nonlinear observers. Automatica 2006; 42: 2091–2103. 8. Zhao L-H, Liu Z-Y and Chen H. Design of a nonlinear observer for vehicle velocity estimation and experiments. IEEE Trans Contr Syst Technol 2011; 19: 664–672. 9. Shraim H, Ouladsine M and Fridman L. Vehicle parameter and states estimation via sliding mode observers. In: Bartolini G, Fridman L, Pisano A, et al. (eds) Modern sliding mode control theory. Berlin, Heidelberg: SpringerVerlag, 2008, pp.345–362. 10. Ouladsine M, Shraim H, Fridman L, et al. Vehicle parameter estimation and stability enhancement using the principles of sliding mode. In: Proceedings of the American control conference 2007: ACC’07, New York, 9–13 July 2007, pp.5224–5229. New York: IEEE. 11. Gray A, Ali M, Gao Y, et al. Semi-autonomous vehicle control for road departure and obstacle avoidance. In: IFAC control of transportation systems, Sofia, 1–6 September 2012. 12. Gao Y, Gray A, Frasch J, et al. Spatial predictive control for agile semi-autonomous ground vehicles. In: Proceedings of the 11th international symposium on advanced vehicle control, Seoul, Korea, 9–12 September 2012. 13. Gray A, Gao Y, Lin T, et al. Predictive control for agile semi-autonomous ground vehicles using motion primitives. In: Proceedings of the American control conference (ACC), Montreal, QC, Canada, 27–29 June 2012, pp. 4239–4244. New York: IEEE. 14. Abbas MA. Non-linear model predictive control for autonomous vehicles. Thesis, University of Ontario Institute of Technology, https://ir.library.dc-uoit.ca/bitstream/10155/ 206/1/Abbas_Muhammad%20Awais.pdf 15. Falcone P, Borrelli F, Asgari J, et al. A model predictive control approach for combined braking and steering in autonomous vehicles. In: Mediterranean conference on control and automation, 2007: MED’07, Athens, 27–29 June 2007, pp.1–6. New York: IEEE. 16. Falcone P, Borrelli F, Asgari J, et al. Predictive active steering control for autonomous vehicle systems. IEEE Trans Contr Syst Technol 2007; 15: 566–580. 17. Falcone P, Tufo M, Borrelli F, et al. A linear time varying model predictive control approach to the integrated vehicle dynamics control problem in autonomous systems. In: 46th IEEE conference on decision and control, New Orleans, LA, 12–14 December 2007, pp.2980–2985. New York: IEEE. 18. Ku¨nhe F, Lages WF and Gomes da Silva JM. Mobile robot trajectory tracking using model predictive control.

19.

20.

21.

22.

23.

24.

25.

26.

27.

28.

29.

In: II IEEE Latin-American robotics symposium, 2005, http://www. ece.ufrgs.br/;fetter/sbai05_10022.pdf Borrelli F, Bemporad A, Fodor M, et al. An MPC/hybrid system approach to traction control. IEEE Trans Contr Syst Technol 2006; 14: 541–552. Borrelli F, Keviczky T, Balas GJ, et al. Hybrid decentralized control of large scale systems. In: Morari M and Thiele L (eds) Hybrid systems: computation and control. Berlin, Heidelberg: Springer-Verlag, 2005, pp.168–183. Keviczky T and Balas GJ. Flight test of a receding horizon controller for autonomous UAV guidance. In: Proceedings of the American control conference, Portland, Oregon, 8–10 June 2005, vol. 5, pp.3518–3523. New York: IEEE. Julier SJ, Uhlmann JK and Durrant-Whyte HF. A new approach for filtering nonlinear systems. In: Proceedings of the American control conference, Seattle, WA, 21–23 June 1995, vol. 3, pp.1628–1632. New York: IEEE. Julier SJ and Uhlmann JK. New extension of the Kalman filter to nonlinear systems. In: Signal processing, sensor fusion, and target recognition VI, Orlando, FL, 21 April 1997, vol. 3068. Bellingham, WA: SPIE. Lee JH, Sevil HE, Dogan A, et al. Estimation of maneuvering aircraft states and time-varying wind with turbulence. Aerosp Sci Technol 2013; 31: 87–98. Peng Y, Han J and Wu Z. Nonlinear backstepping design of ship steering controller: using unscented Kalman filter to estimate the uncertain parameters. In: IEEE international conference on automation and logistics, Jinan, China, 18–21 August 2007, pp.126–131. New York: IEEE. Reali F and Palmerini G. Estimate problems for satellite clusters. In: 2008 IEEE aerospace conference, Big Sky, MT, 1–8 March 2008, pp.1–18. New York: IEEE. Reali F and Palmerini GB. Evaluation of GNC strategies for terminal descent on solar system bodies. In: AIAA guidance, navigation and control conference and exhibit, Hilton Head, SA, 20–23 August 2007, vol. 6855. Reston, VA: AIAA. Xu B, Zhang J and Guan X. Estimation of the parameters of a railway vehicle suspension using model-based filters with uncertainties. Proc IME F: J Rail Rapid Transit. Epub ahead of print 12 February 2014. DOI: 10.1177/ 0954409714521605. Falcone P. Nonlinear model predictive control for autonomous vehicles. Dissertation, University of Sannio, Benevento, 2007.

Downloaded from ade.sagepub.com by guest on September 29, 2016