Discussion on: “Adaptive and Predictive Path

European Journal of Control (2007)4:440–446 # 2007 EUCA DOI:10.3166/EJC.13.440–446

Discussion on: ‘‘Adaptive and Predictive Path Tracking Control for Off-road Mobile Robots’’ Kristijan Mac˘ek1, Jadranko Matusˇ ko2, Agostino Martinelli3 and Roland Siegwart1 1 Swiss Federal Institute of Technology Zurich, Switzerland; 2Faculty of Electrical Engineering and Computing Zagreb, Croatia; 3INRIA Rhoˆne-Alpes, France

1. Introduction The authors are addressing the problem of accurate path tracking in off-road terrains in presence of lateral sliding, with particular application to agriculture vehicles. The adherence conditions are not modeled explicitly (i.e. no tire to ground model), however the lateral sliding angles of the wheels are integrated in the kinematic bicycle model, called extended kinematic model. An observer is used to estimate the unknown sliding angles on-line with respect to steady-state adherence conditions. A steering wheel controller based on chained form transformation of the kinematic bicycle model for the parametric curve tracking is used. Furthermore, in order to compensate for abrupt curvature changes and low level control delay, a model predictive control signal is added. Experimental verification is provided on slippery, wet ground for straight driving on a slope and curved path driving on flat ground, with good tracking results achieved, close to set requirements.

2. Observability Properties The aim of this section is to show that when the exteroceptive sensor consists only of a real time



E-mail: {kristijan.macek; roland.siegward}@mavt.ethz.ch E-mail: [email protected] E-mail: [email protected]





kinematic-global positioning system (GPS), the robot orientation and the sliding angles are observable under the assumption PR ¼ 0, that is approximately true when the vehicle’s speed is low. In order to relax this approximation it is necessary to equip the vehicle with inertial measurement unit (IMU) sensors, that is accelerometers and gyro. In the paper, the vehicle kinematics with sliding taken into account are given by Eq. (3). By indicating ½x,y,T the robot position and orientation in a global frame, this extended kinematic model can be written as: 2 x_ ¼ vL cos  vT sin , 6 y_ ¼ vL sin þ vT cos , 6 ð1Þ 4 F v tanð þ  Þ v L T P  , _ ¼ L L where vL and vT are the longitudinal and transversal rear wheel velocities, respectively: vL ¼ v cosPR ,

ð2Þ

vT ¼ v sinPR :

ð3Þ

Observability properties of the state  ¼ ½x,y,,vL ,vT T will be investigated further. The GPS provides the following observations: zx ¼ x þ wx ,

ð4Þ

zy ¼ y þ w y ,

ð5Þ

where the terms wx and wy represent the noise in the GPS measurements.

441

Discussion on: ‘‘Off-road Mobile Robots Path Tracking’’

Let us introduce the following notation:     x vL , ; v ¼ p ¼ y vT therefore: 2

3 p 6 7  ¼ 4  5: v From Eq. (1) we obtain: _p ¼ RðÞv , where: 

cos RðÞ ¼ sin

 sin : cos

and W4 ¼ ½0,0,vL cos  vT cos,cos,sin vT sin,sin,cos. These four vectors are independent. ð6ÞHowever, higher order Lie derivatives depend on the unknown parameters aL, aT and s. Therefore, the observable space has dimension equal to four and the state  is not completely observable. Only combinations of its elements can be observed. In particular, the sum v2L þ v2T ¼ v2 is observable since it is equal to ð7Þ ðL1 zx Þ2 þ ðL1 zy Þ2 . Under the approximation PR ¼ 0 (i.e. vT=0), vL=v is observable. On the other hand, also, x, y and  are observable in this case, as can be seen by applying the rank criterion to the system whose dynamics are ð8Þ given by: 2 x_ ¼ vcos , 6 ð14Þ 4 y_ ¼ vsin , ð9Þ _ ¼ s ,

By differentiating twice p we obtain: _v ¼ Sv þ a ,

ð10Þ

where a ¼ ½aL ,aT T is the vehicle acceleration in the local frame attached to the vehicle and furthermore:   0 s 1 _ , ð11Þ S ¼ R R ¼ s 0 with s being the vehicle turning rate: _ ¼ s :

ð12Þ

The Eqs. (8), (10) and (12) describe the dynamics of the state : 2 x_ ¼ vL cos  vT sin , 6 6 y_ ¼ vL sin þ vT cos , 6 _ ¼ 6 _ ¼ s , ð13Þ 6 6 4 v_L ¼ s vT þ aL , v_T ¼ s vL þ aT : We want to derive the observability properties of this state when the observations are given by Eqs. (4)–(5). To this goal we apply the observability rank criterion introduced by Hermann and Krener [1]. The procedure to apply this criterion requires to compute all the Lie derivatives of the observations along the dynamics in Eq. (13). The zero derivatives are: L0zx=x and L0 zy ¼ y whose gradients are respectively W1 ¼ ½1,0,0,0,0 and W2 ¼ ½0,1,0,0,0. The first order Lie derivatives are: L1 zx ¼ vL cos  vT sin and L1 zy ¼ vL sin þ vT cos whose gradients are W3 ¼ ½0,0,  vL sin 

with the observations in Eqs. (4)–(5), even if s is unknown. Without the approximation PR ¼ 0, the observability of  is obtained as soon as aL , aT and s are known. Indeed, it is possible to select another vector independent of W1, W2, W3 and W4 from the gradients of higher order Lie derivatives of the observations in Eqs. (4)–(5) along the dynamics in Eq. (13). The required aL , aT and s are in this case evaluated using IMU sensors. Note that the last expression in Eq. (1) cannot be used beforehand, since it depends on the unknown parameter PF . In fact, only after  is estimated can this equation be used to estimate PF .

3. Dynamic Model As mentioned in conclusions of the paper, one of the possible improvements in the estimation of sliding condition would be to use a dynamic model in observer design. The primary motivation is better estimation of the sliding angles at abrupt ground adhesion changes, which cannot be done with the current observer, since it assumes steady-state conditions based on kinematic model. Moreover, the predictive control in the paper takes into account the vehicle inertia by an increased prediction horizon in an implicit fashion, which has to be fine-tuned. Potential improvement of both estimation and control aspects could be achieved by including vehicle dynamics, which is discussed herein. The dynamics modeling taken here is adopted from [2], where the dynamic state of the vehicle can be described by fv, , _g, v being velocity of the center of gravity (CoG),  the vehicle side slip angle and  the vehicle orientation. In particular,  represents the

442

Discussion on: ‘‘Off-road Mobile Robots Path Tracking’’

difference between the actual vehicle velocity direction and the nominal chassis orientation and implicitly contains the information about the side slip angles on the wheels. The single track linear dynamic model compatible with the kinematic bicycle model used in the paper assumes lateral accelerations below 0.4 g (according to [3]), which is well within the range for the agricultural vehicles. In order to be compliant with the steering control of the paper, velocity is considered a parameter here. The dynamics of the reduced model is then described as (see [2] for more details): " # _ €

2 6 ¼6 4



cF þ cR mv

cR l R  cF l F JZ 2 c 3 F 6 mv 7 7 þ6 4 cF l F 5   , JZ

3 cR l R  cR l F " #  1 7  mv2 7 cR l2R þ cF l2F 5 _  JZ v ð15Þ

where the cF and cR cornering stiffness coefficients (side force constants) for the front and rear equivalent wheel, respectively. These constants depend on the lateral tire characteristics, varying for different manufacturers. lF and lR are the distances from the CoG to the front and rear wheel axes, respectively and JZ is the inertial moment around the vertical z-axis. Eq. (15) is derived under assumption that the rotational vehicle motion is determined mainly by the lateral equivalent wheel forces in front FSF and rear FSR of the form: FSF ¼ cF  PF ,

FSR ¼ cR  PR ,

ð16Þ

where the link with the side slip angles PF and PR of the paper and the vehicle side angle  is established as: ! _ l  F PF ¼     , v ! ð17Þ lR _ R P ¼   : v It is assumed that the yaw rate _ is measured from an IMU sensor and that the steering angle  is also known. the state space representation for  Therefore,  x ¼ ,_ , u ¼ ½ can be formulated as: x ¼ Ax þ Bu , y ¼ Cx ,

where:

2

cF þ cR 6  mv A¼6 4 cR l R  cF l F JZ 2 cF 3

3 cR lR  cR lF  1 7 mv2 7 2 2 5, cR lR þ cF lF  JZ v

ð18Þ

6 mv 7 B ¼ 4 c l 5, F F JZ C ¼ ½ 0 1 : The system given by (Eq.) 18 can become unobservable if cF lF  cR lR ¼ 0. In that case slip angle  cannot be estimated according to yaw rate measurement _. This can be checked by calculating observability matrix: 2 3 0 1 cR l2 þ cF l2F 5 : O ¼ ½ C CA T ¼ 4 cR lR  cF lF  R JZ JZ v ð19Þ However this problem could likely be neglected in the case of agricultural vehicles due to their weight distribution and significant differences in characteristics and sizes between front and rear wheels. The dynamic vehicle behavior could be included in the extended kinematic model of Eq. (3) of the paper by taking into account only the body side angle :   v cos ~ þ  , s_ ¼ 1  cðsÞy   y_ ¼ v sin ~ þ  ,   cðsÞ cos ~ þ  _~ _ : ¼v 1  cðsÞy

ð20Þ

Note that the reference point is moved from the center of the rear axes to the CoG of the vehicle. With the  ¼ 0 this new extended kinematic model becomes the basic one of Eq. (1) in the paper.

4. Side Slip Angle Estimation In order to estimate the side slip angle based on the system of Eq. (18), the physical parameters of the model fm,lR ,lF ,cR ,cL ,Jz g have to be known. Whereas the parameters fm,lR ,lF ,Jz g can be obtained by an identification technique, the time varying nature of the cornering stiffness coefficients cR , cF makes the proposed dynamic model sensitive to parameter uncertainties. Therefore, it is necessary to estimate these parameters together with the side slip angle  .

443

Discussion on: ‘‘Off-road Mobile Robots Path Tracking’’

A relatively simple and effective approach to simultaneous state and parameter estimation is dual Kalman filtering [4]. This approach combines two Kalman filters that run in parallel: one for state estimation that uses the most recent parameter estimates and the other for parameter estimation which uses latest state estimates as its parameters. Additionally the dual Kalman filtering technique allows for taking into account the measurement or process noise level during estimation assuming that they have Gaussian white noise characteristics. Note that in the case of simultaneous state and parameter estimation matrices A and B in Eq. (18) become functions of the parameters being estimated, while matrix C remains constant. First, the bicycle model given by Eq. (18) is converted to a discrete state space representation using Euler backward differentiation formula: xk ¼ Fk1 xk1 þ Gk1 uk þ vk , yk ¼ Hk xk þ nk , with:

2

 3 cFk þcRk cRk lR cRk lF 1 T 7 6 1 mv T mv2 6 7 Fk ¼ 6   7, 4 cRk lR cFk lF 5 cRk l2R þcFk l2F T 1 T JZ JZ v 2 cFk 3 T 6 mv 7 Gk ¼ 4 c l 5 , Fk F T JZ Hk ¼ H ¼ ½ 0 1  ,

Measurement update for states KF: Kxk ¼ Pxkjk1 HT ðHPxkjk1 HT þ Rx Þ1 , ^kjk1 þ Kxk ðyk  H^ ^k ¼ x xkjk1 Þ , x x x x Pk ¼ ðI  Kk HÞPkjk1 : Measurement update for parameters KF: Kwk ¼ Pwkjk1 ðHwk ÞT ðHwk Pwkjk1 ðHwk ÞT þ Rw Þ1 , ^ kjk1 þ Kwk ðyk  H^ ^k ¼ w w xkjk1 Þ, w w w w Pk ¼ ðI  Kk Hk ÞPkjk1 ,

^k1 þ Gk1 uk , ^kjk1 ¼ Fk1 x x Pxkjk1 ¼ Fk1 Pxk1 FTk1 þ Qx :

Hwk ¼ 

@ðyk  H^ xkjk1 Þ @^ xkjk1 ¼H : ^ k1 ^ @w @ wk1

Pwkjk1 ¼ Pwk1 þ Qw :

ð26Þ

@^ x

kjk1 Due to term @w matrix Hwk needs to be calculated in a recursive manner using following formulae:

H

@^ xkjk1 @^ xk1jk2 ¼ HFk1 ðI  Kxk1 HÞ ^ k1 ^ k1 @w @w @Fk1 @Gk1 ^k1 þ H þH uk : x ^ k1 ^ k1 @w @w

ð27Þ

After rewriting Eq. (27) using physical parameters following recursive equation is obtained: H

@^ xkjk1 @^ xk1jk2 ¼ HFk1 ðI  Kxk1 HÞ ^ k1 ^ k1 @w @w 3T 2 ^ lR T^k1 l2R T_k1  7 6 7 6 Jz JZ v 7 : þ6 7 6 4 lF T^k1 l2 T^_k1 lF Tk 5 F  þ Jz JZ v JZ ð28Þ

ð22Þ

Prediction for parameters KF: ^ k1 , ^ kjk1 ¼ w w

ð25Þ

where Kkk and Kwk are Kalman gains, and Pxk and Pwk are estimation error covariance matrices for state and parameter Kalman filter, respectively. Indexes kjk  1 and k in state estimates denote a-priori and a-posteriori estimation, respectively. Matrix Qw is the covariance matrix of the parameter model process noise which determines convergence and tracking performance of the parameter estimation. In addition matrix Hwk represents output error sensitivity to the parameters being estimated which is defined as:

ð21Þ where xk ¼ ½k ,_k T , Fk , Gk , Hk are discrete state space matrices calculated at t ¼ kT using parameters wk ¼ ½cRk ,cFk T , uk ¼ ½k , vk ¼ N ð0,Qx Þ is the process noise that in our case represents uncertainty due to imperfect steering angle measurement k , nk ¼ N ð0,Rx Þ is measurement uncertainty of yaw rate _k and T is discretization time. Based on system description given by Eq. (21) dual Kalman filtering procedure for simultaneous state and parameter estimation consists of following steps: Prediction for states KF:

ð24Þ

ð23Þ

5. Summary In Section 2 it has been shown under which conditions the extended kinematic model in the paper is observable based on the observability rank criterion. In Section 3 explicit modeling of vehicle dynamics

444

around the vertical axis was proposed based on the vehicle side slip angle, which implicitly models also the wheels side slip angles of the paper. Section 4 proposes a method for concurrent estimation of the side slip angle and the time varying cornering stiffness parameters based on a dual Kalman technique. The integration of thus obtained vehicle dynamics based on vehicle side slip angle can be included in a straight forward manner in the extended kinematic model used for chained form steering control. It is expected that the proposed dynamic estimation should improve the overall tracking performance of the original system even in transient states where adherence properties of the vehicle change. Moreover, based on explicit

Discussion on: ‘‘Off-road Mobile Robots Path Tracking’’

vehicle dynamics model, precise control objectives in the model predictive scheme could be explored.

References 1. Hermann R, Krener A. Nonlinear controllability and observability. IEEE Trans Autom Control 1977; AC-22(5): 728–740 2. Kiencke U, Nielsen L. Automotive Control Systems, 2nd edn., Springer, 2005 3. Mitschke M. Dynamik der Kraftfahrzeuge, Band C: Fahrverhalten, 2nd edn., Springer, 1990 4. Wan E, Nelson A. Dual EKF Methods, Kalman Filtering and Neural Networks, Haykin S (Ed.) Wiley, 2001

Discussion on: ‘‘Adaptive and Predictive Path Tracking Control for Off-road Mobile Robots’’ J.A. Guerrero and R. Lozano Universite´ de Technologie Compie`gne, HEUDIASYC UMR 6599 CNRS, BP. 20529, C.P. 60205, Compie`gne, France

In this paper the trajectory tracking problem is addressed considering the sideslip in off-road robots for agricultural applications. An extended vehicle dynamic model including sliding phenomena is presented. Due to the fact that actual sensors used in prototype vehicle, sliding parameters can not be measured. A Luenberger observer is proposed to estimate these parameters. Model predictive control algorithm is developed considering the actuator dynamics. This work is an interesting application of the existing tools in adaptive control, nonlinear observers and model predictive control. The extended kinematic model is presented which is similar to the Ackermann model [1]. The authors consider only the sideslip angle due to the tire influence. Fig. 3b shows the vehicle behavior considering the sideslip. In fact, a more complete description of the kinematic model with sideslip can be found in [2]. Estimation of sliding parameters can be done through a direct computation, using Eq. (4). However, this type of computation is sensitive to noise since parameters are functions of the numerical derivation

of measured states taken directly from sensors. Therefore, in order to get a better estimation of sliding parameters required in the extended kinematic model in Eq. (3), a Luenberger observer was proposed. In fact the proposed observer uses the numerical derivation

1

0.5

0

-0.5

-1 PD Control

-1.5

Saturation Control Path to be followed -2

-2.5

E-mail: [email protected] E-mail: [email protected]

Trajectory Tracking Comparison

1.5

0

1

2

3

4 Time (sec)

Fig. 1 Trajectory tracking comparison.

5

6

7

8