the robot*s dynamic properties would give a significant ... The full dynamic model of a robot is given by: @ Dâw . K+w> bw ..... stopping once given an initial push.
G|qdplfv Lghqwl fdwlrq dqg Frqwuro ri dq Lqgxvwuldo UrerwW Urguljr Mdplvrod_ / Pdufhor Dqj/ Mu1_ / Wdr Plqj Olp_ / Rxvvdpd Nkdwleh / Vhu \rqj Olp-_
Qdwlrqdo Xqlyhuvlw| ri Vlqjdsruh h Vwdqirug Xqlyhuvlw| --Jlqwlf Lqvwlwxwh ri Pdqxidfwxulqj Whfkqrorj|
Devwudfw
lqfoxgh irufh dqg frpsoldqw prwlrq frqwuro zkhq wkh urerw lv lqwhudfwlqj zlwk wkh hqylurqphqw/ preloh pd0 qlsxodwlrqv/ dqg zkhq wkh urerw lv uhtxluhg wr pryh dw kljk yhorflwlhv1 Lq wklv sdshu/ zh suhvhqw d phwkrg wr h{shul0 phqwdoo| lghqwli| wkh oxpshg lqhuwld sdudphwhuv ri dq lqgxvwuldo urerw dqg hvwlpdwh ri wkh urerw mrlqw*v ylv0 frxv iulfwlrq1 Zh surylgh ghwdlov ri wkh phwkrg/ glvfxvv lpsohphqwdwlrq lvvxhv dqg vkrz h{shulphqwdo uhvxowv rq d :0GRI Plwvxelvkl SD043 urerw1 Ixoo g|qdplf frqwuro iru wkh SXPD 893 dw rxu frqwuro oderudwru| kdv ehhq grqh gxh wr wkh dydlodelolw| ri wkh SXPD g|qdplf sdudphwhuv dv vwdwhg e| Nkdwle lq ^4`/ zlwk jrrg uhvxowv1 Qr rwkhu urerw kdv ehhq h{whqvlyho| vwxglhg g|qdplfdoo| wkdq wkh SXPD1 Exw zkhq lw frphv wr wkh SD043 Plwvxelvkl Urerw/ wkh qrq0 dydlodelolw| ri wkh lqhuwldo sdudphwhuv zdv d fkdoohqjh1 Wkh vshfl fdwlrq ri wkh Plwvxelvkl urerw rqo| surylghv wkh pdvv dqg fhqwhu ri judylw|1 E| lpsohphqwlqj Nkdwl0 e*v phwkrg ri h{shulphqwdoo| lghqwli|lqj wkh oxpshg lq0 huwld sdudphwhuv ri hdfk olqn dqg wkh ylvfrxv iulfwlrq ri hdfk mrlqw/ vljql fdqw lpsuryhphqw rq wkh SD043 urerw shuirupdqfh lv revhuyhg1
Lw lv nqrzq wkdw wkh shuirupdqfh ri d urerw fdq eh lpsuryhg zlwk wkh lqfrusrudwlrq ri urerw g|qdplfv lqwr lwv frqwuroohu1 Krzhyhu/ ghulydwlrq dqg lpsohphqwdwlrq ri d frpsohwh urerw g|qdplf prgho lv qrw zlgho| xvhg lq sudfwlfh pdlqo| ehfdxvh ri wkh gl!fxow| lq prghoolqj g|qdplf ehkdylru dqg wkh odfn ri lqirupdwlrq derxw wkh sdudphwhuv wkdw ghvfuleh vxfk ehkdylru/ hvshfldoo| olqn lqhuwldv1 Ixuwkhupruh/ prvw lqgxvwuldo urerwv gr qrw do0 orz prgl fdwlrq ri wkhlu frqwuro dojrulwkpv/ zlwk rqo| d ihz doorzlqj wkh Sursruwlrqdo dqg Ghulydwlyh +SG, jdlqv ri wkhlu frqwuroohuv wr eh dowhuhg/ wkdw ixuwkhu kdpshuv urerw ixoo g|qdplfv frqwuro lpsohphqwdwlrq1 Lq wklv sd0 shu/ zh suhvhqw d phwkrg wr lghqwli| wkh oxpshg lqhuwldo sdudphwhuv wrjhwkhu zlwk dq hvwlpdwh ri ylvfrxv iulfwlrq/ dqg wkh ghulydwlrq dqg lpsohphqwdwlrq ri d ixoo g|qdplf prgho ri dq lqgxvwuldo urerw1 Zh surylgh ghwdlov ri wkh phwkrg/ glvfxvv lpsohphqwdwlrq lvvxhv dqg vkrz h{shul0 phqwdo uhvxowv rq d :0GRI Plwvxelvkl SD043 urerw1
L1
Lqwurgxfwlrq
Lpsohphqwlqj ixoo g|qdplf frqwuro rq d urerw vwloo uhpdlqv d fkdoohqjh wr urerw vflhqwlvwv dqg uhvhdufkhuv wrgd|1 Wkh frpsoh{lw| dqg pruh lpsruwdqwo|/ wkh odfn ri nqrzohgjh derxw wkh g|qdplf sdudphwhuv ri wkh ur0 erw/ ohdg urerwv wr eh frqwuroohg prvwo| e| SG + sur0 sruwlrqdo/ ghulydwlyh, ru SLG +sursruwlrqdo/ lqwhjudo/ ghulydwlyh, frqwuro/ zkhuh wkh frqwuro lv grqh lqghshq0 ghqwo| iru hdfk mrlqw1 Dowkrxjk lw lv nqrzq wkdw lqfrusr0 udwlqj wkh sk|vlfdo lqirupdwlrq ri wkh urerw wr lwv frqwuro dojrulwkp lpsuryhv wkh shuirupdqfh ri wkh urerw/ vr idu/ wkh prvw frqyhqlhqw frpsrqhqw ri wkh sk|vlfdo prgho ri wkh urerw wkdw fdq eh hdvlo| ghulyhg dqg lqfoxghg lq lwv frqwuro duh wkh judylwdwlrqdo sdudphwhuv1 Wkh urerw frq0 wuroohu uholhv prvwo| rq lwv odujh jdlqv wr frpshqvdwh iru wkh frxsolqj ehwzhhq urerw olqnv dqg dfklhyh wkh urerw*v ghvluhg uhvsrqvh1 Doprvw doo lqgxvwuldo urerwv wrgd| duh srvlwlrq frqwuroohg dqg khqfh SG ru SLG phwkrg kdv ehhq h{whqvlyho| xvhg1 Wkh qhhg wr ixoo| lpsohphqw g|0 qdplf frqwuro rq d urerw dulvhv zkhq wkh lqfoxvlrq ri wkh urerw*v g|qdplf surshuwlhv zrxog jlyh d vljql fdqw lpsuryhphqw lq lwv shuirupdqfh fdsdelolwlhv1 H{dpsohv
LL1
Wkh Grplqdqw Lqhuwld Lghqwlilfdwlrq
Wkh ixoo g|qdplf prgho ri d urerw lv jlyhq e|=
. K+w> w, b . J+w, @ Dw
+4,
zkhuh lv wkh yhfwru ri mrlqw wrutxh/ w lv wkh yhfwru ri mrlqw dqjohv/ D lv wkh lqhuwld pdwul{/ K lv wkh Frulrolv dqg fhqwulixjdo yhfwru/ dqg J lv wkh judylw| yhfwru1 w/ wb / dqg w duh wkh mrlqw srvlwlrq/ yhorflw| dqg dffhohudwlrq yhfwruv/ uhvshfwlyho|1 Qh{w/ zh gh qh wkh grplqdqw lqhuwld ri wkh urerw olqn dv wkh wrwdo lqhuwld phdvxuhg dw wkh mrlqw lq txhvwlrq1 Iru wklv sxusrvh/ doo wkh rwkhu mrlqwv duh orfnhg/ wuhdw0 lqj wkh oxpshg olqnv dv d vlqjoh olqn dqg frqvhtxhqwo|/ zlwkrxw frxsolqj h hfwv1 Wkh g|qdplf prgho ri wkh oxpshg olqn lv
L . nyq b . j+, @ x>
+5,
zkhuh L lv wkh olqn grplqdqw lqhuwld/ nyq lv wkh ylvfrxv iulfwlrq frh!flhqw +gdpslqj,/ j+, lv wkh judylw| whup dqg x lv wkh dfwxdwlrq lqsxw1 Wkh SG frqwuro rq wkh mrlqw lq txhvwlrq lv jlyhq lq ^5` dv
W Wklv zrun lv vsrqvruhg e| Jlqwlf Lqvwlwxwh ri Pdqxidfwxu0
lqj Whfkqrorj|1 Sxeolvkhg rq wkh Surfhhglqjv ri wkh Qlqwk Lqwhuqdwlrqdo Frqihuhqfh rq Dgydqfh Urerwlfv +* L
+:,
ns @ $q5 = L
+;,
dqg
zkhuh wkhuh lv yluwxdoo| qr pruh uhvlvwdqfh +gdps0 lqj, rq wkh mrlqw dv wkh olqn lv shuwxuehg1 Wklv lv wkh srlqw zkhuh ny fdqfhov wkh h hfw ri wkh nlqhwlf iulfwlrq frh!flhqw/ nyq / wkdw lv/ ny @ nyq = Vlqfh ns @ 3/ wkh urerw ehkdyhv olnh d xqlw pdvv v|vwhp zlwkrxw gdpslqj1 Khuh wkh urerw olqn pryhv zlwk d sxvk ri wkh kdqg dqg frqwlqxhv wr pryh zlwkrxw vwrsslqj1 Wkh irufh h{huwhg e| wkh kdqg wr pryh wkh urerw olqn uhsuhvhqwv wkh vwdwlf iulfwlrq ri wkh mrlqw1
Hdfk ri wkh urerw olqn*v grplqdqw lqhuwld fdq eh phdvxuhg gluhfwo| iurp wkh urerw uhvsrqvh1 Wkh hdvlhvw zd| wr gr wklv lv e| fdqfhoolqj wkh h hfw ri wkh ylvfrxv iulfwlrq ri wkh urerw mrlqw/ uhvxowlqj lq wkh urerw dfklhy0 lqj xqgdpshg rvfloodwlrq/ dqg phdvxulqj wkh grplqdqw olqn lqhuwld iurp wkhuh1 Wr dfklhyh xqgdpshg rvfloodwlrq zh qhhg wr vhw htxdwlrq +9, zlwk @ 3= Wkxv wkh htxdwlrq ehfrphv/
+v5 . $q5 ,+v, @ 3>
91 Wkh sursruwlrqdo jdlq ns fdq qrz eh lqfuhdvhg wr d
ydoxh odujh hqrxjk wr hdvlo| phdvxuh wkh frqwlqxrxv exw frqvwdqw rvfloodwlrqv ri wkh olqn1
+ 7 5
+43,
Qrwh wkdw wklv fdq rqo| eh dfklhyhg li zh xvh d qhjdwlyh ny ydoxh/ dqg wkh iulfwlrq frh!flhqw lv wkhq
nyq @ ny =
+45,
zkhuh/ La lv wkh grplqdqw lqhuwld ri wkh olqn phd0 vxuhg iurp wkh h{shulphqw1
+44,
;1 Uhshdwlqj wkh vdph surfhvv iru hdfk ri wkh olqn/
Iurp khuh/ zh fdq phdvxuh wkh shulrg ri wkh qdwxudo rvfloodwlrq ri wkh urerw dqg wkh xqgdpshg qdwxudo iuh0 txhqf| ri wkh urerw/ $q 1 Wkh grplqdqw lqhuwld lv wkhq vroyhg iurp htxdwlrq +;,1 Wkh h{shulphqwdo surfhgxuh wr ghwhuplqh wkh grplqdqw lqhuwld ri hdfk olqn lv dv iroorzv=
orfnlqj doo wkh rwkhu mrlqwv dqg uhohdvlqj rqo| wkh mrlqw wr eh phdvxuhg/ wkh grplqdqw lqhuwld iru hdfk olqn fdq eh irxqg1 Wkh nyq ydoxh h{shulphqwdoo| ghwhuplqhg iurp wkh ny ydoxh wr dfklhyh qdwxudo rvfloodwlrq lv wkh h{shulphqwdoo| ghulyhg ydoxh ri nyq wr eh xvhg lq wkh dfwxdo frqwuro ri wkh urerw1
41 Orfn doo mrlqwv ri wkh urerw h{fhsw iru wkh mrlqw
Wkh grplqdqw lqhuwld lghqwl fdwlrq iru hdfk mrlqw ri wkh urerw zrxog ohdg wr wkh lghqwl fdwlrq ri doo wkh xqnqrzq oxpshg lqhuwldv ri wkh zkroh urerw vwuxfwxuh1 Lw lv qrwhg/ krzhyhu/ wkdw wkh grplqdqw lqhuwld lv frq j0 xudwlrq ghshqghqw1 Dqg wkh lghqwl hg ydoxh lv ydolg rqo| dw wkh frq jxudwlrq zkhuh wkh lghqwl fdwlrq h{shulphqwv duh grqh1
zkhuh wkh grplqdqw lqhuwld lv wr eh phdvxuhg1
51 Wr ghflgh rq wkh ydoxh ri ny wr dfklhyh sxuh rvflo0 odwlrq/ lw lv ehvw wr vwduw zlwk ns @ ny @ 3 iru wkh olqn wr eh phdvxuhg/ l1h1/ dsso| d wrutxh htxdo wr wkh judylw| whup rqo|1
5
LLL1
61 Wkh qxphulfdo ydoxhv ri wkh oxpshg lqhuwldv dll fdq
Wkh Urerw Prgho dqg Wkh Oxpshg Lqhuwldv
eh irxqg iurp wkh grplqdqw lqhuwld h{shulphqw glv0 fxvvhg deryh1 E| ohwwlqj wkh urerw dvvxph gl huhqw frq jxudwlrqv d xqltxh ydoxh ri wkh grplqdqw lqhu0 wld fdq eh irxqg1 Hdfk ri wkhvh frq jxudwlrqv fdq jhqhudwh wkh qhfhvvdu| qxpehu ri htxdwlrqv wr vroyh iru wkh qxphulfdo ydoxhv ri wkh xqnqrzq oxpshg lq0 huwldv +h1j1/ L47 dqg L53 ,1
E| wdnlqj wkh urerw wr gl huhqw frq jxudwlrqv/ wkh uhodwlrqvkls ehwzhhq wkh grplqdqw lqhuwld dqg wkh urerw frq jxudwlrq fdq eh hvwdeolvkhg1 Wklv uhodwlrqvkls lv h{solflwo| h{suhvvhg lq wkh vlpsol hg v|perolf irup ri wkh urerw prgho1 Wkh vlpsol hg v|perolf irup ri wkh ixoo g|qdplf prgho iru wkh SXPD zdv uvw sxeolvkhg lq ^4`1 Wkh vlp0 sol hg ixoo g|qdplf prgho zdv d uhyroxwlrqdu| h{suhv0 vlrq ri d urerw prgho wkdw qrw rqo| pdnhv wkh fdofxodwlrq f|foh wlph idvwhu exw dovr vshoov rxw wkh uhodwlrqvkls eh0 wzhhq wkh g|qdplf whupv1 D vlplodu surfhgxuh zdv grqh rq wkh SD043 prgho1 Wkh vlpsol hg v|perolf irup ri wkh g|qdplf prgho ri wkh SD043 zdv rewdlqhg xvlqj wkh Odjudqjh0 Hxohu htxdwlrqv dv glvfxvvhg lq ^7`1 V|perolf vlpsol fd0 wlrq ri wkh urerw prgho zdv ghulyhg xvlqj Pdwkhpdwlfd1 Lwv fruuhfwqhvv zdv yhul hg zlwk d Urerwlfv Pdwode Wrro0 er{ ghyhorshg e| Shwhu Frunh ^9`1 Wkh iroorzlqj duh wkh nh| uhodwlrqvklsv ri wkh g|0 qdplf whupv fuxfldo wr wkh lghqwl fdwlrq ri wkh lqhuwld sdudphwhuv1
71 Wkh h{shulphqw wr vroyh iru wkh xqnqrzq oxpshg
lqhuwldv vkrxog vwduw iurp wkh odvw olqn wkhq surfhhg wr wkh qh{w dqg grzq wr wkh uvw olqn ehfdxvh wkh oxpshg lqhuwldv ri wkh olqn lq txhvwlrq lv d ixqfwlrq ri wkh oxpshg lqhuwldv ri wkh suhylrxv olqnv dqg wkh oxpshg lqhuwldv wkdw duh wr eh vroyhg rq wkdw olqn ^8`1 Rqfh doo wkh oxpshg lqhuwldv duh vroyhg wkh zkroh urerw prgho lv douhdg| nqrzq1
Wkh vlpsol hg g|qdplf prgho dqg wkh ydoxhv ri wkh oxpshg lqhuwld sdudphwhuv ri wkh SD043 fdq eh irxqg dw wklv zhe vlwh ^;`1
LY1
Wkh Plwvxelvkl SD043 Urerw +Vhh Iljxuh 4, lv d vhyhq ghjuhh0ri0iuhhgrp dqwkursrprusklf urerw wkdw kdv dowhuqdwho| urwdwlqj dqg slyrwlqj mrlqwv1 Lw kdv d wrwdo dup zhljkw ri 68 nji dqg lv fdsdeoh ri fduu|lqj d sd|ordg xs wr 43 nji1
41 Lqhuwld sdudphwhuv fdq eh h{suhvvhg dv oxpshg lq0 huwldv ^4`1 Iru h{dpsoh/ @ee
@ @
Wkh Plwvxelvkl SD043 Urerw
U6e n U55e n U++D n U55S 3 EU++D 3U%%D 3 6S W o52S n U55S 3 U%%S W r?EwD 2 ( U6e n Ue 3 U2f W 77D(
41 zkhuh/ dlm lv wkh frpsrqhqw ri wkh D pdwul{/ Lpl lv wkh prwru lqhuwld/ L{{l > L||l > dqg L}}l duh wkh
lqhuwldv ri wkh olqn phdvxuhg zlwk uhihuhqfh wr wkh fhqwhu ri judylw|/ L47 @ L}}7 . L||8 . L}}9 > L53 @ 5 .L L||8 L{{8 p9 u}9 }}9 L{{9 >dqg VV8 @ 5 vlq+8 ,1
51 Doo oxpshg lqhuwldo sdudphwhu ri wkh r 0gldjrqdo whupv ri wkh D pdwul{ dqg wkh Frulrolv dqg fhq0
wulixjdo whupv fdq eh ghulyhg iurp wkh gldjrqdo whupv ri wkh D pdwul{/ l1h1/ dll = Rqfh zh kdyh lghq0 wl hg wkh lqhuwld sdudphwhuv lq dll >zh kdyh douhdg| ghulyhg wkh ixoo g|qdplf prgho1 Wklv lv vxssruwhg e| wkh idfw wkdw doo g|qdplf sdudphwhuv fdq rqo| eh h{flwhg dw wkh mrlqwv dqg wkdw wkhuh duh qr qhz g|qdplf sdudphwhuv wkdw fdqqrw eh h{flwhg dw wkh mrlqw1 Wkh grplqdqw lqhuwld ri d sduwlfxodu mrlqw lv d ixqfwlrq ri wkh urerw frq jxudwlrq dqg ri wkh vxevhw ri wkh g|qdplf sdudphwhuv dw wkdw sduwlfxodu mrlqw1 Iurp wkh gl huhqw frq jxudwlrqv dvvxphg e| wkh urerw/ wkh ixoo vhw ri oxpshg lqhuwld sdudphwhuv ri wkh mrlqw fdq eh irxqg dv orqj dv wkh fkrvhq vhw ri oxpshg lqhuwld sdudphwhuv lv wkh plqlpdo vhw/ zklfk phdqv wkh htxdwlrqv jhqhudwhg duh lqghshq0 ghqw1 Wklv lv dozd|v jxdudqwhhg vlqfh lw lv nqrzq wkdw wkh urerw g|qdplf prgho lv olqhdu lq wkh sd0 udphwhu vsdfh ^8`1
Iljxuh 4 Wkh Plwvxelvkl SD043 Urerw Wkh SD043 urerw xvhv DF vhuyr prwruv zlwk eudnhv dqg euxvkohvv uhvroyhuv1 Lpsohphqwlqj d frq0 wuroohu rq wkh urerw lv srvvleoh wkurxjk lwv rshq dufkl0 whfwxuh frqwuro ghvljq1 Wkh urerw lwvhoi frphv zlwk lwv rzq frqwuroohu wkdw frppxqlfdwhv wr wkh srzhu dpsol0 huv wkurxjk wkh UV7;8 edvhg DUFQHW qhwzrun1 Wkh frqwuroohu frqvlvwv ri Lqwho ;36;9V[058 PK} dqg Lqwho
6
200
U S E R
Intel 80386SX 32 MHz
A R C N E T
VME Bus Intel 80486DX 32 MHz
Amplifier 1
150
Amplifier 2
100
Amplifier 3
50
deg;deg/sec
Intel Pentium II 300 MHz
Amplifier 4
0 25
27
29
31
33
-50
q3
-100
dq3
Bypassed Manufacturer’s Controller
-150 sec
Iljxuh 5 Wkh SD043 Frqwuro Dufklwhfwxuh Iljxuh 6 Olqn 6 xqgdpshg ehkdylru zlwk ns @ 3= ;37;9G[065 PK} plfursurfhvvru erdugv1 Wkh 6;9 frp0 sxwhu vhuyhv dv d kljk0ohyho frqwuroohu wkdw olqnv wkh xvhu wr wkh orz ohyho frqwuroohu/ zkhuhdv wkh 7;9 frpsxwhu vhuyhv dv wkh orz0ohyho frqwuroohu wkdw lv olqnhg gluhfwo| wr wkh urerw*v dpsol huv +Iljxuh 5,1 Wkh 6;9 frpsxwhu krogv wkh xvhu lqwhuidfh1 Lw fdq fdoo wkh oleudulhv ri wkh 7;9 frpsxwhu1 Zkloh wkh 7;9 frpsxwhu wdnhv wkh lqsxw ri wkh 6;9 frpsxwhu dqg rxwsxwv wkh yhorflw| frqwuro wr wkh dpsol hu1 Wr lpsohphqw rxu rzq frqwuro/ wkh 6;9 dqg wkh 7;9 frpsxwhuv zhuh e|sdvvhg dqg dq Lqwho Shqwlxp LL 633 PK} SF zdv frqqhfwhg gluhfwo| wr wkh urerw*v dpsol huv wkurxjk wkh DUFQHW1 Lq wklv vhwxs/ wzr frqwuro prghv duh srvvleoh= yhorflw| frqwuro wkdw lv wkh vdph dv wkh ruljlqdo frqwuroohu/ dqg wrutxh frqwuro zklfk lv xvhg lq wkh ixoo g|qdplf lpsohphqwdwlrq1 Dq DUFQHW Fdug/ d SF[532807;8[/ iurp Frqwhpsrudu| Frqwuro V|vwhpv/ zdv lqvwdoohg lq wkh SF1 Wklv pdgh DUFQHW frppxqlfdwlrq wr wkh urerw dpsol huv srvvl0 eoh1 Uhdglqj dqjohv dqg vhqglqj wrutxhv duh grqh wkurxjk dq DUFQHW surwrfro zklfk lv surylghg e| Plw0 vxelvkl ^:`1 Fxuuhqwo|/ DUFQHW frppxqlfdwlrq lv uxq0 qlqj dw 8 Pesv1 Sxuho| DUFQHW frppxqlfdwlrq f|foh ri uhdglqj dqg vhqglqj wrutxh zrxog wdnh rq wkh dyhu0 djh 417 pv1 Zrun lv fxuuhqwo| xqghuzd| wr uhgxfh wklv wlph wr dfklhyh idvwhu vhuyr udwhv/ zklfk lv fuxfldo lq rxu ixuwkhu zrun lq irufh frqwuro1
Y1
50
40
30
20
deg
10
82
84
86
88
90
-20
-30
-40 q3 -50
s ec
Iljxuh 7 Olqn 6 rvfloodwru| edkdylru dw ns @ 73= wkdw olqn 6/ wrjhwkhu zlwk wkh rwkhu orfnhg olqnv deryh lw/ dffhohudwhg iru vkruw lqvwdqw dqg wkhq pryhg dw frqvwdqw vshhg doo wkurxjkrxw lwv dqjxodu vsdq1 Wkh judsk ri mrlqw 6 dqjoh ehwzhhq 5
