By using fuzzy system, the parameters of the kinematical controller are functions of the lateral, longitudinal and orientation errors of the motion.. In [14] a dynamical fuzzy control la
Trang 1Fuzzy adaptive EKF motion control for
non-holonomic and underactuated cars with
parametric and non-parametric uncertainties
F M Raimondi and M Melluso
Abstract: A new fuzzy adaptive motion control system including on-line extended Kalman’s filter
(EKF) for wheeled underactuated cars with non-holonomic constraints on the motion is presented
The presence of parametric uncertainties in the kinematics and in the dynamics is treated using
suit-able differential adaptation laws We merge adaptive control with fuzzy inference system By using
fuzzy system, the parameters of the kinematical controller are functions of the lateral, longitudinal
and orientation errors of the motion In this way we have a robust control system where the
dynamics of the motion errors is with lower time response than the adaptive control without
fuzzy Also Lyapunov’s stability of the motion errors is proved based on the properties of the
fuzzy maps If data from incremental encoders are employed for the feedback directly, sensor
noises can damage the performance of the motion control in terms of the motion errors and of
the parametric adaptation These noises are aleatory and denote a kind of non-parametric
uncertain-ties which perturb the nominal model of the car Therefore an EKF is inserted in the adaptive
control system to compensate for the above non-parametric uncertainties The control algorithm
efficiency is confirmed through simulation tests in Matlab environment
1 Introduction
In recent years much attention has been focused upon the
control of non-holonomic mechanical systems [1] A
mobile wheeled car actuated by a differential drive is
usually studied as a typical non-holonomic system, where
non-holonomic constraints arise under the no-slip
con-straints Our approach is about motion control of
non-holonomic wheeled cars Due to non-non-holonomic motion,
the cars are underactuated In fact there are three
general-ized coordinates i.e lateral position, longitudinal position
and car orientation to be controlled, whereas there are two
control inputs only, that is steering and longitudinal
inputs About the motion control, older research effort use
only kinematical controllers [2] The main idea is to
define velocity control inputs which stabilize the closed
loop system However, if one considers kinematical
control-ler only, then one assumes that there exists perfect velocity
tracking, that is the control signals affect the car velocities
instantaneously and this is not true More recently control
researchers have targeted the problem of motion control
of underactuated wheeled cars using a backstepping
approach [3, 4]which allows many of the steering system
commands to be converted to torques, taking into account
dynamic parameter (mass, inertia, friction) The
fundamen-tal problems of the backstepping control are the parameters
uncertainties, i.e the unknown dynamical parameters of the
car, that is mass, inertia, position of the mass centre and
the partialy known kinematical parameters, that is ray of
the wheels, distance from the reference point of the motion to the mass centre In this sense adaptive control schemes have been presented [5 – 7] In [5, 7] adaptive laws of the dynamical parameters only have been devel-oped, whereas in [6] adaptive laws both the dynamical and the kinematical parameters have been presented However in all the works above the parameters of the kin-ematic control laws are constant for every value of the motion errors, so that they must be chosen suitably to guar-antee a good dynamics of the motion errors; also the problem of on-line localization, that is an optimal esti-mation of the car’s position, has not been treated Really,
at each sampling instant the position of the car is estimated
on the basis of the encoders’ increment along the sampling interval A drawback of this method is that the errors of each measure caused by the encoder are summed up Many researchers have solved the problem of localization, by an off-line sensors data fusion based on the use of extended Kalman’s filters (EKF) [8 – 10] An interesting approach has been developed in [11], where a conventional PID control strategy with a Kalman-based active observer con-troller has been used to solve a problem of path following for non-holonomic cars To apply EKF it is necessary for
a discrete non-linear system model which describes the state transition relationship during a sampling interval A discrete model for non-holonomic cars has been proposed [10] where the position coordinates of the car have been expressed with respect to a ground reference About fuzzy inference mechanism for non-holonomic and under-actuated cars, fuzzy controllers have been presented [12, 13], where the stability of the motion is not always assured In [14] a dynamical fuzzy control law with Lyapunov’s stability proof has been proposed, whereas in [15]a kinematical control law, where the parameters have been obtained using a suitably fuzzy inference system, has been shown and the asymptotical stability of the
# The Institution of Engineering and Technology 2007
doi:10.1049/iet-cta:20060459
Paper first received 25th October 2006 and in revised form 22nd January 2007
The authors are with the Dipartimento di Ingegneria dell’Automazione e dei
Sistemi, University of Palermo, Italy
E-mail: maurizio.melluso@dias.unipa.it
Trang 2motion error has been proved However, by using the
approach of[15], because of the noises of the encoders
posi-tioned in right and left wheels of the car, the feedback
signals of the control system (i.e the actual position and
orientation of the car provided by the encoders) are
noised The aleatory noises above are responsible for
non-parametric uncertainties which perturb the model of the
car The non-parametric uncertainties above can corrupt
the performance of the fuzzy control system in terms of
dynamics of the motion errors and in terms of the dynamical
parametric adaptation
In order to continue this research line, a fuzzy adaptive
motion control system with on-line EKF for non-holonomic
and underactuated cars is presented The following
contri-butions are given:
1 Merging of adaptive kinematic and dynamic controllers
with a fuzzy inference system where stability and
conver-gence analysis is built on Lyapunov’s theory, based on the
properties of the fuzzy maps This assures robustness and
lower time response than the adaptive control without
fuzzy [6]
2 A sampled chained form model for non-holonomic car
with non parametric uncertainties In[10]the position
coor-dinates of the car are expressed in a ground reference,
whereas in our work the coordinates are expressed in a
body fixed reference In this way the noises of the
proprio-ceptive sensors (i.e encoders positioned in right and left
wheels of the car) are naturally expressed in a frame
attached to the car body
3 A methodology for solving the on-line sensors data
fusion problem through EKF An EKF has to be introduced
in the fuzzy adaptive control system above to fuse data from
multiple proprioceptive sensors (i.e encoders, vector
compass and sensor position) and to estimate the filtered
feedback signals, that is the actual position of the car by
on-line recursive predictions and corrections The EKF
compensates the non-parametric uncertainties effects (i.e
discontinuities in the dynamics of the motion errors and
of the parametric adaptation)
By using our fuzzy solution, the constant parameters
of the conventional kinematic control laws [3, 6] are
obviated In fact, in our approach, the parameters are
nonlinear functions of the motion errors and this assures
faster convergence and more robustness than the
conven-tional adaptive controller[6] Based on the input – output
properties of the fuzzy map, the asymptotical stability of
the motion errors has been proved in Section 4.2 Section
4.3 adds an adaptive kinematical control with stability
proof to solve the problem of partialy known kinematical
parameters In Section 5, based on the adaptive
backstep-ping approach [6], a dynamical extension is presented
The EKF estimates the filtered state from the noised
outputs provided from more odometric sensors and
assures a good localization The filter above requires to
derive a linear discrete time stochastic state space
representation of the car model and of the measure
process About the state space representation, we introduce
a sampled form of the kinematic ‘chained form’ model[16]
where the inputs are provided by the data of the encoders
About the measure equations, we consider data provided
from proprioceptive sensors of position and orientation
2 Time continuous models for wheeled cars
Let us consider a mobile car ofFig 1with generalized
coor-dinates q [ <n, subject to m constraints The well known
dynamic model in generalized coordinates is given in [3, 5, 6]
M(q) €q þ C(q, _q) _q ¼ E(q)t AT(q)l (1) where M(q) [ <nn is a symmetric, positive definite matrix; C(q, _q) [ <nn is the centripetal Coriolis matrix;
t [ <n1 is a vector including torques applied to right and left wheels; A(q) [ <mn is the matrix of nonholo-nomic constraints and l [ <m1 is a vector of lagrange multipliers Supposing that the m constraints are time invar-iant leads to
Let S(q) [ <n(nm)be a full rank matrix made up by a set of smooth and linearly independent vectors spanning the null space of A(q), that is
We indicate with (x0(t), y0(t)) the P coordinates (see Fig 1) at time t in an inertial cartesian frame (x, y) and with f(t) the car orientation with respect to the inertial basis at the same time Also let r be the ray of the wheels (see Fig.1) Let C be the mass center of the car, which is
on the X-axis, and let d be the distance from P to C For the later description, mcis the mass of the car without the driving wheels, mwis the mass of each driving wheels,
Ic, Iwand Imare the inertia moments of the body around a vertical axis through P, the wheel with a motor about the wheel axis, and the wheel with a motor about the wheel diameter, respectively It is possible to find a v [ <nm vector as it follows
where u(t) and v(t) are, respectively, the linear and angular body-fixed (X, Y ) time varying velocities Indicate with
˙
ur(t) and ˙ul(t) the time varying angular velocities of right and left wheels, respectively The relationship between v and the angular velocities above is the following
˙
ur(t)
˙
ul(t)
¼
1 r
b r 1 r
b r
2 6
3 7
Equation (5) can be rewritten as follows
h ¼
1 r
b r 1 r
b r
2 6
3 7
where
hT¼ ˙ur(t) ˙ul(t)
Fig 1 Constrained wheeled car with references
IET Control Theory Appl., Vol 1, No 5, September 2007 1312
Trang 3Now considering the vector
qT¼[x0(t) y0(t) f(t)] (7) leads to the following kinematic model
_q ¼
_x0(t)
_y0(t)
˙
f (t)
2
4
3
5 ¼ cb cos f(t)cb sin f(t) cb cos f(t)cb sin f(t)
2 4
3
5 u˙r(t)
˙
ul(t)
where
c ¼ r=2b About the dynamic model in body fixed coordinates
(X, Y ), differentiating (8), replacing it into (1) and
perform-ing additional operations with S(q) lead to
M ¼ b2c2m þ c2I þ Iw b2c2m c2I
b2c2m c2I b2c2m þ c2I þ Iw
3
dmc( ˙ur ˙ul)
2bc3dmc( ˙ur ˙ul) 0
B ¼ diag[1 1]; tT¼[tr(t) tl(t)]
where
m ¼ mcþ2mw; I ¼ mcd2þIcþ2mwb2þ2Im
The M and Vmmatrices are, respectively, the Inertia and
Coriolis matrices in body-fixed (X, Y ) coordinates system
(see Fig 1) The vector t has in its components the
torques applied to the right and left wheels, respectively
Substituting (6) into (8) leads to
_q ¼
cos f(t) 0 sin f(t) 0
2 4
3
5 u v
¼ S2(q)v (10)
3 Discrete time kinematic model for
non-holonomic and underactuated cars
with non-parametric uncertainties
Preliminarily we consider the following change of
coordi-nates
j(t) ¼ [j1(t) j2(t) j3(t)]T¼ R(f)[x0(t) y0(t) f(t)]T
(11) where
R(f) ¼
cos f(t) sin f(t) 0
sin f(t) cos f(t) 0
2 4
3
Applying the transformation (11) to model (10) leads to
a chained form model[16] An analogical to digital converter
(ADC) obtains samples of the chained form model
We assume constant sampling period Dtk¼ T and
denote k þ 1 ¼ (k þ 1)T, k [ Z So, from sampling of the
vector (11), we can define the sampled vector j(k) After
some algebra, the perturbed sampled state space model yields
j(k þ 1) ¼ A(k)j(k) þ x(k) þ w(k)
wT(k) ¼ [w1(k) w2(k) w3(k)] k [ Z (13)
where w(k) is the process noise with Gaussian statistical dis-tribution This is a non parametric uncertainty which perturbs the state-input model (13) Statistical mean and variance of the noise above are the following:
E{w(k)} ¼ 0; E{w(i)wT(j)} ¼ 0 for i = j (14) E{w(k)wT(k)} ¼ Q
where Q is the diagonal covariance matrix Also it is:
A(k) ¼
0 cos (v(k)T ) sin (v(k)T )
0 sin (v(k)T ) cos (v(k)T )
2 6
3
xT(k) ¼hv(k)T u(k)sin (v(k)T )v(k) u(k)cos (v(k)T ) 1v(k) i
(16) where u(k) and v(k) are derived as sampling of the com-ponents of the vector given by (4) The localization sensors (i.e vector compass and position proprioceptive sensor) directly produce the full-state of the car So, if q coordinates are used, it yields
where z is the output vector and C is an identity matrix Applying the transformation (11) and considering an additive measurement noise n(k) (i.e, non parametric uncertainty per-turbing the output equation) lead to
z(k) ¼ CR1(f)j(k) þ n(k) ¼ g(j(k)) þ n(k) (18) where the statistical parameters of the noise are
E{n(k)} ¼ 0; E{n(i)nT(j)} ¼ 0 for i = j
and R is the diagonal covariance matrix Note that, in conse-quence of the rotation (11), the g function of (18) is naturally nonlinear Also w(k) and n(k) noises are independent We may write a new governing equation that linearizes the measurement process It yields
j(k þ 1) ¼ A(k)j(k) þ x(k) þ w(k)
Hgj(k)j¼j
(k)¼
(j2(k) sin (j1(k)) j3(k) cos (j1(k)))
(j2(k) cos (j1(k)) þ j3(k) sin (j1(k)))
1
2 6
cos j1(k) sin j1(k)
sin j1(k) cos j1(k)
3
where j(k) is solution of the process model (13) without noise
Remark 1: With respect to q coordinates, the new reference
in j coordinates is with the same origin of the world frame but rotated so as to align the axis with the car orientation Therefore the noises of the new ji (i ¼ 1, 2, 3) variables are expressed in a frame attached to the car body Since the noises are caused by sensors located in the automatic
Trang 4car, the reference using j coordinates is more natural than
the reference using q coordinates
4 Adaptive fuzzy kinematic motion control
of nonholonomic and underactuated cars:
Lyapunov’s stability analysis
4.1 Fuzzy kinematical motion control and
preliminary input – output properties of the fuzzy
system for the Lyapunov’s stability
Let the reference position be:
_xr(t) ¼ ur(t) cos fr(t) _yr(t) ¼ ur(t) sin fr(t)
˙
fr(t) ¼ vr(t)
(22)
where ur(t) 0 for all t is the reference linear velocity and
vr(t) is the reference angular velocity Then the motion
error between the reference position qr¼[xr(t)
yr(t) fr(t)]T and the actual position q given by (7) can be
expressed in the car local frame (X, Y ) as in[3]
e ¼
ex
ey
ef
2
4
3
5
¼
cos f(t) sin f(t) 0
sin f(t) cos f(t) 0
2
4
3
5 xyrr(t) x(t) y00(t)(t)
fr(t) f(t)
2 4
3
where ex and ey are the lateral and longitudinal position
errors, while efis the orientation error of the wheeled car
It is evident that the components of the vector (23) are
time functions We omit the argument for simplicity
notation
Now a new fuzzy kinematic control law is proposed
uc(t) ¼ ur(t) cos efþkx(t)ex
vc(t) ¼ vr(t) þ ur(t)(ky(t)eyþkf(t) sin ef)
(24)
where uc(t) and vc(t) are the kinematic control laws in terms
of steering and longitudinal velocities The control law
above depends on the error vector (23) and on the following
parameters
kT(e) ¼ [kx(t) ky(t) kf(t)] (25) Parameters (25) are provided by a fuzzy controller In this
way the parameters of the kinematic control law are not
constant as in the conventional controllers [3, 6], but they
are nonlinear functions of the motion errors (23) Since
the fuzzy maps are adjusted based on the control
perform-ance, the updating of the parameters (25) assures a good
robustness and faster convergence than the conventional
adaptive control without the fuzzy system [6] Now the
fuzzy inference system is described Fig 2 shows the
input and output membership functions The fuzzy rules
are shown in Table 1 The input and output memberships
are generalized bell functions and three linguistic labels
are defined:
S ¼ Small; M ¼ Medium; H ¼ High;
Opp ¼ Opposite:
The inputs of the fuzzification process are the absolute values of the motion errors, so that the input sets are single-ton (cf 23), whereas the outputs of the input fuzzification memberships are the degree of membership in the qualify-ing lqualify-inguistic sets (always the interval between 0 and 1) The implemented method for the logical ‘and’ and for the implication are the ‘minimum’ and the ‘fuzzy minimum’ respectively The consequents of each rule have been recombined using a maximum (max) method The defuzzi-fication method is the ‘centroid’ So the outputs of the fuzzy system are crisp values that is the parameters (25) (see Fig 2b)
Remark 2: The generalized bell functions have been chosen for the smoothness which assures continuous func-tions to guarantee the Lyapunov’s stability of the control system
Remark 3: To guarantee the stability of the motion errors, the parameters kx, kyand kfare positive numbers
At this point one must investigate on the input – output properties of the fuzzy system The properties above have
to be the following:
Property 1a: The parameters (25) are continuous time functions;
Property 2a: The vector k(e(t)) (cf 25) is equal to zero if only if e is equal to zero that is
kT(e) ¼ [kx(t) ky(t) kf(t)] ¼ 0 , e ¼ 0
Fig 2 Membership functions
a Input memberships
b Output memberships
IET Control Theory Appl., Vol 1, No 5, September 2007 1314
Trang 5Property 3a: All the outputs of the fuzzy inference system
are positive numbers and are bounded that is
0 kx(t) kx max; 0 ky(t) ky max
0 kf(t) kf max
Property 4a: The fuzzy inference system assures that
dky
de
de dt
’dky
dey
dey
dt 0 8t:
Now considering the fuzzy control law (24) and differen-tiating (23) lead to the following closed loop model _eT¼ _e x _ey _ef
¼
(vr(t) þ ur(t)(ky(t))eyþkf(t) sin ef)eykx(t)ex
(vr(t) þ ur(t)(ky(t)eyþkf(t) sin ef)exþur(t) sin ef
ur(t)(ky(t)eyþkf(t) sin ef)
2 6
3 7
(26) 4.2 Lyapunov’s stability proof based on the input – output properties of the fuzzy system From the fuzzy inference system, equations and properties
so far, the first main results of this work follow
Theorem 1: Let the kinematic model and the fuzzy kinematic control laws be (10) and (24), respectively Let the linear reference velocity ur(t) be positive The properties 1a – 4a are verified for hypothesis Then the equilibrium state of the non autonomous closed loop system (26) is the origin of the state space and this is asymp-totically stable
Proof: Since the vector k(e) (cf 25) is equal to zero if only
if e is equal to zero, the equilibrium state of the closed loop system (26) is the origin of the state space The system (26)
is non-autonomous The following Lyapunov function is chosen
V0 ¼1
2(e
2
xþe2y) þ (1 cos ef)g(t) (27) where
and for hypothesis
Therefore the Lyapunov’s function (27) is positive defi-nite The time derivative of (27) is
_
V0¼ex_exþey_eyþ_efsin efg(t) þ (1 cos ef) _g(t) (30) where
_g(t) ¼ d dt
1
ky(t)
!
¼ dky=dt
k2(t) ¼
(dky=de)T(de=dt)
k2(t) (31)
By substituting (26) into (30), it yields _
V0¼ kx(t)e2xur(t)kf(t) sin2(ef)g(t)
þ(1 cos ef) _g(t)
(32)
The function (32) is negative semidefinite In fact, due to property (3a) of the fuzzy map, the first and second term of (32) are non-negative and, because of property (4a), the third term of the same function is non-negative Now the function above does not depend on eyerror Since it results
V0 ¼1
2(e
2
xþe2y) þ (1 cos ef)g(t) 1
2(e
2
xþe2y)
where
Table 1: Table of the rules
Trang 6then the (27) is a decrescent function Therefore vector (23)
is bounded and the equilibrium state of the closed loop
system (26) is stable Since the second time derivative of
(27) depends on bounded variables, it is a bounded function
Therefore the function (32) is uniformly continuous From
Lyapunov-like Barbalat’s Lemma it yields
lim t!1 _
From (32) and (35), exand efconverge to zero From the
second equation of (26) that is
_ey¼ (vr(t) þ ur(t)(ky(t)eyþkf(t) sin ef)exþur(t) sin ef
the function _eyconverges to zero Therefore the steady state
error along y direction is constant Examining the third
equation of (26) leads to
_ef(1) ¼ urky(1)ey (36) where eyis the steady state value of ey Since efconverges
to zero, eyconverges to zero Now kyis equal to zero if eyis
equal to zero Therefore the equilibrium point of the closed
loop system (26) is asymptotically stable A
4.3 Fuzzy adaptive kinematic motion control with
Lyapunov’s stability proof
Herein, the second main result of this work is explained An
adaptive controller is added to previous fuzzy control and
the stability is proved This step is necessary because the
kinematical parameters as the ray of the wheels and
particu-larly the distance between the driving wheels and the axis of
symmetry can be difficult to be determined accurately
Preliminarily, after simple calculations, the closed loop
kinematical control system can be written as follows
d
dt
ex
ey
ef
2
6
3
7
5 ¼ ˙ur(t)
r
2þ
r 2bey
r 2bex
r 2b
2 6 6 4
3 7 7 5
þ ˙ul(t)
r
2
r 2b r 2bex r 2b
2 6 6 4
3 7 7 5
þ
ur(t) cos ef
ur(t) sin ef
vr(t)
2
6
3
We set
Differential equation (37) can be exploited by
consider-ing the estimation errors of the kinematical parameters (38)
^
a ¼ ¯a a; b ¼ ¯^ b b (39) where ¯a and ¯b are the estimated values It results
d
dt
ex
ey
ef
2
6
3
7
5 ¼ 1 þaa^
uc(t)
1 0 0
2 6
3 7
þ 1 þb^
b
!
vc(t)
ey
ex
1
2 6
3 7
þ u r(t) cos ef ur(t) sin ef vr(t)T
(40) Now it is possible to formulate the following theorem
Theorem 2: Let the kinematic model and the fuzzy control law be (10) and (24), respectively The linear reference and angular velocities are bounded functions and the angular velocity reference converges to zero Assume the following adaptive kinematic control law
_¯a ¼ gexuc(t); _¯b ¼ dvc(t) sin ef
ky(t) g, d 0 (41) Then the motion errors of the closed loop system (40) converge to zero
Proof: An extended state vector can be defined
eT¼ ex ey ef a^ b^
(42) The Lyapunov function can be chosen as follows
V1¼V0þ 1
2gaa^
2þ 1 2dbb^
2
g, d 0 (43)
where V0is given by (27) Since V0is positive definite, it is obvious that V1 is positive definite Substituting the fuzzy control law (24) into (41) and differentiating (43) lead to
_
V1¼ _V0þ a^
ga( _a gexuc(t)) þ
^ b
db _¯b dvc(t) sin (ef)
ky(t)
!
(44)
where uc(t) and vc(t) are given by (24) and _V0 is given by (32) Function (44) is negative semidefinite if and only if (41) is verified In this case it results
_
Since the function (45) does not depend on eycomponent (cf 32), it is negative semidefinite Therefore the closed loop system (40) is stable and the components of the state vector (42) are bounded It is also possible to calculate the second time derivative of the Lyapunov function (43) Since it depends on bounded variables, from Barbalat’s Lemma it results
lim t!1 _
Therefore exand efconverge to zero Now, by substitut-ing (24) into (40), it results
_ef¼ 1 þb^
b
! (vr(t) þ ur(t)(ky(t)ey
þkf(t) sin ef)) þ vr(t) (47) Since ef converges to zero, _efconverges to zero; there-fore eyconverges to zero only if the angular velocity
Remark 4: From the previous results, the adaptive fuzzy kinematic control law can be written in terms of angular velocity of left ( ˙ulc) and right ( ˙urc) wheels as it follows
hc¼ u˙
rc(t)
˙
ulc(t)
¼ a¯ b¯
¯
a ¯b
uc(t)
vc(t)
(48)
where ¯a and ¯b are the solutions of the differential equations (41), while uc(t) and vc(t) are the fuzzy control laws given
by (24)
Remark 5: Note that the control of the mobile car is expressed in terms of the angular velocities of the right
IET Control Theory Appl., Vol 1, No 5, September 2007 1316
Trang 7and left wheels In this way we have two control
com-ponents Really three variables are controlled, that is the
longitudinal and the lateral position and the orientation of
the car Consequently the system is underactuated
5 Adaptive dynamic motion control extension
Here, a low-level adaptive controller based on backstepping
method[3, 4, 6], is added to previous fuzzy adaptive
high-level control for nonholonomic and underactuated cars The
conventional computed torque controller[3]requires exact
knowledge of the dynamics of the car in order to work
prop-erly In this work, instead, an adaptive mechanism is
inserted because the dynamical parameters of the model
(9) cannot be accurately known
Preliminarily important properties of the dynamical
model (9) and kinematical model (8) must be presented
Property 1.b: The linearity in the parameters p of the
dyna-mical model (9) is shown
M ˙h þ Vm(h)h ¼ Y(h, ˙h)p (49)
where the vector p [ <l and Y(h, ˙h ) [ <(nm)l are:
p ¼ p 1 p2 p3T
¼
b2c2m þ c2I þ Iw
b2c2m c2I 2bc3dmc
2 6
3
Y(h, ˙h ) ¼ u¨r u¨l ( ˙ur ˙ul) ˙ul
¨
ul u¨r ( ˙ur ˙ul) ˙ur
(51)
The elements of the vector p consist of unknown
dynami-cal parameters
Property 2.b: The kinematical model (8) appears as it
follows:
_x0(t)
_y0(t)
˙
f (t)
2
6
3
7
5 ¼
r
2cos f(t)
r
2cos f(t) r
2sin f(t)
r
2sin f(t) r
r 2b
2
6
6
4
3 7 7 5
˙
ur(t)
˙
ul(t)
" #
¼
r
2cos f(t)
r
2sin f(t)
r
2b
2
6
6
4
3 7 7 5
˙
ur(t) þ
r
2cos f(t) r
2sin f(t)
r 2b
2 6 6 4
3 7 7 5
˙
ul(t)
¼
cos f(t) 0
sin f(t) 0
2
6
3 7
r 2 r 2b
2 6
3 7
5 ˙ur(t) þ
cos f(t) 0 sin f(t) 0
2 6
3 7
r 2 r 2b
2
6
3 7
5 ˙ul(t) ¼ S1u1u˙r(t) þ S2u2u˙l(t) (52)
where u1and u2are parametric vectors whereas S1and S2
are matrices whose elements consist of known functions
Now, by inserting the new fuzzy inference system of the
previous sections, the results on the adaptive backstepping
technique[6]are reformulated
From (41) and (48) the fuzzy kinematical adaptive track-ing controller model can be written as
hc¼hc(e, ¯a, ¯b ) ¼ hc(q,qr, ¯a , ¯b ) _¯a ¼ f1(e, ¯a) ¼ f1(q,qr, ¯a ) _¯b ¼ f2(e, ¯b ) ¼ f2(q,qr, ¯b ) (53) Also the Lyapunov function (43) appears as it follows
V1¼V1(e, ¯a , ¯b ) ¼ V1(q,qr, ¯a , ¯b ) (54) Assumption 1: The adaptive tracking controller (53) exists for the kinematical model (8) Also there exists a positive definite and radially unbounded function V1such that
_
V1¼@V1
@q _q þ@V1
@qr_qrþ@V1
@ ¯a f1þ
@V1
@ ¯b f20 (55) where all the signals are bounded
Now the following adaptive dynamical control law can be chosen[6]
t ¼ tl(t)
tr(t)
¼ B1 ( Kdh þ Y ^p ~ @V1
@q ^S
_^ui¼Li @V1
@q Si
˜
hi i ¼ 1, , 2; _^p ¼ CYTh~ (56)
where tlis the control torque applied to the left wheel; tris the control torque applied to the right wheel; ^ui is the esti-mation of ui, i ¼ 1, 2 (cf 52); Y and p are given by (50) and (51); ^p is the estimation of the dynamical parameters of p vector; Si (i ¼ 1,2) matrices are given by (52), V1 is given by (44) and satisfies the assumption 1; ^S is the Jacobian matrix (cf 8) and it depends on estimated kin-ematic parameters ^uifor i ¼ 1,2; ~h is given by
~
h ¼ hch ¼ [ ˜h1 h˜2]T (57) where hcis given by (48) and h is the dynamical velocity vector of model (9); kd, C and Liare simmetric and positive definite matrices with appropriate dimensions
In this way ~h converges to zero asymptotically[6] Remark 6: Here, the adaptive fuzzy kinematic control (48) has been converted into adaptive torque control law (56) Therefore the torque control has been selected in (9)
so that the nonholonomic car exhibits the desired behaviour thus justifying the specific choice of the velocity h Also,
by using the kinematic model (8), the dynamic velocity may be converted into actual position p The measurement
of the actual position and orientation using encoders only can be affected by Gaussian noises The noises above cause non parametric uncertainties in the kinematic model of the car (cfr 13, 18) Therefore an EKF in the feedback of the fuzzy control system above has to be inserted, to fuse data provided from more sensors and to obtain good estimations of the position and orientation of the car
6 EKF in feedback of the adaptive fuzzy control From output data provided by encoders, an information on the actual feedback position signal q for the adaptive control system of the previous section may be obtained suit-ably However the information above is corrupted by noises
of the encoders Therefore an EKF has to be introduced in the adaptive control system From data of more sensors (i.e data fusion with encoders, vector compass and position
Trang 8sensor) the filter above estimates a filtered position signal
for the feedback Consider the sample state model (13) in
j coordinates to elaborate the encoders data Furthermore,
consider the output equations Dy(k) of (20) to have position
and orientation measurements from vector compass and
sensor position We desire estimates ^j(k) of the state j(k)
based on observation of the output y(k) alone The
Kalman’s filtering task is to determine a Kalman gain K
to minimize the variance of the ‘a posteriori’ estimation
error, which is denoted D(k)
D(k) ¼ E{(j(k) ^j(k))(j(k) ^j(k))T} (58)
Let us consider ‘a priori’ state estimate j(k) so that
j(k) ¼ A(k 1)^j(k 1) þ x(k 1) (59)
with error variance given by
F(k) ¼ E{(j(k) j(k))(j(k) j(k))T} (60)
Now consider the following incremental update:
D ^j(k) ¼ Dj(k) þ K(k)(Dy(k) Hgj(k)Dj(k)) (61) where K(k) [ <31is the Kalman’s gain and Hjg(k) is given
by (21) Adding j(k) on both sides of (61) and considering
j(k) ¼ j(k) lead to
^j(k) ¼ j(k) þ K(k)(y(k) g(j(k)) (62) Now we seek the optimal Kalman’s gain
Ko(k) ¼ arg min
K(k)
X3 i¼1 var[(ji(k) ˆji(k))]
After some computations it yields
Ko(k) ¼ F(k)Hgj(k)(Hgj(k)F(k)(Hgj(k))Tþ R)1
Po(k) ¼ (I Ko(k)Hgj(k))F(k) (63) The steps of the Kalman’s algorithm for the sensors data fusion are the following
† evaluate the gain factor by using the first equation of (63);
† solve the equation of measurement update (62);
† update the error variance by using the second equation of (63);
† prediction of the future state by using (59);
† prediction of the covariance error, where
F(k þ 1) ¼ A(k)P(k)AT(k) þ Q (64)
† update the time and repeat the steps
After the estimation of ^j(k), applying the inverse of the transformation (11) and using a ‘digital to analogic conver-ter’ with ‘zero order hold’ lead to analogical information ^q for generating the motion errors (23) and for applying the adaptive control laws (48) and (56)
7 Simulation tests Simulation tests are performed in Matlab environment where the kinematic and dynamic models (cfr 8, 9) with the fuzzy
Fig 3 Block schemes of the fuzzy adaptive control
a Block scheme without EKF
b Block scheme with EKF
Fig 4 Reference non-holonomic motion (solid line);motion of the car (dashed line)
IET Control Theory Appl., Vol 1, No 5, September 2007 1318
Trang 9adaptive control laws (48) and (56) have been implemented
suitably The EKF algorithm has been implemented using C
language with sequential acquisition and filtering of the
informations provided by proprioceptive sensors, that is
encoders, position sensors and vector compass which have
been simulated using Matlab Simulink
Fig 3 shows the block schemes of the fuzzy adaptive control systems without and with EKF, which have been projected in this paper
While in case of fuzzy adaptive control without EKF, the feedback signal is q (cf 7), in case of the same control strategy with EKF, the feedback one is ^q (i.e an
Fig 5 Motion errors
a,b Longitudinal and lateral motion error
c,d Steady state of the longitudinal and lateral motion error
e, f Longitudinal and lateral motion errors using gains given by (66) and fuzzy approach
Trang 10estimation of the position of the car as it is explained in
Section 6)
The simulation results were obtained using the
LABMATE platform nominal parameters, that is
b ¼ 0:75 m; d ¼ 0:3 m;
r ¼ 0:15 m; mc¼30 Kg
mw¼1 Kg; Ic¼15:6 Kg m2;
Iw¼0:005 Kg m2; Im¼0:0025 Kg m2:
The parameters of the adaptive laws (41) and (56) are
g ¼ 0:005; d ¼ 20:75; Kd¼5 I2; c ¼ 5 I3
where I2and I3 are identity matrices (2 2) and (3 3)
respectively In case of adaptive kinematic control without
fuzzy, the parameters of the velocity control law (24) are
chosen as
The initial conditions for the reference and car positions
are the following
(xr(0), yr(0), fr(0)) ¼ (0, 0, 3:48 rad)
(x(0), y(0), f(0)) ¼ ( 30, 20, 5:68 rad)
The sample time for the EKF is T ¼ 1024s The initial
values for the EKF parameters are
F(0) ¼ diag(0:9 0:7 0:7);
R ¼ diag(0:00003 0:00003 0:00003);
Q ¼ diag(0:1 0:1 0:1)
We compare for cases
1 adaptive dynamic and kinematic motion control without
fuzzy system and without EKF[6], where the parameters of
the velocity control are constant (cf 65) (seeFig 3a, where
one eliminates the fuzzy inference system block);
2 adaptive dynamic and kinematic motion control without
fuzzy system and with EKF (seeFig 3b, where one
elimin-ates the fuzzy inference system block and uses constants
parameters given by 65);
3 adaptive dynamic and kinematic motion control with
fuzzy system and without EKF, where the parameters of
the velocity control depend on the fuzzy system described
in Section 4.1 (seeFig 3a);
4 adaptive dynamic and kinematic motion control with
fuzzy system and with EKF (see Fig 3b)
Note that, if the components of the vector k(e) (cf 25) are
linear functions, the properties which assure the stability are
verified Therefore one may consider the following case
kT(e) ¼ [k1abs(ex) k2abs(ey) k3abs(ef)] (66)
where
k1 ¼k2¼k3¼5
We compare the performances of the feedback adaptive
controller where the gains are given by (66) with the
fuzzy adaptive approach of our paper
Fig 4shows the reference and the actual motions of the car
in case of our algorithm (i.e case 4) The reference trajectory
is feasible, that is it does not violate the non-holonomic
con-straints Figs 5a and b show the longitudinal and lateral
motion errors of the car in cases 1 – 4 respectively.Figs 5c
anddshow the same errors, where the quality of the steady state is evident.Fig 5eshows the motion errors in case of adaptive control with application of (66) and in case of our fuzzy adaptive approach
In Figs 5aandbone considers the motion error from 0
to 8 s for showing in a better way the initial transient and the improvement of the adaptive fuzzy control with respect to the adaptive control without fuzzy [6] By comparing the performances of the control systems with and without EKF, one observes that, both in case of adaptive fuzzy control and in case of adaptive control without fuzzy, the EKF filters the measurement noises of the longitudinal motion error in a good way By comparing the performances of the control system with and without fuzzy inference system, both with EKF and without, one notes a lower response time in case of adaptive control with fuzzy system than in case of adaptive control without fuzzy The EKF improves also the steady state performances Figs 5eandfshow the better performances of the initial tran-sient in case of our fuzzy approach than in case of adaptive control using the gains (66) Fig 6 shows the control torques (56) in case of fuzzy adaptive control with EKF Consider outside disturbances violating the non-holonomic constraints The following simulation tests show the performances of the fuzzy adaptive motion control system with and without EKF The disturbance above can be caused by the impact of the wheeled car with the external environment, as for example the road conditions and the contact between the wheels and the ground where the
Fig 6 Control torques
Fig 7 Lateral motion error with perturbations
IET Control Theory Appl., Vol 1, No 5, September 2007 1320