1 UVOD
Bilateralno teleoperiranje označuje teleoperiranje s
povratno silo. Tako zagotovi operaterju poleg vizualne
in zvočne informacije tudi informacijo o reakcijski sili
okolice [1]. Praviloma je bilateralni teleoperator
sestavljen iz gospodarja in sužnja. Gospodar je
namenjen interakciji operaterja, suženj pa interakciji z
oddaljenim objektom [2]. Za visoko zmogljivo
bilateralno teleoperiranje sta potrebni informaciji o
položaju in zunanji sili gospodarja in sužnja. Kadar sta
položaja in zunanji sili simultani, je takšen teleoperator
transparenten, tako da je impedanca oddaljenega objekta
idealno prenesena do operaterja [3]. V tem primeru je
dosežena idealna haptična zaznava. Za zmogljivo
bilateralno teleoperiranje je treba zagotoviti refleksijo
reakcijske sile z visoko vernostjo.
Visoka haptična vernost med izvajanjem zahtevnih
nalog z bilateralnim teleoperiratorjem je izjemno
pomembna. Za to pa je treba zagotoviti zadovoljivo
informacijo o položaju gospodarja in sužnja. Članek
opisuje αβ-sledilnik s fazno zaklenjeno zanko (ang.
Phase Locked Loop - PLL) za estimacijo položaja in
hitrosti. V naših prejšnjih raziskavah smo predstavili
psevdo-brezsenzorski pristop za bilateralno
teleoperiranje [4]. Informacija o položaju je pridobljena
z analognimi Hallovimi senzorji in namenskim
algoritmom za estimacijo hitrosti in zunanje sile.
Vendar pa takšen algoritem zahteva ustrezen nizko-
pasovni filter, ker je prisoten šuma. Implementacija
takšnega filtra pa lahko omeji pasovno širino vodenja.
Fazno zaklenjena zanka (PLL) praviloma zagotavlja
visoko robustnost na šum [5]. Nekaj raziskovalcev jo je
uporabilo pri aplikacijah za vodenje motorja z visoko
natančnostjo. Emura je predstavil kvadraturni PLL, pri
katerem je uporabil inkrementalni dajalnik [6]. V tem
članku predstavljamo kombinacijo αβ- sledilnika in PLL
koncepta. Takšen pristop omogoča poenostavitev
algoritma in »fleksibilnejšo« implementacijo. To je še
posebej pomembno pri implementaciji z vezjem FPGA
[7].
2 REGULACIJSKI ALGORITEM ZA
BILATERALNO VODENJE
V članku je uporabljen robustni algoritem za bilateralno
vodenje, ki je izpeljan po postopku vodenja v drsnem
režimu [8]. Virtualni način vključuje skupni in
Prejet 10. oktober, 2013
Odobren 12. november, 2013
mailto:marko.franc@gmail.com
148 HACE, FRANC
diferencialni način, ki omogoča sledenje po položaju in
sili. Ta načina sta razklopljena, kar bistveno poenostavi
načrtovanje vodenja. Regulacijski algoritem zagotavlja
delovanje brez drhtenja (ang. chattering-free) in
preprosto implementacijo. Visoka robustnost na motnje
je zagotovljena z robustnim regulatorjem praktično brez
modela.
Zakon vodenja je opisan z
1
f MT u

  (1)
0
( )u u u x
t
eq eq vD dt   , (2)
kjer je u=[uc,ud]
T
in , ,[ , ]u
T
eq eq c eq du u , , /eq c c cu f M
in , ( )eq d v d p du k x k x   , x Txv  ,  ,x
T
m sx x
1 1
1 1
T
 
  
 
. (3)
( , )M m sdiag m m  je masna matrika bilateralnega
teleoperatorja. mm, ms, xm, xs, fm in fs so mase, položaji in
sile vodenja gospodarja in sužnja. fh je akcijska sila
operaterja in fe je reakcijska sila okolice. cx , dx , cf  in
cf  so položaji in sile v virtualnem načinu. Indeksa (.)c,
in (.)d označujeta skupni in diferencialni način. cu  in du
označujeta izhod robustnega regulatorja. Regulacijski
parametri cM , D , pk , in vk  označujejo virtualno
maso, parameter robustnosti, položajno ojačenje in
ojačenje hitrosti v virtualnem načinu. Blokovna shema
robustnega algoritma za bilateralno vodenje je prikazana
na sliki 1.
3 MERITEV HITROSTI
3.1 αβ-sledilnik
Poglavje opisuje estimacijo položaja in hitrosti z
uporabo analognih Hallovih senzorjev in αβ-sledilnika.
Slednji je poenostavljena oblika Kalmanovega
opazovalnika [9]. Obravnavati ga je mogoče kot
rekurzivni filter, ki omogoča estimacijo položaja in
hitrosti v zelo širokem območju. Prednost takšne
estimacije položaja in hitrosti je tudi v preprosti
implementaciji. To je še posebej pomembno, kadar je
količina strojnih virov omejena [10].
Algoritem αβ-sledilnika je sestavljen iz dveh delov:
modela, ki daje napoved in korekcijo, s pomočjo katere
se izračunajo izhodne vrednosti. Pri izračunu je
predpostavljena konstantna hitrost znotraj tipalnega
intervala. Napoved položaja kx  in napoved hitrosti kv
opisujeta enačbi
1 1ˆ ˆk k s kx x T v    (4)
1ˆk kv v  , (5)
kjer sta 1ˆkx   in 1ˆkv   ocenjen položaj in ocenjena hitrost
v časovnem odtipku 1k  . Korekcija napovedi položaja
in korekcija napovedi hitrosti je podana z
ˆ ( )k k k kx x x x    (6)
1ˆ ˆ / ( )k k s k kv v T x x   . (7)
Blokovna shema αβ-sledilnika je prikazana na sliki 2.
Po preureditvi enačb (4)-(5) se lahko zapiše ocena
položaja in hitrosti z
1 1 1 1ˆ ˆ ˆ ˆ ˆ( )k k s k k k s kx x T v x x T v         (8)
1 1 1ˆ ˆ ˆ ˆ( )( )k k s k k s kv v T x x T v      . (9)
Vrednosti   in   sta pozitivni konstanti, ki sta izbrani
tako, da αβ-sledilnik opisuje limitni Kalmanov filter [9].
Pri izbiri vrednosti parametrov je treba omeniti
kompromis med natančnostjo sledenja in sposobnost
odstranjevanja šuma. Asimptotično stabilen odziv je
zagotovljen, če sta   in   izbrana v območju
0 1   in
2
0 / (2 )     . Vrednosti so
določene glede na lego polov, ki jih poda krivulja lege
korenov na podlagi prenosnih funkcij (10)-(11). Poli so
lahko realni ali kompleksni. Podroben postopek izbire
polov je opisal Čadonič v [11].
2
2
ˆ ( )
( 2) (1 )
x z z
x z z
  
  
 

    
(10)
Slika 2: Blokovna shema αβ-sledilnika
Slika 1: Blokovna shema robustnega algoritma za
bilateralno vodenje
IZBOLJŠANJE HAPTIČNEGA TELEOPERIRANJA Z UPORABO PLL αβ-SLEDILNIKA 149
2
2
ˆ
( 2) (1 )s
v z z
x T z z

  


    
(11)
3.2 PLL αβ-sledilnik
PLL αβ-sledilnik je izpeljan iz splošne strukture αβ-
sledilnika. αβ-sledilnik je modificiran tako, da vključuje
tudi nekatere značilnosti PLL koncepta, za katerega je
značilna visoka robustnost na signalni šum [5]. V
nekaterih primerih pa lahko tudi poenostavitev
implementacije na FPGA vezju.
Splošno blokovno shemo PLL sestavljajo: i) detektor
faze (PD), ii) nizkopasovni filter (LPF) in iii)
napetostno vodeni oscilator (VCO) (glej sliko 3).
PD je križni produkt med vhodnim parom
sinusoidnih signalov [ , ]a bu u  in izhodnim parom
sinusoidnih signalov [sin( ),cos( )]x x . Sestavljen je iz
dveh množilnikov in seštevalnika. Vhodni par so
transformirane napetosti analognih Hallovih senzorjev,
ki so potrebne pri algoritmu za izračun položaja [4].
Transformacija je izvedena z uporabo Clarkine
transformacije.
VCO izračuna sinusni in kosinusni signal glede na
filtrirani vhodni podatek kx . Kadar [sin( ),cos( )]x x
sledi [sin( ),cos( )]x x , je zanka zaklenjena in je izhodni
par zaklenjen na vhodni par. Kadar je to doseženo, PLL
ohranja zaklenjeno stanje ( x  je zaklenjen na x ).
Zaklenjeno stanje lahko ohranja tudi tedaj, ko je na
signalu prisoten šum.
Blokovna shema PLL αβ-sledilnika je prikazana na
sliki 4. Takšna blokovna shema vključuje vse
komponente splošne PLL strukture (PD, LPF in VCO).
Glede na αβ-sledilnika je pri PLL αβ-sledilniku vhod
zamenjan s sinusnim in kosinusnim signalom.
PD na sliki 4 implementira križni produkt med
vhodnim parom sinusnih signalov [ , ]a bu u  in izhodnim
parom sinusnih signalov [sin( ),cos( )]x x . Sestavljen je
iz dveh množilnikov in seštevalnika tako, da z
cos( ) sin( )
sin( )cos( ) cos( )sin( ) sin( )
e a bx u x u x
x x x x x x
  
   
(12)
izračuna signal ex , ki vključuje informacijo o faznem
zamiku. Kadar x  sledi x  tako, da velja x x , je zanka
zaklenjena.
Estimacija položaja in hitrosti (ang. Position and
velocity estimation - PVE) izračuna estimacijo položaja
ˆ
kx , estimacijo hitrosti ˆkv  in napoved položaja kx . PVE
izhaja iz αβ-sledilnika tako, da se enačbi αβ-sledilnika
(8) in (9) preoblikujeta v
ˆk k ex x x   (13)
1ˆ ˆ ( / )k k s ev v T x  . (14)
Načrtovanje vrednosti   in   za PLL αβ-sledilnik
poteka enako kot pri αβ-sledilniku (glej poglavje 3.1).
Lomna frekvenca PVE mora biti dovolj visoka, da je
zanka sposobna zagotoviti sledenje vhodnega signala
(zaklenjeno stanje). V nasprotnem primeru, kadar zanka
ni sposobna slediti vhodnemu signalu (kadar pade iz
zaklenjenega stanja), estimacija položaja in hitrosti ni
uporabna.
3.3 Primerjava αβ-sledilnika in PLL αβ-sledilnika
Prednost PLL αβ-sledilnika je, da je takšen sledilnik
sposoben ohranjati zaklenjeno stanje tudi pri prehodu
magnetnih polovih parov in kadar je na signalu prisoten
šum. Algoritem za štetje prehodov polovih parov ni
potreben.
Pomembna prednost PLL αβ-sledilnika je
implementacija na vezje FPGA. Ta poteka praviloma z
uporabo zapisa števil s fiksno vejico, vendar pa je
takšen izračun pomemben vir pogreška oz. računskega
šuma (ang. Computation noise). Vsaka računska
operacija vnaša pogrešek in lahko pri večjem številu
računskih operacij celo ogrozi delovanje sistema.
Takšen pogrešek zmanjša večja resolucija pri zapisu
števil, vendar pa je za to potrebne več strojne opreme.
Prednost vezja FPGA je fleksibilnost pri
implementaciji. V praktičnih aplikacijah so atan2, cos
in sin implementirani z uporabo algoritma CORDIC  Pri
takšni implementaciji je poraba strojnih virov
primerljiva (1185 LUT za 16-bitni atan2 in 1093 LUT
za cos in sin [13]). Dodatno pa omogoča implementacija
sin in cos več fleksibilnosti. V [14] je bilo
predstavljenih več pristopov implementacije sin in cos.
Poleg tega ponujajo izdelovalci vezij FPGA vpogledne
tabele za sin in cos, ki predstavljajo preprosto
Slika 4: Blokovna shema PLL αβ-sledilnika
Slika 3: Splošna blokovna shema fazno zaklenjene zanke
(PLL)
150 HACE, FRANC
implementacijo. Zato je mogoče doseči želeno
implementacijo glede na uporabljen strojni vir (register,
vpogledne tabele in DSP enote). Vendar pa PLL αβ-
sledilnik potrebuje za izračun PD dve množenji več kot
αβ-sledilnik, kar je razvidno v tabeli 1 pri porabi
namenskih enot DSP48s. PLL αβ-sledilnik potrebuje
tudi začetne vrednosti za 0x . Te se lahko določijo ob
začetku merjenja brez uporabe strojnih virov, kar je
izjemno pomembno pri implementaciji z vezjem FPGA.
Tabela 1: Poraba strojnih virov pri implementaciji αβ-
sledilnika in PLL αβ-sledilnika
Strojni viri FPGA
vezja
αβ-sledilnik PLL αβ-
sledilnik
Logični bloki 771 605
Registri 1275 1207
Vpogledne tabele 1456 1330
DSP48s 7 9
Slika 5 prikazuje simulacijske rezultate primerjave
bilateralnega teleoperiranja z uporabo αβ-sledilnika in
PLL αβ-sledilnika. Zgoraj sta prikazani dejanska
vrednost hitrosti in hitrost, izračunana z uporabo αβ-
sledilnika in PLL αβ-sledilnika. Spodaj sta prikazani
dejanski sili operaterja in objekta in izračunani sili z
uporabo opazovalnika sile [4]. V času do 0.5s je
uporabljen αβ-sledilnik, v času do 1s pa PLL αβ-
sledilnik. Prikazano je, da računski šum bistveno
poslabša delovanje sistema pri uporabi αβ-sledilnika.
Takšna estimacija položaja in hitrosti lahko izboljša
prenos oddaljenega občutka dotika pri bilateralnem
teleoperiranju.
4 EKSPERIMENT
4.1 Eksperimentalni sistem
Delovanje PLL αβ-sledilnika smo validirali s preprostim
laboratorijskim sistemom 1-DoF za bilateralno
teleoperirenje. Načelna zgradba sistema je prikazana na
sliki 6. Sestavljen je iz gospodarja in sužnja, ki sta
predstavljena z linearnim morjem Faulhaber LM1247-
080-01 (glej sliko 7). Linearni motor vključuje tri
analogne Hallove senzorje, ki so bili uporabljeni za
estimacijo položaja in hitrosti. Akcijska sila operaterja
in reakcijska sila oddaljenega objekta sta izračunani z
uporabo opazovalnika sile [11]. Sila vodenja pomeni
referenčno vrednost gonilniku linearnega motorja.
Robustni algoritem za bilateralno vodenje in algoritmi
zajema podatkov so bili implementirani z NI PXI-
7841R s FPGA Virtex-5. Konfiguracija FPGA
logičnega vezja in uporabniški vmesnik sta bila
načrtovana v programskem okolju LabVIEW.
Tabela 2: Parametri manipulatorja in regulatorja
Parameter
Masa gospodarja mm g 80
Masa sužnja ms g 35
Lomna frekv. PLL αβ-sledilnika rad/s 350
Koeficient robustnosti D 1/s 350
Položajno ojačenje kp 1/s
2
65000
Ojačenje hitrosti kv 1/s 200
Virtualna masa Mc g 35
Perioda vodenja Tc μs 10
Slika 5: Simulacijski rezultati primerjave bilateralnega
teleoperiranja z uporabo αβ-sledilnika in PLL αβ-sledilnika
Slika 6: Načelna zgradba eksperimentalnega sistema za bilateralno teleoperiranje
IZBOLJŠANJE HAPTIČNEGA TELEOPERIRANJA Z UPORABO PLL αβ-SLEDILNIKA 151
4.2 Eksperimentalni rezultati
Validacija bilateralnega teleoperiranja vključuje prosto
gibanje, dotik mehkega objekta in dotik trdega objekta.
Parametri manipulatorja in regulatorja so prikazani v
tabeli 1. Eksperimentalni rezultati so prikazani na sliki
8, ki zajema prosto gibanje (slika 8a), dotik mehkega
objekta (slika 8c) in dotik trdega objekta (slika 8b).
Položaj gospodarja in akcijska sila operaterja sta
prikazani z rdečo neprekinjeno črto, položaj sužnja in
reakcijska sila oddaljenega objekta pa z zeleno
prekinjeno črto.
Pri prostem gibanju in pri dotiku mehkega objekta je
položajno sledenje in sledenje zunanjih sil odlično. Pri
dotiku trdega objekta je mogoče opaziti pogrešek pri
položajnem sledenju in odlično sledenje zunanjih sil.
Glavni vzroki za pogrešek pri položajnem sledenju je i)
omejena pasovna širina komercialno dobavljivih
gonilnikov linearnih motorjev, ii) meritev vhodnih
signalov ni idealna, ampak vključuje tudi šum in tudi iii)
nizka dinamika aktuatorjev, ki ni obravnavana pri
načrtovanju vodenja. To omejuje prenos impedance
oddaljenega objekta z visoko dinamiko, ki je potrebno
pri dotiku trdega objekta. Teleoperator je visoko
transparenten pri dotiku mehkega objekta.
Delovanje sistema je bilo validirano tudi s primerjavo
realne trdote mehkega objekta in trdote, ki jo čuti
Slika 7: Eksperimentalni sistem za bilateralno teleoperiranje
0.1 0.2 0.3 0.4
0
5
10
P
o
lo
z
a
j
[m
m
]
gospodar suzenj
0.1 0.2 0.3 0.4
-2
0
2
Cas [s]
S
il
a
[
N
]
f
h
f
e
0.8 1 1.2 1.4
-0.2
0
0.2
0.4
P
o
lo
z
a
j
[m
m
]
gospodar suzenj
0.8 1 1.2 1.4
0
1
2
3
Cas [s]
S
il
a
[
N
]
f
h
f
e
a) Prosto gibanje b) Dotik trdega objekta
1.5 2 2.5
-2
0
2
P
o
lo
z
a
j
[m
m
]
gospodar suzenj
1.5 2 2.5
0
1
2
3
Cas [s]
S
il
a
[
N
]
f
h
f
e
0 0.5 1 1.5 2
0.5
1
1.5
2
S
il
a
[
N
]
Polozaj [mm]
operater
okolica
c) Dotik mehkega objekta d) Trdota pri dotiku mehkega objekta
Slika 8: Eksperimentalni rezultati za prosto gibanje (a), dotik trdega objekta (b), dotik mehkega objekta (c) in trdota pri dotiku
mehkega objekta (d)
152 HACE, FRANC
operater (glej sliko 8d). Naklon pomeni trdoto mehkega
objekta (neprekinjena rdeča črta) in trdoto
teleoperatorja, ki jo čuti operater (neprekinjena zelena
črta). Pokrivanje trdote objekta in trdote, ki jo čuti
operater, določa transparentnost teleoperatorja. Pri trdoti
je opazen tudi razmik, ki pa je posledica relativno
počasnega objekta. Razvidno je, da je teleoperator zelo
transparenten pri dotiku mehkega objekta.
5 SKLEP
Predstavljen je bil PLL αβ-sledilnik za estimacijo
položaja in hitrosti. Takšna estimacija omogoča visoko
zmogljivo bilateralno teleoperiranje. Prednost pred
predhodno opisanim αβ-sledilnikom je odprava
algoritma za zaznavo in štetje prehodov magnetnih
polovih parov. Posledično ni več večkratnih prehodov
(ang. dither). Poleg tega omogoča implementacija
funkcij sin in cos večjo fleksibilnost glede na optimalno
porabo strojnih virov in hitrost procesiranja kot
implementacija funkcije atan2. Takšna implementacija
je lahko v nekaterih primerih vodenja obsežnih
mehatronskih sistemih ključen dejavnik. Poleg tega je
pomembna prednost večja neobčutljivost na računski
šum. Ta lahko bistveno poslabša delovanje sistema.
Predlagan pristop estimacije položaja in hitrosti lahko
izboljša prenos oddaljenega občutka dotika pri
bilateralnem teleoperiranju.
Predlagan pristop je bil validiran s preprostim
eksperimentalnim sistemom. Eksperimentalni rezultati
prikazujejo odlično položajno sledenje in sledenje
zunanjih sil v vseh režimih delovanja, pri postem
gibanju, pri dotiku mehkega objekta in pri dotiku trdega
objekta. Pri dotiku trdega objekta nastane relativno
majhen položajni pogrešek. Vendar pa je razvidno, da je
teleoperator zelo transparenten pri dotiku mehkega
objekta.