Bài tập lớn môn Robotics

Bài tập lớn môn robotics đại học Bách khoa Hà nội. Cấu trúc là robot ba bậc tự do toàn khớp quay. Có tính toán đầy đủ mô hình động học, động lực học, tĩnh học, thiết kế bộ điều khiển. Mô phỏng bằng maple, matlab.

MỤC LỤC

I Xây dựng cấu trúc, thiết lập hệ phương trình động học của robot……… 2

a Thiết lập hệ tọa độ theo quy tắc Denavit Hartenberg……… 2

b Thiết lập hệ phương trình động học của robot……… 2

II Giải bài toán động học……… …….3

2.1 Giải bài toán động học thuận……… ……….… 3

a, xác định vận tốc điểm tác động cuối và vận tốc góc khâu thao tác …… 3

b, xây dựng quy luật chuyển động từng khâu từ đó vẽ quỹ đạo điểm E, vận tốc điểm E và vận tốc góc ……… 4

2.2 Giải bài toán động học ngược……… 6

a, giải bằng phương pháp giải tích……… 6

b, xây dựng quy luật chuyển động khâu thao tác E và giải động học ngược bằng phương pháp số Newton-Raphson……… …7

III Tính toán tĩnh học……… 11

3.1 Cho lực tác động vào khâu thao tác tìm lực dẫn động……….11

3.2 Cho lực ( mô men ) dẫn động tính lực ( mô men ) khâu thao tác tác dụng lên đối tượng……… 14

IV Tính toán động lực học……… 16

4.1 Xây dựng cấu trúc động lực học và các thành phần cần thiết để viết phương trình động lực học……… 16

4.2, thiết lập phương trình Lagrang của robot……… 19

V Luật điều khiển………22

1 Hệ thống điều khiển trong không gian khớp………22

1.1 Hệ thống điều khiển phản hồi……….22

1.2 Hệ thống điều khiển momen tính toán………23

2 Hệ thống điều khiển trong không gian làm việc……… 25

TÀI LIỆU THAM KHẢO ……… 27

I Xây dựng cấu trúc, thiết lập hệ phương trình động học của robot

Ta thiết lập phương trình động học theo phương pháp ma trận Denavit Hartenberg

a Thiết lập hệ tọa độ theo quy tắc Denavit Hartenberg

Khâu 0: đế ta chọn hệ tọa độ XoYoZo có trục Zo chọn

trùng với khớp 1, trục Xo chọn tùy ý sao cho phù hợp nhất

như hình vẽ , trục Yo chọn theo quay tắc tam diện thuận

Khâu 1: ta chọn hệ tọa độ X1Y1Z1 có trục Z1 trùng với

khớp 2, trục X1 ta chọn theo hướng Zo x Z1, trục Y1 chọn

theo quay tắc tam diện thuận

Khâu 2: ta chọn hệ tọa độ X2Y2Z2 có trục Z2 trùng với

khớp 3,X2 chọn theo đường vuông góc chung Z1 và Z2, Y2

chọn theo quy tắc tam diện thuận

Khâu 3: Ta chọn hẹ X3Y3Z3 có trục Z3 song song Z2 X2

chọn theo đường vuông góc chung Z2 và Z3, Y3 chọn theo

quy tắn tam diện thuận

Hình 2.1 Hệ tọa độ Robot RRR

b Thiết lập hệ phương trình động học của robot

Từ việc chọn hệ tọa độ ta có bảng DH sau:

cos( ) sin( ) 0 os( )

sin( ) os( ) 0 sin( )

2

A

 =0A1.1

2

0

0 0 0 1

C C C S S a C C S C S S C a S C S C d a S                 0 3 ( )q A  =0A1.1 2 A .2 3 A = 1 23 1 23 1 1 3 23 2 2 1 23 1 23 1 1 3 23 2 2 23 23 1 2 2 3 23 ( )

( )

0

0 0 0 1

C C C S S C a C a C S C S S C S a C a C S C d a S a S                    Với C1=cos(q1), S1=sin(q1), C2=cos(q2), S2=sin(q2), S23=sin(q2+q3), C23=cos(q2+q3), q=[q1, q2, q3]T Mặt khác ta lại mô tả được hướng và vị trí qua ma trận sau thông qua vector p=[xE, yE, zE, α, β, η]T α, β, η là 3 góc Cardan 0 3 ( )p A = os( ) os( ) os( )sin( ) sin( )

sin( )sin( ) os( ) os( )sin( ) sin( )sin( )sin( ) os( ) os( E c c c x c c c c                    ) sin( ) os( )

os( )sin( ) os( ) sin( )sin( ) os( )sin( )sin( ) sin( ) os( ) os( ) os( )

0

E E c y c c c c c c z                   0 0 1

So sánh hai ma trận

0

3 ( )q

A

,

0

3 ( )p

A ta thiết lập được hệ phương trình động học sau:

( )[1,1] ( )

E E E

[1,1] os( ) os( ) os( ) os( ) 0 ( )[2, 2] ( )[2, 2] sin( )sin( )sin( ) os( ) os( ) sin( )sin( ) 0 ( )[3,3] ( )[3,3] os( ) os( ) 0























II Giải bài toán động học

2.1 Giải bài toán động học thuận

Để tính toán ta chọn d1=1 (m), a2=0.5 (m), a3=0.4 (m)

a, xác định vận tốc điểm tác động cuối và vận tốc góc khâu thao tác

Từ phương trình động ở trên ta rút ra ( với q=[q1, q2, q3]T )

0

E

r =

os( ) os( ) os( ) sin( ) os( ) os( ) sin( ) sin( )

E

E

E

c q a c q q a c q x

 

 

A ta rút trân cosin chỉ hướng

Hình 2.3 Đồ thị vận tốc điểm E theo t Hình 2.4 Đồ thị vận tốc góc khâu 3 theo

t

Để tính toán ta sử dụng hai thủ tuc đó là hai hàm con viết bằng maple như sau:

Hàm doitm có tác dụng đổi các tham số các ma trận có các thành phần là hàm

theo thời gian

Hàm wmo có tác dụng tính vector vân tốc góc tuyệt đối chiếu trên hệ tọa độ

khâu

i

i

 

2.2 Giải bài toán động học ngược

a, giải bằng phương pháp giải tích

E E E

Từ phương trình 1, 2 trong hệ ta rút ra : q1 a tan 2( ,y x E E)

Từ (2.1), (2.2), (2.3) ta xác định được dưới dạng giải thức của các biến khớp

liên hệ với điểm tác động cuối, khi xác định được các biến khớp ngược lại ta tìm

0

E E E

q2, q3]T tại mỗi thời điểm đó

Ta lấy giá trị sát giá trị đầu để tiến hành quá trình lặp Newton-Raphson

lần k+1 với lần k nhỏ hơn giá trị cho phép

Dưới đây là 1 doạn chương trình viết bằng maple sử dụng phương pháp

Newton-Raphson lưu kết quả tính toán ra file txt và 1 đoạn vẽ đồ thị từ file txt

được viết bằng matlab

Với quỹ đạo điểm tác động cuối là đường thẳng có phương trình như sau

0.60.4 0.30.5 0.4

E E E

>

% -a = fsc% -anf(fid, '%g %g %g %g %g %g %g', [7 inf])

set(gcf,'Color',[1.0,1.0,1.0]);

grid on

figure(2);

plot(d1,d3,'c','LineWidth',2);xlabel('t');ylabel('q2(rad)'); legend('q2');

set(gcf,'Color',[1.0,1.0,1.0]);

grid on

figure(3);

plot(d1,d4,'c','LineWidth',2);xlabel('t');ylabel('q3(rad)'); legend('q3');

Hình 2.6 Đồ thị q 3 , và điểm tác động cuối được vẽ từ q 1 , q 2 , q 3

III Tính toán tĩnh học

3.1 Cho lực tác động vào khâu thao tác tìm lực dẫn động

Ta biết lực tác động vào khâu cuối đề bài ra

 Xét khâu 3:

3 1 23 0

a C C r

a S

R

P R

a C C r

a S

R

P R

Để có kết quả tính toán trên ta sử dụng các hàm con để tím lực và momen viết

bằng maple như sau :

3.2 Cho lực ( mô men ) dẫn động tính lực ( mô men ) khâu thao tác tác dụng lên đối tượng.

Nhận xét : robot ở đây là 3 bậc RRR không gian tương ứng 3 momen dẫn động U  [U U U1 , 2 , 3 ]Tnhư vậy bài toán tính lực ( mô men ) khâu thao tác tác dụng lên đối tượng khi giải ra chỉ cho ta ba thành phần hoặc 3 lực hoặc 3 mô men hoặc

2 lực và 1 mô men hoặc 2 mô men và 1 lực Ở đây ta tính 3 lực đối tượng tác dụnglên khâu cuối 0 3 [ , , ]T

IV Tính toán động lực học

4.1 Xây dựng cấu trúc động lực học và các thành phần cần thiết để viết

phương trình động lực học

Gọi lc1, lc2, lc3 lần lượt là khoảng cách từ gốc tọa độ O0, O1,

O2 đến khối tâm của từng khâu 1, 2, 3

1

cos( ) 0 sin( ) 0 0

( ) sin( ) 0 os( ) 0

0 1 0 0

1

0 0 0 1

c q q d l q c q C A C d                   1 0 1 1 0 0 0 0 1 c c c l r l                                    1 2 1 2 1 2 1 2 2 2 1 2 1 2 1 2 1 2 0 0 2 2 2 2 2 2 1 2 2 ( )

0

0 0

1

0 0 0 1

c C C C S S a C C a l S C S S C a S C C A C S C d a S                           2 1 2 2 1 2 2 1 2 0 2 2 1 2 1 2 2 1 2 2 1

c c c c c c c l C C l C C l S C r l S C d l S d l S                                 1 23 1 23 1 1 3 23 2 2 1 23 1 23 1 1 3 23 2 2 0 0 3 3 3 3 23 23 1 2 2 3 23 ( ) (

( )

0

0 0 0 1

C C C S S C a C a C a S C S S C S a C a C C A C S C d a S a S                       1 3 23 2 2 3 3 1 3 23 2 2 1 3 23 2 2 0 3 1 3 23 2 2 1 2 2 3 23 1 2 2 3 23 ( ) ) ( ) ( ) 0

( ) 0

1 1

c c c c c c c c C l C a C l C l C a C S l C a C r S l C a C d a S l S d a S l S                                              1 0 0 1 1 1 1 0 0 T R R q                 1 2 2 0 0 2 2 2 1 2 2 T q S R R q C q                  

1 23

3 0 0

3 3 3 1 23

2 3

T

q S







Từ các vị trí trọng tâm và vận tốc góc ta tính được các ma trân Jacobi tịnh tiến

và quay sau

I I I

I I I

21 22 23 1

. T. Ci

mig r

 

 m l g m g d1 1c  2 [ 1l c2sin( )]q2 m g d3 [ 1a2sin( )q2 l c3sin(q2 q3)]

4.2, thiết lập phương trình Lagrang của robot

Chúng ta đã biết phương trình lagrang loại 2 viết từng tọa độ suy rộng như sau:

ij ij

có dạng ma trân như sau: ta xác định được

21 22 23 1

Với các thành phần như sau :

Điểm tác động cuối robot chịu 1 lực F E [- ,F x F y, F z]T:

Để tính toán ta sử dụng các thủ tục hàm con viết bằng maple để tính ma trận

Jacobi quay , các thành phần ma trận C q q( , )còn tính các thành phần như ma trận jacobi tịnh tiến, tính ma trân M(q) thì có thể sử dụng câu lệnh đã có trong thư viện của maple :

V Luật điều khiển

Tất cả các hệ thống điều khiển nêu dưới đây đều theo luật điều khiển PD Khi

thiết kế hệ thống điều khiển ta bỏ qua động học của cơ cấu chấp hành, quán tính động cơ Như vậy chức năng của bộ điều khiển là tạo ra một moomen cần thiết để truyền động khớp robot đảm bảo khớp robot luôn bám theo vị trí đặt

1 Hệ thống điều khiển trong không gian khớp

Tín hiệu đặt đó là quỹ đạo bậc 3 của các

1.1 Hệ thống điều khiển phản hồi

Luật điều khiển

Hình 5.1.Sơ đồ cấu trúc hệ thống điều khiển robot với bộ điều khiển PD

Ta có phương trình động lực học : ( ) ( , ) ( ) T

E

M q q C q q q G q        U J F

,

gọi H(q)=M(q)tránh nhầm với M là vector momen và V q q( , ) C q q q( , )  , M U

tiếp đến ta coi robot không chịu tác dụng của ngoại lực vì luật điều khiển bám quỹ đạoF  0như vậy phương trình động lực học được rút gọn như sau:

Hàm VL biểu thị tổng năng lượng của hệ thống robot: Thành phần chứa Kp tỷ

lệ với năng lượng đầu vào, thành phần sau là động năng của robot mà Kp và H là các ma trận có hệ số dương Nên hàm VL> 0 với q khác qd

Tính đạo hàm cấp 1 của VL ta nhận được:

 V  L q T Kd q   0

(5.3)

Từ (5.2) ,(5.3) cho thấy rằng, mức độ dương của VL phụ thuộc vào Kp; mức độ

âm của VLphụ thuộc vào Kd Do đó tăng tốc độ hội tụ bằng tăng giá trị Kd Nâng

cao độ chính xác tinh của hệ thống điều khiển đạt được bằng tăng hệ số Kp của khâu khuếch đại Tuy nhiên ,Kp và Kd quá lớn sẽ làm giảm độ ổn định và chất lượng quá trình quá độ như độ quá điều chỉnh , thời gian quá độ tăng

1.2 Hệ thống điều khiển momen tính toán

Luật điều khiển

Hình 5.6 Sơ đồ điều khiển

Phươngpháp cơ bản của luật điều khiển là lựa chọn luật điều khiển sao cho khử được các thành phần phi tuyến của phương trình động lực học và phân li đặc tính động lực của thanh nối Kết quả sẽ nhận được một hệ thống tuyến tính đảm bảo độ chính xác chuyển động yêu cầu

Dựa trên phương trình động lực học : M  H q q V q q ( )   ( , )   G q ( )

đây là phương trình vi phân tuyến tính cấp 2 độc lập giữa các khớp Do đó có thể thiết kế các bộ điều khiển độc lập có cấu chúc PD cho từng khớp như sau : U dk qd K p K d (5.6)

Từ (5.4),(5.5) và (5.6) ta rút ra được phương trình vi phân sai số vị trí của hệ

thống kín có dạng như sau : K d K p 0

Phương trình đặc tính ở dạng toán tử Laplace là :

s I K s K   Viết cho từng khớp riêng lẻ (đó là khâu quán tính bậc hai):

2 di pi 0

s K s K 

Các hệ số Kdi, Kpi được chọn luôn dương nên đảm bảo ổn định, và chúng được tính toán theo yêu cấu về chất lượng điều khiển như độ quá điều chỉnh σ , Thời than quá độ Tqd

Trong hệ thống điều khiển không gian làm việc tín hiệu đặt trực tiếp là quỹ đạo chuyển động mong muốn của tay máy robot trong không gian làm việc , lươngphản hồi về sẽ được tính từ vị trí của khớp thông qua khâu động học thuận Khâu tính toán động học ngược được đặt trong mạch vòng điều khiển phản hồi sẽ tính đổi các biến về không gian khớp.

Hệ thống điều khiển trong không gian làm việc sử dụng hiệu quả khi thực hiện tương tác giữa tay máy và môi trường

Hệ thống điều khiển ma trận Jacobien chuyển vị

Luật điều khiển

Lực cần thiết để di chuyển tay máy theo quỹ đạo đặt trong không gian làm việc được xác định từ sai lệch vị trí và sai lệch tốc độ trong không gian làm việc tương ứng với luật điều khiển phản hồi PD đinh điển:

Hình 5.11 Sơ đồ điều khiển

TÀI LIỆU THAM KHẢO

[1] Bài giảng Robotics - PGS TS Phan Bùi Khôi.

[2] Động lực học hệ nhiều vật - GS TSKH Nguyễn Văn Khang [3] Điều khiển Robot công nghiệp – TS Nguyễn Mạnh Tiến [4] Cơ sở Robot công nghiệp -GS.TSKH Nguyễn Văn Khang [5] Robot công nghiệp – GS TSKH Nguyễn Thiện Phúc [6] Phần help của maple

In màu 4,5,10