Transcript Robot3

‫فصل سوم‬
‫سينماتيك مستقيم‬
‫‪1‬‬
‫محتواي فصل‬
‫‪ ‬تعريف مجموعه فازي‬
‫‪ ‬تابع عضويت‬
‫‪ ‬نمايش مجموعه هاي فازي‬
‫‪ ‬برش آلفا‬
‫‪‬متغيرهاي زباني‬
‫‪‬ساخت مجموعه هاي فازي‬
‫‪2‬‬
‫مقدمه‬
‫يادآوري ميکنيم که بازوي يک روبات را ميتوان بصورت مجموعهاي از لينکها که‬
‫توسط جوينتهاي دوراني يا خطي بهم متصل شدهاند‪ ،‬مدل کرد‪ .‬هدف اين فصل بدست‬
‫آوردن روش ي براي نسبت دادن دستگاه مختصات مستقل به هر لينک است‪ .‬پس از اين‬
‫مرحله‪ ،‬معادلة كلي بازو ‪ General Arm Equation‬بدست ميآيد؛ که سينماتيک‬
‫حرکت لينکها را بدست ميدهد‪ .‬ابتدا به معرفي پارامترهاي مربوطه ميپردازيم‪.‬‬
‫‪3‬‬
‫پارامترهاي سينماتيک‬
‫دو لينک مجاور‪ ،‬بوسيله جوينت دوراني يا خطي بهم متصل شدهاند‪ .‬موقعيت و جهت‬
‫نسبي اين دو لينک‪ ،‬بوسيله دو پارامتر جوينت شناسايي ميشود‪.‬‬
‫‪4‬‬
‫پارامترهاي سينماتيک‬
‫‪5‬‬
‫پارامترهاي سينماتيک‬
‫‪6‬‬
‫پارامترهاي سينماتيک‬
‫همانگونه که يک جوينت‪ ،‬دو لينک مجاور را به هم متصل ميکند‪ ،‬ميان دو جوينت متوالي‬
‫نيز يک لينک قرار دارد‪ .‬موقعيت و جهت نسبي محورهاي دو جوينت متوالي را ميتوان‬
‫بوسيله دو پارامتر لينک بصورت شکل زير توصيف کرد‪:‬‬
‫‪7‬‬
‫پارامترهاي سينماتيک‬
‫‪8‬‬
‫پارامترهاي سينماتيک‬
‫‪9‬‬
‫پارامترهاي سينماتيک‬
‫‪10‬‬
‫بردارهاي سهگانة مچ‬
‫‪11‬‬
‫بردارهاي سهگانة مچ‬
‫‪12‬‬
‫بردارهاي سهگانة مچ‬
‫‪13‬‬
)Denavit-Hartenberg( D-H ‫نمايش‬
14
)Denavit-Hartenberg( D-H ‫نمايش‬
15
)Denavit-Hartenberg( D-H ‫نمايش‬
16
)Denavit-Hartenberg( D-H ‫نمايش‬
17
)Denavit-Hartenberg( D-H ‫نمايش‬
18
‫نمايش ‪)Denavit-Hartenberg( D-H‬‬
‫الگوريتم فوق‪ ،‬بنام الگوريتم ‪D-H‬معروف ميباشد‪ .‬توجه داشته باشيد که اين‬
‫الگوريتم‪ ،‬در اصل از دو بخش تشکيل شده است‪ .‬قسمت اول (مراحل ‪ 1‬تا ‪)7‬‬
‫دستگاههاي مختصات راستگردي را به انتهاي هر لينک نسبت ميدهد و قسمت دوم‬
‫(مراحل ‪ 8‬تا ‪ )13‬مقادير پارامترهاي سينماتيک را محاسبه ميكند‪.‬‬
‫‪19‬‬
‫نمايش ‪)Denavit-Hartenberg( D-H‬‬
‫مثال ‪ :1‬به عنوان يک نمونه از اجراي الگوريتم ‪ ، D-H‬روبات ‪ 5‬مفصلي ‪ Alpha II‬که در‬
‫شکل زير نمايش داده شده است را درنظر بگيريد‪.‬‬
‫‪20‬‬
)Denavit-Hartenberg( D-H ‫نمايش‬
4
1
Elbow
3
2
5
21
)Denavit-Hartenberg( D-H ‫نمايش‬
z
z
z
2
1
z
z
3
y
0
4
0
x
Base
0
22
)Denavit-Hartenberg( D-H ‫نمايش‬
z
z
0
z
1
y
0
z
z
x
Base
2
0
3
4
23
)Denavit-Hartenberg( D-H ‫نمايش‬
z
L2
2
z
3
L3
L4
z
1
y
0
z
4
L1
z
0
L0
x
Base
0
24
)Denavit-Hartenberg( D-H ‫نمايش‬
z
L2
2
x
2
z
L3
L4
z
3
x
3
x
4
1
z
4
L1
z
0
y
x
1
x
0
0
L0
Base
25
)Denavit-Hartenberg( D-H ‫نمايش‬
z
x
2
2
z
L2
3
L3
L4
z
y
1
L1
y
z
1
0
y
x
1
x
0
y
3
3
x
4
4
z
4
0
L0
Base
y
2
x
26
)Denavit-Hartenberg( D-H ‫نمايش‬
z
x
2
2
z
L2
3
L3
L4
z
y
1
L1
y
z
1
0
y
x
1
x
0
y
3
3
x
4
4
z
4
0
L0
Base
y
2
x
27
)Denavit-Hartenberg( D-H ‫نمايش‬
z
x
2
2
z
L2
3
L3
L4
z
y
1
y
2
y
L1
y
z
1
0
x
y
Base
y
x
4
4
z
5
4
L5
x
x
3
1
0
L0
3
x
0
5
z
5
28
)Denavit-Hartenberg( D-H ‫نمايش‬
z
x
2
2
z
3
bL4
bL32
3
L4
z
y
1
y
2
y
b 2 bL11
y
z
1
0
x
y
Base
y
x
4
4
z
5
4
L
b 55
x
x
3
1
0
L0
3
x
5
z
5
0
29
)Denavit-Hartenberg( D-H ‫نمايش‬
3
z
2
z
b 2 bL11
y
z
1
0
1
y
x
2
z
bL4
bL32
3
L4
y
2
y
y
3
x
3
x
4
4
z
5
4
5
L
b 55
x
x
3
1
0
L0
Base
2
y
1
x
4
5
z
5
0
30
)Denavit-Hartenberg( D-H ‫نمايش‬
d4  0
d3  0
z
x
2
2
z
bL4
bL32
d2  0
3
3
L4
z
y
1
y
2
y
b 2 bL11
y
z
1
0
x
y
z
y
5
x
4
d5
4
L5
b5
d1
x
x
Base
4
3
1
0
L0
3
x
5
z
5
0
31
)Denavit-Hartenberg( D-H ‫نمايش‬
z
ax 32
2
z
bL4
bL32
a1  0
3
3
L4
z
y
1
b 2 bL11
y
z
x
y
y
4
z
4
3
x
4
a4  0
a5  0
y
5
L5
b5
x
x
3
1
0
L0
Base
2
a2
1
0
y
x
5
z
5
0
32
)Denavit-Hartenberg( D-H ‫نمايش‬
4  90
1   9 0
z
x
2
2
z
L2
3
L3
L4
3  0
2 0
z
y
1
y
2
y
L1
y
z
1
0
x
y
Base
y
x
4
4
z
5
4
L5
x
x
3
1
0
L0
3
x
5  0
5
z
5
0
33
)Denavit-Hartenberg( D-H ‫نمايش‬
34
‫معادله بازو ( ‪)Arm Equation‬‬
‫وقتي که براي هر لينک با استفاده از الگوريتم ‪ ، D-H‬يک دستگاه مختصات مستقل‬
‫نسبت داده شود‪ ،‬ميتوان با بکارگيري يک ماتريس تبديل مختصات همگن‪ ،‬مختصات‬
‫هرنقطه را از دستگاه ‪ k‬به دستگاه ‪ k-1‬تبديل کرد‪ .‬با ضرب کردن چند ماتريس تبديل‬
‫مختصات همگن در يکديگر‪ ،‬يک ماتريس تبديل مختصات ترکيبي بدست ميآيد که‬
‫مختصات ابزار را به مختصات پايه تصوير ميکند‪ .‬اين ماتريس تبديل مختصات همگن را‬
‫«ماتريس بازو» گويند‪.‬‬
‫‪35‬‬
‫معادله بازو ( ‪)Arm Equation‬‬
‫براي ساختن ماتريس تبديل مختصات همگن که مختصات دستگاه ‪ k‬را به دستگاه ‪k-1‬‬
‫تبديل کند‪ ،‬چهار مرحله وجود دارد‪ .‬دستگاه مختصات ‪ k-1‬را بايستي حول دستگاه‬
‫مختصات ‪k‬طوري دوران و انتقال داد تا دو دستگاه بر هم منطبق شوند‪ .‬پياده سازي‬
‫مراحل ‪ 8‬تا ‪ 12‬از الگوريتم ‪ D-H‬به چهار عمليات اساس ي منتهي ميگردد که در جدول‬
‫زير خالصه شده است‪.‬‬
‫‪36‬‬
 k 1
k
z
z
Joint k
dk
ak
k 1
L k 1
y
k
k

Lk
k
y
k
x
k
k 1
x
k 1
37
 k 1
k
z
z
Joint k
dk
ak
k 1
L k 1
y
k
k

Lk
k
y
k
x
k
k 1
x
k 1
38
 k 1
k
z
z
Joint k
dk
ak
k 1
L k 1
y
k
k

Lk
k
y
k
x
k
k 1
x
k 1
39
)Arm Equation ( ‫معادله بازو‬
40
‫معادله بازو ( ‪)Arm Equation‬‬
‫بطور کلي ‪ T‬نشان دهنده ماتريس تبديل مختصات همگن است و انديس بال‬
‫نمايش دهندة دستگاه مبدأ و انديس پايين نشان دهندة دستگاه مقصد است‪.‬‬
‫با استفاده از معادله بال و تعريف تبديل ‪ Screw‬نتيجه زير ميرسيم‪.‬‬
‫‪41‬‬
)Arm Equation ( ‫معادله بازو‬
q 
k 1
k
T k 1






T
k
k 1
q 
k
C k
C k S k
S k S k
a k C k
S k
C k C k
 S k C k
a k S k
0
S k
C k
dk
0
0
0
1






42
)Arm Equation ( ‫معادله بازو‬
43
‫معادله بازو ( ‪)Arm Equation‬‬
‫‪n‬‬
‫)‪T0 (q‬‬
‫‪‬‬
‫‪n‬‬
‫) ‪.T n 1 ( q n‬‬
‫‪..‬‬
‫‪1‬‬
‫‪2‬‬
‫) ‪T 0 ( q 1 )T1 ( q 2‬‬
‫‪(q ) ‬‬
‫‪tool‬‬
‫‪Tb a s e‬‬
‫جهت محاسبة ماتريس بازو‪ ،‬ميتواند ماتريس تبديل را در مچ روبات به دو قسمت تقسيم كرد‪ .‬يكي‬
‫تبديل از نوك ابزار به مچ و ديگري از مچ به پاية روبات‪ .‬اولي قابل استفاده براي جهتگيريهاي‬
‫مختلف ابزار و دومي قابل استفاده براي موقعيتهاي متفاوت ابزار ميباشد ‪.‬‬
‫‪3‬‬
‫) ‪T0 (q‬‬
‫‪n‬‬
‫‪‬‬
‫‪1‬‬
‫‪2‬‬
‫‪3‬‬
‫) ‪T 0 ( q 1 ) T1 ( q 2 )T 2 ( q 3‬‬
‫‪n‬‬
‫‪5‬‬
‫‪4‬‬
‫‪(q ) ‬‬
‫‪wrist‬‬
‫‪Tb a s e‬‬
‫‪tool‬‬
‫) ‪T w r i s t ( q )  T 3 ( q 4 )T 4 ( q 5 ) . . .T n 1 ( q n )  T 3 ( q‬‬
‫‪44‬‬
)Arm Equation ( ‫معادله بازو‬
45
)Arm Equation ( ‫معادله بازو‬
46
)Arm Equation ( ‫معادله بازو‬
47
)Arm Equation ( ‫معادله بازو‬
: ‫ درجة روبرو خواهيم داشت‬5 ‫مثال براي روبات‬
k
T k 1
wrist
Tb a s e

C1

S1
1 2 3

T 0 T1 T 2 
 0

 0






0
 S1
0
C1
1
0
0
0
 C 1C 2 3

S 1C 2 3


 S 2 3

 0
C k
C k S k
S k S k
a k C k
S k
C k C k
 S k C k
a k S k
0
S k
C k
dk
0
0
0
1
0  C 2

0
S
 2
d1   0

1  0
C 1S 2 3
S1
 S 1S 2 3
C1
C 2 3
0
0
0
S 2
0
C2
0
0
1
0
0
a 2C 2  C 3

a2S 2 S3

0  0

1  0
C 1 ( a 2 C 2  a 3C 2 3 ) 

S 1 ( a 2 C 2  a 3C 2 3 )

d1  a 2S 2  a3S 2 3 

1

S3
0
C3
0
0
1
0
0






a 3C 3 

a3S 3

0 

1 
48
)Arm Equation ( ‫معادله بازو‬
: ‫و همينطور براي تبديل ابزار تا مچ داريم‬
tool
Tw r i s t

4 5
T3 T4
C 4

S4


 0

 0
0
S 4
0
C4
1
0
0
0
C 4C 5

S C
 4 5
 S5

 0
0  C 5

0 S5

0  0

1  0
C 4 S 4
S 4
S 4S5
C4
C 5
0
0
0
S5
0
C5
0
0
1
0
0
0 

0

d5

1 
d 5S 4 

d 5C 4

0 

1 
49
)Arm Equation ( ‫معادله بازو‬
‫نهايتا ماتريس بازو از ضرب دو تبديل فوق حاصل خواهد شده كه بصورت زير‬
.‫ميباشد‬
 C 1C 2 3 4 C 5  S 1 S 5

S 1C 2 3 4 C 5  C 1 S 5
tool

Tb a s e (q ) 

 S 2 3 4C 5

0

 C 1C 2 3 4 S 5  S 1C 5
 S 2 3 4C1
 S 1C 2 3 4 S 5  C 1C 5
S 2 3 4S1
S 2 3 4S5
C 2 3 4
0
0
C 1 (a 2 C 2  a 3C 2 3  d 5 S 2 3 4 ) 

S 1 ( a 2 C 2  a 3C 2 3  d 5 S 2 3 4 )

d 1  a 2 S 2  a 3 S 2 3  d 5C 2 3 4 ) 

1

50
‫معادلت بازو براي چند روبات صنعتي‬
‫روبات چهار درجه اسكارا‬
‫‪51‬‬
‫معادلت بازو براي چند روبات صنعتي‬
‫دياگرام ‪D-H‬‬
‫‪52‬‬
‫معادلت بازو براي چند روبات صنعتي‬
‫پارامترهاي سينماتيك‬
‫‪53‬‬
‫معادلت بازو براي چند روبات صنعتي‬
‫پارامترهاي حال براي محاسبة معادلة بازو‪ ،‬از آنجا كه در اين مثال فقط چهار محور وجود‬
‫دارد‪ ،‬نيازي به تقسيم ماتريس بازو به دو بخش ابزار تا مچ و مچ تا پايه نيست و ميتوان‬
‫مستقيما ماتريس بازو را به صورت زير بدست آورد‪.‬‬
‫‪a 2C 2 ‬‬
‫‪‬‬
‫‪a2S 2‬‬
‫‪‬‬
‫‪0 ‬‬
‫‪‬‬
‫‪1 ‬‬
‫‪0 ‬‬
‫‪‬‬
‫‪0‬‬
‫‪‬‬
‫‪d4‬‬
‫‪‬‬
‫‪1 ‬‬
‫‪54‬‬
‫‪0‬‬
‫‪ S2‬‬
‫‪0‬‬
‫‪C2‬‬
‫‪1‬‬
‫‪0‬‬
‫‪0‬‬
‫‪0‬‬
‫‪C 2‬‬
‫‪‬‬
‫‪S2‬‬
‫‪‬‬
‫‪‬‬
‫‪ 0‬‬
‫‪‬‬
‫‪ 0‬‬
‫‪0‬‬
‫‪S 4‬‬
‫‪0‬‬
‫‪C4‬‬
‫‪1‬‬
‫‪0‬‬
‫‪0‬‬
‫‪0‬‬
‫‪C 4‬‬
‫‪‬‬
‫‪S4‬‬
‫‪‬‬
‫‪‬‬
‫‪ 0‬‬
‫‪‬‬
‫‪ 0‬‬
‫‪2‬‬
‫‪T1‬‬
‫‪4‬‬
‫‪T3‬‬
‫‪a 1C 1 ‬‬
‫‪‬‬
‫‪a1 S 1‬‬
‫‪‬‬
‫‪d1 ‬‬
‫‪‬‬
‫‪1 ‬‬
‫‪0‬‬
‫‪S1‬‬
‫‪0‬‬
‫‪ C1‬‬
‫‪1‬‬
‫‪0‬‬
‫‪0‬‬
‫‪0‬‬
‫‪0‬‬
‫‪‬‬
‫‪0‬‬
‫‪‬‬
‫‪q3 ‬‬
‫‪‬‬
‫‪1‬‬
‫‪0‬‬
‫‪0‬‬
‫‪0‬‬
‫‪1‬‬
‫‪1‬‬
‫‪0‬‬
‫‪0‬‬
‫‪0‬‬
‫‪C1‬‬
‫‪‬‬
‫‪S1‬‬
‫‪1‬‬
‫‪T0  ‬‬
‫‪0‬‬
‫‪‬‬
‫‪0‬‬
‫‪1‬‬
‫‪‬‬
‫‪0‬‬
‫‪‬‬
‫‪‬‬
‫‪0‬‬
‫‪‬‬
‫‪0‬‬
‫‪3‬‬
‫‪2‬‬
‫‪T‬‬
‫معادلت بازو براي چند روبات صنعتي‬
‫پارامترهاي با ضرب چهار ماتريس تبديل فوق ماتريس تبديل نهايي بصورت زير خواهد شد‪.‬‬
‫‪a 1C 1  a 2 C 1 2 ‬‬
‫‪‬‬
‫‪a 1 S 1  a 2 S 1 2‬‬
‫‪‬‬
‫‪d1  q3  d 4 ‬‬
‫‪‬‬
‫‪1‬‬
‫‪‬‬
‫‪55‬‬
‫‪0‬‬
‫‪S 1 2  4‬‬
‫‪0‬‬
‫‪ C 1 2  4‬‬
‫‪1‬‬
‫‪S 2 3 4S5‬‬
‫‪0‬‬
‫‪0‬‬
‫‪ C 1 2  4‬‬
‫‪‬‬
‫‪S 1 2  4‬‬
‫‪‬‬
‫‪‬‬
‫‪ 0‬‬
‫‪‬‬
‫‪ 0‬‬
‫‪1 2 3 4‬‬
‫‪T 0 T1 T 2 T 3‬‬
‫‪(q ) ‬‬
‫‪tool‬‬
‫‪Tb a s e‬‬
‫معادلت بازو براي چند روبات صنعتي‬
‫‪56‬‬
‫پايان فصل سوم‬
‫‪57‬‬