Dong hoc thuan

download Dong hoc thuan

of 17

Transcript of Dong hoc thuan

  • 8/7/2019 Dong hoc thuan

    1/17

    F o r w a r d D y n a m i c s , E l i m i n a t i o n M e t h o d s , a n d

    F o r m u l a t i o n S t i n e s s i n R o b o t S i m u l a t i o n

    U r i M . A s c h e r

    , D i n e s h K . P a i

    y

    a n d B e n o i t P . C l o u t i e r

    z

    D e p a r t m e n t o f C o m p u t e r S c i e n c e

    U n i v e r s i t y o f B r i t i s h C o l u m b i a

    V a n c o u v e r , B . C . V 6 T 1 Z 4 , C a n a d a

    M a y 2 7 , 1 9 9 6

    A b s t r a c t

    T h e n u m e r i c a l s i m u l a t i o n p r o b l e m o f t r e e - s t r u c t u r e d m u l t i b o d y s y s t e m s ,

    s u c h a s r o b o t m a n i p u l a t o r s , i s u s u a l l y t r e a t e d a s t w o s e p a r a t e p r o b l e m s : i

    t h e f o r w a r d d y n a m i c s p r o b l e m f o r c o m p u t i n g s y s t e m a c c e l e r a t i o n s , a n d i i t h e

    n u m e r i c a l i n t e g r a t i o n p r o b l e m f o r a d v a n c i n g t h e s t a t e i n t i m e . T h e i n t e r a c -

    t i o n o f t h e s e t w o p r o b l e m s c a n b e i m p o r t a n t a n d h a s l e d t o n e w c o n c l u s i o n s

    a b o u t t h e o v e r a l l e c i e n c y o f m u l t i b o d y s i m u l a t i o n a l g o r i t h m s C l o u t i e r e t a l . ,

    1 9 9 5 . I n p a r t i c u l a r , t h e f a s t e s t f o r w a r d d y n a m i c s m e t h o d s a r e n o t n e c e s s a r -

    i l y t h e m o s t n u m e r i c a l l y s t a b l e , a n d i n i l l - c o n d i t i o n e d c a s e s m a y s l o w d o w n

    p o p u l a r a d a p t i v e s t e p - s i z e i n t e g r a t i o n m e t h o d s . T h i s p h e n o m e n o n i s c a l l e d

    f o r m u l a t i o n s t i n e s s . "

    I n t h i s p a p e r , w e r s t u n i f y t h e d e r i v a t i o n o f b o t h t h e c o m p o s i t e r i g i d b o d y

    m e t h o d W a l k e r & O r i n , 1 9 8 2 a n d t h e a r t i c u l a t e d - b o d y m e t h o d F e a t h e r s t o n e ,

    1 9 8 3 ; F e a t h e r s t o n e , 1 9 8 7 a s t w o e l i m i n a t i o n m e t h o d s t o s o l v e t h e s a m e l i n e a r

    s y s t e m , w i t h t h e a r t i c u l a t e d b o d y m e t h o d t a k i n g a d v a n t a g e o f s p a r s i t y . T h e n

    t h e n u m e r i c a l i n s t a b i l i t y p h e n o m e n o n f o r t h e c o m p o s i t e r i g i d b o d y m e t h o d i s

    e x p l a i n e d a s a c a n c e l l a t i o n e r r o r t h a t c a n b e a v o i d e d , o r a t l e a s t m i n i m i z e d ,

    w h e n u s i n g a n a p p r o p r i a t e v e r s i o n o f t h e a r t i c u l a t e d b o d y m e t h o d . S p e c i c a l l y ,

    w e s h o w t h a t a v a r i a n t o f t h e a r t i c u l a t e d - b o d y m e t h o d i s b e t t e r s u i t e d t o d e a l

    w i t h c e r t a i n t y p e s o f i l l - c o n d i t i o n i n g t h a n t h e c o m p o s i t e r i g i d b o d y m e t h o d .

    T h e u n i e d d e r i v a t i o n a l s o c l a r i e s t h e u n d e r l y i n g l i n e a r a l g e b r a o f f o r w a r d

    d y n a m i c s a l g o r i t h m s a n d i s t h e r e f o r e o f i n t e r e s t i n i t s o w n a c c o r d .

    S u p p o r t e d i n p a r t b y a n N S E R C R e s e a r c h G r a n t .

    y

    S u p p o r t e d i n p a r t b y g r a n t s f r o m N S E R C , B . C . A S I , I R I S , a n d a C S A S t e a r 8 c o n t r a c t t h r o u g h

    M P B T e c h n o l o g i e s .

    z

    S u p p o r t e d i n p a r t b y N S E R C a n d F C A R f e l l o w s h i p s . C u r r e n t l y w i t h M P B T e c h n o l o g i e s I n c . ,

    P o i n t e C l a i r e , C a n a d a .

    1

  • 8/7/2019 Dong hoc thuan

    2/17

    1 I n t r o d u c t i o n

    S i m u l a t i o n o f r o b o t d y n a m i c s h a s b e e n u s e d i n o - l i n e t a s k s s u c h a s r o b o t d e s i g n t e s t -

    i n g , r o b o t p r o g r a m m i n g , a n d c o n t r o l l e r v a l i d a t i o n . R e c e n t l y , t h e n e e d f o r r e a l - t i m e

    p e r f o r m a n c e h a s b e c o m e i m p o r t a n t f o r o n - l i n e a p p l i c a t i o n s s u c h a s v i r t u a l e n v i r o n -

    m e n t s f o r o p e r a t o r t r a i n i n g , p r e d i c t i v e d i s p l a y s f o r t i m e - d e l a y e d t e l e o p e r a t i o n , a n d

    d e v e l o p m e n t o f a d v a n c e d r o b o t c o n t r o l s c h e m e s . H e n c e i t h a s b e c o m e m o r e i m p o r t a n t

    t o u n d e r s t a n d a n d c o m p a r e t h e a c t u a l p e r f o r m a n c e o f s i m u l a t i o n a l g o r i t h m s .

    T h i s p a p e r f o c u s e s o n a l g o r i t h m s f o r s i m u l a t i n g t h e m o t i o n o f r o b o t m a n i p u l a t o r s

    w i t h o u t c l o s e d l o o p s f o r e x a m p l e r o b o t s w h o s e m o t i o n i s n o t c o n s t r a i n e d b y c o n t a c t s

    w i t h t h e e n v i r o n m e n t . F o r t h e s i m u l a t i o n o f s u c h r o b o t s , t h e e l i m i n a t i o n m e t h o d

    l e a d s t o a n O D E f o r m u l a t i o n o f t h e e q u a t i o n s o f m o t i o n :

    = M q q + c q ; _q

    w h e r e :

    i s t h e N 1 v e c t o r o f t o r q u e s f o r c e s a p p l i e d b y t h e j o i n t a c t u a t o r s ,

    q i s t h e N 1 v e c t o r o f j o i n t v a r i a b l e s _ q a n d q a r e t h e j o i n t v e l o c i t i e s a n d

    a c c e l e r a t i o n s ,

    M i s t h e N N j o i n t - s p a c e i n e r t i a m a t r i x J S I M o r g e n e r a l i z e d i n e r t i a m a t r i x ,

    c i s t h e N 1 b i a s v e c t o r r e p r e s e n t i n g t h e t o r q u e s f o r c e s d u e t o g r a v i t y ,

    c e n t r i f u g a l a n d C o r i o l i s a c c e l e r a t i o n s , a n d a n y e x t e r n a l m o m e n t s a n d f o r c e s

    a c t i n g o n t h e e n d e e c t o r o f t h e m a n i p u l a t o r .

    H e r e , N i s t h e n u m b e r o f d e g r e e s o f f r e e d o m .

    T h e s i m u l a t i o n p r o b l e m r e q u i r e s t h a t t h e j o i n t t r a j e c t o r y o f a r o b o t b e d e t e r m i n e d

    g i v e n p r i o r k n o w l e d g e o f t h e t o r q u e s a n d f o r c e s a p p l i e d b y t h e a c t u a t o r s a n d t h e i n i t i a l

    s t a t e o f t h e r o b o t . T h e s i m u l a t i o n p r o b l e m i s u s u a l l y d i v i d e d i n t o t w o s u b p r o b l e m s .

    T h e f o r w a r d d y n a m i c s p r o b l e m : c o m p u t i n g t h e j o i n t a c c e l e r a t i o n s g i v e n t h e

    a c t u a t o r f o r c e s a n d t o r q u e s .

    T h e m o t i o n i n t e g r a t i o n p r o b l e m : c o m p u t i n g t h e j o i n t t r a j e c t o r y j o i n t p o s i t i o n s

    a n d v e l o c i t i e s g i v e n t h e j o i n t a c c e l e r a t i o n s a n d i n i t i a l c o n d i t i o n s .

    I n r e c e n t y e a r s , m a n y d i e r e n t a l g o r i t h m s h a v e b e e n p r o p o s e d f o r s o l v i n g t h e f o r -

    w a r d d y n a m i c s p r o b l e m , r a n g i n g i n c o m p u t a t i o n a l c o m p l e x i t y f r o m O N

    3

    e . g . t h e

    c o m p o s i t e r i g i d b o d y m e t h o d C R B M W a l k e r & O r i n , 1 9 8 2 t o O N e . g . t h e

    a r t i c u l a t e d - b o d y m e t h o d A B M F e a t h e r s t o n e , 1 9 8 3 ; F e a t h e r s t o n e , 1 9 8 7 . O p e r a -

    t i o n c o u n t s o f f o r w a r d d y n a m i c s a l g o r i t h m s h a v e b e c o m e o n e o f t h e m o s t c o m m o n l y

    u s e d m e a n s o f c o m p a r i n g t h e p e r f o r m a n c e o f d i e r e n t a l g o r i t h m s , i . e . t h e t w o p a r t s o f

    2

  • 8/7/2019 Dong hoc thuan

    3/17

  • 8/7/2019 Dong hoc thuan

    4/17

    -491.8

    -491.6

    -491.4

    -491.2

    -491

    -490.8

    -490.6

    -490.4

    0 1 2 3 4 5 6

    Joint2

    Acceleration

    q[1]

    "q1vAabm-single"

    A r t i c u l a t e d - b o d y M e t h o d

    -493

    -492.5

    -492

    -491.5

    -491

    -490.5

    -490

    0 1 2 3 4 5 6

    Joint2

    Acceleration

    q[1]

    "q1vAcrbm-single"

    C o m p o s i t e R i g i d - B o d y M e t h o d

    F i g u r e 1 : C o m p a r i s o n o f c a l c u l a t e d j o i n t 2 b a s e j o i n t a c c e l e r a t i o n s . N o t e t h e

    s m o o t h n e s s o f t h e A B M s o l u t i o n .

    4

  • 8/7/2019 Dong hoc thuan

    5/17

    r n

    A B M

    n

    C R B M

    O

    m a x

    0 . 1 0 0 0 3 1 6 9 3 0 4 1 1 0

    2

    0 . 0 5 0 0 4 6 2 3 5 1 0 9 1 0

    3

    0 . 0 1 0 0 8 1 8 9 8 0 8 2 1 0

    4

    0 . 0 0 5 0 1 0 5 3 4 1 0 4 2 0 1 0

    5

    0 . 0 0 4 0 1 1 1 7 0 1 1 8 3 5 1 0

    5

    0 . 0 0 3 5 1 2 3 7 5 1 3 7 9 2 1 0

    5

    0 . 0 0 3 0 1 1 9 9 0 1 8 9 8 6 1 0

    5

    0 . 0 0 2 5 1 2 1 7 0 3 5 4 0 6 1 0

    5

    0 . 0 0 2 0 1 2 7 7 9 4 4 2 9 4 1 0

    5

    0 . 0 0 1 8 1 2 4 1 8 8 6 6 5 6 1 0

    5

    0 . 0 0 1 5 1 2 8 6 0 f a i l 1 0

    5

    T a b l e 1 : N u m b e r o f f o r w a r d d y n a m i c s e v a l u a t i o n s n

    A B M

    a n d n

    C R B M

    r e q u i r e d t o

    s i m u l a t e 1 0 s e c o n d s o f m o t i o n f o r v a r i o u s 2 l i n k c h a i n s o f i n c r e a s i n g c o n d i t i o n n u m b e r .

    T h e t a b l e d i s p l a y s t h e l i n k l e n g t h r a t i o r = l

    2

    = l

    1

    + l

    2

    , t h e n u m b e r o f f o r w a r d

    d y n a m i c s f u n c t i o n e v a l u a t i o n s f o r e a c h m e t h o d n

    A B M

    , n

    C R B M

    , a n d t h e o r d e r o f

    m a g n i t u d e o f t h e m a x i m u m c o n d i t i o n n u m b e r

    m a x

    o f M . F o r t h e s e s i m u l a t i o n s ,

    a f o u r t h o r d e r R u n g e - K u t t a a d a p t i v e s t e p s i z e i n t e g r a t o r w a s u s e d , w i t h s t e p s i z e

    a c c u r a c y o f 1 e - 4 .

    t o t h e l e n g t h o f t h e a r m , n o f r i c t i o n , n o a p p l i e d a c t u a t o r t o r q u e s , u n i f o r m l i n k w i d t h s

    = 0 0 2 m , r s t d i s t a l l i n k o f l e n g t h l

    1

    = 2 0 m a n d m a s s m

    1

    = 1 0 0 k g , a n d s e c o n d

    p r o x i m a l l i n k o f l e n g t h l

    2

    = 0 0 2 m a n d m a s s m

    2

    = 0 1 k g . N o t e t h a t t h e l i n k s a r e

    n u m b e r e d s t a r t i n g f r o m t h e f r e e e n d f o l l o w i n g J a i n , 1 9 9 1 s e e x 2

    O b s e r v e t h a t t h e A B m e t h o d y i e l d s a m u c h s m o o t h e r a c c e l e r a t i o n t h a n t h e C R B

    m e t h o d . W h e n t h i s i s f e d t o a n a d a p t i v e O D E m o t i o n i n t e g r a t o r t h e r e s u l t i s t h a t m u c h

    s m a l l e r i n t e g r a t i o n s t e p s a r e n e e d e d w i t h t h e C R B M t o m a i n t a i n a g i v e n a c c u r a c y ,

    b e c a u s e o f t h e r o u g h n e s s o f t h e c a l c u l a t e d a c c e l e r a t i o n s ; h e n c e t h e A B M y i e l d s a n

    o v e r a l l f a s t e r a l g o r i t h m , e v e n t h o u g h a t e a c h p o i n t t t h e C R B M i s f a s t e r n o t e t h a t

    N = 2 i s v e r y s m a l l .

    T a b l e 1 i l l u s t r a t e s t h i s w i t h t h e r e s u l t s o f 1 0 s e c o n d s i m u l a t i o n s o f v a r i o u s t w o l i n k

    c h a i n s . E a c h s y s t e m s t a r t s a t r e s t f r o m t h e c o n g u r a t i o n q

    1

    = q

    2

    = 0 0 . A c c e l e r a t i o n s

    a r e c o m p u t e d u n d e r s i m i l a r c o n d i t i o n s t o t h o s e d e s c r i b e d a b o v e , w i t h t h e e x c e p t i o n

    t h a t , i n t h i s c a s e , t h e r a t i o o f t h e l i n k l e n g t h s a n d m a s s e s i s v a r i e d w h i l e k e e p i n g t h e

    s a m e t o t a l m a s s a n d l e n g t h t h i s e n s u r e s t h a t e a c h s y s t e m h a s t h e s a m e t o t a l e n e r g y .

    T h e l i n k s a r e r e c t a n g u l a r w i t h l i n k l e n g t h s l

    1

    + l

    2

    = 1 0 m , w i d t h s h

    1

    = h

    2

    = 0 1 m ,

    a n d m a s s e s m

    1

    + m

    2

    = 1 0 k g , w i t h l

    1

    = m

    1

    a n d l

    2

    = m

    2

    . I t i s c l e a r f r o m T a b l e 1 t h a t

    f o r i l l - c o n d i t i o n e d s y s t e m s t h e A B M b e c o m e s a n o v e r a l l m o r e e e c t i v e m e t h o d t h a n

    t h e C R B M , e v e n f o r s m a l l N

    5

  • 8/7/2019 Dong hoc thuan

    6/17

    2

    M o r e d e t a i l s o n t h i s e x a m p l e a r e g i v e n i n C l o u t i e r e t a l . , 1 9 9 5 . T h e v a l i d i t y o f t h e

    s i m u l a t i o n r e s u l t s w a s c o n r m e d b y c o m p a r i n g o u r r e s u l t s t o t h o s e o f a c o m m e r c i a l l y

    a v a i l a b l e s i m u l a t i o n p a c k a g e S D F A S T

    4

    . O u r p u r p o s e h e r e i s t o e x p l a i n t h e o b s e r v e d

    r e s u l t s .

    F o r t h i s p u r p o s e w e m u s t r s t g i v e t h e s e m e t h o d s a u n i e d , a l g e b r a i c d e s c r i p t i o n .

    T h i s i s d o n e i n x 2 , f o l l o w i n g J a i n , 1 9 9 1 a n d L u b i c h e t a l . , 1 9 9 2 . W e s h o w t h a t ,

    u s i n g s p a t i a l r e p r e s e n t a t i o n , t h e d i e r e n t f o r w a r d d y n a m i c s a l g o r i t h m s c a n i n f a c t

    b e v i e w e d a s d i e r e n t G a u s s i a n e l i m i n a t i o n m e t h o d s t o s o l v e t h e s a m e s y s t e m o f

    a l g e b r a i c l i n e a r e q u a t i o n s . T h e A B M s i m p l y u s e s t h e s p a r s i t y s t r u c t u r e o f t h e

    s y s t e m m o r e e e c t i v e l y w h e n N i s l a r g e , w h e r e a s i t r e q u i r e s m o r e o v e r h e a d t h a n

    C R B M w h e n N i s s m a l l s a y N 7

    I n r e l a t e d w o r k , E l l i s e t a l . , 1 9 9 2 ; E l l i s & R i c k e r , 1 9 9 4 c o n s i d e r e d t h e n u m e r i c a l

    s t a b i l i t y o f a r t i c u l a t e d b o d y a l g o r i t h m s . T h e y s h o w e d t h a t t h e A B M c a n b e c o n s i d -

    e r e d a s a m o d i e d G a u s s i a n e l i m i n a t i o n p e r f o r m e d o n a d e n s e i n e r t i a l s u p e r m a t r i x , "

    a n d t h e r e f o r e s t a b l e a s l o n g a s t h e p o s i t i v e d e n i t e n e s s i s m a i n t a i n e d . H o w e v e r , t h e y

    d i d n o t o b s e r v e a n y n u m e r i c a l s u p e r i o r i t y o f t h e A B M o v e r o t h e r m e t h o d s s u c h a s

    C R B M a n d i t i s n o t o b v i o u s t o u s h o w t o a p p l y t h e a n a l y s i s t h e r e t o e x p l a i n f o r m u -

    l a t i o n s t i n e s s . O u r d e r i v a t i o n i s n o t r e l a t e d t o t h e i n e r t i a l s u p e r m a t r i x b u t r a t h e r

    i s p e r f o r m e d o n a s p a r s e K u h n - T u c k e r " - t y p e m a t r i x d e r i v e d f r o m t h e d y n a m i c s o f

    i n d i v i d u a l b o d i e s a n d t h e i r c o n s t r a i n t s e . g . L u b i c h e t a l . , 1 9 9 2 . T h i s c o r r e s p o n d s

    t o f o r m u l a t i n g t h e p r o b l e m a s a s y s t e m o f d i e r e n t i a l - a l g e b r a i c e q u a t i o n s D A E s

    r a t h e r t h a n O D E s . I n p a r t i c u l a r , o n c e t h e m a t r i x i s f o r m e d , w e f o r m a l l y d e r i v e b o t h

    t h e C R B M a n d A B M f r o m l i n e a r a l g e b r a c o n s i d e r a t i o n s o n l y .

    A r m e d w i t h t h e a l g o r i t h m d e s c r i p t i o n s o f x 2 w e t h e n e x p l a i n t h e s o u r c e o f n u -

    m e r i c a l i n s t a b i l i t y i n t h e C R B M i n x 3 . T h e s u p e r i o r i t y o f t h e A B M i n t h i s r e s p e c t

    a r i s e s f r o m a b e t t e r o r d e r i n w h i c h t h e p i v o t a l e l e m e n t s i n t h e e l i m i n a t i o n m a t r i x

    a r e f o r m e d , w h i c h a l l o w s f o r l e s s c a n c e l l a t i o n e r r o r t h a n f o r t h e C R B M . W i t h t h e

    A B M , t o o , a t t e n t i o n m u s t b e p a i d t o t h e o r d e r i n w h i c h o p e r a t i o n s a r e p e r f o r m e d .

    T h e u s u a l m e t h o d e . g . F e a t h e r s t o n e , 1 9 8 7 p e r f o r m s w e l l i n i l l - c o n d i t i o n e d c a s e s

    o n l y i f l o c a l c o o r d i n a t e s y s t e m s a r e e s s e n t i a l l y a l i g n e d w i t h t h e p r i n c i p a l d i r e c t i o n s

    o f m o t i o n . F o r t h e g e n e r a l c a s e a v a r i a n t o f A B M c a l l e d m o d i e d A B M i s d e v e l o p e d

    i n x 4

    4

    S D F A S T i s a t r a d e m a r k o f S y m b o l i c D y n a m i c s , I n c . , 5 6 1 B u s h S t r e e t , M o u n t a i n V i e w , C A

    9 4 0 4 1 U S A .

    6

  • 8/7/2019 Dong hoc thuan

    7/17

    Link k-1

    Link k

    Link k+1

    Joint k

    Joint k-1

    Ok Ok

    O k-1

    O k-1

    + +

    Tip Base

    dk

    = center of mass

    Ok+1

    lk+1,k

    F i g u r e 2 : L i n k k o f c h a i n

    2 U n i e d p r o b l e m f o r m u l a t i o n a n d s o l u t i o n m e t h -

    o d s

    W e w i l l u s e e s s e n t i a l l y t h e n o t a t i o n d e v e l o p e d b y R o d r i g u e z , J a i n , a n d K r e u t z -

    D e l g a d o R o d r i g u e z e t a l . , 1 9 9 1 ; J a i n , 1 9 9 1 , w h i c h i n t u r n u s e s a v a r i a n t o f t h e

    s p a t i a l v e c t o r n o t a t i o n i n t r o d u c e d b y F e a t h e r s t o n e , 1 9 8 7 . C o n s i d e r , f o r t h e s a k e o f

    s i m p l i c i t y , a c h a i n o f n l i n k s n u m b e r e d f r o m t i p t o b a s e . C o n c e n t r a t i n g o n t h e k t h

    l i n k a n d k t h j o i n t s e e g u r e 2 , w e d e n e

    M

    k

    =

    J

    k

    , m

    k

    ~

    d

    k

    2

    m

    k

    ~

    d

    k

    , m

    k

    ~

    d

    k

    m

    k

    I

    !

    m

    k

    m a s s o f k t h b o d y

    J

    k

    m o m e n t o f i n e r t i a a b o u t c e n t e r o f m a s s

    O

    k

    r e f e r e n c e l o c a t i o n o f k t h j o i n t o n k t h l i n k

    d

    k

    i s t h e d i s p l a c e m e n t f r o m O

    k

    t o t h e c e n t e r o f m a s s ;

    ~

    d

    k

    i s t h e s k e w - s y m m e t r i c

    m a t r i x o f t h e c r o s s - p r o d u c t .

    M

    k

    6 6 s p a t i a l i n e r t i a o f k t h l i n k a b o u t O

    k

    k k , 1

    c o m p o s i t e b o d y t r a n s f o r m a t i o n o p e r a t o r w h i c h t r a n s f o r m s s p a t i a l v e -

    l o c i t i e s a n d o t h e r c o n t r a v a r i a n t q u a n t i t i e s f r o m O

    k

    t o O

    k , 1

    a 6 6 m a t r i x ,

    w h i c h d e p e n d s o n t h e d i s p l a c e m e n t v e c t o r b e t w e e n t h e s e t w o p o i n t s . T h e

    "

    d e n o t e s a d j o i n t . T h e r e f o r e

    k k , 1

    t r a n s f o r m s s p a t i a l f o r c e s f r o m O

    k , 1

    t o O

    k

    H

    k

    j o i n t m a t r i x f o r k t h j o i n t f o r a 1 D O F j o i n t , H

    k

    i s a 6 - v e c t o r , o t h e r w i s e

    i t ' s a m a t r i x ; i f a l l j o i n t s h a v e 1 D O F t h e n N = n

    7

  • 8/7/2019 Dong hoc thuan

    8/17

    _p

    k

    s p a t i a l v e l o c i t y o f k t h l i n k a t O

    k

    _p

    k

    i s a 6 - v e c t o r . W e n o t e t h a t t h e s p a t i a l

    v e l o c i t y _ p

    k

    s h o u l d n o t b e i n t e r p r e t e d a s t h e d e r i v a t i v e o f a g l o b a l l y d e n e d s e t

    o f c o o r d i n a t e s p

    k

    d u e t o w e l l k n o w n p r o p e r t i e s o f S E 3 .

    S o , t h e r e l a t i v e s p a t i a l v e l o c i t y a c r o s s t h e k t h j o i n t i s H

    k

    _q

    k

    , a n d

    _p

    k

    =

    k + 1 k

    _p

    k + 1

    + H

    k

    _q

    k

    p

    k

    =

    k + 1 k

    p

    k + 1

    + H

    k

    q

    k

    + a

    k

    1

    f o r k = n ; n , 1 ; : : : ; 1 , _ p

    n + 1

    = p

    n + 1

    = 0 , w h e r e a

    k

    i s C o r i o l i s a n d c e n t r i f u g a l

    a c c e l e r a t i o n .

    T h e e q u a t i o n s o f m o t i o n o f t h e k t h l i n k a b o u t O

    k

    a r e

    f

    k

    =

    k k , 1

    f

    k , 1

    + M

    k

    p

    k

    + b

    k

    k

    = H

    k

    f

    k

    2

    f o r k = 1 ; 2 ; : : : ; n f

    0

    = 0 , w h e r e

    k

    j o i n t f o r c e a t k t h j o i n t s c a l a r f o r a 1 D O F j o i n t

    b

    k

    g y r o s c o p i c f o r c e

    D e n i n g

    "

    =

    0

    B

    B

    B

    B

    B

    B

    @

    0 0 0 0 0

    2 1

    0 0 0

    0

    3 2

    0 0

    0 0

    n n , 1

    0

    1

    C

    C

    C

    C

    C

    C

    A

    w e c a n p u t t h e r e c u r s i o n r e l a t i o n s 1 , 2 i n m a t r i x f o r m ,

    = I , "

    , 1

    = I + "

    + + "

    n , 1

    M = d i a g f M

    1

    ; : : : ; M

    n

    g

    H = d i a g f H

    1

    ; : : : ; H

    n

    g

    T h i s r e s u l t s i n t h e e q u a t i o n s

    _p =

    H

    _q

    p =

    H

    q + a

    f = M p + b

    8

  • 8/7/2019 Dong hoc thuan

    9/17

    = H f = H M

    H

    q + H M

    a + b

    t h e v a r i o u s v e c t o r s q ; p ; f ; a ; b ; h a v e a n o b v i o u s c o m p o n e n t w i s e n o t a t i o n i n 1 , 2 ,

    o r

    M q = , c 3

    T h e C R B M s o l v e s t h i s l a t t e r l i n e a r s y s t e m o f e q u a t i o n s f o r q . T h i s t a k e s O N

    3

    o p e r a t i o n s u s i n g a d i r e c t G a u s s i a n e l i m i n a t i o n m e t h o d . O n t h e o t h e r h a n d , O N

    m e t h o d s l i k e A B M c o n s i s t i n p r o p a g a t i n g t h e r e c u r s i o n s w i t h p , q , r a t h e r t h a n f o r m i n g

    e q u a t i o n s f o r q r s t . T h i s i d e a h a s b e e n r e d i s c o v e r e d a f e w t i m e s i n t h e l i t e r a t u r e ,

    s e e , e . g . A r m s t r o n g , 1 9 7 9 ; B a e & H a u g , 1 9 8 7 ; B r a n d l e t a l . , 1 9 8 6 ; F e a t h e r s t o n e ,

    1 9 8 3 ; R o d r i g u e z , 1 9 8 7 ; V e r e s h c h a g i n , 1 9 7 4 .

    T o u n d e r s t a n d t h i s b e t t e r , w e w r i t e t h e e q u a t i o n s i n v o l v i n g q ; p a n d f a s o n e

    a l g e b r a i c s y s t e m , f o l l o w i n g L u b i c h e t a l . , 1 9 9 2 ,

    0

    B

    @

    M 0

    , 1

    0 0 H

    , 1

    H

    0

    1

    C

    A

    0

    B

    @

    , p

    q

    f

    1

    C

    A

    =

    0

    B

    @

    b

    , a

    1

    C

    A

    4

    a n d c o n s i d e r v a r i o u s s t r a t e g i e s o f G a u s s i a n e l i m i n a t i o n f o r t h i s s y s t e m .

    5

    1 . S t r a i g h t f o r w a r d b l o c k - r o w e l i m i n a t i o n o f t h e s e c o n d b l o c k - r o w i n 4 u s i n g t h e

    r s t a n d t h e n t h e t h i r d b l o c k - r o w s g i v e s

    0

    B

    @

    M 0

    , 1

    0 M 0

    , 1

    H

    0

    1

    C

    A

    0

    B

    @

    , p

    q

    f

    1

    C

    A

    =

    0

    B

    @

    b

    , c

    , a

    1

    C

    A

    w h e r e M = H M

    H

    a n d c = H M

    a + b . T h e s e c o n d b l o c k - r o w i s n o w

    d e c o u p l e d f r o m t h e r e s t , a n d y i e l d s t h e s y s t e m 3 . T h i s e s s e n t i a l l y g i v e s t h e

    C R B m e t h o d

    6

    2 . I n s t e a d , w e c a n a t t e m p t t o t a k e a d v a n t a g e o f t h e s p a r s i t y o f 4 . W e w r i t e t h e

    s y s t e m i n b l o c k f o r m

    0

    B

    @

    M

    k

    0 I

    0 0 H

    k

    I H

    k

    0

    1

    C

    A

    0

    B

    @

    , p

    k

    q

    k

    f

    k

    1

    C

    A

    =

    0

    B

    @

    ~

    b

    k

    k

    , ~a

    k

    1

    C

    A

    5

    5

    N o t e t h a t 4 c o r r e s p o n d s t o a n e m b e d d i n g o f t h e O D E s y s t e m 3 i n a l a r g e r D A E s y s t e m .

    T h i s , h o w e v e r , i s j u s t a c o n v e n i e n t r e p r e s e n t a t i o n o f t h e s a m e m o d e l , a n d w e d o n o t a d v o c a t e u s e o f

    D A E m e t h o d s a s s u c h f o r s i m p l e m u l t i b o d y c h a i n s . I n b o t h a l g o r i t h m s d e s c r i b e d h e r e , t h e s o l u t i o n

    p r o c e s s d o e s n o t i n c l u d e n d i n g t h e a l g e b r a i c v a r i a b l e s .

    6

    W e n o t e t h a t t h e C R B M a c t u a l l y i n v o l v e s o n e m o r e f e a t u r e t h a t i s e s s e n t i a l f o r t h e e c i e n c y o f

    t h e m e t h o d : j o i n t s p a c e i n e r t i a m a t r i x M i s c o m p u t e d a s M = H R +

    ~

    R + R

    ~

    H

    w h e r e R i s a

    b l o c k d i a g o n a l m a t r i x o f c o m p o s i t e r i g i d b o d y i n e r t i a s a n d

    ~

    = , I . H o w e v e r , o u r e x p e r i m e n t a l

    r e s u l t s C l o u t i e r e t a l . , 1 9 9 5 s u g g e s t t h a t t h e i n s t a b i l i t y i s p r i m a r i l y a c o n s e q u e n c e o f f o r m i n g t h e

    p o o r l y c o n d i t i o n e d j o i n t s p a c e i n e r t i a m a t r i x M

    9

  • 8/7/2019 Dong hoc thuan

    10/17

    w h e r e

    ~a

    k

    = a

    k

    +

    k + 1 k

    p

    k + 1

    ~

    b

    k

    = b

    k

    +

    k k , 1

    f

    k , 1

    a n d p e r f o r m t h e e l i m i n a t i o n p r o c e s s a s m u c h a s p o s s i b l e w i t h i n e a c h b l o c k 5 .

    F o r t h i s p u r p o s e , t h e b l o c k s k m u s t b e d e c o u p l e d u s i n g f

    0

    = 0 , p

    n + 1

    = 0 .

    T h i s e l i m i n a t i o n s t r a t e g y y i e l d s t h e A B M .

    S p e c i c a l l y , f o r t h e b l o c k - o r i e n t e d a p p r o a c h A B M w e o b t a i n a p e r m u t a t i o n o f

    t h e s y s t e m 4 . F o r i n s t a n c e , n = 3 g i v e s t h e s y s t e m M x = b , w h e r e

    M =

    0

    B

    B

    B

    B

    B

    B

    B

    B

    B

    B

    B

    B

    B

    B

    B

    @

    M

    1

    0 I

    0 0 H

    1

    I H

    1

    0 ,

    2 1

    ,

    2 1

    M

    2

    0 I

    0 0 H

    2

    I H

    2

    0 ,

    3 2

    ,

    3 2

    M

    3

    0 I

    0 0 H

    3

    I H

    3

    0

    1

    C

    C

    C

    C

    C

    C

    C

    C

    C

    C

    C

    C

    C

    C

    C

    A

    6

    x =

    0

    B

    B

    B

    B

    B

    B

    B

    B

    B

    B

    B

    B

    B

    B

    B

    @

    , p

    1

    q

    1

    f

    1

    , p

    2

    q

    2

    f

    2

    , p

    3

    q

    3

    f

    3

    1

    C

    C

    C

    C

    C

    C

    C

    C

    C

    C

    C

    C

    C

    C

    C

    A

    ; b =

    0

    B

    B

    B

    B

    B

    B

    B

    B

    B

    B

    B

    B

    B

    B

    B

    @

    b

    1

    1

    , a

    1

    b

    2

    2

    , a

    2

    b

    3

    3

    , a

    3

    1

    C

    C

    C

    C

    C

    C

    C

    C

    C

    C

    C

    C

    C

    C

    C

    A

    T h e e l i m i n a t i o n t h e r e f o r e p r o c e e d s a s f o l l o w s : f o r k = 1 ; 2 ; : : : ; n , e l i m i n a t e t h e

    m i d d l e r o w s i n t h e k

    t h

    b l o c k p r e c i s e l y a s i n t h e C R B M c a s e 4 , a n d a l s o f o r k n

    u s e t h e k

    t h

    b l o c k b l o c k - r o w s t o e l i m i n a t e ,

    k + 1 k

    i n o r d e r t o d e c o u p l e t h e n e x t b l o c k .

    T h e e l i m i n a t i o n o f ,

    k + 1 k

    u s i n g b l o c k k i n t h e f o r m c o r r e s p o n d i n g t o 4 p r o d u c e s

    a d e c o u p l e d b l o c k k + 1 w h e r e o n l y M

    k + 1

    n e e d s t o b e u p d a t e d t o

    M

    k + 1

    = M

    k + 1

    +

    k + 1 k

    M

    k

    k + 1 k

    ,

    k + 1 k

    M

    k

    H

    k

    H

    k

    M

    k

    H

    k

    , 1

    H

    k

    M

    k

    k + 1 k

    7

    M

    1

    = M

    1

    , a n d a c o r r e s p o n d i n g u p d a t e i s a p p l i e d t o t h e r i g h t h a n d s i d e :

    k

    , c

    k

    =

    k

    , H

    k

    b

    k

    , H

    k

    M

    k

    a

    k

    1 0

  • 8/7/2019 Dong hoc thuan

    11/17

  • 8/7/2019 Dong hoc thuan

    12/17

    c o o r d i n a t e s b u t a l s o t h e a b s o l u t e o n e s a r e c a l c u l a t e d . T h i s g i v e s t h e e d g e f o r s m a l l

    s y s t e m s t o t h e C R B M , s o l o n g a s t h e t w o a l g o r i t h m s p r o d u c e e s s e n t i a l l y t h e s a m e

    n u m e r i c a l r e s u l t s , w h i c h i s t h e c a s e i n w e l l - c o n d i t i o n e d m u l t i b o d y s y s t e m s . B u t

    E x a m p l e 1 s h o w s t h a t a d i e r e n t s i t u a t i o n a r i s e s f o r i l l - c o n d i t i o n e d s y s t e m s .

    I t i s s i m p l e t o e x t e n d t h i s a l g o r i t h m d e s c r i p t i o n t o t r e e s t r u c t u r e d s y s t e m s a n d

    e v e n t o s y s t e m s w i t h c l o s e d l o o p s L u b i c h e t a l . , 1 9 9 2 , e v e n t h o u g h t h e l a t t e r l e a d

    t o d i e r e n t i a l - a l g e b r a i c e q u a t i o n s w h i c h c a n n o t b e s i m p l y r e d u c e d t o O D E s a s i n 3

    a n d t h e O N c o m p l e x i t y i s a c h i e v e d o n l y f o r a x e d n u m b e r o f c l o s e d l o o p s . H e r e ,

    h o w e v e r , w e p r o c e e d t o e x p l a i n t h e s t a b i l i t y o f t h e s e a l g o r i t h m s , w h i c h m a y a l r e a d y

    b e o b s e r v e d f o r s i m p l e m u l t i b o d y c h a i n s .

    3 E x p l a i n i n g f o r m u l a t i o n s t i n e s s

    S i n c e w e h a v e s h o w n t h a t t h e C R B M a n d A B M s o l v e p r e c i s e l y t h e s a m e l i n e a r s y s t e m

    o f a l g e b r a i c e q u a t i o n s a t e a c h t i m e i n s t a n c e , o n e m a y s u s p e c t t h a t t h e d i e r e n c e i n

    r o u n d o e r r o r e e c t s i s d u e e i t h e r t o i n s u c i e n t p i v o t i n g i n t h e G a u s s i a n e l i m i n a t i o n

    p r o c e s s o f t h e C R B M o r t o a n e x c e s s i v e m i x i n g o f c o n t r i b u t i o n s f r o m d i e r e n t b l o c k s

    w h i c h r e s u l t s i n a n u n f o r t u n a t e c a n c e l l a t i o n e r r o r w h e n u s i n g o a t i n g p o i n t a r i t h -

    m e t i c . W e n o w c o n s i d e r a t w o - l i n k c h a i n w i t h t h e l i n k c o n n e c t e d t o t h e b a s e b e i n g

    m u c h l i g h t e r a n d s h o r t e r t h a n t h e o t h e r l i n k , a s i n E x a m p l e 1 , a n d s h o w t h a t p o o r

    c a n c e l l a t i o n i s t h e s o u r c e o f t h e o b s e r v e d r o u n d o e r r o r e e c t s .

    C o n s i d e r t h e A B M r s t . U s i n g t h e r s t a n d t h i r d b l o c k - r o w s t o e l i m i n a t e t h e

    s e c o n d r e s u l t s i n t h e e q u a t i o n s

    H

    1

    M

    1

    H

    1

    q

    1

    + H

    1

    M

    1

    2 1

    p

    2

    =

    1

    , H

    1

    b

    1

    + H

    1

    M

    1

    a

    1

    = :

    1

    , c

    1

    U s i n g t h e s a m e b l o c k - r o w s p l u s t h e a b o v e e q u a t i o n t o d e c o u p l e t h e s e c o n d b l o c k f r o m

    t h e r s t r e s u l t s i n t h e e q u a t i o n s f o r t h e s e c o n d b l o c k

    0

    B

    @

    M

    2

    0 I

    0 0 H

    2

    I H

    2

    0

    1

    C

    A

    0

    B

    @

    , p

    2

    q

    2

    f

    2

    1

    C

    A

    =

    0

    B

    @

    b

    2

    2

    , a

    2

    1

    C

    A

    8

    w h e r e

    M

    2

    = M

    2

    +

    2 1

    M

    1

    2 1

    ,

    2 1

    M

    1

    H

    1

    H

    1

    M

    1

    H

    1

    , 1

    H

    1

    M

    1

    2 1

    , a n d

    b

    2

    = b

    2

    +

    2 1

    b

    1

    +

    2 1

    M

    1

    a

    1

    +

    2 1

    M

    1

    H

    1

    H

    1

    M

    1

    H

    1

    , 1

    1

    , c

    1

    . N e x t u s i n g t h e r s t a n d t h i r d b l o c k - r o w s

    i n 8 t o e l i m i n a t e t h e s e c o n d r e s u l t s i n t h e e q u a t i o n s

    H

    2

    M

    2

    H

    2

    q

    2

    =

    2

    , H

    2

    b

    2

    + H

    2

    M

    2

    a

    2

    = :

    2

    , c

    2

    T h i s c o m p l e t e s t h e f o r w a r d p a s s . N o w w e s o l v e b a c k w a r d s f o r q

    2

    , t h e n p

    2

    = a

    2

    + H

    2

    q

    2

    ,

    a n d n a l l y w e s o l v e f o r q

    1

    N e x t c o n s i d e r t h e C R B M . W e h a v e

    M =

    H

    1

    M

    1

    H

    1

    H

    1

    M

    1

    2 1

    H

    2

    H

    2

    2 1

    M

    1

    H

    1

    H

    2

    M

    2

    +

    2 1

    M

    1

    2 1

    H

    2

    9

    1 2

  • 8/7/2019 Dong hoc thuan

    13/17

    T o s o l v e u s i n g t h e 2 2 m a t r i x M , w e e l i m i n a t e t h e l o w e r l e f t e l e m e n t b y a b l o c k - r o w

    o p e r a t i o n . T h i s g i v e s i n t h e l o w e r r i g h t d i a g o n a l p o s i t i o n p r e c i s e l y H

    2

    M

    2

    H

    2

    T h e r e f o r e , t h e r e i s n o p o o r p i v o t i n g s t r a t e g y i n v o l v e d h e r e , e v e n w h e n m

    1

    m

    2

    R a t h e r , t h e o n l y d i e r e n c e i s i n t h e o r d e r i n w h i c h

    M

    2

    i s b e i n g f o r m e d ! I n t h e C R B M

    w e m u s t , w h i l e f o r m i n g M , a d d t h e s m a l l M

    2

    t o t h e l a r g e

    2 1

    M

    1

    2 1

    . T h e n t h e

    o t h e r l a r g e , r a n k d e c i e n t t e r m

    2 1

    M

    1

    H

    1

    H

    1

    M

    1

    H

    1

    , 1

    H

    1

    M

    1

    2 1

    i s s u b t r a c t e d d u r i n g

    t h e s o l u t i o n p r o c e s s . T h i s m a y c a u s e c a n c e l l a t i o n e r r o r w h i c h a p p e a r s a s a r a n d o m

    f u n c t i o n o f t i m e t , e v e n t h o u g h a l l t h e q u a n t i t i e s i n 3 a r e s m o o t h f u n c t i o n s o f t

    W h e n s u c h n o i s e b e c o m e s r e l a t i v e l y l a r g e a n o n s m o o t h s o l u t i o n p r o l e r e s u l t s w h i c h

    w o u l d s l o w d o w n a n u n s u s p e c t i n g a d a p t i v e O D E d y n a m i c s s o l v e r .

    T h e A B M c a n b e e a s i l y f o r m u l a t e d t o p e r f o r m a s b a d l y a s t h e C R B M ! B u t w i t h

    t h e A B M w e c a n a l s o d o b e t t e r , b y r s t f o r m i n g t h e p r o j e c t e d i n e r t i a m a t r i x

    M

    1

    =

    M

    1

    , M

    1

    H

    1

    H

    1

    M

    1

    H

    1

    , 1

    H

    1

    M

    1

    . T h e n f o r m i n g

    2 1

    M

    1

    2 1

    g i v e s a r a n k d e c i e n t m a t r i x

    w i t h p o s s i b l y l a r g e t e r m s , a n d o n l y t h e n M

    2

    i s a d d e d t o f o r m

    M

    2

    E x a m p l e 2 I n o r d e r t o i l l u s t r a t e t h i s l a s t p o i n t , c o n s i d e r t h e c a s e w h e r e H

    1

    =

    0 ; 0 ; 0 ; 0 ; 0 ; 1 ,

    2 1

    = I a n d H

    2

    = H

    1

    . T h i s c h o i c e c o r r e s p o n d s t o c o l l i n e a r p r i s -

    m a t i c j o i n t s ; t h e c h o i c e o f

    2 1

    i s a g o o d a p p r o x i m a t i o n t o t h e a c t u a l t r a n s f o r m a t i o n

    w h e n t h e b a s e l i n k i s s h o r t ; a n d t h e c h o i c e o f p a r a l l e l H

    2

    g i v e s a w o r s t c a s e s c e n a r i o .

    F u r t h e r , t o s i m p l i f y n o t a t i o n a s s u m e t h a t t h e d i e r e n c e b e t w e e n t h e m a s s e s m

    1

    a n d

    m

    2

    i s s o l a r g e t h a t i n o a t i n g p o i n t a r i t h m e t i c f l m

    1

    + m

    2

    = f l m

    1

    . T h e n , w h e n

    u s i n g C R B M o n e f o r m s M

    2

    +

    2 1

    M

    1

    2 1

    , t h e c o n t r i b u t i o n o f m

    2

    i s e n t i r e l y l o s t , a n d

    t h e s y s t e m 3 b e c o m e s s i n g u l a r s e e 9 , w h e r e a l l e n t r i e s b e c o m e e q u a l , a n d T a b l e 2 .

    I n p a r t i c u l a r , o n e o b t a i n s H

    2

    M

    2

    H

    2

    = 0 . O n t h e o t h e r h a n d , i t i s e a s y t o s e e t h a t

    M

    1

    = M

    1

    I , H

    1

    H

    1

    , s o t h e 6 ; 6 , e n t r y o f

    M

    1

    i s z e r o w h i l e t h e r e s t a r e l i k e i n M

    1

    F o r m i n g M

    2

    +

    2 1

    M

    1

    2 1

    n o w g i v e s m

    2

    i n t h e l o w e r r i g h t c o r n e r , s o H

    2

    M

    2

    H

    2

    = m

    2

    c o r r e c t l y .

    W e r e m a r k t h a t s u c h b a d c a s e s c a n o c c u r i n p r a c t i c e ; f o r i n s t a n c e , i f t h e r e a r e t h r e e

    i n t e r s e c t i n g r e v o l u t e j o i n t s a t t h e s h o u l d e r , t w o o f t h e s e j o i n t s c a n b e c o m e c o l l i n e a r

    a t a s h o u l d e r s i n g u l a r i t y .

    2

    T h e i m p o r t a n c e o f M

    2

    i s t h e r e f o r e i n i t s c o n t r i b u t i o n t o t h e n u l l s p a c e o f

    ~

    M

    2

    =

    2 1

    M

    1

    2 1

    , p a r t i c u l a r l y w h a t e e c t r e m a i n s a f t e r t h e q u a d r a t i c f o r m w i t h H

    2

    i s

    t a k e n . M o r e g e n e r a l l y , w e c a n n o t a s s u m e t h a t

    2 1

    = I o r t h a t H

    2

    = H

    1

    . L e t

    S

    2

    = r a n g e f

    , 1

    2 1

    H

    1

    g a n d p r o j e c t H

    2

    i n t o t h i s s u b s p a c e ,

    H

    2

    = E

    2

    + F

    2

    w h e r e F

    2

    2 S

    2

    a n d E

    2

    ? S

    2

    . S i n c e

    M

    1

    H

    1

    = 0 ;

    1 3

  • 8/7/2019 Dong hoc thuan

    14/17

    w e h a v e t h a t

    H

    2

    M

    2

    H

    2

    = H

    2

    M

    2

    H

    2

    + E

    2

    ~

    M

    2

    E

    2

    1 0

    s o t h e t e r m i n v o l v i n g M

    2

    d o e s n o t g e t s w a l l o w e d i n n i t e p r e c i s i o n a r i t h m e t i c i f t h e

    o t h e r t e r m o n t h e r i g h t h a n d s i d e o f 1 0 i s n o t m u c h l a r g e r , a n d t h i s i n t u r n o c c u r s

    w h e n E

    2

    i s s u c i e n t l y s m a l l i n n o r m , w h i c h i s t h e o n l y c a s e w h e n t h e c o n t r i b u t i o n

    o f H

    2

    M

    2

    H

    2

    m a t t e r s .

    7

    I n 1 0 w e c o n s i d e r t h e e e c t o f a d d i n g H

    2

    M

    2

    H

    2

    t o H

    2

    ~

    M

    2

    H

    2

    . H o w e v e r , t h e o r d e r

    o f o p e r a t i o n s u s i n g t h e A B M w i t h t h e s t a n d a r d i m p l e m e n t a t i o n i s t o f o r m M

    2

    +

    ~

    M

    2

    r s t , a n d o n l y t h e n c a l c u l a t e t h e q u a d r a t i c f o r m w i t h H

    2

    . T h i s p r o c e d u r e c a n b e

    r u i n e d b y a p o o r c h o i c e o f c o o r d i n a t e s :

    ~

    M

    2

    =

    2 1

    M

    1

    2 1

    i s r a n k d e c i e n t , b u t c o u l d

    h a v e a l l l a r g e e n t r i e s i n g e n e r a l . F o r a n e x a m p l e , r e p e a t E x a m p l e 2 w i t h e v e r y t h i n g

    t h e s a m e e x c e p t H

    2

    = H

    1

    =

    1

    p

    2

    0 ; 0 ; 0 ; 0 ; 1 ; 1 . I n t h i s c a s e t h e A B M c o u l d p e r f o r m

    a s b a d l y a s t h e C R B M . W e o b s e r v e t h i s p h e n o m e n o n w i t h t h e S D F A S T p a c k a g e a s

    w e l l . F o r i n s t a n c e , i n E x a m p l e 1 , i f t h e c o o r d i n a t e s u s e d f o r d e n i n g t h e m o m e n t s

    o f i n e r t i a a r e r o t a t e d b y 4 5 d e g r e e s , t h e i n t e g r a t o r u s i n g t h e A B M o p t i o n b r e a k s

    d o w n a t r o u g h l y t h e s a m e c o n d i t i o n n u m b e r a s t h e C R B M . W h i l e s u c h d i c u l t i e s

    c a n s o m e t i m e s b e a d d r e s s e d b y a c a r e f u l c h o i c e o f c o o r d i n a t e s , t h i s i s n o t a l w a y s

    c o n v e n i e n t o r p o s s i b l e d u e t o t h e c o n s t r a i n t s p l a c e d b y m o d e l i n g s o f t w a r e . I n x 4

    b e l o w , w e d e s c r i b e a m o d i e d A B M t h a t a d d r e s s e s t h i s p r o b l e m .

    I t c a n b e v e r i e d i n g e n e r a l i . e . a l s o f o r n 2 t h a t w h e n u s i n g t h e C R B M t h e

    b l o c k p i v o t e l e m e n t s o b t a i n e d b y t h e G a u s s i a n e l i m i n a t i o n p r o c e s s

    8

    a r e H

    k

    M

    k

    H

    k

    , i . e .

    t h e y a r e t h e s a m e a s i n t h e A B M d e s c r i b e d i n F e a t h e r s t o n e , 1 9 8 3 ; J a i n , 1 9 9 1 . T h u s ,

    o b s e r v a t i o n s a n d e x p l a n a t i o n s m a d e h e r e r e g a r d i n g i l l - c o n d i t i o n e d r o b o t i c s s y s t e m s

    e x t e n d t o t h e m o r e g e n e r a l c a s e . I n f a c t , a d d i n g m o r e l i n k s a t t h e t i p e n d o f t h e

    i l l - c o n d i t i o n e d t w o - b o d y s y s t e m m a k e s t h e c o n d i t i o n i n g w o r s e . T h i s h a s a l s o b e e n

    v e r i e d b y e x p e r i m e n t a t i o n w i t h t h e S D F A S T p a c k a g e .

    4 M o d i e d A B M

    T h e a b o v e d e l i b e r a t i o n s h i g h l i g h t t h e p a r t i c u l a r i m p o r t a n c e o f c a l c u l a t i n g D

    k

    =

    H

    k

    M

    k

    H

    k

    a s a c c u r a t e l y a s p o s s i b l e . I n f a c t , u s i n g A B M t h e c a l c u l a t i o n o f

    M

    k

    m a y

    b e s i g n i c a n t l y m o r e p o l l u t e d b y o a t i n g p o i n t c a n c e l l a t i o n e r r o r e e c t s t h a n t h a t o f

    D

    k

    i f l o c a l c o o r d i n a t e s y s t e m s a r e n o t a l i g n e d w i t h t h e p r i n c i p a l d i r e c t i o n s o f m o t i o n .

    T h i s t h e n s u g g e s t s t o r e o r d e r t h e c a l c u l a t i o n s i n t h e A B M s o a s n o t t o u s e

    M

    k

    f o r

    c a l c u l a t i n g D

    k

    : S t a r t i n g w i t h D

    1

    = H

    1

    M

    1

    H

    1

    ,

    M

    k

    =

    M

    k

    ,

    M

    k

    H

    k

    D

    , 1

    k

    H

    k

    M

    k

    7

    W e n o t e t h a t i n n i t e p r e c i s i o n a r i t h m e t i c , t h e r e l a t i o n s h i p 1 0 a l s o h o l d s o n l y a p p r o x i m a t e l y ;

    b u t t h i s d o e s n o t a l t e r t h e e s s e n c e o f t h e a r g u m e n t .

    8

    u s i n g n o r o w p e r m u t a t i o n s a n d o u r s y s t e m o f l i n k e n u m e r a t i o n .

    1 4

  • 8/7/2019 Dong hoc thuan

    15/17

    C R B M A B M M o d i e d A B M

    m

    1

    c a s e D

    2

    q

    1

    D

    2

    q

    1

    D

    2

    q

    1

    1 . e + 7 1 1 . 0 9 7 1 . 1 9 1 . 0 9 7 1 . 1 9 1 . 0 9 7 1 . 1 9

    1 . e + 8 1 0 1 . 0 9 7 1 . 1 9 1 . 0 9 7 1 . 1 9

    1 . e + 9 1 0 1 . 0 9 7 1 . 1 9 1 . 0 9 7 1 . 1 9

    1 . e + 6 2 1 . 0 3 9 4 1 . 7 6 1 . 0 3 9 4 1 . 7 6 1 . 0 9 7 1 . 1 9

    1 . e + 7 2 1 . 5 6 4 7 . 4 6 1 . 5 6 4 7 . 4 6 1 . 0 9 7 1 . 1 9

    1 . e + 8 2 0 0 1 . 0 9 7 1 . 1 9

    1 . e + 9 2 0 0 1 . 0 9 7 1 . 1 9

    T a b l e 2 : C o m p a r i s o n o f m o d i e d A B M w i t h C R B M a n d A B M .

    ~

    M

    k + 1

    =

    k + 1 k

    M

    k

    k + 1 k

    D

    k + 1

    = H

    k + 1

    M

    k + 1

    H

    k + 1

    + H

    k + 1

    ~

    M

    k + 1

    H

    k + 1

    1 1

    M

    k + 1

    = M

    k + 1

    +

    ~

    M

    k + 1

    I n w o r d s , w e c o m p u t e a n d m a i n t a i n D

    k

    s e p a r a t e l y f r o m

    M

    k

    , k = 1 ; 2 ; : : : ; n . T h i s h a s

    t h e s m a l l e x t r a e x p e n s e o f c o m p u t i n g t h e q u a d r a t i c f o r m w i t h H

    k + 1

    t w i c e a t e a c h s t e p

    i n s t e a d o f o n c e , b u t i t h a s t h e a d d e d s t a b i l i t y i n c a s e o f a g e n e r a l l o c a l c o o r d i n a t e

    s y s t e m . A s b e f o r e , n o t e t h a t i n f a c t ,

    H

    k + 1

    ~

    M

    k + 1

    H

    k + 1

    = E

    k + 1

    ~

    M

    k + 1

    E

    k + 1

    w h e r e E

    k + 1

    i s t h e d i s t a n c e m a t r i x v e c t o r f o r a 1 D O F j o i n t f r o m H

    k + 1

    t o t h e s u b -

    s p a c e S

    k + 1

    = r a n g e f

    , 1

    k + 1 k

    H

    k

    g w h i c h i s t h e n u l l s p a c e o f

    ~

    M

    k + 1

    . T h e c o n t r i b u t i o n

    o f H

    k + 1

    M

    k + 1

    H

    k + 1

    t o D

    k + 1

    i s i m p o r t a n t , e v e n w h e n k M

    k + 1

    k k

    ~

    M

    k + 1

    k , p r e c i s e l y

    w h e n k E

    k + 1

    k i s s m a l l .

    E x a m p l e 3 I n T a b l e 2 w e l i s t c o m p u t a t i o n a l r e s u l t s o b t a i n e d i n s i n g l e p r e c i s i o n f o r

    t h e e x t e n d e d E x a m p l e 2 . W e t a k e m

    2

    = 1 a n d l i s t m

    1

    . T h e l e n g t h s o f t h e l i n k s a r e

    p r o p o r t i o n a l t o t h e i r m a s s e s . W e c h o o s e a

    k

    = b

    k

    = 0 ;

    1

    = 9 8 1 ;

    2

    = 9 8 1 ; a n d l i s t

    r e s u l t s f o r t h e t w o c a s e s : 1 H = 0 ; 0 ; 0 ; 0 ; 0 ; 1

    T

    a n d 2 H =

    1

    p

    2

    0 ; 0 ; 0 ; 0 ; 1 ; 1 I t

    i s e a s y t o s e e t h a t i n e x a c t a r i t h m e t i c D

    2

    = 1 r e g a r d l e s s o f m

    1

    . W e l i s t i t a n d t h e

    a c c e l e r a t i o n o f t h e t i p j o i n t . N o t e t h a t t h e m o d i e d A B M g i v e n i n 1 1 g i v e s b e t t e r

    r e s u l t s t h a n t h e C R B M . I n c a s e 2 , w h e r e t h e l o c a l c o o r d i n a t e s y s t e m s a r e n o t a l i g n e d

    w i t h t h e p r i n c i p a l d i r e c t i o n s o f m o t i o n , t h e r e s u l t s a r e a l s o b e t t e r t h a n t h e u s u a l A B M .

    T h e u s u a l A B M d o e s n o t i m p r o v e o v e r t h e C R B M i n t h e l a t t e r c a s e .

    2

    1 5

  • 8/7/2019 Dong hoc thuan

    16/17

    5 C o n c l u s i o n s

    W h i l e t h e r e h a s b e e n c o n s i d e r a b l e w o r k i n r e d u c i n g t h e c o m p u t a t i o n a l c o m p l e x i t y

    o f a l g o r i t h m s f o r c o m p u t i n g t h e f o r w a r d d y n a m i c s o f r o b o t m a n i p u l a t o r s , i t i s o n l y

    r e c e n t l y t h a t t h e n u m e r i c a l p r o p e r t i e s o f t h e s e a l g o r i t h m s h a v e b e e n c o n s i d e r e d c a r e -

    f u l l y . T h i s i s i m p o r t a n t s i n c e f o r w a r d d y n a m i c s a l g o r i t h m s a r e t y p i c a l l y u s e d w i t h

    a d a p t i v e s t e p - s i z e n u m e r i c a l i n t e g r a t o r s w h i c h c o u l d i n t e r p r e t t h e p o o r n u m e r i c a l

    p e r f o r m a n c e o f t h e f o r w a r d d y n a m i c s a s r e q u i r i n g m u c h s m a l l e r s t e p s i z e s . W e h a v e

    s h o w n h o w s o m e o f t h e s e n u m e r i c a l p r o p e r t i e s , i n p a r t i c u l a r f o r m u l a t i o n s t i n e s s , c a n

    b e a n a l y z e d .

    W e p r e s e n t e d a u n i e d f o r m u l a t i o n o f t w o i m p o r t a n t m e t h o d s f o r c o m p u t i n g f o r -

    w a r d d y n a m i c s , v i z . , t h e c o m p o s i t e r i g i d b o d y m e t h o d a n d t h e a r t i c u l a t e d b o d y

    m e t h o d , a s e l i m i n a t i o n m e t h o d s f o r s o l v i n g a l a r g e , s p a r s e , l i n e a r s y s t e m . T h e a r -

    t i c u l a t e d b o d y m e t h o d i s s h o w n t o o b t a i n i t s l i n e a r t i m e c o m p l e x i t y b y e x p l o i t i n g

    t h e s p a r s i t y o f t h e s y s t e m . W e a n a l y z e d t h e f o r m u l a t i o n s t i n e s s o f t h e s e a l g o r i t h m s

    a n d s h o w e d t h a t t h e p o o r b e h a v i o r o f t h e c o m p o s i t e r i g i d b o d y m e t h o d i n c e r t a i n

    i l l - c o n d i t i o n e d s y s t e m s i s d u e t o c e r t a i n c a n c e l l a t i o n e r r o r s , a n d t h i s p r o b l e m c a n b e

    a v o i d e d i n t h e a r t i c u l a t e d b o d y m e t h o d . O u r a n a l y s i s a l s o r e v e a l s p i t f a l l s f o r i m p l e -

    m e n t o r s o f t h e a r t i c u l a t e d b o d y m e t h o d a n d o t h e r f o r w a r d d y n a m i c s a l g o r i t h m s .

    A m o d i e d a r t i c u l a t e d b o d y m e t h o d w i t h s u p e r i o r s t a b i l i t y p r o p e r t i e s f o r g e n e r a l ,

    i l l - c o n d i t i o n e d m u l t i b o d y p r o b l e m s w a s p r o p o s e d a n d d e m o n s t r a t e d .

    A c k n o w l e d g e m e n t : W e w i s h t o t h a n k P r o f . J . V a r a h f o r f r u i t f u l d i s c u s s i o n s .

    R e f e r e n c e s

    A n g e l e s , J . , & M a , O . 1 9 8 8 . D y n a m i c S i m u l a t i o n o f n - A x i s S e r i a l R o b o t i c M a n i p u l a -

    t o r s U s i n g a N a t u r a l O r t h o g o n a l C o m p l e m e n 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 o t i c s

    R e s e a r c h , 7 5 , 3 2 4 7 .

    A r m s t r o n g , W . W . 1 9 7 9 . R e c u r s i v e S o l u t i o n t o t h e E q u a t i o n s o f M o t i o n o f a n n - l i n k

    M a n i p u l a t o r . P a g e s 1 3 4 3 1 3 4 6 o f : P r o c e e d i n g s o f t h e 5 t h W o r l d C o n g r e s s o n

    T h e o r y o f M a c h i n e s a n d M e c h a n i s m s , v o l . 2 .

    A s c h e r , U . , & L i n , P . 1 9 9 5 . S e q u e n t i a l R e g u l a r i z a t i o n M e t h o d s f o r N o n l i n e a r H i g h e r

    I n d e x D A E s . T e c h . r e p o r t 9 5 - 1 4 , C o m p . S c i , U B C , V a n c o u v e r .

    B a e , D . S . , & H a u g , E . J . 1 9 8 7 . A R e c u r s i v e F o r m u l a t i o n f o r C o n s t r a i n e d M e c h a n -

    i c a l S y s t e m D y n a m i c s : P a r t I . O p e n L o o p S y s t e m s . M e c h a n i c a l S t r u c t u r e s &

    M a c h i n e s , 1 5 3 , 3 5 9 3 8 2 .

    B o c k , G . , & v o n S c h w e r i n , R . 1 9 9 3 . A n i n v e r s e d y n a m i c s A d a m s - m e t h o d f o r c o n -

    s t r a i n e d m u l t i b o d y s y s t e m s . T e c h . r e p o r t 9 3 - 2 7 , I W R , U n i v . H e i d e l b e r g .

    1 6

  • 8/7/2019 Dong hoc thuan

    17/17

    B r a n d l , H . , J o h a n n i , R . , & O t t e r , M . 1 9 8 6 D e c e m b e r . A V e r y E c i e n t A l g o r i t h m

    f o r t h e S i m u l a t i o n o f R o b o t s a n d S i m i l a r M u l t i b o d y S y s t e m s W i t h o u t I n v e r -

    s i o n o f t h e M a s s M a t r i x . I n : P r o c e e d i n g s o f I F A C I F I P I M A C S I n t e r n a t i o n a l

    S y m p o s i u m o n t h e T h e o r y o f R o b o t s

    C l o u t i e r , B . , P a i , D . K . , & A s c h e r , U . M . 1 9 9 5 . T h e f o r m u l a t i o n s t i n e s s o f f o r w a r d

    d y n a m i c s a l g o r i t h m s a n d i m p l i c a t i o n s f o r r o b o t s i m u l a t i o n . I 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

    E l l i s , R . E . , & R i c k e r , S . L . 1 9 9 4 . T w o N u m e r i c a l I s s u e s i n S t i m u l a t i n g C o n s t r a i n e d

    R o b o t D y n a m i c s . I E E E T r a n s a c t i o n s o n S y s t e m s , M a n , a n d C y b e r n e t i c s , 2 4 1 ,

    1 9 2 7 .

    E l l i s , R . E . , I s m a e i l , O . M . , & C a r m i c h a e l , I . H . 1 9 9 2 . N u m e r i c a l S t a b i l i t y o f F o r w a r d -

    D y n a m i c s A l g o r i t h m s . I 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

    F e a t h e r s t o n e , R . 1 9 8 3 . T h e C a l c u l a t i o n o f R o b o t D y n a m i c s U s i n g A r t i c u l a t e d - B o d y

    I n e r t i a s . 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 o t i c s R e s e a r c h , 2 1 , 1 3 3 0 .

    F e a t h e r s t o n e , R . 1 9 8 7 . R o b o t D y n a m i c s A l g o r i t h m s . N o r w e l l , M A : K l u w e r A c a d e m i c

    P u b l i s h e r s .

    J a i n , A . 1 9 9 1 . U n i e d F o r m u l a t i o n o f D y n a m i c s f o r S e r i a l R i g i d M u l t i b o d y S y s t e m s .

    J o u r n a l o f G u i d a n c e , C o n t r o l , a n d D y n a m i c s , 1 4 3 , 5 3 1 5 4 2 .

    L u b i c h , C . , N o w a k , U . , P o h l e , U . , & E n g s t l e r , C h . 1 9 9 2 . M E X X N u m e r i c a l s o f t w a r e

    f o r t h e i n t e g r a t i o n o f c o n s t r a i n e d m e c h a n i c a l m u l t i b o d y s y s t e m s . T e c h . r e p o r t

    S C 9 2 - 1 2 , K o n r a d - Z u s e - Z e n t r u m f u r I n f o r m a t i o n s t e c h n i k , B e r l i n .

    R o d r i g u e z , G . 1 9 8 7 . K a l m a n F i l t e r i n g , S m o o t h i n g a n d R e c u r s i v e R o b o t A r m F o r w a r d

    a n d I n v e r s e D y n a m i c s . I E E E J o u r n a l o f R o b o t i c s a n d A u t o m a t i o n , 3 6 , 6 2 4 6 3 9 .

    R o d r i g u e z , G . , J a i n , A . , & K r e u t z - D e l g a d o , K . 1 9 9 1 . A S p a t i a l O p e r a t o r A l g e b r a

    f o r M a n i p u l a t o r M o d e l i n g a n d C o n t r o l . T h e 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 o t i c s

    R e s e a r c h , 1 0 4 , 3 7 1 3 8 1 .

    V e r e s h c h a g i n , A . F . 1 9 7 4 . C o m p u t e r S i m u l a t i o n o f t h e D y n a m i c s o f C o m p l i c a t e d

    M e c h a n i s m s o f R o b o t - M a n i p u l a t o r s . E n g i n e e r i n g C y b e r n e t i c s , 6 , 6 5 7 0 .

    W a l k e r , M . W . , & O r i n , D . E . 1 9 8 2 . E c i e n t D y n a m i c C o m p u t e r S i m u l a t i o n o f R o b o t i c

    M e c h a n i s m s . J o u r n a l o f D y n a m i c 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 , 1 0 4 ,

    2 0 5 2 1 1 .

    1 7