Bài số 1: Cho robot 3 DOF như hình vẽ: Cho các thông số của robot như sau: d1 = 1m; l1 = 1,5 m ; l2 = 2m ; d3 = 0,5 m θ1 = л/3 ; θ2 = л/6 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 chuyển động theo quỹ đạo là 1 đường thẳng trong không gian từ A(0.8;1.2;0,2) đến B(0;1;0,4) trong thòi gian 4s. Tính toán quỹ đạo các khớ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ị trí dạng PI cho khớp: x 0 z 0 z 1 x 1 z 2 z 3 x 2 x 3 l 1 l 2 d 1 d 3 Cho quỹ đạo đặt các khớp như tính toán tại phần 2. Vẽ quỹ đạo tay robot trong không gian làm việ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ệ toạ độ cố định O 0 x 0 y 0 z 0 đặt tại trục khớp 1 Hệ toạ độ O 1 x 1 y 1 z 1 có gốc O 1 đặt tại trục khớp 2 Hệ toạ độ O 2 x 2 y 2 z 2 có gốc O 2 đặt tại trục khớp 3 Hệ toạ độ 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ố và hệ toạ độ đã được thiết lập ta có bảng thông số DH của robot như sau: i θ i d i a i α i 1 θ 1 0 l 1 0 2 θ 2 0 l 2 0 3 0 d 1 -d 3 0 0 Phép biến đổi tổng quát từ hệ tọa độ (i-1) sang hệ tọa độ i: 1 0 0 0 0 1 i i i i i i i i i i i i i i i i i i i c s c s s a c s c c c s a s T s c d                        Áp dụng vào robot 3DOF ta có: 1 1 0 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         2 2 1 2 2 2 0 2 2 2 0 2 0 0 1 0 0 0 0 1 c s l c s c l s T         2 3 13 1 0 0 0 0 1 0 0 0 0 1 0 0 0 1 T dd         Phương trình động học thuận được xác định như sau: 0 T 3 = 0 T 1 1 T 2 2 T 3 12 12 0 3 13 1 1 0 1 2 2 0 2 1 0 0 0 1 1 0 1 2 2 0 2 0 1 0 0 0 0 1 0 0 0 1 0 0 0 1 0 0 0 1 0 0 0 1 0 0 0 1 c s l c c s l c s c l s s c l s T dd                                        12 12 0 3 13 12 12 0 1 12 12 12 0 1 12 0 0 1 0 0 0 1 c s l c l c s c l s l s T dd           Trong đó ta ký hiệu: 11 22 12 12 1 ; 1 2 ; 2 12 ( ) 12 ( ) c c s s c c s s cc ss                Với: 1 2 1 2 1 3 ; ; 1,5 ; 2 ; 1 ; 0,5 36 l m l m d m d m         0 3 0 0,75 1 0 0 2 0,75 3 0 0 1 0,5 0 0 0 1 10 T            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 toạ độ tay robot, cần xác định giá trị các biến khớp  1 ,  2 và d 3 12 12 0 3 13 12 12 0 1 12 12 12 0 1 12 0 0 1 0 0 0 1 0 0 0 1 x x x x y y y y z z z z n o a p c s l c l c n o a p s c l s l s T n o a p d d                             Theo phương pháp phân ly biến ta có: 0 1 0 1 2 1 3 2 3 T T T T   Với 1 1 0 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 01 1 1 1 0 1 1 0 0 0 0 1 0 0 0 0 1 c s l sc T           1 0 1 0 1 1 3 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 z c s l n o a p s c n o a p T T T n o a p                               Mặt khác: 22 22 1 1 2 3 2 3 1 3 1 3 2 2 0 2 1 0 0 0 2 2 0 2 2 2 0 2 0 1 0 0 2 2 0 2 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 s c l s s c l s T T T d d d d                                          Cân bằng hệ 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: 2 2 2 2 1 2 1 2 22 xy p p l l ll c    2 2 2 2 12 12 2 2 2 2 2 2 2 2 1 2 1 2 2 1 2 1 2 2 2 atan2( 1- , ) 22 xy x y x y p p l l c ll p p l l p p l l l l ll               - Cũng theo trên ta có : 2 1 2 22 2 2 1 2 1 2 22 22 1 . 1 . 1 2 . 1 . 1 2 2 2 1 y y x x y x y x y y x x xy p l c p l p l s s p s p c l s p p p c p s l c l p l s p l p l c c pp                         1 2 1 2 2 1 2 atan2( 2 2; 2 2) y y x y x x p l c p l p l s p l s p l p l c        - Xét thành phần ở hàng 3 cột 4 ma trận 0 T 3 : 1 3 3 1zz p d d d d p     Vậy: 2 2 2 2 2 2 2 2 1 2 1 2 2 1 2 1 2 1 2 1 2 2 1 2 31 atan2( 1- , ) 22 atan2( 2 2, 2 2) x y x y y y x y x x z p 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 z p l c l c p l s l s dd p              12 1 2 2 3 1 12 12 0 x x x p l s l s p ls p d                         12 1 2 2 3 1 12 12 0 y y y p l c l c p lc p d                       3 1 3 2 3 3 0 0 1 d d d d                     3 . . 1 2 . . x y z p pJ d p              1 2 2 1 2 2 1 12 12 0 1 12 12 0 0 0 1 l s l s l s J l c l c l c             II. Tính toán quỹ đạo các khớp Khi cho tọa độ vị trí tay robot trong không gian làm việc (x,y,z) ta có thể xác định được các biến khớp thông qua động học ngược vị trí: 2 2 2 2 2 2 2 2 1 2 1 2 2 1 2 1 2 1 2 1 2 2 1 2 31 atan2( 1- , ) 22 atan2( 2 2, 2 2) x y x y y y x y x x z p 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                         Yêu cầu tay robot chuyển động từ điểm A( 0,8; 1,2; 0,2 ) đến điểm B( 0; 1; 0,4) Xây dựng được quỹ đạo đường thẳng đi qua hai điểm trên trong không gian: 0.8 1.2 0.2 0 0.8 1 1.2 0.4 0.2 0.8 1.2 0.2 0.8 0.2 0.2 x y z t x y z t                  Hay : 0.8 0.8 1.2 0.2 0.2 0.2 xt yt zt         Với thời gian chuyển động trên quỹ đạo là t = 4s, gián đoạn hóa quỹ đạo thành 1000 điểm, chu kỳ trích mẫu : T = 4/1000 = 4ms 0.8 0.0032 1.2 0.0008 0 1000 0.2 0.0008 xi y i i zi             Mô phỏng bằng matlab: for i=0:1:1000 l1 = 1.5; l2 = 2; x = 0.8 - 0.0032*i; y = 1+0.001*i; z = 0.2+0.0002*i; theta2=atan2(sqrt(1-[(x*x+y*y-l1*l1-l2*l2)/(2*l1*l2)]),(x*x+y*y-l1*l1- l2*l2)/(2*l1*l2)); theta1=atan2((y*l2*cos(theta2)+y*l1-x*l2*sin(theta2)), (y*l2*sin(theta2)+x*l1+x*l2*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 / 0.01( ) I UV rad s Ts          → Mô hình đối tượng: Mô hình simulink: Creat subsystem Khối vị trí ban đầu: Creat subsystem 2. Thiết kế bộ điều khiển vị trí dạng PI cho khớp Cho: tọa độ điểm ban đầu: A(0.8;1.2;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.5; l2 = 2; x = ; y =; z =; theta2=atan2(sqrt(1-[(x*x+y*y-l1*l1-l2*l2)/(2*l1*l2)]),(x*x+y*y-l1*l1- l2*l2)/(2*l1*l2)); theta1=atan2((y*l2*cos(theta2)+y*l1-x*l2*sin(theta2)), (y*l2*sin(theta2)+x*l1+x*l2*cos(theta2))); d3 = 1- z; Ta xác định được - Góc ban đầu: 10 20 30 0.28 2.06 0.8 d               - Góc đặt: 1 2 3 0.01 1.9 0.6 d d d d              [...]...Mô hình bộ điều khiển: Thuật toán điều khiển: t U dk  K p e  K I  e( )d 0 Trong đó: e  qd  q : sai số vị trí của hệ thống Mô hình simulink bộ điều khiển: Creat subsystem Tổng hợp bộ điều khiển: Xác định các tham số của bộ điều khiển sao cho không có quá điều chỉnh: Ta chọn:  K P1  10 K I 1  0   K P 2  10 K I 2  0  K  15... khiển sao cho không có quá điều chỉnh: Ta chọn:  K P1  10 K I 1  0   K P 2  10 K I 2  0  K  15 K  0 I3  P1 Kết quả mô phỏng: - Qũy đạo khớp 1: - Qũy đạo khớp 2: - Qũy đạo khớp 3: - Quỹ đạo tay robot trong khong gian làm việc: . Mô hình bộ điều khiển: Thuật toán điều khiển: 0 . ( ) t dk p I U K e K e d    Trong đó: d e q q : sai số vị trí của hệ thống Mô hình simulink bộ điều khiển: Creat. bộ điều khiển: Creat subsystem Tổng hợp bộ điều khiển: Xác định các tham số của bộ điều khiển sao cho không có quá điều chỉnh: Ta chọn: 11 22 13 10 0 10 0 15 0 PI PI PI KK KK KK         . 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 / 0.01( ) I UV rad