Elektrotehniški vestnik 80(4): 147-152, 2013 Izvirni znanstveni članek
Izboljšanje haptičnega teleoperiranja z uporabo PLL ap-sledilnika
Aleš Hace1, Marko Franc2
1	Univerza v Mariboru, Fakulteta za elektrotehniko, računalništvo in informatiko, Smetanova 17, 2000 Maribor, Slovenija
2	Isomat d.o.o., Celovška 3, Mežica, Slovenija E-pošta: marko.franc@gmail.com
Povzetek. V delu je obravnavano bilateralno teleoperiranje s poudarkom na informaciji o hitrosti. Predstavljen je fazno zaklenjen a^-sledilnik (PLL a^-sledilnik). Takšen pristop omogoča prednosti pred metodo, ki smo jo predstavili v prejšnjem raziskovalnem delu. Nova metoda omogoča pridobitev zadovoljive informacije o položaju, hitrosti in sili za izvedbo teleoperatorja s haptičnim prikazovalnikom. Glavna prednost predlaganega pristopa je naslovljena na poenostavitev metode in tudi implementacije na digitalnem vezju, npr. vezju FPGA. Takšen namenski haptični prikazovalnik bi lahko bistveno izboljšal operacijo s kirurškim robotom. Predlagan pristop je bil eksperimentalno validiran s preprostim laboratorijskim sistemom 1-DoF za bilateralno teleoperiranje.
Ključne besede: PLL, estimacija hitrosti, bilateralno vodenje, haptika
Improvements of haptic teleoperation by using a PLL ap-
tracker
In this paper, the problem of velocity information in bilateral teleoperation is addressed. An approach using a Phase Locked Loop (PLL) a^-tracker is presented. The approach offers some advantages over the method developed in our previous research. It allows obtaining sufficient position, velocity, and force information for teleoperation by using a haptic interface. However, the main advantages of the method are simplification and possible improvements of the FPGA implementation. Such bilateral teleoperation approach with a dedicated haptic interface can significantly improve surgical robotics. The approach was experimentally validated by a simple 1-DoF laboratory bilateral teleoperation system.
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 ap-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 ap- 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
f = MT-1u
(1) (2)
kjer je u=[uc,uá] in u„ = [ueq c,u^d] , ueq c = fc /Mc in lleq,d =	+ kpxti), xv = Tx, x = , xs ]
T =
1 1 1 -1
(3)
Slika 1: Blokovna shema robustnega algoritma za bilateralno vodenje
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
Slika 2: Blokovna shema aP-sledilnika
estimacije položaja in hitrosti je tudi v preprosti implementaciji. To je še posebej pomembno, kadar je količina strojnih virov omejena [10].
Algoritem afi-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 xk in napoved hitrosti vk opisujeta enačbi
Xk — Xk-1 + TsVk_!
V* = AV, .
(4)
(5)
kjer sta xk-i in vk-i ocenjen položaj in ocenjena hitrost
v časovnem odtipku k -1. Korekcija napovedi položaja in korekcija napovedi hitrosti je podana z
Xk ~ Xk + «O* Xk )
ñ =vk_l+ß/Ts(xk-xk).
(6) (7)
M = diag(mm,ms) 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. xc, xd, fc in fc so položaji in sile v virtualnem načinu. Indeksa (.)„ in (.)d označujeta skupni in diferencialni način. uc in ud označujeta izhod robustnega regulatorja. Regulacijski parametri Mc, D , kp, in kv 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 afi-sledilnik
Poglavje opisuje estimacijo položaja in hitrosti z uporabo analognih Hallovih senzorjev in aP-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
Blokovna shema aP-sledilnika je prikazana na sliki 2. Po preureditvi enačb (4)-(5) se lahko zapiše ocena položaja in hitrosti z
xk = xk-i + TA-i +a(Xk -xk-i -Tsvk-i) (8) vk = vk-i + (MTS)(xk - xk-i - TsVk-i) . (9)
Vrednosti a in p sta pozitivni konstanti, ki sta izbrani tako, da aP-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 a in p izbrana v območju
0 <a<i in 0 <p <a2 /(2-a). 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].
a z2 + (P-a) z
z2 + (a + p- 2) z + (1 -a)
x
P
Ts z2 + (a + P~2)z + (1 -a)
(11)
3.2 PLL afi-sledilnik
PLL afi-sledilnik je izpeljan iz splošne strukture ap-sledilnika. ap-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).
Glede na ap-sledilnika je pri PLL afi-sledilniku vhod zamenjan s sinusnim in kosinusnim signalom.
PD na sliki 4 implementira križni produkt med vhodnim parom sinusnih signalov [ua, ub ] in izhodnim parom sinusnih signalov |sin(x).cos(x)|. Sestavljen je iz dveh množilnikov in seštevalnika tako, da z
xe = ua cos(x) - ub sin(x) =
= sin(x) cos(x) - cos(x) sin(x) = sin(x - x)
(12)
izračuna signal xe, 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 xk, estimacijo hitrosti v,, in napoved položaja xt. PVE izhaja iz aP-sledilnika tako, da se enačbi aP-sledilnika (8) in (9) preoblikujeta v
Slika 3: Splošna blokovna shema fazno zaklenjene zanke (PLL)
PD je križni produkt med vhodnim parom sinusoidnih signalov [ua, ub ] in izhodnim parom sinusoidnih signalov |sin(x).cos(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.
v = v-i + (p/ Ts)xe.
(13)
(14)
Slika 4: Blokovna shema PLL ap-sledilnika
VCO izračuna sinusni in kosinusni signal glede na filtrirani vhodni podatek xk. Kadar [sin(x),cos(x)] sledi [sin(x),cos(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 afi-sledilnika je prikazana na sliki 4. Takšna blokovna shema vključuje vse komponente splošne PLL strukture (PD, LPF in VCO).
Načrtovanje vrednosti a in p za PLL ap-sledilnik poteka enako kot pri ap-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 afi-sledilnika in PLL afi-sledilnika
Prednost PLL afi-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 afi-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
z2 - z
implementacijo. Zato je mogoče doseči želeno implementacijo glede na uporabljen strojni vir (register, vpogledne tabele in DSP enote). Vendar pa PLL afi-sledilnik potrebuje za izračun PD dve množenji več kot afi-sledilnik, kar je razvidno v tabeli 1 pri porabi namenskih enot DSP48s. PLL afi-sledilnik potrebuje
tudi začetne vrednosti za x0. 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 aP-sledilnika in PLL afi-sledilnika Strojni viri FPGA ap-sledilnik PLL a0-vezja_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 afi-sledilnika in PLL afi-sledilnika. Zgoraj sta prikazani dejanska vrednost hitrosti in hitrost, izračunana z uporabo afi-sledilnika in PLL afi-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 afi-sledilnik, v času do 1s pa PLL afi-
sledilnik. Prikazano je, da računski šum bistveno poslabša delovanje sistema pri uporabi afi-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 aP-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 manipulatoija in regulatorja Parameter
Masa gospodarja mm	g	80
Masa sužnja ms	g	35
Lomna frekv. PLL afi-sledilnika	rad/s	350
Koeficient robustnosti D	1/s	350
Položajno ojačenje kp	1/s2	65000
Ojačenje hitrosti kv	1/s	200
Virtualna masa Mc	g	35
Perioda vodenja Tc	^s	10
— vm—vm<cf¡ -vm(PLL)^\ -	
/ v\	/
i i i i	1 1 1 1
- ^V nit-R-	/** v
\ r — f* — fh(FO)	fe— fe(FO)\ /
Cas [s]
Slika 5: Simulacijski rezultati primerjave bilateralnega teleoperiranja z uporabo afi-sledilnika in PLL afi-sledilnika
Slika 6: Načelna zgradba eksperimentalnega sistema za bilateralno teleoperiranje
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.
Slika 7: Eksperimentalni sistem za bilateralno teleoperiranje
Delovanje sistema je bilo validirano tudi s primerjavo realne trdote mehkega objekta in trdote, ki jo čuti
E10 £
'ST 5
N
o o
0
0.1
0.2
0.3
0.4
I 0.4 0.2
ro
N
o o
-0.2
0
gospodar—suzenj

1.2
1.4
„ 2 z ^ 0
0.1 0.2 0.3 Cas [s] a) Prosto gibanje
	—f --f h e
	
0.4

0_ 0.8
1 1.2 Cas [s]
b) Dotik trdega objekta
1.4
E
tm
0
N O O CL
2
iO
gospodar — suzenj
3 ]2 1 0
1.5
2.5
-f
2
Cas [s] c) Dotik mehkega objekta
2.5
1.5
CÖ
0.5,
0.5	1	1.5
Polozaj [mm]
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)
8
1
5 1
2
5
2
f
1
h
2
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 a^-sledilnik za estimacijo položaja in hitrosti. Takšna estimacija omogoča visoko zmogljivo bilateralno teleoperiranje. Prednost pred predhodno opisanim a^-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.
Literatura
[1]	P. F. Hokayem, and M. W. Spong", Bilateral teleoperation: An historical survey," Automatica, vol. 42, pp. 2035-2057, Dec. 2006.
[2]	T. B. Sheridan, "Telerobotics Automation & Human", MIT Press, 1992.
[3]	D. A. Lawrence, "Stability and transparency in bilateral teleoperation", IEEE Trans. on Robotics and Automation, vol. 9, no. 5, pp. 624-637, 1993.
[4]	A. Hace, and M. Franc, "Pseudo-Sensorless High Performance Bilateral Teleoperation by Sliding Mode Control and FPGA", in IEEE	Trans.	on	Mechatronics,	DOI: 10.1109/TMECH.2013.2241778, 2013.
[5]	H. V. Hoang, and J. W. Jeon, "An Efficient Approach to Correct the Signals and Generate High-Resolution Quadrature Pulses for Magnetic Encoders" IEEE Trans. Industrial Electronics, vol. 58, no. 8 pp. 3634-3646, aug. 2011.
[6]	T. Emura, and L. Wang, "A high-resolution interpolator for incremental encoders based on the quadrature PLL method", IEEE Trans. on Industrial Electronics, vol. 47, no. 1, pp. 84-90, feb. 2000.
[7]	M. Franc, and A. Hace, "A Study on the FPGA Implementation of the Bilateral Control Algorithm Towards Haptic Teleoperation", Automatika, vol. 54, no. 1, pp. 49-61, 2013.
[8]	A. Hace, and M. Franc, "FPGA Implementation of Sliding Mode Control Algorithm for Scaled Bilateral Teleoperation," IEEE Trans. Industrial Informatics, vol. PP., no. PP, pp. 1, 2013.
[9]	E. P. Cunningham, "Digital Filtering: An Introduction," Houghton Mifflin Company, Boston 1992.
[10]	E. Monmasson, L. Idkhajine, M. N. Cirstea, I. Bahri, A. Tisan, and M. W. Naouar, "FPGAs in Industrial Control Applications," IEEE Trans. Industrial Informatics, vol. 7, no. 2, pp. 224-243, May 2011.
[11]	M. Cadonic: Alfa-beta sledilnik, diplomsko delo, Maribor, 2000.
[12]	S. Katsura, Y. Matsumoto, and K. Ohnishi, "Modeling of Force Sensing and Validation of Disturbance Observer for Force Control," IEEE Trans. Industrial Electronics, vol. 54, no. 1, pp. 530-538, Feb. 2007.
[13]	F. D. Dinechin, M. Istoan, and, G. Sergent, "Fixed-Point Trigonometric Functions on FPGAs", ensl-00802777, version 1, http://hal-ens-lyon.archives-ouvertes.fr/ensl-00802777, oai:hal-ens-lyon.archives-ouvertes.fr:ensl-00802777, 2013.
[14]	Xilinx, "LogiCORE IP CORDIC v4.0", DS249, march 2011.
Aleš Hace je diplomiral leta 1994, magistriral leta 1998 in doktoriral leta 2001 na Fakulteti za elektrotehniko, računalništvo in informatiko Univerze v Mariboru.
Leta 1994 se je pridružil Inštitutu za robotiko na Fakulteti za elektrotehniko, računalništvo in informatiko na UM-FERI, kjer je izredni profesor. Leta 1999 je bil gostujoči raziskovalec (Visiting Research Fellow) pri prof. R. Parkinu na Loughborough University, Leicestershire, U.K. Zanimajo ga raziskovalna področja haptike, mehatronike, robotike, servosistemov in vodenje v drsnem režimu. Dr. Hace je član mednarodnega združenja IEEE in član Društva avtomatikov Slovenije.
Marko Franc je diplomiral leta 2009 in doktoriral leta 2013 na Fakulteti za elektrotehniko, računalništvo in informatiko Univerze v Mariboru. Njegova raziskovalna zanimanja vključujejo bilateralno vodenje, haptiko, servosisteme in vezja FPGA.