Bai tap dai robot cong nghiep co loi giai bai so 1 – Bài số 1: Cho robot 3 DOF như hình vẽ: Cho các – StuDocu

Bài số 1:

Cho robot 3 DOF như hình vẽ :Cho các thông số kỹ thuật của robot như sau : d1 = 1 m ; l1 = 1,5 m ; l2 = 2 m ; d3 = 0,5 m

θ1 = л/3 ; θ2 = л/

1. Tính toán động học thuận, động học ngược vị trí, ma trận jacoby
2. Yêu cầu tay robot hoạt động theo quỹ đạo là 1 đường thẳng trong không

gian từ A ( 0 ; 1 ; 0,2 ) đến B ( 0 ; 1 ; 0,4 ) trong thòi gian 4 s. Tính toán quỹ đạo cáckhớp

3. Cho mô hình động học khớp robot:

(s)

u(s) 1

i i

K

Ts

 



Thiết kế bộ điều khiển và tinh chỉnh vị trí dạng PI cho khớp 😡 0z 0 z 1x 1z 2z 3x 2x 3l 1 l 2 d 1d 3Cho quỹ đạo đặt các khớp như giám sát tại phần 2. Vẽ quỹ đạo tay robottrong khoảng trống thao tác

Bài làm

I. Tính toán động học thuận, động học ngược vị trí và ma trận Jacoby

1. Động học thuận vị trí

a. Thiết lập hệ toạ độ

Hệ tọa độ cố định và thắt chặt O 0 x 0 y 0 z 0 đặt tại trục khớp 1 Hệ tọa độ O 1 x 1 y 1 z 1 có gốc O 1 đặt tại trục khớp 2 Hệ tọa độ O 2 x 2 y 2 z 2 có gốc O 2 đặt tại trục khớp 3 Hệ tọa độ O 3 x 3 y 3 z 3 có gốc O 3 đặt tại bàn tay robot. Bốn trục z 0, z 1, z 2, z 3 cùng nằm trên mặt phẳng tờ giấy .

b. Bảng D-H

Căn cứ vào các thông số kỹ thuật và hệ tọa độ đã được thiết lập ta có bảng thông số kỹ thuậtDH của robot như sau :i θi di ai αi 1 θ 1 0 l 1 0 2 θ 2 0 l 2 0 3 0 d 1 – d 3 0 0Phép biến hóa tổng quát từ hệ tọa độ ( i-1 ) sang hệ tọa độ i :1 0 0 0 0 1i i i i i i i i i i i i i i i i i i ic s c s s a c T s c c c s a s s c d

     

     

















2. Động học ngược vị trí robot

Biết ma trận 0 T 3, tức là biết vị trí và hướng của khung tọa độ tay robot, cầnxác lập giá trị các biến khớp  1,  2 và d 312 0 3 12 1312 12 0 1 12 12 12 0 1 12 0 0 1 0 0 0 1 0 0 0 1x x x x y y y y z z z zn o a p c s l c l c T n o a p s c l s l s n o a p d d                             Theo giải pháp phân ly biến ta có :0 T 1  1 0 .. T 3  1 T T 22 3Với1 0 1 1

1 1 0 1

1 1 0 1

0 0 1 0

0 0 0 1

c s l c s c l s T













1 011

1 1 0

1 1 0 0

0 0 1 0

0 0 0 1

c s l sc T 













1 0 1 1 0 3 1 3

1 1 0

.. 1 1 0 0

0 0 1 0

0 0 0 1 0 0 0 1

x x x x y y y y z z z zc s l n o a pT T s c n o a p T n o a p

    

   

    

   

   

   

Mặt khác :22 1 3 1 22 3 22 1 3 1 3

2 2 0 2 1 0 0 0 2 2 0 2

.. 2202 0 1 0 02202

0 0 1 0 0 0 1 0 0 1

0 0 0 1 0 0 0 1 0 0 0 1

c s l c c s l c T T T s c l s s c l s d d d d

     

     

      

     

     

     

Cân bằng thông số 2 ma trận :

• Xét thành phần ở hàng 1,2 cột 3:

2 21

1. 1. 2

1. 1 2

xy xy

s p c p l s
c p s p l c l

  

   



Bình phương 2 vế và cộng lại ta được phương trình sau :pxy 2     p 2 l 12 l 22 22 l l c 1 2 2 2 122 2 12 2 2 12 22 2 2 12 22 2 1 2 1 2

2

2

atan2 ( 1 -, ) 22xyx y x yc p p l l ll p p l l p p l l l l l l



  

        

• Cũng theo trên ta có :

2 1 2 2 22 2 1 2 1 2 22

1 22

. 1. 1 2

. 1. 1 2 1 2 2

y y x x y x y x y y x x xys p l c p l p l s p s p c l s p p p c p s l c l c p l s p l p l c pp

  

    



    

  

 

  1 atan2( p l cy 2 2   p ly 1 p l s p l sx 2 2; y 2 2   p lx 1 p l cx 2 2)

• Xét thành phần ở hàng 3 cột 4 ma trận 0 T 3 : pzz      d 1 d 3 d 3 d 1 p

Vậy :2 2 12 22 2 2 12 22 2 1 2 1 2 1 2 1 2 2 1 2 31atan2 ( 1 – 22, )atan2 ( 2 2, 2 2 )x y x yy y x y x x zp p l l p p l l l l l l p l c p l p l s p l s p l p l c ddp





      

 



     

 





3. Ma trận Jacoby

Ta có :12 12 13

1 12

1 12

x y zp l c l c p l s l s p dd









0 0.

1 0.

0 0.

xt yt zt

 

 

 

Với thời hạn hoạt động trên quỹ đạo là t = 4 s, gián đoạn hóa quỹ đạothành 1000 điểm, chu kỳ luân hồi trích mẫu : T = 4/1000 = 4 ms0 0. 1 0 0 1000 0 0 .xi y i i zi

 

    



 

Mô phỏng bằng matlab:

for i = 0 : 1 : l1 = 1 ; l2 = 2 ; x = 0 – 0 i ; y = 1 + 0 i ; z = 0 + 0 * i ;theta2 = atan2 ( sqrt ( 1 – [ ( xx + yy-l1l1-l2l2 ) / ( 2 l1l2 ) ] ), ( xx + yy-l1l1 – l2l2 ) / ( 2 l1l2 ) ) ; theta1 = atan2 ( ( yl2cos ( theta2 ) + yl1-xl2 * sin ( theta2 ) ) ,( yl2sin ( theta2 ) + xl1 + xl2 * cos ( theta2 ) ) ) ; d3 = 1 – z ;% vẽ đồ thị : plot3 ( theta1, theta2, d3 ) ; hold on ; end grid on ;Kết quả :

III. Thiết kế bộ điều khiển vị trí dạng PI cho khớp

1. Cho mô hình động học khớp robot:

(s)

u(s) 1

i i

K

Ts

 



: 10 10

: 5 5 /

I 0( )

UV

rad s

Ts



 

 



 

→ Mô hình đối tượng người dùng :Creat subsystem

2. Thiết kế bộ điều khiển vị trí dạng PI cho khớp

Cho : tọa độ điểm bắt đầu : A ( 0 ; 1 ; 0,2 )tọa độ điểm cuối : B ( 0 ; 1 ; 0,4 )Sử dụng động học ngược vị trí :l1 = 1 ; l2 = 2 ; x = ; y = ; z = ; theta2 = atan2 ( sqrt ( 1 – [ ( xx + yy-l1l1-l2l2 ) / ( 2 l1l2 ) ] ), ( xx + yy-l1 * l1 -l2l2 ) / ( 2 l1l2 ) ) ; theta1 = atan2 ( ( yl2cos ( theta2 ) + yl1-xl2sin ( theta2 ) ) ,( yl2sin ( theta2 ) + xl1 + xl2 * cos ( theta2 ) ) ) ; d3 = 1 – z ;Ta xác lập được

• Góc ban đầu:

10 20 30

0.

2.

d 0.













• Góc đặt:

1 2 3

0.

1.

0.

d d

dd













Mô hình bộ tinh chỉnh và điều khiển :Thuật toán tinh chỉnh và điều khiển :0

. ( )

t

Udk  K e K ep I   d

Trong đó: e q  d q : sai số vị trí của hệ thống

Mô hình simulink bộ điều khiển và tinh chỉnh :11 22 13

10 0

10 0

15 0

PI PI PI

KK

KK

KK

 

 



 

Kết quả mô phỏng :

• Qũy đạo khớp 1:

  Xem thêm: Giải hệ phương trình bằng phương pháp thế và phương pháp cộng đại số

• Quỹ đạo khớp 2 :
• Quỹ đạo khớp 3 :
• Quỹ đạo tay robot trong khong gian thao tác :

Source: https://vietsofa.vn
Category : Góc học tập