ON-VEHICLE VISUAL TRACKER WITH
DISTURBANCE REJECTION
F. G´omez-Es e n ∗F.R. Rubio ∗J. A acil ∗
∗Dep . Ingenie ´ıa de Sis emas y Au om´a ica
Escuela Supe io de Ingenie os. Uni e sidad de Se illa
Camino de los Descub imien os s/n. 41092-Se illa. SPAIN
e-mails: gs@ca uja.us.es,{ ubio, a acil}@esi.us.es
TEL: +34 954487346 FAX: +34 954487340
Abs ac : This pape p esen s he syn hesis and implemen a ion o a passi i y
based con olle designed o isual acking o mo ing objec s in a dis u bance
ejec ion scheme. The pu pose o his applica ion is o con ol he line o sigh o
a 2-DOF senso pla o m in a mo ing coo dina e ame, when he mo emen o
he ehicle ca ying he pla o m ac s as a dis u bance o fini e ene gy. Ex e nal
o ques due o ehicle mo emen downg ade he isual acking pe o mance and
can cause ins abili y and o al loss o sigh o he objec i e. These dis u bances
can be compensa ed o in an effec i ely manne in oducing a classical L2-gain
dis u bance ejec ion scheme, widely deployed in ine ial ame obo ic sys ems.
The p oposed con olle has been es ed in a eal 2-DOF pla o m moun ed on a
mo ing es bench ha emula es diffe en ypes o non-ine ial ehicle su aces.
Keywo ds: Nonlinea sys ems, Visual acking, Mobile obo ics, Passi i y–based
con ol
1. INTRODUCTION
In ecen wo ks, he heo ies o passi e and po Hamil-
onian sys ems ha e ui ully gi en ise o a se o
ools ha succes ully ackle he dis u bance supp es-
sion p oblem in elec omechanical sys ems. In (Van de
Scha , 1989) he p ope ies o po -con olled Hamil o-
nian sys ems (PCH) ha e been s a ed (see also mo e spe-
cific app oaches (Wang and Li, In e nal epo 2002) o
mul imachine powe sys ems and (Slo ine and Li, 1988)
o obo ic sys ems). These ools p o ide a bounded-
dis u bance bounded-ou pu con ol in he sense o he
L2gain.
On he o he hand, on- ehicle senso s like sa elli e an en-
nas and isual acking sys ems a e inhe en ly affec ed
by undesi ed pe u ba ions due o mo emen s in he
coo dina e ame whe e hey li e. These pe u ba ions
appea as Co iolis and cen i ugal o ces ypical o non-
ine ial ames. Consequen ly, he p oblem o keeping a
s a ic line o sigh in his ype o senso is a dis u bance
ejec ion p oblem, whe e he dis u bance can be caused
by ehicle mo emen s o by displacemen s o he a ge
poin (e.g. non-geos a iona y sa elli e). When acking
objec i es a e a , he o me case is he one ha mo e
se e ely affec s he s abili y o he line o sigh and hese
will be he ones s udied in his wo k. In essels, he
a o emen ioned dis u bance is mainly due o sea wa es
and a e usually app oxima ed wi h sinusoidal unc ions.
This ype o signals, when ampli udes a e low, a e well
handled in he L2dis u bance a enua ion amewo k.
We will s udy a 2-DOF pla o m designed o essel
guidance and hence pe u bed by wa e-induced mo e-
men s o he ship su ace. Wi h a labo a o y se up ha
physically emula es diffe en ypes o pe u ba ion, we
will p esen and analyze a isual se oing sys em based
on he Slo ine and Li con olle (Slo ine and Li, 1988)
ha gua an ees L2dis u bance ejec ion p ope ies.
IAV2004 - PREPRINTS
5 h IFAC/EURON Symposium on In elligen Au onomous Vehicles
Ins i u o Supe io Técnico, Lisboa, Po ugal
July 5-7, 2004
316
Wi h he aim o s udying he p ope ies and effec s
o on- ehicle pe u ba ions, a wo deg ee o eedom
pla o m has been se up on a des abilizing 2-DOF desk.
A isual se oing sys em has been implemen ed wi h a
CCD came a. When analyzing expe imen ally he effec
o he eal dis u bances on he isual acking loop, a
subs an ial imp o emen o he o e all beha io has been
achie ed wi h he p oposed con olle . These esul s a e
p esen ed g aphically.
This pape is o ganized as ollows: in Sec ion 2, he
simplified sys em model o he pla o m is gi en. In
Sec ion 3, he new con ol s uc u e is p esen ed, di ided
in isual es ima o and ajec o y acke . In Sec ion
4, echnical de ails o he physical se up and con olle
implemen a ion a e gi en. Sec ion 5 p esen s some ex-
pe imen al esul s. Finally, a se o conclusi e ema ks
a e gi en in Sec ion 6.
2. SYSTEM MODEL
The sys em consis s o a pla o m wi h wo deg ees
o eedom imme sed in a non ine ial e e ence sys-
em emula ed by a des abilizing desk. The syn hesis o
con olle s o his pla o m aises impo an challenges
like he obus s abiliza ion ejec ing dis u bances, isual
acking o mo ing objec s and gy oscopic s abiliza ion
o compensa e o he coo dina e ame mo emen s.
The model used (G´omez-Es e n e al., 2000) co esponds
o he 2-DOF pla o m shown in Fig.1. I is composed o
wo main bodies: he base, whose posi ion is de e mined
by he azimu h angle ϕand he head body whose coo di-
na e is he ele a ion angle θ. In ou expe imen al se up,
his sys em is moun ed on a mechanical desk capable o
u ning in wo axes. The whole sys em has ou deg ees
o eedom, bu only he wo local coo dina es ( hose
o he acke pla o m) a e a ailable om he buil -in
encode s. The e o e only he 2-DOF pla o m kinema ics
a e conside ed, while any coupling effec due o he
mo ing su ace will be ea ed as a dis u bance (G´omez-
Es e n e al., 2003) .
The load o he ele a ion axis is concen a ed along
he sha , hence he po en ial ene gy is in a ian and
he e o e disca ded om he equa ions. The o igin o
he azimu h angle is a bi a y. The a iable θis ze o
when bo h bodies a e pe pendicula . The gene alized
coo dina es o he sys em a e (ϕ,θ).
The equa ions o mo ion a e easily ob ained in Hamil o-
nian o m. Unde he assump ion s a ed abo e, he o al
ene gy o he plan is educed o he kine ic ene gy:
1
2˙ϕ˙
θΓ(θ)0
0Iyy2˙ϕ
˙
θ(1)
The e m Γ(θ)s ands o
Γ(θ)Izz1+Ixx2sin2(θ)+Izz2cos2(θ)(2)
Fig. 1. Two deg ee-o - eedom pla o m.
whe e Izz1is he zaxis ine ia o he base, and Ixx2,Iyy2
and Izz2a e he second body (ele a ion) ine ia momen a
wi h espec o he x,yand zaxes espec i ely.
3. VISUAL TRACKING IN NON INERTIAL
VEHICLES
The isual acking p oblem in his pla o m can educed
in some cases o he s abiliza ion o equilib ia. Indeed,
i he objec o be acked (e.g. s a iona y sa elli e) is
fixed o has ela i ely slow mo ion, and he pla o m is
ins alled on a s a ic su ace, an equilib ium s abiliza ion
con olle wi h slowly a ying se poin will do he
job. The se poin is es ima ed by an image p ocessing
algo i hm.
Howe e i he sys em is imme sed in a as o a ing
coo dina e ame, as is he case o a ehicle an enna, he
pe o mance o a se poin s abiliza ion con olle wi h
a ying e e ence is quickly downg aded e en o slow
mo ions, as small line o sigh e o s due o dis u bances
a eamplifiedwhen he ackedobjec sa edis an .The
la e case is a a mo e challenging acking p oblem.
Summa izing, he angles o he line o sigh o he a ge
wi h espec o he came a mo e by one o wo causes
•Visual acking o mobile objec s om a pla o m
on a fixed g ound. This p oblem can be ackled as
apa icula case o he ollowing.
•Fixed o mobile objec acking om a pla o m
moun ed on boa d o a g ound ehicle o a essel.
The objec i e is o ake o ze o he angula eloci y
ec o o he line- o- a ge when i is e e ed o
he came a coo dina e ame. This ec o is ac ually
he composi ion o he mo ion o he ehicle (o sea
wa es) and he eac ion o he 2-DOF pla o ms in
o de o compensa e i .
As he second o hese si ua ions is mo e gene al, his
will be he one s udied in his pape .
317
Rada
Video
Encode /gy o
Passi i y based
ajec o y acke
Visual algo i hm o
ajec o y compu a ion
D( )
x( ),y( )
q( ),q'( )
qd( ),q'd( )
Dis u bance (wa e mo ion)
+
+
u
Senso Pla o m
Fig. 2. Passi e isual acking scheme.
The con ol p oblem will be di ided in wo pa s
(G´omez-Es e n, 2003). Fi s , a passi i y-based ajec-
o y acking sys em wi h dis u bance supp ession is
designed exploi ing de ailed knowledge o he mechanical
s uc u e o he pla o m. Then, a isual acking algo-
i hm es ima es he de ia ion o he acked objec om
he line o sigh and, based on geome ic conside a ions,
i gene a es he e e ence ajec o y o be acked by he
passi e sys em. The p oposed scheme is illus a ed in
Fig. 2.
3.1 Visual acking
The design o he fi s block consis o calcula ing he
desi ed ajec o y o he pla o m coo dina es, qd( )=
(ϕd( ),θ
d( )) in such a way ha he image o he mo ing
objec s abilizes a he cen e o he sc een. The inpu o
his block is a 2D ec o wi h he (x, y)coo dina esin
he sc een, e e ed o he o igin a he cen e poin . A
simple image scanning algo i hm compu es his ec o
a a 10ms a e. We will assume ha he e is a unique
ajec o y qd( )∈C
2in he local pla o m coo dina es
such ha
q( )≡qd( )⇒(x( ),y( )) = (0,0),∀ >0
This ajec o y is ob iously unknown, as i depends on
he mo ion o he coo dina e ame o he ehicle and he
mobile a ge .Howe e ,aswillbeseen, heknowledge
o he p ojec ion o he a ge on he sc een is enough o
compu e he desi ed ajec o y qd( ) and an es ima e o
he de i a i es a any ins an .Onceqd( )isknown,
a passi i y-based con olle will be designed in o de
o ensu e asymp o ic s abili y o he ajec o y and,
hence o h
lim
→∞
(x( ),y( )) = (0,0)
The sc een coo dina es o he acked objec (x,y)canbe
iewed as a measu e o he acking e o , since hese a e
ze o i and only i he objec is pe ec ly acked. Howe e
i is necessa y o exp ess his e o in e ms o he
angles o he pla o m join s, (ϕ, θ). These can be easily
ob ained om (x,y)as ollows.Assume(X, Y, Z) a e he
3D coo dina es o he a ge in he came a coo dina e
ame (see Fig. 3). By iangle equi alence, we ha e By
iangle equi alence, we ha e
x
d=X
Z
y
d=Y
Z
whe e dis he dis ance o he plane o he image o
he iewe ( his pa ame e can be calib a ed ollowing
came a ins uc ions). A e some manipula ions, he 3D
coo dina es a e isola ed
X=xD
d2+x2+y2
Y=yD
d2+x2+y2
The azimu h and ele a ion e o s o he came a in e ms
o xand ya e gi en by
eϕ= a c an X
Z=a c anx
d=ϕd−ϕ
eθ= a c an Y
Z=a c any
d=θd−θ
De i ing his exp ession wi h espec o ime, we ha e
˙eϕ=d
d a c an X
Z=d
d a c an x
d
=˙xd
d2+x2=˙ϕd−˙ϕ
˙eθ=d
d a c an Y
Z=d
d a c an y
d
=˙yd
d2+y2=˙
θd−˙
θ,
F om whe e we ob ain he desi ed ajec o y o he
pla o m angles a any ins an
qd( )=ϕ( )
θ( )+eϕ( )
eθ( )(3)
˙qd( )=˙ϕ( )
˙
θ( )+˙eϕ( )
˙eθ( )(4)
318
Y
Z
d
Side iew
Uppe iew
Z
d
X
y
x
Image p ojec ion plane
Image p ojec ion plane
Fig. 3. Sc een and eal coo dina es.
The gene alized coo dina es (ϕ( ),θ( )) a e measu ed
in eal ime wi h he buil -in encode s o he pla o m
independen ly o he Eule angles o he ehicle. This
eal ime compu ed a ge ajec o y will be he e e ence
o he passi i i y-based ajec o y acking con olle
whose design will be illus a ed in he ollowing.
3.2 T ajec o y acking subsys em
In o de o ge he ajec o y acking we will s a om
he Eule –Lag ange equa ions, and use he celeb a ed
esul s o Slo ine and Li (Slo ine and Li, 1988). Gi en
he nolinea ine ia ma ix o he pla o m
M=Γ(θ)0
0Iyy2(5)
we ob ain he dynamic equa ions in open loop using he
Eule -Lag ange desc ip ion o obo ic sys ems
M(q)¨q+C(q, ˙q)˙q=τ, (6)
whe e no po en ial ene gy e ms a e being conside ed be-
cause he head o he pla o m is mechanically balanced
by cons uc ion (in o de o educe ene gy demand). I
hey we e nonze o he ea men would be equi alen ex-
cep in he ac ha an inclinome e would be necessa y
o measu e he eal angles o he pla o m wi h espec
o an ine ial ame, in o de o p ope ly measu e and
compensa e g a i y effec s. In o de o asymp o ically
ack he e e ence ajec o y qd( ) we will apply he
con ol law
τ=M(q)˙
ξ+C(q, ˙q)ξ+ν(7)
whe e
ξ=˙qd−Λ(q−qd)(8)
and Λ = ΛT>0. Subs i u ing in (7) yields
M(q)˙s+C(q, ˙q)s=ν(9)
whe e s˙q−ζ. Defining he ene gy unc ion as
H(s, q)=1
2sTM(q)s(10)
hen, along he ajec o ies (9)
d
d H=sTM(q)˙s+1
2sT˙
Ms
=−sTCs +1
2sT˙
Ms+sTν=sTν(11)
The las exp ession is due o he skew-symme y o he
ma ix ( ˙
M−2C). The sys em is dissipa i e wi h espec
o (sTν) and ep esen s a passi e ν→ sbecause H≥0,
o any ini ial condi ion. I , addi ionally, we define
ˆν=Ks (12)
whe e K=KT>0 is a posi i e defini e ma ix ha
Fig. 4. Con ol s uc u e o ajec o y acking
appea s in he eedback block o Fig.4. Then we ha e
ha
P oposi ion 1. The con ol law
319
τ=M(q)[¨qd−Λ( ˙q−˙qd)]
+C(q, ˙q)( ˙qd−Λ(q−qd))
+Kd[˙q−˙qd+Λ(q−qd)]
whe e Kd=KT
d>0andΛ=Λ
T>0 a e uning pa am-
e e s, and qd( ) is he compu ed ajec o y ha mus be
acked, asymp o ically s abilizes he e o e( )=q( )−
qd( )in heo igin
Fo mo e de ails on he p oo we e e he eade o (Van
de Scha , 1989).
3.3 Dis u bance ejec ion in ajec o y acking
Thep oposedschemeisbasedon hewellknownSlo-
ine and Li con olle (Slo ine and Li, 1988), which in
p inciple lacks o quan ifiable dis u bance supp ession
p ope ies when he posi ion e o o he obo is aken
as he ou pu o he sys em. A known ac om he
li e a u e (see (Sadegh and Ho owi h, 1990)) is ha he
addi ion o a e m p opo ional o he posi ion e o
o he Slo ine and Li con olle p oduces a dis u bance
supp ession beha oi in he L2-gain sense om he inpu
o he ou pu e( )=qd( )−q( ) wi h a a enua ion le el
γa bi a ily small in he case o ully ac ua ed sys ems.
Hence he p e ious p oposi ion is ans o med in o
P oposi ion 2. Conside he 2-DOF pla o m wi h he
con ol law
τ=M(q)[¨qd−Λ( ˙q−˙qd)]
+C(q, ˙q)( ˙qd−Λ(q−qd))
+Kd[˙q−˙qd+Λ(q−qd)] + Kp(q−qd)
wi h he same cons an s defined in p oposi ion 1, and
suppose, addi ionally, ha he ollowing condi ions hold:
Kd=1
21
γ2+1+λ
Kp=1
2γ2+1+λ.
Then, he L2-gain om he inpu τ o he ou pu (qd−q)
is less o equal o he a bi a ily defined gain γ,and
he ansien beha io can be adjus ed using he posi i e
scala λ.
The p oo o his esul is based in ha o (Sadegh and
Ho owi h, 1990). We will use i o i s implemen a ion
in he eal 2-DOF pla o m inside he isual eedback
loop. Bu fi s ly we will desc ibe, in he ollowing sec ion,
how he in o ma ion is con eyed among he diffe en
con olle blocks.
4. IMPLEMENTATION
Figu e 5 shows he expe imen al se up. The pla o m
u ns in wo axis powe ed by wo b ushless DC mo o s.
The des abilizing desk is also 2-DOF and is powe ed by
h ee-phase mo o s con olled wi h a eloci y a ia o .
Themeasu emen so heposi ionsenso s(diffe en ial
encode s o he desk and inclinome e s o he desk)
and eloci ies (gy oscopes) a e ed back in o a PC ha
con ains wo dSpaceTM DSP ca ds (one o each axis)
based on he DSP TMS320C31 p ocesso and equipped
wi h a con ol de elopmen en i onmen Con olDesk.
Fig. 5. Expe imen al se up.
Fo he isual acke we ha e de eloped and simple al-
go i hm o image scanning unning on a spa e PC. Such
algo i hm is a C++ ou ine ha de e mines he sc een
coo dina es o he acking a ge (x,y), in ou expe i-
men jus a black ci cle p in ed on a whi e backg ound.
This image is fi s used o calib a e he pa ame e s o
he came a. The acqui ed da a is packed in o an UDP
da ag am wi h a ime s amp and sen ia E he ne o
he pee compu e , whe e he dSpace ca ds a e unning
he acking con olle in connec ion wi h he pla o m
senso s and ac ua o s. In o de o check he alidi y o
he p oposed scheme, i has been obse ed ha he o al
ime in acqui ing an image, iden i ying he angles o he
a ge , and sending he da ag am, does no exceed a o al
ime o 20ms. Thus, wi h a sample pe iod o 100ms he
ji e due o unmodeled delays is no la ge han 20% o
he sampling ime.
The PC ac ing as con olle ecei es he UDP packe s
(no packe losses ha e been accoun ed o ), finds ou
he e o angles om he ecei ed (x,y) coo dina es, and
con eys he da a o he DSP ca ds, whe e he alue
o qdis compu ed acco ding o encode measu emen s
and equa ion (3). In hese p ocesso s, a SimulinkTM
320

RTW 1algo i hm applies he ol age he o que o he
DC se omo o o he co esponding axis .
The physical cons an s o he model ha e been ob ained
h ough leas squa es iden ifica ion o e se e al eco ds
on he s ep esponse cap u ed by he DSP boa ds. As
ic ion will be o some ex en compensa ed o in he
final con olle . he LuG e model pa ame e s ha e been
iden ified as indica ed in (As om K.J., 1995).
5. EXPERIMENTAL RESULTS
The quali y o he expe imen al esul s significan ly in-
c eases when ou con olle is enhanced wi h a eed o -
wa d compensa o o he ic ion phenomena based on
he LuG e model.
Theelemen sdepic edin(6)a e heslowsinusoidal
e e ence p o ided by he isual acke when he des a-
bilizing desk oscilla es in sinusoidal slow mo ion (ma ine
ehicle emula ion), oge he wi h he posi ion gi en by
he encode s. The acking con olle is also a ificially
pe u bedwi ha as sinusoidalsignal(includedin he
figu e) di ec ly added o he DC mo o inpu s. The small
de ia ions be ween he posi ion and he e e ence a e
due o his dis u bance, which is he one o be a en-
ua ed wi h he obus scheme. The L2-gaino he as
dis u bance- o-ou pu map has been adjus ed acco ding
o p oposi ion 2. Al hough he alue o γcan be made
a bi a ily small, in expe imen s we ha e obse ed ha
i should no be chosen below a ce ain alue due o
he appea ance o a sus ained oscilla ion, possibly due
o communica ion delays inducing a limi cycle. As a
ma e o ac any unmodeled oscilla ion is amplified
h ough he de i a i e e m o he con olle , which is
in u n inc eased as pa ame e γgoes smalle .
Fig. 6. Visual acking o a fixed a ge wi h sinusoidal
desk mo ion and addi i e whi e noise dis u bance.
6. CONCLUSIONS
In his wo k we ha e p esen ed he design and p ac i-
calimplemen a iono acon olle basedon heSlo ine
1Real Time Wo kshopTM.
and Li scheme imme sed in a isual eedback loop wi h
E he ne da a ansmission. The con olle is spli be-
ween image p ocessing, a ge ajec o y compu a ion
and ajec o y acking. The la e block is a passi e
con olle specifically designed o supp ession o unde-
si ed dis u bances up o a le el ha , in heo y, can be
made a bi a ily small.
The main con ibu ion lies in he use o passi i y and
obus ness echniques in a isual se oing amewo k.
In he expe imen al esul s we ha e checked ha well
known con ol echniques designed o o he domains
ha e p o ed o be effec i e in his challenging ype o
applica ion. The ex ension o he echniques o isual
se oing sys ems is na u al and only elies on he com-
pu a ion o a ajec o y o be acked as a esul o a as
image analysis C++ algo i hm.
The powe ul expe imen al se up consis ing o a 2-DOF
senso pla o m moun ed on a 2-DOF des abilizing desk
has allowed us o e i y expe imen al esul s in a mos
ealis ic en i onmen , and his makes us belie e ha he
con olle can be easily upg aded in o he eal g ound
and wa e ehicle wo ld.
Acknowledgemen s. This wo k has been pa ially
unded by he Spanish Minis y o Science and Tech-
nology unde g an s DPI2001-2424, DPI2003-00429 and
HF2001-0126.
REFERENCES
As om K.J., Canudas de Wi C., Olsson H. Lischin-
scky P. (1995). A new model o con ol on sys ems
wi h ic ion. IEEE Tansac ions o Au oma ic Con-
ol 40(3), 419–425.
G´omez-Es e n, F. (2003). PhD Thesis: Con ol de Sis-
emas no Lineales basado en la Es uc u a Hamil o-
niana.. Dep . Ing de Sis emas y Au om´a ica. Uni-
e sidad de Se illa.
G´omez-Es e n, F., J. A acil and F.R. Rubio (2000).
P ac ical implemen a ion o a hamil onian–based 2–
DOF pla o m. In: P oc. IFAC Symposium on Robo
Con ol. Vienna, AU.
G´omez-Es e n, F., M.G. Co dones and F.R. Rubio
(2003). Seguimien o isual basado en pasi idad en
sis emas no ine ciales. In: P oc. o he XXIV Jo -
nadas de Au om´a ica. Leon, Spain.
Sadegh, N. and R. Ho owi h (1990). S abili y and o-
bus ness analysis o a class o adap i e con olle s o
obo manipula o s. In . Jou nal. ob. Res. 9, 74–92.
Slo ine, J.J. and W. Li (1988). Adap i e manipula o
con ol: a case s udy. IEEE T ans. on Au oma ic
Con ol 13(5), 995–1003.
Van de Scha , A.J. (1989). L2-Gain and Passi i y
Techniques in Nonlinea Con ol. Sp inge -Ve lag.
Wang, Y., Cheng D. and Ge Y. Li, C. (In e nal e-
po 2002). Dissipa i e hamil onian ealiza ion and
ene gy-based l2-dis u bance a enua ion con ol o
mul imachine powe sys ems.
321