a matlab based low cost autopilot for autonomous ...

$0$7/$%%$6('/2:&267$8723,/27 )25$872120286+(/,&237(5 '(3/2@ 7ZR PDMRU DSSURDFKHV DUH DYDLODEOHLQWKH)&6GHYHORSPHQWL GHYHORSPHQWRIFXVWRP V\VWHP DQG LL  SURFXUHPHQW RI RIIWKHVKHOI DXWRSLORW 7KH GHVLJQ DQG GHYHORSPHQW RI FXVWRPL]HG DXWRSLORW IRU )&6 KDV EHHQ UHSRUWHG DORQJVLGH UHVHDUFK DFWLYLWLHV LQ DXWRQRPRXV KHOLFRSWHURUVRPHWLPHVDVDVWDQGDORQHUHVHDUFKVWXG\$PRQJ WKH SURPLQHQW DFKLHYHPHQWV LQ WKLV UHJDUGV DUH WKH GHYHORSPHQW RI FXVWRP )&6 XVLQJ 3,& SURFHVVRU ERDUG ZLWK41;1HXWULQRUHDOWLPHRSHUDWLQJV\VWHP5726 IRUWKH 0,7;FHOO>@DQGIRUWKH186DXWRQRPRXVKHOLFRSWHU>@ &DUQHJLH 0HOORQ &08  DXWRQRPRXV KHOLFRSWHU EDVHG RQ @ +HUH WKH 03& ZDV XVHG IRU VHQVRUV DQG

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¶V 5HDO 7LPH :RUNVKRS57: IRUHPEHGGHGFRGHJHQHUDWLRQLVUHSRUWHGLQ

Proc. of the 8th International Conference on Intelligent Unmanned Systems (ICIUS 2012) Copyright © 2012 ICIUS 2012 Organisers ISBN: 978-981-07-4225-6 67

Proc. of the 8th International Conference on Intelligent Unmanned Systems (ICIUS 2012)

>@ &RQVLGHULQJ WKH IDFW WKDW PRVW RI WKH DXWRQRPRXV KHOLFRSWHU GHVLJQ DFWLYLWLHV DUH FRPPRQO\ FDUULHG RXW XVLQJ 0$7/$% WRROV WKLV HPEHGGHG GHVLJQ DSSURDFK SURPLVHV DQ LQWHJUDWHG GHVLJQ HQYLURQPHQW IRU UDSLGSURWRW\SLQJ RI DXWRSLORW IRU )&6 GHVLJQ DQG GHSOR\PHQW ,Q DGGLWLRQ WKH JHQHUDO EHOLHYH WKDW WKH DXWRPDWLFDOO\ FRGH JHQHUDWHG WKURXJK WKLV SURFHVV LV QHLWKHU UHOLDEOH QRU DV HIILFLHQW DV KDQGZULWWHQ FRGHV KDV EHHQ QRWHG WR EH DQ RXWGDWHG YLHZV LQ HPEHGGHG SURJUDPPLQJ >@ ,W KDV EHHQ GHPRQVWUDWHG WKDW WKH FRGH JHQHUDWHG LV RQO\  ODUJHU DQG  VORZHU WKDQ WKH KDQGZULWWHQ FRGH DQG WKLV DSSURDFK KDV EHHQ VXFFHVVIXOO\ HPSOR\HG LQ VHYHUDO DSSOLFDWLRQV VXFK DV LQGXVWULDO 3/& SURJUDPPLQJ QDYLJDWLRQ DQG IOLJKW FRQWURO V\VWHP  DQG K\SHUVRQLFVFUDPSMHWV>@0RUHVRWKHXVHRIWKLVDXWRPDWLF FRGHJHQHUDWLRQKDVEHQQVKRZQWRUHGXFHWKHGHVLJQF\FOHE\ DURXQG  >@ 0HDQZKLOH WKH JHQHULF FRGH JHQHUDWHG E\ 0$7/$% 57: LV FXUUHQWO\ OLPLWHG WR VSHFLILF IHDWXUHV RI D JLYHQ SURFHVVRU YLD 6LPXOLQN (PEHGGHG WDUJHW (7  7KHUH PD\ EH QHHG WR GR IHZ FRGH YHULILFDWLRQ SULRU WR FRPSLODWLRQ LQWR WKH WDUJHW KDUGZDUH 5HFHQWO\ DOWHUQDWLYH (7 KDV EHHQ SURSRVHG WDUJHWLQJ GHGLFDWHG PLFURFRQWUROOHU VXFK DV 0LFURFKLS 3,& XSZDUGV >@ 7KH LQWHJUDWLRQ RI WKLV WKLUG SDUW\ (7 WRROV SURYLGHV DXWRPDWLF JHQHUDWLRQ RI UHDG\WR GHSOR\FRGHIRUPLFURFKLS3,&GV3,& DQG3,&VHULHV 7KH DSSOLFDWLRQ RI WKLV LQWHJUDWHG GHVLJQ DSSURDFK LV UHSRUWHG LQ>@

,,

+$5':$5(&20321(1766(/(&7,216$1' '(6&5,37,21

7KH VHOHFWLRQ RI KDUGZDUH FRPSRQHQWV IRU WKH V\VWHP GHYHORSPHQW LV PDMRUO\ DIIHFWHG E\ ILUVW E\ WKH LQWHQGLQJ DSSOLFDWLRQ FRVW SHUIRUPDQFH H[SHFWDWLRQ DQG UHTXLUHG GHYHORSPHQW WRROV VRIWZDUH  7KH PDMRU KDUGZDUH HPSOR\HG DUH5&KHOLFRSWHUSODWIRUPLQHUWLDOPHDVXUHPHQWXQLWV,08  JOREDO SRVLWLRQLQJ V\VWHP *36  XOWUDVRQLF EDVHG DOWLPHWHU VHUYRVZLWFKLQJPXOWLSOH[HU $+HOLFRSWHU3ODWIRUP $  FODVV +LURER 5& KHOLFRSWHU +LURER 6';  LV VHOHFWHG DV WKH DXWRQRPRXV KHOLFRSWHU SODWIRUP ZLWK D )87$%$WHQFKDQQHOV5&WUDQVPLWWHUDQGUHFHLYHU+LURER FODVV6';LVDVLQJOHURWRUKREE\KHOLFRSWHUZLWKVWDELOL]HUEDU FDSDEOHRIERWKEDVLFDQG'DHUREDWLFIOLJKW7KHKHOLFRSWHULV VKRZQ LQ )LJ  DQG LWV JHQHUDO VSHFLILFDWLRQV DUH JLYHQ LQ 7DEOH

+HQFH DV SDUW RI DQ RQJRLQJ UHVHDUFK RQ DXWRQRPRXV KHOLFRSWHUWKLVVWXG\SUHVHQWVWKHGHYHORSPHQWRID0$7/$% EDVHGDXWRSLORWIRUIOLJKWFRQWUROV\VWHPGHSOR\PHQW7KRXJK WKH RYHUDOO JRDO RI WKH )&6 LV WR IXOO\ SURYLGH HVVHQWLDO IXQFWLRQDOLWLHVIRUDXWRQRPRXVKHOLFRSWHUGHSOR\PHQWDVVKRZQ LQ )LJ  WKH SUHVHQW VWXG\ IRFXVHV RQ WKH IOLJKW GDWD DFTXLVLWLRQ VWDJH 7KH UDSLGSURWRW\SLQJ RI ORZFRVW ELWV GLJLWDO VLJQDO SURFHVVRU GV3,&  LV IDFLOLWDWHG XVLQJ WKH LQWHJUDWHG GHVLJQ HQYLURQPHQW LQYROYLQJ 0$7/$% 6,08/,1.57:DWKLUGSDUW\GV3,&0$7/$%EORFNVHWV>@ DQGPLFURFKLSGHVLJQWRROV7KHSHUIRUPDQFHRIWKHGHYHORSHG DXWRSLORW LV HYDOXDWHG LQ UHDOIOLJKW WHVW RQ D VPDOO VFDOH KHOLFRSWHUSODWIRUP

)LJXUH+LURER6';  7$%/(,

*(1(5$/63(&,),&$7,2162)+,52%26';+(/,&237(5

&RPSRQHQWV )XOOOHQJWKRIIXVHODJH )XOOZLGWKRIIXVHODJH +HLJKW 0DLQURWRUGLDPHWHU 7DLOURWRUGLDPHWHU 6NLGUDWLRQ 'U\:HLJKW (QJLQH SD\ORDG

7KH UHVW RI WKH SDSHU LV DV IROORZV 6HFWLRQ ,, JLYHV EULHI RYHUYLHZ RI WKH  KDUGZDUH VHOHFWLRQ DQG GHVFULSWLRQ IROORZHG E\ WKH 0$7/$%EDVHG HPEHGGHG SURJUDPPH LQ VHFWLRQ ,,, 7KH KDUGZDUHVRIWZDUH LQWHJUDWLRQ LV SUHVHQWHG LQ VHFWLRQ ,9 ZKLOH WKH UHVXOWV RI ERWK VWDWLF DQG ILHOG WHVWV DUH SURYLGHG LQ VHFWLRQ 9 7KH SDSHU LV FRQFOXGHG LQ VHFWLRQ 9, ZLWK IXWXUH VWXG\



6SHFLILFDWLRQV PP PP PP PP PP  J L]HV530 J

 %2QERDUG3URFHVVRU 6HOHFWLRQ RI WKLV GHYLFH GHSHQGV SDUWO\ RQ WKH FRPSXWLQJ VSHFLILFDWLRQV SURFHVVLQJ VSHHG SURJUDP DQG GDWD PHPRU\  SRZHUFRQVXPSWLRQZHLJKWDQGRQWKHUHTXLUHGSURJUDPPLQJ GHYHORSPHQW WRROV  &RQVLGHULQJ DOO WKHVH IDFWRUV D ELWV GV3,&IMJ0+]&38.EIODVKPHPRU\.E 65$0 PHPRU\  WRJHWKHU ZLWK D ELWV 3,&00;)/ 0+] &38 .E IODVK PHPRU\ .E 65$0 PHPRU\  PLFURFRQWUROOHU ERDUGV DUH VHOHFWHG 7KHGV3,&LVWRSURYLGHVHQVRUVLQWHUIDFHDQGGDWDDFTXLVLWLRQ ZKLOH WKH 3,& LV PHDQW IRU FRQWURO DOJRULWKP LPSOHPHQWDWLRQ %RWK DUH WR EH LQWHUFRQQHFWHG IRU IXOO )&6 7KH\ DUH HTXLSSHG ZLWK VHYHUDO SHULSKHUDOV LQWHUIDFH IRU 63, 8$57 ,& 3:0 DQDORJ DQG GLJLWDO SRUWV FRPPXQLFDWLRQ 0RUHRYHU WKLV GHYLFH LV UHDGLO\ SURJUDPPDEOH ZLWK WKH

 )LJXUH%OFRNGLDJUDPRI)OLJKWFRQWUROV\VWHPFRPSRQHQWV

68

Proc. of the 8th International Conference on Intelligent Unmanned Systems (ICIUS 2012)

SURSRVHG 0$7/$%EDVHG HPEHGGHG GHVLJQ WRROV LQ WKLV VWXG\

'*OREDO3RVLWLRQLQJ6\VWHP*36  $  FKDQQHO (0$ *36 VPDUW DQWHQQD UHFHLYHU LV VHOHFWHGWRSURYLGHSRVLWLRQLQIRUPDWLRQ7KH*36LVFDSDEOHRI GDWD RXWSXW XS HYHU\  VHFRQG +]   ,W SURYLGHV 77/ VHULDO LQWHUIDFH ESV DQG RSHUDWHV DW 9P$ 7KH GHWDLOHG VSHFLILFDWLRQFDQEHREWDLQHGLQWKHGHYLFHGDWDVKHHW*OREDVDW  

2SHUDWLRQDOO\ WKH VRIWZDUH DOJRULWKP LV H[SHFWHG WR FRQ)LJ WKH 0&8 DQG LWV SHULSKHUDOV WR HVWDEOLVK WKH FRPPXQLFDWLRQ ZLWK WKH VHQVRUV WKURXJK WKH DSSURSULDWH LQWHUIDFH 7KHQ SURFHVV DQG WUDQVIRUP WKH DFTXLUHG GDWD LQWR DSSURSULDWH IRUPDW IRU RQZDUG WUDQVPLVVLRQ WR JURXQG VWDWLRQ DQG WR WKH IOLJKW FRQWURO DOJRULWKP DV IHHGEDFN VWDWHV $PRQJ WKH GDWD SURFHVVLQJ WDVNV WR EH SHUIRUPHG DUH L  ILOWHULQJ HVSHFLDOO\WKHDFFHOHURPHWHUDQGUDWHVJ\URGDWDDQGLL *36 GDWD SURFHVVLQJ ZKLFK PDMRUO\ LQYROYHV WUDQVIRUPDWLRQ IURP /DWLWXGH /RQJLWXGH DQG $OWLWXGH //$   IRUPDW WR (DUWK &HQWHUHG(DUWK)L[HG(&() IRUPDWDQGILQDOO\WR1RUWK(DVW 'RZQ 1('  7KH IOLJKW DOJRULWKP LV UHVSRQVLEOH IRU WKH H[HFXWLRQ RI WKH IOLJKW FRQWURO ODZ GHVLJQHG WR DXWRQRPRXVO\ JXLGH WKH KHOLFRSWHU IRU WKH LQWHQGHG PLVVLRQ 7KH RYHUDOO PLVVLRQ H[HFXWLRQ DQG FRRUGLQDWLRQ DUH KDQGOHG E\ WKH VXSHUYLVRU\ DOJRULWKP XQLW 7KLV EULHI RSHUDWLRQDO VHTXHQFH VHUYHVDVEDFNERQHIRUWKHVRIWZDUHGHYHORSPHQWWDVN7RWKLV HQG WKH SURJUDPPLQJ WDVN LV PRGXODUL]HG LQWR L  7DUJHW 0&8 FRQILJXUDWLRQ LL  GDWD DFTXLVLWLRQ SURFHVVLQJ DQG WUDQVPLVVLRQLLL JURXQGVWDWLRQLQWHUIDFHDQGGDWDORJJLQJLY  IOLJKW FRQWURO H[HFXWLRQ DQG Y  PLVVLRQ FRRUGLQDWLRQ DOJRULWKP  7KH ILUVW WKUHH PRGXOHV DUH GHWDLOHG LQ WKLV VWXG\ ZKLOH WKH ODVW WZR PRGXOHV FRQVWLWXWH SDUW RI WKH RQJRLQJ UHVHDUFK DFWLYLWLHV WRZDUGV IXOO DXWRQRPRXV KHOLFRSWHU V\VWHP GHYHORSPHQW

(:LUHOHVV'HYLFH &RQVLGHULQJWKHFRVWWKHHDVHRIFRQILJXUDWLRQDQGWKHQHHG WR RSHUDWH WKH V\VWHP ZLWKLQ .P GLVWDQFH IRU UHVHDUFK SXUSRVHVDYHU\SRSXODUPHPEHURI=LJEHHIDPLO\RIZLUHOHVV GHYLFHV *+] ;EHH SUR PRGXOH ZLWK ZLUHDQWHQQD  LV VHOHFWHG WR SURYLGH ELGLUHFWLRQDO FRPPXQLFDWLRQ EHWZHHQ WKH JURXQG FRPSXWHU DQG RQERDUG DYLRQLFV V\VWHP XS WR P RXWGRRU RSHUDWLRQ 0RUH VR FRPSDWLELOLW\ RI VHYHUDO =LJEHH GHYLFHV DOORZV IRU HDV\ IXWXUH XSJUDGH WR KLJKHU GLVWDQFH WUDQVPLVVLRQ

7KHLQWHJUDWHGHPEHGGHGGHVLJQHQYLURQPHQWFRPSULVHVRI IRXU SURJUDPPLQJ WRROV 0$7/$% WRROER[HV6,08/,1. D 0$7/$% GV3,& EORFNVHWV SURYLGHV 6,08/,1. EORFNV IRU 3,&SHULSKHUDOVLQWHUIDFH &ODQJXDJHDQGPLFURFKLS03/$% WRROV 7KHVH IRXU WRROV DUH LQWHJUDWHG ZLWKLQ 0$7/$% HQYLURQPHQW WR DFKLHYH D VLQJOH HQYLURQPHQW EDVHG UDSLG SURWRW\SLQJ RI WKH  ELWV FRUH GDWD DFTXLVLWLRQ 0&8 '$4 0&8   $ VLQJOH FOLFN LV UHTXLUHG WR FRPSLOH WKH GHYHORSHG 0$7/$% 6,08/,1. EDVHG DOJRULWKP WKDW LV GRZQORDGDEOH LQWRWKHHPEHGGHGWDUJHWXVLQJPLFURFKLSELWVFRPSLOHU DQG,QFLUFXLW3URJUDPPLQJDQG'HEXJJLQJ,&' 

)2WKHUV 2WKHU VXSSRUWLQJ KDUGZDUH DUH XOWUDVRQLF VHQVRU E\ 0D[ERWLF 0D[ERWLF  IRU ORZ DOWLWXGH PHDVXUHPHQW DQG &\WURQ VHUYR VZLWFKLQJ PXOWLSOH[HU 6&0  IRU PDQXDODXWR RSHUDWLRQ

)LJVKRZVWKHFRUHHPEHGGHG6,08/,1.EORFNVHPSOR\HG LQ WKH SURJUDPPLQJ  7KH HVVHQWLDO SURJUDPPLQJ VHTXHQFH LV SUHVHQWHGKHUHZKLOHWKHGHWDLOHGGHVFULSWLRQRIWKHVHEXLOGLQJ EORFNVFDQEHREWDLQHGLQ>@

&,QHUWLDO0HDVXUHPHQW8QLW,08  $ 0(0V EDVHG 91 E\ 9HFWRUQDY LV VHOHFWHG WR SURYLGHDWWLWXGHDQGKHDGLQJPHDVXUHPHQWRIWKH58$9VWDWHV 7KH GHYLFH LV VPDOO [[PP  OLJKW ZHLJKW J  ORZ SRZHU FRQVXPSWLRQ P$YROWV  $OVR LW FRPELQHV D[LV DFFHOHURPHWHU D[LV J\URV DQG D[LV PDJQHWLF VHQVRUV DV ZHOO DV D ELW SURFHVVRU RQ D VLQJOH PRGXOH ZLWK DQ LQEXLOW TXDWHUQLRQ EDVHG GULIW FRPSHQVDWLRQ .DOPDQILOWHU WR SURGXFH V\VWHPVWDWHVDWWLWXGHDQGKHDGLQJ DWDUDWHXSWR+]7KH ILOWHUFDQEHWXQHGWRPHHWDVSHFLILFDSSOLFDWLRQDQGRSHUDWLQJ FRQGLWLRQ /DVWO\ WKH GHYLFH FRPHV ZLWK D &OLEUDU\ WKDW IDFLOLWDWHV WLPHO\ LQWHUIDFLQJ RI WKH GHYLFH ZLWK RQERDUG SURFHVVRU

0&8FRQILJXUDWLRQWKLVLVDFKLHYHGZLWKGV3,&PDVWHUEORFN RI)LJD7KHWDUJHW0&8LVFRQ)LJGWRRSHUDWH0,36E\ DFWLYDWLQJ3KDVH/RFN/RRS3// 7KHVDPSOLQJUDWHLVVHWDW +]

,,, 0$7/$%%$6('(0%(''('62)7:$5( '(6,*1 7KHRSHUDWLRQRIWKHRQERDUGPLFURFRQWUROOHUXQLW0&8 LV JXLGHG E\ WKH VRIWZDUH GHYHORSHG WR FDUU\ RXW WKH QHFHVVDU\ WDVNV IRU DXWRQRPRXV IOLJKW 7KH PDMRU WKUHH IXQFWLRQDO FRPSRQHQWVRIWKHRQERDUG0&8DUHVKRZQLQ)LJ

 )LJXUH0$7/$%6,08/,1.GV3,&3,&EORFNVHWV





6HQVRU ,QWHUIDFLQJ WKLV LQYROYHV FRQILJXUDWLRQ RI WKH 0&8 SHULSKHUDOV IRU WKH VHQVRUV¶ LQWHUIDFH DQG GDWD WUDQVIRUPDWLRQ

)LJXUH)XQFWLRQDO%ORFNGLDJUDPRIWKH0&8

69

Proc. of the 8th International Conference on Intelligent Unmanned Systems (ICIUS 2012)

DQGSURFHVVLQJXVLQJDSSURSULDWHFXVWRP0$7/$%&FXVWRP SURJUDPPHV7KH8$57SRUWVDQGDUHFRQILJXUHGXVLQJWKH EORFNVHWVRI)LJFIRU*36DW.EV DQGZLUHOHVVPRGXOH .EV  UHVSHFWLYHO\ 7KH WUDQVPLVVLRQ UDWH RI WKH *36 LV VSHFLILHG EDVHG RQ WKH GHYLFH GHIDXOW UDWH RI .EV ZKLOH WKDWRIWKHZLUHOHVVGHYLFHLVFRPSXWHGEDVHGRQWRWDOH[SHFWHG GDWDVL]HSHUVDPSOHWLPH7RWDORIVWDWHGDWDFRPSULVHVRI DFFHOHURPHWHU  J\UR  UDWHV  DWWLWXGHVKHDGLQJ  SRVLWLRQ GDWD  YHORFLW\ GDWD  *36 UDZ GDWD  3:0 VHUYR FRQWURO GDWDDQGDOWLWXGHDUHH[SHFWHGWREHWUDQVPLWWHG7KLVJLYHVD WRWDORIELWVGDWDVDPSOHWLPHZLWKHDFKGDWDEHHQVHQWDW ELWV GDWD VL]H 7KH 63, SRUW LV FRQILJXUHG WR FRPPXQLFDWH ZLWK 91 ,08 VHQVRU XVLQJ WKH EORFN RI )LJ G ZLWK 0+] 63, FORFN IUHTXHQF\ $ FXVWRP &FRGHV LV ZULWWHQ WR FRPPXQLFDWHZLWKWKHGHYLFHOLEUDU\7KHFRGHVDUHLQWHJUDWHG LQWR WKH 6,08/,1. HQYLURQPHQW XVLQJ WKH EORFNVHWV RI )LJ K 7KH XOWUDVRQLF VHQVRU GDWD LV FDSWXUHG XVLQJ RQH RI WKH H[WHUQDO LQWHUUXSW SRUWV RI WKH 0&8 ZKLOH WKH IRXU 3:0 FRQWUROVLJQDOVF\FOLFFROOHFWLYHDQGSHGDO DUHFDSWXUHGZLWK IRXU RXWSXW FRPSDUH SRUWV 2&2&  1HFHVVDU\ GDWD WUDQVIRUPDWLRQDQGFRQYHUVLRQDUHGRQHXVLQJDFXVWRPPILOHV IXQFWLRQ LPSOHPHQWHG XVLQJ 0$7/$% HPEHGGHG WDUJHW 6,08/,1.EORFN

'HILQHG [ X p Y p Z p ] DVORFDWLRQRIWKHKHOLFRSWHUV\VWHP ZLWKFHQWHUFRLQFLGHGDSSUR[LPDWHO\WRWKH&*RIWKHV\VWHP DQG [ X r Y r Z r ] DV WKH ORFDWLRQ RI WKH *6 WKHQ WKH YHFWRU SRLQW [ x y z ] RI WKH KHOLFRSWHU IURP WKH *6 ORFDWLRQ LV FRPSXWHGXVLQJ ªx «y « «¬ z 

( N  h ) cos M cos O 

 

Y

( N  h ) cos M sin O  

 

Z

(

b N  h ) sin M  a



 N

  e  sin  M





ªp x  «« p y «¬ p z 



 f



b

a(  f ) 

º » » »¼

ª xˆ º ª x _ offset º « yˆ »  « y _ offset »   « » « » «¬ hˆ »¼ «¬ z _ offset »¼

ZKHUHWKHRIIVHWYHFWRU >x _ offset y _ offset

@ > $OVRWKH YHORFLW\ YHFWRULQ 1('FRRUGLQDWHV >v z _ offset

 @ 

v east @  north LV FRPSXWHG IURP WKH *36 UHSRUWHG KHDGLQJ GHJUHH  LQIRUPDWLRQRWKHUZLVHUHIHUWRDVFRXUVHRIWKHJURXQG&2*  DQGVSHHGRYHUWKHJURXQGPV 62* XVLQJWKHH[SUHVVLRQ 



 v

 v

north

east

SOG cos( COG ) 

 

SOG sin( COG )  

>

 

@

7KH ILQDO RXWSXW YHORFLW\ YHFWRU v x v y LV FRPSXWHG XVLQJ WKH YHORFLW\ UDWH ILOWHU WKDW XSGDWHV WKH YHORFLW\ LQIRUPDWLRQXVLQJ

 

 v

 

i 

v i  ( .  'v

new

 .  'v

old

) * T s 

 

ZKHUH T s LVWKHDSSOLFDWLRQVDPSOLQJWLPHDQGYHORFLW\UDWH LV 'v LVJLYHQE\

DQG SDUDPHWHUV D E DQG H DUH  WKH :*6 SDUDPHWHUV JLYHQDV

a

cos M r º ª X p  X r º » «  » « Y p Y r » »  sin M r »¼ «¬ Z p  Z r »¼   

&RQVLGHULQJWKHRIIVHWRIWKH*36GHYLFHIURPWKHORFDWLRQ RI WKH ,08 ZKLFK LV DSSUR[LPDWHO\ DW WKH &* RI WKH 58$9 WKHILQDOSRVLWLRQHVWLPDWLRQLVFRPSXWHGE\

ZKHUH1LVWKH5DGLXVRIFXUYDWXUHPHWHUV GHILQHGDV a

ª  sin M r cos O r sin M r sin O r «  sin O cos O r r « «¬ cos M r cos O r cos M r sin O r    

ZKHUH  M r DQG  O r DUH ODWLWXGH DQG ORQJLWXGH DQJOHV UHVSHFWLYHO\RIWKH*6ORFDWLRQ

*36 SRVLWLRQYHORFLW\ ILOWHUWKH*36SRVLWLRQLQIRUPDWLRQLQ /DWLWXGH /RQJLWXGH DQG $OWLWXGH //$  LQ *HRGHWLF FRRUGLQDWHVLVFRQYHUWHGWRSRVLWLRQLQQRUWKHDVWGRZQ1('  VXLWDEOH IRU QDYLJDWLRQ V\VWHP 7KH WUDQVIRUPDWLRQ LQYROYHV WKUHH OHYHOV //$ WR (DUWK &HQWHU (DUWK )L[HG (&()  FRRUGLQDWH (&() WR 1(' 7KH FRQYHUVLRQ IURP //$ [ M , O , h ] WR (&() [ X ,Y , Z ] LV DFKLHYHG XVLQJ WKH UHODWLRQVKLS>@ X

º » » »¼

 'v

v i v Ts

i 





 

+HDGLQJFRPSXWDWLRQ WKHKHDGLQJDQJOHDVDPHDVXUHGRI DQJOH PHDVXUHG FORFNZLVH IURP D WUXH 1RUWK HDUWK¶V SRODU D[LV  GLUHFWLRQ LV FRPSXWHG IURP WKH PHDVXUHG PDJQHWRPHWHU UHDGLQJV E\ WKH ,08 VHQVRU 91  7KH KHDGLQJ LV FDOFXODWHGXVLQJWKHUHODWLRQVKLS

    . 

7KH WUDQVIRUPDWLRQ IURP WKH (&() WR 1(8 LV DFFRPSOLVKHGZLWKUHVSHFWWRDORFDOUHIHUHQFHSRLQWZKLFKLV WDNHQ WR EH WKH ORFDWLRQ RI WKH *URXQG 6WDWLRQ *6  V\VWHP 1(8LVIRUPHGIURPDSODQHWDQJHQWWRWKHHDUWK¶VVXUIDFHDQG IL[HG WR WKH &* RI WKH KHOLFRSWHU KHQFH LW LV WHUPHG D ORFDO WDQJHQW RU ORFDO JHRGHWLF SODQH %\ FRQYHQWLRQ WKH QRUWK LV ODEHOHG[D[LVWKHHDVWDV\D[LVDQGXSDVWKH]D[LV

 Heading

tan  ( Y mc X mc ) 

 

ZKHUH X mc DQGY mc DUH [D[LV DQ \D[LV PDJQHWLF UHDGLQJ UHVSHFWLYHO\

70

Proc. of the 8th International Conference on Intelligent Unmanned Systems (ICIUS 2012)



7RDFFRXQWIRUWKHRULHQWDWLRQRIWKHV\VWHP X mc DQGY mc  DUH FRPSXWHG IURP DFWXDO WKUHH D[LV PDJQHWLF UHDGLQJV  X m Y m  Z m XVLQJWKHPDWKHPDWLFDOH[SUHVVLRQ X mc

X

m

 

+DUGZDUH VHWXS

0DWODEEDVHG SURJUDPPLQJ

'HSOR\PHQWLQ DXWRQRPRXV KHOLFRSWHU



cos I  Y m sin T sin I  Z m cos T sin I    Y mc Y m cos T  Z m sin T   

ZKHUH I DQG T DUHWKHUROODQGSLWFKDQJOHVGHVFULELQJWKH RULHQWDWLRQ RI WKH V\VWHP 1RWH WKDW ZKHUH LV V\VWHP LV KRUL]RQWDOO\IODW X mc X m DQGY mc Y m  7KH VLJQ RI KHDGLQJ FRPSXWDWLRQ LV GHWHUPLQHG XVLQJ WKH ORJLFDOH[SUHVVLRQDVIROORZV>@

­  tan  ( Y mc X mc ) if °  if °° tan ( Y mc X mc )  c c Heading   tan ( Y X ) if ® m m ° if ° °¯ if    



)LJXUH&RPSOHWHV\VWHPLQWHJUDWLRQVWDJHV

( X mc   ( X mc !  ,Y mc   ) ( X mc !  ,Y mc !  )  ( X mc  ,Y mc   ) ( X mc  ,Y mc !  )   

7$%/((67,0$7('&267:(,*+7$1'&855(175$7,1*

7KHHIIHFWRIPDJQHWLFGHFOLQDWLRQDQJOHZDVFRQVLGHUHGE\ VXEWUDFWLQJ WKH FRXQWU\ GHFOLQDWLRQ DQJOH 7KH DQJOH RI GHFOLQDWLRQRIWKHFRXQWU\PDJQHWLFILHOGZDVREWDLQHGIURPWKH 1DWLRQDO*HRSK\VLFDO&HQWHU12$$ >@DVGHJUHH 7KLVZDVDFFRXQWHGIRULQWKHILQDOKHDGLQJFRPSXWDWLRQXVLQJ

7KHVH LQFOXGH VWDWLF WHVW RQ JURXQG WHVW  DQG IOLJKW WHVW 7KH DWWLWXGHV KHDGLQJ DFFHOHUDWLRQ DQG UDWHV UHDGLQJ RI WKH ,08 VHQVRU DUH ORJJHG IRU XS WR PLQXWHV V  DW UDWH RI PV 7KLV ZDV UHSHDWHG ILYH WLPHV WKHQ WKH PHDQ DQG VWDQGDUG GHYLDWLRQ RI WKH VWDWLF UHDGLQJV DUH FRPSXWHG 7KH UHVXOWV REWDLQHG ZHUH ZLWKLQ WKH DFFXUDF\ RI WKH VHQVRU SHUIRUPDQFH VSHFLILFDWLRQ %\ VWDUWLQJ IURP D GLUHFWLRQ DSSUR[LPDWHO\FORVHGWRWUXH1RUWKXVLQJDFRPSDVVGHYLFHWKH KHOLFRSWHU V\VWHP ZDV URWDWHG FORFNZLVHO\ DQG EDFN WR WKH RULJLQDO SRLQW 7KH KHDGLQJ LQIRUPDWLRQ FRPSXWHG E\ WKH RQERDUGV\VWHPLVVKRZQLQ)LJ

+HDGLQJBILQDO KHDGLQJBFRPSXWHG   $ 0$7/$% PILOH IXQFWLRQV DQG UHOHYDQW 6,08/,1. EORFNVZHUHXVHGWRFRPSXWHERWKWKHSRVLWLRQYHORFLW\ILOWHUV DQGKHDGLQJFRPSXWDWLRQXVLQJWKHH[SUHVVLRQ 7KLVZDV WKHQ LPSOHPHQWHG XVLQJ 0$7/$% HPEHGGHG IXQFWLRQ 6LPXOLQNEORFN ,9 6@ )LJ  VKRZV WKH FRPSDUDWLYH YLHZ RI WKH UHSRUWHG *36 LQIRUPDWLRQ DQG *RRJOHPDS$VVKRZQLQ)LJWKH*36DFFXUDWHO\WUDFNHG WKH*RRJOH(DUWKZD\SRLQWV 



+HDGLQJIURPWUXH1RUWKGHJUHH

9





3(5)250$1&((9$/8$7,21$1'5(68/76











6HULHVRIWHVWVZHUHFDUULHGRXWWRHYDOXDWHWKHSHUIRUPDQFHRI WKHGHYHORSHGDXWRSLORWRQWKHDXWRQRPRXVKHOLFRSWHUV\VWHP



 











7LPHV



)LJXUH6DPSOHWHVWUHVXOWRI+HDGLQJIURPWUXH1RUWK

71



Proc. of the 8th International Conference on Intelligent Unmanned Systems (ICIUS 2012)



IOLJKW WHVWV RI WKH GHYHORSHG V\VWHP VKRZ WKH VXFFHVV DQG DELOLW\ RI WKH DXWRSLORW V\VWHP WR SURYLGH HIIHFWLYH IOLJKW GDWD HVWLPDWLRQ DQG DFTXLVLWLRQ UHTXLUHG LQ WKH DXWRQRPRXV KHOLFRSWHU V\VWHP GHVLJQ DQG GHSOR\PHQW 7KLV SURSRVHG LQWHJUDWHG GHVLJQ HQYLURPHQW LV H[SHFWHG WR VLPSOLILHG  WKH IXWXUH XSJUDGH DQG UHFRQILJXUDWLRQ RI V\VWHP IXQFWLRQDOLWLHV )XWXUHVWXG\LQYROYHVXSJUDGHRIWKHIOLJKWFRQWUROV\VWHPIRU IOLJKWFRQWURODOJRULWKPLPSOHPHQWDWLRQ  DFFHOHUDWLRQPV



D[PV





   

















































 7LPHV















D\PV



)LJXUH&RPSDULVRQRI*36DQG*RRJOHPDSSRVLWLRQLQIRUPDWLRQ

  



D]PV

 



)LHOGWHVW7KHV\VWHPLVFRPPDQGHGPDQXDOO\WRSHUIRUP YDULRXV IOLJKW RSHUDWLRQV VXFK DV WDNHRII DQG ODQGLQJ KRYHULQJ DQG FUXLVH IOLJKW DW YDU\LQJ VSHHG )RU D FRPSOHWH IOLJKW RSHUDWLRQ WHVW WKH SLORW WDNHVRII WKH KHOLFRSWHU RII WKH JURXQG HIIHFW KRYHULQJ DQG WKHQ FOLPEV YHUWLFDOO\ SHUIRUP KRYHULQJDQGH[FLWHWKHDWWLWXGHVDQGKHDGLQJZLWKVZHHSLQSXW VLJQDO WKHQ QDYLJDWH DURXQG WKH IOLJKW ILHOG 7KH REMHFWLYH RI WKLV WHVW LV WR HYDOXDWH WKH DELOLW\ DQG UHOLDELOLW\ RI WKH GHYHORSHG V\VWHP DQG LWV RQERDUG HOHFWURQLFV WR SHUIRUP WKH LQWHQGHG RSHUDWLRQ DQG ZLWKVWDQGLQJ VHYHUH RSHUDWLRQ FRQGLWLRQ )LJ )LJ VKRZ VDPSOHV RI FRQWURO LQSXWV DQG VWDWH UHVSRQVHV RI WKH V\VWHP IRU YDULRXV IOLJKW PRGHV 7KHVH IOLJKW GDWD FRQVWLWXWHG WKH LQIRUPDWLRQ UHTXLUHG IRU V\VWHP PRGHOLQJDQGDQDO\VLVLQRWKHUYDULRXVDXWRQRPRXVKHOLFRSWHU V\VWHP GHYHORSPHQWV 7KH VDPSOH RI SRVLWLRQ LQIRUPDWLRQ LQ 1(' FRRUGLQDWH LV VKRZQ LQ )LJ  ZKLOH WKH *RRJOH PDS YLHZRIDIOLJKWSDWWHUQLVVKRZQLQ)LJ

 



DWWLWXGHVGHJ 

UROOGHJ

   































 7LPHV













SLWFKGHJ

     





]P

     





 \P

VHUYRFRPPDQGXQLWV  ODW



)LJXUHPHDVXUHGDWWLWXGHV

7KH UHVXOWV RI ERWK JURXQG DQG ILHOG WHVWV VKRZ WKH HIIHFWLYHQHVV RI WKH GHYHORSHG DXWRSLORW WR SURYLGH QHFHVVDU\ IOLJKW GDWD DFTXLVLWLRQ DQG VWDWH HVWLPDWLRQ IRU WKH KHOLFRSWHU V\VWHPDQDO\VLV

































[P



  





















 ORQ



)LJXUHPHDVXUHGDFFHOHUDWLRQ

)LJXUH6DPSOHSRVLWLRQHVWLPDWLRQ

   

















































 7LPHV











 FRO

  



SHG

   



)LJXUHVHUYRFRPPDQGVLJQDO UDWHVGHJUHHV

SGHJV

    



















































 7LPHV











TGHJV

   



UGHJV





  



)LJXUHPHDVXUHGDQJXODUUDWHVUHVSRQVHV

)LJXUH6DPSOHIOLJKWWHVWURXWHZLWKLQWKHFDPSXVIO\LQJILHOG





$&.12:/('*0(17 9,&21&/86,21

7KLV UHVHDUFK ZDV VXSSRUWHG E\ WKH 50*6 5HVHDUFK 0DWFKLQJ *UDQW 6FKHPH  5HVHDUFK 0DQDJHPHQW &HQWHU ,,800DOD\VLD50*6

7KH GHYHORSPHQW RI D 0$7/$%EDVHG DXWRSLORW V\VWHP IRU DXWRQRPRXV KHOLFRSWHU V\VWHP KDV EHHQ UHSRUWHG LQ WKLV SDSHU 3HUIRUPDQFH HYDOXDWLRQ UHVXOWV IRU ERWK VWDWLF DQG LQ

72

Proc. of the 8th International Conference on Intelligent Unmanned Systems (ICIUS 2012) >@ 0 , /L]DUUDJD ³$XWRQRPRXV ODQGLQJ V\VWHP IRU D 8$9´ 0DVWHU¶V WKHVLV1DYDO3RVWJUDGXDWH6FKRRO0RQWHUH\&$86$0DUFK >@ %\RXQJ-LQB/HH 6HXQJ-XQB/HH DQG 3URI 6DQJN\XQJ 6XQJ 5RWDU\ 8$9 $XWRSLORW  1DYLJDWLRQ DQG $XWRSLORW 6\VWHP 'HYHORSPHQW RI 58$9 EDVHG RQ 9LUWXDO ,QVWUXPHQWDWLRQ 3ODWIRUP 2QERDUG $YLRQLFV ZLWK /DEYLHZ  1, 'HYLFHV ´ DYDLODEOH RQOLQH DW KWWSVGHFLEHOQLFRPFRQWHQWGRFV'2& DFFHVVHG GDWH -XQH   >@ 'DQLHO (UQVW  .LPRQ 9DODYDQLV  5LFKDUG *DUFLD  -HII &UDLJKHDG ³8QPDQQHG9HKLFOH&RQWUROOHU'HVLJQ(YDOXDWLRQDQG,PSOHPHQWDWLRQ )URP 0$7/$% WR 3ULQWHG &LUFXLW %RDUG´ - ,QWHOO 5RERW 6\VW   ± >@ 7KH 0DWKZRUNV ,QF ³1$6$¶V ;$ 6FUDPMHW $FKLHYHV 5HFRUG %UHDNLQJ 0DFK  6SHHG 8VLQJ 0DWK:RUNV 7RROV IRU 0RGHO%DVHG 'HVLJQ´8VHU6WRU\ >@ / .HUKXHO ³0DWODEVLPXOLQN GHYLFH GULYHU  EORFNVHW IRU 3,&GV3,& PLFURFRQWUROOHUV´KWWSZZZNHUKXHOHXZLNLLQGH[SKS >@ 0LFUREOR[³'XP7UDQVIRUPDWLRQVRI*36SRVLWLRQV´DSSOLFDWLRQQRWH ZZZXEOR[FK >@ +RQH\ZHOO³&RPSDVV KHDGLQJ XVLQJ PDJQHWRPHWHUV´ KWWSZZZKRQH\ZHOOFRPDHURFRPPRQGRFXPHQWVP\DHURVSDFHFDW DORJGRFXPHQWV'HIHQVHB%URFKXUHV GRFXPHQWV0DJQHWLFBB/LWHUDWXUHB$SSOLFDWLRQBQRWHV GRFXPHQWV$1B&RPSDVVB+HDGLQJB8VLQJB0DJQHWRPHWHUVSGI >@ 12$$³:RUOGPDJQHWLFPRGHO´  KWWSZZZQJGFQRDDJRYJHRPDJPRGHOV,*5):00MVS"GHIDXOW0RGH O :00 >@ $GDP6FKQHLGHU³*36YLVXDOL]HU´KWWSZZZJSVYLVXDOL]HUFRP 

5()(5(1&(6 >@ 7LPRWK\ + &R[ &KULVWRSKHU - 1DJ\0DUN $ 6NRRJ DQG  ,YDQ $ 6RPHUV ³ &LYLO 8$9 &DSDELOLW\ $VVHVVPHQW´ SUHSDUHG IRU  /DZUHQFH &DPDFKR 8$9 9HKLFOH 6HFWRU 0DQDJHU 9HKLFOH 6\VWHPV 3URJUDP 1$6$$HURQDXWLFV5HVHDUFK0LVVLRQ'LUHFWRUDWH >@ *DYULOHWV9$6KWHUHQEHUJ0$'DKOHK()HURQµ$YLRQLFV6\VWHP )RU$6PDOO8QPDQQHG+HOLFRSWHU3HUIRUPLQJ$JUHVVLYH0DQHXYHUV´ 'LJLWDO $YLRQLFV 6\VWHPV &RQIHUHQFH  3URFHHGLQJV '$6& 7KH WK >@ *XRZHL&DL/LQ)HQJ%HQ0&KHQ7RQJ+/HHµ6\VWHPDWLFGHVLJQ PHWKRGRORJ\ DQG FRQVWUXFWLRQ RI 8$9 KHOLFRSWHUV¶ 0HFKDWURQLFV   ±(OVHYLHU >@ 'DYLG 9LVVLHUH 3LHUUH-HDQ % $ODLQ 3LHUUH DQG 1LFRODV 3HWLW ³([SHULPHQWDO DXWRQRPRXV IOLJKW RI D VPDOOVFDOHG KHOLFRSWHU XVLQJ DFFXUDWH G\QDPLFV PRGHO DQG ORZFRVW VHQVRUV´ LQ SURFHHGLQJ RI WKH WK:RUOGFRQJUHVV,)$&6HRXO.RUHD-XO\ >@ % 9DJOLHQWL 5 +RDJ DQG 0 1LFXOHVFX ³3LFFROR 6\VWHP 8VHU¶V *XLGH´&ORXG&DS7HFKQRORJ\+RRG5LYHU2UHJRQ >@ 0LFUR3LORW ³0LFURSLORW :RUOG OHDGHU LQ PLQLDWXUH XDY DXWRSLORWV´ KWWSZZZPLFURSLORWFRP >@ 3URFHUXV7HFKQRORJLHV³.HVWUHODXWRSLORW´  KWWSZZZSURFHUXVXDYFRPSURGXFWV.HVWUHO$XWRSLORWSKS >@ &KULV$QGHUVRQ³$UGX3LORW´  KWWSGL\GURQHVQLQJFRPSURILOHVEORJVKRZ"LG $%ORJ3RVW $ >@ 3%ULVVHW$'URXLQ0*RUUD]3+XDUGDQG-7\OHU³7KH3DSDUD]]L 6ROXWLRQ´ QG 86(XURSHDQ &RPSHWLWLRQ DQG :RUNVKRS RQ 0LFUR $LU 9HKLFOHV1RYHPEHU >@ +DL