Brezsenzorska implementacija robustnega bilateralnega teleoperiranja z FPGA vezjem
1 2 Marko Franc , Aleš Hace
'Isomat d.o.o., Mežica, Slovenija 2 Fakulteta za elektrotehniko, računalništvo in informatiko, Univerza v Mariboru, Slovenija
E-pošta: marko.franc@uni-mb.si
Sensorless Implementation of Robust Bilateral Teleoperation by FPGA
The paper presents a sensorless implementation of a robust control algorithm for high-performance bilateral robot teleoperation. The algorithm is derived by a chattering-free Sliding Mode Control design procedure that ensures an easy to implement robustness to the system disturbances. However, the algorithm requires information of the position and velocity as well as the external forces acting on the robots. The required signals are acquired without using an external mechanical sensor. Thus, the implementation of the algorithm is practically sensorless; the position is measured by the internal Hall-effect sensors integrated within the motor housing. The other signals are obtained by estimation methods. The data acquisition algorithms as well as the robust control algorithm are implemented by FPGA in order to provide a high control rate that can further increase robustness and enhance haptic fidelity of the teleoperator system. The bilateral teleoperator system implementation was validated by a simple laboratory experimental system. It is shown that the proposed sensorless implementation by FPGA provides a high bilateral teleoperation performance.
1 Uvod
Robotsko teleoperiranje omogoča opravljanje nalog v oddaljenem okolju. Človek operater z uporabo lokalnega robota (gospodarja) generira referenčno gibanje, ki mu oddaljen robot (suženj) sledi. Pri vodenju ima lahko na voljo poleg vizualne in zvočne informacije iz oddaljenega okolja tudi informacijo o reakcijski sili, ki nastane pri dotiku sužnja z okolico. V tem primeru govorimo o bilateralnem teleoperiranju. Bilateralni telerobotski sistem omogoča realizacijo haptičnega občutka na daljavo.
Kvaliteta zagotavljanja oddaljenega haptičnega občutka močno vpliva na izvedbo naloge, ki jo operater opravlja na daljavo. To je še posebej pomembno pri takšnih aplikacijah kot je robotska kirurgija, ki se je v zadnjih letih že dodobra uveljavila na področju minimalno invazivnih operativnih posegov (Minimal Invasive Surgery - MIS) [1]. Takšen napredni robotski sistem zahteva celosten mehatronski pristop k
načrtovanju. Pri načrtovanju je potrebno upoštevati tudi kriterij velikosti sistema. Praviloma uporaba standardnih zunanjih mehanskih senzorjev zahteva dodatne mehanske komponente, ki lahko povečajo dimenzije sistema, lahko pa tudi poslabšajo njegove dinamične lastnosti.
Članek predstavlja izvedbo robustnega regulacijskega algoritma za bilateralno teleoperiranje brez uporabe zunanjih mehanskih senzorjev. Bilateralni regulacijski algoritem je zasnovan z modalno dekompozicijo [1], zakon vodenja pa je potem izpeljan po postopku vodenja v drsnem režimu (Sliding Mode Control - SMC). Meritev položaja je izvedena z uporabo integriranih analognih Hall-ovih senzorjev. Hitrost je ocenjena z uporabo afi sledilnika, zunanja reakcijska sila pa z uporabo opazovalnika motenj. Vsi algoritmi za vodenje sistema so implementirani na FPGA vezju, da zagotovimo kratke in natančne tipalne in regulacijske periodične intervale. Implementacija bilateralnega teleoperatorja je validirana s preprostim laboratorijskim eksperimentalnim sistemom.
2 Regulacijski algoritem
Dinamiko bilateralnega teleoperiranja opisuje poenostavljen model dveh robotskih mehanizmov. Lineariziran in razklopljen sistem opisujeta enačbi
mJm=fm+fk	C1)
mX=fS~fe	(2)
kjer so mm, ms, xm, xs,fminf masa, pospešek in sila vodenja gospodarja in sužnja. fh in fe sta akcijska sila operaterja in reakcijska sila okolice.
Načrtovanje bilateralnega vodenja sistema gospodar-suženj je zahtevna naloga. Poenostavitev takšnega načrtovanja omogoča modalna dekompozicija, ki transformira dinamiko teleoperatorja v virtualni modalni prostor. Takšna transformacija razklopi koordinati položaja in sile sklopljene arhitekture bilateralnega teleoperatorja v dve neodvisni virtualni modalni koordinati. Fizične koordinate transformiramo v virtualne modalne koordinate s transformacijsko matriko T
x, = Tx
ff = Tfe
T =
1 1 1 -1
Cilj vodenja za bilateralno teleoperiranje zapišemo z (5) in (6),
M x = f
C C J C
xd+kvxd+kpxd=0
ct =x -f /m
C	C J C	C
°'¡t = +kv*it +kpx,i
u. = u .+
i eq, i
D i
ti dt-x.)
eq,i	i s
kjer so u,,
x, in D izhod regulacijskega algoritma.
signal ekvivalentnega vodenja, hitrost in regulacijski
parameter robustnosti. Indeks (.)■ označuje skupni (.)c in diferencialni način (.)d. Vodenje f = [fm, fs]T v fizičnem prostoru potem izračunamo z enačbo (10),
kjer je x = [xm, ]T, f= [fh,-f]T, xv = [xc, xd]T in fr = [ fc, fd ]T . xm in sta položaja gospodarja in sužnja v fizičnem prostoru. xc, xd sta položaja virtualnem modalnem prostoru. fc in fd sta zunanji sili v virtualnem modalnem prostoru. Indeksa (.)c in (.)d označujeta skupni in diferencialni način, pri čemer je potrebno omeniti, da diferencialni način skrbi za položajno sledenje in skupni način za ujemanje sil. Transformacijska matrika T je opisana z (4).
f = MT-1u
(10)
(4) lahko
(5)
(6)
kjer so Mc, kv in kp masa, hitrostno in položajno ojačenje v virtualnem prostoru.
Z uporabo SMC postopka za načrtovanje regulacijskega algoritma zagotovimo robustnost na motnje v sistemu. V okviru tega postopka določimo t.i. drsno mnogoterost a = 0, kjer je gibanje sistema asimptotično stabilno in neodvisno od motenj. Naloga vodenja v drsnem režimu je, da omeji gibanje sistema na izbrano drsno mnogoterost. Tipično se vodenje u={ u', u+} določi glede na t.i. preklopno funkcijo a tako, da velja relacija u"<ueq<u+, pri čemer je ueq ekvivalentno vodenje, ki je potrebno za gibanje sistema na drsni mnogoterosti (a = 0 => i/eq).
V nadaljevanju je prikazan povzetek postopka izpeljave zakona vodenja za bilateralni teleoperator. Podrobno izpeljavo opisuje članek [2]. Za bilateralno vodenje v tem članku izberemo preklopne funkcije na podlagi želene modalne dinamike (5) in (6),
kjer je M=diag(mm,mx) nominalna masna matrika teleoperatorja in uv=[uc,ud]T je pospeškovno vodenje v virtualnem prostoru. T_i je inverz transformacijske matrike T in velja T =1T.
3 Algoritmi za zajem podatkov 3.1 Meritev položaja
V standardnih robotskih sistemih se za meritev položaja uporabljajo digitalni inkrementalni dajalniki in digitalni Hall-ovi senzorji. V zadnjem času so se pojavili analogni Hall-ovi senzorji, ki jih lahko integriramo v ohišje motorja in omogočajo meritev položaja z visoko resolucijo. Takšna izvedba ne potrebuje zunanjih senzorjev, kar je izjemno pomembno pri aplikacijah kot je MIS, saj ni potrebno vključevati dodatnih mehanskih komponent.
Hall-ovi senzorji reagirajo na prisotnost magnetnega polja. Če je motor zgrajen tako, da so na njegovi osi nameščeni trajni magneti, potem lahko merimo položaj motorja s Hall-ovimi senzorji. Izhodne signale analognih Hall-ovih senzorjev lahko zapišemo kot
u1 = sin(^)
u2 = sin(^ + 2n /3)
u3 = sin(^ - 2n /3)
(11)
kjer je ut napetost /'-tega senzorja in y = 2nxt / Tm. Tm je razmik magnetnih polovih parov.
Signali uh u2 in u3 se lahko uporabijo za izračun položaja tako, da najprej opravimo transformacijo v fazi ua in ub z uporabo Clarke-ove transformacije
(7)
(8)
Ua = "|(U1 - 2 U2 - 2 U3)
Ub = f(( U - f U3)
(12)
Iz ua in ub lahko nato izračunamo položaj xT, ki določa absolutni položaj znotraj magnetnega razmika Tm.
kjer sta ac in ad preklopni funkcija za skupni in diferencialni način v virtualnem modalnem prostoru.
V nadaljevanju SMC postopka je zakon vodenja izpeljan z uporabo zahteve ct = -Daj in nadaljnjo preureditvijo enačb tako, da pospešek ni potreben za izračun regulacijskega algoritma
X = atan 2(u„ , Ub)
(13)
(9)
u

Brezsenzorska implementacija robustnega bilateralnega teleoperiranja z FPGA vezjem	207
Za izračun položaja v celotnem območju gibanja osi motorja x je potrebno zaznati in šteti prehode med polovimi pari k. Položaj x se izračuna z
x = xT + kzm, k = 0, +1, +2,...	(14)
3.2 Estimaci ja hitrosti
V tem članku za estimacijo hitrosti uporabljamo afi sledilnik, ki je poenostavljena oblika Kalman-ovega opazovalnika. afi sledilnik omogoča estimacijo hitrosti v zelo širokem območju.
Algoritem afi sledilnika lahko zapišemo v dveh delih: z modelom, ki daje napoved, in korekcijo, s pomočjo katere dobimo izhodne vrednosti. Napoved položaja X* in napoved hitrosti v* opisujeta enačbi
x* = xk-i + Tsvk-i	(15)
v* = Vk-i (16)
Slika 2. Načrtovalna metodologija logičnega vezja za FPGA. Tabela 1. Poraba virov XC5LVX30
FPGA vir	Na voljo	Porabljeno	%
Rezine	4800	4731	98.6
Registri	19200	10769	56.1
LUT	19200	17218	89.4
Množilniki	32	27	84.4
Block RAM	32	1	3.1
kjer je Xt-1 in v^ ocenjen položaj in ocenjena hitrost v časovnem odtipku (k-1). Ts je perioda tipanja.
Korekcija napovedi položaja in korekcija napovedi hitrosti je podana z
xk = xi+a(xk- xk) (17) Vk = Vk-i +p/Ts(Xk -x*)	(18)
kjer so xk, a in fi izmerjen položaj, ojačenje položajne korekcije in ojačenje hitrostne korekcije. Blokovna shema afi sledilnika je prikazana na sliki 1.
Vrednosti a in fi sta pozitivni konstanti, ki sta izbrani tako, da afi sledilnik opisuje limitni Kalman-ov filter. Asimptotično stabilni odziv zagotovimo, če a in fi izberemo v območju 0<a<1 in 0<fi<a2/(2-a).
3.3 Opazovalnik zunanje sile
Opazovalnik zunanje sile temelji na opazovalniku motenj, ki poda oceno motnje z uporabo nizko pasovnega filtra. Algoritem za izračun ocene zunanje sile je opisan z
g
L, = —- (/ + gmx) - gmx	(19)
s + g
kjer so /„,. f. k. m in g ocena zunanje sile, sila
vodenja, izmerjena hitrost, nominalna masa in lomna frekvenca opazovalnika sile. Blokovna shema opazovalnika sile je prikazana na sliki 2.
4	Metodologija načrtovanja FPGA logičnega vezja
V članku smo uporabili načrtovalsko metodologijo (slika 3) [3], kjer je poudarek na preliminarni specifikaciji zahtev in načrtovanju logičnega za FPGA. Preliminarna specifikacija zahtev vključuje časovno in računsko analizo.
Časovna analiza definira začetne zahteve implementacije procesov. Definira čas izvedbe posameznega procesa in celoten čas izvedbe regulacijskega algoritma. Procesi se lahko implementirajo sekvenčno ali paralelno kar močno vpliva na porabo virov FPGA in na čas izvedbe regulacijskega algoritma.
Računska analiza določa implementacijo matematičnih operacij in zapis vrednosti števil med izračunom. V članku je uporabljen zapis vrednosti z notacijo s fiksno vejico. Optimalna izbira zapisa vrednosti lahko močno zmanjša porabo FPGA virov.
Poraba virov je prikazana v tabeli 1 pri čemer je potrebno omeniti, da je poraba virov močno povezana s časom izvajanja. Paralelna izvedba procesov lahko močno zmanjša čas izvedbe regulacijskega algoritma in hkrati močno poveča porabo FPGA virov.
5	Eksperimenti
5.1 Eksperimentalni sistem
Preprosti eksperimentalni sistem sestavljata dva linearna motorja Faulhaber Quickshaft LM1247-080-01 s pripadajočima servoregulatorjema. Prvi motor deluje kot gospodar, drugi kot suženj. Oba imata vgrajene tri analogne Hall-ove senzorje, s katerimi merimo položaj.
Tabela 2. Parametri sistema
Parameter	Vrednost
Magnetni odmik Tm	18 mm
Masa gospodarja mm	35 g
Masa sužnja ms	35 g
Lomna frekv. afi sledilnika m0	350 rad/s
Lomna frekv. opazovalnika sile g	350 rad/s
Koeficient robustnosti D	350 1/s
Položajno ojačenje kp	12000 1/s2
Hitrostno ojačenje kv	200 1/s
Frekvenca regulatorja Tc	50 kHz
45 I 40 35 Š 30 25.
2
iT
a 0 -2
3 3.2 3.4 3.6 3.8 4 4.2 4.4 4.6 4.8 5 Cas [s]
a)
41 41.2 41.4 41.6 41.8 42 42.2 42.4 42.6 42.8 43
Hitrost je estimirana z uporabo afi sledilnika. Zunanjo silo izračunamo z opazovalnikom. Regulacijski algoritem in ostali algoritmi za zajem podatkov so bili načrtovani v programu LabVIEW in implementirani na NI PXI-7841R z FPGA Virtex-5.
5.2 Rezultati
Izvedeni so bili naslednji eksperimenti: prosto gibanje, dotik mehkega objekta in dotik trdega objekta. Parametri manipulatoija in regulacijski parametri so prikazani v tabeli 2.
Na sliki 4 so prikazani rezultati eksperimentov za prosto gibanje (a), dotik mehkega objekta (b) in dotik trdega objekta (c). Pri vsakem eksperimentu smo posneli položaj in zunanjo silo. Položaj gospodarja xm in sila operaterja fh sta označena z rdečo črto, položaj sužnja xs in reakcijska sila okolice fe pa z zeleno črto.
Pri prostem gibanju (slika 4a) lahko vidimo odlično sledenje po položaju. V tem eksperimentu se suženj ne dotika okolice, vendar kljub temu lahko opazimo zunanjo silo (fh, f ^ 0N), ki jo izračunamo z opazovalnikom. Razlog so motnje sistema, kot so modelna odstopanja in šum pri meritvi analognih signalov. Pri dotiku mehkega objekta (slika 4b) je položajno sledenje in ujemanje sil odlično. Pri dotiku trdega objekta (slika 4c) je ujemanje zunanjih sil prav tako odlično, medtem ko je sledenje položajev slabše, kar kaže na nižjo maksimalno prenosno impedanco sistema, kot bi bila potrebna za realizacijo kvalitetnega haptičnega občutka v tem primeru.
6 Zaključek
V članku je opisana izvedba regulacijskega algoritma za bilateralno teleoperiranje. Izpeljan SMC algoritem je enostaven za implementacijo na programirljivem vezju FPGA in zagotavlja robustnost na motnje sistema. Uporaba programirljivega vezja FPGA omogoča, da smo dosegli kratke in natančne tipalne in regulacijske periodične intervale. Z uporabo FPGA smo tako dosegli bistveno višjo frekvenco izvajanja regulacijskega algoritma kot je to možno pri klasični procesorski izvedbi, s čimer se lahko doseže kvalitetnejši haptični občutek pri teleoperiranju.
41 41.2 41.4 41.6 41.8 42 42.2 42.4 42.6 42.8 43 Cas [s]
b)

: 10-	Trda ovira
9 1 71.2 71.4 71.6 71.8 72 72.2 72.4 72.6 72.8 73
2
iT « 0
-2
7 1 71.2 71.4 71.6 71.8 72 72.2 72.4 72.6 72.8 73 Cas [s]
c)
Slika 4. Rezultati eksperimentov: prosto gibanje (a), dotik mehkega objekta (b) in dotik trdega objekta (c).
Opisana izvedba eksperimentalnega sistema bilateralnega teleoperatorja ne potrebuje zunanjih mehanskih senzorjev. Med glavne prednosti te izvedbe štejemo tudi manjšo kompleksnost zgradbe in velikost sistema, kar je zelo pomembno pri aplikacijah kot je MIS. Uporaba zunanjih mehanskih senzorjev lahko vključuje dodatne mehanske dele, kar lahko poslabša dinamiko sistema in poveča ceno sistema. Čeprav je lahko resolucija analognih Hall-ovih senzorjev nižja od resolucije zunanjih senzorjev (lin. pomik: 0.1 ^m, rot. pomik: 324000 pulzov/obrat) smo dosegli teleoperiranje z relativno kvalitetnim haptičnim občutkom.
Operacijo delno financira Evropska unija, in sicer iz Evropskega socialnega sklada.
Literatura
[1]	K. Ohnishi, S. Katsura in T. Shimono, "Motion Control for Real-world Haptics," IEEE Industrial Electronics Magazine, vol. 4, no. 2, pp. 16-19, June 2010.
[2]	A. Hace, M. Franc in K. Jezernik, "Sliding Mode Control for Scaled Bilateral Teleoperation," in 37th Proc. IEEE Conf. IECON, pp. 3430-3435, 2011.
[3]	M. Franc in A. Hace, "FPGA implementation of the bilateral control algorithm for a high performance haptic teleoperation," IEEE int. Conf AMC, pp 1-6, 2012.
h	e