Bài tập nhóm mô phỏng hệ cơ điện tử

dạng quỹ đạo thẳng, trònThuận Sử dụng kết quả tính ngược tính ngược lại các biến khớp Yes Chọn hệ số Từ bài toán động lực học thuận ko có PID so sánh độ lệch đồ thị tính toán, thực tìm

MỤC LỤC

THÀNH VIÊN VÀ PHÂN CÔNG CÔNG VIỆC:

NOTE: - Các kết quả tính toán quá dài sẽ được trình bày trong phụ lục.

Phạm Thanh Tú 20156794 Động học (thuận ngược

code maple, matlab) +động lực học (tổngquát+ngược) + thiết kế quỹđạo

10/10Tích cực

Nguyễn Trọng Huy 201557

06

3d + xuất stl + hỗ trợ

mô phỏng Simulink(sửa các lỗi xuất,trục, body), open GL

10/10Tích cực

93

Động học (thuận,code maple) + độnglực học( hỗ trợ sửalỗi, sửa code)

10/10Tích cực

Bùi Văn Trường 3d+Mô phỏng Open GL+

PID+ động lực học ( thamgia hỗ trợ giải, sửa lỗi)

10/10Tích cực

Nguyễn Mạnh Thắng 20156502 3d+Mô phỏng Simulink

+PID +động lực học (viết

thuận+ngược)

10/10Tích cực

Đỗ văn Đức 20155389 PID + Hỗ trợ mô phỏng

Simulink + hỗ trợ sửa lỗicode

10/10Tích cực

TIẾN ĐỘ HOÀN THÀNH VÀ CHƯA HOÀN THÀNH

ST

T

1 Động học Thuận Quy luật quỹ đạo điểm cuối Yes

Mô phỏng số (Đặc biệt và bất kì)

Quỹ đạo tròn thẳng theo thời gian

Yes

dạng quỹ đạo thẳng, tròn

Thuận Sử dụng kết quả tính ngược tính

ngược lại các biến khớp

Yes

Chọn hệ số

Từ bài toán động lực học thuận

ko có PID so sánh độ lệch đồ thị (tính toán, thực) tìm các hệ số.

Yes

Mô phỏng

Mô phỏng PID trên simulink Yes

phỏng

Open GL

Chạy thành công quỹ đạo thẳng tròn

Yes

Mô phỏng trên Matlab

Chạy thành công quỹ đạo thẳng tròn trên matlab Mô hình có gắn PID và sử dụng kết quả tính toán động lực học.

Yes

6 Báo cáo Words Hoàn thành báo cáo chi tiết Yes

Power point

DANH MỤC HÌNH ẢNH

CHƯƠNG 1: PHÂN TÍCH, THIẾT KẾ CẤU TRÚC 3D

1.1 Phân tích cấu trúc.

Hình 1 Mô hình Robot và hệ trục Dh tương ứng

Sử dụng phương pháp DH :

-Trục zi được đặt tại khớp i+1, chiều của trục zi tùy ý.

-Trục xi được xác định là đường vuông góc chung giữa khớp i và khớp i+1, nếu hai khớp i và i+1 song song xi có thể là bất cứ đường vuông góc chung nào Nếu hai trục i và i+1 cắt nhau xi được xác định dựa vào zi và zi+1 theo quy tắc bàn tay phải

-Trục yi xác định dựa vào xi và zi theo quy tắc bàn tay phải

Hình 2 Minh họa về phương pháp đặt hệ trục D-H:

- Lập ma trận Denavit – Hartenberg

Các thông số động học Denavit – Hartenberg được xác định như sau:

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

• θi: góc giữa 2 đường vuông góc chung Là góc quay quanh trục zi-1 để trục x

i-1 chuyển đến trục xi theo qui tắc bàn tay phải

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

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

Cách đặt hệ trục mong muốn trong solidword:

1 Chọn Move/copy , bôi đen toàn bộ khâu

2 Chọn tọa độ điểm đầu và điểm cần đặt hệ trục:

3 Chọn để hiển thị hệ trục:

- Hình ảnh thiết kế 3D ( từng khâu):

Hình 3 Khâu đế

-Hệ trục đặt tại khâu 0 khớp 1:

Hình 4 Cách đặt Hệ trục khâu đế

Hình 5 Khâu 1

-Hệ trục đặt tại khớp 2 khâu 1:

Hình 6 Cách đặt hệ trục khâu 1

Hình 7 Khâu 2

-Hệ trục đặt tại khớp 3 khâu 2:

Hình 8 Cách đặt hệ trục khâu tịnh tiến ( khâu 2)

Hình 9 Khâu 3

-Hệ trục đặt tại khớp 4 khâu 3 :

Hình 10 Cách đặt hệ trục khâu quay (Khâu 3)

Hình 11 Khâu 4

-Hệ trục đặt tại điểm lắp dụng cụ khâu 4:

Hình 12 Cách đặt hệ trục khâu quay (khâu 4)

1.2.2 Lắm ghép và xuất file.

- Hình ảnh lắm ghép ( từng khâu)

- Các bước xuất ra file dùng để mô phỏng:

CHƯƠNG 2: PHẦN ĐỘNG HỌC

2.1 Động học thuận.

Mục tiêu: - Khảo sát chuyển động khâu cuối phụ thuộc vào quy luật biến khớp

cho trước và tìm miền làm việc của Robot

-Ứng dụng trong thực tế: Từ các quan hện giữa quỹ đạo điểm cuối và biến khớp,

giúp thiết lập miền không gian làm việc phù hợp, thiết kế chiều dài các khâu phù hợp với yêu cầu làm việc

2.1.1 Tình các phương trình động học cần thiết.

Từ bảng D-H ta lập được ma trận biến đổi thuần nhất từ hệ sang hệ Tính matrận chuyển từ hệ 0 sang hệ bằng công thức:

Ma trận biến đổi tọa độ thuần nhất Denavit – Hartenberg:

Ta quy ước như sau :

Khâu 2 sang khâu 3:

Khâu 3 sang khâu 4:

Ma trận chuyển từ khâu 0 sang khâu 4 được tính theo công thức:

Note: Kết quả chi tiết ở phụ lục

Lập các phương trình động học:

Note: Kết quả chi tiết ở phụ lục

2.1.2 Phương trình quỹ đạo điểm cuối, các vận tốc, gia tốc.

Ta giải quỹ đạo điểm cuối bằng cách chọn giải 3 trong tổng số 12 phương trìnhlập được ở mục 2.1.1

Giải được tọa độ điểm cuối như sau:

Vận tốc gia tốc khối tâm khâu:

Tọa độ khối tâm khâu trong hệ tọa độ gắn với khâu: (các thông số để dạng chữ Số

Tọa độ khối tâm khâu trong hệ cơ sở được tính:

Vận tốc khối tâm khâu-ma trận jacobi tịnh tiến khối tâm khâu

Miền làm việc của robot:

- Từ giới hạn từng khâu và phương trình quỹ đạo điểm cuối ta tính toán miềnlàm việc:

- Giới hạn các biến khớp:

Sử dụng tính toán lập trình trên matlab ta có các đồ thị miền làm việc:

Hình 13 Miền làm việc theo phương X-Y

Hình 14 Miền làm việc trong không gian Y-Z

Hình 15 Miền làm việc trong không gian X-Z

Hình 16 Miền làm việc vẽ trong không gian 3D

2.1.3 Mô phỏng động học thuận.

1 Mô phỏng bằng quy luật đặc biệt ( kiểm tra bài toán thuận):

Chọn các quy luật biến khớp đặc biệt: ( thuộc giới hạn biến khớp)

Tính toán các tọa độ điểm cuối bằng cả phương pháp hình học và phương pháp DH

so sánh 2 kết quả để kiểm tra

Tọa độ điểm cuối dễ dàng được xác định bằng phương pháp hình học:

- Tại thì [

- Tại thì [

Tọa độ điểm cuối được tính bằng phương pháp giải số:

Hình 17 Quỹ đạo điểm cuối bài toán thuận kiểm tra

Từ đồ thị ta thấy:

Kết luận: Ta thấy kết quả tính toán hình học và kết quả tính toán bằng ma trận

DH trên maple hoàn toàn trùng nhau nên kết quả giải động học thuận là đúng.

2 Thiết kế mô phỏng theo quỹ đạo bất kì:

Quỹ đạo điểm E

Hình 18 Quỹ đạo mô phỏng thuận

Vận tốc, gia tốc điểm E:

Hình 20 Đồ thị vận tốc, gia tốc điểm E theo phương y

Hình 21 Đồ thị vận tốc gia tốc điểm E theo phương z

Đồ thị vận tốc, gia tốc khối tâm các khâu:

Khâu 1

Hình 22 Đồ thị vận tốc, gia tốc khối tâm khâu 1 theo x.

Hình 24 Đồ thị vận tốc, gia tốc khối tâm khâu 1 theo z.

Khâu 2

Hình 25 Đồ thị vận tốc, gia tốc khối tâm khâu 2 theo x.

Hình 26 Đồ thị vận tốc, gia tốc khối tâm khâu 2 theo y

Khâu 3

Hình 28 Đồ thị vận tốc, gia tốc khối tâm khâu 3 theo x

Hình 29 Đồ thị vận tốc, gia tốc khối tâm khâu 3 theo y

Hình 30 Đồ thị vận tốc, gia tốc khối tâm khâu 3 theo z

Khâu 4

Hình 32 Đồ thị vận tốc, gia tốc khối tâm khâu 4 theo y

Hình 33 Đồ thị vận tốc, gia tốc khối tâm khâu 4 theo z

Đồ thị vận tốc góc các khâu:

Khâu 1:

Khâu 2:

Hình 35 Vận tốc gia tốc góc khâu 2 theo x,y,z

Khâu 3:

Hình 36 Vận tốc gia tốc góc khâu 1 theo x,y,z

Khâu 4:

Hình 37 Vận tốc gia tốc góc khâu 1 theo x,y,z

2.2Động học ngược

Mục tiêu: - Khảo sát quy luật biến khớp phụ thuộc vào quy luật chuyển động

điểm cuối cho trước Nhằm phục vụ bài toán thiết kế quỹ đạo

- Ứng dụng trong thực tế: Giúp quá trình mô phỏng chuyển động và khảo sát

các biến khớp khi cho chuyển động theo quỹ đạo mong muốn

2.2.1 Giải bài toán động học ngược bằng phương pháp số (Newton-raphson).

Lý thuyết tính toán:

Trong giải tích số, phương pháp Newton-Rapshon là một phương pháp tìm nghiệm xấu xỉ gần đúng của một hàm số có tham số thực dựa vào tính hội tụ và ý nghĩa hình học của hàm số:

Phương pháp được thực hiện như sau:

Phương pháp bắt đầu với hàm là hàm cần tìm xấp sỉ ở đây là 3phương trìnhgiải góc Cardan (rotx roty rotz) mà ta đã chọn Tính các đạo hàm của 3 phươngtrình trên thực hiện đáp ứng ta tính được là 1 xấp xỉ tốt hơn :

là nghiệm đầu ta giải được tại t=0

Lặp đi lặp lại quá trình trên ta được xấp xỉ

Ta chọn các giải các phương trình:

Mô phỏng số bài toán ngược (đường thẳng ):

Quỹ đạo đặt:

Bộ nghiệm đầu: thay vào 4 phương trình được chọn giải ra bộ nghiệm.

Các đồ thị vẽ được:

- Đồ thị thật: Đồ thị được vẽ bằng phương trình quỹ đạo đặt cho trước

Hình 38 Quỹ đại điểm cuối theo quy luật cho trước

- Đồ thị bài toán ngược: sau khi gải bài toán ngược được các giá trị biếnkhớp tương ứng, thay lại vào bài toán thuận để vẽ đồ thị quỹ đạo điểmcuối

Mô phỏng số bài toán ngược (đường tròn):

Quỹ đạo đặt:

Bộ nghiệm đầu: [Các đồ thị vẽ được:

- Đồ thị thật: Đồ thị được vẽ bằng phương trình quỹ đạo đặt cho trước

Hình 41 Đồ thị quy luật cho trước

- Đồ thị bài toán ngược: sau khi gải bài toán ngược được các giá trị biếnkhớp tương ứng, thay lại vào bài toán thuận để vẽ đồ thị quỹ đạo điểmcuối

Hình 42 Đồ thị kiểm tra

- Kiểm tra chính xác về mặt quỹ đạo t vẽ chung 2 đồ thị trên cùng mộtmiền

Hình 43 Vẽ hai đồ thị trên cùng một miền

Kết luận: Bài toán ngược chính xác về mặt quỹ đạo.

2.2.2 Kết quả tính toán các biến khớp, vận tốc khớp, giá tốc khớp

Để kiểm tra bài toán ngược ta có các điều khiện sau:

1 Tại các tọa độ cực trị của đồ thị biến khớp q ( q chuyển từ tăng thành giảm hoặc ngược lại) thì vận tốc của khớp tại thời điểm đó phải bằng 0

2 Tại các điểm cực trị của vận tốc khớp ( vận tốc chuyển từ tang thành giảm hoặc ngược lại) thì gia tốc tại thời điểm đó phải bằng 0

a) Đồ thị đối với

Hình 44 Đồ thị q1

Kiểm tra:

- Tại các điểm trên đồ thị khi đồ thị đổi chiều thì vận tốc

- Tại các điểm trên đồ thị khi đồ thị đổi chiều thì vận tốc

b) Đồ thị đối với

Hình 45 Đồ thị q2

Kiểm tra:

- Tại Đồ thị đặt cực trị ( đổi chiều tăng giảm) thì

- Tại Đồ thị đặt cực trị ( đổi chiều tăng giảm) thì

c) Đồ thị đối với

Hình 46 Đồ thị q3

Kiểm tra:

- Tại Đồ thị đặt cực trị ( đổi chiều tăng giảm) thì

- Tại Đồ thị đặt cực trị ( đổi chiều tăng giảm) thì

Kết luận: Bài toán ngược giải ra đúng.

d) Đồ thị đối với

Hình 47 Đồ thị q4

Kiểm tra:

- Tại Đồ thị đặt cực trị ( đổi chiều tăng giảm) thì

- Tại Đồ thị đặt cực trị ( đổi chiều tăng giảm) thì

Kết luận: Bài toán ngược giải ra đúng.

2.2.3 Kiểm tra tính chính xác của bài toán ngược:

Phương pháp:

- Tính ma trận Jacobi đối với khâu cuối

- Vận tốc điểm E tính theo Jacobi có công thức:

- So sánh với vận tốc được tính từ quy luật chúng ta đặt ban đầu

Các đồ thị kiểm tra:

Hình 48 Đồ thị vận tốc thực và vận tốc kiểm tra phương x

Hình 49 Đồ thị vận tốc thực và vận tốc kiểm tra phương y

Nhận xét: Các kết quả đồ thị nhận được tương đối chính xác với mức sai tối

ta trên đồ thị là 0.6 mm/s.

Kết luận chung:

Bài toán ngược giải bằng phương pháp số trên là chính xác

CHƯƠNG 3: THIẾT KẾ QUỸ ĐẠO

3.1Thiết kê quỹ đạo hình học đơn giản:

Mục tiêu: thiết kế các quỹ đạo chuyển động khâu cuối theo thời gian phù hợp

với chuyển động, miền làm việc của Robot

- Ứng dụng trong thực tế: Áp dụng cho bài toán động học ngược hoặc bài

Thay các điều kiện:

Được quy luật điểm cuối:

Sử dụng phương pháp tang thức bậc 3 ta tính được các phương trình :

Đường tròn đi tâm I ( 600,0 , 200) và bán kính R =100 chuyển động trong mặt phẳng song song Oxz.

Phương trình đường thẳng đi qua A, B:

Thay các điều kiện:

Được quy luật điểm cuối:

Yêu cầu toán: Thiết kế quỹ đạo trơn đi qua 5 điểm bất kì trong miền làm việc

của Robot

Lý thuyết: Với điểm thuộc quỹ đạo cần thiết kế ta sử dụng phương phápSpline thiết kế quỹ đạo đoạn là các đa thức bậc (ở đây ta chọn thiết kế theo quỹđạo bậc 3) là đa thức nội suy Lagrange, nội suy Newton,

Các bước tính toán sử dụng Cubic Spline:

1 Chọn tọa độ các điểm và giải biến khớp tương ứng thời gian:

điểm Sử dụng kết quả bài toán ngược ta giải được các giá trị biến khớp tương ứng trong bảng sau:

2 Tính các bước thời gian:

3 Xây dựng ma trận hệ số A véc tơ vế phải r

4 Giải phương trình

5 Giải tìm các hệ số theo công thức:

- Quỹ đạo có điều khiển vận tốc đầu cuối:

- Vận tốc đặt :

Phương trình q1:

Phương trình q2

Phương trình q3:

Phương trình q4:

Vận tốc các biến khớp:

Với là số thứ tự biên khớp : Với là trạng thái biên khớp tại điểm thứ :

Note: kết quả chi tiết ở phụ lục

Gia tốc các biến khớp:

Với là số thứ tự biên khớp : Với là trạng thái biên khớp tại điểm thứ :

Note: kết quả chi tiết ở phụ lục

Đồ thị quỹ đạo từng đoạn và đồ thị toàn bộ quỹ đạo:

- Đoạn 1 A(600, 0, 200) sang B(650, 0, 250):

Hình 51 Quỹ đạo đoạn 1

- Đoạn 2 B( 650, 0, 250) sang C(670, 0 200):

- Đoạn 3 C(670, 0, 200) sang D(700, 0, 210):

Hình 53 Quỹ đạo đoạn 3

Đoạn 4 D (700, 0, 210) sang E (720, 0, 300):

Hình 54 Quỹ đạo đoạn 4

Đồ thị toàn bộ quỹ đạo trơn đi qua 5 điểm có vận tốc đặt ban đầu:

Hình 55 Quỹ đọa đi qua 5 điểm

Nhận xét: Quỹ đạo là các đường cong liên tục không đứt đoạn.

Đồ thị vận tốc từng đoạn và đồ thị vận tốc trên toàn bộ quỹ đạo:

Vận tốc q1:

CHƯƠNG 4: MÔ PHỎNG OPEN GL

4.1 Giới thiệu chung về OpenGL

Giới thiệu chung:

Theo định nghĩa tin học, OpenGL (Open Graphics Library) là một tiêuchuẩn kỹ thuật đồ họa được hình thành với mục đích định ra một giao diện lậptrình ứng dụng (API) theo không gian 3 chiều Trước khi OpenGL xuất hiện, bất

cứ công ty nào muốn phát triển một ứng dụng đồ họa thường phải viết lại phần

đồ họa của nó cho phù hơp với mỗi nền tảng hệ điều hành cũng như phần cứng

đồ họa Với OpenGL, một ứng dụng có thể tạo ra các hiệu ứng tương tự trongbất kỳ hệ điều hành nào sử dụng các bộ điều hợp đồ họa của OpenGL Có thểnói, OpenGL giống như một ngôn ngữ đồ họa độc lập và có khả năng tươngthích với mọi nền tảng, mọi kiểu máy tính, thậm chí cả trên những máy tínhkhông hỗ trợ đồ họa cao cấp

OpenGL chỉ định một tập hợp các "lệnh" hoặc các hàm phải được thực thingay lập tức Trong đó mỗi lệnh phụ trách một hành động vẽ hoặc tạo ra cáchiệu ứng đặc biệt nào đó Một danh sách các lệnh như vậy có thể được tạo ra đểtạo các hiệu ứng lặp đi lặp lại OpenGL độc lập với các đặc tính của mỗi hệ điềuhành, nhưng cung cấp các quy trình "glue" đặc biệt cho mỗi hệ điều hành, điềunày cho phép OpenGL hoạt động được trong môi trường của hệ thống đó

Công dụng:

Tóm lại, OpenGL được thiết kế nhằm thỏa mãn mục đích chính sau:

- Đơn giản hóa việc tương tác giữa các mô hình không gian 3 chiều bằngmột giao diện lập trình thống nhất

- Hỗ trợ tối đa các chức năng của giao diện OpenGL bằng cách ép buộc cácphần cứng 3 chiều khác nhau phải tương thích Ngay cả khi không thể ép phầncứng hỗ trợ hoàn toàn, OpenGL có thể yêu cầu hệ thống sử dụng thêm sức mạnhphần mềm để xử lý

- Tiêu chuẩn OpenGL nhận các nguyên hàm hình học như điểm, đườngthẳng và đa giác rồi chuyển thành các điểm đồ họa (pixel) trên màn hình Quátrình này được thực hiện thông qua luồng ống dẫn đồ họa (thuật ngữ graphicspipeline) Một tên gọi khác của OpenGL cũng được giới kỹ thuật chia sẻ đó là

bộ máy trạng thái OpenGL

Quá trình nhập file STL từ Solidworks sang môi trường OpenGl:

 Khâu 1(Thân robot)

 Vào tích chọn để kiểm tra hệ tọa độ khâu O sao với bảng D-H

 Nếu chưa đúng hệ tọa độ D-H, ta đặt lại gốc tọa độ của khâu 0

Kích chuột trái vào mặt phẳng cần đặt gốc tọa độ, chọn biểu tượng

Sketch

Tại Sketch đã chọn ta tạo một điểm để đặt gốc tọa độ

 Hộp thoại Move/Copy Body xuất hiện, ta chọn toàn bộ khối để di chuyển

 Hộp thoại Move/Copy Body xuất hiện, ta chọn toàn bộ khối để di chuyển

 Hộp thoại Coordinate system xuất hiện, ở phần selections ta điều chỉnh

các trục theo hệ tọa độ D-H

 Kích chọn để hoàn thành việc đặt hệ trục tọa độ cho khâu 0

 Khâu 1

 Khâu 2

 Khâu 3

 Khâu 4

Xuất file STL

Sau khi thiết kế mô hình robot và đặt hệ trục tọa độ cho robot xong, để có thể

mô phỏng được robot trong visual c++ 6.0 với thư viện OpenGL, ta phải chuyểnđịnh dạng sang đuôi stl

 Để lưu file dưới dạng stl, ta vào file chọn save as và chọn định dạng

STL

 Để hệ trục khi đặt không bị thay đổi khi xuất file stl, ở mục Options… ta

tích chọn vào Do not translate stl output data to positive space.