Robot công nghiệp và người máy

Tieåu luaän moân hoïc: Robot coâng nghieäp vaø ngöôøi maùy Hoïc vieân: Nguyeãn Minh Hoaø Lôùp: ÑKHKT K14 Trang 1 1. Giôùi thieäu veà Robot Scara Robot SCARA ra ñôøi vaøo naêm 1979, ñaây laø moät kieåu tay maùy coù caáu taïo ñaëc bieät ñöôïc söû duïng nhieàu trong caùc coâng vieäc laép raùp caùc taûi troïng nhoû theo phöông thaúng ñöùng. Robot SCARA bao goàm hai khôùp noái vaø hai caùnh tay. Treân hai khôùp noái seõ gaén hai ñoäng cô ñeå cung caáp moment 21 ,ττ ñieàu khieån hai caùnh tay l1,l2 nhö hình veõ. 1θ 2θ 1l 2l Hình 1: Hình chieáu baèng Robot Scara 2. Moâ hình ñoäng hoïc cuûa Robot SCARA Phöông trình ñoäng hoïc cuûa Robot SCARA nhö sau: )1(),()( .... τ=+ qqqCqqM Trong ñoù 21 ,qq laø goùc quay cuûa hai truïc ñoäng cô DC vaø 21 ,θθ laø goùc quay cuûa hai khôùp xoay cuûa robot. Trong ñoù: ⎥⎦ ⎤⎢⎣ ⎡ +++− +−+= 2 2322121 2121 2 11 ))(cos( ))(cos( )( mlIIqqklml qqklmlmlI qM e e ⎥⎥⎦ ⎤ ⎢⎢⎣ ⎡ + += 2 . 12121 . 221211 . ))(sin( ))(sin(),( Vqqqkklml qqqkklmlVqqC ee ee Quan heä giöõa [ ]21 qq vaø [ ]21 θθ nhö sau: )2( 0 2 1 2 1 ⎥⎦ ⎤⎢⎣ ⎡⎥⎦ ⎤⎢⎣ ⎡−=⎥⎦ ⎤⎢⎣ ⎡ q q kk k ee e θ θ Caùc thoâng soá ñöôïc cho nhö sau: 2 1 .8.1 mKgI = : Moment quaùn tính cuûa caùnh tay 1 2 2 .041.0 mKgI = : Moment quaùn tính cuûa caùnh tay 2 2 3 .134.0 mKgI = : Moment quaùn tính cuûa taûi troïng Kgm 10= : Khoái löôïng taûi Tieåu luaän moân hoïc: Robot coâng nghieäp vaø ngöôøi maùy Hoïc vieân: Nguyeãn Minh Hoaø Lôùp: ÑKHKT K14 Trang 2 ml 4.01 = : Chieàu daøi cuûa caùnh tay 1 ml 35.02 = : Chieàu daøi cuûa caùnh tay 2 5.0=ek : Heä soá truyeàn ñoäng cuûa khôùp noái 2 smKgV /.5 21 = : Heä soá ma saùt cuûa khôùp noái 1 smKgV /.3 22 = : Heä soá ma saùt cuûa khôùp noái 2 Tín hieäu vaøo: Moment 21 ,ττ cuûa 2 ñoäng cô DC(hoaëc ñieän aùp) Tín hieäu ra: Goùc 21 ,θθ cuûa 2 caùnh tay robot SCARA. 3. Heä phöông trình bieán traïng thaùi moâ taû robot Bieán ñoåi phöông trình (1) vaø (2) ta ñöôïc: [ ]2222122232211 2211 2 232 2 . 221 . 11 2 232 2 2 .. 21 2 232 2 1 .. 2 2 2 1 2 1 .. ))(( )( )()( AllmmlIImlI AlmlmlII qAVlmlqVmlIIqBklmlmlIIqABkllm q ee −+++ ⎥⎥⎦ ⎤ ⎢⎢⎣ ⎡ +++ +−++−++−− = ττ [ ]2222122232211 2 2 11121 2 . 2 2 111 . 121 2 2 .. 2 2 2 1 2 2 1 .. 21 2 11 2 .. ))(( )( )()( AllmmlIImlI mlIAlml qVmlIqAVlmlqABllmqBklmlmlI q e −+++ ⎥⎥⎦ ⎤ ⎢⎢⎣ ⎡ ++ ++−−−+− = ττ (3) Vôùi ))(sin())(cos( 2121 qqkBqqkA ee +=+= Ñaët caùc bieán traïng thaùi nhö sau: 2 . 43 . 23 1 . 21 . 11 )4( qxx qx qxx qx == = == = Töø (3) vaø (4) ta ñöôïc heä phöông trình bieán traïng thaùi moâ taû robot sau: [ ] [ ]2222122232211 2 2 11121 42 2 112121 2 4 2 2 2 1 22 221 2 11 4 . 43 . 22 2 2 1 22 232 2 11 2211 2 232 422121 2 232 2 421 2 232 2 2 2 2 2 1 2 2 . 21 . ))(( )( )()( )5( ))(( )( )()( AllmmlIImlI mlIAlml xVmlIAxVlmlABxllmBxklmlmlI x xx AllmmlIImlI AlmlmlII AxVlmlxVmlIIBxklmlmlIIABxkllm x xx e ee −+++ ⎥⎥⎦ ⎤ ⎢⎢⎣ ⎡ ++ ++−−−+− = = −+++ ⎥⎥⎦ ⎤ ⎢⎢⎣ ⎡ +++ +−++−++−− = = ττ ττ Tieåu luaän moân hoïc: Robot coâng nghieäp vaø ngöôøi maùy Hoïc vieân: Nguyeãn Minh Hoaø Lôùp: ÑKHKT K14 Trang 3 4. Quyõ ñaïo ñaàu muùt caùnh tay Robot SCARA Moái lieân heä giöõa caùc khôùp noái 1 vaø 2 coù toaï ñoä goùc [ ] Tqq 21 vôùi caùc caùnh tay coù toïa ñoä goùc [ ] T21 θθ , ñöôïc moâ taû bôûi phöông trình (2) ⎥⎦ ⎤⎢⎣ ⎡⎥⎦ ⎤⎢⎣ ⎡−=⎥⎦ ⎤⎢⎣ ⎡ 2 1 2 1 0 q q kk k ee e θ θ Toïa ñoä ñaàu muùt cuûa caùnh tay robot SCARA ñöôïc xaùc ñònh bôûi phöông trình sau: )6()sin(sin )cos(cos 21211 21211 θθθ θθθ ++= ++= lly llx Caùc phöông trình treân aùp duïng cho baøi toaùn ñieàu khieån theo khoâng gian khôùp noái. Cho pheùp xaùc ñònh caëp goùc [ ] T21 θθ töø toïa ñoä (x,y) cho tröôùc cuûa ñaàu muùt caùnh tay robot SCARA trong khoâng gian hoaït ñoäng. Trong quaù trình ñieàu khieån robot, öùng vôùi moät vò trí (x,y) trong khoâng gian hoaït ñoäng coù theå coù nhieàu caëp goùc trong khoâng gian khôùp noái. Do ñoù, ta phaûi choïn löïa moät caëp nghieäm phuø hôïp vôùi yeâu caàu ñieàu khieån. Coù nhieàu caùch choïn löïa tuyø theo ñoái töôïng vaø muïc ñích: - Toång quaõng ñöôøng dòch chuyeån cuûa caùc khôùp ngaén nhaát. - Giaûm bôùt aûnh höôûng töông taùc giöõa caùc khôùp. - Naêng löôïng tieâu thuï thaáp nhaát. - Giôùi haïn ñoäng hoïc cuûa heä thoáng. Robot SCARA vôùi hai khôùp noái öùng vôùi moãi toïa ñoä (x,y) cuûa ñaàu muùt caùnh tay chuùng ta seõ coù hai nghieäm cho caùc caëp goùc. Do ñoù, ta seõ choïn caëp goùc sao cho toång bieán thieân cuûa caëp goùc töø vò trí k-1 ñeán vò trí k laø nhoû nhaát. ( ) )7()1()()1()(min 2211 −−+−− kkkk θθθθ Giôùi haïn khoâng gian hoaït ñoäng cuûa heä thoáng thoâng qua caëp goùc [ ] Tqq 21 )8(2 1 πθπ πθπ ≤≤− ≤≤− 5. Ñieàu khieån robot SCARA duøng moment tính(computed torque) Trong phöông phaùp moment tính, ta phaûi xaây döïng hai voøng ñieàu khieån. Voøng hoài tieáp trong ñöôïc xaây döïng döïa treân ñoäng löïc hoïc cuûa heä thoáng ñeå buø taát caû caùc thaønh phaàn phi tuyeán cuûa heä thoáng. Voøng hoài tieáp ngoaøi xaây döïng döïa treân sai leäch giöõa tín hieäu ra vaø tín hieäu ñaët. Chöùc naêng cuûa voøng hoài tieáp trong laø tuyeán tính hoaù moái lieân heä giöõa tín hieäu ñaët vaø tín hieäu ra trong khi voøng hoài tieáp ngoaøi laøm oån ñònh hoaù heä thoáng. Tieåu luaän moân hoïc: Robot coâng nghieäp vaø ngöôøi maùy Hoïc vieân: Nguyeãn Minh Hoaø Lôùp: ÑKHKT K14 Trang 4 5.1 Caáu truùc cuûa boä ñieàu khieån moment tính Boä ñieàu khieån PD Chieán löôïc ñieàu khieån Hoài tieáp tuyeán tính hoaù SCARA ROBOT Heä thoáng tuyeán tính hoaù Voøng ñieàu khieån trong Voøng ñieàu khieån ngoaøi Hình 2: Sô ñoà heä thoáng ñieàu khieån duøng moment tính 5.1.1 Hoài tieáp tuyeán tính hoaù Moâ hình robot SCARA ñöôïc cho bôûi (1) nhö sau: τ=+ .... ),()( qqqCqqM Vôùi ⎥⎦ ⎤⎢⎣ ⎡ +++− +−+= 2 2322121 2121 2 11 ))(cos( ))(cos( )( mlIIqqklml qqklmlmlI qM e e ⎥⎥⎦ ⎤ ⎢⎢⎣ ⎡ + += 2 . 12121 . 221211 . ))(sin( ))(sin(),( Vqqqkklml qqqkklmlVqqC ee ee Ñeå tuyeán tính hoaù vaø phaân ly, ta choïn luaät ñieàu khieån τ goàm hai thaønh phaàn nhö sau: )9(NL τττ += Vôùi )11( )10( .. ... qM qCqM dL nN = += τ τ SCARA ROBOT ),( . qqN + + Nτ Lτ q Hình 3: Sô ñoà caáu truùc hoài tieáp tuyeán tính hoaù ⎥⎦ ⎤⎢⎣ ⎡ +− +−= 0))(cos( ))(cos(0 2121 2121 qqklml qqklml M e e n Tieåu luaän moân hoïc: Robot coâng nghieäp vaø ngöôøi maùy Hoïc vieân: Nguyeãn Minh Hoaø Lôùp: ÑKHKT K14 Trang 5 ⎥⎦ ⎤⎢⎣ ⎡ ++ += 2 231 2 11 0 0 mlII mlI M d ⎥⎥⎦ ⎤ ⎢⎢⎣ ⎡ + += 2 . 12121 . 221211 . ))(sin( ))(sin(),( Vqqqkklml qqqkklmlVqqC ee ee 5.1.2 Ñieàu khieån PD Ñaët .. qy = vôùi y laø tín hieäu vaøo môùi, ta coù luaät ñieàu khieån ñöôïc moâ taû nhö sau: )12(yM dL =τ Muïc ñích ñieàu khieån laø tín hieäu ra )(tq baùm theo quyõ ñaïo thieát keá )(tqd Ta coù: )13( ... dddpd qKqKqr ++= Choïn luaät ñieàu khieån )14( . rqKqKy dp +−−= Thay (13) vaøo (14) ta ñöôïc qqe KeKe d pd −= ++ )15(... Giaû thuyeát Kp vaø Kd laø caùc ma traän döông xaùc ñònh, oån ñònh tieäm caän Töø (10) vaø (11) ta ñöôïc ⎥⎥ ⎥ ⎦ ⎤ ⎢⎢ ⎢ ⎣ ⎡ ++− ++−=⎥⎦ ⎤⎢⎣ ⎡ 2 1 .. 212 . 21 .. 21 2 2 .. 211 . 12 .. 21 2 1 qBklmlqVqAlml qBklmlqVqAlml e e N N τ τ ))(sin())(cos( 2121 qqkBqqkA ee +=+= ⎥⎥⎦ ⎤ ⎢⎢⎣ ⎡ ++++ +++=⎥⎦ ⎤⎢⎣ ⎡ ))(( ))(( 2 . 22 .. 2 232 1 . 11 .. 2 11 2 1 eKeKqmlII eKeKqmlI pdd pdd L L τ τ ... qqe qqe d d −= −= Tieåu luaän moân hoïc: Robot coâng nghieäp vaø ngöôøi maùy Hoïc vieân: Nguyeãn Minh Hoaø Lôùp: ÑKHKT K14 Trang 6 PDChieán löôïcñieàu khieån SCARA ROBOT ),( . qqN + + Nτ Lτ q Hình 4: Sô ñoà khoái heä thoáng ñieàu khieån duøng moment tính 5.2 Thöïc hieän moâ phoûng heä thoáng ñieàu khieån robot SCARA duøng moment tính 5.2.1 Xaây döïng sô ñoà moâ phoûng Xaây döïng moâ hình robot SCARA theo phöông trình (1) treân Simulink nhö sau: Hình 5: Moâ hình robot SCARA Tieåu luaän moân hoïc: Robot coâng nghieäp vaø ngöôøi maùy Hoïc vieân: Nguyeãn Minh Hoaø Lôùp: ÑKHKT K14 Trang 7 Hình 6: Sô ñoà ñieàu khieån duøng moment tính Hình 7: Sô ñoà khoái phaàn hoài tieáp tuyeán tính hoaù Tieåu luaän moân hoïc: Robot coâng nghieäp vaø ngöôøi maùy Hoïc vieân: Nguyeãn Minh Hoaø Lôùp: ÑKHKT K14 Trang 8 Hình 8: Sô ñoà khoái boä ñieàu khieån PD 5.2.2 Keát quaû moâ phoûng 5.2.2.1 Ñaùp öùng böôùc: 0 20 40 60 0 0.2 0.4 0.6 0.8 1 Tin hieu qd1-q1 0 20 40 60 0 0.2 0.4 0.6 0.8 1 Tin hieu qd2-q2 Hình 9: Ñaùp öùng böôùc cuûa q1 vaø q2 Tieåu luaän moân hoïc: Robot coâng nghieäp vaø ngöôøi maùy Hoïc vieân: Nguyeãn Minh Hoaø Lôùp: ÑKHKT K14 Trang 9 Hình 10: Moment cuûa 2 ñoäng cô khi tín hieäu vaøo laø haøm böôùc Nhaän xeùt: Ñaùp öùng böôùc khoâng coù voït loá, loaïi boû ñöôïc söï aûnh höôûng laån nhau giöõa hai khôùp noái 5.2.2.2 Baùm theo quyõ ñaïo hình sin: 0 10 20 -1 -0.5 0 0.5 1 qd1-q1 0 5 10 -1 -0.5 0 0.5 1 qd2-q2 q1 qd1 q2 qd2 Hình 11: Ñaùp öùng ngoõ ra q1 vaø q2 khi tín hieäu vaøo laø hình sin Tieåu luaän moân hoïc: Robot coâng nghieäp vaø ngöôøi maùy Hoïc vieân: Nguyeãn Minh Hoaø Lôùp: ÑKHKT K14 Trang 10 Hình 12: Moment cuûa 2 ñoäng cô DC 1 vaø DC2 khi tín hieäu vaøo laø sin Nhaän xeùt: Ngoû ra cuûa heä thoáng baùm theo tín hieäu vaøo hình sin raát toát,gaàn nhö khoâng coù sai soá,heä phaân ly toát caùc khôùp khoâng bò aûnh höôûng laån nhau.