OBSTACLE AVOIDANCE FOR UNMANNED SURFACE VEHICLES: SIMULATIONS AND EXPERIMENTS A thesis submitted to the Department of Electrical Engineering, University of Moratuwa in partial fulfillment of the requirements for the Degree of Master of Philosophy by RANDOBAGEGEETHJAYENDRA Supervised by: Dr. Sisil Kumarawadu Department of Electrical Engineering University of Moratuwa, Sri Lanka 2009 93025 Abstract Sri Lanka ports authority and many other organizations are increasingly interested in the use of Unmanned Surface Vehicles (USV) for harbor security and surveillance applications. USVs can be used to collect information, samples and perform experiments inside a harbor or outside by. Navigating through ships and other objects. This research study is focused on finding algorithms for obstacle avoidance (OA) of USVs. The initial paradigm that is used to establish the solution was the OA of Unmanned Ground Vehicles (UGV). The algorithms developed for UGV were implemented practically with the limitations of hardware. Then, effort is taken to apply those algorithms to the surface vehicles with some modifications. In this study, a novel OA algorithm is proposed for static obstacles based on the Morphin algorithm. This proposed algorithm and the previous algorithm which is developed based on ground vehicles are compared with the potential field method. Static OA without dynamic OA is not helpful for unmanned vehicles on sea. A lot of researches have been carried out to avoid dynamic objects, but have failed to find an optimum solution although comparatively good approaches have been presented. Intelligent techniques have been rarely applied for dynamic obstacle avoidance. In this research, the effectiveness of applying intelligent or mathematical techniques for path prediction of dynamic obstacles is discussed with simulations to pick the best for a given situation. Then a noval projected dynamic obstacle area method is presented to avoid dynamic obstacles effectively. Comparative results are presented at the end to prove the strength "of the noval dynamic obstacle area method. D E C L A R A T I O N T h e w o r k s u b m i t t e d i n t h i s t h e s i s i s t h e r e s u l t o f m y o w n i n v e s t i g a t i o n , e x c e p t w h e r e o t h e r w i s e s t a t e d . I t h a s n o t a l r e a d y b e e n a c c e p t e d f o r a n y d e g r e e , a n d i s a l s o n o t b e i n g c o n c u r r e n t l y s u b m i t t e d f o r a n y o t h e r d e g r e e . P L ' - / r - R . G . J a y e n d r a ( C a n d i d a t e ) 2 3 r d M a r c h 2 0 0 9 I e n d o r s e t h e d e c l a r a t i o n b y t h e c a n d i d a t e . \ \ \ ( / - - c - · · · - - - " \ - . \ - - . - - ~- - - \ - - \ -~- . . , . _ _ ' \ _ _ _ _ - - - ~-- ' ' ' - - - · - " ~ · - · - - - , ' - _ ) - \ _ D r . S i s i l K u m a r a w a d u ( S u p e r v i s o r ) C O N T E N T S C h a p t e r T i t l e P a g e A b s t r a c t v A c k n o w l e d g e m e n t V l L i s t o f F i g u r e s V l l L i s t o f T a b l e s X l l L i s t o f A c r o n y m s X l l l 1 I n t r o d u c t i o n 1 1 . 1 A p p l i c a t i o n s o f U n m a n n e d S u r f a c e V e h i c l e s 1 . 2 O b s t a c l e A v o i d a n c e o f U n m a n n e d G r o u n d V e h i c l e s 1 . 3 A p p l i n g g r o u n d v e h i c l e t e c h n o l o g i e s f o r s u r f a c e v e h i c l e s , . , . ) 1 . 4 P o t e n t i a l f i e l d m e t h o d f o r p a t h p l a n n i n g 4 1 . 5 M o r p h i n a l g o r i t h m f o r p a t h p l a n n i n g 5 1 . 6 D e f i n i n g s a f e t y d i s t a n c e f o r p a t h p l a n n i n g 6 1 . 7 D y n a m i c o b s t a c l e a v o i d a n c e 6 1 . 8 S e n s o r c o n s i d e r a t i o n s 7 1 . 8 . 1 R a d a r C o n t a c t s 8 2 O b s t a c l e A v o i d a n c e o f U n m a n n e d G r o u n d V e h i c l e s 9 2 . 1 S e n s o r s e l e c t i o n f o r p r o t o t y p e s 9 2 . 2 D e v e l o p m e n t o f p r o t o t y p e s 1 0 2 . 2 . 1 D i g i t a l C o n t r o l l e r S e l e c t i o n 1 0 2 . 2 . 2 D i g i t a l C o m p a s s M o d u l e S e l e c t i o n 1 2 2 . 2 . 3 D i g i t a l E n c o d e r a n d E n c o d e r W h e e l 1 4 2 . 2 . 4 S e r v o M o t o r s 1 5 2 . 2 . 5 U l t r a - S o n i c R a n g e S e n s o r s 1 5 2 . 2 . 6 I n t e r V e h i c u l a r C o m m u n i c a t i o n s c h e m e 1 7 2 . 2 . 7 E R 4 0 0 R S R e c e i v e r 1 8 2 . 2 . 8 E R 4 0 0 T X T r a n s m i t t e r 1 9 2 . 2 . 9 S e r i a l I n t e r f a c e C i r c u i t D e s i g n 2 0 2 . 2 . 1 0 I n t e g r 2 . t i n g S e n s o r s t o t h e C o n t r o l l e r 2 2 2 . 2 . 1 1 I n t e r f a c i n g S o f t w a r e f o r P r o t o t y p e s 2 4 2 . 3 E x p e r i m e n t i n g w i t h p r o t o t y p e s 2 4 2 . 3 . 1 P o s i t i o n T r a c k i n g A l g o r i t h m 2 5 2 . 3 . 2 P e r i p h e r a l O b s t a c l e A v o i d a n c e 2 6 2 . 3 . 3 C o l l i s i o n A v o i d a n c e 2 6 2 . 3 . 4 P o s i t i o n T r a c k i n g w i t h o u t t h e D i g i t a l C o m p a s s 2 6 2 . 3 . 5 F u z z y B a s e d C o n t r o l l i n g 3 0 2 . 3 . 6 C o l l i s i o n C o n d i t i o n F u n c t i o n 3 1 2 . 3 . 7 R e l a t i v e D i s t a n c e F u n c t i o n 3 2 2 . 3 . 8 M a s t e r S l a v e S w i t c h i n g 3 2 2 . 3 . 9 C o n t r o l l i n g F u n c t i o n , . , , . , . ) . ) 2 . 4 R e s u l t s 3 5 2 . 5 S u m m a r y 3 6 1 1 3 4 5 6 D e s i g n o f N a v i g a t i o n a l C o n t r o l l e r f o r U S V 3 7 3 . 1 I m p l e m e n t i n g t h e c o n t r o l l e r 3 7 3 . 2 M a t h e m a t i c a l m o d e l f o r U S V 4 0 3 . 3 R e s u l t s f r o m t h e n a v i g a t i o n a l c o n t r o l l e r 4 2 3 . 4 S u m m a r y 4 5 S t a t i c O b s t a c l e A v o i d a n c e o f U S V 4 6 4 . 1 U t i l i z i n g G r o u n d V e h i c l e T e c h n o l o g i e s f o r S u r f a c e V e h i c l e s 4 6 4 . 1 . 1 D e s i g n o f O A c o n t r o l l e r 4 6 4 . 1 . 1 . 1 I n p u t F u n c t i m 1 s 4 6 4 . 1 . 1 . 2 O u t p u t F u n c t i o n s 5 2 4 . 1 . 2 A l g o r i t h m s f o r s i m u l a t i o n o f t h e c o n t r o l l e r 5 5 4 . 1 . 3 S i m u l a t i o n R e s u l t s f r o m t h e C o n t r o l l e r 5 7 4 . 1 . 4 S u m m a r y 5 9 4 . 2 N o v e l A l g o r i t h m f o r O A 6 0 4 . 2 . 1 M e t h o d o l o g y o f t h e N o v e l A l g o r i t h m 6 0 4 . 2 . 2 D e v e l o p e d A l g o r i t h m s U t i l i z e d f o r S i m u l a t i o n s 6 5 D y n a m i c O b s t a c l e A v o i d a n c e o f U S V 6 7 5 . 1 I n t r o d u c t i o n t o N o v e l D y n a m i c O b s t a c l e A v o i d a n c e M e t h o d 5 . 2 A r e a P r e d i c t i o n o f D y n a m i c O b s t a c l e s 5 . 3 P a t h P r e d i c t i o n o f D y n a m i c O b s t a c l e s 5 . 3 . I P o l y n o m i a l A p p r o x i m a t i o n M e t h o d f o r P a t h P r e d i c t i o n 5 . 3 . 2 G e n e r a l i z e d R e g r e s s i o n N e u r a l N e t w o r k f o r P a t h P r e d i c t i o n S i m u l a t i o n R e s u l t s 6 7 6 9 7 1 7 3 7 3 7 4 6 . 1 S i m u l a t i o n R e s u l t s b y A p p l y i n g U G V t h e o r i e s f o r U S V 7 4 6 . 2 S i m u l a t i o n R e s u l t s f r o m t h e N o v e l A l g o r i t h m 7 5 6 . 3 S i m u l a t i o n R e s u l t s f r o m P o t e n t i a l F i e l d M e t h o d 8 0 6 . 4 C o m p a r i s o n o f O b s t a c l e a v o i d a n c e m e t h o d s 8 2 6 . 5 S i m u l a t i o n R e s u l t s D y n a m i c O b s t a c l e A v o i d a n c e 8 4 6 . 5 . 1 S i m u l a t i o n R e s u l t s f o r p a t h p r e d i c t i o n o f D y n a m i c O b s t a c l e s 8 6 6 . 5 . 1 . 1 P o l y n o m i a l a p p r o x i m a t i o n m e t h o d f o r p a t h p r e d i c t i o n 8 6 6 . 5 . 1 . 1 . 1 S i m u l a t i o n s w i t h o u t s e n s o r n o i s e 8 6 6 . 5 . 1 . 1 . 2 S i m u l a t i o n s w i t h s e n s o r n o i s e 8 9 6 . 5 . 1 . 2 R B N N m e t h o d f o r p a t h p r e d i c t i o n 9 5 6 . 5 . 1 . 2 . 1 S i m u l a t i o n s \ V i t h o u t s e n s o r n o i s e 9 5 6 . 5 . 1 . 2 . 2 S i m u l a t i o n s w i t h s e n s o r n o i s e 9 7 6 . 5 . 1 . 3 S u m m a r y o f S i m u l a t i o n R e s u l t s f o r p a t h p r e d i c t i o n 9 9 6 . 5 . 2 S i m u l a t i o n R e s u l t s f o r O b s t a c l e a r e a p r e d i c t i o n o f D y n a m i c O b s t a c l e s 1 0 0 6 . 5 . 2 . 1 S i m u l a t i o n R e s u l t s f r o m V e l o s i t y o b s t a c l e m e t h o d 1 0 0 6 . 5 . 2 . 2 S i m u l a t i o n R e s u l t s f r o m t h e n o v e l m e t h o d 1 0 4 6 . 5 . 2 . 3 C o m p a r i s o n R e s u l t s 1 0 9 1 1 1 7 C o n c l u s i o n a n d R e c o m m e n d a t i o n s 1 1 3 R e f e r e n c e s A p p e n d i x A A p p e n d i x B A p p e n d i x C A p p e n d i x D A p p e n d i x E A p p e n d i x F A p p e n d i x G 1 1 5 O O P i c B a s i c P r o g r a m f o r V e h i c u l a r P r o t o t y p e s 1 1 9 M a t L a b P r o g r a m o f F u z z y P D N a v i g a t i o n a l C o n t r o l l e r w i t h t h e D y n a m i c m o d e l 1 2 9 M a t L a b P r o g r a m f o r A p p l i n g U G V a l g o r i t h m s t o U S V 1 3 5 M a t L a b P r o g r a m f o r N o v e l A l g o r i t h m t o A v o i d S t a t i c O b s t a c l e s 1 3 9 M a t L a b P r o g r a m f o r A p p l i n g P o l y n o m i a l A p p r o x i m a t i o n t o P a t h P r e d i c t i o n 1 4 3 M a t L a b P r o g r a m f o r A p p l i n g G R N N t o P a t h P r e d i c t i o n 1 4 5 M a t L a b P r o g r a m t o C o m p a r e V e l o c i t y O b s t a c l e M e t h o d w i t h N o v e l M e t h o d f o r A r e a P r e d i c t i o n o f D y n a m i c O b s t a c l e s 1 4 9 l V A c k n o w l e d g e m e n t M a n y t h a n k s a r e d u e f i r s t t o m y s u p e r v i s o r , D r . S i s i l K u m a r a w a d u , f o r h i s g r e a t i n s i g h t s , p e r s p e c t i v e s a n d g u i d a n c e t h r o u g h o u t t h e e n t i r e d u r a t i o n o f t h e s t u d y . A u t h o r e x t e n d s h i s s i n c e r e g r a t i t u d e t o D r . J . P . K a r u n a d a s a , H e a d o f t h e D e p a r t m e n t o f E l e c t r i c a l E n g i n e e r i n g , f o r p r o v i d i n g h i m t h e R e s e a r c h A s s i s t a n t s h i p a n d e x c e l l e n t a s s i s t a n c e d u r i n g t h e s t u d y p e r i o d . M a n y t h a n k s a n d a p p r e c i a t i o n s a r e d u e t o P r o f e s s o r H . Y . R . P e r e r a , C o m m i s s i o n e r G e n e r a l o f P u b l i c U t i l i t i e s C o m m i s s i o n o f S r i L a n k a a s w e l l . S i n c e r e t h a n k s a r e a l s o d u e t o t h e o f f i c e r s i n P o s t G r a d u a t e O f f i c e o f t h e F a c u l t y o f E n g i n e e r i n g , U n i v e r s i t y o f M o r a t u w a f o r h e l p i n g i n v a r i o u s w a y s t o c l a r i f y t h e t h i n g s r e l a t e d t o a c a d e m i c w o r k s i n t i m e w i t h e x c e l l e n t c o o p e r a t i o n a n d g u i d a n c e . T h a n k s a r e a l s o d u e t o t h e s t a f f o f t h e D e p a r t m e n t o f E l e c t r i c a l E n g i n e e r i n g f o r t h e s u p p o r t e x t e n d e d d u r i n g t h e s t u d y p e r i o d . A l s o , I w i s h t o g r a t e f u l l y a c k n o w l e d g e t h e a s s i s t a n c e e x t e n d e d b y M r . R a v i p r i y a R a n a t u n g a , M r . S a m i t h a R a n s a r a , M r . S a n j e e w a P r i y a d h a r s h a n a , M r . G a m i n i J a y a s i g h e , M r . L a c k s h a n P i y a s i g h e , M r . K o l i t h a D h a r m a p r i y a , M r . J . B a c k , M r . H a r s h a n a S o m a p r i y a , M r . N a d u n C h a m i k a r a a n d m y b r o t h e r C h a n u k a J a y e n d r a . M a n y t h a n k s a r e a l s o d u e t o m a n y i n d i v i d u a l s , f r i e n d s a n d c o l l e a g u e s w h o h a v e n o t b e e n m e n t i o n e d h e r e b y n a m e i n m a k i n g t h i s e d u c a t i o n a l p r o c e s s a s s u c c e s s . L a s t l y , t h e a u t h o r e x p r e s s e s h i s d e e p a p p r e c i a t i o n t o w a r d s h i s f a m i l y f o r t h e i r e n c o u r a g e m e n t a n d s u p p o r t . T h i s w o r k i s d e d i c a t e d t o h i s b e l o v e d m o t h e r a n d l a t e f a t h e r . V I L i s t o f F i g u r e s N o . D e s c r i p t i o n P a g e 1 . 1 S a f e t y D i s t a n c e f o r O b s t a c l e s 6 2 . 1 B a s i c B l o c k D i a g r a m o f t h e s y s t e m 1 0 2 . 2 O O P i c R M i c r o C o n t r o l l e r B o a r d 1 2 ' ) " l _ , . ) D i g i t a l C o m p a s s M o d u l e 1 3 2 . 4 T h e W a y o f M o u n t i n g t h e C o m p a s s t o t h e P r o t o t y p e 1 3 2 . 5 C o n n e c t i o n A r r a n g e m e n t o f t h e E n c o d e r 1 4 2 . 6 T h e w a y o f m o u n t i n g t h e E n c o d e r t o t h e P r o t o t y p e 1 4 2 . 7 H S - 4 2 2 S e r v o M o t o r 1 5 2 . 8 B e a m P a t t e r n o f t h e S R F 2 3 5 ' P e n c i l b e a m ' U l t r a - s o n i c S e n s o r 1 6 2 . 9 S R F 2 3 5 P e n c i l B e a m U l t r a - s o n i c S o n a r S e n s o r 1 6 2 . 1 0 L P R S E R 4 0 0 R a d i o M o d u l e s 1 7 2 . 1 1 E v a l u a t i o n S o f t w a r e 1 7 2 . 1 2 C o m m u n i c a t i o n C h a n n e l D e d i c a t i o n 1 8 2 . 1 3 R e c e i v e r 1 9 2 . 1 4 T r a n s m i t t e r 2 0 2 . 1 5 S e r i a l I n t e r f a c e C i r c u i t 2 1 2 . 1 6 T h e W a y o f M o u n t i n g T r a n s c e i v e r M o d u l e 2 2 2 . 1 7 P r o p o s e d P r o t o t y p e ( P l a n ) 2 3 2 . 1 8 T h e P r o p o s e d P r o t o t y p e ( S i d e v i e w ) 2 3 2 . 1 9 I m p l e m e n t e d P r o t o t y p e 2 4 2 . 2 0 P o s i t i o n T r a c k i n g w i t h C o m p a s s 2 5 2 . 2 1 E x e c u t e d P r o g r a m i n O O P i c 2 8 2 . 2 2 P o s i t i o n T r a c k i n g w i t h o u t C o m p a s s 2 9 2 . 2 3 P o s i t i o n T r a c k i n g w i t h o u t C o m p a s s ( b l o c k d i a g r a m ) 3 0 2 . 2 4 F u z z y B a s e d C o n t r o l l i n g 3 1 2 . 2 5 C o l l i s i o n C o n d i t i o n F u n c t i o n a f t e r T r a i n i n g 3 4 2 . 2 6 R e l a t i v e D i s t a n c e F u n c t i o n a f t e r T r a i n i n g 3 4 2 . 2 7 M a s t e r - S l a v e S w i t c h i n g F u n c t i o n a f t e r T r a i n i n g 3 5 2 . 2 8 S c r e e n s h o t o f t h e D e v e l o p e d G U I 3 6 3 . 1 B o a t v v i t h F u z z y B a s e d N a v i g a t i o n a l C o n t r o l l e r 3 7 " l " ) J . - " - F u z z y B a s e d N a v i g a t i o n a l C o n t r o l l e r 3 8 V l l ~ , . , E r r o r I n p u t M e m b e r s h i p F u n c t i o n o f ) . ) t h e F u z z y b a s e d N a v i g a t i o n a l C o n t r o l l e r 3 8 3 . 4 R a t e o f C h a n g e E r r o r I n p u t M e m b e r s h i p F u n c t i o n o f t h e F u z z y b a s e d N a v i g a t i o n a l C o n t r o l l e r 3 9 3 . 5 R a t e o f C h a n g e E r r o r I n p u t M e m b e r s h i p F u n c t i o n o f t h e F u z z y - b a s e d N a v i g a t i o n a l C o n t r o l l e r 3 9 3 . 6 O u t p u t S u r f a c e o f t h e F u z z y b a s e d N a v i g a t i o n a l C o n t r o l l e r 4 0 3 . 7 P i c t u r e o f t h e A c t u a l B o a t M o d e l 4 1 3 . 8 P a t h T r a c k i n g o f a S i n u s o i d a l T r a j e c t o r y w i t h P D C o n t r o l l e r 4 2 3 . 9 P a t h T r a c k i n g o f a S i n u s o i d a l T r a j e c t o r y w i t h F u z z y C o n t r o l l e r 4 3 3 . 1 0 E r r o r i n L a t e r a l D i r e c t i o n o f a S i n u s o i d a l T r a j e c t o r y w i t h F u z z y C o n t r o l l e r 4 3 3 . 1 1 E r r o r i n L o n g i t u d i n a l D i r e c t i o n o f a S i n u s o i d a l T r a j e c t o r y w i t h F u z z y C o n t r o l l e r 4 4 3 . 1 2 P a t h T r a c k i n g o f a S t r a i g h t T r a j e c t o r y w i t h F u z z y C o n t r o l l e r 4 4 4 . 1 C a l c u l a t i n g C o l l i s i o n D i r e c t i o n 4 7 4 . 2 C a l c u l a t i n g C o l l i s i o n D i r e c t i o n 4 8 4 . 3 B o a t s P a t h N e a r O b s t a c l e w i t h o u t O b s t a c l e A v o i d a n c e C o n t r o l l e r 4 9 4 . 4 j J C h a n g e s N e a r O b s t a c l e 4 9 4 . 5 N a m e s o f C o l l i s i o n D i r e c t i o n I n p u t M e m b e r s h i p F u n c t i o n 5 0 4 . 6 C o l l i s i o n D i r e c t i o n I n p u t M e m b e r s h i p F u n c t i o n t o t h e O b s t a c l e A v o i d a n c e C o n t r o l l e r 5 0 4 . 7 R e l a t i v e D i s t a n c e I n p u t M e m b e r s h i p F u n c t i o n t o t h e O b s t a c l e A v o i d a n c e C o n t r o l l e r 5 1 4 . 8 X D i r e c t i o n V c l o c i t y O u t p u t S u r f a c e o f t h e O b s t a c l e A v o i d a n c e C o n t r o l l e r 5 3 4 . 9 Y D i r e c t i o n V e l o c i t y O u t p u t S u r f a c e o f t h e O b s t a c l e A v o i d a n c e C o n t r o l l e r 5 4 4 . 1 0 A l g o r i t h m o f t h e O b s t a c l e A v o i d a n c e C o n t r o l l e r 5 5 4 . 1 1 S i m u l a t i o n S e t u p o f t h e O b s t a c l e A v o i d a n c e C o n t r o l l e r 5 6 4 . 1 2 P a t h S t a r t i n g N e a r R o o t o f t h e C o o r d i n a t e S y s t e m 5 7 4 . 1 3 P a t h S t a r t i n g N e a r R o o t o f t h e C o o r d i n a t e S y s t e m 5 7 4 . 1 4 P a t h S t a r t i n g n e a r t h e L e f t C o r n e r o f t h e C o o r d i n a t e S y s t e m 5 8 4 . 1 5 P a t h F i n i s h i n g N e a r R o o t 5 8 4 . 1 6 P a t h S t a r t i n g N c a r O b s t a c l e 5 9 4 . 1 7 O b s t a c l e i n t h e G r i d 6 0 4 . 1 8 O b s t a c l e M a t r i x w i t h O b s t a c l e 6 1 4 . 1 9 P o s s i b l e P a t h s o f t h e U S V 6 1 V l l l 4 . 2 0 P o s s i b l e P a t h s o f t h e U S V o n t h e O b s t a c l e M a t r i x 6 2 4 . 2 1 O b s t a c l e C o o r d i n a t e C o n v e r s i o n 6 3 4 . 2 2 P a t h V a l u e C a l c u l a t i o n 6 4 4 . 2 3 S i m p l i f i e d F l o w C h a r t o f t h e N o v e l A l g o r i t h m 6 6 5 . 1 T w o D y n a m i c O b s t a c l e s w i t h P r o j e c t e d O b s t a c l e A r e a s 6 7 5 . 2 T w o D y n a m i c O b s t a c l e s w i t h t h e i r R e d u c e d P r o j e c t e d O b s t a c l e A r e a s 6 8 5 . 3 P a t h P l a n n i n g B e t w e e n t h r e e D y n a m i c O b s t a c l e s 6 8 5 . 4 E f f e c t i v e T i m e P r e d i c t i o n o f D y n a r n i c O _ b s t a c l c s 6 9 5 . 5 A r e a C a l c u l a t i o n s f o r D y n a m i c O b s t a c l e 7 0 5 . 6 S i m u l a t i o n r e s u l t o f a n A c t u a l a n d P r e d i c t e d p a t h i n 3 D s p a c e 7 1 6 . 1 O b s t a c l e A v o i d a n c e U s i n g U G V A l g o r i t h m s ( S a f e t y d i s t a n c e = 5 0 m ) 7 4 6 . 2 O b s t a c l e A v o i d a n c e U s i n g U G V A l g o r i t h n ; s ( S a f e t y d i s t a n c e = 6 0 m ) 7 5 6 . 3 P o s s i b l e P a t h s M a t r i x o f t h e U S V 7 6 6 . 4 O b s t a c l e o n t h e O b s t a c l e M a t r i x A 7 6 6 . 5 O b s t a c l e o n t h e O b s t a c l e M a t r i x B 7 7 6 . 6 O b s t a c l e o n t h e O b s t a c l e M a t r i x C 7 7 6 . 7 O b s t a c l e o n t h e O b s t a c l e M a t r i x D 7 7 6 . 8 O b s t a c l e A v o i d a n c e u s i n g N a v a l A l g o r i t h m ( S a f e t y d i s t a n c e = 4 0 m ) 7 8 6 . 9 O b s t a c l e A v o i d a n c e u s i n g N a v a l A l g o r i t h m ( S a f e t y d i s t a n c e = 6 0 m ) 7 9 6 . 1 0 C o n t o u r M a p o f P o t e n t i a l F i e l d w i t h 3 o b s t a c l e s 8 0 6 . 1 1 3 D S u r f a c e o f P o t e n t i a l F i e l d w i t h 3 o b s t a c l e s 8 1 6 . 1 2 O b s t a c l e A v o i d a n c e U s i n g P F M 8 1 6 . 1 3 C o m p a r i s o n o f S a f e t y D i s t a n c e s 8 2 6 . 1 4 C o m p a r i s o n o f D i s t a n c e s T o w a r d s t w o O b s t a c l e s 8 3 6 . 1 5 D y n a m i c O b s t a c l e A v o i d a n c e w i t h T i m e V a l u e s 8 4 6 . 1 6 D y n a m i c O b s t a c l e A v o i d a n c e w i t h 3 O b s t a c l e s 8 5 6 1 7 L o n g i t u d i n a l C o o r d i n a t e s o f A c t u a l P a t h 8 6 6 . 1 8 L a t e r a l C o o r d i n a t e s o f A c t u a l P a t h 8 6 6 . 1 9 A c t u a l P a t h a n d P r e d i c t e d P a t h s o f t h e O b s t a c l e f o r D i f f e r e n t D e g r e e s 8 7 6 . 2 0 L o n g i t u d i n a l E r r o r o f P r e d i c t e d P a t h s f o r D i f f e r e n t D e g r e e s o f P o l y n o m i a l s 8 8 6 . 2 1 L a t e r a l E r r o r o f P r e d i c t e d P a t h s f o r D i f f e r e n t D e g r e e s o f P o l y n o m i a l s 8 8 6 . 2 2 L o n g i t u d i n a l C o o r d i n a t e s o f A c t u a l P a t h w i t h N o i s e ( n o i s e = 5 m ) 8 9 0 . 2 3 L a t e r a l C o o r d i n a t e s o f A c t u a l P a t h w i t h N o i s e ( n o i s e = 5 m ) 8 9 6 . 2 4 A c t u a l P a t h o f t h e O b s t a c l e w i t h N o i s e ( n o i s e = 5 m ) 9 0 6 . 2 5 A c t u a l a n d P r e d i c t e d P a t h o f t h e O b s t a c l e ( D e g r e e = 6 , n o i s e = l m ) 9 1 l X 6 . 2 6 A c t u a l a n d P r e d i c t e d P a t h o f t h e O b s t a c l e ( D e g r e e = 5 , n o i s e = 1 m ) 9 2 6 . 2 7 A c t u a l a n d P r e d i c t e d P a t h o f t h e O b s t a c l e ( D e g r e e = 4 , n o i s e = 1 m ) 9 3 6 . 2 8 A c t u a l a n d P r e d i c t e d P a t h o f t h e O b s t a c l e ( D e g r e e = 4 , n o i s e = 5 m ) 9 4 6 . 2 9 A c t u a l P a t h a n d P r e d i c t e d P a t h s o f t h e O b s t a c l e f o r D i f f e r e n t S p r e a d s 9 5 6 . 3 0 L o n g i t u d i n a l E r r o r o f P r e d i c t e d P a t h s f o r D i f f e r e n t S p r e a d s 9 6 6 . 3 1 L a t e r a l E r r o r o f P r e d i c t e d P a t h s f o r D i f f e r e n t S p r e a d s 9 6 6 . 3 2 A c t u a l P a t h a n d P r e d i c t e d P a t h s o f t h e O b s t a c l e f o r D i f f e r e n t N o i s e V a l u e s 9 7 6 . 3 3 L o n g i t u d i n a l E r r o r o f P r e d i c t e d P a t h s f o r D i f f e r e n t N o i s e V a l u e s 9 8 6 . 3 4 L a t e r a l E r r o r o f P r e d i c t e d P a t h s f o r D i f f e r e n t N o i s e V a l u e s 9 9 6 . 3 5 L o n g i t u d i n a l V e l o c i t y o f t h e O b s t a c l e 1 0 0 6 . 3 6 L a t e r a l V e l o c i t y o f t h e O b s t a c l e 1 0 1 6 . 3 7 U p p e r a n d L o w e r B o u n d a r i e s o f P r c d i c t e d O b s t a c l e s A r e a , L o n g i t u d i n a l D i r e c t i o n 1 0 1 6 . 3 8 U p p e r a n d L o w e r B o u n d a r i e s o f P r e d i c t e d O b s t a c l e s A r e a , L a t e r a l D i r e c t i o n 1 0 2 6 . 3 9 E r r o r t o w a r d s L o n g i t u d i n a l D i r e c t i o n 1 0 3 6 . 4 0 E r r o r t o w a r d s L a t e r a l D i r e c t i o n 1 0 3 6 . 4 1 U p p e r a n d L o w e r B o u n d a r i e s o f P r e d i c t e d O b s t a c l e s A r e a t o w a r d s L o n g i t u d i n a l D i r e c t i o n ( S = 0 . 5 ) 6 . 4 2 U p p e r a n d L o w e r B o u n d a r i e s o f P r e d i c t e d O b s t a c l e s A r e a T o w a r d s L a t e r a l D i r e c t i o n ( S = 0 . 5 ) 6 . 4 3 E r r o r T o w a r d s L o n g i t u d i n a l D i r e c t i o n ( S = 0 . 5 ) 6 . 4 4 E r r o r T o w a r d s L a t e r a l D i r e c t i o n ( S = 0 . 5 ) 6 . 4 5 U p p e r a n d L o w e r B o u n d a r i e s o f P r e d i c t e d O b s t a c l e s A r e a T o w a r d s L o n g i t u d i n a l D i r e c t i o n f o r D i f f e r e n t S e a C o n d i t i o n V a l u e s 6 . 4 6 U p p e r a n d L o w e r B o u n d a r i e s o f P r e d i c t e d O b s t a c l e s A r e a t o w a r d s L a t e r a l D i r e c t i o n f o r D i f f e r e n t S e a C o n d i t i o n V a l u e s 6 . 4 7 W i d t h o f P r e d i c t e d O b s t a c l e s A r e a T o v , : a r d s L o n g i t u d i n a l D i r e c t i o n F o r D i f f e r e n t S e a C o n d i t i o n V a l u e s 1 0 4 1 0 5 1 0 5 1 0 6 1 0 6 1 0 7 1 0 7 6 . 4 8 E r r o r t o w a r d s L o n g i t u d i n a l D i r e c t i o n f o r D i f f e r e n t S e a C o n d i t i o n V a l u e s 1 0 8 6 . 4 9 E r r o r t o w a r d s L a t e r a l D i r e c t i o n f o r d i f f e r e n t S e a c o n d i t i o n v a l u e s 1 0 8 6 . 5 0 U p p e r a n d L o v , · e r B o u n d a r i e s o f P r e d i c t e d O b s t a c l e s A r e a t o w a r d s L o n g i t u d i n a l D i r e c t i o n f r o m C o n v e n t i o n a l a n d N o v e l M e t h o d s 6 ) 1 U p p e r a n d L o w e r B o u n d a r i e s o f P r e d i c t e d O b s t a c l e s A r e a t o w a r d s L a t e r a l D i r e c t i o n f r o m C o n v e n t i o n a l a n d N o v e l M e t h o d s 1 0 9 1 1 0 6 . 5 2 E r r o r T o w a r d s L o n g i t u d i n a l D i r e c t i o n f o r C o n v e n t i o n a l a n d N o v e l M e t h o d s 1 1 0 X 6 . 5 3 E r r o r t o w a r d s L a t e r a l D i r e c t i o n f o r C o n v e n t i o n a l a n d N o v e l M e t h o d s 1 1 1 6 . 5 4 W i d t h o f P r e d i c t e d O b s t a c l e A r e a t o w a r d s L o n g i t u d i n a l d i r e c t i o n f r o m C o n v e n t i o n a l a n d N o v e l M e t h o d s 1 1 1 6 . 5 5 P r e d i c t e d D i s t a n c e r e d u c t i o n f r o m N o v e l M e t h o d 1 1 2 X l L i s t o f T a b l e s N o . D e s c r i p t i o n P a g e 2 . 1 R F C h a n n e l S e t t i n g C o m m a n d s 2 0 2 . 2 C e n t e r s o f G a u s s i a n M e m b e r s h i p F u n c t i o n s 3 1 3 . 1 R u l e T a b l e f o r F u z z y P D C o n t r o l l e r - 4 0 X l l u s v U G V D M v f N G P S I N U P F M Y O M D N C G R N N A C R O N Y M S U n m a n n e d S u r f a c e V e h i c l e s U n m a n n e d G r o u n d V e h i c l e s D i s t r i b u t e d A r c h i t e c t u r e f o r M o b i l e N a v i g a t i o n G l o b a l P o s i t i o n S y s t e m I n e r t i a l N a v i g a t i o n U n i t P o t e n t i a l f i e l d m e t h o c i V e l o c i t y O b s t a c l e m e t h o d D i g i t a l N a u t i c a l C h a r t G e n e r a l i z e d R e g r e s s i o n N e u r a l N e t w o r k X l l l 1 . 1 A p p l i c a t i o n s o f U n m a n n e d S u r f a c e V e h i c l e s ( U S V ) C h a p t e r 1 I n t r o d u c t i o n U S V c a n b e i n t e g r a t e d f o r r e m o t e l y c o n t r o l l e d c o m b a t s y s t e m i d e a l l y s u i t e d t o m e e t f o r c e p r o t e c t i o n r e q u i r e m e n t s i n a l l m a r i t i m e s e t t i n g s . B y p r o v i d i n g l o n g r a n g e s t a n d - o f f s u r v e i l l a n c e , i d e n t i f i c a t i o n a n d e n g a g e m e n t c a p a b i l i t y , U S V c a n b e q u i c k l y d e p l o y e d t o d e f e n d h i g h v a l u e a s s e t s i n c l u d i n g n a v a l v e s s e l s , p o r t o p e r a t i o n s , o i l r i g s a n d c o a s t a l p o w e r p l a n t s . P r o t e c t o r i s a n a m e o f a n U S V d e v e l o p e d b y I s r a e l ' s R a f a e l A r m a m e n t D e v e l o p m e n t A u t h o r i t y i n r e s p o n s e t o e m e r g i n g t e r r o r i s t t h r e a t s a g a i n s t m a r i t i m e a s s e t s [ 2 7 ] . T h a t U S V i s s t e a l t h y , h i g h l y a u t o n o m o u s a n d c a n o p e r a t e w i t h g e n e r a l g u i d a n c e f r o m a c o m m a n d e r i n p o r t , h a r b o r a n d c o a s t a l w a t e r w a y s i n a v a r i e t y o f r o l e s , t h a n k s t o t h e p l u g - a n d - p l a y d e s i g n o f i t s v a r i o u s m i s s i o n m o d u l e s , s u c h a s f o r c e p r o t e c t i o n , a n t i - t e r r o r , s u r v e i l l a n c e a n d r e c o n n a i s s a n c e , m i n e w a r f a r e a n d e l e c t r o n i c w a r f a r e . W i t h i n t e g r a t e d n a v i g a t i o n a l s e n s o r s i n c l u d i n g G P S , n a v i g a t i o n r a d a r a n d v i d e o c a m e r a s , t h e U S V c a n c o n d u c t h a r b o r s u r v e i l l a n c e e v e n i n b u s y w a t e r w a y s . H i g h l y a u t o n o m o u s a n d r e m o t e l y c o n t r o l l e d , U S V c a n s u c c e s s f u l l y m o n i t o r w a t e r w a y s w i t h g e n e r a l g u i d a n c e f r o m a c o m m a n d e r a n d o p e r a t o r a t s e a o r f r o m s h o r e - n o m a t t e r h o w h a z a r d o u s t h e c o n d i t i o n [ 3 8 ] . T h e U S V h a v i n g a n o n - m o u n t c a m e r a a l l o w i n g f o r d a y a n d n i g h t o p e r a t i o n a n d h a s a f o r w a r d - l o o k i n g i n f r a r e d l a s e r r a n g e f i n d e r c a p a b i l i t y t o d e t e c t a n d t r a c k t a r g e t s i n t h e n e a r v i c i n i t y . T h e B o a t C o n t r o l u n i t ' s n a v i g a t i o n s e n s o r s a r e u s e d t o o b t a i n l o c a t i o n , s p e e d , h e a d i n g a n d c o u r s e d a t a . 1 . 2 O b s t a c l e A v o i d a n c e o f U n m a n n e d G r o u n d V e h i c l e s A n i n t e l l i g e n t v e h i c u l a r s y s t e m w h i c h i s a s u b t o p i c t h a t c o m e s u n d e r " I n t e l l i g e n t a u t o n o m o u s s y s t e m s " i s a n i m p o r t a n t r e s e a r c h t o p i c t o d a y , d u e t o i t s i m p o r t a n c e i n t h e f i e l d o f " A u t o n o m o u s s u r f a c e v e h i c l e s a n d i n t e l l i g e n t t r a n s p o r t a t i o n s y s t e m s " . T h e s e - 1 - s y s t e m s c a n b e c l a s s i f i e d a c c o r d i n g t o t h e t e c h n o l o g y b a s e d o n t h e m a n d t h e i r m e t h o d o f i m p l e m e n t a t i o n . T h i s l i e s f r o m b a s i c v e h i c u l a r m a n a g e m e n t s y s t e m s s u c h a s t r a f f i c l i g h t i n g s y s t e m s , c o n t a i n e r m a n a g e m e n t s y s t e m s a n d s i m p l e n a v i g a t i o n s y s t e m s t o m o r e a d v a n c e d s y s t e m s s u c h a s t h e s y s t e m s g e t t i n g f e e d b a c k f r o m t h e o t h e r c o m p a t i b l e e x t e r n a l s y s t e m s , a n d e x t e r n a l s o u r c e s s u c h a s l i v e f e e d b a c k , w h e t h e r i n f o r m a t i o n a n d s o o n . P r e d i c t i v e t e c h n i q u e s h a v e b e e n d e v e l o p e d t o i m p l e m e n t b e t t e r i n f e r e n c e s y s t e m s . T h e s e m e t h o d s a l l o w s o m e a d v a n c e d m o d e l i n g a n d c o m p a r i s o n w i t h h i s t o r i c a l b a s e l i n e d a t a a n d r e a l - t i m e d a t a h e n c e t o p r o v i d e a n i n t e l l i g e n t i n f e r e n c e s y s t e m w h i c h c a n d e d u c t t h e b e s t o p t i o n a t a t i m e t o c o n t r o l t h e s y s t e m [ 3 1 ] . C o l l i s i o n a v o i d a n c e t e c h n i q u e s s h o u l d b e i m p l e m e n t e d w i t h i n t h e p l a t f o r m o r w i t h i n a n e x t e r n a l s y s t e m a n d a p r o p e r c o m m u n i c a t i o n s c h e m e s h o u l d b e m a i n t a i n e d i n o r d e r t o p r e v e n t a n y l i f e o r m a t e r i a l h a z a r d s , f o r t h e s e i n t e l l i g e n t v e h i c u l a r s y s t e m s . I n m o s t o f t h e t i m e s t h e s e i n t e r p l a t f o r m c o m m u n i c a t i o n s c h e m e f o r s h o r t r a n g e s ( l e s s t h a n 4 0 0 m e t e r s ) i s a c c o m p l i s h e d b y u s i n g I E E E 8 0 2 . 1 1 p r o t o c o l s o r t h e D e d i c a t e d S h o r t R a n g e C o m m u n i c a t i o n s s t a n d a r d b e i n g p r o m o t e d b y t h e I n t e l l i g e n t T r a n s p o r t a t i o n S o c i e t y o f A m e r i c a a n d t h e U n i t e d S t a t e s D e p a r t m e n t o f T r a n s p o r t a t i o n [ 3 1 ] . I n t h e c a s e o f l o n g r a n g e c o m m u n i c a t i o n s c h e m e s , t h i s i s a c c o m p l i s h e d b y u s i n g i n f r a s t r u c t u r e n e t w o r k s s u c h a s W i M A X ( I E E E 8 0 2 . 1 6 ) o r G l o b a l S y s t e m f o r M o b i l e C o m m u n i c a t i o n s ( G S M ) . O b s t a c l e a v o i d i n g a l g o r i t h m s p l a y m a j o r r o l e i n t h e o v e r a l l p r o c e s s o f n a v i g a t i o n . W a y p o i n t n a v i g a t i o n w i t h o u t o b s t a c l e a v o i d a n c e i s g i v e n o n l y l i m i t e d c a p a b i l i t i e s t o t h e U S V s i n a r e a l - w o r l d m i s s i o n . I n o r d e r t o p r o v i d e m o r e f u n c t i o n a l i t y a n d r e d u c e t h e r e l i a n c e o n o p e r a t o r o v e r s i g h t , a r o b u s t o b s t a c l e a v o i d a n c e c a p a b i l i t y m u s t b e a d d e d . M o r e a d v a n c e d b e h a v i o r s c a n b e a d d e d , s u c h a s a u t o n o m o u s r e c o v e r y i n t h e c a s e o f l o s t c o m m u n i c a t i o n s , t a r g e t t r a c k i n g a n d i n t e r c e p t i o n , e t c . , a f t e r a d d i n g a o b s t a c l e a v o i d a n c e c o n t r o l l e r w i t h a l g o r i t h m s . I n t h i s r e s e a r c h , a t y p i c a l i n t e l l i g e n t v e h i c l e p r o t o t y p e w a s i m p l e m e n t e d a n d t e s t e d u n d e r l a b o r a t o r y c o n d i t i o n s f o r i t s m o b i l i t y , c o n t r o l l a b i l i t y a n d c o m m u n i c a t i o n c a p a b i l i t i e s b e t w e e n t h e v e h i c l e a n d a p e r s o n a l c o m p u t e r . T h e n a n o t h e r i d e n t i c a l p r o t o t y p e w a s i m p l e m e n t e d t o t e s t , t h e c o m m u n i c a t i o n c a p a b i l i t i e s b e t w e e n t h e t v v o p r o t o t y p e s ( i n t e r v e h i c u l a r c o m m u n i c a t i o n c a p a b i l i t i e s ) a n d c o m m u n i c a t i o n b e t w e e n - 2 - t h e v e h i c l e s a n d a p e r s o n a l c o m p u t e r . A d e a d r e c k o n i n g a l g o r i t h m i s u s e d a s t h e t r a c k i n g a l g o r i t h m f o r t h e v e h i c l e s . T h e n o v e l i n t e r a c t i v e c o n t r o l p a r a d i g m s i n h e r e h a v e b e e n t e s t i f i e d a s e f f e c t i v e s o l u t i o n s f o r r e l i a b l e c o l l i s i o n a v o i d a n c e o f a u t o n o m o u s v e h i c u l a r s y s t e m s v i a c o m p u t e r s i m u l a t i o n s w i l l b e e x p e r i m e n t a l l y v a l i d a t e d . T h e s o - c a l l e d i n t e r a c t i v e c o n t r o l l e r n e g o t i a t e s c o l l i s i o n s c e n a r i o s b e t w e e n t w o v e h i c u l a r s y s t e m s l e a d i n g t o c o o p e r a t i v e m a n e u v e r s . T h e k e y - p o i n t i s t h a t i n o r d e r t o a v o i d a p r o b a b l e c o l l i s i o n s i t u a t i o n , b o t h t h e v e h i c u l a r s y s t e m s i n t e r a c t i v e l y c a r r y o u t m a n e u v e r s . T h e h i e r a r c h i c a l d i f f e r e n t i a t i o n o f t h e p a r t i c i p a t o r y v e h i c u l a r s u b s y s t e m s i s d o n e b y u s i n g a m a t e r - s l a v e c o n c e p t . I n t h e e x p e r i m e n t a l v a l i d a t i o n u s i n g t h e p r o t o t y p e s , t h e a d v a n c e d c o l l i s i o n a v o i d a n c e a l g o r i t h m s a r e i m p l e m e n t e d i n t h e p e r s o n a l c o m p u t e r d u e t o t h e l i m i t a t i o n s o f m e m o r y a n d c o m p u t a t i o n a l s p e e d i n t h e p r o t o t y p e . R S 2 3 2 s e r i a l c o m m u n i c a t i o n s t a n d a r d i s u s e d f o r t h e w i r e l e s s c o m m u n i c a t i o n b e t w e e n t h r e e n o d e s ( t h e c o m p u t e r a n d t w o p r o t o t y p e s ) b a s e d o n a s u i t a b l e c o m m u n i c a t i o n p r o t o c o l t h a t i s d e v e l o p e d s p e c i a l l y f o r t h i s s c e n a r i o [ 3 2 ] . T h e d e v e l o p e d p r o t o t y p e s w e r e f u l l y e q u i p p e d w i t h r e q u i r e d h a r d w a r e s u c h a s s e n s o r s a n d a c t u a t o r s . T h i s h a s t h e m o t i o n c o n t r o l a n d p o s i t i o n t r a c k i n g a b i l i t i e s . T h e c o m m u n i c a t i o n s c h e m e w a s i m p l e m e n t e d v i a t h e R F b r o a d c a s t i n g . T h e o b s t a c l e d e t e c t i o n w a s d o n e b y u l t r a s o n i c s e n s o r s . D e t e c t i o n o f t h e o t h e r p r o t o t y p e ( a s a n o b s t a c l e , i n c o l l i s i o n c o n d i t i o n s ) w a s d o n e b y t h e r e l a t i v e d i s t a n c e , r e l a t i v e a n g l e a n d r e l a t i v e v e l o c i t y i n f o r m a t i o n , t h a t a r e e x c h a n g e d b e t w e e n v e h i c l e s v i a a n i n t e r - v e h i c u l a r c o m m u n i c a t i o n s c h e m e t h a t w a s i m p l e m e n t e d . M i c r o c o n t r o l l e r b o a r d w a s p r o g r a m m e d i n o r d e r t o a c t a s t h e c e n t r a l i n f o r m a t i o n p r o c e s s i n g u n i t [ 8 ] . 1 . 3 A p p l i n g G r o u n d V e h i c l e T e c h n o l o g i e s f o r S u r f a c e V c h i c l e s C u r r e n t u n m a n n e d v e h i c l e s a d h e r e t o d i f f e r e n t l e v e l s o f a u t o n o m y a s d e f i n e d b y e x i s t i n g t e c h n o l o g y l i m i t a t i o n s a n d u s e d s e n s o r s . I m p o r t a n t o p e r a t i o n a l c h a r a c t e r i s t i c s r e l a t e d t o u n m a n n e d v e h i c l e f u n c t i o n a l i t y ( a e r i a l , s u r f a c e a n d g r o u n d ) , i n c l u d e p e r c e p t i o n , i n t e l l i g e n c e a n d a c t i o n . H e r e , t h e a c q u i r i n g a n d u s e k n o w l e d g e a b o u t t h e e n v i r o n m e n t a n d i t s e l f i s c a l l e d t h e p e r c e p t i o n [ 1 7 ] . T h i s i s d o n e b y t a k i n g m e a s u r e m e n t s u s i n g v a r i o u s s e n s i n g d e v i c e s a n d t h e n e x t r a c t i n g m e a n i n g f u l " ) - _ ) - i n f o r m a t i o n t h a t s h o u l d b e u s e d i n a l l l a t e r t a s k s s u c h a s l o c a l i z a t i o n , p l a n n i n g , c o l l i s i o n f r e e m o t i o n c o n t r o l . T h e m e a n i n g o f t h e I n t e l l i g e n c e r e l e v a n t t o u n m a n n e d v e h i c l e s , i s o p e r a t i n g f o r a c o n s i d e r a b l e t i m e p e r i o d w i t h o u t h u m a n i n t e r v e n t i o n . T h i s i s a s s o c i a t e d w i t h t h e l e a r n i n g a n d i n f e r e n c e c a p a b i l i t i e s , w h i c h o f t h e v e h i c l e s h o u l d b e a b l e t o a d a p t t o t h e e n v i r o n m e n t . T h e a c t i o n i s t h e w a y t h a t u n m a n n e d v e h i c l e s h o u l d t r a v e l f r o m o n e p o i n t t o a n o t h e r . T h e v e h i c l e s h o u l d u t i l i z e p r e d e f i n e d a n d a c q u i r e d k n o w l e d g e t o m o v e i n d y n a m i c e n v i r o n m e n t s w i t h o u t i n v o l v i n g h u m a n s i n t h e n a v i g a t i o n l o o p . S o , t h e a l g o r l . t h m s a n d t e c h n o l o g i e s d e v e l o p e d f o r u n m a n n e d g r o u n d v e h i c l e s c a n a p p l y f o r t h e s u r f a c e v e h i c l e s a s w e l l d u e t o t h o s e s i m i l a r i t i e s . 1 . 4 P o t e n t i a l F i e l d M e t h o d f o r O b s t a c l e A v o i d a n c e D u r i n g t h e p a s t f e w y e a r s , p o t e n t i a l f i e l d m e t h o d s ( P F M ) f o r o b s t a c l e a v o i d a n c e h a v e g a i n e d i n c r e a s e d p o p u l a r i t y a m o n g r e s e a r c h e r s i n t h e f i e l d o f r o b o t s a n d m o b i l e r o b o t s [ 3 4 ] . T h e i d e a o f i m a g i n a r y f o r c e s a c t i n g o n a r o b o t h a s b e e n s u g g e s t e d b y K h a t i b i n 1 9 8 5 [ 1 8 ] . I n t h e s e a p p r o a c h e s o b s t a c l e s e x e r t r e p u l s i v e f o r c e s o n t o t h e r o b o t , w h i l e t h e t a r g e t a p p l i e s a n a t t r a c t i v e f o r c e t o t h e r o b o t [ 4 1 ] . T h e s u m o f a l l f o r c e s , t h e r e s u l t a n t f o r c e , d e t e r m i n e s t h e s u b s e q u e n t d i r e c t i o n a n d s p e e d o f t r a v e l . O n e o f t h e r e a s o n s f o r t h e p o p u l a r i t y o f t h i s m e t h o d i s i t s s i m p l i c i t y . S i m p l e P F M s c a n b e i m p l e m e n t e d q u i c k l y a n d i n i t i a l l y p r o v i d e a c c e p t a b l e r e s u l t s w i t h o u t r e q u i r i n g m a n y r e f i n e m e n t s . P F M c a n n o t b e a p p l i e d f o r d y n a m i c o b s t a c l e s d i r e c t l y . A n o n g o i n g u n p u b l i s h e d r e s e a r c h w o r k i s t h e r e t o a p p l y p o t e n t i a l f i e l d m e t h o d f o r d y n a m i c o b s t a c l e a v o i d a n c e . T h e v e l o c i t y d i p o l e i s u s e d f o r t h a t . T h e v e l o c i t y d i p o l e f i e l d i s p r e s e n t e d f o r r e a l - t i m e c o l l i s i o n a v o i d a n c e o f m o b i l e r o b o t s . T h e d i r e c t i o n o f m o t i o n o f t h e o b s t a c l e i s u s e d a s t h e a x i s o f t h e d i p o l e f i e l d , a n d t h e s p e e d o f t h e o b s t a c l e i s u s e d t o p r o p o r t i o n a l l y s t r e n g t h e n t h e d i p o l e f i e l d . T h e e l l i p t i c a l f i e l d l i n e s o f t h e d i p o l e f i e l d a r e u s e f u l t o s k i l l f u l l y g u i d e t h e r o b o t a r o u n d o b s t a c l e s , q u i t e s i m i l a r t o t h e w a y h u m a n s a v o i d m o v i n g o b s t a c l e s . T h a t s y s t e m s e e m s t o h a v e t h e c a p a b i l i t y o f a n e w r e a l - t i m e c o l l i s i o n a v o i d a n c e s t r a t e g y a n d i t w i l l o v e r c o m e t h e w e a k n e s s e s i n t h e c o n v e n t i o n a l p o t e n t i a l f i e l d m e t h o d . - 4 - T h e S o f t w a r e d e v e l o p e d b y L e e F e n g [ 2 0 ] f o r p o t e n t i a l f i e l d a p p l i c a t i o n s w a s u t i l i z e d i n t h i s r e s e a r c h t o c o m p a r e t h e p r o p o s e d m e t h o d s w i t h t h e p o t e n t i a l f i e l d m e t h o d . 1 . 5 M o r p h i n a l g o r i t h m f o r p a t h p l a n n i n g M o r p h i n i s a n a r e a - b a s e d a l g o r i t h m a n d i t a n a l y z e s o b s t a c l e s i n t h e a r e a w h i c h c a n d i s t u r b i t s n a v i g a t i o n . I t p r o j e c t s a l l t h e p o s s i b l e p a t h s t o t h e f r o n t i n i t i a l l y . T o s e l e c t f r o m a m o n g m u l t i p l e p a t h s , p a t h e v a l u a t i o n s a r e a s s i g n e d t o a l l p o s s i b l e c a n d i d a t e p a t h s a c c o r d i n g t o h o w e f f e c t i v e l y e a c h p a t h w o u l d d r i v e t h e r o v e r t o w a r d i t s g o a l p o i n t . T h e p a t h t h a t w o u l d l e a d d i r e c t l y t o w a r d t h e g o a l w i t h l e s s o b s t a c l e s i s g i v e n t h e h i g h e s t e v a l u a t i o n ; o t h e r p a t h s a r e a s s i g n e d l e s s e r v a l u e s a c c o r d i n g a p r e d e f i n e f u n c t i o n [ 2 1 ] . T h e s e e v a l u a t i o n s a r e t h e n c o m b i n e d w i t h t h e u s e r ' s p r e f e r e n c e s t o d e t e r m i n e t h e o v e r a l l b e s t c o m m a n d , w h i c h i s t h e n s e n t t o t h e r o v e r t o b e e x e c u t e d . T h e c y c l e t i m e f o r t h i s p r o c e s s i s a b o u t 1 - 2 s e c o n d s , w i t h t h e s t e r e o c o m p u t a t i o n s t a k i n g u p a b o u t 7 5 % o f t h e t o t a l t i m e . T h i s a l g o r i t h m i s n o v e l b u t t h e a p p r o a c h h a s a l o n g h i s t o r y o f a p p l i c a t i o n s i n r e a l - w o r l d s y s t e m s ( i n c l u d i n g t h e M a r s R o v e r s ) a n d h a s i t s l i n e a g e b a c k t o t h e C a r n e g i e M e l l o n U n i v e r s i t y M o r p h i n a l g o r i t h m a n d D i s t r i b u t e d A r c h i t e c t u r e f o r M o b i l e N a v i g a t i o n ( D A M N ) [ 2 9 ] . I n t h e p r a c t i c a l a p p l i c a t i o n s , t h e p r o b l e m s w h i c h a r e m a i n l y a t t r i b u t a b l e t o a n a b u n d a n c e o f n o i s e , p a r t i c u l a r l y i n t h e s t e r e o - p r o d u c e d o b s t a c l e m a p s a n d G l o b a l P o s i t i o n S y s t e m s ( G P S ) a r e j a m m e d o r t h e I n e r t i a l N a v i g a t i o n U n i t ( I N U ) d r i f t s c a n c r e a t e p r o b l e m s t o t h e p r e v i o u s a l g o r i t h m s s i n c e t h e y a r e h e a v i l y d e p e n d o n O b s t a c l e p o s i t i o n s . S o a n o t h e r a p p r o a c h h a d t o b e c h o s e n t o d e v e l o p n e w a l g o r i t h m s t o o v e r c o m e a b o v e p a r t i c u l a r s f o r s m o o t h a n d s a f e n a v i g a t i o n o f t h e U S V . A n a p p l y i n g g r o u n d v e h i c l e s a l g o r i t h m f o r s u r f a c e v e h i c l e s a n d P F M w e r e d o n e p r e v i o u s l y . S o M o r p h i n a p p r o a c h w a s c h o s e n d u e t o i t ' s p r o v e n c a p a b i l i t i e s f o r g r o u n d r o b o t s l i k e f a m o u s M a r s R o v e r s a n d L u n a r R o v e r s [ 2 1 ] . T h e a l g o r i t h m i s n o v e L b u t t h e a p p r o a c h h a s a l o n g h i s t o r y o f a p p l i c a t i o n s i n r e a l - w o r l d s y s t e m s a n d h a s i t s l i n e a g e b a c k t o t h e D A M N o f C a r n e g i e M e l l o n U n i v e r s i t y . - 5 - 1 . 6 D e f i n i n g s a f e t y d i s t a n c e f o r p a t h p l a n n i n g S a f e t y d i s t a n c e s n e e d t o b e d e f i n e d f o r d i f f e r e n t o b s t a c l e s c o n s i d e r i n g t h e i r g e o m e t r i c a l s h a p e s . T h e y m i g h t c h a n g e a r o u n d o b s t a c l e s c e n t e r o f g r a v i t y . T h a t w a s n o t c o n s i d e r e d i n t h i s s t u d y . T h e r e f o r e c i r c u l a r s a f e t y d i s t a n c e i s d e f i n e d f o r t h e s a f e t y o f t h e U S V . T h e w a y s o f d e f i n i n g s a f e t y d i s t a n c e i s s h o w n i n F i g u r e 1 . 1 . O b s t a c l e S a f e t y Mprgi~ ~~, I I I ofsta~l• I F i g u r e 1 . 1 - S a f e t y D i s t a n c e f o r O b s t a c l e s S a f e t y M a r g i n :~ F o r a n e x a m p l e t h e l e n g t h o f t h e U S V n a m e d P r o t e c t o r u s e d b y I I s r a e l i S e a C o r p s i s 9 m e t e r s . 1 . 7 D y n a m i c o b s t a c l e a v o i d a n c e A l o t o f r e s e a r c h e s h a s b e e n c a r r y i n g o u t t o a v o i d d y n a m i c o b j e c t s a n d u n a b l e t o f i n d a b e s t s o l u t i o n f o r t h a t a l t h o u g h c o m p a r a t i v e l y g o o d a p p r o a c h e s h a s b e e n p r e s e n t e d . C a n n y a n d R e i f [ 1 3 ] s h o w e d t h a t m o t i o n p l a n n i n g f o r a p o i n t i n a p l a n e w i t h b o u n d e d v e l o c i t y i n t h e p r e s e n c e o f m o v i n g o b s t a c l e s i s N o n - d e t e r m i n i s t i c P o l y n o m i a l - t i m e h a r d . A g g a r w a l a n d F u j i m u r a [ 2 2 ] s h o w t h a t a m o r e o p t i m a l s o l u t i o n c a n b e f o u n d b y a d d i n g a t h i r d d i m e n s i o n o f t i m e a n d p l o t t i n g t h e l o c a t i o n o f t h e m o v i n g o b s t a c l e s a l o n g t h a t t h r e e - d i m e n s i o n a l ( 3 D ) s t r u c t u r e . F u j i m u r a a n d S a m e t [ 1 5 ] p r o v i d e y e t a n o t h e r s o l u t i o n , b u t e v e n t h e y a d m i t t h e s o l u t i o n i s b e s t w i t h f e w m o v i n g o b s t a c l e s . - 6 - A s o l u t i o n f o r d y n a m i c o b s t a c l e a v o i d a n c e i s p r e s e n t e d b y S p a c e a n d N a v a l W a r f a r e S y s t e m s C e n t e r , S a n D i e g o f o r t h e r e q u i r e m e n t o f r o b u s t U S V o p e r a t i o n i n a r e a l w o r l d e n v i r o n m e n t , p r i m a r i l y f o c u s i n g o n a u t o n o m o u s n a v i g a t i o n , o b s t a c l e a v o i d a n c e , a n d p a t h p l a n n i n g . V e l o c i t y O b s t a c l e m e t h o d ( V O M ) i s o n e o f t h e g o o d m e t h o d s f o r d y n a m i c o b s t a c l e a v o i d a n c e i s u t i l i z e d m a i n l y f o r t h o s e d e v e l o p m e n t s . T o a v o i d m o v i n g o b s t a c l e s a n d m a i n t a i n t h e d e s i r e d p a t h s e t b y t h e u s e r , t h e s a f e v e l o c i t y r a n g e s u s i n g t h e V e l o c i t y O b s t a c l e m e t h o d [ 2 5 ] h a v e t o b e d e t e r m i n e d b y t h e c o n t r o l l e r . T h i s a l g o r i t h m t r a n s f o r m s a m o v i n g o b s t a c l e i n t o a s t a t i o n a r y o n e b y c o n s i d e r i n g t h e r e l a t i v e v e l o c i t y a n d t r a j e c t o r y o f t h e U S V w i t h r e s p e c t t o t h e o b s t a c l e . A f t e r p r o d u c i n g a c o l l i s i o n a r e a c a l l e d t h e V e l o c i t y O b s t a c l e , d e f i n e d u s i n g t h e r e l a t i v e v e l o c i t y v e c t o r , t h e a l g o r i t h m r e t u r n s a s e t o f U S V v e l o c i t y v e c t o r s w h i c h a r e g u a r a n t y t h e c o l l i s i o n a v o i d a n c e . T h i s t r a n s f o r m a t i o n a n d c o l l i s i o n a r e a d e t e c t i o n r e d u c e s t h e c o m p l e x i t y o f t h e p a t h p l m m i n g a m o n g m o v i n g o b s t a c l e s w i t h r e s p e c t t o t i m e . T h i s i s u s e d a s a f i r s t p a s s t o a v o i d m o v i n g o b s t a c l e s . I n t h e c a s e t h a t c h a n g i n g v e l o c i t y t h e c o n t r o l l e r h a s t o c h a n g e t h e p a t h b y c r e a t i n g p r o j e c t e d o b s t a c l e a r e a s f o r e a c h o b s t a c l e a n d d e t e r m i n i n g a s a f e a l t e r n a t i v e r o u t e . 1 . 8 S e n s o r c o n s i d e r a t i o n s A s w i t h a n y u n m a n n e d v e h i c l e a t t e m p t i n g t o n a v i g a t e i n a c o m p l e x e n v i r o n m e n t , g o o d s e n s o r d a t a i s c r i t i c a l , a n d g e t t i n g g o o d d a t a i s o f t e n t h e m o s t d i f f i c u l t p a r t o f t h e p r o j e c t o f d e v e l o p i n g a U S V . T h e o c e a n i c e n v i r o n m e n t p o s e s m a n y c h a l l e n g e s i n c l u d i n g w a v e s , s p r a y , a n d a d i s o r d e r e d o b s t a c l e s e t t i n g . T h e r e a r e s o m e a d v a n t a g e s t o t h e m a r i n e e n v i r o n m e n t i n c l u d i n g w e l l c h a r t e d o p e r a t i n g a r e a s , a b s e n c e o f n e g a t i v e o b s t a c l e s ( h o l e s o r c l i f f s ) , a m o s t l y p l a n a r s u r f a c e ( e x c e p t f o r t h e w a v e s ) , n o v e g e t a t i o n , e t c . I t ' s i m p o r t a n t t h a t t h e s e n s o r s a r e s e l e c t e d t o m a k e t h e m o s t o f t h e e n v i r o n m e n t a l a d v a n t a g e s a n d t o p r o v i d e t h e b e s t d a t a p o s s i b l e i n t h e c h a l l e n g i n g t e r r i t o r i e s . T h e s e n s o r s f o r t h e o b s t a c l e a v o i d a n c e n e e d t o p r o v i d e d a t a a b o u t o b s t a c l e s i n t h e f a r - f i e l d ( e . g . , > 2 0 0 - 3 0 0 y a r d s ) a n d p r o v i d e s t a t e i n f o r m a t i o n ( p o s i t i o n , c o u r s e , a n d s p e e d ) f o r t h e m o v i n g o b s t a c l e s . - 7 - H i g h - r e s o l u t i o n a n d a t a m u c h h i g h e r r a t e d a t a i s n e e d e d a b o u t t h e o b s t a c l e s i n c l o s e p r o x i m i t y t o t h e U S V ( e . g . , < 2 0 0 - 3 0 0 y a r d s ) . S o m e o f t h e s e s e n s o r s a r e t y p i c a l l y n o t f o u n d i n t h e c o m m e r c i a l m a r i n e i n d u s t r y b u t m a n y h a v e b e e n u s e d e x t e n s i v e l y i n U G V p r o g r a m s . 1 . 8 . 1 R a d a r C o n t a c t s S t a n d a r d m a r i n e r a d a r ( F u r u n o ) w i t h a t h i r d - p a r t y P C c o n t r o l l e r c a n b e u s e d f o r U S V s . T h e c o n t r o l l e r , d e v e l o p e d b y X e n e x I n n o v a t i o n s L t d . , p r o v i d e s a d i g i t a l n e t w o r k e d i n t e r f a c e f o r t h e r a d a r . T h e X e n e x s y s t e m p r o v i d e s a n A P I t o a c c e s s t h e r a d a r d a t a a n d c o n t r o l s a s w e l l a s a n A d v a n c e d R a d a r P l o t t i n g A i d ( A R P A ) S o f t w a r e D e v e l o p m e n t K i t , w h i c h p r o v i d e s a l g o r i t h m s t o a u t o m a t i c a l l y a c q u i r e a n d t r a c k u p t o 1 0 0 c o n t a c t s [ 2 3 ] . O n e s i g n i f i c a n t p r o b l e m w i t h t h e r a d a r i s t h a t i t t e n d s t o c l a s s i f y n o i s e f r o m t h e s h o r e l i n e r e t u r n a s c o n t a c t s w h i c h a r e o f t e n s h o w n t o b e m o v i n g a t a s i g n i f i c a n t v e l o c i t y a n d i n t h e d i r e c t i o n o f t h e U S V . T h e s e f a l s e c o n t a c t s a r e o b v i o u s l y d e t r i m e n t a l t o t h e s u c c e s s f u l o p e r a t i o n o f t h e p a t h p l a n n e r . T o m i t i g a t e t h i s p r o b l e m , t h e o n - b o a r d n a u t i c a l c h a r t s e r v e r c a n b e u s e d t o c a l c u l a t e d p o l y g o n s t h a t f o l l o w t h e s h o r e l i n e a n d s t r u c t u r e s a l o n g t h e s h o r e l i n e . T h e r a d a r c o n t a c t s a r e c o m p a r e d w i t h t h e s e p o l y g o n s a n d t h o s e t h a t f a l l i n s i d e a p o l y g o n a r e r e j e c t e d a n d d e l e t e d f r o m t h e r a d a r ' s l i s t [ 2 8 ] . L a b o r a t o r y ( J P L ) f o r a n u m b e r o f y e a r s t o t r a n s i t i o n t e c h n o l o g y t o i t s U G V p r o g r a m s . T h a t w o r k i s n o w b e i n g e x t e n d e d t o t h e U S V d o m a i n [ 1 4 ] . T h e s t e r e o v i s i o n s y s t e m p r o v i d e s h i g h - r e s o l u t i o n 3 D d a t a a b o u t t h e n e a r - f i e l d e n v i r o n m e n t , w h i c h c a n b e c o n v e r t e d i n t o a 2 D o b s t a c l e m a p a n d f u s e d w i t h d a t a f r o m t h e o t h e r s e n s o r s . S t e r e o v i s i o n i s c a p a b l e o f p r o v i d i n g v e r y h i g h q u a l i t y 3 D d a t a b u t a l s o h a s t h e d i s a d v a n t a g e o f r e q u i r i n g p r e c i s e c a l i b r a t i o n e v e r y t i m e t h e c a m e r a s a r e m o u n t e d . T h e r e i s a l s o t h e r i s k t h a t t h e c a m e r a s m a y m o v e r e l a t i v e t o o n e a n o t h e r s l i g h t l y w h i c h w i l l a f f e c t t h e c a l i b r a t i o n a n d r e s u l t i n e r r o n e o u s d a t a . S o t h e m o n o c u l a r v i s i o n v v i t h s o p h i s t i c a t e d a l g o r i t h m s c a n b e u t i l i z e d f o r t h a t a s w e l l . - 8 - . . C h a p t e r 2 O b s t a c l e A v o i d a n c e o f U n m a n n e d G r o u n d V e h i c l e s 2 . 1 S e n s o r s e l e c t i o n f o r p r o t o t y p e s T h e p r o p o s e d v e h i c u l a r p r o t o t y p e h a s u l t r a - s o n i c r a n g e s e n s o r s t o d e t e c t c o l l i s i o n c o n d i t i o n s e n c o u n t e r e d b y t h e p r o t o t y p e , a d i g i t a l c o m p a s s m o d u l e t o o b t a i n t h e h e a d i n g a n g l e , o p t i c a l s h a f t e n c o d e r m o d u l e s t o g e t t h e p o s i t i o n o f t h e p r o t o t y p e , R F t r a n s c e i v e r m o d u l e f o r c o m m u n i c a t i o n , s e r v o d r i v e r s , a n d a m i c r o c o n t r o l l e r t o p r o c e s s t h e i n f o r m a t i o n a n d t o g i v e t h e c o n t r o l s i g n a l s t o t h e a c t u a t o r s . A b l o c k d i a g r a m r e p r e s e n t a t i o n o f t h e c o m p l e t e s y s t e m i s g i v e n i n F i g u r e 2 . 1 . T h e m a i n o b j e c t i v e o f t h e d e v e l o p e d p r o t o t y p e s i s t o t e s t , f i n e t u n e , a n d e x p e r i m e n t a l l y v a l i d a t e i n t e l l i g e n t c o l l i s i o n a v o i d a n c e a l g o r i t h m s f o r g r o u n d v e h i c l e s . S o , t h e f e a t u r e s e x p e c t e d i n t h e v e h i c u l a r p l a t f o r m s s h o u l d b e i d e n t i f i e d a t t h e i n i t i a l s t a g e o f t h e p r o j e c t . I n d i v i d u a l p r o t o t y p e s s h o u l d i n c l u d e a s u i t a b l e c o n t r o l l e r w h i c h i s c a p a b l e o f i m p l e m e n t i n g t h e c o l l i s i o n a v o i d a n c e c o m p u t a t i o n a l i n t e l l i g e n c e a l g o r i t h m s . T h e m o t o r s s e l e c t e d n e e d t o b e e a s y t o c o n t r o l . M o s t i m p o r t a n t l y , t h e p r o t o t y p e s s h o u l d b e a b l e t o c o m m u n i c a t e e a c h o t h e r . T h e h e a d i n g a n g l e s o f t h e v e h i c l e s a r e v i t a l i n f o r m a t i o n i n o r d e r t o r e a l i z e s u c c e s s f u l c o l l i s i o n a v o i d a n c e m a n e u v e r s . - 9 - ~··--,;e. ; . r ! ' i ~~ ~ ~ ' . . . : - r , : , : ; , " : : ' _;L~t:+- ~· ~ r - C . • · ! . . : ! . _/~ ~ , h . · · ; . • f • • ~ . . . . ! 1 . > F " . . . . - , : : . . : ~' ' ) f . · · , , . . _ . / l _ , f - ' . - , . . : ; . ~ ~-~ :~~;J·t~'i;~ - - - - - - - - -~~1 ;-~~-tt;,r.:/ . . , , . r . L . : ! . F i g u r e 2 . 1 - B a s i c B l o c k D i a g r a m o f t h e S y s t e m 2 . 2 D e v e l o p m e n t o f p r o t o t y p e s 2 . 2 . 1 D i g i t a l C o n t r o l l e r S e l e c t i o n . . . :;·~~;(;.;. ~-:~ f " : : ~:." ~ ·=~ ~?~~7::~:: D i g i t a l c o n t r o l l e r i s t h e h e a r t o f t h e s y s t e m a n d s h o u l d b e c a r e f u l l y c h o s e n . T h e c o n t r o l l e r s h o u l d h a v e a g o o d m e m o r y c a p a c i t y a n d a v e r y h i g h f r e q u e n c y . M o s t i m p o r t a n t l y , t h e p r o g r a m m i n g o f t h e c o n t r o l l e r s h o u l d b e s t r a i g h t f o r w a r d . I n t h i s a p p l i c a t i o n , t h e o n b o a r d c o n t r o l l e r c o n t r o l s t w o m o t o r s , w h i l e e x e c u t i n g t h e o t h e r i n s t r u c t i o n s . T h e O O P i c - R [ 3 ] w a s s e l e c t e d f o r t h e p r o j e c t . I t p r o v i d e s 1 6 d i g i t a l l / 0 l i n e s i n c l u d i n g p o w e r a n d g r o u n d c o n n e c t i o n . I t s s m a l l e r s i z e s u i t s w e l l f o r p r o t o t y p e d e v e l o p m e n t . - 1 0 - T h e s p e a k e r o n b o a r d O O P i c - R i s u s e f u l i n t h e d e v e l o p m e n t a l s t a g e . T h e s p e a k e r c a n b e u s e d t o t a k e r e q u i r e d o u t p u t s a n d w a r n i n g s i g n a l s a t t h e d e v e l o p m e n t s t a g e . A l l t h e m o d u l e s w e r e t e s t e d s e p a r a t e l y b e f o r e i n t e g r a t i n g . T h e s p e a k e r w a s d e p l o y e d t o c h e c k t h e o u t p u t v a l u e s o f t h o s e m o d u l e s s u c c e s s f u l l y . e c n e t w o r k c o n n e c t o r o f O O P i c - R c a n b e u s e d t o n e t w o r k t w o o r m o r e O O P i c - R s . M u l t i p l e p o w e r l i n e s a r e p r o v i d e d t o u s e d i f f e r e n t p o w e r c o n s u m i n g m o d u l e s ( F i g u r e 2 . 2 ) . T h e j u m p e r s o n b o a r d c a n b e u s e d t o s e t t h e d i f f e r e n t p o w e r o u t p u t s . O O P i c - R c o n t a i n s 3 o n b o a r d L E D s h a v i n g d i f f e r e n t c o l o r s . T h e y a r e i n t e r n a l l y c o m 1 e c t e d t o t h r e e d i f f e r e n t I O l i n e s . T h o s e L E D s a r e u s e d i n t h e d e v e l o p m e n t s t a g e f o r d e b u g g i n g a n d m o n i t o r i n g . T h e R S 2 3 2 s e r i a l p o r t c o n n e c t o r c a n b e u s e d t o c o m m u n i c a t e w i t h P C v i a M A X 2 3 2 c h i p . A s e p a r a t e t h r e e - p i n c o n n e c t o r p r o v i d e d d i r e c t a n d e a s y c o n n e c t i o n f o r t h e L C D d i s p l a y s . T h e g r e e n L E D c o n n e c t e d n e a r t h e p r o g r a m m i n g c o n n e c t o r i s l i t b r i g h t l y w h e n p o w e r s u p p l y i s g o o d . I t i s a g o o d i n d i c a t o r o f s u p p l y b a t t e r y l e v e l . R e s e t b u t t o n o n t h e b o a r d c a n b e u s e d t o r e s e t t h e c o n t r o l l e r w h e n t h a t b e c o m e s s t u c k . O O P i c h a s a n o b j e c t - o r i e n t e d o p e r a t i n g s y s t e m t h a t h a s b e e n p r e - p r o g r a m m e d i n t o a M i c r o c h i p P I C . I t a l s o p r o v i d e s a n o b j e c t - o r i e n t e d l a n g u a g e m o d e l d e s i g n t o i n t e r a c t w i t h t h e e l e c t r i c a l h a r d w a r e c o m p o n e n t s t h a t a r e a t t a c h e d t o t h e P I C . H a r d w a r e i n t e r f a c e i s d e s i g n e d u s i n g p r o g r a m m i n g s o f t w a r e b y c r e a t i n g o b j e c t s a n d s e t t i n g t h e i r p r o p e r t i e s t o d e f i n e t h e i r b e h a v i o r a n d i n t e r a c t i o n w i t h t h e h a r d w a r e . T h e s e o b j e c t s c a n a l s o b e i n t e r c o n n e c t e d t o f o r m a v i r t u a l c i r c u i t a n d t h e n u t i l i z e t h i s h a r d w a r e i n t e r f a c e a n d i t s a s s o c i a t e d v i r t u a l c i r c u i t s b y w r i t i n g a p r o g r a m t h a t c o n t r o l s a n d r e s p o n d s t o h a r d w a r e e v e n t s t h a t o c c u r . - 1 1 - [ O O P i c - R j ! ( . Di~tilal L O l i n e : , w i t h p H I \ ' C r a n d j . ! n m n d c o n n c c t i d t h ( J f ! a n g c d f < J r R C Servo~. T h t nr~l 4 C d ! l h e u n ; l l o g i n . h D t t ; l t < l i 1 . 0 l i n e s . m : l 1 1 [ ! C d t { H D u . J i D C \ h l l < • r s \ \ ' l l h ~ n a m i B r a k e s . l D i g i t ; i l L O l i n e f ( • r c o n t r o l l i n g P L H p u l d i s p t t y s s t t d t a s S i . T i n l L C D & S n w l V i d . . : o { ) Y o \ l ' l : r C i < ' t ' d L F D R S 2 1 2 S . . : n J I f ' t l r l l i H ' p n • g m m m i n ; t d e b u g g i n g a n d n : n H ' t e n m t r o l v i a P C . P , t l m f ' i l o 1 . E t c . I \ l u l t t p l c P • J W e r ( > p t i o n s w i t h u p t o 3 \ ' o l t a p : c R r g u l a t o r , ; a n d j u m p e r s f o r p o w e r s e l e c t i o n . F i g u r e 2 . 2 - O O P i c R M i c r o c o n t r o l l e r b o a r d 2 . 2 . 2 D i g i t a l C o m p a s s M o d u l e S e l e c t i o n P a t h o f t h e v e h i c l e t r a v e l s s h o u l d b e m a p p e d t o a n a l y z e t h e w o r k a b i l i t y o f t h e t h e o r i e s . T h e r e a r e t w o m a j o r o p t i o n s t o m a p t h o s e p a t h s . T h e y a r e G P S m a p p i n g a n d e m p l o y i n g a s u i t a b l e d e a d r e c k o n i n g a l g o r i t h m . G P S m o d u l e i s n e e d e d f o r G P S m a p p i n g . B u t e v e n d i f f e r e n t i a l e r r o r c o r r e c t i n g G P S h a s a n e r r o r a r o u n d 5 m [ 1 6 ] . B u t a s e a c h p r o t o t y p e d e v e l o p e d i s 3 0 c m x 2 0 c m i n s i z e , G P S m a p p i n g i s n o t a n o p t i o n a n d a d e a d r e c k o n i n g a l g o r i t h m i s a d o p t e d f o r p a t h m a p p i n g . A d i g i t a l c o m p a s s a n d a n o p t i c a l e n c o d e r w e r e u s e d i n t h e d e a d r e c k o n i n g a l g o r i t h m . T h e C M P 0 3 d i g i t a l c o m p a s s m o d u l e b y D e v a n t e c h L t d w a s s e l e c t e d f o r t h i s s t u d y ( F i g u r e 2 . 3 ) . E s p e c i a l l y , t o g e t h e r w i t h t h e o p t i c a l e n c o d e r s , t h i s c o m p a s s i s m e a n t t o b e u s e d f o r d e a d r e c k o n i n g p u r p o s e s . T h e C M P 0 3 d i g i t a l c o m p a s s u s e s t h e P h i l i p s K M Z 5 1 m a g n e t i c f i e l d s e n s o r , w h i c h d e t e c t s t h e E a r t h s m a g n e t i c f i e l d . T h e o u t p u t f r o m t h e t w o o f t h e m m o u n t e d a t r i g h t a n g l e s t o e a c h o t h e r i s u s e d t o f i n d t h e h e a d i n g a n g l e . T h e c o m p a s s r e a d i n g s c a n b e o b t a i n e d b o t h f r o m t h e e c c h a n n e l s a s w e l l a s a P W M s i g n a l . T h e r e s o l u t i o n o f t h e c o m p a s s i s 0 . 1 d e g r e e s . T h e c o m p a s s n e e d s t o b e c a l i b r a t e d i n t h e a r e a i t i s b e i n g u s e d . - 1 2 - F i g u r e 2 . 3 - D i g i t a l c o m p a s s m o d u l e I 2 C o b j e c t i n t h e O O P i c m u l t i - l a n g u a g e c o m p i l e r w a s u s e d t o a c c e s s t h e d i g i t a l c o m p a s s . e c n o d e a d d r e s s w a s s e t t o O x C O . L o c a t i o n 1 i n t h e c o m p a s s m o d u l e w a s a c c e s s t o t a k e h e a d i n g a n g l e v a l u e f r o m 8 b i t v a l u e . T h e v a l u e t h a t c a m e f r o m t h e m o d u l e w a s b e t w e e n 0 t o 2 5 5 . T h i s w a s m a p p e d t o t h e 0 - 3 6 0 d e g r e e r a n g e . Compass---~ F i g u r e 2 . 4 - T h e w a y o f m o u n t i n g t h e c o m p a s s t o t h e p r o t o t y p e T h e m o u n t i n g p o s i t i o n o f t h e c o m p a s s w a s v e r y i m p o r t a n t d u e t o i t s h i g h m a g n e t i c f i e l d s e n s i t i v i t y . B e c a u s e t h e D C s e r v o s u s e d t o p o w e r t h e w h e e l s g e n e r a t e m a g n e t i c f i e l d s w h i c h m a y i n t e r f e r e w i t h t h e d i g i t a l c o m p a s s , t h e d i g i t a l c o m p a s s w a s m o u n t e d a s f a r a s p o s s i b l e f r o m t h e D C s e r v o m o t o r s a s s h o w n i n F i g u r e 2 . 4 . - 1 3 - " " 2 . 2 . 3 D i g i t a l E n c o d e r a n d E n c o d e r W h e e l T h e o p t i c a l e n c o d e r m o d u l e h a s t h r e e c h m m e l i n c r e m e n t a l e n c o d e r s w i t h a c o d e w h e e l i s c h o o s e n a s s h o w n i n F i g u r e 2 . 5 [ 3 ] . T h e s p e e d o f t h e t w o w h e e l s o f t h e p r o t o t y p e i s i n t e n d e d t o b e m e a s u r e d u s i n g t h i s . O p t i c a l e n c o d e r o u t p u t s t o g e t h e r w i t h t h a t o f t h e d i g i t a l c o m p a s s c a n b e u s e d f o r d e a d r e c k o n i n g i n n a v i g a t i o n p u r p o s e s o f t h e p r o t o t y p e s . ·~V R = 2 . 7 1 o : O Q T O O U T P U T L O G I C ! O N E . T T l l O A D P E R O U T P U T ) F i g u r e 2 . 5 - C o n n e c t i o n a r r a n g e m e n t o f t h e e n c o d e r Q e n c o d e e n c o d e r o b j e c t w a s u s e d t o g e t d a t a f r o m t h e d i g i t a l e n c o d e r . P o s i t i o n p r o p e r t y o f t h e Q e n c o d e o b j e c t w a s u t i l i z e d t o t a k e t h e e n c o d e r p o s i t i o n . C h a n n e l A a n d C h a n n e l B w e r e c o n n e c t e d t o I O l i n e s o n t h e O O P i c a n d t h e n u m b e r s o f t h e I O l i n e s w e r e s e t t o t h e Q e n c o d e o b j e c t p r o p e r t i e s . T h e e n c o d e r w h e e l a n d t h e O p t i c a l s e n s o r s s h o u l d b e m o u n t e d w i t h c a r e t o h a v e a g o o d a l i g n m e n t a s s h o w n i n F i g u r e 2 . 6 . E n c o d e r W h e e l F i g u r e 2 . 6 - T h e w a y o f m o u n t i n g t h e e n c o d e r t o t h e p r o t o t y p e - 1 4 - 2 . 2 . 4 S e r v o M o t o r s T h e H S - 4 2 2 s t a n d a r d d e l u x e s e r v o m o t o r s , b y H i t e c R C D I n c . , a r e 3 - p o l e f e r r i t e t y p e m o t o r a t t a c h e d w i t h a n i n - b u i l t p o t e n t i o m e t e r . S u c h a m o t o r i s s h o w n i n F i g u r e 2 . 7 . T h e s e m o t o r s a r e w i t h t h e c o n t r o l s y s t e m o f p u l s e w i d t h 1 5 0 0 , L L S n e u t r a l t y p e [ 1 1 ] . F i g u r e 2 . 7 - H S - 4 2 2 S e r v o M o t o r T h e p o t e n t i o m e t e r \ V a s r e m o v e d f r o m t h e s e r v o a n d a g e a r w h e e l a d j u s t m e n t w a s d o n e f o r c o n t i n u o u s r u n . T h e p r a c t i c a l s p e e d o f t h e p r o t o t y p e w a s q u i t e b e l o w t h e r e q u i r e d s p e e d . S o t h e c o n t r o l l i n g c i r c u i t o f t h e s e r v o w a s r e m o v e d a n d p o w e r w a s d i r e c t l y g i v e n t o t h e m o t o r t h r o u g h a m o t o r d r i v e r ( L 2 9 8 N ) c h i p . A s u p p l y v o l t a g e o f 7 V w a s g i v e n t o t h e s e r v o w h e n m a x i m u m s p e e d w a s r e q u i r e d . 2 . 2 . 5 U l t r a - S o n i c R a n g e S e n s o r s S e n s i n g t h e o b s t a c l e s a r o u n d e a c h v e h i c l e p r o t o t y p e w h i l e m o v i n g , i s i m p o r t a n t . T h e S R F 2 3 5 u l t r a s o n i c r a n g e s e n s o r s a r e c h o s e n f o r t h a t . T h o s e b y D e v a n t e c h L t d . , a r e f o r t h e p u r p o s e o f d e t e c t i n g o b s t a c l e s . T h e u l t r a s o n i c s e n s o r s a r e n o t m e a n t t o i d e n t i f y t h e o t h e r v e h i c l e p r o t o t y p e s i n c l o s e p r o x i m i t y . T h e a d j a c e n t p r o t o t y p e s a r e m e a n t t o b e i d e n t i f i e d , b y e a c h o t h e r , w i t h t h e R F c o m m u n i c a t i o n b e t w e e n t h e m o r b y a d d i n g a s h i e l d t o p r o t o t y p e ( m i n o r a d j u s t m e n t s ) . - 1 5 - " " T h e s e l e c t i o n o f t h i s t y p e o f u l t r a - s o n i c s e n s o r w i t h a n a r r o w b e a m p a t t e r n , a s i n d i c a t e d i n F i g u r e 2 . 8 , g i v e s t h e o p p o r t u n i t y t o d e t e c t t h e o b s t a c l e s , b u t n o t t h e f l o o r a s a n o b s t a c l e , a f a l s e a l a r m . M o u n t i n g a r r a n g e m e n t s o f t h e s e s e n s o r s w e r e a l s o c o n s i d e r e d w i t h s p e c i a l c o n c e r n . 0 2 7 0 1 \ I l ~~ J l I f J 9 0 1 8 0 F i g u r e 2 . 8 - B e a m p a t t e r n o f t h e S R F 2 3 5 ' P e n c i l b e a m ' u l t r a s o n i c s e n s o r T h e u l t r a - s o n i c s e n s o r i s w i t h a s i n g l e t r a n s d u c e r f o r b o t h t r a n s m i t a n d r e c e i v e . T h e r e f o r e , t h e r e i s a b l a n k i n g z o n e o f 1 O c m , s o t h e e f f e c t i v e r a n g e i s 1 O c m t o 1 . 2 m . C o m m u n i c a t i o n w i t h t h e S R F 2 3 5 u l t r a s o n i c r a n g e f i n d e r i s v i a t h e I 2 C b u s . T h e r e f o r e , t h i s i s e a s i l y c o n n e c t e d t o t h e O O P i c R + w i t h i t s c a p a b i l i t y o f I 2 C . I n o r d e r t o c o n n e c t t h e s e s e n s o r s t o t h e O O P i c R + , t h e a d d r e s s o f t h e s e n s o r s h a v e t o b e c h a n g e d . F i g u r e 2 . 9 i s t h e p i c t u r e o f S R F 2 3 5 . + 5 v - S O , D . , - - - S C L N o C o n n e c t / O n G r o u n d ( O v ) F i g u r e 2 . 9 - S R F 2 3 5 P e n c i l b e a m u l t r a s o n i c s o n a r s e n s o r - 1 6 - . . . . 2 . 2 . 6 I n t e r V e h i c u l a r C o m m u n i c a t i o n s c h e m e I n t e r v e h i c u l a r c o m m u n i c a t i o n w a s t h e m o s t i m p o r t a n t p a r t o f t h e s y s t e m . E R 4 0 0 r a d i o m o d u l e s , s h o w n i n F i g u r e 2 . 1 0 , w e r e u s e d f o r t h a t . F e w i m p o r t a n t f e a t u r e s l i k e s e v e r a l c h a n n e l s , l o w p o w e r c o n s u m p t i o n , v e r y s t a b l e o p e r a t i n g f r e q u e n c y a n d g o o d b a n d w i d t h f o r d a t a t r a n s m i s s i o n , r e l i a b l e c o m m u n i c a t i o n a n d g o o d d a t a r a t e s w e r e e x p e c t e d f r o m t h e R F m o d u l e . S e v e r a l C h a n n e l s w e r e n e e d e d t o e s t a b l i s h a g o o d - c o m m u n i c a t i o n . T h e c h o s e n m o d u l e s c a n h a v e 1 0 d i f f e r e n t c h a n n e l s , w h i c h m e e t o u r r e q u i r e m e n t . ~ ,~//'_]·~ '~ r / ' J / / J _ ) F i g u r e 2 . 1 0 - L P R S E R 4 0 0 R a d i o M o d u l e s T h e p r o g r a m m i n g s o f t w a r e , w h i c h c a n d o w n l o a d f r o m t h e w e b , w a s u s e d t o g r v e c o m m a n d s t o t h e m o d u l e a s s h o w n i n F i g u r e 2 . 1 1 [ 4 ] . ? '~ I r_vcJ~r,w-«::akn~l,,,h,; MvJ)~ F1~-l'Jo>:'IK\' ~'ludtJ~ f t t x - : . E H G O l 5 • • , { < ) f o . J 1 ' c < ; J > J e 8 A L V . : > I f l e ~ f P I J i l ~C.xrw.-!>!d: 5 q { y j ( , ' n Y f 1 " l f l < l " ' ' " ( R _ C M D # l J 1 4 + 0 0 l R . . l M O # ! J 1 " 1 1 0 0 ! H L M O # T : l [ - R - - l M D # 1 3 " ' f R J M O # l J ' : l L H . . t : M O # I J 3 A C K F R J M O # O I I ( _ ( M O . # ( I l R . C M O . < t U f R J M D # O . t . n : : U > ·ljiiW 1 -..MiJ)'- 1 fULJ:J•~)' [ l l . _ ( M O I ' M ? ( R _ C M [ ) , r t J F i g u r e 2 . 1 1 - E v a l u a t i o n S o f t w a r e - 1 7 - . . . T h a t s o f t w a r e w a s u s e d t o c h a n g e t h e b a u d r a t e , p o w e r l e v e l a n d c h a n n e l s o f t h e m o d u l e s . T h e b a u d r a t e w a s s e t t o 9 6 0 0 . T h e c h a n n e l s o f R F m o d u l e s w e r e s e t a c c o r d i n g t o t h e r e q u i r e m e n t s a n d t o a v o i d j a m m i n g a s s h o w n i n F i g u r e 2 . 1 2 . ~ ~annel3 ChannelS~ ~ . , . . . . . _ _ C h a n n e l 9 ---~ Chann~--· : l l l n l l===r~ ~'-'fr:=_ 8 \ ( l ) ' - - . / F i g u r e 2 . 1 2 - C o m m u n i c a t i o n C h a n n e l D e d i c a t i o n 2 . 2 . 7 E R 4 0 0 R S R e c e i v e r T h e B l o c k d i a g r a m o f t h e r e c e i v e r m o d u l e i s g i v e n i n F i g u r e 2 . 1 3 . P i n n u m b e r s 6 w a s u s e d t o g i v e c o m m a n d s t o m i c r o p r o c e s s o r t o c h a n g e t h e i n t e r n a l s e t t i n g s o f t h e m o d u l e . A p r o g r a m m i n g s o f t w a r e w a s u s e d t o g i v e c o m m a n d s t o t h e m o d u l e [ 4 ] . - 1 8 - . . . . A n t e n n a ( 1 ) I - ' - - r - - 1 - - f - - R F R e c e i v e r ~ R F G r o u n d ( 2 ) l R e g u l a t o r t - M i c r o ~ P r o c e s s o 1 · ~ ~ - ! + I , _ _ _ + ~ 1 - + - R S S I O u t p u t ( 3 ) V e e ( 8 ) B S Y ( 4 ) S e n a l D a t a O u t p u t ( 5 ) S e r i a l D a t a I n p u t ( 6 ) R O Y ( 7 ) G r o u n d ( 9 ) F i g u r e 2 . 1 3 - R e c e i v c r I t w a s q u i t e e a s y t o w o r k w i t h t h e s e m o d u l e s , b e c a u s e f e e d b a c k c h a r a c t e r s w e r e r e c e i v e d a f t e r e v e r y c o m m a n d . T h u s , t h e v e r i f i c a t i o n o f t h e c h a n g e s w a s s t r a i g h t f o r w a r d . 2 . 2 . 8 E R 4 0 0 T X T r a n s m i t t e r T h e s e t r a n s m i t t e r m o d u l e s d o n o t p r o v i d e a n y f e e d b a c k t o t h e p r o g r a m m i n g s o f t w a r e . O n l y t h e s e r i a l d a t a t r a n s m i s s i o n l i n e w a s p r o v i d e d a s s e e n i n F i g u r e 2 . 1 4 . S o i t w a s q u i t e d i f f i c u l t t o c h a n g e a n d v e r i f y t h e s e t t i n g s o f t h e t r a n s m i t t e r m o d u l e s . T w o s o f t w a r e w i n d o w s w e r e e m p l o y e d t o c h a n g e t h e s e t t i n g s o f t h o s e t r a n s m i t t e r m o d u l e s . O n e w i n d o w c o n n e c t e d t o c o m 1 s e r i a l p o r t w a s u s e d t o s e n d s e r i a l d a t a w h i l e t h e o t h e r w i n d o w c o n n e c t e d t o c o m 2 s e r i a l p o r t w a s u s e d t o r e c e i v e d a t a f r o m t h e r e c e i v e r m o d u l e . T h e R F C h a n n e l s e t t i n g c o m m a r n d s a r e p r e s e n t e d i n T a b l e 2 . 1 . I n i t i a l l y , 1 9 2 0 0 b p s b a u d r a t e w a s u s e d t o g i v e c o m m a n d t o t h e m o d u l e . T h a t b a u d r a t e c a n b e c h a n g e b y s e n d i n g b a u d r a t e s e t t i n g c o m m a n d . A n u n a m b i g u o u s b a u d r a t e h a s t o b e u s e d w h e n d e a l i n g w i t h t r a n s m i t t e r m o d u l e s . T h e b a u d r a t e d i f f e r e n c e s w e r e c a u s e d d a t a c o r r u p t i o n s . ; j , 1 t 1 . . . I - 1 9 - . . . . A n t e n n a ( 2 ) I / 1 R e g u l a t o r l - f - V c c ( 3 ) I . . . . _ _ R F T r a n s m i t t e r M i c r o . . . . - T X D ( 5 ) P r o c e s s o r - R F G n d ( " 1 l G n d ( 4 ) F i g u r e 2 . 1 4 - T r a n s m i t t e r T a b l e 2 . 1 - R F C h a n n e l s e t t i n g c o m m a n d s C h a n n e l C o m m a n d F r e q u e n c y n u m b e r E R C M D # C O 0 4 3 3 . 2 3 M H z E R C M D # C 1 1 4 3 3 . 3 0 M H z E R C M D # C 2 2 4 3 3 . 4 5 M H z E R C M D # C 3 " \ 4 3 3 . 5 5 M H z . ) E R C M D # C 4 4 4 3 3 . 6 8 M H z E R C M D # C 5 5 4 3 3 . 8 3 M H z E R C M D # C 6 6 4 3 3 . 8 8 M H z E R C M D # C 7 7 4 3 4 . 0 0 M H z i E R _ C M D # C 8 8 4 3 4 . 1 5 M H z I E R _ C M D # C 9 9 4 3 4 . 3 5 M H z 2 . 2 . 9 S e r i a l I n t e r f a c e C i r c u i t D e s i g n R S 2 3 2 s e r i a l w o r k w i t h 0 t o 1 5 V l o g i c l e v e l s w h i l e R F m o d u l e s w o r k w i t h 0 t o 5 V . T h e M A X 2 3 2 c h i p w a s u s e d t o d o t h e l e v e l c o n v e r s i o n . T h e c i r c u i t d i a g r a m o f t h e s e r i a l i n t e r f a c e c i r c u i t i s g i v e n i n F i g u r e 2 . 1 5 . - 2 0 - . . . . A t w o - w a y s w i t c h w a s e m p l o y e d i n t h e c i r c u i t t o c h a n g e d a t a t r a n s m i s s i o n m o d e t o s e t t i n g c h a n g i n g m o d e . B e c a u s e t h e d a t a t r a n s m i s s i o n l i n e f r o m t h e s e r i a l p o r t h a d t o c o n n e c t t o s e r i a l d a t a t r a n s m i s s i o n p i n v i a c h i p w h e n d a t a t r a n s m i s s i o n w a s n e e d a n d i t h a d t o b e c o n n e c t e d v i a c h i p t o s e r i a l d a t a i n p u t o f t h e r e c e i v i n g m o d u l e w h e n t h e s e t t i n g s c h a n g e w a s n e e d e d . T h e 7 8 0 5 r e g u l a t o r w a s u s e d t o g i v e p o w e r t o t h e R F t r a n s m i t t e r a n d r e c e i v e r m o d u l e s . T h e R F m o d u l e s w e r e m o u n t e d t o t h e b o a r d a s s h o w n i n F i g u r e 2 . 1 6 . T h e r e g u l a t o r m o d u l e w a s u s e d t o a c h i e v e a s m o o t h p o w e r s u p p l y t o t h e t r a n s c e i v e r , h e n c e m i n i m i z e d t h e e r r o r s i g n a l s t h a t c a n b e c a u s e d b y v a r y i n g i n p u t p o w e r . 1 N 4 1 4 8 P C A T F i g u r e 2 . 1 5 - S e r i a l I n t e r f a c e C i r c u i t D C D D S R R X D R T S T X D C T S D T R R l G N D - 2 1 - . . . . R e c e i v e r T r a n s m i t t e r F i g u r e 2 . 1 6 - T h e w a y o f m o u n t i n g T r a n s c e i v e r m o d u l e 2 . 2 . 1 0 I n t e g r a t i n g S e n s o r s t o t h e C o n t r o l l e r I n t e g r a t i n g s e n s o r s t o t h e c o n t r o l l e r w a s d o n e u s i n g t h e I 2 C b u s . D i f f e r e n t h e x a d e c i m a l a d d r e s s e s w e r e a s s i g n e d t o d i f f e r e n t c o m p o n e n t s t o a c c e s s t h r o u g h t h e I 2 C b u s . F i v e m o d u l e s w e r e c o n n e c t e d t o t h e I 2 C b u s . P o w e r f o r t h o s e m o d u l e s w a s g i v e n d i r e c t l y f r o m t h e 5 V r e g u l a t o r . S i d e e l e v a t i o n a n d p l a n o f t h e p r o p o s e d p r o t o t y p e i s p r e s e n t s i n F i g u r e s 2 . 1 7 a n d 2 . 1 8 . O O P i c B a s i c P r o g r a m f o r V e h i c u l a r P r o t o t y p e s i s g i v e n i n a p p e n d i x A . - 2 2 - . . . . . . U l t r a s o n c : Ser~sor U l t r < J s o n i < ; O O P i c R A o m d U l t r < J s o n i c S e n s o r U l t r a s o n i c S e n s o r F i g u r e 2 . 1 7 - P r o p o s e d P r o t o t y p e ( P l a n ) U l t r a s o n i c S e n s o r Tr~1 O O P i c R B o t : H G o . · . · · · · · · " " · ~ A e r i a l s o f l r a r M o d u l e r . · i o d t , : ! e I U l t r a s o n i c S c : n s o r D r i v i n : J V V h e e l F i g u r e 2 . 1 8 - T h e p r o p o s e d p r o t o t y p e ( S i d e e l e v a t i o n ) U l t r a s o m e D r i v e n V v ' h e e l - 2 3 - i L T h e a c t u a l i m p l e m e n t a t i o n o f t h e p r o t o t y p e c a n b e p r e s e n t a s f o l l o w s ( f i g u r e 2 . 1 9 ) . F i g u r e 2 . 1 9 - D e v e l o p e d v e h i c l e 2 . 2 . 1 1 I n t e r f a c i n g S o f t w a r e f o r P r o t o t y p e s I n t e r f a c i n g s o f t w a r e w a s d e v e l o p e d t o g i v e c o n t r o l s i g n a l s t o t h e v e h i c l e . V i s u a l B a s i c 6 w a s u s e d t o d e v e l o p t h e s o f t w a r e i n t e r f a c e f o r t h e p r o t o t y p e . G M S A c t i v e X c o n t r o l l e r s v v e r e u s e d a s j o y s t i c k s a n d d i g i t a l c o m p a s s . . I t h a s t h e a b i l i t y t o v i e w t h e c u r r e n t p o s i t i o n o f t h e p r o t o t y p e s i n i t s s k e t c h p a d , t h e c u r r e n t h e a d i n g a n g l e o f e a c h p r o t o t y p e , c u r r e n t c o o r d i n a t e s o f e a c h p r o t o t y p e i n s k e t c h p a d , c o m m u n i c a t e w i t h e a c h p r o t o t y p e , g i v e c o n t r o l s i g n a l s t o p r o t o t y p e s v i a t h e e s t a b l i s h e d R F l i n k t h r o u g h t h e s e r i a l p o r t , a n d d r a w t h e p a t h f o l l o w e d b y e a c h p r o t o t y p e i n t h e s k e t c h p a d . - 2 4 - ~ 2 . 3 E x p e r i m e n t i n g w i t h p r o t o t y p e s T w o i d e n t i c a l p r o t o t y p e s w e r e d e v e l o p e d t o c a r r y o u t t h e t e s t i n g p r o c e s s o f o b s t a c l e a v o i d a n c e a n d i n t e r v e h i c u l a r c o l l i s i o n a v o i d a n c e . 2 . 3 . 1 P o s i t i o n T r a c k i n g A l g o r i t h m A s G P S i s n o t a n o p t i o n b e c a u s e a f t h e s m a l l d i s t a n c e s i n v o l v e d i n t h e e x p e r i m e n t s w i t h s c a l e d d o w n p r o t o t y p e s , t h e f o l l o w i n g d e a d r e c k o n i n g a l g o r i t h m i s a d o p t e d f o r p o s i t i o n t r a c k i n g . T h e s p e e d o f t h e p r o t o t y p e w a s c a l c u l a t e d w i t h i n t h e p r o g r a m b y u s i n g t h e i n p u t v a l u e s o f t w o e n c o d e r s a n d a v e r a g i n g i t s v a l u e s i m i l a r l y a s m e q u a t i o n s 2 . 1 a n d 2 . 2 . T h e x ( k ) a n d y ( k ) p o s i t i o n s c a l c u l a t e d a s b e l o w . x ( k ) = x ( k - 1 ) + x x s i n ( B ) x ( l l t ) y ( k ) = y ( k - l ) + x x c o s ( 0 ) x ( ! l t ) - - - - - - - - - - - - - - - - - - - - - - - ( 2 . 1 ) - - - - - - - - - - - - - - - - - - - - - - - ( 2 . 2 ) W h e r e , B i s t h e h e a d i n g a n g l e o f t h e p r o t o t y p e . F i g u r e 2 . 2 0 e l a b o r a t e s t h i s f u r t h e r . - - - - . , - - - - - - - - - ; ; ' ' ' ' P a t h o f t h e v e h i c l e F i g u r e 2 . 2 0 - P o s i t i o n T r a c k i n g w i t h C o m p a s s x ( m ) - 2 5 - F i g u r e 2 . 2 1 s h o w s t h e p r o g r a m t h a t i s e x e c u t e d i n O O P i c , w h i c h u s e d t o m a n e u v e r t h e Y e h i c l e w i t h t h e c o m p a s s . T h e p r o g r a m b e g i n s w i t h i n i t i a l i z a t i o n o f t h e o b j e c t s t h a t a r e u s e d t o r e p r e s e n t t h e v e h i c l e . B a s i c a l l y , t h e s e n s o r s a n d t h e o b j e c t s a r e n e e d e d f o r c o m m u n i c a t i o n p u r p o s e s . A f t e r t h a t , i n p u t s f r o m t h e s e n s o r y d e v i c e s a r e f e d t o t h e e x e c u t i o n t h r o u g h t h e i n p u t - o u t p u t l i n e s t h a t a r e a l r e a d y s e t . P o s i t i o n m a n i p u l a t i o n t a k e s p l a c e a f t e r t h e i n p u t s a r e a n a l y z e d t o m i n i m i z e t h e e r r o r s t h a t c a n o c c u r d u e t o w r o n g i n p u t v a l u e s . T h e n , t h e p r o g r a m t r a n s m i t s t h e c a l c u l a t e d c u r r e n t p o s i t i o n o f t h e v e h i c l e t o t h e o t h e r v e h i c l e , o r t o t h e c o m p u t e r v i a t h e R F l i n k . 2 . 3 . 2 P e r i p h e r a l O b s t a c l e A v o i d a n c e T h i s o b s t a c l e a v o i d a n c e a l g o r i t h m i s f o r s t a t i o n a r y o b s t a c l e s s u c h a s w a l l s a n d o t h e r b a r r i e r s . T h i s m o d e a c t i v a t e s w h e n t h e d i s t a n c e t o t h e o b s t a c l e i s l e s s t h a n 1 5 e m [ 3 2 ] . T h e a l g o r i t h m u s e d i n t h i s s t u d y c a n b e p r e s e n t e d a s f o l l o w s . I f O b s t a c l e o n L e f t a n d d i s t a n c e d e c r e a s i n g , t h e n T u r n R i g h t f o r 3 [ s ] I f O b s t a c l e o n R i g h t a n d d i s t a n c e d e c r e a s i n g , t h e n T u r n L e f t f o r 3 [ s ] I f O b s t a c l e o n F r o n t & L e f t a n d d i s t a n c e d e c r e a s i n g , t h e n T u r n R i g h t f o r 3 [ s ] I f O b s t a c l e o n F r o n t & R i g h t a n d d i s t a n c e d e c r e a s i n g , t h e n T u r n L e f t f o r 3 [ s ] I f O b s t a c l e o n F r o n t t h e n S t o p , R e v e r s e f o r 2 [ s ] , S t o p a n d T u r n R i g h t f o r 3 [ s ] I f O b s t a c l e o n B a c k t h e n S t o p ( i f R e v e r s i n g ) a n d T u r n R i g h t f o r 3 [ s ] 2 . 3 . 3 C o l l i s i o n A v o i d a n c e T h i s a l g o r i t h m a p p l i e s w h e n t h e r e l a t i v e d i s t a n c e o f t h e v e h i c l e s i s l e s s t h a n o r e q u a l t o 3 0 e m . T h i s r e q u i r e s a d v a n c e i n f e r e n c e t e c h n i q u e s , t h a t p r o c e s s e d b y f u z z y b a s e d i n f e r e n c e e n g i n e . I f a p r o t o t y p e i n C o l l i s i o n s i t u a t i o n t h e n S t o p a n d t a k e a " r i g h t t u r n " f o r 3 [ s ] . I f s t i l l i n c o l l i s i o n s i t u a t i o n S t o p a n d t a k e a " l e f t t u r n " f o r 3 [ s ] E l s e " r e v e r s e " t h e p l a t f o r m f o r 3 [ s ] . 2 . 3 . 4 P o s i t i o n T r a c k i n g w i t h o u t t h e D i g i t a l C o m p a s s T h e c o m p a s s m o d u l e p l a y s a v i t a l r o l e i n t h e p o s i t i o n t r a c k i n g a l g o r i t h m . B u t s o m e e r r o r s m a y o c c u r d u e t o t h e m a g n e t i c f i e l d s g e n e r a t e d b y s e r v o m o t o r s . T h i s c a n o f t e n b e t h e c a s e i n e x p e r i m e n t i n g w i t h t h e p h y s i c a l l y s c a l e d d o w n p r o t o t y p e s a s k e e p i n g a n - 2 6 - .~ : l ~ l l i 1 I ~. a d e q u a t e d i s t a n c e b e t w e e n t h e m o t o r s a n d t h e d i g i t a l c o m p a s s i s n o t p r a c t i c a l . H e r e , w e p r e s e n t a m e t h o d t o c a l c u l a t e t h e h e a d i n g a n g l e s t o b e u s e d w h e n t h e c o m p a s s r e a d i n g s a r e n o t r e l i a b l e [ 3 6 ] . F i g u r e 2 . 2 2 s h o w s a t y p i c a l p a t h t h a t t h e v e h i c l e f o l l o w s a n d t h e f o o t p r i n t o f i t s t i r e s . T h e c h a n g e o f a n g l e f r o m t h e p r e v i o u s p o s i t i o n c a n b e a p p r o x i m a t e d u s i n g t h e w h e e l m o v e m e n t s a n d s i m p l e g e o m e t r y a s e l a b o r a t e s i n e q u a t i o n 2 . 3 a n d 2 . 4 . - 2 7 - 1 : ! i t i ~ . . . T L r n t h o p l . J t f , ; : ; r m t c J ( ) t 1---~ F i g u r e 2 . 2 1 - E x e c u t e d p r o g r a m i n O O P i c - 2 8 - 8 , = S J r e 2 = S 2 r , . . . - - l . - j r - : t . . I " " ) T ' t · · , I / I - • • I I . I - - - - I : J i 1 -r~z-. I ! T ' - I I - ! ' . I I I 7'!~.·-l 1 • I " / • . · - · ' ' , ' • · - ~ J I " , o r - - - I , · - / ' -. . ~ e ---~- , / ' 2 '4~·- ,/~· ' ' . . . / . · - - / . ' I ' S " / / ' ' / I • ' ; ; , . . / / ' , r / " ' " ' J l / S ? ' / ' i l ' . ' ) " ' < ' , , _ / / • • ' I ' , ' : : / 1 2 1 I ' ' I / ' " > f , I I ' , , / / / . 1 t 91~ . . / - ' I f - -------L~1' / . . . / , 2 t v ! o t i o n P a t h ' ;:--~:. I I ' , # t ! : - - - - - · : . I - - - 1 I ' I I I . _ r : I - - - - - - - - - - _ _ j _ o f V o l l i c l u I I • F i g u r e 2 . 2 2 - P o s i t i o n T r a c k i n g w i t h o u t C o m p a s s - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - ( 2 . 3 ) - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - ( 2 . 4 ) W h e r e S i s t h e t r a v e l i n g d i s t a n c e o f t h e w h e e l . r i s l e n g t h b e t w e e n t w o w h e e l s . T h e r e f o r e , t h e t o t a l a n g l e c h a n g e w i t h i n t h e c o u r s e r e l a t i v e t o t h e i n i t i a l d i r e c t i o n c a n b e o b t a i n e d b y c o n s i d e r i n g t h e a b o v e i n s t a n t a n e o u s a n g u l a r c h a n g e s . T h e O O P i c r e a d s t h e e n c o d e r r e a d i n g s e v e r y 2 s e c o n d s a n d e x e c u t e s t h e a l g o r i t h m a s i l l u s t r a t e d i n F i g 5 w h e r e E n c l a n d E n c 2 r e p r e s e n t t h e r e a d i n g s o f e n c o d e r 1 a n d 2 , r e s p e c t i v e l y . . O n e l i m i t a t i o n o f t h i s m e t h o d i s t h a t f l o a t i n g p o i n t s m a y o c c u r d u r i n g t h e e x e c u t i o n r e q u i r i n g s o m e o f f s e t a c t i o n s t o m i n i m i z e t h e e r r o r s c a u s e d b y t h i s . I f t h e d i f f e r e n c e b e t w e e n t w o e n c o d e r v a l u e s i s l e s s t h a n 2 0 , p r e v i o u s l y c a l c u l a t e d 8 v a l u e w a s f o n v a r d e d t o t h e n e x t s t e p a s s u m i n g a s t r a i g h t m o t i o n o f t h e p r o t o t y p e o r t h e p r o t o t y p e i s n o t m o v i n g . T h i s p r o g r a m s e g m e n t c a n b e p r e s e n t i n a f l o w l a y o u t a s i n f i g u r e 2 . 2 3 . - 2 9 - I ' I ~ I I I I f 2 . 3 . 5 F u z z y B a s e d C o n t r o l l i n g T h e p o s i t i o n i n f o r m a t i o n o b t a i n e d v i a t h e m e t h o d s e x p l a i n e d p r e v i o u s l y i s s e n t t o t h e c e n t r a l P C t h a t i m p l e m e n t s t h e f u z z y c o n t r o l l e r . P o s i t i o n i n f o r m a t i o n w a s p r o c e s s e d a n d t h e f u z z y b a s e c o n t r o l l i n g s i g n a l s w e r e g i v e n t o b o t h p r o t o t y p e s v i a R F l i n k . T h e b l o c k d i a g r a m o f c o m p o n e n t s i n c l u d i n g f u z z y i n f e r e n c e e n g i n e i s g i v e n i n f i g u r e 2 . 2 4 . E n c = E n c 1 - E n c 2 e U p ( b t e P o s i t i o n U p d a t e E n c = E n c 2 - E n c 1 S a m e E l e U p d < 1 t e F i g u r e 2 . 2 3 - P o s i t i o n T r a c k i n g w i t h o u t C o m p a s s ( b l o c k d i a g r a m ) T h e e n c r y p t i o n m o d u l e t h e r e i n a c t s a s a c o n v e r t e r o f d e f u z z y f i e d o u t p u t t o t h e c o d e t h a t t h e c o m m u n i c a t i o n m o d u l e c a n u n d e r s t a n d . T h e f u z z y b a s e d c o n t r o l l i n g f u n c t i o n s w e r e d e v e l o p e d b y m e a n s o f M a t L a b s i m u l a t i o n s o f t w a r e s i m u l i n k . A N F I S t o o l b o x w a s e m p l o y e d f o r t h a t . T h e i n p u t m e m b e r s h i p f u n c t i o n s w e r e d e f i n e d i n i t i a l l y . - 3 0 - : ~ • I ; ' : ~ t R F L i r k V c h 1 d e 1 ! i n k . P o s i t i O n D a t a E n c r y p t i o n f v l o d u l e F i g u r e 2 . 2 4 - F u z z y B a s e d C o n t r o l l i n g 2 . 3 . 6 C o l l i s i o n C o n d i t i o n F u n c t i o n T h i s a s t h e n a m e i m p l i e s , t o i d e n t i f y t h e v e h i c l e s a r e i n c o l l i s i o n s t a t e s t o t r i g g e r t h e c o l l i s i o n a v o i d a n c e s c e n a r i o . T h e t h e o r y o f r e l a t i v e v e l o c i t y b e t w e e n t w o v e h i c l e s w a s e m p l o y e d f o r t h i s p r o c e s s . T h e p r o g r a m v i r t u a l l y c r e a t e s t h e i n e r t i a l f r a m e w i t h r e s p e c t t o o n e v e h i c l e a n d c h e c k w h e t h e r t h e v e h i c l e s a r e i n c o l l i s i o n c o n d i t i o n b y t h e a i d o f t h e r e l a t i v e d i s t a n c e a n d v e l o c i t y . B y m e a n s o f t h e p a t h o f a v e h i c l e r e l a t i v e t o t h e o t h e r , s o m e c o l l i s i o n s i t u a t i o n s c a n b e d e f i n e d . I f t h e c o l l i s i o n s b e t w e e n t w o v e h i c l e s o c c u r i n l i n e , m e a n s t h a t a d i r e c t c o l l i s i o n , t h a t s t a t e w a s d e f i n e d a s ' i n l i n e c o l l i s i o n ' . T h e o t h e r t w o s t a t e s , n a m e d a s ' i n l i n e o f l i k e l y c o l l i s i o n ' a n d ' n o t i n l i n e c o l l i s i o n ' f o l l o w i n g t h e s a m e m e t h o d o l o g y t h a t w a s p r e v i o u s l y s t a t e d . T h i s i s e s s e n t i a l t o q u a n t i f y t h e i n p u t s , w h i c h n e e d s f o r f u z z i f i c a t i o n p r o c e s s . T h e r a n g e o f v a l n e s f o r t h e c o l l i s i o n c o n d i t i o n f u n c t i o n w a s t a k e n a s - 3 t o 0 , a n d d e c i d e d t h e c e n t e r o f t h e G a u s s i a n m e m b e r s h i p f u n c t i o n a s f o l l o w s [ 2 4 ] . T a b l e 2 . 2 - C e n t e r s o f G a u s s i a n M e m b e r s h i p F u n c t i o n s C o l l i s i o n S i t u a t i o n C e n t e r o f t h e G a u s s i a n m e m b e r s h i p f u n c t i o n I n l i n e c o l l i s i o n - 3 I n l i n e o f l i k e l y - 1 . 5 c o l l i s i o n N o t i n l i n e c o l l i s i o n 0 - 3 1 - ' I ' I 2 . 3 . 7 R e l a t i v e D i s t a n c e F u n c t i o n T h i s i s t h e E u c l i d i a n d i s t a n c e b e t w e e n t h e v e h i c l e s , i n a n i n e r t i a l f r a m e o f o n e v e h i c l e . A v i r t u a l c i r c l e i s d r a w n a r o u n d t h e v e h i c l e s s u c h t h a t i t c o v e r s t h e w h o l e p a r t s o f t h e v e h i c l e . T h e c o l l i s i o n s t a t e c a n b e s t a t e d a s f o l l o w s . I f t h e d i s t a n c e b e t w e e n t h e c e n t e r s o f t h e v i r t u a l c i r c l e s i s l e s s t h a n o r e q u a l t o t h e d i a m e t e r o f a v i r t u a l c i r c l e , t h e n t h e v e h i c l e s a r e i n c o l l i s i o n s t a t e ( E q u a t i o n 2 . 5 ) . R e l a t i v e D i s t a n c e = ( ( x 1 - x 2 ) 2 + ( y 1 - y 2 ) 2 ) ! i - - - - - - - - - - - - - - - - - - - - - - - - - - - ( 2 . 5 ) T h e r e f o r e t h e b r e a k i n g c r i t i c a l d i s t a n c e ( d b r ) c a n b e p r e s e n t a s 2 ~ ( v 2 + ( v - v r e t ) 2 ) , f o r h e a d - o n c o l l i s i o n s d b r = ~ 2~ ( V 2 - ( v - v r e l ) 2 ) ' f o r r e a r - e n d c o l l i s i o n s 1 v 2 o t h e r w i s e 2 a ' - - - - - - - - - - - - - - - - - - - - - - - - - - ( 2 . 6 ) W h e r e v i s t h e v e l o c i t y o f t h e v e h i c l e a n d V r e l i s t h e r e l a t i v e v e l o c i t y o f t h e v e h i c l e w i t h r e s p e c t t o t h e o t h e r . a i s , t h e m a x i m u m p o s s i b l e d e c e l e r a t i o n o f t h e v e h i c l e a s s h o w n i n e q u a t i o n 2 . 6 [ 3 ] . 2 . 3 . 8 M a s t e r S l a v e S w i t c h i n g T h i s f u n c t i o n i s m a i n l y f o r h i e r a r c h i c a l c o n t r o l l i n g o f t h e v e h i c l e s . T h i s a s s i g n s t h e l a b e l s ' M a s t e r ' a n d ' S l a v e ' f o r t h e v e h i c l e a n d i t i s d e p e n d s u p o n t h e c u r r e n t s i t u a t i o n o f t h e v e h i c l e s . T h e ' M a s t e r v e h i c l e ' h a s m o r e p o w e r r e l a t i v e t o t h e ' S l a v e v e h i c l e ' . T h i s w a s u s e d i n t h e d e c i s i o n m a k i n g p r o c e s s , i n s i d e t h e i n f e r e n c e e n g i n e . A t y p i c a l l a b e l a s s i g n a t i o n c r i t e r i o n i s t h e s p e e d o f t h e v e h i c l e . W h e n i n I n t e r v e h i c u l a r c o m m u n i c a t i o n m o d e , t h e m a s t e r - s l a v e s t a t u s w e r e c a l c u l a t e d , t r a n s m i t t e d a n d a c k n o w l e d g e d b y e a c h v e h i c l e . W h e n t h e c o m m u n i c a t i o n s c h e m e i s t h r o u g h t h e c o m p u t e r , i t a s s i g n s t h e m a s t e r - s l a v e s t a t e s t o t h e v e h i c l e s b y a n a l y z i n g t h e m o t i o n s o f t h e m , a c c o r d i n g t o p r e p r o g r a m m e d a l g o r i t h m [ 3 2 ] . - 3 2 - ·~ : ! ~ I , j M a s t e r - S l a v e s w i t c h i n g ( M S S w i t c h ) f u n c t i o n h a s t h r e e v a r i a b l e s . T h e y a r e m a s t e r , s l a v e , a n d d r i v e r c o n t r o l . I n o r d e r t o q u a n t i f y t h e s e v a r i a b l e s , ' m a s t e r ' i s a s s i g n e d ' l ' w h i l e t h e ' s l a v e ' i s a s s i g n e d w i t h ' 0 ' . T h e v a l u e g i v e n t o ' d r i v e r c o n t r o l ' i s ' 5 ' . A s d i s c u s s e d b e f o r e , t h e s e v a l u e s a r e t a k e n a s t h e i n i t i a l c e n t e r s o f t h e G a u s s i a n m e m b e r s h i p f u n c t i o n s o f t h e c o r r e s p o n d i n g f u z z y v a r i a b l e s . 2 . 3 . 9 C o n t r o l l i n g F u n c t i o n T h e s e i n p u t m e m b e r s h i p f u n c t i o n s w e r e t r a i n e d u s i n g 6 5 0 p a i r s . T h o s e d a t a p a i r s w e r e g e n e r a t e d e m p l o y i n g s p r e a d s h e e t p r o g r a m a c c o r d a n c e w i t h t h e c o n t r o l l e r a l g o r i t h m , c o n s i d e r i n g t h e r a n g e s o f t h e i n p u t a n d o u t p u t v a r i a b l e s . T h e g e n e r a t e d d a t a s e t e n a b l e d t o t r a i n t h e A N F I S s o t h a t i t m i m i c s t h e b e h a v i o r o f a n e x p e r t . T h e t r a i n e d i n p u t f u n c t i o n s w e r e t a k e n f r o m t h e M a t L a b s o f t w a r e . T h e s h a p e s o f t h o s e f u n c t i o n s w e r e a d j u s t e d a t t h e l e a r n i n g a c c o r d i n g t o t h e t r a i n i n g d a t a . T h e T a k a g i - S u g e n o t y p e 5 4 o u t p u t f u n c t i o n s a f t e r t r a i n i n g , f o r t h e b r a k i n g c o n t r o l l e r w e r e t a k e n a n d t h e y a r e g i v e n b e l o w [ 1 5 ] . . h = 1 . 8 5 x J + 1 . 5 6 6 e - 8 X r 2 . 7 9 e - 2 1 X 3 - 2 . 1 8 9 4 e - 1 5 X 4 - 1 . 4 8 J ; = 0 . 0 0 1 9 X 1 - 0 . 0 3 9 X r 5 . 6 5 5 e - 2 3 x 1 - 4 . 7 2 e - 1 7 X 4 - 4 . 2 2 5 ~ = - 0 . 3 1 1 X 1 - 2 . 8 9 2 e - 6 X r 5 . 5 7 7 e - 2 5 X 3 - 5 . 6 6 7 e - 1 8 X 4 + 0 . 0 9 2 1 - - - - - - - - - - - - - - - - - ( 2 . 7 ) /~ 4 =-1.710e-15 X 1 + 4 . 9 1 1 e - 1 8 X 2 - 4 . 9 1 0 e - 1 4 X 3 + 5 . 6 7 7 e - 1 5 X 4 + 5 . 6 7 9 e - 1 5 T h e T a k a g i - S u g e n o t y p e 5 4 o u t p u t f u n c t i o n s a f t e r t r a i n i n g , f o r t h e s t e e r i n g c o n t r o l l e r w a s t a k e n a n d t h e y a r e g i v e n b e l o w [ 3 2 ] . J ; = - 0 . 3 2 2 X 1 + 0 . 0 0 3 6 X 2 + 5 . 7 9 8 e - 2 2 X 3 + 0 . 0 0 4 5 9 X 4 + 0 . 2 3 3 f ; = - 0 . 0 1 4 1 X J + 0 . 0 0 3 2 1 X 2 + l . 0 6 9 e - 2 2 X 3 + 0 . 0 0 0 2 1 X 4 + 0 . 6 2 1 .~ = - 0 . 1 5 1 x J + 0 . 0 0 0 6 5 9 X 2 + l . 9 9 6 e - 2 3 X 3 + 0 . 0 0 3 9 x 4 + 0 . 0 6 5 2 - - - - - - - - - - - - - - - - - - ( 2 . 8 ) / 5 4 = 2 . 6 9 c - 2 4 X 1 + 1 . 7 5 5 e - 2 4 X 2 - 3 . 9 1 1 e - 2 5 X 3 + 2 . 6 9 1 e - 2 3 x 4 - 1 . 2 1 7 e - 2 3 - 3 3 - ' ' ' I I t I n t h e p r o t o t y p e e x p e r i m e n t s , t h e a b o v e 1 0 8 ( 5 4 X 2 ) f u n c t i o n s w e r e i m p l e m e n t e d i n t h e P C b y m e a n s o f M i c r o s o f t V i s u a l B a s i c 6 I D E . T h e p o s i t i o n , h e a d i n g , a n d s p e e d i n f o r m a t i o n f r o m t h e p r o t o t y p e s w a s t r a n s m i t t e d a n d t a k e n v i a a s e r i a l p o r t t o t h e c e n t r a l P C . T h i s i n f o r m a t i o n i s r e q u i r e d t o a s s e s s t h e i n p u t m e m b e r s h i p v a l u e s [ 3 2 ] . T h e c o n t r o l l i n g s i g n a l s g e n e r a t e d w e r e t r a n s m i t t e d t o t h e p r o t o t y p e s v i a t h e R F l i n k . F i g u r e 2 . 2 5 t o 2 . 2 7 i l l u s t r a t e s t h e i n p u t m e m b e r s h i p f u n c t i o n s a f t e r t r a i n i n g . T h e s e i l l u s t r a t i o n s w e r e c r e a t e d i n t h e M a t L a b e n v i r o n m e n t ; w i t h t h e a i d o f t h e A N F I S e d i t t o o l , a n d S i m u l i n k . . 8 - 0 8 ~ u E ! o 6 E N o C o l l i s i o n 0 ~--------~--------~----------~~ - 3 - 2 . 5 - 2 - 1 . 5 - 1 - 0 . 5 0 C o i l i s i o n C o n d i t i o n F i g u r e 2 . 2 5 - C o l l i s i o n C o n d i t i o n F u n c t i o n a f t e r T r a i n i n g . § 0 8 ~ Q ) , D ~0.6 O J E ' - ~04 Q ) ' " ' b D Q ) C l o 2 Y e r v C l o s e 0 C J o s e M o d e r a t e D i s t a n t 1 0 1 5 2 0 2 5 R e l a t i v e D i s t a n c e [ m ] F i g u r e 2 . 2 6 - R e l a t i v e D i s t a n c e F u n c t i o n a f t e r T r a i n i n g - 3 4 - ' ' ' I I t I i ! 1 1 ' S l a v e j~ . & 0 . 8 ~ u s 0 6 v 8 4 - < ~ 0 4 2 o J ) ( . ) 0 0 . 2 0 L . _ 0 0 . 5 M n s t e r 1 5 2 . 5 M S S w i t c h D r i v e r C o n t r o 3 . 5 4 4 . 5 F i g u r e 2 . 2 7 - M a s t e r - S l a v e S w i t c h i n g F u n c t i o n a f t e r T r a i n i n g 2 . 4 R e s u l t s T r a j e c t o r i e s o f t h e p r o t o t y p e s w e r e t a k e n f r o m t h e g r a p h i c a l u s e r i n t e r f a c e o f t h e d e v e l o p e d s o f t w a r e . I t w a s d e v e l o p e d u s i n g s h a p e o b j e c t s i n V i s u a l b a s i c 6 a n d J o y s t i c k o b j e c t s o f G l o b a l M a g i c S o f t w a r e ' s . P r e d e f i n e d c o - o r d i n a t e s y s t e m w a s u s e d t o m a p t h e p r o t o t y p e s . T w o j o y s t i c k s w e r e u s e d a t t h e m a n u a l d r i v e r m o d e . W h e n t h e v e h i c l e s r e c o g n i z e a p o s s i b l e c o l l i s i o n s c e n a r i o , t h e j o y s t i c k c o m m a n d s a r e s i m p l y i g n o r e d a n d t h e a u t o m a t i c c o l l i s i o n a v o i d a n c e c o n t r o l l e r t u r n s o n . I t i s a u t o m a t i c a l l y c h a n g e d i n t o m a n u a l m o d e a f t e r a v o i d i n g c o l l i s i o n s c e n a r i o s . F i g 2 8 r e p r e s e n t s a s c r e e n s h o t t h a t w a s t a k e n i n a t y p i c a l t e s t c a s e o f t h e s t u d y t h a t h a s b e e n c a r r i e d o u t w i t h t h e t w o p r o t o t y p e s . I t i l l u s t r a t e s t h e p a t h s f o l l o w e d b y t h e p r o t o t y p e s a n d t h e e n c r y p t e d d a t a t h a t h a s b e e n r e c e i v e d . - 3 5 - : c ' ( . . , . I . ; , < t • r . r ; : @ F i g u r e 2 . 2 8 - S c r e e n s h o t o f t h e d e v e l o p e d G U I T h e s p e e d o f t h e p r o t o t y p e s w a s q u i t e l o w d u e t o t h e p e r f o r m a n c e o f t h e d e s e r v o s . B u t i t w a s h e a l t h y w i t h t h e w h o l e s y s t e m . B e c a u s e o f t h e s e r i a l c o m m u n i c a t i o n , e c b u s a n d O O P i c w e r e m a i n l y i n t r o d u c e d d e l a y s t o w h o l e s y s t e m . B u t t h o s e c a n b e m i n i m i z i n g b y u s i n g a p p r o p r i a t e h a r d w a r e w h e n t h i s s y s t e m i s g o i n g t o b e i m p l e m e n t e d f o r g e n u i n e v e h i c l e s . T h i s c o n c e p t c a n b e e x t e n d e d f o r m u l t i p l e v e h i c l e s b y c o n s i d e r i n g t h e m a s p a r r s . 2 . 5 S u m m a r y T h i s c h a p t e r h a s p r e s e n t e d a n d e x p e r i m e n t a l l y v a l i d a t e d a n i n t e r a c t i v e i n t e l l i g e n t c o l l i s i o n a v o i d a n c e c o n t r o l l e r v i a t e s t i n g o n v e h i c u l a r p r o t o t y p e s . A m a s t e r - s l a v e m e c h a n i s m i s e n g a g e d t o e f f e c t i v e l y n e g o t i a t e t h e c o o p e r a t i v e m a n e u v e r s b y t h e v e h i c l e o n t h e v e r g e o f a c o l l i s i o n t o o p t i m a l l y a v o i d t h e c o l l i s i o n . T h e c o n t r o l s t r a t e g y w a s d e v e l o p e d b a s e d o n A N F I S f u z z y a n d h a s b e e n t h o r o u g h l y v a l i d a t e d v i a c o m p u t e r s i m u l a t i o n s i n f o r a l l p o s s i b l e c o l l i s i o n s c e n a r i o s . T h e c e n t r a l c o m m u n i c a t i o n P C w a s u s e d t o a v o i d m e m o r y c o n s t r a i n s i n t h e d i g i t a l c o n t r o l l e r ( O O P i c ) a t t h e c o s t o f s o m e d e l a y s i n t h e w h o l e s y s t e m . A b e t t e r D S P c a n e l i m i n a t e t h e c e n t r a l P C t o e a s i l y o v e r c o m e t h i s p r o b l e m i n p r a c t i c a l i m p l e m e n t a t i o n . - 3 6 - 3 . 1 I m p l e m e n t i n g t h e c o n t r o l l e r C h a p t e r 3 D e s i g n o f N a v i g a t i o n a l C o n t r o l l e r f o r U S V A g o o d n a v i g a t i o n a l c o n t r o l l e r i s a m a i n r e q u i r e m e n t f o r t r a v e l i n g . S o t h e T a k a g i - S u g e n o t y p e f u z z y l o g i c b a s e c o n t r o l l e r w a s i m p l e m e n t e d s u c c e s s f u l l y f o r n a v i g a t i o n [ 5 ] . T h e b o a t i s t r a v e l i n g a s s h o w n i n F i g u r e 3 . 1 . y X b o a t T A T , ~ f ) X E a r t h f i x f r a m e F i g u r e 3 . 1 - B o a t w i t h f u z z y b a s e d n a v i g a t i o n a l c o n t r o l l e r T x a n d T y a r e t h e t h r u s t f o r c e s e x e r t e d b y t h e p r o p e l l e r s o f t h e b o a t . T h e h e a d i n g o f t h e b o a t i s g i v e n b y 0 . T h e t h r u s t f o r c e i s c o n t r o l l e d u s i n g a f u z z y c o n t r o l l e r . T h e o v e r a l l c o n t r o l s y s t e m ( n a v i g a t i o n ) i s s h o w n i n F i g u r e 3 . 2 . I f t h e r e q u i r e d p o s i t i o n o f t h e b o a t i s ( x , , y J a n d t h e a c t u a l p o s i t i o n i s ( x , y ) t h e n t h e e r r o r e i s g i v e n b y e = x r - X . - 3 7 - ~ ~ , , ) l i . . . . e d d t 1 e F u z z y c o n t r o l l e r B o a t F i g u r e 3 . 2 - F u z z y b a s e d n a v i g a t i o n a l c o n t r o l l e r F i v e i n p u t m e m b e r s h i p f u n c t i o n s w e r e d e f i n e d t o r e p r e s e n t t h e e r r o r i n p u t n a m e d n e g a t i v e l a r g e ( N L ) , n e g a t i v e s m a l l ( N S ) , z e r o ( Z ) , p o s i t i v e s m a l l ( P S ) , p o s i t i v e l a r g e ( P L ) a n d s a m e n a m e s w e r e u s e d f o r t h e c h a n g e r a t e o f e r r o r a s w e l l . I n p u t m e m b e r s h i p f u n c t i o n s t o f u z z y c o n t r o l l e r a n d o u t p u t m e m b e r s h i p f u n c t i o n s f r o m t h e f u z z y c o n t r o l l e r a r e g i v e n i n F i g u r e 3 . 3 , , 3 . 4 a n d 3 . 5 . D e g r e e o f m e m b e r s h i p N e g a t i v e l a r g e N e g a t i v e S m a l l Z e r o P o s i t i v e S m a l l P o s i t i v e L a r g e E r r o r - 3 e - 2 e - e - e / 4 0 e / 4 e 2 e 3 e F i g u r e 3 . 3 - E r r o r i n p u t m e m b e r s h i p f u n c t i o n o f t h e f u z z y b a s e d n a v i g a t i o n a l c o n t r o l l e r - 3 8 - ~I , , D e g r e e o f m e m b e r s h i p N e g a t i v e l a r g e N e g a t i v e S m a l l 1 I Z e r o P o s i t i v e S m a l l - 3 E d - 2 E d - E d - E d / 4 0 E d / 4 E d 2 E d P o s i t i v e L a r g e I I I I I I I I 1 R a t e o f 1 c h a n g i n g e r r o r 3 E d F i g u r e 3 . 4 - R a t e o f c h a n g e e r r o r i n p u t m e m b e r s h i p f u n c t i o n o f t h e f u z z y b a s e d n a v i g a t i o n a l c o n t r o l l e r D e g r e e . o f m e m b e r s h i p Z e r o P o s i t i v e S m a l l P o s i t i v e L a r g e N e g a t i v e l a r g e N e g a t i v e S m a l l - 2 T - T 0 T 2 T F i g u r e 3 . 5 - R a t e o f c h a n g e e r r o r i n p u t m e m b e r s h i p f u n c t i o n o f t h e f u z z y b a s e d n a v i g a t i o n a l c o n t r o l l e r R u l e b a s e i s o n e o f t h e i m p o r t a n t p a r t s i n t h e f u z z y c o n t r o l l e r . W h e n T a b l e 3 . 1 s h o w s t h a t t h e " t h r u s t i s p o s i t i v e l a r g e " m e a n s t h a t t h e f u z z y c o n t r o l l e r w i l l g i v e c o n t r o l l i n g s i g n a b t o t h e b o a t ' s t h r u s t f o r c e c o n t r o l l e r t o i n c r e a s e i t s t h r u s t f o r c e b y 2 d T ( m a x i m u m s a f e t h r u s t i n c r e a s e ) b y c h a n g i n g p r o p e l l e r a n g l e a n d t o r q u e . " T h r u s t i s n e g a t i v e l a r g e " i s r e p r e s e n t e d t h e m a x i m u m t h r u s t f o r c e r e d u c t i o n o f t h e b o a t . T h e F i g u r e 3 . 6 p r e s e n t s t h e o u t p u t s u r f a c e o f t h e f u z z y - l o g i c n a v i g a t i o n a l c o n t r o l l e r . - 3 9 - 0 . 5 i ! U l 2 0 i . r : . 1 - - 0 . 5 • I - 1 I 5 T a b l e 3 . 1 - R u l e t a b l e f o r f u z z y P D c o n t r o l l e r ~ N L N S z P S P L N L N L N L N S N S N S N S N L N S N S N S N S z N S N S z P S P S P S P S P S P S - P S P L P L P S P S P S P L P L 5 0 0 E r r o r R a t e - 5 - 5 E r r o r F i g u r e 3 . 6 - O u t p u t s u r f a c e o f t h e f u z z y b a s e d n a v i g a t i o n a l c o n t r o l l e r 3 . 2 M a t h e m a t i c a l m o d e l f o r U S V T h e m a t h e m a t i c a l m o d e l o f t h e b o a t c a l l e d D e l f i m ( F i g u r e 3 . 7 ) , d e v e l o p e d b y D y n a m i c a l S y s t e m s a n d O c e a n R o b o t i c s L a b o r a t o r y ( P o r t u g a l ) [ 3 7 ] i s u t i l i z e d f o r t h e s i m u l a t i o n p u r p o s e s . T h e f o l l o w i n g d y n a m i c e q u a t i o n s a r e m o d e l e d i n M a t L a b - 4 0 - : j , } l e n v i r o n m e n t [ 1 9 ] . T h i s m a t h e m a t i c a l m o d e l i s c o n t r o l l e d b y u s i n g t h e f u z z y l o g i c n a v i g a t i o n a l c o n t r o l l e r i n t h e s i m u l a t i o n s w h i c h a r e p e r f o r m e d F i g u r e 3 . 7 - P i c t u r e o f t h e a c t u a l b o a t m o d e l T h e v e l o c i t i e s o f s u r g e ( X B - d i r e c t i o n ) , s w a y ( Y B - d i r e c t i o n ) , a n d y a w ( r o t a t i o n a b o u t Z B - d i r e c t i o n ) a r e d e f i n e d a s u = u ( t ) , v = v ( t ) , a n d r = r ( t ) . T h e n t h e d y n a m i c e q u a t i o n s f o r t h e m o d e l a r e g i v e n a s b e l o w , n n i = - D ( u , v , r ) + m v r + T c o s a + T c o s a X P p S · ' - - - - - - - - - - - - - - - ( 3 . 1 ) m v = - D ( z t , v , r ) - m u r + T s i n a + T s i n a y p ! ' s s - - - - - - - - - - - - - - - ( 3 . 2 ) m l ; = - D ( u , v , r ) + T c o s a x d + T c o s a x d + T s i n a x d + T s i n a x d . f : ! ' ! ' f l Y , \ ' S · ' Y P p X S S X ( " ' " ' > ) - - - - . : l . . : l W h e r e T i s t h r u s t d e l i v e r e d b y p o r t s i d e a n d s t a r b o a r d s i d e p r o p e l l e r s , r e s p e c t i v e l y t h e a 1 ) a n d a s a r e i n c l i n a t i o n s t o t h e X B - a x i s , d i s d i a m e t e r o f t h e p r o p e l l e r . R . t s i c s t e p s o f t h e M a t L a b p r o g r a m a r e g i v e n b e l o w , a ) D e f i n e a l l t h e v a r i a b l e s a n d s e t t h e d i m e n s i o n s o f t h e b o a t b ) C a l c u l a t e m a s s a n d i n e r t i a m a t r i x o f t h e b o a t c ) C a l c u l a t e f r i c t i o n a l , f o r m a n d a d d i t i o n a l r e s i s t a n c e f o r c e s - 4 1 - \ _ f } l e ) C a l c u l a t e t h e d a m p i n g a n d t o t a l m o m e n t s f ) S e t a l l t h e v a r i a b l e s t o i t s i n i t i a l v a l u e s , i n c l u d i n g t i m e t , t = 0 g ) D e f i n e b o u n d a r y c o n d i t i o n s f o r a l l v a r i a b l e s o f i n p u t a n d o u t p u t h ) I n i t i a l i z e t h e r e f e r e n c e v a l u e s , s u c h a s r e f e r e n c e t r a j e c t o r y o f t h e s h i p i n 2 - D p l a n e i ) W h i l e t < t s t o p C a l c u l a t e t h e p o s i t i o n e r r o r F e e d t h e i n p u t s t o t h e c o n t r o l l e r ( w i t h i n e q u a l i n t e r v a l s ) P r o c e s s t h e r u l e s a c c o r d i n g t o t h e i n p u t s , b y m e a n s o f M a t L a b F I S . G e t t h e d e f u z z i f i e d o u t p u t S o l v e d y n a m i c e q u a t i o n s t o f i n d n e w p o s i t i o n P l o t t h e r e s u l t s 3 . 3 R e s u l t s f r o m t h e n a v i g a t i o n a l c o n t r o l l e r T h e p e r f o n n a n c e o f t h e f u z z y b a s e d n a v i g a t i o n a l c o n t r o l l e r i s c o m p a r e d w i t h t h e p e r f o r m a n c e o f a P D c o n t r o l l e r t o p r o v e t h e b r i l l i a n c e o f t h e f u z z y l o g i c b a s e d c o n t r o l l e r s . T h e r e s u l t s a r e p r e s e n t e d i n F i g u r e 3 . 8 t o 3 . 1 2 a n d M a t L a b p r o g a m i s g i v e n i n A p p e n d i x B . T h e d e s i r e d p a t h i s p r e s e n t e d i n r e d w h i l e b l u e i s u t i l i z e d f o r t h e a c t u a l p a t h . P a t h t r a c k i n g : S i n o s o i d a l t r a j e c t o r y 10 .-----~---.----,-----.---~~---.-----.----,-----. : : : ' a \: · : : \ , , , I , ' , ' I : : : ' : : : 5t - - 1 : - - - - - - - - \ - - i - - - - - -~- - - -----~--1-r - l - - - - - - ---~ - - - - - - - - + _ _ _ _ - - - - · - - - - - - - - - ' ' ,, . . . I , . " . . l j ~. : : : J : " : : ~E 1 : ~ . : : : I : : ~\ : : I : • : : : : : ~ : : ~ : t \ : : :, . : : : \ : : ~ 0 - - - - - - - - - - - - : - \ - - - - - --~-- - - - - - - - 1 - ! - - - - - - - ~-------- ~l- - - - - - - - - : - - - - - - - - - - · - - - - - - - - - : : : t l . : : ~ ~ : t : · ~ : , ' . . ' . ' \ ' ' • ~ : : , : : : : : 1 : . . ' ' . , ' • ' \ ' . 1 1 1 • • ' : · ' • ' I . • - " \ \ : I I : : ~ , : : : , g - 5 - - - - - - - - - ---------- i-~L _ _ _ [ _ _ ---- '~ -~----- _ _ _ _ ; _ _ _ _ _ _ _ _ _ _ } ; _ 0 J !, _ - _ _ _ [ _ _ _ _ _ _ If_ ~ - - - - - - 1 1 1 . a ' , , , , : ' , , I I : \ \ : : : : \ \ : : . ! § : \ : : : : ' \ : : j j j j j \ j j - 1 0 f - - - - - - - - - - - - - - - - - - - ! - - - - - - - : ---------~----------!----- - - - ~---- - - : - -------~ - - - - - - - - ' I I I I f I t I I I I I I I I I I I I I I I I I I I I I f I I I I I I I I I I I I I I I I t I I I I I I I I t I I I I I -15 L---~ _ _ _ _ _ L _ _ _ _ ~ _ _ _ _ L _ _ _ _ _ L _ _ _ ~ _ _ _ _ _ L _ _ _ _ ~--~ 0 5 1 0 1 5 2 0 2 5 3 0 3 5 4 0 4 5 L o n g i t u d i n a l d i s t a n c e / X e - [ m ] F i g u r e 3 . 8 - P a t h t r a c k i n g o f a S i n u s o i d a l t r a j e c t o r y w i t h P D c o n t r o l l e r - 4 2 - P a t h t r a c k i n g : S i n o s o i d a l t r a j e c t o r y 15~--~--~--~--~--~--~--~--~~ I 1 0 I - ( \ ' ' '-----------~---{:\------ ' - - - - - - - - · :-----------~---------- - - - - - - - - - - - - - - - - - - - - - - - - - - ~ - , ' ' - I ; I I I i I I E , . . , , , , , • . , ~ . r i i i _ _ _ _ _ t _ L ·1 l -~ _ _ d l 5 - ~ · - . - -, - . - - - · ' ' ' ' > - • • ' ' ' ' • ' ' ' • : : : ' . : : G l : : 4 J I . , , , : . : , ; , , , ; : : : : • : ' . . a : : o c : I · , , , - - : ,a : - , - . . • ' ! I t ' ' • ' • ' c a . . • : " a : : : . : : • • : . . . . ' ~· ' ' • ' t l • i - - - - - - - - - 1 - - - - - - - - - ) - - - - - - - - - j - - - - - - - - { - - - - - - - - - : - - - - - - -l - - - - - - - - - - - - - - Jr · - - - - - Q ) ' ' ' ' " \ t . i i i : ' : ; I : : . / ' . . . J : : · : • : : :~ · : i : ~\ : ; : i i : ~· i - s l - - - - _ _ _ _ J _ - - - - - - - - - r ~-1 -- - - > - - - t - r · - - - ------~ - - - - - - j lc - - - - - - - : - - - - - - - - - - . \ ' ~ ' ' ' \ \ ' ' . . ' ' ' "\ ' i I i i : - · ' ' ' . . ' ' ' ' ' ' ' - 1 0 1 I ' - I , L I ' " ' I I 0 5 1 0 1 5 2 0 2 5 3 0 3 5 4 0 4 5 L o n g i t u d i n a l d i s t a n c e / X e - [ m ] F i g u r e 3 . 9 - P a t h t r a c k i n g o f a S i n u s o i d a l t r a j e c t o r y w i t h F u z z y c o n t r o l l e r E r r o r i n L a t e r a l d i r e c t i o n 1 .2 ,----,----,----,---,.----,---,,----,---.----~ 0 . 8 . . . 0 . 6 ~ ~~ - i 0 . 4 . ! ! ? . . ' Q ) E 0 . 2 i = . . I t " . . , . ' . . . . , : 4 . · . , . c . • • • • 8 . . , • ~ ' · . . . \,.~ ~ i l \ . : . . • • : ( . , ; · . ; v · : r , " ' : · · · . . ! - , - . ; . . i 0 ! i . . . . ~ l i ' : - ' i · : . : • • • • · ! 4 . . 1 I \ • • 7 • . . ~21 " - { ) _ 4 I - < l . 6 0 5 1 0 1 5 : " . . . . . 2 0 2 5 L o n g i t u d i n a l e r r o r - [ m ] . . . " 3 0 3 5 4 0 4 5 F i g u r e 3 . 1 0 - E r r o r i n L a t e r a l d i r e c t i o n o f a S i n u s o i d a l t r a j e c t o r y w i t h F u z z y C o n t r o l l e r - 4 3 - 0 . 6 0 . 4 0 . 2 J( 0 ! - 0 . 2 I C D E - 0 . 4 i = - 0 . 6 I • " - . . . - 0 . 8 ~ " " - 1 1 - • • . . : t ) , . . . . . : \ ~-;;.;. ) . . . . E r r o r i n l o n g i t u d i n a l d i r e c t i o n . , . . : ? . : ' : \ . -~ . t · · \ . . ; . ~·!\ / ' 1 . : ~ , ; : ~ J . . • . . " . . : • ' . ' . \ I \ • • . . . . . ~ \ ( . • l . . · . , . " \ : " . . . . : ; \ . . ~· ~ . . . . · : r : · ~. , . ~ ! C : , . , . · i - 1 . 2 L _ _ _ _ L _ _ _ _ J L _ _ _ l _ _ _ _ _ l _ _ _ l _ _ _ _ _ l _ _ _ L _ _ _ _ _ L _ _ _ _ j 0 5 1 0 1 5 2 0 2 5 3 0 3 5 4 0 4 5 l o n g i t u d i n a l e r r o r - [ m ] F i g u r e 3 . 1 1 - E r r o r i n L o n g i t u d i n a l d i r e c t i o n o f a S i n u s o i d a l t r a j e c t o r y w i t h F u z z y c o n t r o l l e r P a t h t r a c k i n g : s t r a i g h t t r a j e c t o r y 4 0 3 5 - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - _ _ _ _ _ _ _ _ _ _ _ _ . _ _ _ _ _ _ _ _ 3 0 - - - - - - - - - - · - - - - - - - - - - - - ' ' _ _ _ _ _ J _ _ _ _ _ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - _ _ , _ ' ' ' ' . s c D 2 5 - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - _ _ _ - - - - - , - - - - - - - - - - - - - - - - - - - - - - > - Q ; 0 f i i 2 0 ' ' - - - - - - - - . - - - - - - - - - - - - · - - - - - - - -- - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - i i i ' 0 1 ! 1 5 Q ) - " ' . . J ' ' - - - - - - - - - - - - - - - - - - - - , - - - - - - - - - - - · · - · · - - - - - - - , - - - - - · - - - - - - - - - - - - - - - - - ' ' ' ' ' ' ' ' ' ' ' ' ' ' - - - - - - - - - - - - - - - - - - - - - - - - - · · - - - - - - - ' ' 1 0 ' ' ' ' - - · - - - · - . . - - - - - - - - - - - - . - - - - - - - - - - · - - - - - - - - - - - - - - - - - - - - - - 5 ' ' _ _ _ _ _ _ _ _ _ . . _ _ _ _ _ _ _ _ _ _ _ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - ' ' ' ' ' ' ' ' ' ' ' ' ' ' ' ' ' ' 5 1 0 1 5 2 0 2 5 3 0 3 5 4 0 4 5 L o n g i t u d i n a l d i s t a n c e / Y e - [ m ] F i g u r e 3 . 1 2 - P a t h t r a c k i n g o f a S t r a i g h t t r a j e c t o r y w i t h F u z z y c o n t r o l l e r - 4 4 - 3 . 4 S u m m a r y T h i s c h a p t e r m a i n l y d e s c r i b e d a b o u t d e s i g n i n g a n d s i m u l a t i n g a f u z z y l o g i c - b a s e d n a v i g a t i o n a l c o n t r o l l e r f o r u n m a n n e d s u r f a c e v e h i c l e s . A n a l r e a d y d e v e l o p e d d y n a m i c m o d e l o f a b o a t i s u t i l i z e d f o r t h e s i m u l a t i o n s t h o u g h o u t t h e r e p o r t . T h e c o n t r o l l e r c o n s i d e r e d i n t h i s s t u d y i s a f u z z y b a s e d s y s t e m . M a t L a b f r a m e w o r k w i t h F u z z y - l o g i c t o o l b o x w a s u s e d t o d e s i g n a n d i m p l e m e n t i n g t h e w h o l e s y s t e m . M a t L a b p r o g r a m s w e r e e m p l o y e d f o r t h e n a v i g a t i o n a l c o n t r o l l e r s i m u l a t i o n s . T h e b a s i c d e s i g n p r o c e d u r e a n d t h e s i m u l a t i o n p r o c e d u r e w e r e i n c l u d e d i n d e t a i l . T h e r e s u l t s i n c l u d i n g t h e d e s i r e d a n d a c t u a l p a t h s a r e p l o t t e d a t t h e e n d . - 4 5 - C h a p t e r 4 S t a t i c O b s t a c l e A v o i d a n c e o f U S V 4 . 1 U t i l i z i n g G r o u n d V e h i c l e T e c h n o l o g i e s f o r S u r f a c e V e h i c l e s 4 . 1 . 1 D e s i g n o f O A c o n t r o l l e r O b s t a c l e a v o i d a n c e c o n t r o l l e r i s e s s e n t i a l - f o r a u t o n o m o u s , s e m i - a u t o n o m o u s n a v i g a t i o n o r a s a d r i v e r a s s i s t a n c e s y s t e m t o e n c o u n t e r t h e c o l l i s i o n s i t u a t i o n s f o r s a f e n a v i g a t i o n . T h e n a v i g a t i o n c o n t r o l l e r a n d t h e o b s t a c l e a v o i d a n c e c o n t r o l l e r a r e s y n t h e s i z e d i n d e p e n d e n t l y b u t o p e r a t e c o m b i n e d s i n c e b o t h c o n t r o l l e r s a r e e s s e n t i a l t o e n s u r e s a f e n a v i g a t i o n . T h i s O A c o n t r o l l e r i s a l s o i m p l e m e n t e d i n M a t l a b f r a m e w o r k , u s i n g f u z z y l o g i c t o o l b o x a n d F u z z y I n f e r e n c e s y s t e m s ( F I S ) e d i t o r G U I . T r a p e z o i d a l m e m b e r s h i p f u n c t i o n s a r e s e l e c t e d a s t h e i n p u t m e m b e r s h i p f u n c t i o n s . S u g e n o t y p e i n f e r e n c e s y s t e m i s u s e d f o r t h e c o n t r o l l e r s y n t h e s i s [ 7 ] . 4 . 1 . 1 . 1 I n p u t F u n c t i o n s T w o a u x i l i a r y i n p u t f u n c t i o n s a r e d e v e l o p e d t o b e u s e d a s t h e i n p u t s t o t h e f u z z y c o n t r o l l e r . T h e y a r e a ) C o l l i s i o n d i r e c t i o n f u n c t i o n a n d b ) R e l a t i v e d i s t a n c e f u n c t i o n b e t w e e n t h e o b s t a c l e ( s t a t i o n a r y o r d y n a m i c ) a n d t h e v e h i c l e C o l l i s i o n d i r e c t i o n f u n c t i o n i s u s e d t o t a k e t h e o b s t a c l e d i r e c t i o n w i t h r e s p e c t i v e t o t h e b o a t h e a d i n g . I 1 e a d i n g a n g l e o f t h e b o a t a n d o b s t a c l e a n g l e w i t h r e s p e c t i v e t o t h e e a r t h a r e u s e d t o f i n d t h i s c o l l i s i o n d i r e c t i o n i n t h e M a t L a b s i m u l a t i o n . C o l l i s i o n d i r e c t i o n a n g l e i s m e a s u r e d f r o m t h e Y a x i s o f t h e b o a t f r a m e . F i g u r e 4 . 1 a n d 4 . 2 s h o w t h e m e a s u r e m e n t o f c o l l i s i o n d i r e c t i o n a n g l e . C a l c u l a t i o n w a s c o r r e c t e d f o r t h e w h o l e C a r t e s i a n p l a n . T h e c o l l i s i o n d i r e c t i o n a n g l e i s d e f i n e d a s - 4 6 - f 3 = 9 0 + 8 - a - - - - - - - - - - - - - - - - - - - - - - - ( 4 . 1 ) W h e r e e i s h e a d i n g a n g l e o f t h e b o a t a n d a i s o b s t a c l e a n g l e . y ' - . e " \ \ Y B o a t I I I I I ( \ \ 1 a~~ O b s t a c l e X B o a t I I I I F i g u r e 4 . 1 - C a l c u l a t i n g c o l l i s i o n d i r e c t i o n X - 4 7 - . . . . y a ~ 8 \ ' , Y B o a t X B o a t I ( \ ' - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - 0 O b s t a c l e X F i g u r e 4 . 2 - C a l c u l a t i n g c o l l i s i o n d i r e c t i o n j J c h a n g e s n e a r o b s t a c l e a l o n e a d e s i r e d p a t h i s p r e s e n t e d i n F i g u r e 4 . 4 . T h a t s t r a i g h t p a t h o f t h e b o a t i s g i v e n b e l o w ( F i g u r e 4 . 3 ) . - 4 8 - 2 o o o 1 - - - - - - - ~----- - - - - - - - - - - - - - - , - - - - - - - - - 1 8 0 0 1 6 0 0 1 4 0 0 I ! 1 2 0 0 . 1 0 0 0 1 - 8 0 0 V ' ' ----~------- _ _ L _ _ _ ~-- 6 0 0 7 0 0 8 0 0 9 0 0 1 0 0 0 1 1 0 0 1 2 0 0 1 3 0 0 1 4 0 0 1 5 0 0 1 6 0 0 F i g u r e 4 . 3 - B o a t s p a t h n e a r o b s t a c l e w i t h o u t O b s t a c l e a v o i d a n c e c o n t r o l l e r 7 6 · 5 . 0 4 - Q ) I ! ! } _ , I Q ) ~ 3 , 1 - I 2 1 - o l _ _ 0 B e t a c h a n g e s a l o n e t h e p a t h \ " - - - - - - - 5 0 1 0 0 ~\ 1 5 0 A n g l e - [ r a d ] 2 0 0 F i g u r e 4 . 4 - j J c h a n g e s n e a r o b s t a c l e J 2 5 0 3 0 0 - 4 9 - X X Y X N Y Y N Y N X Y N Y N X N X F i g u r e 4 . 5 - N a m e s o f c o l l i s i o n d i r e c t i o n i n p u t m e m b e r s h i p f u n c t i o n D e g r e e o f m e m b e r s h i p Y X X X N Y N Y N Y N X N X N X Y y ( I 7 r 3 6 3 6 2 3 6 3 5 7 r 7 r 3 7 7 r 3 6 3 6 5 3 7 r 3 7 r 5 5 7 r 3 6 2 3 6 7 1 7 r 2 7 r 3 6 F i g u r e 4 . 6 - C o l l i s i o n d i r e c t i o n i n p u t m e m b e r s h i p f u n c t i o n t o t h e o b s t a c l e a v o i d a n c e c o n t r o l l e r E i g h t d i t i e r e n t m e m b e r s h i p f u n c t i o n s a r e d e f i n e d f o r t h e c o l l i s i o n d i r e c t i o n i n p u t f u n c t i o n . T r i a n g u l a r f u n c t i o n s w h i c h a r e h a v i n g 1 0 ° w i t h w e r e u s e d t o i n p u t c o l l i s i o n d i r e c t i o n s o n m a i n a x e s o f t h e b o a t f i x e d f r a m e . C e n t r e s o f t h o s e f u n c t i o n s a r e 0 , I 1 / 2 . n . 3 T I / 2 a n d 2 T I r e s p e c t i v e l y . T r i a n g u l a r a n d t r a p e z o i d a l f u n c t i o n s a r e c h o s e n a n d t h e s e f u n c t i o n s c a n b e i m p l e m e n t s w i t h o u t m a n y c o m p l i c a t i o n s . T h e a p p r o a c h o f n a m i n g t h e m e m b e r s h i p f u n c t i o n i s g i v e n i n F i g u r e 4 . 6 w h i l e F i g u r e 4 . 5 p r e s e n t s t h e m e m b e r s h i p f u n c t i o n s o f t h e C o l l i s i o n d i r e c t i o n f u n c t i o n . T h e r a d a r m a p s a n d s t e r e o Y i s i o n c a n b e u s e d t o f i n d t h i s c o l l i s i o n d i r e c t i o n i n r e a l w o r l d s i t u a t i o n s [ 1 4 ] . - 5 0 - 4 . 1 . 1 . 1 . 2 R e l a t i v e D i s t a n c e F u n c t i o n R e l a t i v e d i s t a n c e t o w a r d s o b s t a c l e i s c a l c u l a t e d b y m e a n s o f c o o r d i n a t e s i n t h e c o o r d i n a t e f r a m e s y s t e m [ 3 3 ] . T h i s i s t h e E u c l i d i a n d i s t a n c e b e t w e e n t h e c e n t e r o f g r a v i t y o f t h e b o a t a n d t h e o b s t a c l e . T w o t r i a n g u l a r m e m b e r s h i p f u n c t i o n s a r e d e f i n e d a s " L o w " a n d " H i g h " ( F i g u r e 4 . 7 ) . L o w 0 8 ~ j 0 6 ~ 0 ~ 0 4 0 0 2 5 R e l - d r s t a n c e H t g h 1 0 F i g u r e 4 . 7 - R e l a t i v e d i s t a n c e i n p u t m e m b e r s h i p f u n c t i o n t o t h e o b s t a c l e a v o i d a n c e c o n t r o l l e r T h e x - s c a l e o f t h e s e f u n c t i o n s m a y s u b j e c t t o c h a n g e s a c c o r d i n g t o t h e d i m e n s i o n s a n d s p e e d o f t h e c o n s i d e r e d b o a t p l a t f o r m , b u t t h e d e s i g n a n d s i m u l a t i o n a l g o r i t h m r e m a i n s a m e . I n p u t s f o r t h e r e l a t i v e d i s t a n c e f u n c t i o n a r e d e f i n e d i n b e t w e e n 0 t o I O O m f o r t h e s i m u l a t i o n . I t m e a n s t h a t " L o w " f u z z y s e t i s d e f i n e f r o m 0 t o 5 0 m w h i l e · ' H i g h " i s d e f i n e f r o m 5 0 m t o I O O m . 4 . 1 . 1 . 2 R u l e b a s e o f t h e c o n t r o l l e r r h e r u l e s f o r t h e c o n t r o l l e r a r e g i v e n b e l o w . X d o t a n d Y d o t a r e t h e v e l o c i t y c o m p o n e n t s o f t h e m a i n d i r e c t i o n . H e r e " D D D " p r e s e n t s t h e m a x i m u m v e l o c i t y o f t h e b o a t w h i l e " D D " a n d " D " p r e s e n t s t h e 2 / 3 a n d 1 / 3 o f t h e m a x i m u m v e l o c i t y c o r r e s p o n d i n g l y . N e g a t i v e v e l o c i t i e s o f t h e a b o v e v e l o c i t i e s a r e a p p e a r e d a s . . N D D D " , " N D D " a n d " N D " . - 5 1 - 1 . I f ( C o l - d i r e c i s Y ) a n d ( R e i - d i s t a n c e i s H i g h ) t h e n ( X d o t i s Z e r o ) a n d ( Y d o t i s N D ) 2 . I f ( C o l - d i r e c i s Y ) a n d ( R e i - d i s t a n c e i s H i g h ) t h e n ( X d o t i s Z e r o ) a n d ( Y d o t i s N D ) 3 . I f ( C o l - d i r e c i s Y ) a n d ( R e i - d i s t a n c e i s L o w ) t h e n ( X d o t i s D ) a n d ( Y d o t i s N D D ) 4 . I f ( C o l - d i r e c i s Y ) a n d ( R e i - d i s t a n c e i s L o w ) t h e n ( X d o t i s D ) a n d ( Y d o t i s N D D ) 5 . I f ( C o l - d i r e c i s N Y ) a n d ( R e i - d i s t a n c e i s H i g h ) t h e n ( X d o t i s Z e r o ) a n d ( Y d o t i s D ) 6 . I f ( C o l - d i r e c i s N Y ) a n d ( R e i - d i s t a n c e i s L o w ) t h e n ( X d o t i s D ) a n d ( Y d o t i s D D ) 7 . I f ( C o l - d i r e c i s N X ) a n d ( R e i - d i s t a n c e i s H i g h ) t h e n ( X d o t i s D ) a n d ( Y d o t i s Z e r o ) 8 . I f ( C o l - d i r e c i s N X ) a n d ( R e i - d i s t a n c e i s L o w ) t h e n ( X d o t i s D D ) a n d ( Y d o t i s D ) 9 . I f ( C o l - d i r e c i s X ) a n d ( R e i - d i s t a n c e i s H i g h ) t h e n ( X d o t i s N D ) a n d ( Y d o t i s Z e r o ) 1 0 . I f ( C o l - d i r e c i s X ) a n d ( R e i - d i s t a n c e i s L o w ) t h e n ( X d o t i s N D D ) a n d ( Y d o t i s D ) 1 1 . I f ( C o l - d i r e c i s N X Y ) a n d ( R e l - d i s t a n c e i s L o w ) t h e n ( X d o t i s D ) a n d ( Y d o t i s N D D ) 1 2 . I f ( C o l - d i r e c i s N X Y ) a n d ( R e l - d i s t a n c e i s H i g h ) t h e n ( X d o t i s Z e r o ) a n d ( Y d o t i s N D ) 1 3 . I f ( C o l - d i r e c i s N X N Y ) a n d ( R e l - d i s t a n c e i s L o w ) t h e n ( X d o t i s D D ) a n d ( Y d o t i s D ) 1 4 . I f ( C o l - d i r e c i s N X N Y ) a n d ( R e l - d i s t a n c e i s H i g h ) t h e n ( X d o t i s Z e r o ) a n d ( Y d o t i s D ) 1 5 . I f ( C o l - d i r e c i s Y X ) a n d ( R e l - d i s t a n c e i s H i g h ) t h e n ( X d o t i s Z e r o ) a n d ( Y d o t i s N D ) 1 6 . I f ( C o l - d i r e c i s Y X ) a n d ( R e l - d i s t a n c e i s L o w ) t h e n ( X d o t i s N D ) a n d ( Y d o t i s N D D ) 1 7 . I f ( C o l - d i r e c i s X N Y ) a n d ( R e l - d i s t a n c e i s H i g h ) t h e n ( X d o t i s Z e r o ) a n d ( Y d o t i s D ) 1 8 . I f ( C o l - d i r e c i s X N Y ) a n d ( R e l - d i s t a n c e i s L o w ) t h e n ( X d o t i s N D ) a n d ( Y d o t i s D D ) 4 . 1 . 1 . 3 O u t p u t F u n c t i o n s O n l y t w o o u t p u t m e m b e r s h i p f u n c t i o n s a r e u s e d t o r e d u c e t h e c o m p l e x i t y o f t h e c o n t r o l l e r . T h e y a r e 1 ) V e l o c i t y c o m p o n e n t a l o n g x a x i s ( x ) 2 ) V e l o c i t y c o m p o n e n t a l o n g y a x i s ( y ) O u t p u t s u r f a c e o f t h e x i s g i v e n i n F i g u r e 4 . 8 a n d o u t p u t s u r f a c e o f t h e y i s g i v e n i n F i g u r e 4 . 9 . T h o s e v e l o c i t i e s a r e g i v e n w i t h r e s p e c t t o t h e b o a t f r a m e a n d t h e y a r e c o n v e r t e d t o t h e w o r l d f r a m e b e f o r e c a l c u l a t i n g t h e d e s i r e d b o a t p o s i t i o n s . T h e c o n v e r s i o n m a t r i x i s g i v e n b e l o w : - 5 2 - 0 " 0 X 1 = ( c o s ( ) - s i n e ) s i n ( ) c o s ( ) ) 0 . 8 0 . 6 0 . 4 I o . 2 I 0 - 0 2 . . ! - 0 . 4 1 1 0 5 R e i - d i s t a n c e ( 5 ) 6 4 0 0 C o l - d i r e c t i o n F i g u r e 4 . 8 - X d i r e c t i o n v e l o c i t y o u t p u t s u r f a c e o f t h e o b s t a c l e a v o i d a n c e c o n t r o l l e r - 5 3 - 0 5 _ g 0 > - - 0 . 5 1 0 R e l - d i s t a n c e 0 0 C o l - d i r e c t i o n F i g u r e 4 . 9 - Y d i r e c t i o n v e l o c i t y o u t p u t s u r f a c e o f t h e o b s t a c l e a v o i d a n c e c o n t r o l l e r T h e a l g o r i t h m f o r p a t h p l a n n i n g w i t h o b s t a c l e a v o i d a n c e i s g i v e n i n f i g u r e 4 . 1 0 . T h i s a l g o r i t h m w a s d e v e l o p e d a s a m o d u l e i n M a t L a b . T h a t m o d u l e c a n b e r e u s e d f o r f u t u r e w o r k s . F e w r e s u l t s f r o m t h e s i m u l a t i o n s a r e p r e s e n t e d a t t h e e n d o f t h i s c h a p t e r . - 5 4 - 4 . 1 . 2 A l g o r i t h m s f o r s i m u l a t i o n o f t h e c o n t r o l l e r + E v a l u a t e f u z z y o u t p u t s F u z X , F u z Y - f v , = F u z X x v x c o s B v v = F u z Y x v x s i n B D e f i n e G o a l , St~rt~~dl O b s t a c l e p o i n t s D o u n t i l t h e b o a t r e a c h t h e G o a l . . . I n d e x = i n d c x + 1 C a l c u l a t e B o a t ( B ) , O b s t a c l e ( a ) a n d G o a l d i r e c t i o n a n g l e s . • j J = 9 0 + 8 - a t C a l c u l a t e r e l a t i v e d i s t a n c e ( R D ) ~ v x = v x c o s e v = v x s i n B . . . x n e w = x o / d + v , . X T i m e l n t e r v a l Y n e w = Y o / ( / + v y X T i m e l n t e r v a l j . P l o t t h e n e w p o i n t o n t h e p l a n • C a l c u l a t e X d e s r r e d ' Y d e s r r c d X d c s 1 r c d ' Y d e s i r e d X d e s i r e d ' Y d e s r r e d t S t o r e a b o v e v a l u e s w i t h t i m e F i g u r e 4 . 1 0 - A l g o r i t h m o f t h e o b s t a c l e a v o i d a n c e c o n t r o l l e r - 5 5 - B l o c k d i a g r a m o f t h e s i m u l a t i o n p r o g r a m i s s h o w n i n F i g u r e 4 . 1 1 . T h e a l g o r i t h m o f t h e p a t h p l a n n i n g m o d u l e i s g i v e n i n F i g u r e 4 . 1 0 . T h e m a i n m o d u l e i s d e f i n e d t h e t o t a l t i m e s p a n , i n i t i a l c o n f i g u r a t i o n o f t h e b o a t , g o a l p o i n t a n d t h e o b s t a c l e p o i n t s . T h e n t h o s e d a t a i s s e n t t o t h e P a t h P l a n n i n g M o d u l e . T h e t i m e s p a n , d e s i r e d p a t h a n d b o a t ' s c o n f i g u r a t i o n s a r e s e n t t o t h e B o a t ' s D y n a m i c S i m u l a t i o n M o d u l e . T h e n t h e a c t u a l p a t h a n d c o n f i g u r a t i o n s o f t h e b o a t i s t a k e n b y t h e M a i n m o d u l e t o p l o t t h e a c t u a l p a t h s o f t h e b o a t . R e s u l t s f r o m t h i s m o d u l e a r e p r e s e n t e d f r o m F i g u r e 4 . 1 2 t o 4 . 1 6 . R e l a v e n t M a t L a b p r o g r a m i s g i v e n w i t h A p p e n d i x C . P a t h P l a n n i n g M o d u l e T i m e s p a n I n i t i a l c o n f i g u r a t i o n o f t h e b o a t G o a l p o i n t O b s t a c l e p o i n t s M a i n M o d u l e P a t h P l o t t i n g M o d u l e T i m e s p a n D e s i r e d p a t h a n d b o a t ' s c o n f i g u r a t i o n s B o a t ' s D y n a m i c S i m u l a t i o n M o d u l e A c t u a l c o n f i g u r a t i o n s o f t h e b o a t F i g u r e 4 . 1 1 - S i m u l a t i o n s e t u p o f t h e o b s t a c l e a v o i d a n c e c o n t r o l l e r - 5 6 - 4 . 1 . 3 S i m u l a t i o n R e s u l t s f r o m t h e C o n t r o l l e r P a t h n e a r o b s t a c l e s 1 0 0 0 9 0 0 G o a l p o i n t 8 0 0 7 0 0 - E ' 6 0 0 . O b s t a c l e Q ) l l c 5 0 0 C 1 l • 1 i ) 1 5 > - 4 0 0 i 3 0 0 2 0 0 1 0 0 S t a r t i n g p o i n t 0 ~---- - _ _ _ ] _ _ _ _ _ _ _ _ _ _ _ l _ [ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ j _ _ _ _ _ _ _ _ _ _ _ _ 0 1 0 0 2 0 0 3 0 0 4 0 0 5 0 0 6 0 0 7 0 0 8 0 0 9 0 0 1 0 0 0 X d i s t a n c e [ m ] F i g u r e 4 . 1 2 - P a t h s t a r t i n g n e a r r o o t o f t h e c o o r d i n a t e s y s t e m P a t h n e a r o b s t a c l e s 1 0 0 0 ~-- 9 0 0 G o a l p o i n t 8 0 0 7 0 0 ' E 6 0 0 ( i ) ( . ) O b s t a c l e c 5 0 0 r o u ; ' 6 > - 4 0 0 • 3 0 0 S t a r t i n g p o i n t 2 0 0 1 0 0 o i L - - - 0 1 0 0 2 0 0 3 0 0 4 0 0 5 0 0 6 0 0 7 0 0 8 0 0 9 0 0 1 0 0 0 X d i s t a n c e [ m ] F i g u r e 4 . 1 3 - P a t h s t a r t i n g n e a r r o o t o f t h e c o o r d i n a t e s y s t e m - 5 7 - P a t h n e a r o b s t a c l e s 1 0 0 0 9 0 0 1 I D o a l p o i n t 8 0 0 7 0 0 1 O b s t a c l e E 6 0 0 . Q ) • u c 5 o o r r o t l i 5 > - 4 0 0 - 3 o o I I 2 0 0 1 1 0 0 L I o t j _ _ _ _ _ _ _ _ _ _ _ _ - - _ _ _ _ : _ _ _ _ _ _ _ _ ~_l _ _ l _ _ _ _ _ _ _ _ j _ _ _ 0 1 0 0 2 0 0 3 0 0 4 0 0 5 0 0 6 0 0 7 0 0 8 0 0 9 0 0 1 0 0 0 X d i s t a n c e [ m ] F i g u r e 4 . 1 4 - P a t h s t a r t i n g n e a r t h e l e f t c o r n e r o f t h e c o o r d i n a t e s y s t e m P a t h n e a r o b s t a c l e s 1 o o o I - - --------~------- -r----~----- · - - - - - - - - - - - - I I 9 0 0 ~ S t a r t i n g p o i n t , / / / 8 0 0 1 I 7 0 0 I ' E 6 0 0 ~ Q ) O b s t a c l e • u 5 0 0 ~ c r o ! U l " 0 > - 4 0 0 3 0 0 ~ G o a l p o i n t I 2 0 0 - 1 0 0 1 ! 0 -~- ~_l_ _ _ _ _ - _ _ , _ _ _ _ _ _ _ [ _ _ _ - - - - - _ _ L _ _ _ _ - - - - - - - - - - - 0 1 0 0 2 0 0 3 0 0 4 0 0 5 0 0 6 0 0 7 0 0 8 0 0 9 0 0 X d i s t a n c e [ m ] F i g u r e 4 . 1 5 - P a t h f i n i s h i n g n e a r r o o t - 5 8 - 1 0 0 0 . . . . 9 5 0 - 9 0 0 ! I 8 5 0 ~ E ' 8 0 0 i l l 0 c 7 5 0 C l l U J u > - 7 0 0 I I 6 5 0 - 6 0 0 1 5 5 0 5 0 0 3 0 0 4 0 0 4 . 1 . 4 S u m m a r y P a t h n e a r o b s t a c l e s G o a l p o i n t O b s t a c l e . . 5 0 0 6 0 0 7 0 0 X d i s t a n c e [ m ] 8 0 0 J - 1 ~ I ' · - - - - _ _ _ j 9 0 0 1 0 0 0 F i g u r e 4 . 1 6 - P a t h s t a r t i n g n e a r o b s t a c l e T h i s c h a p t e r p r e s e n t e d o b s t a c l e a v o i d a n c e a l g o r i t h m s f o r u n m a n n e d s u r f a c e v e h i c l e s w h i c h a r e a l r e a d y d e v e l o p e d f o r u n m a n n e d g r o u n d v e h i c l e s . A p r e v i o u s l y d e v e l o p e d m a t h e m a t i c a l m o d e l a n d a f u z z y - b a s e d n a v i g a t i o n a l c o n t r o l l e r i s u t i l i z e d w i t h o b s t a c l e a v o i d a n c e a l g o r i t h m s , f o r s i m u l a t i o n s . T h i s w a s a l s o i m p l e m e n t e d i n M a t l a b f r a m e w o r k , u s i n g f u z z y l o g i c t o o l b o x a n d f u z z y i n f e r e n c e s y s t e m s ( F I S ) e d i t o r G U I . T r a p e z o i d a l a n d m e m b e r s h i p f u n c t i o n s w a r e s e l e c t e d a s t h e i n p u t m e m b e r s h i p f u n c t i o n s . S u g e n o t y p e i n f e r e n c e s y s t e m i s u s e d f o r t h e c o n t r o l l e r s y n t h e s i s . A l l t h e p r o g r a m s a r e d e v e l o p e d i n t h e M a t L a b e n v i r o n m e n t . O b s t a c l e a v o i d a n c e c o n t r o l i s e s s e n t i a l f o r a u t o n o m o u s , s e m i - a u t o n o m o u s o p e r a t i o n o f v e h i c l e s o r a s a d r i v e r a s s i s t a n c e s y s t e m t o e n c o u n t e r t h e c o l l i s i o n s i t u a t i o n s f o r s a f e n a v i g a t i o n . T h e n a v i g a t i o n c o n t r o l l e r a n d t h e o b s t a c l e a v o i d a n c e c o n t r o l l e r a r e s y n t h e s i z e d i n d e p e n d e n t l y b u t o p e r a t e t o g e t h e r , s i n c e b o t h c o n t r o l l e r s a r e e s s e n t i a l t o e n s u r e t h e t a s k o f s a f e n a v i g a t i o n . - 5 9 - 4 . 2 N o v e l A l g o r i t h m f o r O A 4 . 2 . 1 M e t h o d o l o g y o f t h e N o v e l A l g o r i t h m A l t h o u g h t h e M o r p h i n a l g o r i t h m h a s b e e n u s e d f o r m a n y p r a c t i c a l a p p l i c a t i o n s , n o n e o f t h e d e t a i l e d a l g o r i t h m s h a s i s n o t p u b l i s h e d y e t . S o t h i s s e c t i o n d e s c r i b e s t h e d e v e l o p m e n t o f n o v e l a l g o r i t h m f o r U S V s , b a s e d o n M o r p h i n a l g o r i t h m . I n t h i s m e t h o d s t e r e o c a m e r a s a n d S u r v e i l l a n c e r a d a r s c a n b e u s e t o c a p t u r e o b s t a c l e s i n t h e f i e l d [ 2 1 ] . T h e c a p t u r e d d a t a i s t r a n s f o r m e d i n t o a g r i d . T h e n t h a t g r i d i s u t i l i z e d a s o b s t a c l e m a t r i x t o a v o i d t h a t o b s t a c l e . F i g u r e 4 . 1 7 i s p r e s e n t s a n o b s t a c l e o f t h a t g r i d a n d F i g u r e 4 . 1 8 s h o w s t h e c o r r e s p o n d i n g o b s t a c l e m a t r i x w i t h v a l u e s . O n e s a n d z e r o e s a r e u s e d t o r e p r e s e n t s o b s t a c l e s a n d o b s t a c l e s f r e e a r e a s . T h i s m o r p h i n a l g o r i t h m i s b a s i c a l l y d e v e l o p e d f o r u n m a n n e d g r o u n d v e h i c l e s b u t i t i s n o t p o s s i b l e t o u s e o n e s a n d z e r o e s f o r t h e m . B e c a u s e t h e h e i g h t a n d a p p e a r a n c e o f t h e o b s t a c l e s o n g r o u n d a r e n e e d e d t o s t o r e o b v i o u s l y . B u t t h e c a s e i s s o m e w h a t d i f f e r e n t w i t h t h e s u r f a c e v e h i c l e s b e c a u s e t h e y c a n n o t o v e r c o m e o b s t a c l e s a s g r o u n d v e h i c l e s . S o , i t i s s u f f i c i e n t t o u s e o n e s a n d z e r o e s i n t h e o b s t a c l e m a t r i x . - - - l I I l _ F i g u r e 4 . 1 7 - O b s t a c l e i n t h e g r i d - 6 0 - F i g u r e 4 . 1 8 - O b s t a c l e m a t r i x w i t h o b s t a c l e F i g u r e 4 . 1 9 s h o w s n u m b e r o f a r c s p r o j e c t e d i n f r o n t o f t h e b o a t o v e r t h e l o c a l w o r l d - m o d e l o b s t a c l e m a p . T h e n u m b e r o f a r c s c o n s i d e r e d i s a f u n c t i o n o f t h e m a p s i z e a n d g r i d s p a c i n g , w i t h t h e a r c s s p a c e d s u c h t h a t o n e a r c p a s s e s t h r o u g h e a c h o f t h e o u t e r c e l l s . T h i s a p p r o a c h g u a r a n t e e s t h a t e a c h c e l l i n t h e g r i d i s c o v e r e d b y a t l e a s t o n e a r c s o t h a t a l l n a v i g a b l e p a t h s a r e c o n s i d e r e d . M a t h e m a t i c a l f u n c t i o n s o f t h e c i r c l e s a n d l i n e s a r e u t i l i z e d t o c a l c u l a t e t h e c o o r d i n a t e s o f t h e p a t h s . T h e y a r e p u t i n t o a p a t h m a t r i x w i t h a p a t h i n d e x . S t r a i g h t l i n e f u n c t i o n s a r e e m p l o y e d f o r t h e m i d d l e p a t h a s t h e i r r a d i u s i s t e n t t o i n f i n i t y . . . . . . r t . I 1 - - - - - _ _ _ j • I I ~ I L . . r - - " " • c " 1 - - - - I - - l - Y · J J ! - - - 1 . . F i g u r e 4 . 1 9 - P o s s i b l e p a t h s o f t h e U S V - 6 1 - T h e p r o j e c t e d p a t h s a n d t h e o b s t a c l e m a t r i x i s s h o w n i n F i g u r e 4 . 2 0 . H e r e t h e p a t h s o n t h e o b s t a c l e s a r e a s s i g n e d s m a l l w e i g h t w h i l e o t h e r p a t h s a r e a s s i g n e d u s i n g h i g h w e i g h t s . A c u r t a i n w e i g h t v a l u e s a r e f u r t h e r a d d e d f o r t h e p a t h s h e a d i n g t o w a r d s t h e g o a l a s w e l l . S o , t h e b e s t p a t h i s c h o s e n d e p e n d i n g o n t h e w e i g h t s a s s i g n e d a t t h a t t i m e . T h e n t h e b o a t m o v e s a c e r t a i n d i s t a n c e a l o n g t h a t p a t h . A g a i n , t h e n e w o b s t a c l e m a t r i x i s p r o c e s s e d a n d o b s t a c l e m a t r i x i s a d d e d t o t h e s y s t e m f o r o t h e r i t e r a t i o n s . . oT j ---1-~~ ' / j _ _ I : / - ; · . ·~L~ . : ~'= - _ _ _ _ - c . ' - c = - 0 • / ' c - . _ _ _ , Y n e w ) - . : . . . . F i g u r e 4 . 2 2 - P a t h v a l u e c a l c u l a t i o n G L G L P a t h v a l ( i ) = - - ( G d , + k ) ( T d , + k ) - - - - - - - - - - - - - - - - - - - - - - - - - - - ( 4 . 2 ) . J 5 r ; : : v v h e r e - G L > T d , > 0 , ' . ! 2 G L > G d 1 > 0 2 E q u a t i o n 4 . 2 i s u t i l i z e d t o c a l c u l a t e t h e p a t h v a l u e s . 1 0 i s a s s i g n e d i n t o t h e c o n s t a n t k f o r t h e s i m u l a t i o n s i n t h i s s t u d y . H e r e t h e p a t h s o n t h e o b s t a c l e s a r e a s s i g n e d s m a l l \ a l u e s w h i l e o t h e r p a t h s a r e p r e a r r a n g e d u s i n g h i g h v a l u e s . A c u r t a i n w e i g h t v a l u e s a r e f u r t h e r a d d e d f o r t h e p a t h s h e a d i n g t o w a r d s t h e g o a l a s w e l l . S o t h e b e s t p a t h i s c h o s e n d e p e n d i n g o n t h e w e i g h t s a r e a s s i g n e d a t t h a t t i m e [ 6 ] . - 6 4 - T h e n t h e b o a t i s m o v e d c e r t a i n d i s t a n c e o n t h a t p a t h . A g a i n t h e n e w o b s t a c l e m a t r i x i s p r o c e s s e d a n d o b s t a c l e m a t r i x i s a d d e d t o t h e s y s t e m f o r o t h e r i t e r a t i o n o f t h e s a m e p a t h . 4 . 2 . 2 S i m u l a t i o n o f A l g o r i t h m s . I n i t i a l l y t h e N o v e l p a t h p l a n n e r t a k e s t h e g r i d s i z e , s u b g r i d s i z e , g o a l p o i n t a n d o b s t a c l e m a p . T h e n t h e c o o r d i n a t e s o f t h e c u r v e d p a t h s a r e e v a l u a t e d a n d a s s i g n e d i n t o t h e Y p a t h s m a t r i x w i t h p a t h i n d e x . I t w a s s t r a i g h t f o r w a r d w a y t o f i n d o b s t a c l e s o n e a c h p a t h . B i g g a p s w e r e f o r m e d w h e n y c o o r d i n a t e s o f t h e s t r a i g h t l i n e f u n c t i o n s w e r e u t i l i z e d . S o t h a t a n o t h e r m a t r i x f o r t h e s t r a i g h t l i n e f u n c t i o n s i s d e f i n e d f o r t h e s a c k o f i n c o n v e n i e n c e . T h e n x c o o r d i n a t e s o f t h e s t r a i g h t l i n e s a r e s t o r e d i n X p a t h s . E v a l u a t i o n o f t h e g o a l m a t r i x i s d o n e b y c o m p a r i n g t h e o b s t a c l e m a t r i x w i t h t h e p a t h m a t r i x e s . M i n i m u m d i s t a n c e t h a t b o a t c a n t r a v e l o n e a c h p a t h i s s t o r e d i n t h e D i s t a n c e _ M a t r i x f u n c t i o n . T h e a b o v e p r o c e s s i s c o n t i n u e d f o r t h e e a c h a n d e v e r y p a t h . T h e n t h e g o a l d i r e c t i o n i s e v a l u a t e d a n d t h a t v a l u e i s a l s o e m p l o y e d w i t h t h e o b s t a c l e l i b e r a t e d d i s t a n c e t o c h o o s e t h e o p t i m u m p a t h a m o n g e a c h a n d e v e r y p o s s i b l e p a t h . T h e b o a t i s t r a v e l e d 1 ; 4 o f t h e g r i d s i z e o n t h e c h o s e n p a t h t h e i r a f t e r . T h a t w h o l e p r o c e s s i s p r e s e n t e d i n s i m p l y f o r t h e c o n v e n i e n c e o f t h e r e a d e r . T h e a b o v e p r o c e s s i s d e v e l o p e d a s a s i n g l e m o d u l e . A n o t h e r m o d u l e i s n e e d e d t o c a l l t h a t m o d u l e s e q u e n t i a l l y u n t i l t h e c h o s e n b o a t r e a c h e s t h e g o a l . T h a t a l g o r i t h m i s a p p e a r e d i n t h e F i g u r e 4 . 2 3 . - 6 5 - T a k e s g r i d s i z e , s u b g r i d s i z e , p a t h i n d e x , G o a l p o i n t s , O b s t a c l e m a p • C r e a t e c u r v e d path~ Y p a t h s ( p a t h i n d e x , < F o r a l l x > ) = y C r e a t e s t r a i g h t p a t h s t=i~Ln . • - r l t a ~__; ' ~~· ,ru.~=-j-- ' 1 . - i ' ' l I X p a t h s ( p a t h i n d e x , < F o r a l l y > ) = x N o E v a l u a t e D i s t a n c e _ M a t r i x ( p a t h i n d e x ) = M i n { O b s t a c l e d i s t a n c e } f g r i d a r e a i s c o v e r e d Y e s E v a l u a t e g o a l d i r e c t i o n C h o o s e t h e o p t i m u m p a t h T M o v e % o f t h e g r i d s i z e ( O n t h e c h o s e n p a t h ) F i g u r e 4 . 2 3 - S i m p l i f i e d f l o w c h a r t o f t h e N o v e l a l g o r i t h m - 6 6 - C h a p t e r 5 D y n a m i c O b s t a c l e A v o i d a n c e o f U S V O b s t a c l e a v o i d a n c e w i t h o u t d y n a m i c o b s t a c l e i s n o t p r a c t i c a l w i t h o b s t a c l e s o n S e a . D y n a m i c o b s t a c l e a v o i d a n c e i s d i s c u s s e d i n t h i s c h a p t e r i n d e t a i l . S i m u l a t i o n r e s u l t s r e l e v a n t t o d y n a m i c o b s t a c l e a v o i d a n c e i s d i s c u s s e d i n t h e n e x t c h a p t e r . 5 . 1 I n t r o d u c t i o n t o N o v e l D y n a m i c O b s t a c l e A v o i d a n c e M e t h o d P r o j e c t e d o b s t a c l e a r e a m e t h o d i s a v e r y f a m o u s m e t h o d f o r d y n a m i c o b s t a c l e a v o i d a n c e . D y n a m i c o b s t a c l e s c a n f r e e z e t i m e w i t h t h e h e l p o f t h a t m e t h o d . T h e n a n y t y p e o f s t a t i c o b s t a c l e a v o i d a n c e m e t h o d c a n u t i l i z e t o a v o i d t h a t . T h i s m e t h o d i s e m p l o y e d i n s e v e r a l p l a c e s o n g r o u n d a n d w a t e r . D y n a m i c o b s t a c l e i s t r a n s f o r m e d t o a n o t h e r s t a t i c o b s t a c l e w h i c h i s h a v i n g l a r g e d i m e n s i o n s i n t h e a b o v e m e t h o d . T h a t m e a n s i t u t i l i z e d t h e e f f e c t i v e a r e a o f t h e p a t h p l a n n i n g p l a n w h i c h c a n b e e m p l o y e d f o r p a t h p l a n n i n g . T h a t m a y b e c o u r s e d t o p l a n i n e f f i c i e n t p a t h s t o a v o i d d y n a m i c o b s t a c l e s . T h e n U S V m a y a b l e t o t r a v e l l o n g e r o r i t m a y s t o p s s u d d e n l y d u e t o l a c k o f e f f e c t i v e a r e a s o n t h e p a t h p l a n n i n g p l a n . S o i t i s v e r y v i t a l t o u t i l i z e t h e t r a v e l i n g a r e a e f f e c t i v e l y a s w e l l a s a v o i d i n g d y n a m i c o b s t a c l e s . S o t h e n o v e l m e t h o d i s p r o p o s e d t o a v o i d D y n a m i c o b s t a c l e s b y e m p l o y i n g t h e m i n i m u m a r e a s o n t h e e f f e c t i v e a r e a o f t h e p a t h p l a n n i n g p l a n e . • • ~- , , ; I F i g u r e 5 . 1 - T w o D y n a m i c o b s t a c l e s w i t h P r o j e c t e d O b s t a c l e A r e a s - 6 7 - F i g u r e 5 . 1 p r e s e n t s t w o d y n a m i c o b s t a c l e s w h i c h a r e f r e e z e d w i t h t i m e . T h o s e t w o p r e d i c t e d a r e a s h a v e b l o c k e d t h e e f f e c t i v e p a t h f r o m t h e i r e d g e s . T h e f o l l o w i n g F i g u r e 5 . 2 s h o w s t h e a d v a n t a g e o f r e d u c i n g t h a t p r e d i c t e d a r e a s . S o t h e p a t h p l a n n e r i s g e n e r a t e d t h e e f f i c i e n t p a t h b e t w e e n t w o o b s t a c l e a r e a s . • • ~ I ' I 1 - I F i g u r e 5 . 2 - T w o D y n a m i c o b s t a c l e s w i t h t h e i r r e d u c e d p r o j e c t e d o b s t a c l e a r e a s / - - I 1 I I / 2 / , . . . . . . - - . . - - " " " ' E n d p o i n t I I / F i g u r e 5 . 3 - P a t h p l a n n i n g b e t w e e n t h r e e d y n a m i c o b s t a c l e s " ) , j - 6 8 - F i g u r e 5 . 3 p r e s e n t s t h e p r e d i c t e d p a t h a m o n g 3 d y n a m i c o b s t a c l e s . T h e s e o b s t a c l e a r e a s a r e g e n e r a t e d b y f r e e z i n g 3 d y n a m i c o b s t a c l e s w i t h t i m e . S o t h e p a t h p l a n n e r c o n s i d e r s t h o s e a r e a s a s a n o t h e r s t a t i c o b s t a c l e a n d t h e n i t c a n a v o i d t h o s e b y u t i l i z i n g a n y s t a t i c o b s t a c l e a v o i d a n c e m e t h o d . 5 . 2 A r e a P r e d i c t i o n o f D y n a m i c O b s t a c l e s T h e e f f e c t i v e t i m e e s t i m a t i o n o f D y n a m i c o b s t a c l e s i s v e r y i m p o r t a n t t o a v o i d t h e m . S o m e d y n a m i c o b s t a c l e s n e a r t o U S V m a y n o t c a u s e a n y e f f e c t o n t h e U S V . T h e y m a y t r a v e l a w a y f r o m t h e U S V . S o m e d y n a m i c o b s t a c l e s m a y t r a v e l t o w a r d s t h e U S V . S o t h a t i t i s v e r y i m p o r t a n t t o e s t i m a t e t h e e f f e c t i v e t i m e o f a D y n a m i c o b s t a c l e . T h e t i m e e s t i m a t e d i s p r e s e n t e d f r o m t a r e a i n E q u a t i o n 5 . 2 . E q u a t i o n 5 . 1 i s u t i l i z e d t o c a l c u l a t e T a f t e r r e v e l i n g t 1 a n d t 2 • t 1 i s c a l c u l a t e d m a k i n g u s e o f t h e v e l o c i t y o f t h e o b s t a c l e w h i l e t 2 i s c a l c u l a t e d b y e m p l o y i n g v e l o c i t y o f t h e U S V . t 2 i s t h e t i m e t a k e n t o t h e U S V t o c r o s s t h e p r e d i c t e d p a t h o f a m o v i n g o b s t a c l e w h i l e t 1 i s t h e t i m e t a k e n f o r t h a t m o v i n g o b s t a c l e t o c r o s s t h e d e f i n e d p a t h o f t h e U S V . T h e t r a v e l i n g p a t h o f t h e U S V i s k n o w n b e f o r e h a n d a n d h a s t o u t i l i z e a p a t h p r e d i c t i o n m o d u l e t o p r e d i c t t h e p a t h o f o b s t a c l e s . I M o v i n g p a t h : o f / t h e o b s t a c l e - - " / , / "~~-~~ \ ; F i g u r e 5 . 4 - E f f e c t i v c t i m e p r e d i c t i o n o f D y n a m i c o b s t a c l e s ' > I u s v - 6 9 - T = < D It~- t 2 1 - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - ( 5 . 1 ) I m m E [ ( I ' -~ ) , ( t , + ~)] - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - ( 5 . 2 ) W h e r e t 2 i s t h e t i m e t a k e n t o t h e U S V t o c r o s s t h e p r e d i c t e d p a t h o f a m o v i n g o b s t a c l e , t 1 i s t h e t i m e t a k e n f o r t h a t m o v i n g o b s t a c l e t o c r o s s t h e d e f i n e d p a t h o f t h e u s v t = t 2 + T / 2 I I I ~I I I I I I I /sxt~ t = t 2 _ , _ _ _ I I I I I I / d l t , - t 2 - T / 2 O b s t a c l e p a t h F i g u r e 5 . 5 - A r e a C a l c u l a t i o n s f o r D y n a m i c o b s t a c l e t = O F i g u r e 5 . 5 p r e s e n t s t h e a r e a c a l c u l a t i o n o f D y n a m i c o b s t a c l e s . T h a t a r e a i s d e p e n d i n g o n t h e r e l a t i v e v e l o c i t y o f t h e o b s t a c l e a s w e l l . K i s c h o s e n d e p e n d i n g o n t h e s e a . B e c a u s e t h e b e h a v i o r o f t h e o b s t a c l e i s d i r e c t l y d e p e n d o n t h e s e a c o n d i t i o n a s w e l l . T h e o b s t a c l e m o v e m e n t i s v e r y h i g h o n a r u f f S e a . S o t h a t p r e d i c t e d a r e a s f o r o b s t a c l e s o n a r u f f s e a s h o u l d b e c o m p a r a t i v e l y l a r g e r t h a n a n o r m a l S e a . H i g h v e l o c i t i e s a r e i n c r e a s i n g t h e p o s s i b i l i t y o f h i g h d e v i a t i o n s f r o m t h e p r e d i c t e d p a t h s . T h e i m p o r t a n c e o f t h e o b s t a c l e v e l o c i t y t o t h e p r e d i c t e d a r e a i s r e f l e c t e d t h r o u g h n e w e q u a t i o n w e l l . T h e c o n s t a n t i n t h e a b o v e e q u a t i o n s c a n b e c h a n g e a t t h e p r a c t i c a l i m p l e m e n t a t i o n p h a s e . T h e d i m e n s i o n s o f t h a t a r e a c a n b e c h a n g e d b y c h a n g i n g t h e c o n s t a n t i n t h e a b o v e e q u a t i o n . - 7 0 - 5 . 3 P a t h P r e d i c t i o n o f D y n a m i c O b s t a c l e s I t i s v e r y i m p o r t a n t t o d e t e r m i n e t h e p a t h o f d y n a m i c o b s t a c l e s t o a v o i d t h e m . R a d a r s a n d o t h e r o b s t a c l e d e t e c t i o n m e t h o d s c a n u t i l i z e t o t a k e m o v i n g c o o r d i n a t e s o f t h e o b s t a c l e s . T h e n t h o s e d a t a a r e s t o r e d i n a n a r r ( ! y . S o , t h a t a r r a y c a n u t i l i z e d t h o s e d a t a t o p r e d i c t t h e f u t u r e m o v e m e n t s o f t h e o b s t a c l e s i n t h e n e x t s t e p . I t i s r e q u i r e d t o s t o r e l a t e r a l a n d l o n g i t u d i n a l c o o r d i n a t e s w i t h t i m e f o r p a t h p r e d i c t i o n p u r p o s e . I t i s q u i t e c o m p l e x t h i n g t o a n a l y z e . S o t w o s e p a r a t e a r r a y s c o n s i s t i n g l a t e r a l c o o r d i n a t e s w i t h t i m e a n d l o n g i t u d i n a l c o o r d i n a t e s w i t h t i m e a r e u t i l i z e d f o r a n a l y z e s . F i g u r e 5 . 6 p r e s e n t s a s i m u l a t i o n r e s u l t s h o w i n g t h e a c t u a l a n d p r e d i c t e d p a t h i n 3 - D s p a c e . 1 4 0 0 1 2 0 0 1 0 0 0 8 0 0 ~ Q ) E 6 0 0 ; : ; 4 0 0 2 0 0 0 3 0 0 0 2 5 0 0 2 0 0 0 1 5 0 0 Y c o o r d i n a t e s P r e d i c t e d p a t h i n t h e t i m e s p a c e 1 0 0 0 5 0 0 5 0 0 0 0 1 0 0 0 X c o o r d i n a t e s 1 5 0 0 A c t u a l p a t h P r e d i c t e d p a t h F i g u r e 5 . 7 - S i m u l a t i o n r e s u l t o f a n A c t u a l a n d P r e d i c t e d p a t h i n 3 D s p a c e - 7 1 - T w o p r e d i c t i o n m e t h o d s a r e u t i l i z e d f o r p a t h p r e d i c t i o n i n t h i s s t u d y . F i r s t c o n v e n t i o n a l m a t h e m a t i c a l m e t h o d i s a t t e m p t e d a n d t h e n g e n e r a l i z e d r e g r e s s i o n n e u r a l n e t w o r k ( G R N N ) m e t h o d i s e m p l o y e d [ 2 6 , 9 ] . A l l t h e r e s u l t s a r e c o l l e c t e d a n d a n a l y z e d a t t h e e n d . 5 . 3 . 1 P o l y n o m i a l A p p r o x i m a t i o n M e t h o d f o r P a t h P r e d i c t i o n C o n v e n t i o n a l m a t h e m a t i c a l m e t h o d , p o l y n o m i a l a p p r o x i m a t i o n i s u t i l i z e d t o p r e d i c t t h e m o v i n g p a t h o f t h e o b s t a c l e s f i r s t . T h e d a t a a r r a y w h i c h w a s u t i l i z e d t o s t o r e i n i t i a l p o s i t i o n s o f t h a t o b s t a c l e i s t a k e n t o a p p r o x i m a t e t h e m o v i n g f u n c t i o n o f t h e o b s t a c l e . T w o f u n c t i o n s h a v e a p p r o x i m a t e s f o r l a t e r a l m o v e m e n t a n d l o n g i t u d i n a l m o v e m e n t s . T h e n t h o s e t w o p o l y n o m i a l f u n c t i o n s a r e e m p l o y e d t o p r e d i c t t h e p a t h o f t h e o b s t a c l e . S e v e r a l a r r a y s c a n b e e m p l o y i n g e a s i l y f o r s e v e r a l D y n a m i c o b s t a c l e s a s e x p l a i n e d e a r l i e r . T h e d i f f e r e n c e b e t w e e n p r e d i c t e d a n d a c t u a l p a t h i s a n a l y z e b y c h a n g i n g t h e d e g r e e o f t h e p o l y n o m i a l . O b v i o u s l y t h e s e n s o r n o i s e c a n n o t b e n e g l e c t e d a s i t i s p a r t o f t h e a c t u a l d a t a . B e c a u s e t h e G P S a n d R a d e r d a t a h a v i n g t h e i r o w n d e f e r e n t n o i s e . S o s e n s o r n o i s e i s a d d e d t o t h e d a t a a n d a n a l y z e d a t t h e e n d . 5 . 3 . 2 G e n e r a l i z e d R e g r e s s i o n N e u r a l N e t w o r k f o r P a t h P r e d i c t i o n G R N N i s o f t e n u s e d f o r f u n c t i o n a p p r o x i m a t i o n . I t h a s a r a d i a l b a s i s l a y e r a n d a s p e c i a l l i n e a r l a y e r . I t c o n s i s t s o f t w o - l a y e r n e t w o r k . T h e f i r s t l a y e r h a s r a d i a l b a s i s n e u r o n s a n d t h e s e c o n d l a y e r h a s l i n e a r n e u r o n s [ 9 ] . M a t L a b N e u r a l N e t w o r k t o o l b o x i s u t i l i z e d f o r t h e s i m u l a t i o n s o f t h i s s t u d y . I t i s c u s t o m i z e d b y c h a n g i n g t h e v a r i a b l e n a m e ' s p r e a d ' . A l a r g e r ' s p r e a d ' l e a d s t o a l a r g e a r e a a r o u n d t h e i n p u t v e c t o r w h e r e l a y e r 1 n e u r o n s \ \ i l l r e s p o n d w i t h s i g n i f i c a n t o u t p u t s . T h e r e f o r e i f ' s p r e a d ' i s s m a l l t h e r a d i a l b a s i s f u n c t i o n i s v e r y s t e e p , s o t h a t t h e n e u r o n w i t h t h e w e i g h t v e c t o r c l o s e s t t o t h e i n p u t - 7 2 - w i l l h a v e a m u c h l a r g e r o u t p u t t h a n o t h e r n e u r o n s . T h e n e t w o r k t e n d s t o r e s p o n d w i t h t h e t a r g e t v e c t o r a s s o c i a t e d w i t h t h e n e a r e s t d e s i g n i n p u t v e c t o r . A s ' s p r e a d ' b e c o m e s l a r g e r t h e r a d i a l b a s i s f u n c t i o n ' s s l o p e b e c o m e s s m o o t h e r a n d s e Y e r a l n e u r o n s c a n r e s p o n d t o a n i n p u t v e c t o r . T h e n e t w o r k t h e n a c t s a s i f i t i s t a k i n g a w e i g h t e d a v e r a g e b e t w e e n t a r g e t v e c t o r s w h o s e d e s i g n i n p u t v e c t o r s a r e c l o s e s t t o t h e n e w i n p u t v e c t o r . A s ' s p r e a d ' b e c o m e s l a r g e r m o r e a n d m o r e n e u r o n s c o n t r i b u t e t o t h e a v e r a g e , w i t h t h e r e s u l t t h a t t h e n e t w o r k f u n c t i o n b e c o m e s s m o o t h e r . - 7 3 - C h a p t e r 6 S i m u l a t i o n R e s u l t s 6 . 1 S i m u l a t i o n R e s u l t s b y A p p l y i n g U G V t h e o r i e s f o r U S V T h e p a t h p l a n n i n g w i t h t w o o b s t a c l e s b y u t i l i z i n g a l g o r i t h m s w h i c h a r e d e v e l o p e d a n d t e s t e d f o r U G V i s g i v e n b e l o w . F i g u r e 6 . 1 s h o w s p l a n e d p a t h s w i t h o b s t a c l e s h a v i n g 5 0 m s a f e t y d i s t a n c e . T h e s a f e t y d i s t a n c e i s e q u a l t o 6 0 m o f t h e o b s t a c l e s w h i c h a r e u t i l i z e d f o r t h e n e x t s i m u l a t i o n s , p r e s e n t e d i n f i g u r e 6 . 2 . 5 0 0 4 5 0 4 0 0 3 5 0 3 0 0 e . . " i i ~ 2 5 0 ~ 2 0 0 1 1 5 0 1 0 0 5 0 O b s t a c l e 1 • 5 0 1 0 0 1 5 0 2 0 0 2 5 0 X . . C o o r d i n a t e [ m ] 3 0 0 O b s t a c l e 2 3 5 0 4 0 0 F i g u r e 6 . 1 - O b s t a c l e a v o i d a n c e u s i n g U G V a l g o r i t h m s ( S a f e t y d i s t a n c e f r o m O b s t a c l e s = 5 0 m ) - 7 4 - 5 0 0 4 5 0 4 0 0 3 5 0 3 0 0 E ' Q ; ' i i i ~ 2 5 0 0 0 u . ; . 2 0 0 I 1 5 0 1 0 0 5 0 5 0 O b s t a c l e 1 • 1 0 0 1 5 0 2 0 0 2 5 0 X - C o o r d i n a t e [ m ] • O b s t a c l e 2 3 0 0 3 5 0 4 0 0 F i g u r e 6 . 2 - O b s t a c l e a v o i d a n c e u s i n g U G V a l g o r i t h m s ( S a f e t y d i s t a n c e f r o m O b s t a c l e s = 6 0 m ) 6 . 2 S i m u l a t i o n R e s u l t s f r o m t h e N o v e l A l g o r i t h m 4 5 0 5 0 0 P o s s i b l e p a t h s h a v e t o g e n e r a t e f i r s t t o c h o o s e t h e b e s t a m o n g t h e m a s d i s c u s s e d i n t h e C h a p t e r 5 . S o t h a t p o s s i b l e p a t h s o f t h e U S V a r e g e n e r a t e d a s s h o w n i n F i g u r e 6 . 3 . A l l p o s s i b l e p a t h s a r e g e n e r a t e d i n a 2 0 x 2 0 m a t r i x . T h a t c a n b e u s e d t o p r e s e n t s a 2 0 x 2 0 m a r e a o r m u l t i p l e s o f t h a t s i m i l a r l y ( 4 0 x 4 0 m ) o n t h e S e a o r w a t e r . - 7 5 - 2 4 6 C D 8 - r o c ' E 1 0 0 0 ( . ) ~ 1 2 c t 1 4 1 6 1 8 2 0 2 4 6 8 1 0 1 2 1 4 1 6 1 8 2 0 C o l u m n C o o r d i n a t e F i g u r e 6 . 3 - P o s s i b l e p a t h s m a t r i x o f t h e U S V O b s t a c l e i n s e a c a n i d e n t i f y b y u t i l i z i n g R a d a r s , s t e r e o v i s i o n o r a n y o t h e r m e t h o d s s u c c e s s f u l l y . B u t t h o s e o b s t a c l e s d a t a h a v e t o i n t e r p r e t t o t h e a l g o r i t h m . F i g u r e 6 . 4 t o 6 . 7 i s p r e s e n t e d d i f f e r e n t o b s t a c l e m a t r i x f o r d i f f e r e n t o b s t a c l e p o s i t i o n s . T h e w h o l e p a t h m a t r i x i s p l o t t e d t h e r e w i t h r e d c o l o r p a t h s . T h e n s q u a r e s h a p e o b s t a c l e s w e r e p l a c e d o n d i f f e r e n t l o c a t i o n s o f t h e g r i d . I t o b v i o u s l y s h o u l d b e a 2 0 x 2 0 m a t r i x s i n c e t h e d i m e n s i o n s o f t h e p o s s i b l e p a t h m a t r i x s h o u l d b e e q u a l s t o t h e d i m e n s i o n s o f t h e o b s t a c l e m a t r i x . R e l a v e n t M a t L a b p r o g r a m i s g i v e n w i t h a p p e n d i x D . 2 r - - - - - - - - - - - - - - - - - - - - - - - - - - - - , & . I I B ~ ~ H l u ! 1 2 H I & I B : . 1 ] I I 2 ~ & B I C ~ H I & I B O l D c . . . , " c . n J . . ~ F i g u r e 6 . 4 - O b s t a c l e o n t h e O b s t a c l e m a t r i x A - 7 6 - 2 E . , B l ! ~ I D u ! 1 2 H : : I J ; z n 2 4 E B I l l 1 2 H I E I B Z l co~ . . . " c _ . , . . , F i g u r e 6 . 5 - O b s t a c l e o n t h e O b s t a c l e m a t r i x B 21~ 4 E I E 1 8 ;zn~2=-~,~~,~-s=-~I~D--7.12~-1~4~~1~E--~IB=-~;zn~ C l l l . . m n C - I I I N i e F i g u r e 6 . 6 - O b s t a c l e o n t h e O b s t a c l e m a t r i x C 2 E . I I B ~ i I l l u ! 1 2 H I E I B ; z n 2 E B I D 1 2 H I E m ; z n C I I I L . I f t n C_.lll~le F i g u r e 6 . 7 - O b s t a c l e o n t h e O b s t a c l e m a t r i x D T a b l e 1 p r e s e n t s t h e m a x i m u m t r a v e l i n g u n i t s o n e a c h a n d e v e r y p a t h . T h e n t h o s e t a b l e d a t a i s u t i l i z e d t o c a l c u l a t e p a t h v a l u e s . A f t e r c o n s i d e r i n g p a t h v a l u e s n i n t h p a t h i s c h o s e n f o r t h e o b s t a c l e m a t r i x A . 5 t h , 1 s t a n d 2 n d p a t h s a r e c h o s e n f o r t h e o b s t a c l e m a t r i c e s B , C a n d D r e s p e c t i v e l y . - 7 7 - ~ X r 1 1 I 2 I 3 I 4 I 5 I 6 I 7 I 8 I 9 A 4 0 4 0 4 0 4 0 4 0 4 0 4 0 4 0 1 7 B 4 0 4 0 4 0 4 0 1 8 4 0 4 0 4 0 4 0 c 3 4 0 4 0 4 0 4 0 4 0 4 0 4 0 4 0 D 4 0 3 4 0 4 0 4 0 4 0 4 0 4 0 4 0 T a b l e 6 . 1 - M a x i m u m S a f e t r a v e l i n g d i s t a n c e o n e a c h p a t h F i g u r e 6 . 8 a n d 6 . 9 p r e s e n t s p a t h p l a n n i n g w i t h o b s t a c l e s w h i c h a r e h a v i n g s a f e t y d i s t a n c e s 4 0 m a n d 6 0 m r e s p e c t i v e l y . 5 0 0 4 5 0 4 0 0 3 5 0 ' E 3 o o a r r o . ! : : : 2 5 0 ' E 0 0 y 2 0 0 > - 1 5 0 1 0 0 5 0 • O b s t a c l e 2 O b s t a c l e 1 I I S t a r t i n g p o i n t 0~--~--~----~--~----~--~--~-----~--~--~ 0 5 0 1 0 0 1 5 0 2 0 0 2 5 0 3 0 0 3 5 0 4 0 0 4 5 0 5 0 0 X - C o o r d i n a t e [ m ] F i g u r e 6 . 8 - O b s t a c l e a v o i d a n c e u s i n g t h e N a v a l a l g o r i t h m ( S a f e t y d i s t a n c e f r o m O b s t a c l e s = 4 0 m ) - 7 8 - ' E . . ; ; c 5 0 0 4 5 0 4 0 0 3 5 0 3 0 0 • O b s t a c l e 2 ' 2 2 5 0 0 0 ~ > - 2 0 0 1 O b s t a c l e 1 • 1 5 0 1 0 0 ~----~------~------~-------L------~------i-------L------~~----~---~ 1 0 0 1 5 0 2 0 0 2 5 0 3 0 0 3 5 0 4 0 0 4 5 0 5 0 0 0 5 0 X - C o o r d i n a t e [ m ] F i g u r e 6 . 9 - O b s t a c l e A v o i d a n c e u s i n g t h e N a v a l A l g o r i t h m b a s e d o n M o r p h i n A l g o r i t h m ( S a f e t y d i s t a n c e f r o m O b s t a c l e s = 6 0 m ) - 7 9 - · 4 . a 6 . 3 S i m u l a t i o n R e s u l t s f r o m P o t e n t i a l F i e l d M e t h o d A l r e a d y d e v e l o p e d s o f t w a r e i s e m p l o y e d f o r p a t h p l a n n i n g w i t h P F M . F i g u r e 6 . 1 0 a n d 6 . 1 1 p r e s e n t t h e c o n t o u r m a p a n d 3 d i m e n s i o n a l s u r f a c e s o f 3 o b s t a c l e s . P a t h p l a n n i n g w i t h P F M i s p r o c e s s e d b y c o n s i d e r i n g m i n i m u m p o t e n t i a l f r o m s t a r t i n g p o i n t . -~...._J...._,__ - 8 0 2 4 ' ' ~--~---·--·-------- ' ' . -----~---------y·------- -------~--------~------- ' . I ' I ' - - - - - J · - - · - - - · - ' · · · · - - - - 6 8 F i g u r e 6 . 1 0 - C o n t o u r m a p o f P o t e n t i a l f i e l d w i t h 3 o b s t a c l e s 1 0 - 8 0 - ,1 5 1 0 5 ! 3 l l 4 6 0 « X X : : a ! . , . 3 ) 0 i ~ ~:.EO ~ . . u ; ; : m 1 5 0 10 0 5 0 0 0 · · · · · · · · · · · . .. . . . . · , . . . . . · · · · · · · · · . . . . · · · · · . . . . · · · · - - · · · · · · · · · . . . . . . . . . . . . . . . : · ; · · · . . . . . . 0 - 5 • ' • • ' ' w~'' ' • o . . . . . . . . : . . . . . . . . . . . . . . . . . : . · · · · · ' : · · · · · · · · · • . . . . . . . . . . . . . . . . . . " i . , . . . . . . . : . . . _ . . . . . . . . . · · · · · · · · · · . . . . . . . . . . . . . . . . . . . . . . . . . . , . . . . . . . · · · · · · . . . . . n 5 1 0 1 5 - 1 5 F i g u r e 6 . 1 1 - 3 D s u r f a c e o f P o t e n t i a l f i e l d w i t h 3 o b s t a c l e s 5 0 ' ' ' ' . . . . . . . . , . . . . . . . . . . . . . . . . , . . . . ,... ~ . . . . . . . . . . . , . , . - ~-~ . . . . . . . . . . . . . . . . . . . . . , • l • / ' ' ' 1 0 0 1 5 0 : a : J O 2 5 0 : x J O 3 5 0 4 0 0 4 5 0 X - C t o u l l n n t e s ( n l J F i g u r e 6 . 1 2 - O b s t a c l e a v o i d a n c e u s i n g P F M ( S a f e t y d i s t a n c e f r o m O b s t a c l e s = 5 0 m ) 5 0 0 - 8 1 - 6 . 4 C o m p a r i s o n o f O b s t a c l e a v o i d a n c e m e t h o d s T h e o t h e r t w o o b s t a c l e a v o i d a n c e m e t h o d s w e r e c o m p a r e d a n d r e s u l t s a r e p r e s e n t e d i n F i g u r e 6 . 1 3 a n d 6 . 1 4 . T h e t r a v e l d i s t a n c e v e r s e s s a f e t y d i s t a n c e i s c o m p a r e d i n t h e F i g u r e 6 . 1 3 . A c c o r d i n g t o t h e r e s u l t s w h i c h a r e p r e s e n t e d i n t h e f i g u r e s h o w s t h a t N o v e l a l g o r i t h m i s c a p a b l e o f a c h i e v i n g t h e t a r g e t w i t h m i n i m u m t r a v e l i n g d i s t a n c e . P o t e n t i a l f i e l d m e t h o d s h o w s m i n i m u m t r a v e t d i s t a n c e a t 5 0 m s a f e t y d i s t a n c e s i n c e i t w a s c h a n g e d i t s t r a v e l i n g p a t h b e t w e e n t w o o b s t a c l e s t o a n o t h e r p a t h w h i c h w a s a w a y f r o m t h e t w o o b s t a c l e s . D i s t a n c e t o w a r d s o b s t a c l e s v e r s e s t i m e w e r e t a k e n f o r a l l t h r e e m e t h o d s a n d r e s u l t s a r e g i v e n u s i n g t h r e e d i f f e r e n t c o l o r s i n t h e f i g u r e 6 . 1 4 . I t s h o w s t h a t t h e g e n e r a t e d p a t h s u t i l i z i n g p o t e n t i a l f i e l d m e t h o d i s f a r a w a y f r o m t h e s a f e t y a r e a o f t h e o b s t a c l e c o m p a r e d w i t h o t h e r m e t h o d s . T h a t i s t h e m a i n r e a s o n f o r t r a v e l i n g l o n g e r w h e n t h e p o t e n t i a l f i e l d i s u s e d . 8 5 0 B O O 7 5 0 . . : : ! 7 0 0 ; : ; .~ " " " " . , - . ; ~ 6 5 0 . = 6 0 0 5 5 0 5 0 0 . . . . . . . 3 D 3 5 - P o t e n t i a l f i e l d m e t h o d f o r U S V - u G V a l g o r i t h m s f o r U S V - N o v e l algo~thm f o r U S V 4 0 4 5 ~ 5 0 S . 1 f e t y d i s t o1 1 1 C e 5 5 6 0 F i g u r e 6 . 1 3 - C o m p a r i s o n o f s a f e t y d i s t a n c e s - 6 5 7 0 - 8 2 - 5 5 0 5 0 0 4 5 0 4 0 0 I ~350 J ! l ~ ~ 3 0 0 r ~ £ ~ 2 s o r ~ c 5 2 0 0 " ' 1 5 0 1 0 0 5 0 0 2 0 0 D i s t a n c e t o w a r d s o b s t a c l e s N o v e l a l g o r i t h m A p p l i n g U G V t h e o r i e s P F M O b s t a c l e 1 4 0 0 6 0 0 6 0 0 1 0 0 0 t i m e [ s ) O b s t a c l e 2 1 2 0 0 1 4 0 0 / / 1 6 0 0 1 6 0 0 F i g u r e 6 . 1 4 - C o m p a r i s o n o f d i s t a n c e s t o w a r d s t w o o b s t a c l e s 2 0 0 0 - 8 3 - 6 . 5 S i m u l a t i o n R e s u l t s f o r D y n a m i c O b s t a c l e s A v o i d a n c e F i g u r e 1 a n d 2 p r e s e n t t h e m a k e u s e o f s t a t i c o b s t a c l e a v o i d a n c e f o r d y n a m i c o b s t a c l e s . T h e t i m e i n f o r m a t i o n o f t h e U S V a n d t h e o b s t a c l e } a r e s h o w n i n t h e f i g u r e 1 f o r t h e c o n v e n i e n c e . D y n a m i c o b s t a c l e s c a n f r e e z e o n t h e i r m o v i n g p a t h w i t h t i m e . S o t h e y b e c o m e a n o t h e r s t a t i c o b s t a c l e a t t h e e n d . B u t i t i s r e q u i r e d t o p r e d i c t t h e m o v i n g p a t h o f t h e o b s t a c l e b e f o r e h a n d . S i m u l a t i o n r e s u l t s f r o m t h e p a t h p r e d i c t i o n m e t h o d s w h i c h a r e d i s c u s s e d a b o v e a r e p r e s e n t e d b e l o w . E f f e c t i v e a r e a p r e d i c t i o n i s a l s o e q u a l l y i m p o r t a n t a s p a t h p r e d i c t i o n . S o s e t o f s i m u l a t i o n s h a v e d o n e o n t h a t a n d t h o s e s i m u l a t i o n r e s u l t s a r e p r e s e n t e d a f t e r t h e p a t h p r e d i c t i o n r e s u l t s a s s h o w n b e l o w . 8 0 0 7 0 0 0 0 0 - 5 0 0 . s I l l " ' ~400 ~ 0 0 u > - : n J 2 0 0 1 1 0 0 O b s t a c l e 2 ~ 1 0 0 2 0 0 : n J 4 0 0 X - C o o r d i n a t e s [ m ] 5 0 0 \ \ O b s t a c l e 3 0 0 0 7 0 0 F i g u r e 6 . 1 5 - D y n a m i c o b s t a c l e a v o i d a n c e w i t h t i m e v a l u e s 8 0 0 - 8 4 - 6 0 0 - G o a l t' 5 5 0 O b s t a c l e 2 / ( 6 2 5 , 5 9 0 ) 5 0 0 I I l l i 1 4 S O O b s t a c l e 1 ' e 8 I ' ' > - 4 0 0 ~ O b s t a c l e 31 i ' \ O m 3 5 0 ~. ( 3 5 0 , 3 5 0 ) v ( 3 2 5 , 3 3 0 ) 3 0 0 2 5 0 3 0 0 3 5 0 4 0 0 4 5 0 5 0 0 5 5 0 6 0 0 6 5 0 X < : o o d n a t e s l m ] F i g u r e 6 . 1 6 - D y n a m i c o b s t a c l e a v o i d a n c e w i t h 3 o b s t a c l e s 6 . 5 . 1 S i m u l a t i o n R e s u l t s f o r p a t h p r e d i c t i o n o f D y n a m i c O b s t a c l e s F o r t h e s i m u l a t i o n s o f p a t h p r e d i c t i o n , O c t o m o r p h i c p a t h i s g e n e r a t e d a s t h e a c t u a l p a t h o f t h e o b s t a c l e . B u t t h e p a t h p l a n n i n g m o d u l e i s o n l y s u p p l i e d w i t h p a s t 1 0 0 c o o r d i n a t e s o f t h e o b s t a c l e m o v e m e n t s i n U S V f r a m e . T h e n p a t h p r e d i c t i o n m o d u l e i s p r e d i c t e d a n o t h e r 5 0 c o o r d i n a t e s . T h e n t h o s e c o o r d i n a t e s a n d t h e e r r o r v a l u e s a r e p l o t t e d . 6 . 5 . 1 . 1 P o l y n o m i a l a p p r o x i m a t i o n m e t h o d f o r p a t h p r e d i c t i o n S i m u l a t i o n s d o n e u t i l i z i n g P o l y n o m i a l a p p r o x i m a t i o n m e t h o d c o n s i d e r i n g s e n s o r n o i s e a n d w i t h o u t n o i s e . 6 . 5 . 1 . 1 . 1 S i m u l a t i o n s w i t h o u t s e n s o r n o i s e 4 0 3 0 2 0 : s 1 1 0 D 0 - e } - 1 0 - 2 0 , - 3 0 . 4 Q L ~ 0 1 0 0 2 0 0 3 0 0 4 0 0 5 0 0 6 0 0 7 0 0 l l m e I [ S ] F i g u r e 6 . 1 7 - L o n g i t u d i n a l c o o r d i n a t e s o f a c t u a l p a t h 8 0 - 6 0 4 0 I 2 0 1 - ~ 0 C i i ! : l ~ - 2 0 ~ - 4 0 - 6 0 c - 8 0 0 1 0 0 2 0 0 3 0 0 4 0 0 5 0 0 6 0 0 7 0 0 l l m e I [ S ] F i g u r e 6 . 1 8 - L a t e r a l c o o r d i n a t e s o f a c t u a l p a t h j 8 0 0 , I _ j B O O - 8 6 - F i g u r e 6 . 1 9 - A c t u a l p a t h a n d P r e d i c t e d p a t h s o f t h e O b s t a c l e f o r d i f f e r e n t d e g r e e s o f P o l y n o m i a l s F i g u r e 6 . 1 7 a n d 6 . 1 8 p r e s e n t s t h e l a t e r a l a n d l o n g i t u d i n a l v a l u e c h a n g e s r e s p e c t t o t i m e . T h e n F i g u r e 6 . 1 9 s h o w s t h e a c t u a l O c t o m o r p h i c p a t h o f t h e o b s t a c l e . P r e d i c t e d p a t h s a f t e r c h a n g i n g t h e d e g r e e o f t h e p o l y n o m i a l s a r e p r e s e n t e d i n a s a m e f i g u r e t o c o m p a r e t h e e f f e c t o f t h e d e g r e e . F i g u r e 6 . 2 0 a n d 6 . 2 1 s h o w s t h e l a t e r a l a n d l o n g i t u d i n a l e r r o r s a f t e r c h a n g i n g t h e d e g r e e o f t h e p o l y n o m i a l . A d e v e l o p e d M a t L a b p r o g r a m f o r t h i s i s g i v e n i n a p p e n d i x E . - 8 7 - 4 · 3 2 ' E . . . . g w " i i i c 0 ' 6 : : J ·g , - 1 I 0 _ J - 2 - 3 - 4 0 1 0 0 I I I 2 0 0 3 0 0 4 0 0 T i m e [ S ] 5 0 0 D e g r e e = 3 D e g r e e = 8 D e g r e e = 5 6 0 0 7 0 0 ' ~ - - < 8 0 0 F i g u r e 6 . 2 0 - L o n g i t u d i n a l e r r o r o f p r e d i c t e d p a t h s f o r d i f f e r e n t d e g r e e s o f P o l y n o m i a l s 0 . 5 O A 0 . 3 0 . 2 E 0 . 1 . . . . g w 0 ~ Q ) i i i - 0 . 1 _ J - 0 . 2 - 0 . 3 f - 0 , 4 - 0 . 5 0 ~ D e g r e e = 3 D e g r e e = 5 D e g r e e = 8 1 ' " 1 0 0 2 0 0 3 0 0 4 0 0 T i m e [ S ] ~ J 5 0 0 6 0 0 7 0 0 8 0 0 F i g u r e 6 . 2 1 - L a t e r a l e r r o r o f p r e d i c t e d p a t h s f o r d i f f e r e n t d e g r e e s o f P o l y n o m i a l s - 8 8 - 6 . 5 . 1 . 1 . 2 S i m u l a t i o n s w i t h s e n s o r n o i s e 5 0 I l t V ' 1 11~· 11 ) 4 0 1 r • l t 1 1 , . ; " · ' I r' , . ' ' I , , 1 ' I r ·~ , , J d 3 0 r J ' ~ ' . · 1 I ' 1 1 E I ' • ' ; 2 0 ' I - . . . ' , Q ) I , . J - I u I ' c t t l 1 0 e ' l I ( j j 0 y C i i · \ I ' ~I 1 - c 0 f t ' 5 ' . a I ' t ' -~ "g > - 1 0 , , , , l I 1 0 ' , l ' _ J I , ' ' r ' \ - 2 0 I " ~I I l l . r ' r I I I ' I · ' ' - 3 0 ' · ( I ·'~ t ·~~~ -~',f ' I t ( . ' ( 1 ' I I · · t f I•~·' W 1 ' , \ ~ ' , · , · . . . - 4 0 ' j ~ ~.\ 0 1 0 0 2 0 0 3 0 0 4 0 0 5 0 0 6 0 0 7 0 0 8 0 0 T i m e I [ S ] F i g u r e 6 . 2 2 - L o n g i t u d i n a l c o o r d i n a t e s o f a c t u a l p a t h w i t h n o i s e ( n o i s e = S m ) 1 0 0 · 8 0 6 0 4 0 I " 2 0 ~ 5 5 0 · 2 0 - 4 0 · 6 0 · 8 0 0 . 1 1 ' · ' . ) l f ' ' r ' ·~},Ji! · ' '-~~~ · ' I V • ' ' . r l l i r 1 1 " 1 ' l'Yl~ , . ~~~/' ( I ' ' ' ' - ' ) ' ; ' . • . l ; , . i ~~- 1 ' ' . . . \ ' ' 5 0 1 0 0 1 5 0 2 0 0 I~ , , , ' l ( I • ' I ~ J ~ r r~, l 1 L • • t 2 5 0 l i m e / [ S J f \ 1 1 1 1 t i 1 . ' • . 1, 1 ' r ~·\1 I~ ' ' 1 ; , · J : l . · ' ·! · • , . ;l y . , , . \ • ' ' J • ' I • I l l " ' l l i j l l ' ~'· - , ' . , . _ . 1 ' 3 0 0 3 5 0 4 0 0 4 5 0 5 0 0 F i g u r e 6 . 2 3 - L a t e r a l c o o r d i n a t e s o f a c t u a l p a t h w i t h n o i s e ( n o i s e = S m ) - 8 9 - ~ - " ' u c : 2 ! e n i : S ! ! ! " ' n ; . . . . . / 1 0 0 8 0 L ~ y~- / 7 --.....:.~: - "' 6 0 _ : ; Y ' , : ~ . . ~ 4 0 ~ 2 0 0 - 2 0 4 0 \ - 1 " ' ' '-~"---. . . . :.~ / · - 6 0 \ , . - : - > . " " ' '~ < / ~ l ' ~ c - - < - - - - ~ :::;r~-~- . . " ' ' \ o ~· ~::---­ -----~" ~ . , . _ - 8 0 4 0 - 3 0 - 2 0 j ' - . : : - t K . - - - 1 0 A c t u a l P a t h , _ ' " " v - " < ) ~ " ) , " - . . . . _~,. ~ - - - - - - - - - ' L S , _ , ~ • . x f ; 4 -,?.--~ ~~ 1 ' \ - ' - ~ \ ~- ' \ - -~··\:~ ~/ - , . 7 \ - Y , - / 1 - ~~v . , . t : . " ; ; > < . y _ - - ; 7 ~. ~"" '~ ,-f<~ ~-:..._ ~ '\~-. . { _ , / 1 0 - - · . . . . - - . . --~ 7 " 0 2 0 3 0 4 0 L o o g i t u d i n a l D i s t a n c e I [ m ] F i g u r e 6 . 2 4 - A c t u a l p a t h o f t h e O b s t a c l e w i t h n o i s e ( n o i s e = 5 m ) F i g u r e 6 . 2 2 a n d 6 . 2 3 s h o w s t h e l a t e r a l a n d l o n g i t u d i n a l v a l u e c h a n g e s w i t h n o i s e r e s p e c t t o t h e t i m e . T h e n F i g u r e 6 . 2 4 p r e s e n t s t h e a c t u a l O c t o m o r p h i c p a t h o f t h e o b s t a c l e w i t h r e s p e c t t o t i m e _ T h e n p r e d i c t e d p a t h i s g e n e r a t e d f o r n o i s y d a t a w i t h 6 d e g r e e p o l y n o m i a l . T h a t i s g i v e n i n F i g u r e 6 . 2 5 . T h e n t h a t d e g r e e i s c h a n g e d t o 5 a n d 4 r e s p e c t i v e l y a s s h o w n i n t h e F i g u r e 6 . 2 6 a n d 6 . 2 7 . - 9 0 - 5 0 1 4 0 0 1 2 0 0 1 0 0 0 8 0 0 g - Q ) 0 c : " ' 6 0 0 i i i i 5 ~ Q ) i i i - ' 4 0 0 2 0 0 0 - 2 0 0 - 1 0 0 0 ' , , ' \ \ / \ " ~\ < ~~ . . . - - ' - ' - " " " ' - 5 0 0 0 A c t u a l a n d P r e d i c t e d P a t h - 5 0 0 L o n g i t u d i n a l D i s t a n c e I [ m ] 1 0 0 0 P r e d i c t e d P a t h ( D e g r e e = 6 , N o i s e = 1 m ) l A c t u a l P a t h / ' 1 5 0 0 2 0 0 0 F i g u r e 6 . 2 5 - A c t u a l a n d P r e d i c t e d p a t h o f t h e O b s t a c l e ( D e g r e e o f P o l y n o m i a l = 6 , n o i s e = l m ) - 9 1 - 2 5 0 2 0 0 1 5 0 E : : : : ' 1 0 0 Q ) u c : : " ' I l l : : l ~ 50 ; ; ; - . J 0 -5 0 - 1 0 0 - 1 0 0 P r e d i c t e d P a t h ( D e g r e e = 5 , N o i s e = 1 m ) A c t u a l P a t h ~ ,~ . . . . - ' - / " -~ " · I ! ~' \ ' I / f / / ~ \ , I · - ,, ) i ( l / - j \~ / 1 / ' v -~ ' ·,~, .~_.,_ , L _ _ , . - 5 0 0 5 0 A c t u a l a n d P r e d i c t e d P a t h 1 0 0 1 5 0 2 0 0 L o n g i t u d i n a l Di s t a n c e I [ m ] F i g u r e 6 . 2 6 - A c t u a l a n d P r e d i c t e d p a t h o f t h e O b s t a c l e ( D e g r e e o f P o l y n o m i a l = 5 , n o i s e = l m ) / ~· 2 5 0 3 0 0 - 9 2 - 1 0 0 8 0 6 0 4 0 E 2 0 - Q ) ' - ' 0 : : : : . " ; ! . ! ! ! 0 C l ~ Q ) < ; ; - 2 0 _ J - 4 0 ' . £ 0 - 8 0 - 1 0 0 - 6 0 A c t u a l a n d P r e d i c t e d P a t h ~ l P r e d i c t e d P a t h ( D e g r e e = 4 , N o i s e = 1 m ) l A c t u a l P a t h < / ~ -...:,. - - " -~ , . . . r - , _ - ' / " - , ' " " ' ; \ ¥ ' , , I f s ' ' \ " ' ' , , ." , , " ' - . / / ' " - . < ' - - - . . " ' " ' ' , . . - r _ ' " y \ . t ~> ' t : - ~ - { ~ . . _ _ _ _ . ' ~ :--....,~ ~· J - 4 0 - 2 0 0 2 0 4 0 6 0 L o n g i t u d i n a l D i s t a n c e I [ m ] F i g u r e 6 . 2 7 - A c t u a l a n d P r e d i c t e d p a t h o f t h e O b s t a c l e ( D e g r e e o f P o l y n o m i a l = 4 , n o i s e = l m ) I - " ' g ! C l ) 0 ~ " ' § 5 0 0 4 0 0 3 0 0 2 0 0 1 0 0 0 - 1 0 0 A c t u a l a n d P r e d i c t e d P a t h t:M~.A-~~ -- > ' . . ; : s _ , . . ; , -. . . . , - . " . i - t < . . . r ~ ~ ... ' < • ~ . . . . . . ~~ . . . . . . . C . _ _ "~?:- P r e d i c t e d P a t h (Degree~4. Noise~ S m ) A c t u a l P a t h - 2 0 0 , - 3 0 0 ~ - 4 0 0 - 1 5 0 - 1 0 0 - 5 0 0 5 0 1 0 0 1 5 0 2 0 0 2 5 0 3 0 0 L o n g i t u d i n a l D i s t a n c e I [ m ] F i g u r e 6 . 2 8 - A c t u a l a n d P r e d i c t e d p a t h o f t h e O b s t a c l e ( D e g r e e o f P o l y n o m i a l = 4 , n o i s e = 5 m ) O n e m e t e r n o i s e i s a d d e d t o t h e f i g u r e 6 . 2 5 , 6 . 2 6 a n d 6 . 2 7 . Q u i t e i m p r o v e d p r e d i c t e d p a t h i s r e c e i v e d w i t h s m a l l ( l m ) n o i s e w h e n d e g r e e o f t h e p o l y n o m i a l i s e q u a l t o f o u r . T h e n n o i s e i s i n c r e a s e d b y 4 m t o a n a l y z e t h e e f f e c t o f n o i s e . N o r m a l l y e v e n t h e i m p r o v e d G P S r e c e i v e r s a r e h a v i n g e r r o r o f t w o o r t h r e e m e t e r s a l o n e . S o t h e n o i s e i s c h a n g e d f r o m l m t o 5 m o f 4 t h d e g r e e p o l y n o m i a l . T h a t r e s u l t i s g i v e n i n f i g u r e 6 . 2 8 a n d i t p r o v e s t h a t t h e p o l y n o m i a l a p p r o x i m a t i o n s m e t h o d s a r e n o t g o o d w i t h n o i s e . - 9 4 - - - 1 3 5 0 6 . 5 . 1 . 2 R B N N m e t h o d f o r p a t h p r e d i c t i o n S i m u l a t i o n s a r e d o n e u t i l i z i n g R B N N m e t h o d c o n s i d e r i n g a n d w i t h o u t c o n s i d e r i n g s e n s o r n o i s e . 6 . 5 . 1 . 2 . 1 S i m u l a t i o n s w i t h o u t s e n s o r n o i s e - A c t u a l a n d P r e d i c t e d P a t h s 8 0 - ~ ~ I I ---":"', ------'-'~- - ~ - - - - - - - - 6 0 , " ' ' ' , . . . . . . . . . _ . . . . . . . . . . . . 4 0 . . . . . . . . . . . , . . ~ / " 2 0 ~ E . I A c t u a l P a t h I - s p r e a d = 1 Q l < J s p r e a d = 5 c : ~ 0 " ' s p r e a d = 1 0 i 5 T ! ! Q l i i i . . J - 2 0 / / 1 ' ~/ ~ "~I - 4 0 / ~ ~------ / ' _ . - 6 0 ' / / ~ - - - - . . _;;.--~ « . . . . . . _ · 8 0 - 4 0 - 3 0 - 2 0 - 1 0 0 1 0 2 0 3 0 4 0 L o n g i t u d i n a l D i s t a n c e I [ m ] F i g u r e 6 . 2 9 - A c t u a l p a t h a n d P r e d i c t e d p a t h s o f t h e O b s t a c l e f o r d i f f e r e n t S p r e a d s - 9 5 - 2 0 , s p r e a d = 1 1 5 1 s p r e a d = 1 0 s p r e a d = 5 1 0 ' - - j ' E 5 . . . g / . w J ' i i i 0 . ~ c : I \ ' 6 f , I f . : : J I ' \ 1 ~, I - \ ,, l · c ; , - 5 I c : I I j 0 I _ J I - 1 0 - 1 5 - 2 0 0 1 0 0 2 0 0 3 0 0 4 0 0 5 0 0 6 0 0 7 0 0 8 0 0 T i m e [ S ) F i g u r e 6 . 3 0 - L o n g i t u d i n a l e r r o r o f p r e d i c t e d p a t h s f o r d i f f e r e n t S p r e a d s 4 0 3 0 2 0 ~ 1 0 E . . . . g w 0 ~ Q ) c ; ; _ J - 1 0 - 2 0 - 3 0 - 4 0 0 1 0 0 s p r e a d = 1 s p r e a d = 1 0 s p r e a d = 5 I 2 0 0 I 3 0 0 i 1 ; I 4 0 0 T i m e [ S ] 5 0 0 1 \ I 6 0 0 7 0 0 8 0 0 F i g u r e 6 . 3 1 - L a t e r a l e r r o r o f p r e d i c t e d p a t h s f o r d i f f e r e n t S p r e a d s ~ 9 6 ~ A c t u a l a n d p r e d i c t e d p a t h s f o r d i f f e r e n t s p r e a d s a r e g i v e n i n f i g u r e 6 . 2 9 t o a n a l y z e t h e e f f e c t o f s p r e a d v a l u e e f f i c i e n t l y . T h e n u m b e r o f n e u r o n e n r o l l m e n t s c a n b e i n c r e a s e d b y i n c r e a s i n g t h e s p r e a d v a l u e . T h a t p r e d i c t e d p a t h c a n b e s m o o t h e d m o r e b y i n c r e a s i n g t h e s p r e a d v a l u e . L o n g i t u d i n a l a n d L a t e r a l e r r o r c h a n g e s f o r d i f f e r e n t s p r e a d v a l u e s a r e p r e s e n t e d i n F i g u r e 6 . 3 0 a n d 6 . 3 1 r e s p e c t i v e l y . 6 . 5 . 1 . 2 . 2 S i m u l a t i o n s w i t h s e n s o r n o i s e A c t u a l a n d P r e d i c t e d P a t h s 1 0 0 ' T T - , r A c t u a l P a t h N o i s e = O m 8 0 _ _ _ _ _ N o i s e = 5 m _H : < - " " ' - - , ~ N o i s e = 1 O m ,._,~ 6 0 ~ ~~ \ ' \ . 4 0 ' ~ ~ E " ' ' Q ) 2 0 u c : : " ' ~, X - ' - 2 0 / - 4 0 / / ' ( " . \ / .( - 6 0 \ ~ · / ""~ ~ -8 0 - 5 0 - 4 0 · 3 0 · 2 0 - 1 0 0 1 0 2 0 3 0 4 0 5 0 L o n g i t u d i n a l D i s t a n c e I [ m] F i g u r e 6 . 3 2 - A c t u a l p a t h a n d P r e d i c t e d p a t h s o f t h e O b s t a c l e f o r d i f f e r e n t n o i s e v a l u e s - 9 7 - T h e d a t a w i t h o u t s e n s o r n o i s e c a n n o t b e e x p e c t e d i n t h e f i e l d . S o t h a t t h e n o i s e i s i n t r o d u c e d t o t h e a c t u a l p a t h d a t a a n d t h e n p r e d i c t e d p a t h s a r e t a k e n f o r d i f f e r e n t n o i s e v a l u e s w i t h 3 m s p r e a d . T h o s e r e s u l t s a r e g i v e n b e l o w i n F i g u r e 6 . 3 2 . I t p r o v e s t h e v a l i d i t y o f t h e R B N N m e t h o d f o r s u c c e s s f u l p a t h p r e d i c t i o n w i t h n o i s e s . L o n g i t u d i n a l a n d l a t e r a l e r r o r s f o r d i f f e r e n t n o i s e v a l u e s a r e g i v e n i n F i g u r e 6 . 3 3 a n d 6 . 3 4 r e s p e c t i v e l y . M a t L a b p r o g r a m f o r t h i s i s g i v e n i n a p p e n d i x F . 4 0 N o i s e = 1 0 m 3 0 l N o i s e = 5 m f . 1 1 N o i s e = O m · ~ r ' ~ ~ l I I V 2 0 ' • t l l l l I : ~li f , [ r , ~~ ' i f J i n~ ' · ~ ~ " I~ • I . s 1 0 • , . 1 ~· · r~r r , • . ~ I J ' I ~ I I ' I I ' I I j I 0 I ' I' I I ( ' ~ I I \ ' •' ' I I ' I / ' ' I t \ j , ~,~~~~ , , r~ 1 ~t . 1 r ~· ~·~-' ~~ I . . 0 , l , . • . ~ . . . . . . r t j . ! . ~ ; : 1' 1 ' ' i , , , l l > ~I ~ • . • . ' ' ~'• , l : J , \ I \ · 1\ 1 ~~ ~ ~ ~j. ' I!~'' , : ! : : : J . ' ' ' t· · ~ y I t I~ g > - 1 0 ;~ , · t r 1 I l \~1 ' i ~~ t ' r \ I 0 . I I i I • I . ; ~ . . . J H . I t ) I j ' h !J 1 I ' , - 2 o I I ' 'I I t 1 1 t ' t I ' . I )1~\ l t ~J l t 1 - 3 0 ~ I / 1 - 4 0 0 1 0 0 2 0 0 3 0 0 4 0 0 5 0 0 6 0 0 7 0 0 8 0 0 T i m e [ S ] F i g u r e 6 . 3 3 - L o n g i t u d i n a l e r r o r o f p r e d i c t e d p a t h s f o r d i f f e r e n t n o i s e v a l u e s - 9 8 - 4 0 3 0 2 0 E 1 0 - . . . . . g 0 ~ w ~ I Q ) I . j - 1 0 I I 'I~ l . " ' ; \ ( I j . , - 2 0 I ·~ , I , · i l' . - 3 0 I I ~ · I · I ' I~ ~ ~~ ,~ l ' i If ' I ( ~ / , 1 ~~- i _ , ~ , ' . . tl . J ~ , i t 1. Jl i ' l I · ' ' ~ ~ I \ 1 1: · ./~ •1, r . ' ~'{ ~ 1 , t , , , · I I ' ' I J . . ~ ~~ I · A \ - l i ) " ~t , N o i s e = 1 0 m N o i s e = 5 m N o i s e = O m j . ~, . . . · ' . 'i. 'rl~ tt l 1 · 1 ~· , . ~~: .. ~. ~. , ~~ nt ~· h~~ I I i l l ' ' t~~ I ' M , . . · ~ 1 1 1 . 1 · . v \ i J ' / i I I ' , ~ ru . ' I t~. , . . ' , I l h l r \ 1 , r \ 1 \ (1\ 1 ' \ •i \ ' ' \ I •) \ , ' : I , I n ' t I I - 4 0 0 1 0 0 2 0 0 3 0 0 4 0 0 l i m e [ S ] 5 0 0 6 0 0 7 0 0 8 0 0 F i g u r e 6 . 3 4 - L a t e r a l e r r o r o f p r e d i c t e d p a t h s f o r d i f f e r e n t n o i s e v a l u e s 6 . 5 . 1 . 3 S u m m a r y o f S i m u l a t i o n R e s u l t s f o r p a t h p r e d i c t i o n P a t h p r e d i c t i o n s o f d y n a m i c o b s t a c l e s a r e v e r y i m p o r t a n t f o r d y n a m i c o b s t a c l e a v o i d a n c e . P o l y n o m i a l a p p r o x i m a t i o n m e t h o d i s t r i e d f o r t h a t i n i t i a l l y . I t s q u i t e g o o d m e t h o d a n d g a v e v e r y g o o d r e s u l t s a t t h e b e g i n n i n g , w i t h o u t n o i s e s . B u t p o s i t i o n d a t a c a n n o t b e e x p e c t e d w i t h o u t n o i s e f r o m t h e f i e l d . S o n o i s e w a s i n t r o d u c e d a n d p o l y n o m i a l a p p r o x i m a t i o n m e t h o d i s n o t a b l e t o g i v e c o n s i d e r a b l e r e s u l t s a s e x p e c t e d . R B N N m e t h o d w h i c h i s f a m o u s f o r f u n c t i o n a p p r o x i m a t i o n i s t r i e d a f t e r t h a t . I t s h o w s v e r y g o o d r e s u l t s w i t h n o i s e s . T h o s e p r e d i c t e d p a t h s c a n s m o o t h e n m o r e b y i n c r e a s i n g t h e s p r e a d v a l u e . - 9 9 - 6 . 5 . 2 S i m u l a t i o n R e s u l t s f o r O b s t a c l e a r e a p r e d i c t i o n o f D y n a m i c O b s t a c l e s 6 . 5 . 2 . 1 S i m u l a t i o n R e s u l t s f r o m V e l o s i t y o b s t a c l e m e t h o d V e l o c i t y o b s t a c l e m e t h o d i s a c o n v e n t i o n a l m e t h o d w h i c h i s e m p l o y e d f o r D y n a m i c o b s t a c l e a r e a p r e d i c t i o n s . I t p r e d i c t s t r i a n g u l a r a r e a s f o r D y n a m i c o b s t a c l e s . T h e s i z e o f t h a t a r e a i s d e p e n d s o n a c o n s t a n t . F o l l o w i n g f i g u r e s p r e s e n t t h e s i m u l a t i o n r e s u l t s f r o m t h a t c o n v e n t i o n a l m e t h o d . A n o v e l o b s t a c l e a r e a p r e d i c t i o n m e t h o d i s s i m u l a t e d a f t e r t h a t . T h e m a i n o b j e c t i v e o f t h a t p r o p o s e d m e t h o d i s t o p r e d i c t t h e a r e a s o f d y n a m i c o b s t a c l e m o v e m e n t s e f f e c t i v e l y . S o t h a t n o v e l m e t h o d i s c o m p a r e d w i t h t h e c o n v e n t i o n a l m e t h o d a t t h e e n d . T h o s e s i m u l a t i o n r e s u l t s a r e p r e s e n t e d f i n a l l y . 2 1 . 5 1 0 0 2 0 0 3 0 0 4 0 0 l i m e I [ s ] 5 0 0 6 0 0 7 0 0 F i g u r e 6 . 3 5 - L o n g i t u d i n a l v e l o c i t y o f t h e o b s t a c l e 8 0 0 - 1 0 0 - 2 i 1 / 1 . 5 V I . . . _ . s 0 . 5 . . . _ . ? : - ·g 0 Q i > ~ - 0 . 5 Q ) i i i _ . J - 1 - 1 . 5 - 2 I 0 1 0 0 4 0 0 2 0 0 3 0 0 4 0 0 T i m e I [ s ] 5 0 0 6 0 0 7 0 0 F i g u r e 6 . 3 6 - L a t e r a l v e l o c i t y o f t h e o b s t a c l e 8 0 0 A c t u a l P a t h 3 0 0 k = 0 . 6 1 ' 1 ' : ~ ' ! ~~c,~ l 1 ~ ~~. II ·. l I \ l . ' . ~ , /. ~ / l ~ ' I . / . r '~ . . . ' . ! / 1 " ' l - ' ' J· ' ~ k = 0 . 9 k = 1 . 2 . . ' ' ' ' · 0 0 0 ' . i \ > · 2 0 0 j - 3 0 0 1 - 4 0 0 0 1 0 0 2 0 0 3 0 0 4 0 0 1 i m e ] S ] 5 0 0 6 0 0 7 0 0 8 0 0 F i g u r e 6 . 3 7 - U p p e r a n d L o w e r B o u n d a r i e s o f P r e d i c t e d O b s t a c l e s A r e a t o w a r d s L o n g i t u d i n a l d i r e c t i o n - I 0 I - s o o , 4 0 0 3 0 0 2 0 0 • I 1 0 0 / j / f c S 0 ~ ~ - 1 0 0 - 2 0 0 - 3 0 0 - 4 0 0 - s o o ' 0 ( k l l f~~ I / ' ' . " - , ' \ , , 1 0 0 2 0 0 3 0 0 L " " ' - ~\ 4 0 0 l i m e [ S ) , " " - " " - 5 0 0 A c t u a l P a t h 1 ' k = 0 . 6 k = 0 . 9 k = 1 . 2 1 / 1 ---~~ //~ f I r , r I j , - j 5 0 0 7 0 0 B O O F i g u r e 6 . 3 8 - U p p e r a n d L o w e r B o u n d a r i e s o f P r e d i c t e d O b s t a c l e s A r e a t o w a r d s L a t e r a l d i r e c t i o n F i g u r e 6 . 3 7 a n d 6 . 3 8 p r e s e n t u p p e r a n d l o w e r b o u n d a r i e s o f p r e d i c t e d o b s t a c l e a r e a s u t i l i z i n g c o n v e n t i o n a l m e t h o d t o w a r d s L o n g i t u d i n a l a n d L a t e r a l d i r e c t i o n s . T h o s e b o u n d a r i e s a r e g e n e r a t e d f o r d i f f e r e n t c o n s t a n t v a l u e s . T h i s c o n s t a n t c a n b e a d j u s t e d t o t a k e t h e e f f i c i e n t p r e d i c t e d a r e a . T h e a c t u a l p a t h m a y d e v i a t e o u t f r o m t h e p r e d i c t e d o b s t a c l e a r e a s . T h a t m e a n t h o s e o b s t a c l e s m a y c o l l i d e w i t h t h e U S V . S o t h o s e t y p e s o f s i t u a t i o n s a r e c a l c u l a t e d a n d p l o t t e d w i t h t i m e f o r L o n g i t u d i n a l a n d L a t e r a l d i r e c t i o n s a s s h o w n i n F i g u r e 6 . 3 9 a n d 6 . 4 0 . - 1 0 2 - 1 0 0 ~=ll.l'> k = 0 . 9 9 0 1 I k = 1 . 2 I ' 8 0 I - I I 7 0 I I 6 0 I ' E I i 5 0 w I 4 0 3 0 I I ' I l I : ) 1 2 0 1 I ~ I I i ' ! ~ , •: 1 0 ; I I ' I ' 0 0 1 0 0 2 0 0 3 0 0 4 0 0 5 0 0 6 0 0 7 0 0 B O O l i m e ( S ( F i g u r e 6 . 3 9 - E r r o r t o w a r d s L o n g i t u d i n a l d i r e c t i o n 1 0 0 k = 0 . 6 k = 0 . 9 9 0 r k = 1 . 2 8 0 I 7 0 6 0 ' E J ~ 5 0 w I 4 0 I 3 0 1 2 0 I I . • . \ ' ) I '~ ,. 1 0 I , Y ' \ 0 l 0 1 0 0 2 0 0 3 0 0 4 0 0 5 0 0 6 0 0 7 0 0 B O O l i m e ( S ) F i g u r e 6 . 4 0 - E r r o r t o w a r d s L a t e r a l d i r e c t i o n 6 . 5 . 2 . 2 S i m u l a t i o n R e s u l t s f r o m t h e n o v e l m e t h o d T h e p r e d i c t e d a r e a s g e n e r a t e d f r o m t h e n o v e l m e t h o d a r e d e p e n d i n g o n t h e r e l a t i v e v e l o c i t y o f t h e o b s t a c l e s a s d i s c u s s e d i n p r e v i o u s c h a p t e r s . T h e r e l a t i v e v e l o c i t y v a r i a t i o n o f t h e o b s t a c l e i s p r e s e n t e d i n F i g u r e 6 . 3 5 a n d 6 . 3 6 . T h a t v a r i a t i o n i s d i r e c t l y e f f e c t i n g t h e a r e a p r e d i c t i o n . S o t h e u p p e r a n d l o w e r b o u n d a r i e s o f p r e d i c t e d a r e a s b y m e a n s o f n o v e l m e t h o d a r e p r e s e n t e d i n F i g u r e 6 . 4 1 a n d 6 . 4 2 . T h e e r r o r v a l u e s o f t h o s e a r e s h o w n i n f i g u r e 6 . 4 3 a n d 6 . 4 4 . T h e n t h e S e a c o n d i t i o n f a c t o r i s c h a n g e d a n d t h o s e s i m u l a t i o n r e s u l t s a r e p r e s e n t e d i n F i g u r e 6 . 4 5 , 6 . 4 6 , 6 . 4 7 , 6 . 4 8 a n d 6 . 4 9 . I Q ) t . l " " ' 1 ; j i 5 1 i ! " ' C ~ · c ; , " s 3 0 0 2 0 0 1 0 0 0 - 1 0 0 - 2 0 0 - 3 0 0 1 0 . . I , - ~ \ \ \ . < \ , 1 0 0 2 0 0 3 0 0 A 4 0 0 T i m e [ S ] . / ' ' 5 0 0 ' ' 1 ' 6 0 0 1 I I " ' ' \ ~ / ·, " ' ' ' ' " " " 7 0 0 B O O F i g u r e 6 . 4 1 - U p p e r a n d L o w e r B o u n d a r i e s o f P r e d i c t e d O b s t a c l e s A r e a t o w a r d s L o n g i t u d i n a l d i r e c t i o n ( S = 0 . 5 ) - 1 0 4 - 4 0 0 3 0 0 ~ 2 0 0 / 1 0 0 I C D ! ! ; ) ! 0 i 5 j j ! C D § - 1 0 0 - 2 0 0 - 3 0 0 - 4 0 0 0 1 0 0 ' 2 0 0 3 0 0 4 0 0 5 0 0 6 0 0 l i m e [ S ] j _ 7 0 0 / . ' 1 / - l 6 0 0 F i g u r e 6 . 4 2 - U p p e r a n d L o w e r B o u n d a r i e s o f P r e d i c t e d O b s t a c l e s A r e a t o w a r d s L a t e r a l d i r e c t i o n ( S = 0 . 5 ) 1 0 0 - 9 0 8 0 7 0 , 6 0 s g 5 0 w 4 0 3 0 1 - - i I 2 0 1 0 0 I ' • I ( 0 1 0 0 2 0 0 3 0 0 4 0 0 5 0 0 6 0 0 7 0 0 8 0 0 1 i m e [ S ] F i g u r e 6 . 4 3 - E r r o r t o w a r d s L o n g i t u d i n a l d i r e c t i o n ( S = 0 . 5 ) - 1 0 5 - 1 0 0 : 9 0 8 0 7 0 6 0 ' E . . . . . 5 0 g w 4 0 3 0 2 0 1 0 ' 0 0 3 0 0 2 0 0 1 0 0 2 0 0 3 0 0 4 0 0 l i m e [ S ] 5 0 0 - 1 6 0 0 7 0 0 8 0 0 F i g u r e 6 . 4 4 - E r r o r t o w a r d s L a t e r a l d i r e c t i o n ( S = 0 . 5 ) ~ l A c t u a l P a t h 1 1 I l y l S = 0 . 4 1 l I I ' s = 0 . 6 I I A ' 0 0 ,Y~~"\ . ~ /~"- I : " H"~ E / f \ I 1 \ ' / \ " 1 I '\ 1 " § " 0 / r \ . . \ ' / I • 1 ' ' ) ! l ' ' \ y 1 ' ' 0 ' . " ~.. r ~ . , / , 1 · . , . . • . ~ -100 ~ t i - 2 0 0 · 3 0 0 - 4 0 0 0 1 0 0 I 2 0 0 3 0 0 4 0 0 l i m e [ S ] 5 0 0 6 0 0 } 7 0 0 6 0 0 F i g u r e 6 . 4 5 - U p p e r a n d L o w e r B o u n d a r i e s o f P r e d i c t e d O b s t a c l e s A r e a t o w a r d s L o n g i t u d i n a l d i r e c t i o n f o r d i f f e r e n t S e a c o n d i t i o n v a l u e s - 1 0 6 - 5 0 0 4 0 0 3 0 0 2 0 0 I t O O I l l u c . , 1 i j 0 0 1 ! ! I l l j - 1 0 0 - 2 0 0 - 3 0 0 - 4 0 0 - 5 0 0 0 A c t u a l P a t h s = 0 . 4 ' s = 0 . 6 I I ~~~ l s = 0 . 8 / - " " ' r / I l ; '~, 1 l ; / ' " - , / I ' ~ : I , ,>,~ - I ~I y ' / ~---.~ ~ 1 0 0 2 0 0 3 0 0 4 0 0 5 0 0 6 0 0 7 0 0 B O O T i m e ( S ] F i g u r e 6 . 4 6 - U p p e r a n d L o w e r B o u n d a r i e s o f P r e d i c t e d O b s t a c l e s A r e a t o w a r d s L a t e r a l d i r e c t i o n f o r d i f f e r e n t S e a c o n d i t i o n v a l u e s 7 0 0 6 0 0 5 0 0 I 4 o o " ' ; ; ~ ~ ' § 3 0 0 0 " ' 2 0 0 1 0 0 0 0 t 1 . r 1 0 0 2 0 0 3 0 0 4 0 0 l l m e [ S ] l ' l 5 0 0 6 0 0 l 7 0 0 S=O~ s = 0 . 6 s = 0 . 8 J I 8 0 0 F i g u r e 6 . 4 7 - W i d t h o f P r e d i c t e d O b s t a c l e s A r e a t o w a r d s L o n g i t u d i n a l d i r e c t i o n f o r d i f f e r e n t S e a c o n d i t i o n v a l u e s - 1 0 7 - 1 0 0 · 9 0 8 0 7 0 6 0 E I . . . . . 5 0 I g r w 4 0 i 3 o I I I 2 0 I I 1 0 ; 0 0 ' I J ( 1 0 0 2 0 0 3 0 0 4 0 0 T i m e [ S ] 5 0 0 \ 6 0 0 S = O A s = 0 . 6 s = 0 . 8 7 0 0 8 0 0 F i g u r e 6 . 4 8 - E r r o r t o w a r d s L o n g i t u d i n a l d i r e c t i o n f o r d i f f e r e n t S e a c o n d i t i o n 1 0 0 · 9 0 8 0 7 0 6 0 ' E g 5 0 I w 4 o I I 3 o I 2 0 I I 1 o I I 0 0 I ' J I I I , 1 0 0 2 0 0 3 0 0 v a l u e s 4 0 0 T i m e [ S ] 5 0 0 6 0 0 S = O A s = 0 . 6 s = 0 . 8 7 0 0 8 0 0 F i g u r e 6 . 4 9 - E r r o r t o w a r d s L a t e r a l d i r e c t i o n f o r d i f f e r e n t S e a c o n d i t i o n v a l u e s - 1 0 8 - 6 . 5 . 2 . 3 C o m p a r i s o n R e s u l t s T h e p r o p o s e d n o v e l a l g o r i t h m f o r o b s t a c l e a r e a p r e d i c t i o n i s c o m p a r e d w i t h t h e c o n v e n t i o n a l m e t h o d t o p r o v e t h e e f f e c t i v e n e s s o f t h e n o v e l m e t h o d . T h e u p p e r a n d l o w e r b o u n d s o f t h e p r e d i c t e d o b s t a c l e a r e a s f r o m b o t h m e t h o d s a r e s h o w n i n f i g u r e 6 . 5 0 a n d 6 . 5 1 a n d e r r o r s a r e p r e s e n t e d i n f i g u r e 6 . 5 2 a n d 6 . 5 3 . W i d t h o f t h e p r e d i c t e d a r e a f r o m b o t h m e t h o d s t o w a r d s l o n g i t u d i m r l d i r e c t i o n i s g i v e n i n f i g u r e 6 . 5 4 . T h e a r e a r e d u c t i o n f r o m t h e n o v e l m e t h o d i s i n s p i r e d f r o m t h e d i f f e r e n c e s b e t w e e n t h e w i d t h s o f t h e n o v e l m e t h o d a n d c o n v e n t i o n a l m e t h o d . T h a t d i f f e r e n c e i s g i v e n i n f i g u r e 6 . 5 5 a n d t h a t p r o v e s t h e i m p r o v e m e n t s f r o m t h e n o v e l m e t h o d . M a t l a b p r o g r a m w h i c h i s d e v e l o p e d f o r t h i s i s g i v e n w i t h a p p e n d i x G . 4 0 0 , A c t u a l P a t h 3 0 0 N 0 1 . e l M e t h o d C o n ' - ' l n t i o n a l M e t h o d 2 0 0 ' I 1 0 0 / _ r _ _ _ /r~,,~, / " ' < . l c : " ' ~ 0/ g ! ' 6 2 -~ . . J - 1 0 0 - 2 0 0 - 3 0 0 - 4 0 0 0 r " " ' . 1 ' 1 0 0 2 0 0 " ' " ' - - -~ 3 0 0 / 4 0 0 l i m e [ S ) r ,,~ 5 0 0 6 0 0 l 7 0 0 8 0 0 F i g u r e 6 . 5 0 - U p p e r a n d L o w e r B o u n d a r i e s o f P r e d i c t e d O b s t a c l e s A r e a t o w a r d s L o n g i t u d i n a l d i r e c t i o n f r o m C o n v e n t i o n a l a n d N o v e l M e t h o d s - 1 0 9 - 5 0 0 A c t u a l P a t h 4 0 0 N < M l l M e t h o d 3 0 0 C o m e n t i o n a l M e t h o d 2 0 0 I 1 0 0 //~~ / "~ . : / ' " - . _ " " ' 0 f f i U i 0 i 5 " " ~ " ' ~ - 1 0 0 ~ --~;~ - 2 0 0 - 3 0 0 - 4 0 0 - 5 0 0 0 1 0 0 2 0 0 3 0 0 4 0 0 T i m e [ S ] ~ " ' - " - - 5 0 0 ' \ , 6 0 0 7 0 0 B O O F i g u r e 6 . 5 1 - U p p e r a n d L o w e r B o u n d a r i e s o f P r e d i c t e d O b s t a c l e s A r e a t o w a r d s L a t e r a l d i r e c t i o n f r o m C o n v e n t i o n a l a n d N o v e l M e t h o d s 1 ( ) ( ) 9 0 8 0 7 0 6 0 I g 5 0 w 4 0 3 0 ; I 2 0 , o .i I 0 0 1 0 0 2 0 0 3 0 0 4 0 0 l i m e [ S ] 5 0 0 6 0 0 N o Y e l M e t h o d C o n v e n t i o n a l M e t h o d 7 0 0 B O O F i g u r e 6 . 5 2 - E r r o r t o w a r d s L o n g i t u d i n a l d i r e c t i o n f o r C o n v e n t i o n a l a n d N o v e l M e t h o d s - 1 1 0 - 1 0 0 9 0 8 0 7 0 6 0 ' E i 5 0 w 4 0 3 0 2 0 1 0 0 0 ~~\; 1 0 0 2 0 0 3 0 0 4 0 0 l i m e ( S ] 5 0 0 6 0 0 N o Y e l M e t h o d C o n w n t i o n a l M e t h O O 7 0 0 8 0 0 F i g u r e 6 . 5 3 - E r r o r t o w a r d s L a t e r a l d i r e c t i o n f o r C o n v e n t i o n a l a n d N o v e l M e t h o d s 5 0 0 4 5 0 4 0 0 3 5 0 : [ 3 0 0 ~ i 5 3 ' 2 5 0 " ' ~ g 2 0 0 1 5 0 , 1 0 0 5 0 0 0 1 0 0 2 0 0 ! 3 0 0 4 0 0 l i m e ( S ] 5 0 0 6 0 0 N o w l M e t h o d C o n w n t i o n a l M e t h o d I 7 0 0 8 0 0 F i g u r e 6 . 5 4 - W i d t h o f P r e d i c t e d O b s t a c l e A r e a t o w a r d s L o n g i t u d i n a l d i r e c t i o n f r o m C o n v e n t i o n a l a n d N o v e l M e t h o d s 1 0 0 8 0 6 0 4 0 I " ' ' 5 3 : : 2 0 a l 1 . 1 : J a l a : 0 - 2 0 - 4 0 - 6 0 0 1 0 0 2 0 0 3 0 0 4 0 0 5 0 0 6 0 0 7 0 0 B O O l i m e [ S ) F i g u r e 6 . 5 5 - P r e d i c t e d D i s t a n c e r e d u c t i o n f r o m N o v e l M e t h o d A b o v e f i g u r e p r e s e n t s t h e e f f e c t i v e n e s s o f e m p l o y i n g N o v e l m e t h o d f o r D y n a m i c o b s t a c l e a v o i d a n c e w e l l . B u t t h e N o v e l m e t h o d i s n o t r e d u c i n g t h e p r e d i c t e d a r e a t o t a l l y . I t m a y i n c r e a s e t h a t a r e a a t s h a r p t u r n s r e s p e c t t o t h e c o n v e n t i o n a l m e t h o d . I t c a n b e o b s e r v e d f r o m t h e l o w e r s i d e o f t h e g r a p h g i v e n i n F i g u r e 6 . 5 5 . B u t t h e o v e r a l l p e r f o r m a n c e o f t h e p r o p o s e d m e t h o d i s b e t t e r t h a n c o n v e n t i o n a l o n e a t t h e e n d . - 1 1 2 - C h a p t e r 6 C o n c l u s i o n a n d R e c o m m e n d a t i o n s T h i s \ v o r k h a s p r e s e n t e d a n d d e m o n s t r a t e d s o m e n o v e l a l g o r i t h m s f o r s t a t i c a n d d y n a m i c o b s t a c l e a v o i d a n c e . T h e y h a v e b e e n c o m p a r e d w i t h o t h e r f a m o u s o b s t a c l e a v o i d a n c e m e t h o d s a s w e l l . O t h e r r e s e a r c h w o r k s h a v e p r e s e n t e d a n d d e m o n s t r a t e d t h e c a p a b i l i t i e s o f u t i l i z i n g O A m e t h o d s o f U G V s f o r U S V s . I m p l e m e n t i n g U G V m e t h o d s p r a c t i c a l l y w h i c h w e r e d e v e l o p e d a n d v a l i d a t e d v i a s i m u l a t i o n s , w a s d o n e a s t h e i n i t i a t i v e f o r t h e d e v e l o p m e n t . I t i s o b v i o u s l y t r u e t h a t p r a c t i c a l e x p e r i m e n t r e s u l t s w o u l d c o n t r a d i c t l i t t l e b i t w i t h t h e s i m u l a t i o n r e s u l t s d u e t o t h e i m m a t u r i t y o f t h e a v a i l a b l e h a r d w a r e . H O \ v c v e r t h o s e o b s t a c l e a v o i d a n c e m e t h o d s a r e t r a n s f o r m e d t o a c h i e v e a b e t t e r O A m e t h o d f o r U S V s . A g o o d F u z z y b a s e d n a v i g a t i o n a l c o n t r o l l e r f o r d y n a m i c m o d e l o f a U S V w a s d e v e l o p e d a n d i t g i v e s r e s u l t s a s e x p e c t e d , n e e d e d t o p e r f o r m o b s t a c l e a v o i d a n c e a l g o r i t h m s . T h e f u z z y b a s e d o b s t a c l e a v o i d a n c e c o n t r o l l e r i s d e v e l o p e d a n d l e g a c y o f t h a t f u z z y c o n t r o l l e r i s t h e a l g o r i t h m w h i c h w a s d e v e l o p e d f o r e m e r g e n c y c o l l i s i o n a v o i d a n c e o f g r o u n d v e h i c l e s . T h a t a l g o r i t h m w a s p r a c t i c a l l y i m p l e m e n t e d a t t h e b e g i n n i n g o f t h i s r e s e a r c h . T h a t f u z z y - l o g i c b a s e d s y s t e m g i v e s p r o m i s i n g r e s u l t s i n s i m u l a t i o n s . E v e n t h o u g h i t g i v e s p r o m i s i n g r e s u l t s f o r f a r a w a y o b s t a c l e s , t h e s i m u l a t i o n r e s u l t s s h o w t h a t i t i s n o t m u c h r e l i a b l e f o r l o w s p e e d s , n o i s e s a n d o b s t a c l e s w h i c h a p p e a r s u d d e n l y . T h e n a n o t h e r n o v e l a l g o r i t h m w a s i n t r o d u c e d , w h i c h i s g o o d f o r l o w s p e e d s a s w e l l a s h i g h s p e e d s . T h a t w a s i n h e r i t e d f r o m t h e M o r p h i n a l g o r i t h m o f C a r n e g i e M e l l o n U n i \ · e r s i t y . T h e e x a c t d e t a i l s w e r e n o t p u b l i s h e d a n d h a d t o b u i l d n o v e l a l g o r i t h m s f o r t h a t . T h e p o t e n t i a l f i e l d m e t h o d i s u s e d f o r o b s t a c l e a v o i d a n c e a s w e l l s i n c e i t d o m i n a t e s p a t h p l a n n i n g o f r o b o t s . S o f t w a r e i s u t i l i z e d f o r t h o s e s i m u l a t i o n s s u c c e s s f u l l y . - 1 1 3 - 9 ' ; , _ . ? ~~ A c c o r d i n g t o t h e r e s u l t s w h i c h a r e p r e s e n t e d i n t h e f i g u r e s i t c a n b e c o n c l u d e d t h a t t h e n o v e l a l g o r i t h m i s c a p a b l e o f a c h i e v i n g t h e t a r g e t w i t h m i n i m u m t r a v e l i n g d i s t a n c e . F u r t h e r a l l o f t h o s e a l g o r i t h m s w o r k w e l l a n d n e e d t o b e c u s t o m i z e d m o r e w i t h t h e s i t u a t i o n a n d a p p l i c a t i o n s i n c e e a c h a n d e v e r y o n e i s h a v i n g i t s o w n p l u s e s a n d 1 1 l l l 1 U S e S . O b s t a c l e a v o i d a n c e w i t h o u t d y n a m i c o b s t a c l e s i s n o t f u n c t i o n a l o n i n s e a . C o m p l e t e m e t h o d f o r d y n a m i c o b s t a c l e a v o i d a n c e i s y e t t o b e s o l v e d i n r e s e a r c h f i e l d . B u t s o m e p r a c t i c a l d y n a m i c o b s t a c l e a v o i d a n c e m e t h o d s a r e b e i n g e m p l o y e d t o d a y . M o v i n g p a t h p r e d i c t i o n o f a d y n a m i c o b s t a c l e i s t h e b i g g e s t p r o b l e m t o b e s o l v e d b y t h e r e s e a r c h e r s a n d t w o a p p r o a c h e s a r e d e v e l o p e d t o s o l v e t h a t . R B N N a n d s t a n d a r d p o l y n o m i a l a p p r o x i m a t i o n s a r e c h o s e n a s t w o a p p r o a c h e s s i n c e R B N N a r e v e r y f a m o u s f o r f u n c t i o n p r e d i c t i o n p u r p o s e s . R B N N a n d P o l y n o m i a l a p p r o x i m a t i o n m e t h o d s a r e e m p l o y e d f o r p a t h p r e d i c t i o n a n d c o m p a r e d . I t c a n b e c o n c l u d e d t h a t R B N N m e t h o d i s g o o d f o r p a t h p r e d i c t i o n p u r p o s e s b e c a u s e i t c a n b e u t i l i z e d w i t h h i g h n o i s e v a l u e s a s w e l l . A s m o o t h p r e d i c t e d p a t h c a n b e o b t a i n e d b y i n c r e a s i n g t h e s p r e a d v a l u e o f R B N N . A n o v e l d y n a m i c o b s t a c l e a r e a p r e d i c t i o n m e t h o d i s i n t r o d u c e d a n d i t i s c o m p a r e d w i t h t h e c o n v e n t i o n a l v e l o c i t y o b s t a c l e m e t h o d . S i m u l a t i o n r e s u l t s p r o v e t h e i m p r o v e m e n t o f t h e n o v e l m e t h o d n o t i c e a b l y . T h r e e s t a t i c o b s t a c l e a v o i d a n c e m e t h o d s a n d n o v e l d y n a m i c o b s t a c l e a v o i d a n c e m e t h o d w h i c h i s i n h e r i t e d f r o m p r o j e c t e d o b s t a c l e a r e a m e t h o d p r e s e n t e d a n d s i m u l a t i o n s d o n e t o p r o v e t h e v a l i d i t y o f t h o s e m e t h o d s w i t h s e n s o r n o i s e . T h e h a r d w a r e o f t h e U S V h a s t o b e d e v e l o p e d f i r s t . T h e n t h e s e a l g o r i t h m s c a n b e e m p l o y e d v v i t h t h o s e s e n s o r s . T h e s e a l g o r i t h m s h a v e t o b e f i n e t u n e d . T h e p e r f o r m a n c e o f t h e U S V c a n i m p r o v e b y u t i l i z i n g C u t t i n g - e d g e t e c h n o l o g y f o r s e n s o r s . T h i s w i l l l e a d t o a n e y e o p e n i n g f o r U S V d e v e l o p e r s i n t h e S r i L a n k a a n d w i l l b e a b l e t o f i l l t h e g a p s o f r e s e a r c h w o r k s o n U S V s i n t h e w o r l d . - 1 1 4 - 9 . , R e f e r e n c e s [ 1 1 B o r e n s t e i n , J . a n d K o r e n , Y . , " R e a l - t i m e O b s t a c l e A v o i d a n c e f o r F a s t M o b i l e R o b o t s i n C l u t t e r e d E n v i r o n m e n t s . " T h e 1 9 9 0 I E E E I n t e r n a t i o n a l C o n f e r e n c e o n R o b o t i c s a n d A u t o m a t i o n , C i n c i n n a t i , O h i o , M a y 1 9 9 0 , p p . 5 7 2 - 5 7 7 . [ 2 ] D a w n J . W r i g h t , ' T h e F i r s t T h r e e - D i m e n s i o n a l N a u t i c a l C h a r t ' , U n d e r s e a w i t h G I S , C h a p t e r 7 , E S R I . I n c . , 2 0 0 2 . [ 3 ] · D i g i t a l E n c o r d e r s ' , A g i l e n t T e c h n o l o g i e s I n c . , 5 3 0 1 S t e v e n s C r e e k B l v d , S a n t a C l a r a , C A 9 5 0 5 1 , U S A , 2 0 0 6 . l 4 J ' E R - 4 0 0 T r a n s c e i v e r M o d u l e s ' , L P R S L t d . , T w o R i v e r s I n d u s t r i a l E s t a t e , S t a t i o n L a n e , W i t n e y , O x o n , O X 2 8 4 B H , U K , 2 0 0 7 . l 5 ] G e e t h J a y e n d r a , S i s i l K u m a r a w a d u , L a k s h a n P i y a s i g h e , " F u z z y L o g i c - B a s e d N a v i g a t o r f o r a n U n m a n n e d S u r f a c e V e h i c l e " 1 4 1 1 1 E R U S y m p o s i u m - 2 0 0 8 , C o l o m b o , S r i L a n k a , O c t o b e r 2 0 0 8 . [ 6 ] G e e t h J a y e n d r a , S i s i l K u m a r a w a d u , L a k s h a n P i y a s i g h e , " M o r p h i n b a s e d O b s t a c l e A v o i d a n c e C o n t r o l l e r f o r U S V " , 1 4 t h E R U S y m p o s i u m - 2 0 0 8 , C o l o m b o , S r i L a n k a , O c t o b e r 2 0 0 8 . [ 7 ] G e e t h J a y e n d r a , S i s i l K u m a r a w a d u , L a k s h a n P i y a s i g h e , " O b s t a c l e A v o i d a n c e C o n t r o l l e r f o r a n U n m a n n e d S u r f a c e V e h i c l e " , 1 4 t h E R U S y m p o s i u m - 2 0 0 8 , C o l o m b o , S r i L a n k a , O c t o b e r 2 0 0 8 . [ 8 ] G e e t h J a y e n d r a , S i s i l K u m a r a w a d u , R a v i R a n a t h u n g a a n d S a m i t h a R a n s a r a , ' ' I n t r a c t i v e I n t e l l i g e n t C o l l i s i o n A v o i d a n c e C o n t r o l l e r : T h e o r y a n d E x p e r i m e n t s " , P r o c e e d i n g s o f 2 n d I n t e r n a t i o n a l C o n f e r e n c e o f I n d u s t r i a l a n d I n f o r m a t i o n S y s t e m s , P e r a d e n i y a , S r i L a n k a , A u g u s t 2 0 0 7 . [ 9 J · G e n e r a l i z e d R e g r e s s i o n N e t w o r k s ' , M a t h w o r k s I n c . a v a i l a b l e w v v w . m a t h w o r k s . c o m , a c c e s s e d o n D e c e m b e r 2 0 0 8 . [ 1 O J H . L e e a n d M . T o m i z u k a , " C o o r d i n a t e d l o n g i t u d i n a l a n d l a t e r a l m o t i o n c o n t r o l o f v e h i c l e s f o r I V H S " , A S M E J o u r n a l o f S y s t e m s , M e a s u r e m e n t , a n d C o n t r o l , V o l . 1 2 3 , 2 0 0 1 , p p . 5 3 5 - 5 4 3 . f l 1 ] ' H S - 4 2 2 S t a n d a r d D e l u x e S e r v o m o t o r ' , H i t e c R C D U S A , I n c . , 1 2 1 1 5 P a i n e S t , P o w a y , C A 9 2 0 6 4 , U S A . [ 1 2 ] I . F a n t o n i , R . L o z a n o , F . M a z e n c , a n d K . P e t t e r s e n , " S t a b i l i z a t i o n o f a n o n l i n e a r u n d e r - a c t u a t e d h o v e r c r a f t " , I n t e r n a t i o n a l J o u r n a l o f R o b u s t a n d N o n l i n e a r C o n t r o l , v o l . 1 0 , 2 0 0 0 , p p . 6 4 5 - 6 5 4 . - 1 1 5 - ~ ' 7 \ ·~~ ·~ / l l 3 ] J . C a n n y a n d J . R e i f , " N e w l o w e r b o u n d t e c h n i q u e s f o r r o b o t m o t i o n p l a n n i n g p r o b l e m " . , P r o c e e d i n g s o f 2 8 1 1 1 A n n u a l I E E E S y m p . o n F o u n d a t i o n o f C o m p u t e r S c i e n c e , L o s A n g e l e s , C A 1 9 8 7 , p p . 4 9 - 6 0 . [ 1 4 ] J . E b k c n , M . B r u c h , J . L u m , " A p p l y i n g u n m a n n e d g r o u n d v e h i c l e t e c h n o l o g i e s t o u n m a n n e d s u r f a c e v e h i c l e s " , P r o c e e d i n g s o f S P I E U n m a n n e d G r o u n d V e h i c l e T e c h n o l o g y V I I , v o l . 5 8 0 4 , O r l a n d o , F L , 2 0 0 5 , p p . 5 8 5 - 5 9 6 . [ 1 5 ] K . F u j i m u r a a n d H . S a m e t , " A h i e r a r c h i c a l s t r a t e g y f o r p a t h p l a n n i n g a m o n g m o v i n g o b s t a c l e s " , I E E E T r a n s a c t i o n s o r i R o b o t i c s a n d A u t o m a t i o n , v o l . 5 , 1 9 8 9 , p p . 6 1 - 6 9 . [ 1 6 ] K e v i n J . W a l c h k o , M i c h a e l C . N e c h y b a , E r i c S c h w a r t z , A n t o n i o A r r o y o , " E m b e d d e d L o w C o s t I n e r t i a l N a v i g a t i o n S y s t e m " U n i v e r s i t y o f F l o r i d a , G a i n c s v i l l e , F L , 3 2 6 1 1 - 6 2 0 0 . l 1 7 ] K e v i n M . P a s s i n o , " I n t e l l i g e n t C o n t r o l : A n O v e r v i e w o f T e c h n i q u e s " , D e p a r t m e n t o f E l e c t r i c a l E n g i n e e r i n g , T h e O h i o S t a t e U n i v e r s i t y , 2 0 1 5 N e i l A v e n u e , C o l u m b u s , 2 0 0 6 . [ 1 8 ] K h a t i b , . 0 . , " R e a l - T i m e O b s t a c l e A v o i d a n c e f o r M a n i p u l a t o r s a n d M o b i l e R o b o t s . " 1 9 8 5 I E E E I n t e r n a t i o n a l C o n f e r e n c e o n R o b o t i c s a n d A u t o m a t i o n , S t . L o u i s , M i s s o u r i , M a r c h 1 9 9 0 , p p . S 0 0 - 5 0 5 . [ 1 9 ] K J C K u m a r a a n d S i s i l K u m a r a w a d u , " O n t h e S p e e d c o n t r o l f o r A u t o m a t e d S u r f a c e V e s s e ! O p e r a t i o n " , T h i r d I n t e r n a t i o n a l C o n f e r e n c e o n I n f o r m a t i o n a n d A u t o m a t i o n f o r S u s t a i n a b i l i t y , M e l b o u r n e , A u s t r a l i a , D e c e m b e r 2 0 0 7 . [ 2 0 ] L e e F . , a n d K r o v i , V . , " A S t a n d a r d i z e d T e s t i n g - G r o u n d f o r A r t i f i c i a l P o t e n t i a l - F i e l d b a s e d M o t i o n P l a n n i n g f o r R o b o t C o l l e c t i v e s " , P r o c e e d i n g s o f t h e 2 0 0 6 P e r f o r m a n c e M e t r i c s f o r I n t e l l i g e n t S y s t e m s W o r k s h o p , G a i t h e r s b u r g , M D , A u g u s t 2 0 0 6 . l 2 1 ] M a r k M a i m o n e , J e f f r e y B i e s i a d e c k i , E d w a r d T u n s t e l , Y a n g C h e n g , C h r i s L e g e r , " I n t e l l i g e n c e f o r S p a c e R o b o t i c s " , N A S A J e t P r o p u l s i o n L a b o r a t o r y , U S A , 2 0 0 6 , p p . 5 3 . l 2 2 ] N . A g g a r w a l a n d K . F u j i m u r a , " M o t i o n p l a n n i n g a m i d s t p l a n a r m o v m g o b s t a c l e s " , P r o c e e d i n g s o f I E E E I n t e r n a t i o n a l C o n f e r e n c e o n R o b o t i c s a n d A u t o m a t i o n , v o l . 3 , S a n D i e g o , C A , 1 9 9 4 , p p . 2 1 5 3 - 2 1 5 8 . [ 2 3 ] ' N a v N e t 3 D B r o c h u r e ' , F u r u n o E l e c t r i c C o . , L t d . , A v a i l a b l e : h t t p : / / w w w . n a v n e t . c o m / , a c c e s s e d o n M a y 2 0 0 8 . - 1 1 6 - \ ; ' l : t . ~ · ) f ) ! 2 4 ] · o v e r v i e w o f F u z z y L o g i c T o o l b o x - M a t l a b H e l p F i l e s a n d D o c u m e n t a t i o n ' , M a t h w o r k s I n c . a v a i l a b l e : w w w . m a t h w o r k s . c o m , a c c e s s e d o n 0 5 1 1 1 S e p 2 0 0 7 . [ 2 5 ] P . F i o r i n i a n d Z . S c h i l l e r , " M o t i o n p l m m i n g i n d y n a m i c e n v i r o n m e n t s u s i n g t h e r e l a t i v e v e l o c i t y p a r a d i g m " , P r o c e e d i n g s o f l E E E I n t e r n a t i o n a l C o n f e r e n c e o n R o b o t i c s a n d A u t o m a t i o n , v o l . l , A t l a n t a , G A , 1 9 9 3 , p p . 5 6 0 - 5 6 5 . l 2 6 j ' P o l y n o m i a l c u r v e f i t t i n g ' , M a t h w o r k s l n c . a v a i l a b l e W ' v V w . m a t h w o r k s . c o m , a c c e s s e d o n D e c e m b e r 2 0 0 8 . [ 2 7 ] ' P r o t e c t o r , U n m a n n e d S u r f a c e V c h i c l e ' , D e f e n s e U p d a t e , I n t e r n a t i o n a l O n l i n e D e f e n s e M a g a z i n e , 2 0 0 6 . A v a i l a b l e : h t t p : / / w w w . d e f e n s e u p d a t e . c o m / p r o d u c t s / p / p r o t e c t o r . h t m , a c c e s s e d o n O c t o b e r 2 0 0 8 . [ 2 8 ] ' R a d a r P r o b l e m s ' , S c i T e c h P u b l i s h i n g , I n c . , A v a i l a b l e : h t t p : / / r a d a r p r o b l e m s . c o m / , a c c e s s e d o n M a y 2 0 0 8 . [ 2 9 ] R e i d S i m m o n s , L a r s H e n r i k s e n , L o n n i e C h r i s m a n a n d G r e g W h e l a n , " O b s t a c l e A v o i d a n c e a n d S a f e g u a r d i n g f o r a L u n a r R o v e r " , A I A A F o r u m o n A d v a n c e d D e v e l o p m e n t s i n S p a c e R o b o t i c s , M a d i s o n , W I , A u g u s t 1 9 9 6 . [ 3 0 ] R e n j u T h o m a s , M a n o j F r a n k l i n , C h r i s W i l k e r s o n a n d J a r e d S t a r k . , " I m p r o v i n g b r a n c h p r e d i c t i o n b y d y n a m i c d a t a f l o w b a s e d i d e n t i f i c a t i o n o f c o r r e l a t e d b r a n c h e s f r o m a l a r g e g l o b a l h i s t o r y " , P r o c e e d i n g s o f t h e 3 0 t h I n t e r n a t i o n a l S y m p o s i u m o n C o m p u t e r A r c h i t e c t u r e , S a n D i e g o , C a l i f o r n i a , J u n e 2 0 0 3 . [ 3 1 ] R i c h a r d B i s h h o p , " I n t e l l i g e n t v e h i c l e a p p l i c a t i o n s w o r l d w i d e " , I E E E T r a n s a c t i o n s o n I n t e l l i g e n t T r a n s p o r t a t i o n S y s t e m s , v o l . 5 , 2 0 0 0 , p p . 7 8 - 8 1 . [ 3 2 ] S . R . R a n a t u n g a , " O n I n t e r a c t i v e C o n t r o l F o r I n t e l l i g e n t C o l l i s i o n E v a s i v e E m e r g e n c y I n t e r v e n t i o n I n S m a r t V e h i c l e s " , M a s t e r o f S c i e n c e T h e s i s S u b m i t t e d t o D e p t . E l e c t r i c a l E n g i n e e r i n g , U n i v e r s i t y o f M o r a t u w a , S r i L a n k a , J a n u a r y 2 0 0 7 . [ 3 3 ] S . R . R a n a t u n g a a n d S . P . K u m a r a w a d u , " C o o p e r a t i v e l y C o n t r o l l e d C o l l i s i o n E v a s i v e E m e r g e n c y M a n o e u v r e s " , C o n t r o l a n d I n t e l l i g e n t S y s t e m s , A C T A p r e s s , v o l . 4 , 2 0 0 8 . [ 3 4 ] S h i n g o S h i m o d a , Y o j i K u r o d a a n d K a r l I a g n e m m a , " P o t e n t i a l F i e l d N a v i g a t i o n o f H i g h S p e e d U n m a n n e d G r o u n d V e h i c l e s o n U n e v e n T e r r a i n " , P r o c e e d i n g s o f t h e 2 0 0 5 I E E E , I n t e r n a t i o n a l C o n f e r e n c e o n R o b o t i c s a n d A u t o m a t i o n , B a r c e l o n a , S p a i n , A p r i l 2 0 0 5 . - 1 1 7 - ~ / , . [ 3 5 ] S i s i l K u m a r a w a d u a n d T s u - T i a n L e e , ' N e u r o a d a p t i v e C o m b i n e d L a t e r a l a n d l o n g i t u d i n a l C o n t r o l o f H i g h w a y V e h i c l e s U s i n g R B F N e t w o r k s ' , I E E E T r a n s a c t i o n s o n I n t e l l i g e n t T r a n s p o r t a t i o n S y s t e m s , V o l . 1 7 , N o . 4 , D e c e m b e r 2 0 0 6 , p p . 5 0 0 - 5 1 2 . [ 3 6 ] S t e p h e n W . S o l i d a y , " F u z z y C o n t r o l l e r F o r T w o W h e e l I n d e p e n d e n t D r i v e P i v o t S t e e r i n g V e h i c l e " , N o r t h C a r o l i n a A g r i c u l t u r a l a n d T e c h n i c a l S t a t e U n i v e r s i t y , D e p a r t m e n t o f E l e c t r i c a l E n g i n e e r i n g , 2 0 0 6 . [ 3 7 ] T a n n e n S . V a n Z w i e t e n , ' D y n a m i c s i m u l a t i o n a n d c o n t r o l o f a n a u t o n o m o u s s u r f a c e v e h i c l e ' , M a s t e r o f S c i e n c e t h e s i s s u b m i t t e d t o t h e F l o r i d a A t l a n t i c U n i v e r s i t y , F l o r i d a , D e c e m b e r 2 0 0 3 . [ 3 8 ] ' T h e P r o t e c t o r U n m a n n e d S u r f a c e V e h i c l e ( U S V ) f r o m o n t h e W a t e r ' , G i z m a g , S t K i l d a S o u t h , V i c t o r i a , A u s t r a l i a , 2 0 0 8 . A v a i l a b l e : h t t p : / / w w w . g i z m a g . c o m / g o / 6 0 2 3 / , a c c e s s e d o n O c t o b e r 2 0 0 8 . [ 3 9 ] T i l o v e , R . B . , " L o c a l O b s t a c l e A v o i d a n c e f o r M o b i l e R o b o t s B a s e d o n t h e M e t h o d o f A r t i f i c i a l P o t e n t i a l s . " G e n e r a l M o t o r s R e s e a r c h L a b o r a t o r i e s , R e s e a r c h P u b l i c a t i o n G M R - 6 6 5 0 , S e p t e m b e r 1 9 8 9 . [ 4 0 ] T s o u k a l a s , L . H . a n d U h r i g , R . E . ' F u z z y a n d N e u r a l A p p r o a c h e s m E n g i n e e r i n g ' , J o h n W i l e y a n d S o n s , N e w Y o r k , 1 9 9 7 . [ 4 1 ] Y . K o r e n a n d J . B o r e n s t e i n , ' P o t e n t i a l F i e l d M e t h o d s a n d T h e i r I n h e r e n t L i m i t a t i o n s f o r M o b i l e R o b o t N a v i g a t i o n ' , P r o c e e d i n g s o f t h e I E E E C o n f e r e n c e o n R o b o t i c s a n d A u t o m a t i o n , S a c r a m e n t o , C a l i f o r n i a , A p r i l 1 9 9 1 , p p . 1 3 9 8 - 1 4 0 4 . - 1 1 8 - ~ \ : , . : : ! " . . . . / A p p e n d i x A O O P i c B a s i c P r o g r a m f o r V e h i c u l a r P r o t o t y p e s D i m V e r A s N e w o B y t e D i m S R F O B A s N e w o i 2 C A s o S p e a k e r S p k D i m l J i m D i m D i m R a n g e l A s N e w o h ' o r d ' T o s t o r e s o n a r v a l u e s R a n g e 2 A s N e w o W o r d R a n g e 3 A s N e w o W o r d R a n g e 4 A s N e w o W o r d D i n C o m p a s s A s N e w o i 2 C ' C r e a t e t h e c o m p a s s o b j e c t s D i m L e d A s N e w o D I O l [ J i m B e a r i n g A s N e w o B y t e D i m B e a r i n g l A s N e w o B y t e D i m B e a r i n g 2 A s N e w o B y t e D i m A A s N e w o S e r i a l X ' O b j e c t s D i m B A s N e w o S e r i a l X D i m C A s N e w o D I O l D i m D A s N e w o D I O l f o r s e r i a l J i m E N C A s N e w o Q e n c o d e ' E n c o r d e r o b j e c t s D i m E N C p o s i t i o n N A s N e w o W o r d l J i m E N C p o s i t i o n O A s N e w o W o r d D i m E N C p o s i t i o n A s N e w o W o r d D i m E n c P o s l A s N e w o W o r d D i m E n c P o s 2 A s N e w o W o r d D i m c o u n t A s N e w o W o r d D i m E A s N e w o D I O l ' M o t o r c o n t r o l D . i m F F , s N e w o D I O l : J i m G P " s N e w o B y t e ' ) i n H A s N e w o D I O l D i m I A s N e w o D I O l D i m Y A s o B y t e D i m X A s o B y t e 'Pu~ition u p d a t e s D i m T x C o u n t A s N e w o W o r d :~b m a i n ( ) Corr~unicationSetup C o : n p a s s S e t u p E : n c o r d e r S c : t u p S o n a r S e t u p :~1otorSetup : . _ : o u n t = O ; 1 , J l i . . . : , I c o m m u n i c a t i o n - 1 1 9 - \ ~\ , , 1 i ) · ' i l - ) ( ) T x C o u n t = O C . I n v e r t E n c o r d e r V a l u e s R e a d i n g F o r m a t E n c P o s i t i o n S o n a r V a l u e s R e a d i n g O b s t a l C h e c k i n g C o m p a s s V a l u e R e a d i n g I f ( ( B e a r i n g < 5 ) O r ( B e a r i n g > 2 5 0 ) ) T h e n ' S p k . B e e p ( 6 0 7 5 7 , 1 0 0 , 2 0 0 ) E n d I f F o r m a t B e a r i n g C . I n v e r t I f G = 1 T h e n M o t o r D r i v e E n d i f D a t a T r a n s m i s s i o n T x C o u n t = T x C o u n t + 1 I f T x C o u n t = 4 T h e n T x C o u n t = O E n d i f L o o p : C : n d S u b S u b C o m p a s s S e t u p C o m p a s s . N o d e = 9 6 ' D e c i m a l o f H e x a d d r e s s O x C O s h i f t e d r i g h t b y 1 C o m p a s s . M o d e = c v 1 0 B i t ' I 2 C m o d e i s 1 0 - B i t A d d r e s s i n g . C o m p a s s . N o i n c = 1 ' D o n ' t i n c r e m e n t L e d . I O L i n e = 3 0 ' P i n 2 8 o n 4 0 w a y c o n n e c t o r L e d . D i r e c t i o n E n d S u b c v O u t p u t S u b C o m m u n i c a t i o n S e t u p C . I O L i n e = 5 D . I O L i n e = 6 C . D i r e c t i o n c v O u t p u t ' S t a r t i n g c o m m u n i c a t i o n D . D i r e c t i o n = c v O u t p u t B . I O L i n e S = 2 5 B . I O L i n e F = 1 8 B . B a u d = c v 9 6 0 0 A . I O L i n e S = 2 6 A . I O L i n e F = 1 6 1 0 " . B a - e d = c v 9 6 0 0 - 1 2 0 - ) I , , " E n d S u b S u b E n c o r d e r S e t u p o o P I C . P u l l O p = 1 E N C . I O L i n e l = 2 8 E N C . I O L i n e 2 E N C . O p e r a t e E n d S u b S u b S o n a r S e t u p 2 9 c v T r u e ' F o r E n c o r d e r S R F 0 8 . N o d e = 1 1 2 ' D e c i m a l o f H e x a d d r e s s O x E O s h i f t e d r i g h t b y 1 S R F 0 8 . M o d e = c v l O B i t ' I 2 C m o d e i~ 1 0 - B i t A d d r e s s i n g . S R F 0 8 . N o i n c = 1 ' D o n ' t i n c r e m e n t S R F 0 8 . W i d t h = c v 8 B i t ' 1 b y t e w i d e t r a n s f e r S R F 0 8 . L o c a t i o n = 0 ' R a n g e R e g i s t e r S R F 0 8 = 8 1 ' L i m i t 1 s t s o n a r R a n g e t o 6 m , 1 4 0 x 4 3 m m = 6 m S R F 0 8 . N o d e = 1 1 3 ' D e c i m a l o f H e x a d d r e s s O x E 2 s h i f t e d R i g h t b y 1 S R F 0 8 . M o d e = c v l O B i t ' I 2 C m o d e i s ~O-Bit A d d r e s s i n g . S R F 0 8 . N o i n c = 1 ' D o n ' t i n c r e m e n t S R F 0 8 . W i d t h = c v 8 B i t ' 1 b y t e w i d e t r a n s f e r S R F 0 8 . L o c a t i o n = 0 ' R a n g e R e g i s t e r S R F 0 8 = 8 1 ' L i m i t 2 n d s o n a r R a n g e t o 6 m , 1 4 0 x 4 3 m m = 6 m S R F 0 8 . N o d e = 1 1 4 ' D e c i m a l o f H e x a d d r e s s O x E 4 s h i f t e d r i g h t b y 1 S R F 0 8 . M o d e = c v l O B i t ' I 2 C m o d e i s 1 0 - B i t A d d r e s s i n g . S R F 0 8 . N o i n c = 1 ' D o n ' t i n c r e m e n t S l \ F 0 8 . W i d t h = c v 8 B i t ' 1 b y t e w i d e t r a n s f e r S R F 0 8 . L o c a t i o n = 0 ' R a n g e R e g i s t e r S R F 0 8 = 8 1 ' L i m i t 3 r d s o n a r R a n g e t o 6 m , 1 4 0 x 43~~ = 6 m S R F 0 8 . N o d e = l l 5 ' D e c i m a l o f H e x a d d r e s s O x E 6 s h i f t e d r i g h t b y 1 S R F 0 8 . M o d e = c v l O B i t ' I 2 C m o d e i s 1 0 - B i t A d d r e s s i n g . S R F 0 8 . N o i n c = 1 ' D o n ' t i n c r e m e n t S R F 0 8 . W i d t h = c v 8 B i t ' 1 b y t e w i d e t r a n s f e r S R F 0 8 . L o c a t i o n = 0 ' R a n g e R e g i s t e r S R F 0 8 = 8 1 ' L i m i t 4 t h s o n a r R a n g e t o 6 m , 1 4 0 x 4 3 m m = 6 m E n d S u b S u b S o n a r V a l u e s R e a d i n g S R F 0 8 . N o d e = 1 1 2 ' I 2 C A d d r e s s o f S R F 0 8 s o n a r S R F 0 8 . L o c a t i o n = 0 ' C o m m a n d R e g i s t e r S R F 0 8 . W i d t h = c v 8 B i t ' 1 b y t e w i d e t r a n s f e r S R F 0 8 = 8 1 ' R a n g i n g C o m m a n d - R e s u l t i n e m D o ' W a i t f o r r a n g i n g t o c o m p l e t e V e r S R F 0 8 ' T h i s w i l l w a i t f o r e v e r i f y o u r s o n a r L o o p W h i l e V e r = 2 5 5 ' b e c o m e s d i s c o n n e c t e d , s o y o u m a y p r e f e r S R F 0 8 . W i d t h = c v l 6 B i t S R F 0 8 . L o c a t i o n = 2 ' 2 b y t e w i d e t r a n s f e r ' 1 s t R a n g e R e g i s t e r R a n g e l = S R F 0 8 ' S R F 0 8 . L o c a t i o n ' R a n g e 2 = S R F 0 8 ' G e t R a n g e t o 1 s t o b j e c t 3 ' 2 n d R a n g e R e g i s t e r S R F 0 8 . N o d e = 1 1 3 S R F 0 8 . L o c a t i o n = 0 ' G e t R a n g e t o 2 n d o b j e c t ' I 2 C A d d r e s s o f S R F 0 8 s o n a r ' C o m m a n d R e g i s t e r - 1 2 1 - > ) . · ; ; " . . . , I ~· S R F 0 8 . V ' J i d t h S R F 0 8 = 8 1 c v 8 B i t ' 1 b y t e w i d e t r a n s f e r ' R a n g i n g C o r r i D a n d - R e s u l t i n e m D o ' W a i t f o r r a n g i n g t o c o m p l e t e V e r S R F 0 8 ' T h i s w i l l w a i t f o r e v e r i f y o u r s o n a r L o o p W h i l e V e r = 2 5 5 ' b e c o m e s d i s c o n n e c t e d , s o y o u m a y p r e f e r S R F 0 8 . W i d t h = c v 1 6 B i t ' 2 b y t e w i d e t r a n s f e r S R F 0 8 . L o c a t i o n = 2 ' 1 s t R a n g e R e g i s t e r R a n g e 2 = S R F 0 8 ' G e t R a n g e S R F 0 8 . N o d e = 1 1 4 ' I 2 C A d d r € s s o f S R F 0 8 s o n a r S R F 0 8 . L o c a t i o n = 0 ' C o m m a n d R e g i s t e r S R F 0 8 . W i d t h = c v 8 B i t ' 1 b y t e w i d e t r a n s f e r S R F 0 8 = 8 1 ' R a n g i n g C o m m a n d - R e s u l t l n e r n D o ' W a i t f o r r a n g i n g t o c o m p l e t e V e r = S R F 0 8 ' T h i s w i l l w a i t f o r e v e r i f y o u r s o n a r L o o p W h i l e V e r = 2 5 5 ' b e c o m e s d i s c o n n e c t e d , s o y o u m a y p r e f e r S R F 0 8 . W i d t h = c v 1 6 B i t ' 2 b y t e w i d e t r a n s f e r S R F 0 8 . L o c a t i o n = 2 ' 1 s t R a n g e R e g i s t e r R a n g e 3 = S R F 0 8 ' G e t R a n S R F 0 8 . N o d e = 1 1 5 ' I 2 C A d d r e s s o f S R F 0 8 s o n a r S R F 0 8 . L o c a t i o n = 0 ' C o m m a n d R e g i s t e r S R F 0 8 . W i d t h = c v 8 B i t ' 1 b y t e w i d e t r a n s f e r S R F O 8 = 8 1 ' R a n g i n ; J C o m m a n d - R e s u l t i n e r n D o ' W a i t f o r r a n g i n g t o c o m p l e t e V e r = S R F 0 8 ' T h i s w i l l w a i t f o r e v e r i f y o u r s o n a r L o o p W h i l e V e r = 2 5 5 ' b e c o m e s d i s c o n n e c t e d , s o y o u m a y p r e f e r S R F 0 8 . W i d t h = c v 1 6 B i t ' 2 b y t e w i d e t r a n s f e r S R F 0 8 . L o c a t i o n = 2 ' 1 s t R a n g e R e g i s t e r R a n g e 4 = S R F 0 8 ' G e t R a n E n d S u b S u b C o r n p a s s V a l u e R e a d i n g C o m p a s s . L o c a t i o n = 1 ' A d d r e s s o f s i n g l e b y t e b e a r i n g C o r n p a s s . W i d t h = c v 8 B i t ' C o m p a s s D a t a i s 1 - b y t e w i d e . B e a r i n g = C o r n p a s s . V a l u e ' G e t i t E n d S u b S u b M o t o r S e t u p G = 1 E . I O L i n e F . I O L i n e H . I O L i n e I . I O L i n e E . D i r e c t i o n F . D i r e c t i o n H . D i r e c t i o n I . D i r e c t i o n E n d S u b 1 4 1 2 1 3 1 5 c v O u t p u t c v O u t p u t c v O u t p u t c v O u t p u t S u b M o t o r D r i v e I f 8 = 1 0 0 T h e n ' d D . I n v e r t - 1 2 2 - \ , t } , , , / , . E . L o w F . L o w H . L o w I . L o w E n d i f I f 8 = 1 0 3 T h e n ' g E . H i g h F . L o w H . H i g h I . L o w E n d i f I f 8 = 1 0 4 T h e n ' h E . L o w F . H i g h H . L o w I . H i g h E n d i f I f 8 = 1 0 1 T h e n ' e E . L o w F . H i g h H . L o w I . L o w E n d i f I f 8 = 1 0 2 T h e n ' £ E . L o w F . L o w H . L o w I . H i g h E n d i f E n d S u b S u b O b s t a 1 C h e c k i n g ' C h e c k t h e O b s t a c l e s G = 1 I f R a n g e 1 < 1 7 T h e n I f R a n g e 1 > 0 T h e n G = O E . L o w F . L o w H . L o w I . L o w ' S p k . 8 e e p ( 6 0 7 5 7 , 1 0 , 2 0 0 ) E n d i f E n d i f I f R a n g e 2 < 1 7 T h e n I f R a n g e 2 > 0 T h e n G = O E . L o w F . L o w H . L o w I . L o w ' S p k . 8 e e p ( 6 0 7 5 7 , 1 0 , 2 0 0 ) E n d i f E n d i f I f R a n g e 3 < 1 7 T h e n I f R a n g e 3 > 0 T h e n G = O E . L o v r - 1 2 3 - ) \ } j ' ' · c ' . . . . / i i . F . L o w H . L o v v I . L o w ' S p k . B e e p ( 6 0 7 5 7 , 1 0 , 2 0 0 ) E n d l f E n d i f I f R a n g e 4 < 1 7 T h e n I f R a n g e 4 > 0 T h e n G = O E . L o w F . L o w H . L o w I . L o w ' S p k . B e e p ( 6 0 7 5 7 , 1 0 , 2 0 0 ) E n d i f E n d i f E n d S u b S u b F o r m a t B e a r i n g I f B e a r i n g < 2 0 0 T h e n B e a r i n g 1 = B e a r i n g B e a r i n g 2 = 0 E n d I f I f B e a r i n g > 1 9 9 T h e n B e a r i n g 1 = 1 9 9 B e a r i n g 2 = B e a r i n g - 1 9 9 E n d i f E n d S u b S u b E n c o r d e r V a 1 u e s R e a d i n g ' R e a d s t h e E n c . V a l u e E N C p o s i t i o n N = E N C . P o s i t i o n c o u n t = c o u n t + 1 I f c o u n t = 1 0 0 T h e n E N C p o s i t i o n O = E N C p o s i t i o n N E N C p o s i t i o n N = E N C . P o s i t i o n E N C . P o s i t i o n . C l e a r c o u n t = O P o s i t i o n U p d a t e E n d i f E n d S u b S u b P o s i t i o n U p d a t e I f ( E N C p o s i t i o n N > E N C p o s i t i o n O ) T h e n E N C p o s i t i o n = E N C p o s i t i o n N - E N C p o s i t i o n O X = 1 0 0 + E N C p o s i t i o n * C o s ( B e a r i n g ) Y = 1 5 0 + E N C p o s i t i o n * S i n ( B e a r i n g ) E n d i f I f ( E N C p o s i t i o n N < E N C p o s i t i o n O ) T h e n E N C p o s i t i o n = E N C p o s i t i o n O - E N C p o s i t i o n N - 1 2 4 - \ ; : · } ·~' ' ) / l , i X = E N C p o s i t i o n * C o s ( B e a r i n g ) Y = E N C p o s i t i o n * S i n ( B e a r i n g ) E n d i f E n d S u b S u b F o r m a t E n c P o s i t i o n E n c P o s l = E N C p o s i t i o n N E n c P o s 2 = 0 I f E N C p o s i t i o n N > 2 4 9 T h e n E n c P o s l = 2 5 0 E n c P o s 2 = E N C p o s i t i o n N - 2 5 0 E n d i f E n d S u b S u b D a t a T r a n s m i s s i o n I f T x C o u n t = O T h e n A . V a l u e = 2 5 1 ' u S e n t E n c o r d e r v a l u e s A . V a l u e = X E l s e I f T x C o u n t = l T h e n A . V a l u e = 2 5 2 ' u S e n t E n c o r d e r v a l u e s A . V a l u e = Y E l s e I f T x C o u n t = 2 T h e n A . V a l u e = 2 5 3 ' s e n t C o m p a s s v a l u e A . V a l u e = B e a r i n g l E l s e I f T x C o u n t = 3 T h e n A . V a l u e = 2 5 4 ' A . V a l u e = B e a r i n g 2 E n d I f E n d S u b s e n t C o m p a s s v a l u e ~}- . : : · I - 1 2 5 - ' T h i s p r o g r a m c r e a t e s t w o o S e r i a l X O b j e c t s . O n e i s u s e d t o r e c e i v e a ' s e r i a l s i g n a l a n d t h e o t h e r i s u s e d t o s e n d a s e r i a l s i g n a l . ' 1 \ o D i o 1 i s u s e d t o s h o w t h a t w h i l e t h e o S e r i a l X o b j e c t i s w a i t i n g ' f o r i n c o m i n g s e r i a l d a t a , t h e p r o g r a m f l o w i s s t o p p e d . D i m A A s N e w o S e r i a 1 X D i m B A s N e w o S e r i a l X D i m C A s N e w o D I 0 1 D i m D A s N e w o D I 0 1 D i m V A s N e w B y t e l h m L A s N e w o P 1 i 7 M D i m R A s N e w o P W M D i m C o m p a s s A s N e w o i 2 C D i m L e d A s N e w o D I 0 1 D i m B e a r i n g A s N e w o B y t e . S u b M a i n ( ) C . I O L i n e = 5 D . I O L i n e = 6 C . D i r e c t i o n c v O u t p u t D . D i r e c t i o n = c v O u t p u t B . I O L i n e S = 2 5 B . I O L i n e F = 1 8 B . B a u d = c v 9 6 0 0 1 ' \ . I O L i n e S = 2 6 A . I O L i n e F = 1 6 l ' L B a u d = c v 9 6 0 0 C o m p a s s . N o d e = 9 6 r i g h t b y 1 C o m p a s s . M o d e = c v 1 0 B i t C o m p a s s . N o i n c = 1 L e d . I O L i n e = 3 0 L e d . D i r e c t i o n = c v O u t p u t D o L . P r e S c a l e = 2 L . I O L i n e = 1 7 R . P r e S c a l e = 2 R . I O L i n e = 1 8 L . P e r i o d = 7 9 R . P e r i o d = 7 9 C o r n p a s s . L o c a t i o n = 1 C o m p a s s . W i d t h = c v 8 B i t B e a r i n g = C o m p a s s . V a 1 u e ' A = B ' A . V a l u e = 9 9 ' c A . V a l u e = B e a r i n g ' A . S t r i n g = " B " C . I n v e r t V = B ' C r u i s i n g I f V = 1 0 0 T h e n ' d ' C r e a t e t h e c o m p a s s o b j e c t s ' D e c i m a l o f H e x a d d r e s s O x C O s h i f t e d ' I 2 C m o d e i s 1 0 - B i t A d d r e s s i n g . ' D o n ' t i n c r e m e n t ' P i n 2 8 o n 4 0 w a y c o n n e c t o r ' A d d r e s s o f s i n g l e b y t e b e a r i n g ' C o m p a s s D a t a i s 1 - b y t e w i d e . ' G e t i t - 1 2 6 - \ . : • ' . ! t ~ . . . . { f · ) L . V a l u e = l O R . V a l u e = l O L . O p e r a t e = l R . O p e r a t e = l E n d i f ' L e f t T u r n I f V = l O l T h e n ' e L . V a l u e = l 4 R . V a l u e = l O L . O p e r a t e = l R . O p e r a t e = l E n d i f ' R i g h t T u r n I f V = 1 0 2 T h e n ' f L . V a l u e = l O R . V a l u e = 1 4 L . O p e r a t e = l R . O p e r a t e = l E n d i f ' R e v e r s e I f V = 1 0 3 T h e n ' g L . V a l u e = l 6 R . V a l u e = 1 6 L . O p e r a t e = l R . O p e r a t e = l E n d i f ' S t o p I f V = 1 0 4 T h e n ' h D . I n v e r t L . V a l u e = l O R . V a l u e = l O L . O p e r a t e = O R . O p e r a t e = O E n d i f L o o p F : ! d S u b - 1 2 7 - \ ~·'} '~· ~ / i D i m V e r A s N e w o B y t e D i m S R F 0 8 A s N e w o i 2 C S p k A s o S p e a k e r D i m R a n g e l A s N e w o W o r d D i m R a n g e 2 A s N e w o W o r d D i m R a n g e ] A s N e w o W o r d D i m R a n g e 4 A s N e w o W o r d ~3ub m a i n ( ) S R F 0 8 . N o d e = 1 1 2 s h i f t e d r i g h t b y 1 S R F 0 8 . M o d e = c v l O B i t J , d d r e s s i n g . S R F 0 8 . N o i n c = 1 S R F 0 8 . W i d t h = c v 8 B i t S R F 0 8 . L o c a t i o n = 0 S R F 0 8 = 8 1 1 4 0 x 4 3 m m = 6 m D o e m : _ j o o p E n d S R F 0 8 . N o d e = 1 1 2 S R F 0 8 . L o c a t i o n = 0 S R F 0 8 . W i d t h = c v 8 B i t S R F 0 8 = 8 1 D o V e r = S R F 0 8 L o o p W h i l e V e r = 2 5 5 S R F 0 8 . W i d t h = c v l 6 B i t S R F 0 8 . L o c a t i o n = 2 R a n g e l = S R F 0 8 ' S R F 0 8 . L o c a t i o n = 3 ' R a n g e 2 = S R F 0 8 I f R a n g e l < l 7 T h e n I f R a n g e l > O T h e n S p k . B e e p ( 6 0 7 5 7 , 1 0 , 2 0 0 ) E n d i f E n d i f ' D e c i m a l o f H e x a d d r e s s O x E O I I 2 C m o d e i s 1 0 - B i t ' D o n ' t i n c r e m e n t ' 1 b y t e w i d e t r a n s f e r ' R a n g e R e g i s t e r ' L i m i t 1 s t s o n a r R a n g e t o 6 m , ' I 2 C A d d r e s s o f S R F 0 8 s o n a r ' C o m m a n d R e g i s t e r ' 1 b y t e w i d e t r a n s f e r ' R a n g i n g C o m m a n d - R e s u l t i n ' W a i t f o r r a n g i n g t o c o m p l e t e ' T h i s w i l l w a i t f o r e v e r i f y o u r s o n a r ' b e c o m e s d i s c o n n e c t e d , s o y o u m a y p r e f e r ' 2 b y t e w i d e t r a n s f e r ' 1 s t R a n g e R e g i s t e r ' G e t R a n g e t o 1 s t o b j e c t ' 2 n d R a n g e R e g i s t e r - 1 2 8 - d A p p e n d i x B M a t L a b P r o g r a m o f F u z z y P D N a v i g a t i o n a l C o n t r o l l e r w i t h t h e D y n a m i c m o d e l - 0 0 0 0 _ 0 0 0 _ 0 0 0 0 0 0 0 0 _ 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 c o~~~~b~b~~bbbbbb66b~bbbbbbbb~~bbb~~~b~bbbbbbb~tibtibtibfififi~tififibbfi , f u z z y P O c o n t r o l l e r w i t h D y n a m i c m o d e l 1 2 / 1 2 / 2 0 0 7 ~control F u z z y _ N a v i . m % % % % % % % % % % % % % % % % % % % % % % % % % % 9 o % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % f u n c t i o n X d o t = c o n t r o l F u z z y _ N a v i ( t , y ) % p r o g r a m f o r s p e e d c o n t r o l n = [ y ( l ) y ( 2 ) y ( 3 ) ] ' ; % C o n f i g u r a t i o n v e c t o r = [ x y e p ] V = [ y ( 4 ) y ( S ) y ( 6 ) ] ' ; % V e l o c i t y v e c t o r = [ u v r ] . j c [ c o s ( y ( 3 ) ) - s i n ( y ( 3 ) ) O ; s i n ( y ( 3 ) ) c o s ( y ( 3 ) ) 0 ; 0 0 1 ; ] ; r o t a t i o n m a t r i x n d o t = J * V ; % v e l o c i t i e s w . r . t . e a r t h f i x e d f r a m e d h p y = - 0 . 4 ; d l i s y = 0 . 4 ; % d i s t a n c e f r o m t h e C G t o t h e S b o a r d & P o r t s i d e h u l l s L h = 2 . 5 4 ; % l e n g t h o f t h e h u l l B h = 0 . 1 2 7 ; % b e a m T h = 0 . 1 5 2 4 ; % d r a f t A h = 0 . 6 4 5 2 ; % h u l l a r e a p = l 0 2 5 ; % d e n s i t y o f s e a w a t e r v i s = 1 . 4 * 1 0 A ( - 6 ) ; % k i n e m a t i c v i s c o s i t y 1 = 9 . 8 1 ; ~p=(5*2)*2.2; % m a s s o f b o t h p o n t o o n s [ k g ] [ p z = ( 1 / 1 2 ) * m p * ( T h A 2 + 8 h A 2 ) ; m a s s m o m e n t o f i n e r t i a a b o u t m a s s - c e n t e r z - a x i s [ k g * m A 2 ] m i s c . n e a r p o n t o o n s : : 1 1 = 1 1 * 2 . 2 ; = ( 1 3 * 2 ) * 2 . 2 ; r : l s = 3 . 6 * 2 . 2 ; = 3 3 * 2 . 2 ; % m a s s o f s t r u t [ k g ] . m i s e i n s i d e G e r t l e r b o d y ~3=4*2.2; m 4 = ( 1 1 + 8 7 ) * 2 . 2 ; m 5 = 2 5 * 2 . 2 ; 6 = 1 9 * 2 . 2 ; ~.qcc=l; % B o a t M a x a c c e l a r a t i o n r e s t = 4 0 5 ; · e s t a r r a y = t ; t o t a l % v e r . i c u l a r m a s s : r : = m p + m s + m g + m l + m 2 + m 3 + m 4 + m 5 + m 6 ; I z = ( 3 . 9 4 e 5 ) * 2 . 2 * ( 0 . 0 2 5 4 A 2 ) ; : : = [ r n , O , O ; O , m , O ; O , O , I z ] ; % m a s s % ( a p p r o x i m a t e v a l u e f r o m p r o - E ) m a t r i x f o r t h e r i g i d b o d y : - = [ 0 - m * y ( 6 ) 0 ; m * y ( 6 ) 0 0 ; 0 0 0 ; ] ; C o r i o l i s & c e n t r i p e t a l t e r m s f o r r i g i d % b o d y =~=C*V;%Coriolis t e r m s ; c ; = ( y ( 4 ) - d h p y * y ( 6 ) ) * L h / v i s ; % R e n o l d s N u m b e r 1 2 9 \ " r ' " ' , ! 7 ' i f R e > O & R e < 1 0 0 % a t l a m i n a t o t u r b u l a n t t r a n s i s t i o n p o i n t C f h p = 0 . 0 7 5 / ( ( l o g ( R e ) - 2 ) A 2 ) ; c c o e f f i c i e n t o f f r i c t i o n u s i n g I T T C f o r m u l a r e l s e i f R e > 1 0 0 C f h p = 0 . 0 7 5 / ( ( l o g ( R e ) - 2 ) A 2 ) ; e l s e C f h p = O ; % c o e f f i c i e n t o f f r i c t i o n u s i n g I T T C f o r m u l a r e n d F s h p x u r = C f h p * A h * p * ( y ( 4 ) - d h p y * y ( 6 ) ) * a b s ( y ( 4 ) - d h p y * y ( 6 ) ) ; ~skin f r i c t i o n f o r c e f o r p o r t s i d e h u l l F s h s x u r = C f h p * A h * p * ( y ( 4 ) - d h s y * y ( 6 ) ) * a b s ( y ( 4 ) - d h s y * y ( 6 ) ) ; s k i n f r i c t i o n f o r c e f o r s t a r b o a r d s i d e h u l l F n p i = [ . 2 5 . 3 . 3 5 . 4 . 4 5 . 5 . 5 5 . 6 . 6 5 . 7 ] ; ~to b e u s e d t o f i n d t h e w a v e d r a g ( t h r o u g h i n t e r p o l a t i o n ) C w f = [ 1 . 2 1 . 7 5 2 . 2 5 2 . 5 8 3 . 7 5 4 . 2 5 4 . 1 5 3 . 8 5 3 . 5 8 3 . 4 ] * 1 e - 3 ; ~The n u m b e r s w e r e t a k e n f r o m f i g 6 o f j o u r n a l o f s h i p r e s e a r e c h j u n e 2 0 0 1 p g 9 4 F n p = ( y ( 4 ) - d h p y * y ( 6 ) ) / ( g * L h ) A 0 . 5 ; f o r i = 1 : 9 e n d F n p = ( y ( 4 ) - d h p y * y ( 6 ) ) / ( g * L h ) A 0 . 5 ; i f ( F n p > F n p i ( i ) & F n p < F n p i ( i + 1 ) ) C w = ( C w f ( i ) + C w f ( i + 1 ) ) / 2 ; e l s e C w = C w f ( i ) ; e n d E r h p x u r = C w * A h * p * ( y ( 4 ) - d h p y * y ( 6 ) ) * a b s ( y ( 4 ) - d h p y * y ( 6 ) ) ; ~wavw d r a g i n P o r t h u l l f o r i = l : 9 e n d F n p = ( y ( 4 ) - d h s y * y ( 6 ) ) / ( g * L h ) A 0 . 5 ; i f ( F n p > F n p i ( i ) & F n p < F n p i ( i + 1 ) ) C w = ( C w f ( i ) + C w f ( i + l ) ) / 2 ; e l s e C w = C w f ( i ) ; e n d f r h s x u r = C w * A h * p * ( y ( 4 ) - d h p y * y ( 6 ) ) * a b s ( y ( 4 ) - d h s y * y ( 6 ) ) ; w a v w d r a g i n S b o a r d h u l l X S = L h / 2 ; X S = L h / 2 ; C D h = 2 ; % d r a g c o e f f i c i e n t d x = - 0 . 4 ; % d i s t a n c e f r o m t h e C G t o t h e h u l l c e n t r e i n X b d i r e c t i o n V 3 = y ( 5 ) + ( L h / 2 + d x ) * y ( 6 ) ; % v e l o c i t y @ b o w e n d V S = y ( 5 ) - ( L h / 2 - d x ) * y ( 6 ) ; % v e l o c i t y @ s t e r n e n d A l = 1 / 3 * a b s ( V B - V S ) * ( V B - V S ) * ( X B A 2 * a b s ( X B ) - X S A 2 * a b s ( X S ) ) / L h A 2 ; ~2=0.5*abs(VB-VS)*y(5)*(XB*abs(XB)-XS*abs(XS) ) / L h ; ~3=0.5*abs(VB-VS)*abs(y(5) ) * ( X B A 2 - X S A 2 ) / L h ; A 4 = y ( 5 ) * a b s ( y ( 5 ) ) * ( X B - X S ) ; F h p y v r = 0 . 5 * C D h * T h * p * ( A l + A 2 + A 3 + A 4 ) ; % d a m p i n g f o r c e F h s y v r = 0 . 5 * C D h * T h * p * ( A 1 + A 2 + A 3 + A 4 ) ; % d a m p i n g f o r c e n~xur=(Fshpxur+Frhpxur)*dhpy+(Fshsxur+Frhsxur)*dhsy;%damping m o m e n t 3 1 = 1 / 4 * a b s ( V B - V S ) * ( V B - V S ) / L h A 2 * ( X B A 3 * a b s ( X B ) - X S A 3 * a b s ( X S ) ) ; B 2 = 1 / 3 * a b s ( V B - V S ) / L h * ( ( V B + V S ) / 2 ) * ( X B A 2 * a b s ( X B ) - X S A 2 * a b s ( X S ) ) ; 1 3 0 " ' ' " " . ; _ < j D 3 = 1 / 3 * ( V B - V S ) / L h * a b s ( ( V B + V S ) / 2 ) * ( X B A 3 - X S A 3 ) ; 3 4 = 1 / 2 * ( V B + V S ) / 2 * a b s ( ( V B + V S ) / 2 ) * ( X B A 2 - X S A 2 ) ; ~hpyvr=Oo5*CDh*Th*p*(B1+B2+B3+B4); m h s y v r = O o 5 * C D h * T h * p * ( B 1 + B 2 + B 3 + B 4 ) ; I~huvr=mhxur+mhpyvr+mhsyvr;% t o t a l m o m e n t a l p a = p i / 1 8 ; C L = 4 o 5 8 3 7 * a l p a ; C D = O ; S y = O o 0 9 4 8 ; % p r o j e c t e d a r e a L s = - 1 / 2 * p * a b s ( y ( 4 ) + y ( 5 ) ) * ( y ( 4 ) + y ( 5 ) ) * S y * C L ; % l i f t f o r c e D s = 1 / 2 * p * a b s ( y ( 4 ) + y ( 5 ) ) * ( y ( 4 ) + y ( 5 ) ) * S y * C D ; % D r a g f o r c e F f f s x u r = L s * s i n ( a l p a ) + D s * c o s ( a l p a ) ; b s = O o 0 2 5 ; % b e a m c s = O o 0 7 5 ; S x = O o 0 3 9 5 ; % p r o j e c t e d a r e a V s x = y ( 4 ) - b s / 2 * y ( 6 ) ; F : 1 s i = [ 0 0 o 1 o 2 o 3 o 4 o 5 o 6 o 7 C r s i = [ O 0 0 0 2 0 1 5 o 4 7 1 o 1 2 1 o 0 2 o 7 3 f o r i = 1 : 1 1 F n s = ( y ( 4 ) - d h s y * y ( 6 ) ) / ( g * c s ) A O o 5 ; i f ( F n s > F n s i ( i ) & F n s < F n s i ( i + 1 ) ) C r s = ( C r s i ( i ) + C r s i ( i + 1 ) ) / 2 ; e l s e C r s = C r s i ( i ) ; e n d e n d f r s x u r = 1 / 2 * p * V s x * a b s ( V s x ) * S x * C r s ; F f f s y u r = L s * c o s ( a l p a ) - D s * s i n ( a l p a ) ; V s y = y ( 5 ) + c s / 2 * y ( 6 ) ; ' : : r · s y = 2 ; f r s y u r = 1 / 2 * p * V s y * a b s ( V s y ) * S y * C r s y ; d s y = O ; d s x = O o 6 ; O o 8 1 . 1 2 3 ] ' ; O o 5 5 0 0 2 3 0 0 2 1 O o 2 ] ' ; : : 1 s u v r = ( F f f s x u r + f r s x u r ) * d s y + ( F f f s y u r + f r s y u r ) * d s x ; o 1 5 6 5 ; % s e c t i o n a l a r e a o f t h e g e r t l e r b o d y '~q=1o875; < c o g i = [ 2 4 6 8 1 0 1 2 1 4 1 6 1 8 2 0 2 2 2 4 2 6 2 8 3 0 ] ' * 1 e 6 ; ~eynolds n u m b e r t o m a t c h w / C f g v e c t o r ( f o r i n t e r p o l a t i o n ) ' J l = [ 3 o 9 3 0 4 3 0 2 3 o 0 5 2 0 9 2 0 8 5 2 0 7 5 2 0 7 2 0 6 5 2 0 6 2 o 5 5 2 o 5 2 2 o 5 1 2 o 4 9 · . 4 6 ] ' * 1 e - 3 ; T a k e n f o r G e r t l e r m o d e l · · o r i = 1 : 1 4 r , d R e g = y ( 4 ) * L g / v i s ; i f ( R e g > R e g i ( i ) & R e g < R e g i ( i + 1 ) ) C d g = ( C g i ( i ) + C g i ( i + 1 ) ) / 2 ; e l s e C d g = C g i ( i ) ; e n d I g x u = l / 2 * p * y ( 4 ) * a b s ( y ( 4 ) ) * S g * C d g ; 1 3 1 \ \ . . . , r , , f o r i = 1 : 1 0 f c c l d d g x = [ 0 . 2 0 0 6 0 . 2 1 0 3 0 . 2 1 8 4 0 . 2 2 6 0 0 . 2 3 3 2 0 . 2 4 0 1 0 . 2 4 6 7 . . . . 0 . 2 5 3 1 0 . 2 5 9 2 0 . 2 6 5 1 ] ; V g y ( i ) = y ( 5 ) + d g x ( i ) * y ( 6 ) ; C g y = [ 3 . 9 3 . 4 3 . 2 3 . 0 5 2 . 9 2 . 8 5 2 . 7 5 2 . 7 2 . 6 5 2 . 6 2 . 5 5 2 . 5 2 . . . . 2 . 5 1 2 . 4 9 2 . 4 6 ] ' * l e - 3 ; C g 3 d = [ 1 0 5 5 9 2 0 . 5 9 . 9 4 . 4 2 . 8 1 . 9 1 . 4 1 . 3 1 . 0 . 9 5 1 . 0 . . . . 1 . 1 1 . 0 0 . 2 . 2 4 . 5 4 ] ; A p g = [ 0 . 0 3 7 3 0 . 0 3 9 1 0 . 0 4 0 6 0 . 0 4 2 0 0 . 0 4 3 4 0 . 0 4 4 7 0 . 0 4 5 9 . . . . 0 . 0 4 7 1 0 . 0 4 8 2 0 . 0 4 9 3 ] ; F g y v r r ( i ) = 0 . 5 * p * A p g ( i ) * a b s ( V g y ( i ) ) * V g y ( i ) * C g y ( i ) * C g 3 d ( i ) ; m g u v r r ( i ) = F g y v r r ( i ) * d g x ( i ) ; F g y v r r = [ F g y v r r ( l ) F g y v r r ( 2 ) F g y v r r ( 3 ) F g y v r r ( 4 ) F g y v r r ( 5 ) F g y v r r ( 6 ) F g y v r r ( 7 ) F g y v r r ( 8 ) F g y v r r ( 9 ) F g y v r r ( l O ) ] ; m g u v r r = [ m g u v r r ( l ) m g u v r r ( 2 ) m g u v r r ( 3 ) m g u v r r ( 4 ) m g u v r r ( 5 ) m g u v r r ( 6 ) m g u v r r ( 7 ) m g u v r r ( 8 ) m g u v r r ( 9 ) m g u v r r ( l O ) ] ; F g y v r = s u m ( F g y v r r ) ; m g u v r l = s u m ( m g u v r r ) ; d g y = O ; m g u v r = f g x u * d g y + m g u v r l ; : J V = [ ( F s h p x u r + F s h s x u r + F r h p x u r + F r h s x u r + F f f s x u r + F f f s x u r + f r s x u r + f g x u ) ; ( F h p y v r + F h s y v r + F f f s y u r + f r s y u r ) ; ( m h u v r + m s u v r + m g u v r ) ; ] ; j p x = 0 . 8 5 ; ' 1 V l = [ D V ( l ) D V ( 2 ) D V ( 3 ) ] ' + C r ; 1 V = - [ h V l ( l ) h V 1 ( 2 ) h V 1 ( 3 ) / d p x ] ' ; d i s t a n c e f r o m C G t o p r o p e l l e r s i n X - d i r e c t i o n ~ n c w = [ m 0 0 ; 0 m 0 ; 0 0 I z / d p x ; ] ; % M a s s m a t r i x r e q u i r e d D e s i r e d P r o f i l e L X d , x d o t d , x d d o t d , y d , y d o t d , y d d o t d ] . . . . . . . . . . . . . . . . . . . . . . . . . . . . . e r r o r ~x=x_d-y(l); ~y=y d - y ( 2 ) ; e r r o r d o t ~xdot=xdot_d-ndot(l); ~ydot=ydot d - n d o t ( 2 ) ; a = r e a d f i s ( ' N a v i ' ) ; i f e x > 4 e x = 4 ; e n d i f e x d o t > 4 e x d o t = 4 ; e n d i f e y > 4 e y = 4 ; e n d i f e y d o t > 4 e y d o t = 4 ; e n d i f e x < - 4 e x = - 4 ; D e s i r e d P a t h ( t ) ; . . , t ' ' ' 1 3 2 ; ? e n d i f e x d o t < - 4 e x d o t = - 4 ; e n d i f e y < - 4 e y = - 4 ; e n d i f e y d o t < - 4 e y d o t = - 4 ; e n d F i s X = e v a l f i s ( [ e x e x d o t ] , a ) ; F i s Y = e v a l f i s ( [ e y e y d o t ] , a ) ; a c c _ x = x d d o t _ d + ( ( F i s X * 2 - 1 ) * 1 0 ) ; 0 c c y = y d d o t d + ( ( F i s Y * 2 - 1 ) * 1 0 ) ; G d d o t x y = [ a c c x a c e y ] 1 % ; J _ x y = [ c o s ( y ( 3 ) ) - s i n ( y ( 3 ) ) ; s i n ( y ( 3 ) ) c c s ( y ( 3 ) ) ; ] ; J d o t _ x y = [ - s i n ( y ( 3 ) ) - c o s ( y ( 3 ) ) ; c o s ( y ( 3 ) ) - s i n ( y ( 3 ) ) ; ] ; V x y = [ y ( 4 ) y ( S ) ] 1 ; ; f x y = [ m 0 ; 0 m ; ] ; X y = [ h V ( 1 ) h V ( 2 ) ] 1 ; V c o t _ x y = i n v ( J _ x y ) * ( n d d o t _ x y - J d o t x y * V _ x y ) ; : J _ x y = l . 2 S * M _ x y * V d o t _ x y ; - l . S * h _ x y ; % w i t h M h a t a n d h h a t o r f h a t g x = g _ x y ( l ) ; g y e = g _ x y ( 2 ) ; g U = [ g x g y e g y e ] 1 ; V d o t = i n v ( M n e w ) * ( h V + g U ) ; ~hrust=sqrt(gxA2+gyeA2)%thrust d~lta=atan(qye/qxl%steerinq a n g l e X d o t = [ n d o t ; V d o t ; ] ; ; , £ o ·~, ~%% 9 o % g o % % % % % % % % % % % % % E n d 0 f P r o g r a r n m e % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % N o t e : - T h e a b o v e M a t L a b c o r d i n g f o r D y n a m i c m o d e l i s t a k e n f r o m t h e r e f e r e n c e 2 6 a n d F u z z y P D c o n t r o l l e r i s a d d e d f o r s i m u l a t i o n s l a t e r . 1 3 3 c r . ' ~I%%%%%% % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % ~Plot p o s i t i o n e r r o r o n s i n o s o i d a l p a t h ~12/12/2007 _ p o s i t i o n s e r r o r p d _ S i n . m % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % C T = O : 0 . 1 : 4 0 ; X O = [ O 0 0 0 0 0 ] ; o p t i o n s l J ) ; o d e s e t ( ' R e l T o l ' , l e - l , ' A b s T o l ' , [ l e - l l e - l l e - l l e - l l e - l l e - ' ' " Y ] = o d e 4 S ( ' c o n t r o l F u z z y _ N a v i ' , T , X O , o p t i o n s ) ; m~length ( T ) ; ~': d , x d o t d , x d d o t d , y _ d , y d o t d , y d d o t d ] D e s i r e d P a t h ( T ) ; f o r i = l : m % p a u s e ( O . l ) f i g u r e ( 2 ) t i t l e ( ' E r r o r i n L o n g i t u d i n a l d i r e c t i o n ' ) y l a b e l ( ' L o n g i t u d i n a l e r r o r - [ m ] ' ) PI~d x l a b e l ( ' T i m e - [ s e c ] ' ) e x ( i ) = x _ d ( i ) - Y ( i , l ) ; p l o t ( i / l O , e x ( i ) , ' - - b ' ) ; h o l d o n f i g u r e ( 3 ) t i t l e ( ' E r r o r i n L a t e r a l d i r e c t i o n ' ) x l a b e l ( ' L o n g i t u d i n a l e r r o r - [ m ] ' ) y l a b e l ( ' T i m e - [ s e c ] ' ) e y ( i ) = y _ d ( i ) - Y ( i , 2 ) ; p l o t ( i / l O , e y ( i ) , ' - - b ' ) ; h o l d o n %%%%%%%%%%%%%%%~%%%%%%%%%%%%EndOfPrograMue%%%%%%%%%%%%%%%%%%%%% 1 3 4 . . , r , _ . / A p p e n d i x C M a t L a b P r o g r a m f o r A p p l i n g U G V a l g o r i t h m s t o U S V % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % ~MatLab P r o g r a m f o r A p p l i n g U G V a l g o r i t h m s t o U S V ~12/12/2007 : o A f u z z y b a s e . m 9 - - 9 - - 9 - - 9 - - 9 - - 9 - - 9 - - 9 - - 9 - - 9 . 0 0 0 0 0 0 0 0 0 0 % % % % % % % % % % % % % % % % % % % % ~%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% c l e a r a l l Y m a x = 2 0 0 0 ; % M a x Y o f t h e g r i d X m a x = 2 0 0 0 ; % M a x X o f t h e g r i d t i m e i n t = 1 ; % P l o t t i m e i n t e r v a l s P l o t t i m e = 8 0 0 ; % M a x p l a t i n g t i m e X g = 9 5 0 ; % G o a l p o i n t c o r d i n a t e s Y g = 1 0 ; X s = 3 0 ; % S t a r t i n g p o i n t o f t h e b o a t Y s = 9 1 5 ; X o = 5 0 0 ; % 0 b s t a c l e c o d i n a t e s Y o = 4 5 0 ; X a c t ( 1 ) = X s ; % a c t u a l p o s i t i o n Y a c t ( 1 ) = Y s ; o l d X = X s ; o l d Y = Y s ; 1 3 o ! I = O ; R E = O ; % H e a d i n g t o w a r d s g o a l G r a d i a n t = O ; % t a n ( R H ) R e 1 D i s = 1 0 0 0 ; % R e l a t i v e d i s t a n c e i n d e x = 1 ; v~~s; z = O ; x ( l ) = X s ; y ( 1 ) = Y s ; t o r t = 1 : t i m e i n t : P l o t t i m e t i m e ( i n d e x ) = t ; i f ( ( x ( i n d e x ) - X g ) A 2 + ( y ( i n d e x ) - Y g ) A 2 ) > ( V * t i m e i n t * 2 + 1 ) A 2 , o b j e c t i v e f u n c t i o n i n d e x = i n d e x + 1 ; % I n c r e m e n t s t h e i n d e x i n g t e r m s o t h a t % i n d e x = 1 c o r r e s p o n d s t o t i m e t = O . 1 C a l c u l a t i n g r e q u i r e d h e a d i n g i f ( X g > x ( i n d e x - 1 ) ) i f Y g > y ( i n d e x - 1 ) G r a d i a n t = ( Y g - y ( i n d e x - 1 ) ) / ( X g - x ( i n d e x - 1 ) ) ; R E = a t a n ( G r a d i a n t ) ; e n d i f y ( i n d e x - 1 ) > Y g G r a d i a n t = ( y ( i n d e x - 1 ) - Y g ) / ( X g - x ( i n d e x - 1 ) ) ; R H = 2 * p i - a t a n ( G r a d i a n t ) ; e n d e n d i f ( x ( i n d e x - 1 ) > X g ) 1 3 5 c t , , ; / i f Y g > y ( i n d e x - 1 ) G r a d i a n t = ( Y g - y ( i n d e x - 1 ) ) / ( x ( i n d e x - 1 ) - X g ) ; R H = p i - a t a n ( G r a d i a n t ) ; e n d i f y ( i n d e x - 1 ) > Y g G r a d i a n t = ( y ( i n d e x - 1 ) - Y g ) / ( x ( i n d e x - 1 ) - X g ) ; R H = p i + a t a n ( G r a d i a n t ) ; e n d e n d % C a l c u l a t e o b s t a c l e h e a d i n g i f ( X o > x ( i n d e x - 1 ) ) i f Y o > y ( i n d e x - 1 ) e n d G r a d i a n t = ( Y o - y ( i n d e x - 1 ) ) / ( X o - x ( i n d e x - 1 ) ) ; O b H = a t a n ( G r a d i a n t ) ; i f y ( i n d e x - 1 ) > Y o G r a d i a n t = ( y ( i n d e x - 1 ) - Y o ) / ( X o - x ( i n d e x - 1 ) ) ; O b H = 2 * p i - a t a n ( G r a d i a n t ) ; e n d i f y ( i n d e x - 1 ) = = Y o O b H = O ; e n d e n d i f ( x ( i n d e x - 1 ) > X o ) i f Y o > y ( i n d e x - 1 ) G r a d i a n t = ( Y o - y ( i n d e x - 1 ) ) / ( x ( i n d e x - 1 ) - X o ) ; O b H = p i - a t a n ( G r a d i a n t ) ; e n d i f y ( i n d e x - 1 ) > Y o G r a d i a n t = ( y ( i n d e x - 1 ) - Y o ) / ( x ( i n d e x - 1 ) - X o ) ; O b H = p i + a t a n ( G r a d i a n t ) ; e n d i f Y o = = y ( i n d e x - 1 ) O b H = p i ; e n d e n d i f Y o > y ( i n d e x - 1 ) e n d i f X o = = x ( i n d e x - 1 ) O b H = p i / 2 ; e n d i f Y o < y ( i n d e x - 1 ) e n d i f X o = = x ( i n d e x - 1 ) O b H = p i / 2 * 3 ; e n d % C a l c u l a t e b o a t h e a d i n g i f ( x ( i n d e x - 1 ) > o l d X ) i f y ( i n d e x - 1 ) > o 1 d Y e n d G r a d i a n t = ( y ( i n d e x - 1 ) - o l d Y ) / ( x ( i n d e x - 1 ) - o l d X ) ; B o H = a t a n ( G r a d i a n t ) ; i f o l d Y > y ( i n d e x - 1 ) G r a d i a n t = ( o l d Y - y ( i n d e x - 1 ) ) / ( x ( i n d e x - 1 ) - o l d X ) ; B o H = 2 * p i - a t a n ( G r a d i a n t ) ; 1 3 6 \ .~:'t , , ' } y i e n d e n d i f o l d Y = = y ( i n d e x - 1 ) B o H = O ; e n d i f ( o l d X > x ( i n d e x - 1 ) ) e n d i f y ( i n d e x - 1 ) > o l d Y G r a d i a n t = ( y ( i n d e x - 1 ) - o l d Y ) / ( o l d X - x ( i n d e x - 1 ) ) ; B o H = p i - a t a n ( G r a d i a n t ) ; e n d i f o l d Y > y ( i n d e x - 1 ) e n d G r a d i a n t = ( o l d Y - y ( i n d e x - 1 ) ) / ( o l d X - x ( i n d e x - 1 ) ) ; B o H = p i + a t a n ( G r a d i a n t ) ; i f o l d Y = = y ( i n d e x - 1 ) B o H = p i ; e n d i f y ( i n d e x - 1 ) > o l d Y e n d i f o l d X = = x ( i n d e x - 1 ) B o H = p i / 2 ; e n d i f y ( i n d e x - 1 ) < o l d Y i f o l d X = = x ( i n d e x - 1 ) B o H = p i / 2 * 3 ; e n d e n d o l d Y = y ( i n d e x - 1 ) ; o l d X = x ( i n d e x - 1 ) ; i f ( ( p i / 2 ) + B o H - O b H ) > = O O b s t a c l e _ a n g l e = ( p i / 2 ) + B o H - O b H ; e l s e i f ( ( p i / 2 ) + B o H - O b H ) > 2 * p i O b s t a c l e a n g l e = ( p i / 2 ) + B o H - O b H - 2 * p i ; e l s e i f ( ( p i / 2 ) + B o H - O b H ) > 4 * p i O b s t a c l e a n g l e = ( p i / 2 ) + B o H - O b H - 4 * p i ; e l s e i f \ ( p i / 2 ) + B 0 H - O b H ) < O O b s t a c l e _ a n g l e = ( p i / 2 ) + B o H - O b H + 2 * p l ; e l s e i f ( ( p i / 2 ) + B o H - O b H ) < - 2 * p i O b s t a c l e a n g l e = ( p i / 2 ) + B o H - O b H + 4 * p i ; e n d C a l c u l a t i n g t h e p a t h c o d i n a t e s R e l D i s = s q r t ( ( Y o - y ( i n d e x - l ) ) A 2 + ( X o - x ( i n d e x - 1 ) ) A 2 ) ; C a l c u l a t i n g r e l a t i v e d i s t a n c e t o O b s a t a c l e i f R e 1 D i s > 1 0 0 x ( i n d e x ) = x ( i n d e x - 1 ) + V * t i m e l n t * c o s ( R H ) ; y ( i n d e x ) = y ( i n d e x - 1 ) + V * t i m e l n t * s i n ( R H ) ; t i m e i n t = l O ; l s c b = r e a d f i s ( ' C A n e w ' ) ; a c c _ C A = e v a l f i s ( [ O b s t a c l e a n g l e ( R e l D i s ) / 1 0 0 ] , b ) ; a c c _ x = ( a c c _ C A ( 1 ) - 0 . 5 ) * 2 * c o s ( B o H ) - ( a c c _ C A ( 2 ) - 0 . 5 ) * 2 * s i n ( B o H ) ; c c y = ( a c c C A ( 2 ) - 0 . 5 ) * 2 * c o s ( B o H ) + ( a c c C A ( 1 ) - 0 . 5 ) * 2 * s i n ( B o H ) ; " r ' ' ' l 1 3 7 ~(index)=x(index-l)+acc~x*timeint*cos(RH)*V; \ · ; i n d e x ) = y ( i n d e x - l ) + a c c y * t i m e i n t * s i n ( R H ) * V ; % , - n d . c i - L a . J l d o n ' L g u : c e ( 2 ) t i t l e ( ' P a t h n e a r o b s t a c l e s ' ) l a b e l ( ' X d i s t a n c e [ m ] ' ) y l a b e l ( ' Y d i s t a n c e [ m ] ' ) ' l f n l o t ( x , y , ' r s ' , ' L i n e W i d t h ' , O . S , ' M a r k e r E d g e C o l o r ' , ' g ' , ' M a r k e r F a c e C o l o r ' , ' g ' , . . . ' M a r k e r S i z e ' , l ) i l J 1 d o n r · c O t ( X a c t , Y a c t , I r s I , I L i n e W i d t h I , 0 . 5 , ' M a r k e r E d g e C o l o r ' , ' k ' , ' M a r k e r F a c e C o l o r ' , ' b ' , . . . ' M a r k e r S i z e ' , l ) ! ' , J 1 d o n p l o t ( X o , Y o , ' r s ' , ' L i n e v J i d t h ' , 1 , ' M a r k e r E d g e C o l o r ' , ' k ' , ' M a r k e r F a c e C o l o r ' , ' b ' , . . . ' M a r k e r S i z e ' , S ) : t l e ( ' P a t h n e a r o b s t a c l e s ' ) x~abel('X d i s t a n c e [ m ] ' ) l a b e l ( ' Y d i s t a n c e [ m ] ' ) ' ' , ~%%%%%%%%%%%% % % % % % % E n d O f P r o g r a m m e % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % 1 3 8 A p p e n d i x D M a t L a b P r o g r a m f o r N o v e l A l g o r i t h m t o A v o i d S t a t i c O b s t a c l e s ~ % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % ~Module f o r P l o t N o v e l o b s t a c l e a v o i d a n c e a l g o r i t h m c , 1 2 / 1 2 / 2 0 0 7 \ O A M a i n P r o F o r O A . m % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % t % % % % % % % % % ! t % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % f u n c t i o n [ x n e w , y n e w ] M o r p h i n O A ( x c u r , y c u r , G x , G y , O ) t ) % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % c r e a t e m a p g r i d : 0 = z e r o s ( 2 0 ) ; c r e a t e o b s t a c l e s : W ( 1 : 3 , 1 ) = 1 ; ' 0 ( 1 : 3 , 3 ) = 1 ; 0 ( 1 , 1 : 3 ) = 1 ; , 0 ( 3 , 1 : 3 ) = 1 ; ~0(2,2)=1; 0 ( 1 0 , 1 5 : 9 0 ) = 1 ; f o r n = 0 : 2 0 ; 0 ( 3 0 - n , 3 0 + n ) = 1 ; % d i a g o n a l l i n e e n d ~~% % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % c r e a t e m a p g r i d : M = z e r o s ( s i z e ( O ) ) ; s u b g r i d = S ; F o r G r i d S i z e = s i z e ( M ) ; g r i d s i z e = F o r G r i d S i z e ( 1 , 2 ) ; Y p a t h i n d e x = 1 ; X p a t h i n d e x = 1 ; p a t h Y L = z e r o s ( g r i d s i z e ) ; p a t h Y R = z e r o s ( g r i d s i z e ) ; p a t h X = z e r o s ( g r i d s i z e ) ; : . ? ; % P a t h g r i d s f o r c e n t r e = 1 : s u b g r i d : ( g r i d s i z e / 2 ) ; f o r x = 1 : ( g r i d s i z e / 2 ) ; i : ( g r i d s i z e / 2 - c e n t r e ) A 2 - ( x - c e n t r e ) A 2 > 0 y = r o u n d ( s q r t ( ( g r i d s i z e / 2 - c e n t r e ) A 2 - ( x - c e n t r e ) A 2 ) ) ; e n d J f ( x - c e n t r e ) A 2 - ( g r i d s i z e / 2 - c e n t r e ) A 2 > 0 1 3 9 ~ y = r o u n d ( s q r t ( x - c e n t r e ) A 2 - ( g r i d s i z e / 2 - c e n t r e ) A 2 ) ; e n d i f ( x - c e n t r e ) = = ( g r i d s i z e / 2 - c e n t r e ) y = 1 ; e n d i f g r i d s i z e > y i f y > O M ( y , x ) = 1 ; M ( y , ( g r i d s i z e - x ) ) = 1 ; p a t h Y L ( Y p a t h i n d e x , x ) = y ; p a t h Y R ( Y p a t h i n d e x , ( g r i d s i z e - x ) ) = y ; e n d e n d e n d Y p a t h i n d e x = Y p a t h i n d e x + 1 ; e n d f o r y 1 = 0 : s u b g r i d : g r i d s i z e X p a t h i n d e x = X p a t h i n d e x + 1 ; f o r l x = 1 : g r i d s i z e l y = r o u n d ( ( l x - 1 ) * ( y 1 - g r i d s i z e / 2 ) / ( g r i d s i z e ) + g r i d s i z e / 2 ) ; c ' 1 ( l x , l y ) = 1 ; p a t h X ( X p a t h i n d e x , l x ) = l y ; e n d c o n d ~J_ o o o o o o o o o o o o _ o o o o o o o o q _ _ o o o o o _ o o _ o o o o c o o _ o o o o o o o o g . _ o o o o o o o _ o o o o o o o o o o o _ o o o o u~~~~~~~~~~~~6~b~~6~~o~~~~b~b~~~~6~~~~~~~~~~o~~~~~~b~~~~bb~~~~b~~~~ D G o a l X L ( l : r o u n d ( g r i d s i z e / s u b g r i d * 2 ) ) = 0 ; D G o a l X R ( l : r o u n d ( g r i d s i z e / s u b g r i d * 2 ) ) = 0 ; D G o a l Y ( 1 : r o u n d ( g r i d s i z e / s u b g r i d * 2 ) ) = 0 ; f o r Y p a t h i n d e x = 1 : g r i d s i z e b~gridsize*2; f o r x = l : g r i d s i z e i f ( p a t h Y L ( Y p a t h i n d e x , x ) > O ) 1 4 0 e n d e n d i f ( O ( p a t h Y L ( Y p a t h i n d e x , x ) , x ) = = l ) e n d a = p a t h Y L ( Y p a t h i n d e x , x ) ; i f a < b b = a ; e n d D G o a l X L ( Y p a t h i n d e x ) = b ; e n d % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % f o r Y p a t h i n d e x = l : g r i d s i z e e n d b = g r i d s i z e * 2 ; f o r x = l : g r i d s i z e i f ( p a t h Y R ( Y p a t h i n d e x , x ) > O ) e n d e n d i f ( O ( p a t h Y R ( Y p a t h i n d e x , x ) , x ) = = l ) e n d a = p a t h Y R ( Y p a t h i n d e x , x ) ; i f a < b b = a ; e n d D G o a l X R ( Y p a t h i n d e x ) = b ; % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % f o r X p a t h i n d e x = l : g r i d s i z e a = g r i d s i z e * 2 ; f o r y = l : g r i d s i z e i f p a t h X ( X p a t h i n d e x , x ) > O % H e r e x a n d y s h o u l d b e i n t e r c h a n g e i f ( 0 ( y , ( p a t h X ( X p a t h i n d e x , y ) ) ) = = 1 ) 1 4 1 ~ e n d e n d e n d a = y ; D G o a l Y ( X p a t h i n d e x ) = a ; e n d % % % % R e q u i r e d h e a d i n g a n g l e p = O ; t h e t a = a t a n ( ( G y - y c u r ) / ( G x - x c u r ) ) ; i f G y = = y c u r e l s e p = r o u n d ( ( ( G x - x c u r ) / ( G y - y c u r ) ) * ( 4 - y c u r ) + x c u r ) ; e n d y n e w = l + y c u r ; x n e w = p ; 6 % % % % % % % % % % % % % % % % % % % % E n d O f P r o g r a m m e % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % 1 4 2 A p p e n d i x E M a t L a b P r o g r a m f o r A p p l i n g P o l y n o m i a l A p p r o x i m a t i o n t o P a t h P r e d i c t i o n t-~~%%%%~%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% ~Polynomial A p p r o x i m a t i o n f o r p a t h p r e d i c t i o n S l 2 / 1 0 / 2 0 0 8 I F o r T h e s i s P o l y P l o t . m % % % % % % % % % % 0 0 0 0 0 0 0 0 0 0 - o o o o - o o o o o - o % % % % % % % % % % ~%~%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% c : l c a r t f = 2 0 ; r f = 2 5 0 ; t b = S O % t i m e b l o c k t i m e = [ 0 : 0 . 0 5 : 4 0 ] * t f ; % f r o m 0 t o 1 0 0 0 0 0 s e c o n d s T l = [ O . S * s i n ( ( t i m e * 0 . 1 * p i ) / t f ) ] ; T 1 = T l * r f % + 2 * r a n d ( s i z e ( T l ) ) ; T 2 = [ s i n ( ( t i m e * 0 . 1 * p i / 2 ) / t f ) ] ; T 2 = T 2 * r f % + 2 * r a n d ( s i z e ( T 2 ) ) ; L n d e x = t b - 1 ; f o r z = 1 : ( i n d e x - 1 ) p r e t t ( z ) = t i m e ( z ) ; p r e F y ( z ) = O ; p r e F x ( z ) = O ; e n d t o r k = O : t b : ( l e n g t h ( t i m e ) - t b ) i f k > t b f o r i = - t b : 1 : t b e n d e l s e i f l e n g t h ( t i m e ) > ( k + i ) f x d ( i + t b + 1 ) = T 1 ( k + i ) ; f y d ( i + t b + 1 ) = T 2 ( k + i ) ; t t ( i + t b + 1 ) = t i m e ( k + i ) ; e n d f o r i = 1 : t b e n d i f l e n g t h ( t i m e ) > ( k + i ) f x d ( i ) = T 1 ( k + i ) ; f y d ( i ) = T 2 ( k + i ) ; t t ( i ) = t i m e ( k + i ) ; e n d e n d p o l y d e g = 1 0 ; p y = p o l y f i t ( t t , f y d , p o l y d e g ) ; p x = p o l y f i t ( t t , f x d , p o l y d e g ) ; : o r i = l : t b i n d e x = i n d e x + 1 ; i f l e n g t h ( t i m e ) > i n d e x p r c t t ( i n d e x ) = t i m e ( i n d e x ) ; p r c F y ( i n d e x ) = p o l y v a l ( p y , p r e t t ( i n d e x ) ) ; p r e F x ( i n d e x ) = p o l y v a l ( p x , p r e t t ( i n d e x ) ) ; e n d e n d e n d : i g u r e ( l ) p l o t ( t i m e , T 1 ) 1 4 3 x l a b e l ( ' T i m e I [ S ] ' ) ; y l a b e l ( ' L o n g i t u d i n a l D i s t a n c e I [ m ] ' ) ; f i g u r e ( 2 ) p l o t ( t i m e , T 2 ) x l a b e l ( ' T i m e I [ S ] ' ) ; y l a b e l ( ' L a t e r a l D i s t a n c e I [ m ] ' ) ; f i g u r e ( ] ) p l o t ( T l , T 2 ) x l a b e l ( ' L o n g i t u d i n a l D i s t a n c e I [ m ] ' ) ; y l a b e l ( ' L a t e r a l D i s t a n c e I [ m ] ' ) ; t i t l e ( ' A c t u a l P a t h ' ) ; f i g u r e ( 4 ) p l o t ( T l , T 2 , ' r ' , p r e F x , p r e F y , ' b ' ) x l a b e l ( ' L o n g i t u d i n a l D i s t a n c e I [ m ] ' ) ; y l a b e l ( ' L a t e r a l D i s t a n c e I [ m ] ' ) ; t i t l e ( ' A c t u a l a n d P r e d i c t e d P a t h ' ) ; r = l : m i n ( l e n g t h ( T l ) , l e n g t h ( p r e F x ) ) ; e f x ( r ) = p r e F x ( r ) - T l ( r ) ; s = l : m i n ( l e n g t h ( T 2 ) , l e n g t h ( p r e F y ) ) ; e f y ( s ) = p r e F y ( s ) - T 2 ( s ) ; f i g u r e ( S ) p l o t ( ( e f x + e f y ) l 2 , ' r ' ) x l a b e l ( ' T i m e [ S ] ' ) ; y l a b e l ( ' E r r o r [ m ] ' ) ; ~~%%%~%%%%%%%%%%%%%%%%EndOfProgramme%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 1 4 4 A p p e n d i x F M a t L a b P r o g r a m f o r A p p l i n g G R N N t o P a t h P r e d i c t i o n % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % G R N N f o r p a t h p r e d i c t i o n 1 2 / 1 0 / 2 0 0 8 % F o r T h e s i s N N E r r o r P l o t . m % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % %%%%%%%%%%%%%%%%%%%%%%%%~%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% c l e a r t f = 2 0 ; r f = 2 5 0 ; t b = 5 0 % t i m e b l o c k t i m e = [ 0 : 0 . 0 5 : 4 0 ] * t f ; % f r o m 0 t o 8 0 0 s e c o n d s T 1 = [ 0 . 5 * s i n ( ( t i m e * 0 . 1 * p i ) / t f ) ] ; T A 1 = T 1 * r f ; T 2 = [ s i n ( ( t i m e * 0 . 1 * p i / 2 ) / t f ) ] ; T A 2 = T 2 * r f ; T 1 = [ 0 . 5 * s i n ( ( t i m e * 0 . 1 * p i ) / t f ) ] ; T 1 = T 1 * r f % + 5 * r a n d ( s i z e ( T 1 ) ) ; T 2 = [ s i n ( ( t i m e * 0 . 1 * p i / 2 ) / t f ) ] ; T 2 = T 2 * r f % + 5 * r a n d ( s i z e ( T 2 ) ) ; i n d e x = t b - 1 ; f o r z = 1 : ( i n d e x - 1 ) p r e t t ( z ) = t i m e ( z ) ; p r e F y ( z ) = O ; p r e F x ( z ) = O ; e n d f o r k = O : t b : ( l e n g t h ( t i m e ) - t b ) i f k > t b f o r i = - t b : 1 : t b e n d e l s e i f l e n g t h ( t i m e ) > ( k + i ) f x d ( i + t b + 1 ) = T 1 ( k + i ) ; f y d ( i + t b + 1 ) = T 2 ( k + i ) ; t t ( i + t b + 1 ) = t i m e ( k + i ) ; e n d f o r i = l : t b e n d i f l e n g t h ( t i m e ) > ( k + i ) f x d ( i ) = T l ( k + i ) ; f y d ( i ) = T 2 ( k + i ) ; t t ( i ) = t i m e ( k + i ) ; e n d e n d s p r e a d 1 ; n e t Y = n e w g r n n ( t t , f y d , s p r e a d ) ; n e t X = n e w g r n n ( t t , f x d , s p r e a d ) ; f o r i = 1 : t b i n d e x = i n d e x + 1 ; 1 4 5 e n d e n d i f l e n g t h ( t i m e ) > i n d e x p r e t t ( i n d e x ) = t i m e ( i n d e x ) ; p r e F y ( i n d e x ) = s i m ( n e t Y , p r e t t ( i n d e x ) ) ; p r e F x ( i n d e x ) = s i m ( n e t X , p r e t t ( i n d e x ) ) ; e n d r = 1 : m i n ( l e n g t h ( T 1 ) , l e n g t h ( p r e F x ) ) ; e f x ( r ) = p r e F x ( r ) - T 1 ( r ) ; s = 1 : m i n ( l e n g t h ( T 2 ) , l e n g t h ( p r e F y ) ) ; e f y ( s ) = p r e F y ( s ) - T 2 ( s ) ; % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % i n d e x = t b - 1 ; f o r z = 1 : ( i n d e x - 1 ) p r e t t ( z ) = t i m e ( z ) ; p r e F y 2 ( z ) = O ; p r e F x 2 ( z ) = O ; e n d f o r k = O : t b : ( l e n g t h ( t i m e ) - t b ) i f k > t b f o r i = - t b : 1 : t b e n d e l s e i f l e n g t h ( t i m e ) > ( k + i ) f x d ( i + t b + 1 ) = T 1 ( k + i ) ; f y d ( i + t b + 1 ) = T 2 ( k + i ) ; t t ( i + t b + 1 ) = t i m e ( k + i ) ; e n d f o r i = 1 : t b i f l e n g t h ( t i m e ) > ( k + i ) f x d ( i ) = T 1 ( k + i ) ; f y d ( i ) = T 2 ( k + i ) ; t t ( i ) = t i m e ( k + i ) ; e n d e n d e n d s p r e a d = 5 ; n e t Y = n e w g r n n ( t t , f y d , s p r e a d ) ; n e t X = n e w g r n n ( t t , f x d , s p r G a d ) ; e n d f o r i = 1 : t b i n d e x = i n d e x + 1 ; e n d i f l e n g t h ( t i m e ) > i n d e x p r e t t ( i n d e x ) = t i m e ( i n d e x ) ; p r e F y 2 ( i n d c x ) = s i m ( n e t Y , p r e t t ( i n d e x ) ) ; p r e F x 2 ( i n d e x ) = s i m ( n e t X , p r e t t ( i n d e x ) ) ; e n d r = 1 : m i n ( l e n g t h ( T 1 ) , l e n g t h ( p r e F x 2 ) ) ; e f x 2 ( r ) = p r e F x 2 ( r ) - T 1 ( r ) ; s = 1 : m i n ( l e n g t h ( T 2 ) , l e n g t h ( p r e F y 2 ) ) ; e f y 2 ( s ) = p r e F y 2 ( s ) - T 2 ( s ) ; 1 4 6 %~%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% i n d e x = t b - 1 ; f o r z = 1 : ( i n d e x - 1 ) p r e t t ( z ) = t i r n e ( z ) ; p r e F y 3 ( z ) = O ; p r e F x 3 ( z ) = O ; e n d f o r k = O : t b : ( l e n g t h ( t i r n e ) - t b ) i f k > t b f o r i = - t b : 1 : t b i f l e n g t h ( t i r n e ) > ( k + i ) f x d ( i + t b + 1 ) = T 1 ( k + i ) ; f y d ( i + t b + 1 ) = T 2 ( k + i ) ; t t ( i + t b + 1 ) = t i r n e ( k + i ) ; e n d e n d e l s e f o r i = 1 : t b e n d i f l e n g t h ( t i r n e ) > ( k + i ) f x d ( i ) = T 1 ( k + i ) ; f y d ( i ) = T 2 ( k + i ) ; t t ( i ) = t i r n e ( k + i ) ; e n d e n d s p r e a d 1 0 ; n e t Y = n e w g r n n ( t t , f y d , s p r e a d ) ; n e t X = n e w g r n n ( t t , f x d , s p r e a d ) ; f o r i = 1 : t b i n d e x = i n d e x + 1 ; i f l e n g t h ( t i r n e ) > i n d e x p r e t t ( i n d e x ) = t i r n e ( i n d e x ) ; p r e F y 3 ( i n d e x ) = s i r n ( n e t Y , p r e t t ( i n d e x ) ) ; p r e F x 3 ( i n d e x ) = s i m ( n e t X , p r e t t ( i n d e x ) ) ; e n d e n d e n d r = l : r n i n ( l e n g t h ( T 1 ) , l e n g t h ( p r e F x 3 ) ) ; e f x 3 ( r ) = p r e F x 3 ( r ) - T 1 ( r ) ; s = 1 : r n i n ( l e n g t h ( T 2 ) , l e n g t h ( p r e F y 3 ) ) ; e f y 3 ( s ) = p r e F y 3 ( s ) - T 2 ( s ) ; '~t%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% f i g u r e ( 1 ) p l o t ( t i r n e , T 1 ) x l a b e l ( ' T i r n e I [ S ] ' ) ; y l a b e l ( ' L o n g i t u d i n a l D i s t a n c e I [ m ] ' ) ; f i g u r e ( 2 ) p l o t ( t i r n e , T 2 ) x l a b e l ( ' T i m e I [ S ] ' ) ; y l a b e l ( ' L a t e r a l D i s t a n c e I [ m ] ' ) ; : i g u r e ( 3 ) o t ( T 1 , T 2 ) x l a b e l ( ' L o n g i t u d i n a l D i s t a n c e I [ m ] ' ) ; y l a b e l ( ' L a t e r a l D i s t a n c e I [ m ] ' ) ; t i t l e ( ' A c t u a l P a t h ' ) ; ~,. f . . ' 1 4 7 f i g u r e ( 4 ) p l o t ( T A l 1 T A 2 1 ' k ' I p r e F x 1 p r e F y 1 ' g ' 1 p r e F x 2 1 p r e F y 2 1 ' b ' 1 p r e f x 3 1 p r e F y 3 1 ' r ' ) x J a b e l ( ' L o n g i t u d i n a l D i s t a n c e I [ m ] ' ) ; y l a b e l ( ' L a t e r a l D i s t a n c e I [ m ] ' ) ; t i t l e ( ' A c t u a l a n d P r e d i c t e d P a t h s ' ) ; f i g u r e ( 6 ) p l o t ( e f x l 2 1 ' g ' ) x l a b e l ( ' T i m e [ S ] ' ) ; y l a b e l ( ' L o n g i t u d i n a l E r r o r [ m ] ' ) ; h o l d o n f i g u r e ( 7 ) p l o t ( e f y l 2 1 ' g ' ) x l a b e l ( ' T i m e [ S ] ' ) ; y l a b e l ( ' L a t e r a l E r r o r [ m ] ' ) ; h o l d o n f i g u r e ( 6 ) p l o t ( e f x 3 l 2 1 ' r ' ) x l a b e l ( ' T i m e [ S ] ' ) ; y l a b e l ( ' L o n g i t u d i n a l E r r o r [ m ] ' ) ; h o l d o n f i g u r e ( 7 ) p l o t ( e f y 3 l 2 1 ' r ' ) x l a b e l ( ' T i m e [ S ] ' ) ; y l a b e l ( ' L a t e r a l E r r o r [ m ] ' ) ; h o l d o n f i g u r e ( 6 ) p l o t ( e f x 2 1 2 1 ' b ' ) x l a b e l ( ' T i m e [ S ] ' ) ; y l a b e l ( ' L o n g i t u d i n a l E r r o r [ m ] ' ) ; r , o l d o f f f i g u r e ( 7 ) p l o t ( e f y 2 l 2 1 ' b ' ) x l a b e l ( ' T i m e [ S ] ' ) ; y l a b e l ( ' L a t e r a l E r r o r [ m ] ' ) ; h o l d o f f % % % % % % % % % % % % % % % % % % % E n d 0 £ P r o g r a m m e % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % 1 4 8 A p p e n d i x G M a t L a b P r o g r a m t o C o m p a r e V e l o c i t y O b s t a c l e M e t h o d w i t h N o v e l M e t h o d f o r A r e a P r e d i c t i o n o f D y n a m i c O b s t a c l e s % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % ~Comparing V O M w i t h N o v e l m e t h o d 1 2 / 1 0 / 2 0 0 8 % % % % % % % % % % % C o m p a r e A r e a P r e . m % % % % % % % % % % % % % % % % % % % % ~%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%~%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% c l e a r t f = 2 0 ; r f = 2 5 0 ; t b = S O % t i m e b l o c k N = O % F o r n o i s e s = 0 . 6 % S e a c o n d i t i o n t i m e = [ 0 : 0 . 0 5 : 4 0 ] * t f ; % f r o m 0 t o 8 0 0 s e c o n d s T l = [ O . S * s i n ( ( t i m e * O . l * p i ) / t f ) ] ; T J = T l * r f + S * r a n d ( s i z e ( T l ) ) ; R V l = r f * [ O . S * O . l * p i * c o s ( ( t i m e * O . l * p i ) / t f ) ] / t f ; R V f l = l . 2 + ( 0 . l * R V l / 2 ) ; T 2 = [ s i n ( ( t i m e * O . l * p i / 2 ) / t f ) ] ; T 2 = T 2 * r f + S * r a n d ( s i z e ( T 2 ) ) ; R V 2 = r f * [ O . l * p i / 2 * c o s ( ( t i m e * O . l * p i / 2 ) / t f ) / t f ] ; R V f 2 = 1 . 2 + ( 0 . l * R V l / 2 ) ; i n d e x = t b ; f o r z = l : ( i n d e x - 1 ) p r e t t ( z ) = t i m e ( z ) ; p r e F y ( z ) = O ; A p r e A F y l ( z ) = O ; J l " p r e A F y 2 ( z ) = 0 ; p r e F x ( z ) = O ; A p r e A F x l ( z ) = 0 ; A p r e A F x 2 ( z ) = O ; r~nd f o r k = O : t b : ( l e n g t h ( t i m e ) - t b ) i f k > t b f o r i = - t b : l : t b e n d e l s e i f l e n g t h ( t i m e ) > ( k + i ) f x d ( i + t b + l ) = T l ( k + i ) ; f y d ( i + t b + l ) = T 2 ( k + i ) ; t t ( i + t b + l ) = t i m e ( k + i ) ; e n d f o r i = l : t b i f l e n g t h ( t i m e ) > ( k + i ) 1 4 9 e n d e n d f x d ( i ) = T l ( k + i ) ; f y d ( i ) = T 2 ( k + i ) ; t t ( i ) = t i m e ( k + i ) ; e n d s p r e a d = 7 ; n e t Y n e w g r n n ( t t , f y d , s p r e a d ) ; n e t X = n e w g r n n ( t t , f x d , s p r e a d } ; f o r i = l : t b i n d e x = i n d e x + l ; i f ( l e n g t h ( t i m e ) + l ) > i n d e x p r e t t ( i n d e x ) = t i m e ( i n d e x ) ; p r e F y ( i n d e x ) = s i m ( n e t Y , p r e t t ( i n d e x ) ) ; p r e F x ( i n d e x ) = s i m ( n e t X , p r e t t ( i n d e x ) ) ; A p r e A F y l ( i n d e x ) = s i m ( n e t Y , p r e t t ( i n d e x ) ) + ( N + s * ( i n d e x - k - l ) A R V f 2 ( i n d e x ) ) A p r e A F y 2 ( i n d e x ) = s i m ( n e t Y , p r e t t ( i n d e x ) ) - ( N + s * ( i n d e x - k - l ) A R V f 2 ( i n d e x ) ) A p r e A F x l ( i n d e x ) = s i m ( n e t X , p r e t t ( i n d e x ) ) + ( N + s * ( i n d e x - k - l ) A R V f l ( i n d e x ) ) A p r e A F x 2 ( i n d e x ) = s i m ( n e t X , p r e t t ( i n d e x ) ) - ( N + s * ( i n d e x - k - l ) A R V f l ( i n d e x ) ) e n d e n d e n d r = l : m i n ( l e n g t h ( T l ) , l e n g t h ( p r e F x ) ) ; A w i d t h F x ( r ) = A p r e A F x l ( r ) - A p r e A F x 2 ( r ) ; A w i d t h F x ( r ) = ( ( A w i d t h F x ( r ) . A 2 ) . A ( 0 . 5 ) ) ; A d i s F x l ( r ) = T l ( r ) - A p r e A F x l ( r ) ; ~.disFxl ( r ) = ( ( A d i s F x l ( r ) . A 2 ) . A ( 0 . 5 ) ) ; A d i s F x 2 ( r ) = T l ( r ) - A p r e A F x 2 ( r ) ; T~.ciisfx2 ( r ) = ( ( A d i s f x 2 ( r ) . A 2 ) . A ( 0 . 5 ) ) ; ? l v J a t = s i z e ( r ) ; f o r e = l : R M a t ( 1 , 2 ) l \ e r f x ( e ) = O ; l ? n d i f A d i s F x l ( e ) > A d i s F x 2 ( e ) A e r f x ( e ) = A d i s F x l ( e ) - A w i d t h F x ( e ) ; i f A w i d t h F x ( e ) > = A d i s F x l ( e ) A e r f x ( e ) = O ; e n d e l s e A e r f x ( e ) = A d i s F x 2 ( e ) - A w i d t h F x ( e ) ; i f A w i d t h F x ( e ) > = A d i s F x 2 ( e ) A e r f x ( e ) = O ; e n d e n d 1 5 0 s = l : m i n ( l e n g t h ( T 2 ) , l e n g t h ( p r e F y ) ) ; A w i d t h F y ( s ) = A p r e A F y l ( s ) - A p r e A F y 2 ( s ) ; A w i d t h F y ( s ) = ( ( A w i d t h F y ( s ) . A 2 ) . A ( 0 . 5 ) ) ; A d i s F y l ( s ) = T 2 ( s ) - A p r e A F y l ( s ) ; A . d i s F y l ( s ) = ( ( A d i s F y l ( s ) . A 2 ) . A ( 0 . 5 ) ) ; A d i s F y 2 ( s ) = T 2 ( s ) - A p r e A F y 2 ( s ) ; A d i s F y 2 ( s ) = ( ( A d i s F y 2 ( s ) . A 2 ) . A ( 0 . 5 ) ) ; R M a t = s i z e ( s ) ; f o r e = l : R M a t ( 1 , 2 ) A e r f y ( e ) = O ; e n d i f A d i s F y l ( e ) > A d i s F y 2 ( e ) A e r f y ( e ) = A d i s F y l ( e ) - A w i d t h F y ( e ) ; i f A w i d t h F y ( e ) > = A d i s F y l ( e ) A e r f y ( e ) = O ; e n d e l s e A e r f y ( e ) = A d i s F y 2 ( e ) - A w i d t h F y ( e ) ; i f A w i d t h F y ( e ) > = A d i s F y 2 ( e ) A e r f y ( e ) = O ; e n d e n d t % % % % % % % % % t % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % i n d e x = t b ; f o r z = l : ( i n d e x - 1 ) p r e t t ( z ) = t i m e ( z ) ; p r e F y ( z ) = O ; B p r e A F y l ( z ) = O ; B p r e A F y 2 ( z ) = O ; p r e F x ( z ) = O ; B p r e A F x l ( z ) = O ; B p r e A F x 2 ( z ) = O ; e n d f o r k = O : t b : ( l e n g t h ( t i m e ) - t b ) i f k > t b f o r i = - t b : l : t b e n d e l s e i f l e n g t h ( t i m e ) > ( k + i ) f x d ( i + t b + l ) = T l ( k + i ) ; f y d ( i + t b + l ) = T 2 ( k + i ) ; t t ( i + t b + l ) = t i m e ( k + i ) ; e n d f o r i = l : t b i f l e n g t h ( t i m e ) > ( k + i ) f x d ( i ) = T l ( k + i ) ; f y d ( i ) = T 2 ( k + i ) ; t t ( i ) = t i m e ( k + i ) ; 1 5 1 e n d e n d e n d p o l y d e g = l ; p y = p o l y f i t ( t t , f y d , p o l y d e g ) ; p x = p o l y f i t ( t t , f x d , p o l y d e g ) ; f o r i = l : t b i n d e x = i n d e x + l ; i f ( l e n g t h ( t i m e ) + l ) > i n d e x p r e t t ( i n d e x ) = t i m e ( i n d e x ) ; p r e F y ( i n d e x ) = p o l y v a l ( p y , p r e t t ( i n d e x ) ) ; p r e F x ( i n d e x ) = p o l y v a l ( p x , p r e t t ( i n d e x ) ) ; B p r e A F y l ( i n d e x ) = p o l y v a l ( p y , p r e t t ( i n d e x ) ) + ( N + s * ( i n d e x - k - l ) A r v ) B p r e A F y 2 ( i n d e x ) = p o l y v a l ( p y , p r e t t ( i n d e x ) ) - ( N + s * ( i n d e x - k - l ) A r v ) B p r e A F x l ( i n d e x ) = p o l y v a l ( p x , p r e t t ( i n d e x ) ) + ( N + s * ( i n d e x - k - l ) A r v ) B p r e A F x 2 ( i n d e x ) = p o l y v a l ( p x , p r e t t ( i n d e x ) ) - ( N + s * ( i n d e x - k - l ) A r v ) e n d e n d e n d r = l : m i n ( l e n g t h ( T l ) , l e n g t h ( p r e F x ) ) ; B w i d t h F x ( r ) = B p r e A F x l ( r ) - B p r e A F x 2 ( r ) ; BwidthFx(r)~( ( B w i d t h F x ( r ) . A 2 ) . A ( 0 . 5 ) ) ; B d i s F x l ( r ) = T l ( r ) - B p r e A F x l ( r ) ; B d i s F x l ( r ) = ( ( B d i s F x l ( r ) . A 2 ) . A ( 0 . 5 ) ) ; B d i s F x 2 ( r ) = T l ( r ) - B p r e A F x 2 ( r ) ; B d i s F x 2 ( r ) = ( ( B d i s F x 2 ( r ) . A 2 ) . A ( 0 . 5 ) ) ; P M a t = s i z e ( r ) ; f o r e = l : R M a t ( 1 , 2 ) B e r f x ( e ) = O ; e n d i f B d i s F x l ( e ) > B d i s F x 2 ( e ) B e r f x ( e ) = B d i s F x l ( e ) - B w i d t h F x ( e ) ; i f B w i d t h F x ( e ) > = B d i s F x l ( e ) B e r f x ( e ) = O ; e n d e l s e B e r f x ( e ) = B d i s F x 2 ( e ) - B w i d t h F x ( e ) ; i f B w i d t h F x ( e ) > = B d i s F x 2 ( e ) B e r f x ( e ) = O ; e n d e n d s = l : m i n ( l e n g t h ( T 2 ) , l e n g t h ( p r e F y ) ) ; B w i d t h F y ( s ) = B p r e A F y l ( s ) - B p r e A F y 2 ( s ) ; E v J i d t h F y ( s ) = ( ( B w i d t h F y ( s ) . A 2 ) . A ( 0 . 5 ) ) ; B d i s F y l ( s ) = T 2 ( s ) - B p r e A F y l ( s ) ; B d i s F y l ( s ) = ( ( B d i s F y l ( s ) . A 2 ) . A ( 0 . 5 ) ) ; 1 5 2 I B d i s F y 2 ( s ) = T 2 ( s ) - B p r e A F y 2 ( s ) ; B d i s F y 2 ( s ) = ( ( B d i s F y 2 ( s ) . A 2 ) . A ( 0 . 5 ) ) ; R M a t = s i z e ( s ) ; f o r e = l : R M a t ( l , 2 ) B e r f y ( e ) = O ; i f B d i s F y l ( e ) > B d i s F y 2 ( e ) B e r f y ( e ) = B d i s F y l ( e ) - B w i d t h F y ( e ) ; i f B w i d t h F y ( e ) > = B d i s F y l ( e ) B e r f y ( e ) = O ; e n d e l s e B e r f y ( e ) = B d i s F y 2 ( e ) - B w i d t h F y ( e ) ; i f B w i d t h F y ( e ) > = B d i s F y 2 ( e ) B e r f y ( e ) = O ; e n d e n d e n d f i g u r e ( l ) p l o t ( t i m e , T l , ' k ' , t i m e , A p r e A F x l , ' r ' , t i m e , A p r e A F x 2 , ' r ' , . . . . t i m e , B p r e A F x l , ' b ' , t i m e , B p r e A F x 2 , ' b ' ) x l a b e l ( ' T i m e [ S ] ' ) ; y l a b e l ( ' L o n g i t u d i n a l D i s t a n c e [ m ] ' ) ; f i g u r e ( 2 ) p l o t ( t i m e , T 2 , ' k ' , t i m e , A p r e A F y l , ' r ' , t i m e , A p r e A F y 2 , ' r ' , . . . . t i m e , B p r e A F y l , ' b ' , t i m e , B p r e A F y 2 , ' b ' ) x l a b e l ( ' T i m e [ S ] ' ) ; y l a b e l ( ' L a t e r a l D i s t a n c e [ m ] ' ) ; f i g u r e ( 3 ) p l o t ( t i m e , A e r f x , ' r ' , t i m e , B e r f x , ' b ' ) x l a b e l ( ' T i m e [ S ] ' ) ; y l a b e l ( ' E r r o r [ m ] ' ) ; f i g u r e ( 4 ) p l o t ( t i m e , A e r f y , ' r ' , t i m e , B e r f y , ' b ' ) x l a b e l ( ' T i m e [ S ] ' ) ; y l a b e l ( ' E r r o r [ m ] ' ) ; f i g u r e ( 6 ) p l o t ( t i m e , A w i d t h F x , ' r ' , t i m e , B w i d t h F x , ' b ' ) x l a b e l ( ' T i m e [ S ] ' ) ; y l a b e l ( ' B o u n d a r y W i d t h [ m ] ' ) ; f i g u r e ( 7 ) p l o t ( t i m e , A w i d t h F y , ' r ' , t i m e , B w i d t h F y , ' b ' ) x l a b e l ( ' T i m e [ S ] ' ) ; y l a b e l ( ' B o u n d a r y W i d t h [ m ] ' ) ; f i g u r e ( 8 ) p l o t ( t i m e , R w i d t h F y , ' r ' ) x l a b e l ( ' T i m e [ S ] ' ) ; y l a b e l ( ' R e d u c e d W i d t h [ m ] ' ) ; ._~, · L % % ~%%%~%%%%%%%%EndOfProgramme%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 1 5 3 i > t . : " ' . ,