Dong hoc thuan
-
Upload
congdinh1381 -
Category
Documents
-
view
234 -
download
0
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