Báo cáo robot Elbow

TRƯỜNG ĐẠI HỌC BÁCH KHOA HÀ NỘI

VIỆN ĐIỆN

BỘ MÔN TỰ ĐỘNG HOÁ CÔNG NGHIỆP

====o0o====

BÀI TẬP LỚN

ĐỀ TÀI:

TÍNH TOÁN THIẾT KẾ CHO ROBOT ELBOW

Giáo viên hướng dẫn : PGS. TS. Nguyễn Phạm Thục Anh

Nhóm sinh viên thực hiện : MSSV Lớp

Trần Ngọc Sơn : 20115723 CNĐK&TĐH 1

Nguyễn Văn Đức : 20115523 CNĐK&TĐH 2

Nguyễn Đình Việt : 20149583 ĐK&TĐH(CN lên KS)- K56

Vũ Minh Đức : 20115843 CNĐK&TĐH 1

Hoàng Thanh Hải: 20115540 CNĐK&TĐH 1

Nguyễn Văn Hải : 20106026 ĐK&TĐH6-K55

Hà nội, 11-2015

MỤC LỤC

LỜI MỞ ĐẦU.................................................................................................................1

Chương 1........................................................................................................................2

GIỚI THIỆU ROBOT NHÓM NGHIÊN CỨU.........................................................2

1.1 Kết cấu cơ khí robot Elbow...............................................................................2

1.2. Cổ tay robot..........................................................................................................3

1.3. Bàn tay robot (Cơ cấu tác động cuối)..................................................................3

1.4. Ứng dụng..............................................................................................................4

Chương 2........................................................................................................................6

XÂY DỰNG PHẦN MỀM TÍNH TOÁN ĐỘNG.......................................................6

HỌC THUẬN ROBOT ELBOW.................................................................................6

2.1. Xác định số khớp và các hệ trục tọa độ..............................................................6

2.2 Xác định các thông số động học của robot Elbow thông qua bảng DH...............7

2.3 Các ma trận A của robot Elbow............................................................................8

2.4 Tính toán ma trận T06 ( hàm truyền RTH của robot ).............................................9

2.5. Xây dựng phần mềm tính toán ( sử dụng GUI trong Matlab).........10

Chương 3......................................................................................................................12

VIẾT CHƯƠNG TRÌNH MATLAB TÍNH MA TRẬN JACOBY.........................12

TRỰC TIẾP VÀ THÔNG QUA MA TRẬN ......................................................12

3.1. Tính ma trận Jacoby trực tiếp.............................................................................12

3.1.1 Tính toán......................................................................................................12

3.1.2 Viết chương trình Matlab tính ma trận Jacoby trực tiếp..............................14

3.2 Tính ma trận Jacoby thông qua ma trận JH..........................................................15

3.2.2 Viết chương trình Matlab tính ma trận Jacoby thông qua ma trận HJ..........20

Chương 4......................................................................................................................19

THIẾT KẾ QUỸ ĐẠO TRONG KHÔNG GIAN KHỚP........................................19

4.1. Giới thiệu và cơ sử thiết kế quỹ đạo..................................................................19

4.2. Tính toán thiết kế quỹ đạo chuyển động............................................................19

KẾT LUẬN..................................................................................................................22

TÀI LIỆU THAM KHẢO.............................................................................................23

Lời nói đầu

LỜI MỞ ĐẦU

Từ lâu, con người đã không còn xa lạ với từ “Robot” nữa. Bởi lẽ, Robot đã xuất

hiện nhiều trong cuộc sống của chúng ta và nếu chưa tận mắt chứng kiến những Robot

này hoạt động thì cũng được xem qua các phương tiện thông tin đại chúng.

Có thể thấy Robot đã đi vào mọi lĩnh vực của đời sống con người. Vậy, Robot

được ứng dụng như thế nào? Có thể nói, Robot được ứng dụng rất phong phú trong đời

sống con người, cả trong trường học và trong nông nghiệp. Đặc biệt trong công nghiệp,

Robot được sử dụng rộng rãi để làm một số công việc: vận chuyển nguyên vật liệu, sản

phẩm, máy móc hay làm những công việc nặng nhọc, hoặc trong những môi trường làm

việc có nhiệt độ cao, độc hại…

Đầu tiên, Robot được dùng trong một số ngành công nghiệp kỹ thuật cao như công

nghiệp xe hơi, công nghiệp máy bay…để làm những công việc như hàn thân xe, phun

sơn…Những Robot này là những cánh tay máy mô phỏng con người, mỗi tay máy bao

gồm nhiều khâu liên kết với nhau bằng các khớp. Các khâu này có thể chuyển động

tương đối với nhau và làm thay đổi tầm với của Robot. Thông thường những Robot này

được đặt cố định một chỗ và chỉ thao tác được trong khả năng tầm với của chúng.

Với những kiến thức đã học và sau một thời gian tìm hiểu cùng với sự giúp đỡ tận

tình của cô PGS. TS. Nguyễn Phạm Thục Anh thuộc bộ môn Tự động hóa công nghiệp

trường Đại học bách khoa Hà Nội, và sự đóng góp trao đổi xây dựng của bạn bè nhóm

em đã hoàn thành nhiệm vụ được giao.

Song với những hiểu biết còn hạn chế nên nhóm em không tránh khỏi những thiếu

sót. Chúng em rất mong được sự chỉ bảo của cô giáo và sự góp ý kiến của các bạn trong

lớp Kỹ thuật Robot để bài tập của nhóm em được hoàn thiện hơn.

Chúng em xin chân thành cảm ơn !

1

Chương 1. Giới thiệu robot nhóm nghiên cứu

Chương 1

GIỚI THIỆU ROBOT NHÓM NGHIÊN CỨU

1.1 Kết cấu cơ khí robot Elbow

Robot Elbow ở vị trí ban đầu

Robot Elbow ở vị trí bất ky

Hình 1.1. Kết cấu cơ khí robot Elbow.

+ Cấu hình Robot Elbow :

Là một cấu hình robot 6 DOF (6 bậc tự do) tức là gồm 6 khớp chuyển động độc

lập.

2


1.2. Cổ tay robot

Hình 1.2. Cơ cấu cổ tay ba bậc tự do.

Cổ tay có nhiệm vụ định hướng chính xác bàn tay robot (cơ cấu tác động cuối)

trong không gian làm việc. Ví dụ bàn tay robot cần định hướng chính xác so với chi tiết

để gắp chi tiết.

Thông thường cổ tay robot có 3 bậc tự do tương ứng với 3 chuyển động có cấu tạo

điển hình như hình 1.2: Cổ tay xoay xung quanh trục thanh nối cuối cùng (Roll), cổ tay

quay xung quanh trục thẳng đứng tạo chuyển động lắc phải, trái của bàn tay (Pitch), cổ

tay quay xung quang trục nằm ngang tạo chuyển động lên xuống của bàn tay (Yaw).

1.3. Bàn tay robot (Cơ cấu tác động cuối)

Bàn tay được gắn lên cổ tay robot đảm bảo cho robot thực hiện các nhiệm vụ khác

nhau trong không gian làm việc. Cơ cấu bàn tay có hai dạng khác nhau tùy theo chức

năng của robot trong dây chuyền sản xuất: cơ cấu bàn kẹp (gripper) và cơ cấu dụng cụ

(tool).

Một số dạng bàn tay robot của Robot Unimate (Mỹ, 1961):

3


Hình 1.3. Một số hình ảnh về các cơ cấu kẹp.

1.4. Ứng dụng

Hình 1.4.Video ứng dụng cấu hình robot Elbow điều khiển chuyển động theo cánh tay

trong công nghiệp.

4


Hình 1.5.Ứng dụng cấu hình robot Elbow ngoài trạm không gian vũ trụ.

Hình 1.6. Ứng dụng cấu hình robot Elbow trong

công nghiệp sản xuất và lắp ráp xe hơi.

5

Chương 2. Xây dựng phần mềm tính toán động học thuận Robot Elbow

Chương 2

XÂY DỰNG PHẦN MỀM TÍNH TOÁN ĐỘNG

HỌC THUẬN ROBOT ELBOW

2.1. Xác định số khớp và các hệ trục tọa độ

- Robot Elbow là robot có 6 bậc tự do, gồm 6 khớp quay và 7 thanh nối.

- Ta sẽ lần lượt xác định các trục tọa độ như sau :

+ Xác định trục Zi : vì tất cả các khớp đều là khớp quay, nên Zi là trục mà khớp i+1 quay

xung quanh nó.

+ Xác định trục Xi, Yi : xác định như hình bên dưới. ( Lưu ý : để làm đơn giản cho việc

tính toán, ta sẽ chỉnh sửa một số trục tọa độ như sau. O0 sẽ trùng với O1 . O4 trùng với O5,

O6 . Như vậy : a1=a5=a6=0, di = 0, dễ dàng hơn trong việc tính toán).

Hình 2.1. Vị trí ban đầu của robot Elbow và các hệ tọa độ

6


2.2 Xác định các thông số động học của robot Elbow thông qua bảng DH

- Ta xác định các thông số của bảng D-H như sau :

+ : khoảng cách Oi-1 và Oi theo trục zi-1.

+ : góc vặn của thanh nối i . Là góc quay quanh trục z i-1 để trục xi-1 chuyển đến trục

xi theo qui tắc bàn tay phải.

+ : góc xoay đưa trục zi-1về zi quanh xi theo quy tắc bàn tay phải.

+ : khoảng dịch chuyển giữa 2 trục khớp động kề nhau theo trục x.

- Như vậy ta có :

+ do trục Z0 và Z1 vuông góc với nhau.

+ là khoảng cách giữa Z1 và Z2



+ do trục Z4 và Z5 vuông góc với nhau.

+ do trục Z5 trùng với trục Z6

+ là góc quay quanh trục để biến thành .






+ là khoảng cách đo dọc trục giữa O0 và O1

+ là khoảng cách đo dọc trục giữa O5 và O6

(Như đã nói ở trên ta sẽ cho: d1=d6=0)

7


Như vậy ta được bảng D-H như sau:

Khâu

1 900 0 0

2 0 0

3 0 0

4 -900 0

5 900 0 0

6 0 0 0

2.3 Các ma trận A của robot Elbow

Ký hiệu :

, …,

, …,

, , ….

, ,….

Các ma trận A được xác định bằng ma trận biến đổi tọa độ thuần nhất tổng quát :

8


;

;

;

2.4 Tính toán ma trận T06 ( hàm truyền RTH của robot hay ma trận xác định tọa độ

vị trí và hướng của khâu cuối)

Từ các ma trận biến đổi giữa các trục, ta sẽ xác định được hàm truyền RTH của

robot ( hay chính là ma trận chuyển đổi giữa trục 0 và trục 6 của robot)

Sử dụng công cụ matlab để nhập và nhân các ma trận. Sau khi dùng hàm simplify để rút

gọn, ta sẽ được thành phần của ma trận như sau :

9


2.5. Xây dựng phần mềm tính toán ( sử dụng GUI trong Matlab)

Giao diện phần mềm tính toán động học thuận vị trí robot

- Từ ma trận hàm truyền ta có thể thấy rằng các giá trị của các thành phần

phụ thuộc vào các giá trị của các tham số đầu vào là:

+ Các góc: ( tương ứng với dữ liệu đầu vào : từ ‘Goc Theta1’ đến

‘Góc Theta6’)

+ Các giá trị a2, a3, a4.

- Sau khi thiết kế giao diện và viết code ta sẽ được như hình bên dưới :

10


Hình 2.2. Giao diện tính toán động học thuận.

Ta thử một ví dụ và chạy chương trình sẽ được hình như sau:

Chương trình code : Tham khảo phụ lục 1

11

Chương 3. Viết chương trình matlab tính ma trân Jacoby

Chương 3

VIẾT CHƯƠNG TRÌNH MATLAB TÍNH MA TRẬN JACOBY

TRỰC TIẾP VÀ THÔNG QUA MA TRẬN

3.1. Tính ma trận Jacoby trực tiếp

3.1.1 Tính toán

- Ta có : X= [ px py pz Φx Φy Φz]T

- Với Φx Φy Φz : tương ứng là các góc quay vi sai của tay xung quanh các trục x, y, z.

Tuy nhiên, không tồn tại phương trình duy nhất mô tả sự quay xung quanh các trục, do đó sẽ không có phương trình duy nhất cho tính toán Φx Φy Φz.

Như vậy ta có: X= [ px py pz ]T

- Ta đã xác định được:

- Tiếp theo thực hiện đạo hàm theo biến khớp:

+ Tính đạo hàm px, nhận được phương trình sai phân sau:

Từ đó nhận được các thành phần hàng 1 của ma trận Jacoby:

12


Tương tự với Py, Pz.

Ta được hàng 2:

Ta được hàng 3:

13


3.1.2 Viết chương trình Matlab tính ma trận Jacoby trực tiếp

a. Thiết kế giao diện

Hình 3.1. Giao diện phần mềm tính ma trận Jacoby trực tiếp.

- Nhập thử số và chạy thử ta sẽ được kết quả như sau:

14


Chương trình code: tham khảo phụ lục 2.

3.2 Tính ma trận Jacoby thông qua ma trận JH

3.2.1 Tính toán

- Ta xác định các ma trân iTn theo các hệ tọa độ lần lượt từ khâu cuối trở về gốc:

15


Các hệ số của 0T6 được xác định ở phần động lực thuận vị trí.

- Tiếp theo ta sẽ tính toán ma trận JH bằng việc tính các ma trận theo từng cột, tương ứng

với từng khớp của robot.

+ Với khớp là khớp quay. Sử dụng ma trận :

16


+ Với khớp là khớp quay. Sử dụng ma trận , ta được cột 2 của ma trận ::

+ Với khớp là khớp quay. Sử dụng ma trận , ta được cột 3 của ma trận ::

+ Với khớp là khớp quay. Sử dụng ma trận , ta được cột 4 của ma trận :

17




- Như vậy ta đã xác định được ma trận như trên. Tiếp theo, để xác định được ma

trận Jacoby, ta tiếp tục áp dụng công thức sau:

18


+ Với nx, ny, nz, ox, oy, oz, ax, ay, az được xác định từ ma trận hàm truyền khi tính toán

động lực học thuận vị trí:

+ Sử dụng matlab để nhập và nhân các ma trận. Sau khi dùng hàm simplify ta sẽ được

ma trận Jacoby thông qua ma trận HJ ( Vì biểu thức tính quá dài, để biết thêm về Ma trận

Jacoby, xin xem thêm phần phụ lục 3)

19


3.2.2 Viết chương trình Matlab tính ma trận Jacoby thông qua ma trận HJ

a. Thiết kế giao diện

Hình 3.2. Giao diện tính ma trận Jacoby thông qua ma trận

- Nhập thử số và chạy thử ta sẽ được kết quả như sau:

20


Chương trình code: tham khảo phụ lục 3.

21

Chương 4. Thiết kế quỹ đạo trong không gian khớp : bậc 3 quỹ đạo 2-1-2

Chương 4

THIẾT KẾ QUỸ ĐẠO TRONG KHÔNG GIAN KHỚP

Chọn hai điểm bất ky trong không gian làm việc của robot. Sau đó đi thiết kế quỹ

đạo chuyển động Robot ( quỹ đạo bậc 1, 2, 3, 5..)

4.1. Giới thiệu và cơ sở thiết kế quỹ đạo

Thiết kế quỹ đạo chuyển động của robot có lien quan mật thiết đến bài toán điều

khiển robot di chuyển từ vị trí này sang vị trí khác trong không gian làm việc. Đường đi

và quỹ đạo được thiết kế là đại lượng đặc trưng cho hệ thống điều khiển vị trí của robot.

Do đó độ chính xác của quỹ đạo sẽ ảnh hưởng đến chất lượng di chuyển của robot.

Yêu cầu thiết kế quỹ đạo chuyển động của Robot là:

Khâu chấp hành phải đảm bảo đi qua lần lượt các điểm trong không gian làm việc

hoặc di chuyển theo một quỹ đạo xác định.

Quỹ đạo của Robot phải là đường lien tục về vị trí trong một khoảng các nhất

định.

Không có bước nhảy về vận tốc, gia tốc.

Quỹ đạo là các đường cong có dạng:

+ Đa thức bậc 2:

+ Đa thức bậc 3 :

+ Đa thức bậc cao:

Ta sẽ sử dụng quỹ đạo dạng đa thức bậc 3 để thiết kế cho quỹ đạo 2-1-2

4.2. Tính toán thiết kế quỹ đạo chuyển động

Thiết kế quỹ đạo trong không gian khớp

Chọn hai điểm A, B bất ky trong không gian làm việc, biết tọa độ (xE, yE, zE) và

hướng của các khâu thao tác. Thiết kế quỹ đạo chuyển động bất ky từ A đến B.

Theo bài toán động học ta xác định được các biến khớp tại A và B.

Chọn quỹ đạo thiết kế là hàm đa thức bậc 3 theo thời gian như sau:

19


i =1..6 tương ứng với 6 biến khớp

Ta được hệ phương trình sau:

Giả sử thời gian robot đi từ điểm A đến điểm B là trong t(s) và vân tốc tại 2 điểm đó là

bằng 0.

Từ đó ta có hệ phương trình:

Giả sử robot Enbow cần dịch chuyển từ vị trí A(0.1536, 0.2660, 0.2747) đến điểm

B(0.1541, -0.1120, 0.4048) là 5s tương ứng với tọa độ khớp là A , đến B

Ta suy ra hệ số phương trình của quỹ đạo cho khâu 1 là:

20


Ta tính tương tự cho các góc quay còn lại. Xây dựng đồ thị của các biến khớp trên miền

thời gian.

Khi đó ta có phương trình quỹ đạo vị trí theo thời gian :

Ta có đường biểu diễn vị trí, tốc độ của khớp 1 như sau:

21


4.2.2 Phương pháp quỹ đạo dạng đa thức 2-1-2

Hình 4.4.Mô tả quỹ đạo dạng đa thức 2-1-2

Quỹ đạo ab:

Quỹ đạo bc:

Quỹ đạo cd:

22


Để xác định tb ta cân bằng giá trị tốc độ tại điểm b trên đường ab và bc

với

>> q0=0;

>> qc=30;

>> ddq=5;

>> tc=5;

>> t1=tc/2-0.5*sqrt((tc^2*ddq-4*(qc-q0))/ddq);

>> t2=tc-t1;

>> t=t:t1;

>> t=0:0.01:t1;

>> q=q0+ddq*t.^2/2;

>> t3=t1:0.01:t2;

>> q2=q0+ddq*t1*(t3-t1/2);

>> t4=t2:0.01:tc;

>> q4=qc-(ddq*(t4-tc).^2)/2;

>> plot(t,q,t3,q2,t4,q4)

Quy đạo của robot có dạng

23


Hình 4.5 quỹ đạo 2-2

24

Kết luận

KẾT LUẬN

Sau một thời gian học tập và tìm hiểu kỹ lưỡng cùng với sự hướng dẫn nhiệt tình của

thầy giáo PGS. TS. Nguyễn Phạm Thục Anh cùng các thầy, cô giáo trong bộ môn tự

động hóa. Em đã hoàn thành đề tài “ Thiết kế hệ truyền động cho Robot Elbow”. Nội

dung báo cáo của nhóm gồm 4 chương:

Chương 1: Giới thiệu về Robot nhóm nghiên cứu và các ứng dụng của Robot

Chương 2: Xây dựng phần mềm tính toán động học thuận vị trí Robot

Chương 3: Viết chương trình Matlab tính ma trận Jacoby trực tiếp

Chương 4: Thiết kế quỹ đạo trong không gian khớp

Tuy nhiên do còn hạn chế về kiến thức nên trong báo cáo còn nhiều chỗ chưa đầy đủ và

cụ thể hoặc em chưa đề cập sâu.

Em rất mong được sự đóng góp ý kiến của các thầy cô giáo để báo cáo của nhóm em

được hoàn thiện đầy đủ hơn. Cuối cùng em xin chân thành cảm ơn thầy giáo Nguyễn

Phạm Thục Anh đã hết sức nhiệt tình giúp đỡ em.

Em xin chân thành cảm ơn !

Hà Nội, tháng 11 năm 2015

Nhóm sinh viên thực hiện

22

Tài liệu thảm khảo

TÀI LIỆU THAM KHẢO

[1] Nguyễn Mạnh Tiến, Điều khiển Robot công nghiệp, Nhà xuất bản Khoa học Kỹ

thuật Hà Nội.

[2] Trịnh Quang Vinh, và các tác giả, Robot công nghiệp, Nhà xuất bản Khoa học

Kỹ thuật Hà Nội, 2007.

[3] Nguyễn Văn Khánh, Cơ sở Robot công nghiệp, Nhà xuất bản giáo dục Việt

Nam.

[4] Nguyễn Thiện Phúc, Robot công nghiệp, Nhà xuất bản Khoa học Kỹ thuật Hà

Nội, 2000.

[5]…

23


PHỤ LỤC

1, Phụ lục 1: Code phần mềm tính toán động học thuận robot Elbow trên Matlab:

% --- Executes on button press in pushbutton1.

function pushbutton1_Callback(hObject, eventdata, handles)

% hObject handle to pushbutton1 (see GCBO)

% eventdata reserved - to be defined in a future version of MATLAB

% handles structure with handles and user data (see GUIDATA)

t1=str2double(get(handles.theta1,'string'));






a2=str2double(get(handles.a2,'string'));



nx=cosd(t1)*(cosd(t2+t3+t4)*cosd(t5)*cosd(t6)-sind(t2+t3+t4)*sind(t6))-

sind(t1)*sind(t5)*sind(t6);

ny=sind(t1)*(cosd(t2+t3+t4)*cosd(t5)*cosd(t6)-sind(t2+t3+t4)*sind(t6))

+cosd(t1)*sind(t5)*sind(t6);

nz=sind(t2+t3+t4)*cosd(t5)*cosd(t6)+cosd(t2+t3+t4)*sind(t6);

ox=-cosd(t1)*(cosd(t2+t3+t4)*cosd(t5)*cosd(t6)+sind(t2+t3+t4)*sind(t6))

+sind(t1)*sind(t5)*sind(t6);

oy=-sind(t1)*(cosd(t2+t3+t4)*cosd(t5)*cosd(t6)+sind(t2+t3+t4)*sind(t6))-

cosd(t1)*sind(t5)*sind(t6);

oz=-sind(t2+t3+t4)*cosd(t5)*cosd(t6)+cosd(t2+t3+t4)*cosd(t6);

24


ax=cosd(t1)*cosd(t2+t3+t4)*sind(t5)+sind(t1)*cosd(t5);

ay=sind(t1)*cosd(t2+t3+t4)*sind(t5)-cosd(t1)*cosd(t5);

az=sind(t2+t3+t4)*sind(t5);

px=cosd(t1)*(cosd(t2+t3+t4)*a4+cosd(t2+t3)*a3+cosd(t2)*a2);

py=sind(t1)*(cosd(t2+t3+t4)*a4+cosd(t2+t3)*a3+cosd(t2)*a2);

pz=sind(t2+t3+t4)*a4+sind(t2+t3)*a3+sind(t2)*a2;

set(handles.nx,'string',nx);

set(handles.ny,'string',ny);

set(handles.nz,'string',nz);

set(handles.ox,'string',ox);

set(handles.oy,'string',oy);

set(handles.oz,'string',oz);

set(handles.ax,'string',ax);

set(handles.ay,'string',ay);

set(handles.az,'string',az);

set(handles.px,'string',px);

set(handles.py,'string',py);

set(handles.pz,'string',pz);

2, Phụ lục 2: Code tính toán ma trận Jacoby theo phương pháp trực tiếp






25











j11=-sind(t1)*(cosd(t2+t3+t4)*a4+cosd(t2+t3)*a3+cosd(t2)*a2);

j12=-cosd(t1)*(sind(t2+t3+t4)*a4+sind(t2+t3)*a3+sind(t2)*a2);

j13=-cosd(t1)*(sind(t2+t3+t4)*a4+sind(t2+t3)*a3);

j14=-cosd(t1)*sind(t2+t3+t4)*a4;

j15=0;

j16=0;

j21=cosd(t1)*(cosd(t2+t3+t4)*a4+cosd(t2+t3)*a3+cosd(t2)*a2);

j22=-sind(t1)*(sind(t2+t3+t4)*a4+sind(t2+t3)*a3+sind(t2)*a2);

j23=-sind(t1)*(sind(t2+t3+t4)*a4+sind(t2+t3)*a3);

j24=-sind(t1)*sind(t2+t3+t4)*a4;

j25=0;

j26=0;

j31=0;

j32=cosd(t2+t3+t4)*a4+cosd(t2+t3)*a3+cosd(t2)*a2;

j33=cosd(t2+t3+t4)*a4+cosd(t2+t3)*a3;

j34=cosd(t2+t3+t4)*a4;

j35=0;

26


j36=0;

set(handles.j11,'string',j11);


















3, Phụ lục 3: Code tính toán ma trận Jacoby thông qua ma trận JH




27













j11=sind(t5)*sind(t6)*(cosd(t1)*(sind(t2+t3+t4)*sind(t6)+cosd(t2+t3+t4)*cosd(t5)*cosd(

t6))-sind(t1)*sind(t5)*sind(t6))*(a3*cosd(t2+t3)+a2*cosd(t2)+a4*cosd(t2+t3+t4))-

cosd(t6)*sind(t5)*(cosd(t1)*(sind(t2+t3+t4)*sind(t6)-cosd(t2+t3+t4)*cosd(t5)*cosd(t6))

+sind(t1)*sind(t5)*sind(t6))*(a3*cosd(t2+t3)+a2*cosd(t2)+a4*cosd(t2+t3+t4))-

cosd(t5)*(cosd(t5)*sind(t1)+cosd(t2+t3+t4)*cosd(t1)*sind(t5))*(a3*cosd(t2+t3)+a2*cosd

(t2)+a4*cosd(t2+t3+t4));

j12=(cosd(t1)*(sind(t2+t3+t4)*sind(t6)+cosd(t2+t3+t4)*cosd(t5)*cosd(t6))-

sind(t1)*sind(t5)*sind(t6))*(a2*cosd(t6)*sind(t3)*sind(t4)-a3*cosd(t4)*cosd(t6)-

a2*cosd(t3)*cosd(t4)*cosd(t6)-

a4*cosd(t6)+a3*cosd(t5)*sind(t4)*sind(t6)+a2*cosd(t3)*cosd(t5)*sind(t4)*sind(t6)+a2*c

osd(t4)*cosd(t5)*sind(t3)*sind(t6))-(cosd(t1)*(sind(t2+t3+t4)*sind(t6)-

cosd(t2+t3+t4)*cosd(t5)*cosd(t6))

+sind(t1)*sind(t5)*sind(t6))*(a4*sind(t6)+a3*cosd(t4)*sind(t6)+a2*cosd(t3)*cosd(t4)*si

nd(t6)+a3*cosd(t5)*cosd(t6)*sind(t4)-

a2*sind(t3)*sind(t4)*sind(t6)+a2*cosd(t3)*cosd(t5)*cosd(t6)*sind(t4)+a2*cosd(t4)*cosd

(t5)*cosd(t6)*sind(t3))

+sind(t5)*(a2*sind(t3+t4)+a3*sind(t4))*(cosd(t5)*sind(t1)+cosd(t2+t3+t4)*cosd(t1)*sin

d(t5));

28


j13=a3*sind(t4)*sind(t5)*(cosd(t5)*sind(t1)+cosd(t2+t3+t4)*cosd(t1)*sind(t5))-

(cosd(t1)*(sind(t2+t3+t4)*sind(t6)-cosd(t2+t3+t4)*cosd(t5)*cosd(t6))

+sind(t1)*sind(t5)*sind(t6))*(a4*sind(t6)+a3*cosd(t4)*sind(t6)+a3*cosd(t5)*cosd(t6)*si

nd(t4))-(cosd(t1)*(sind(t2+t3+t4)*sind(t6)+cosd(t2+t3+t4)*cosd(t5)*cosd(t6))-

sind(t1)*sind(t5)*sind(t6))*(a4*cosd(t6)+a3*cosd(t4)*cosd(t6)-

a3*cosd(t5)*sind(t4)*sind(t6));

j14=-a4*cosd(t6)*(cosd(t1)*(sind(t2+t3+t4)*sind(t6)+cosd(t2+t3+t4)*cosd(t5)*cosd(t6))-

sind(t1)*sind(t5)*sind(t6))-a4*sind(t6)*(cosd(t1)*(sind(t2+t3+t4)*sind(t6)-

cosd(t2+t3+t4)*cosd(t5)*cosd(t6))+sind(t1)*sind(t5)*sind(t6));

j15=0;

j16=0;

j21=cosd(t5)*(cosd(t1)*cosd(t5)-

cosd(t2+t3+t4)*sind(t1)*sind(t5))*(a3*cosd(t2+t3)+a2*cosd(t2)+a4*cosd(t2+t3+t4))-

cosd(t6)*sind(t5)*(sind(t1)*(sind(t2+t3+t4)*sind(t6)-cosd(t2+t3+t4)*cosd(t5)*cosd(t6))-

cosd(t1)*sind(t5)*sind(t6))*(a3*cosd(t2+t3)+a2*cosd(t2)+a4*cosd(t2+t3+t4))

+sind(t5)*sind(t6)*(sind(t1)*(sind(t2+t3+t4)*sind(t6)+cosd(t2+t3+t4)*cosd(t5)*cosd(t6))

+cosd(t1)*sind(t5)*sind(t6))*(a3*cosd(t2+t3)+a2*cosd(t2)+a4*cosd(t2+t3+t4));

j22=(sind(t1)*(sind(t2+t3+t4)*sind(t6)+cosd(t2+t3+t4)*cosd(t5)*cosd(t6))

+cosd(t1)*sind(t5)*sind(t6))*(a2*cosd(t6)*sind(t3)*sind(t4)-a3*cosd(t4)*cosd(t6)-



osd(t4)*cosd(t5)*sind(t3)*sind(t6))-(sind(t1)*(sind(t2+t3+t4)*sind(t6)-

cosd(t2+t3+t4)*cosd(t5)*cosd(t6))-

cosd(t1)*sind(t5)*sind(t6))*(a4*sind(t6)+a3*cosd(t4)*sind(t6)+a2*cosd(t3)*cosd(t4)*sin

d(t6)+a3*cosd(t5)*cosd(t6)*sind(t4)-


(t5)*cosd(t6)*sind(t3))-sind(t5)*(a2*sind(t3+t4)+a3*sind(t4))*(cosd(t1)*cosd(t5)-

cosd(t2+t3+t4)*sind(t1)*sind(t5));

j23=-(sind(t1)*(sind(t2+t3+t4)*sind(t6)+cosd(t2+t3+t4)*cosd(t5)*cosd(t6))

+cosd(t1)*sind(t5)*sind(t6))*(a4*cosd(t6)+a3*cosd(t4)*cosd(t6)-

a3*cosd(t5)*sind(t4)*sind(t6))-(sind(t1)*(sind(t2+t3+t4)*sind(t6)-


29


cosd(t1)*sind(t5)*sind(t6))*(a4*sind(t6)+a3*cosd(t4)*sind(t6)+a3*cosd(t5)*cosd(t6)*sin

d(t4))-a3*sind(t4)*sind(t5)*(cosd(t1)*cosd(t5)-cosd(t2+t3+t4)*sind(t1)*sind(t5));

j24=-a4*cosd(t6)*(sind(t1)*(sind(t2+t3+t4)*sind(t6)+cosd(t2+t3+t4)*cosd(t5)*cosd(t6))

+cosd(t1)*sind(t5)*sind(t6))-a4*sind(t6)*(sind(t1)*(sind(t2+t3+t4)*sind(t6)-

cosd(t2+t3+t4)*cosd(t5)*cosd(t6))-cosd(t1)*sind(t5)*sind(t6));

j25=0;

j26=0;

j31=sind(t2+t3+t4)*cosd(t5)*sind(t5)*(a3*cosd(t2+t3)+a2*cosd(t2)+a4*cosd(t2+t3+t4))

*(cosd(t6)^2+sind(t6)*cosd(t6)-1);

j32=(cosd(t2+t3+t4)*sind(t6)+sind(t2+t3+t4)*cosd(t5)*cosd(t6))*(a4*sind(t6)+a3*cosd(t

4)*sind(t6)+a2*cosd(t3)*cosd(t4)*sind(t6)+a3*cosd(t5)*cosd(t6)*sind(t4)-


(t5)*cosd(t6)*sind(t3))-(cosd(t2+t3+t4)*cosd(t6)-

sind(t2+t3+t4)*cosd(t5)*cosd(t6))*(a2*cosd(t6)*sind(t3)*sind(t4)-a3*cosd(t4)*cosd(t6)-



osd(t4)*cosd(t5)*sind(t3)*sind(t6))-

sind(t2+t3+t4)*(a2*sind(t3+t4)+a3*sind(t4))*(cosd(t5)^2-1);

j33=(cosd(t2+t3+t4)*sind(t6)+sind(t2+t3+t4)*cosd(t5)*cosd(t6))*(a4*sind(t6)+a3*cosd(t

4)*sind(t6)+a3*cosd(t5)*cosd(t6)*sind(t4))+(cosd(t2+t3+t4)*cosd(t6)-

sind(t2+t3+t4)*cosd(t5)*cosd(t6))*(a4*cosd(t6)+a3*cosd(t4)*cosd(t6)-

a3*cosd(t5)*sind(t4)*sind(t6))-a3*sind(t2+t3+t4)*sind(t4)*(cosd(t5)^2-1);

j34=a4*cosd(t2+t3+t4)-

a4*sind(t2+t3+t4)*cosd(t5)*cosd(t6)^2+a4*sind(t2+t3+t4)*cosd(t5)*cosd(t6)*sind(t6);

j35=0;

j36=0;

j41=-(cosd(t1)*(sind(t2+t3+t4)*sind(t6)+cosd(t2+t3+t4)*cosd(t5)*cosd(t6))-

sind(t1)*sind(t5)*sind(t6))*(cosd(t2+t3+t4)*cosd(t6)-sind(t2+t3+t4)*cosd(t5)*cosd(t6))-

(cosd(t1)*(sind(t2+t3+t4)*sind(t6)-cosd(t2+t3+t4)*cosd(t5)*cosd(t6))

+sind(t1)*sind(t5)*sind(t6))*(cosd(t2+t3+t4)*sind(t6)+sind(t2+t3+t4)*cosd(t5)*cosd(t6))

;

30


j42=cosd(t6)*sind(t5)*(cosd(t1)*(sind(t2+t3+t4)*sind(t6)-

cosd(t2+t3+t4)*cosd(t5)*cosd(t6))+sind(t1)*sind(t5)*sind(t6))-

sind(t5)*sind(t6)*(cosd(t1)*(sind(t2+t3+t4)*sind(t6)+cosd(t2+t3+t4)*cosd(t5)*cosd(t6))-

sind(t1)*sind(t5)*sind(t6));









j45=-cosd(t6)*(cosd(t1)*(sind(t2+t3+t4)*sind(t6)+cosd(t2+t3+t4)*cosd(t5)*cosd(t6))-

sind(t1)*sind(t5)*sind(t6))-sind(t6)*(cosd(t1)*(sind(t2+t3+t4)*sind(t6)-

cosd(t2+t3+t4)*cosd(t5)*cosd(t6))+sind(t1)*sind(t5)*sind(t6));

j46=0;

j51=-(sind(t1)*(sind(t2+t3+t4)*sind(t6)+cosd(t2+t3+t4)*cosd(t5)*cosd(t6))

+cosd(t1)*sind(t5)*sind(t6))*(cosd(t2+t3+t4)*cosd(t6)-

sind(t2+t3+t4)*cosd(t5)*cosd(t6))-(sind(t1)*(sind(t2+t3+t4)*sind(t6)-


cosd(t1)*sind(t5)*sind(t6))*(cosd(t2+t3+t4)*sind(t6)+sind(t2+t3+t4)*cosd(t5)*cosd(t6));

j52=cosd(t6)*sind(t5)*(sind(t1)*(sind(t2+t3+t4)*sind(t6)-

cosd(t2+t3+t4)*cosd(t5)*cosd(t6))-cosd(t1)*sind(t5)*sind(t6))-

sind(t5)*sind(t6)*(sind(t1)*(sind(t2+t3+t4)*sind(t6)+cosd(t2+t3+t4)*cosd(t5)*cosd(t6))

+cosd(t1)*sind(t5)*sind(t6));







31




j55=-cosd(t6)*(sind(t1)*(sind(t2+t3+t4)*sind(t6)+cosd(t2+t3+t4)*cosd(t5)*cosd(t6))

+cosd(t1)*sind(t5)*sind(t6))-sind(t6)*(sind(t1)*(sind(t2+t3+t4)*sind(t6)-

cosd(t2+t3+t4)*cosd(t5)*cosd(t6))-cosd(t1)*sind(t5)*sind(t6));

j56=0;

j61=(cosd(t2+t3+t4)*cosd(t6)-sind(t2+t3+t4)*cosd(t5)*cosd(t6))^2+

(cosd(t2+t3+t4)*sind(t6)+sind(t2+t3+t4)*cosd(t5)*cosd(t6))^2;

j62=-sind(t2+t3+t4)*cosd(t5)*cosd(t6)*sind(t5)*(cosd(t6)+sind(t6));



j65=cosd(t2+t3+t4)-

sind(t2+t3+t4)*cosd(t5)*cosd(t6)^2+sind(t2+t3+t4)*cosd(t5)*cosd(t6)*sind(t6);

j66=0;

b11=sind(t5)*cosd(t6)*(cosd(t2+t3+t4)*a4+cosd(t2+t3)*a3+cosd(t2)*a2);

b21=-sind(t5)*sind(t6)*(cosd(t2+t3+t4)*a4+cosd(t2+t3)*a3+cosd(t2)*a2);

b31=-cosd(t5)*(cosd(t2+t3+t4)*a4+cosd(t2+t3)*a3+cosd(t2)*a2);

b41=sind(t2+t3+t4)*cosd(t5)*cosd(t6)+cosd(t2+t3+t4)*sind(t6);

b51=-sind(t2+t3+t4)*cosd(t5)*cosd(t6)+cosd(t2+t3+t4)*cosd(t6);

b61=sind(t2+t3+t4)*sind(t5);

b12=a4*sind(t6) + a3*cosd(t4)*sind(t6) + a2*cosd(t3)*cosd(t4)*sind(t6) +

a3*cosd(t5)*cosd(t6)*sind(t4) - a2*sind(t3)*sind(t4)*sind(t6) +

a2*cosd(t3)*cosd(t5)*cosd(t6)*sind(t4) + a2*cosd(t4)*cosd(t5)*cosd(t6)*sind(t3)

b22=a4*cosd(t6) + a3*cosd(t4)*cosd(t6) + a2*cosd(t3)*cosd(t4)*cosd(t6) -

a2*cosd(t6)*sind(t3)*sind(t4) - a3*cosd(t5)*sind(t4)*sind(t6) -

a2*cosd(t3)*cosd(t5)*sind(t4)*sind(t6) - a2*cosd(t4)*cosd(t5)*sind(t3)*sind(t6)

b32=sind(t5)*(a2*sind(t3+t4)+a3*sind(t4));

b42=-sind(t5)*cosd(t6);

32


b52=sind(t5)*sind(t6);

b62=cosd(t5);

b13=a4*sind(t6) + a3*cosd(t4)*sind(t6) + a3*cosd(t5)*cosd(t6)*sind(t4)

b23=a4*cosd(t6)+a3*cosd(t4)*cosd(t6)-a3*cosd(t5)*sind(t4)*sind(t6);

b33=a3*sind(t4)*sind(t5);



b63=cosd(t5);

b14=a4*sind(t6);

b24=a4*cosd(t6);

b34=0;



b64=cosd(t5);

b15=0;

b25=0;

b35=0;

b45=sind(t6);

b55=cosd(t6);

b65=0;

b16=0;

b26=0;

b36=0;

b46=0;

b56=0;

b66=0;

33



























34













set(handles.b11,'string',b11);















35























36

Documents

Báo cáo robot Elbow