Một phân tích động lực học mới để điều khiển robot cá dạng carangiform bám theo quỹ đạo cho trước

Tgp Chi Khoa Hgc Gido Dgc Ky Thugt (33/2015) Tru&ng Dgi Hgc Su Phgm Ky Thugt TR Hd ChiMinh MQT PHAN TICH DQNG LlTC HQC MOI DE DIEU KHIEN ROBOT CA DANG CARANGIFORM BAM THEO QUY DAO CHO TRlTdC THE NEW DYNAMIC ANALYSIS APPROACH AND TRAJECTORY FOLLOWING CONTROL OF A CARANGIFORM FISH ROBOT Hoang Khac Anh, Pham Van Anh, Nguyin Tan Tien, VS Tudng Quan Tru&ng Dgi hgc Bdch Khoa TP.HCM Ng^ytda so^n nh^n bai 20/4/2015, phan bi$n d^nh gi^ 22/4/2015, chip nh$n dang 27/4/2015 TOM TAT Bdi bdo giai thieu mgt phdn tich dgng luc hgc mai de diiu khien robot cd kh&p dgng Carangiform bdm theo 16 trinh xdc dinh vai di xudt su- dung kit hgp phuang trinh Newton-Euler vd Euler-Lagrange Robot cd dugc thiit ki bao gom phdn: phdn ddu vdphdn duoi Viec dieu khiin phan duoi cua robot cd - dugc phdn tich bdng ly thuyet dgng l\rc hoc tay mdy robot - se giup rdbdt cd di chuyin theo lo trinh xdc dinh, chdng hgn nhu co the dieu khien robot cd di chuyin mat phdng ndm ngang Trong bdi bdo ndy, cdc nghien cuu tap trung vdo viic dieu khiin robot cd bai theo quy dgo cho truac mat phdng ngang vd di chuyen den sdu mong muon mat phdng Oyz chua dugc xem xet den Mgt bg dieu khien Fuzzy vd Adaptive BackStepping da dugc phdt trien de dieu khiin phdn duoi giup robot cd bdm theo quy dgo cho truac Tie khoa: robot cd, dgng luc hgc tay mdy, bg dieu khiin backstepping, bg diiu khien Fuzzy ABSTRACT This paper proposes a new dynamic analysis to control a 3-joint Carangiform fish robot swim to along a desired trajectory by using Newton-Euler and Euler-Lagrange concepts The fish robot is designed into two parts: the head and the tail of the fish robot The tail of the fish robot will be analyzed quite similarly to the analysis of the dynamic model of the manipulator By controlling the tail with respect to the desired profiles, one can control the fish robot on the horizontal plane To simplify, it is assumed that the motions of the fish robot include two main actions: the movement offish robot on the horizontal plane and movement offish robot to the desired depth The movement of the fish robot to the desired depth is ignored In this paper, the authors only focus on the control on the fish robot to swim following the desired trajectory on the horizontal plane Fuzzy and Adaptive Back Stepping controllers are developed to control the tail for this purpose Key words: fish robot, dynamic model, backstepping controller, fuzzy controller GIOI THI|;U Ngay nay, hudng nghien cftu v6 robot cd dang phat trien rdt manh va da dat dugc nhiiu tuu ddng ke J Edward Colgate vd Kevin M Lynch da cdng bd mdt bdi bdo rdt cd y nghia trinh bay tdng quan lgi tdt ca v i thiit k i CO cung nhu viec dieu khiin boi cho robot ca [1] Mat khdc v i phdn logi robot ed, ta co thi chia chftng lam hai dang chinh dya vao hinh dang boi cfta chiing: BCF (Body and/or Caudal Fin) vd MPF (Median and/or Paired Fin) [2] BCF dya tten nhftng thay ddi hinh dang cua than, dudi va vdy dudi de tgo lyc Tgp Chi Khoa Hgc Gido Due Ky Thugt (33/2015) Tru&ng Dgi Hgc SuPhgm Ky Thugt TR Hd ChiMinh ddy Dang MPF thi dya vdo sy dao ddng cfta vay hdng hoac vay kit hgp Trong thyc t i ta cd t h i n h | n dinh rdng, ca dang BCF phd biin hon dgng MPF va cd bdn dang boi chft yiu: Anguilliform, Subcarangiform, Carangiform va Thunniform Trong dd, dang Carangiform la dgng di chuyin vdi bien dg dao ddng cua than va dudi la tuong ddi nhd Vi vdy, ttong cdc dgng cua BCF, hdu hit robot ca deu phat trien dya theo dang Carangiform bdi tinh linh hoat va dl dieu khien cfta nd Thdng thudng, robot cd dang Carangiform dugc thiet k i vdi dgng khap hogc khdp Motomu Nakashima vd cac cdng sy da md td phuong trinh sd hpc va kinh nghiem thiet ke robot ca khdp Undultioiy ^ OtcfllBtoiy On motions va sft dung "sefl-excitation" [3] va nhdn thdy rdng robot cd.thS bed vdi van tdc Idn nhdt la 0.42m/s Trong mpt nghien cftu khac, QinYan cung cac cdng sy da phat trien robot cd vdi khdp dudi va xac dinh dugc cac thdng sd anh hudng ddn robot ca nhu tdn sd, bien dO, chieu ddi sdng, dg lech pha va cdc he sd din van tdc di chuyen cfta robot ca [4] Viec thiit kd vd dieu khien robot boi nhu cd thdt bdng cdch sft dyng cac can true Hen tuc thay vi rdi rgc la nhiing ddng gdp cd y nghia [5] Feitian Zhang va cac cgng sy da phdt trien robot ca bang viec ket hgp the mgnh cfta tau lugn dudi nude va robot cd tao mgt robot cd kha nang di chuyen linh ddng [6] Ls^ \^=^\ L^^l pectoral, dorsal, and anal I Ottraet^brm I Hinh 1: Hai dgng di chuyin cua cd: (a) Dgng BCF; (b) Dgng MPF [1] Md hinh hda vd md phdng la hai viec quan tipng ttong diiu khiin robot, chftng se giup ta cd dugc nhung kinh nghiem ttong viec thay ddi cdc thdng sd he thdng \ruac tiiiit k i robot cd tiiat Trong bai bao nay, cdc tdc gia se trinh bay phuong phap md hinh ddng lyc hoc mdi cho robot vdi y tudng thiit k i la su kit hop dpng lyc hgc cua AUVs (Autonomous Underwater Vehicle - thiit bi ty ddng di chuyin dudi nude) va ddng lyc hgc cdnh tay robot Sau phdn thiet ke ben ngoai, cdc bO dieu khiin se giftp robot ca di chuydn theo qu$^ dao mong mudn se dugc thdo luan Nhu ta da bidt robot ca la he phi tuyin, dd chung ta cd th6 sft dung bd dieu khien Ihdng minh hodc bd dieu khiin phi tuyin d i diiu khien robot cd theo quy dgo mong mudn Trong bai bdo cdc tac gia sft dyng bg diiu khiin thdng mmh Fuzzy va bp diiu khiin phi tiiyin Adaptive Back Stepping d i diiu khiin chuyen dgng dudi tft dd diiu khiin robot ca di chuyen ttong mgt phdng ngang Cudi ciing, mdt vai ket qua md phdng se dugc gidi thi^u de chftng minh tinh hi^u qua va tinh khd thi ciia phuong phap dd de xuat Tgp Chi Khoa Hgc Gido Dgc Ky Thudt (33/2015) Tru&ng Dgi Hgc SuPhgm Ky Thugt TR Hd ChiMinh MO HINH HOA ROBOT Thiit k i cfta robot ca dugc d i xudt nhu ttong hinh 2: robot cd cd khdp dudi va phdn ddu, dudi cfta robot dugc tao bdi khdp chu ddng va khdp hi ddng Robot cd di chuyen ttong mat phdng ngang bdng each sft dung dao ddng cua duoi de tgo lyc ddy giftp robot cd di chuyen den phia trudc Cac thdng sd dieu khien cho robot ca dugc trinh bay ttong bang I Bdng 1: Cdc thong so dieu khien cho robot cd Van toe dai & van toe xoay u Vi tri & Goc Euler (H€ tga dg trdi ddt) Chuyen dgng tinh tiin & chuyin dong xoay surge sway heave roll V y z pitch yaw q r Bac ty Hinh 2: Cdu true cua robot c Carangiform Y tudng cfta bd dgng lyc hpc dugc phdt triin dya tten phuong trinh Newton-Euler va dgng lyc hpc tay mdy Phuong trinh chuyin ddng cua phan ddu robot ca la phuang trinh phi tuyen va dugc md ta ttong phuang trinh (1) [8]: M ' r + [C(v) + Div^lv + g{r}^ = F (1) Trong dd: M': ma ttan khdi lugng C(v): ma tran lyc Coriohs va lyc hudng tam D(v): ma ttdn gidm chdn g(Ti): Ma ttdn kit qua cfta ttpng luc va luc Archimedes w p X ^ V Hinh 3: Hi tga gdn trin robot vd hi toa CO dinh gdn tren trdi ddt F: Ma ttan lyc vd moment tac ddng xet ttong he tpa dia phuang v: ma ttan van tdc tinh tiin va quay ttong he tga dp dia phuang v=[u V w p q r ] ' Phuang ttinh (1) md ta phuong trinh ddng lyc hpc cho ddu robot ca ttong khdng gian bac ty vdi he tpa dia phuang Chftng ta cd mdi quan he gifta van tdc tinh tien ttong h? tga dp dia phuang vd toan cue: = ;i(n2)f'' liv (2) Tgp Chi Khoa Hgc Gido Due Ky Thugt (33/2015) Tru&ng Dgi Hgc SuPhgm Ky Thugt TR Hd ChiMinh Trong do: COS^KOSB A ("2) = sirupcosB —sin9 —sinipcos(f> + cosif)sin9sin(p sinipsincf} + cosx})sin9cos(j) cosipcos(() + sinipsin9sintp —cosijJsiTKp + sin^sinBcoscp cos9sin(}} cos9cos(() nhien didu dd chung ta cd thi bd qua vi robot chi di chuydntiongmat phang ndm ngang (3) Luc day dugc tao bdi dudi robot ca Gia tri W cfta lyc phu thupc vdo tdn sd va bien dp cfta Vdi: dudi F^ la lyc qudn tinh chdt long va Fj la lyc coscptanO sin(f)tan0 nang cua vdy dudi Fp la lyc day tgi vdy dudi, —siiKp cosBtt| ^ n ^^y Hinh 20: Ddp img goc q^ cua duoi Hinh 17: Ddp ung vi tri cua robot L V > |L \ > > / J' /} ' L V\ " >k V\ I •" i VV k V \ / f ^\J Tr J / f y rf r iJ TF Hinh 21: Ddp ung goc q^ cua duoi Trong hinh 18, robot cd co thdi gian HXnh 18: Ddp img gde yaw ciia robot hoan quy dao mong mudn la 700 gidy Sai sd xudt hien theo phuong Ox I — \ ° An«ll| A > / V \ Jf k i V \ / f' \J f Tr khodng 0.4m - Bg diiu khiin Adaptive BackStepping Dap ftng cua robot sft dung bd dieu khiin Adaptive BackStepping de dieu khiSn robot cd boi theo quy dao mong mudn dugc trinh bay hinh 26 Hinh 22-25 thi hien cdc dap ftng goc cua cac khdp dudi Tdt ca cac gid tri cfta gde da Hinh 19: Ddp ung goc q, cua duoi dugc quy ddi sang he tga toan cue Tgp Chi Khoa Hgc Gido Due Ky Thudt (33/2015) ^Tru&ng Dgi Hgc SuPhgm Ky Thugt TR Hd ChiMinh I Hinh 22: Ddp ung goc yaw cua robot / /" / / Oulpul Syslem \ Vl I ' ,/\ tt^ ,^ ij Hinh 26: Ddp img vi tri ciia robot he tga todn cue Trong hinh 26, ta thay robot cdn khodng 600 gidy de hoan quj' dao mong mudn Khoang each lech theo phuong Ox la khodng 0.3m KET LUAN Trong bai bao nay, phuang ttinh ddng hpc cua robot ca 3-khdp dang Carangiform dugc gidi thieu Dya vao phuang ttinh ddng lyc hpc I I Hinh 23: Ddp ung goc q^ cita duoi robot ' Hinh 24: Ddp ung goc q^ cua duoi T « Hinh 25 Ddp ung goc q^ ciia duoi ta thiit k i bd dieu khien Id Fuzzy va Adaptive BackStepping de dieu khien robot ttong mat phdng ndm ngang Ket qua md phdng nhdn thdy bd dieu khien Adaptive BackStepping cho ddp ftng tdt ban bd dieu khien Fuzzy ve thdi gian ddp ung ciing nhu sai sd vi tri cudi hanh tiinh Tuy nhien ban thdi gian dau hogt dgng bd dieu khien Fuzzy cho ket qua ddp ftng ve gde dn dinh hon Ta thay sai sd cfta goc yaw sai lech khodng dp Sai sd hoan toan cd the chdp nhdn dugc vi thyc td ca that di chuyen thi van tgo goc lech nhd Qua qua trinh md phdng ta cung danh gia dugc mftc dp chinh xac cfta phuang trinh ddng lyc hgc mdi ma cdc tdc gid da xay dyng Md hmh ddng luc hpc dugc xdy dyng theo phuang phap dd ttinh bdy d cac phdn tten la cd the chap nhgn dugc Cd thi sft dung de tien hanh md phdng danh gia cac thdng sd cfta robot Trong nhftng phan tiep theo, cac tdc gid se tiin hanh thyc nghiem de cd th6 danh gia tdt hon v i mdt thuc t i phuong phdp xdy dyng md Tpp Chi Khoa Hpc Gido Bpc Ky Thudt (33/2015) Truimg Bpi Hpc SuPham Ky Thupt TP Hd ChiMinh hinh njy va danh gia dugc cac bp didu khidn da LCJI CAM ON dupe xay dung, bing each so sanh kdt qua ly Nghien ciiu dugc tai trg bdi Dai hgc thuyet md phdng dugc va kdt qua thuc nghiem Quae gia Thanh phd Hd Chi Minh (VNUSau cac tac gia cd thd nghien cuu giai quydt -HCM) khudn khd dd tai ma sd B-2013bai toan dieu khien dp sau cho robot 20-01 TAI LIEU THAM KHAO [1] J Edward Colgate, Member, IEEE, and Kevin M Lynch, Member, IEEE, Mechanics and Control of Swimming: A Review, IEEE loumal Of Oceanic Engineering, VoL 29, No 3, July 2004 [2] Michael Sfakiotakis, David M Lane and J Bruce C Davies, Review of Fish Swimming Modes for Aquatic Locomotion (Translation Journals style), IEEE Joumal Of Oceanic Engmeering, Vol 24, No 2, April 1999 [3] Motomu Nakashima, Norifiimi Ohgishi and Kyosuke Ono, A study on the Propulsive Mechanism of a Double Joint Fish Robot Utilizing Self-Excitation Control, JSME International Joumal, Series C, Vol 46, No 3, 2003 Qin Yan, Zhen Han, Shi-wu Zhang, Jie Yang, Parametric Research of Experiments on a Carangiform Robotic Fish, Journal of Bionic Engineering (2008) Hadi EL Daou, Taavi Salumae, Asko Ristolainen, Gert Toming, Madis Listak, and Maarja Kruusmaa, A Bio-mimetic Design and Control of a Fish-like Robot using Compliant Structures, The 15th Intemational Conference on Advanced Robotics Tallinn University of Technology Tallmn, Estonia, June 20-23, 2011 Feitian Zhang, John Thon, Cody Thon and Xiaobo Tan, Miniature Underwater Glider: Design, Modeling, and Experimental Results, 2012 IEEE Intemational Conference on Robotics and Automation RiverCentre, Samt Paul, Miimesota, USA May 14-18, 2012 Tuong Quan Vo, Hyoung Seok Kim, Byung Ryong Lee, Propulsive Velocity Optimization of3-Joint Fish Robot Using Genetic-Hill Climbing Algorithm, Joumal of Bionic Engineering, Vol 6, pp 415-429, 2009 Ola-Erik Fjellstad, Control of Unmanned Underwater Vehicles in Six Degrees ofFreedom A Quaternion Feedback Approach, Dr.lng Thesis, Department of Engineering Cybernetics, The Norwegian Institute of Technology, University of Trondheim, November 1994 [4] [5] [6] [7] [8] ... ChiMinh MO HINH HOA ROBOT Thiit k i cfta robot ca dugc d i xudt nhu ttong hinh 2: robot cd cd khdp dudi va phdn ddu, dudi cfta robot dugc tao bdi khdp chu ddng va khdp hi ddng Robot cd di chuyen... cua duoi de tgo lyc ddy giftp robot cd di chuyen den phia trudc Cac thdng sd dieu khien cho robot ca dugc trinh bay ttong bang I Bdng 1: Cdc thong so dieu khien cho robot cd Van toe dai & van toe... duoi robot Hinh 16: Ddp ung gde q^ cua duoi robot Tgp Chi Khoa Hgc Gido Dgc Ky Thugt (33/2015) Tru&ng Dgi Hgc Su Phgm Ky Thugt TR Hd ChiMinh B Diiu khiin robot theo quy dgo mong muon Trong phan robot