Đề tài: Đồ án bài tập mô phỏng hệ cơ điện tử, HAY, 9đ

Bài tập lớn mô phỏng hệ cơ điện tử: Nhóm1 1
MỤC LỤC
MỤC LỤC ..................................................................................................... 1
THÀNH VIÊN VÀ PHÂN CÔNG CÔNG VIỆC:............................................ 3
CHƯƠNG 1: PHÂN TÍCH, THIẾT KẾ CẤU TRÚC 3D ................................11
1.1 Phân tích cấu trúc................................................................................. 11
1.2 Thiết kế 3D Robot trên Solidworks....................................................... 13
1.2.1 Thiết kế từng khâu và đặt hệ trục. ..................................................... 13
1.2.2 Lắm ghép và xuất file. ...................................................................... 21
CHƯƠNG 2: PHẦN ĐỘNG HỌC .................................................................23
2.1 Động học thuận..................................................................................... 23
2.1.1 Tình các phương trình động học cần thiết. ...................................... 23
2.1.2 Phương trình quỹ đạo điểm cuối, các vận tốc, gia tốc...................... 25
2.1.3 Mô phỏng động học thuận.............................................................. 30
2.2 Động học ngược................................................................................. 46
2.2.1 Giải bàitoán động học ngược bằng phương pháp số (Newton-raphson).
46
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............ 51
2.2.3 Kiểm tra tính chính xác của bài toán ngược:................................... 56
CHƯƠNG 3: THIẾT KẾ QUỸ ĐẠO .............................................................59
3.1 Thiết kê quỹ đạo hình học đơn giản:................................................. 59
3.2 Thiết kế quỹ đạo đi qua 5 điểm:............................................................ 60
CHƯƠNG 4: MÔ PHỎNG OPEN GL............................................................70
4.1 Giới thiệu chung về OpenGL................................................................ 70

4.2 Mô phỏng quỹ đạo đường tròn:............................................................ 87
CHƯƠNG 5: ĐỘNG LỰC HỌC....................................................................88
5.1 Động lực học tổng quát ......................................................................... 88
5.3. Mô phỏng số bài toán động lực học ngược:.......................................... 92
5.4 Mô phỏng bài toán động lực học thuận: ............................................... 98
CHƯƠNG 6: THIẾT KẾ BỘ ĐIỀU KHIỂN.................................................107
6.1 Chọn luật điều khiển PID tổng quát ................................................... 107
6.2 Mô phỏng PID bằng Simulink ............................................................ 109
CHƯƠNG 7: KẾT QUẢ VÀ ĐỒ THỊ..........................................................126
7.1 Mô tả vị trí các code tính toán............................................................. 137
7.2 Phần động học tổng quát : .................................................................. 138
7.3 Động học thuận................................................................................... 140
7.4 Động học ngược................................................................................... 147
7.5 Thiết kế quỹ đạo.................................................................................. 153
7.6 Động lực học ngược............................................................................. 155
PHỤ LỤC ...................................................................................................158

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.
Họ và tên MSSV Phân công Đánh giá.
Phạm Thanh Tú 20156794 Động học (thuận ngược
code maple, matlab) +
động lực học (tổng
quát+ngược) + thiết kế quỹ
đạo
10/10
Tích cực
Nguyễn Trọng Huy 20155706 3d + xuất stl + hỗ trợ mô
phỏng Simulink (sửa các
lỗi xuất, trục, body), open
GL
10/10
Tích cực
Nguyễn Tiến Lợi 20155993 Động học (thuận, code
maple) + động lực học( hỗ
trợ sửa lỗi, sửa code)
10/10
Tích cực
Bùi Văn Trường 3d+Mô phỏng Open GL+
PID+ động lực học ( tham
gia hỗ trợ giải, sửa lỗi)
10/10
Tích cực
Nguyễn Mạnh Thắng 20156502 3d+Mô phỏng Simulink
+PID +động lực học (viết
khối simulink +
thuận+ngược)
10/10
Tích cực
Đỗ văn Đức 20155389 PID + Hỗ trợ mô phỏng
Simulink + hỗ trợ sửa lỗi
code.
10/10
Tích cực

TIẾN ĐỘ HOÀN THÀNH VÀ CHƯA HOÀN THÀNH
STT Mục Công việc Hoàn thành
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ì) Yes
Ngược Mô phỏng số đường thẳng Yes
Mô phỏng số tròn Yes
Kiểm tra bài toán ở mức vận tốc Yes
2 Thiết kế
quỹ đạo
Đơn
giản
Quỹ đạo tròn thẳng theo thời
gian
Yes
5 điểm Thiết kế ra quy luật khớp, vận
tốc
Yes
3 Động lực Tổng
quát
Tính các ma trận khối lượng,
ma trận C, các phương trình vi
phân
Yes
Ngược Tính được các mô men đối vơi
dạng quỹ đạo thẳng, tròn
Yes

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
4 PID Thiết kế Thiết kế xong PID 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
5 Mô
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
Hoàn thành báo cáo No

DANH MỤC HÌNH ẢNH
Hình 1 Mô hình Robot và hệ trục Dh tương ứng .........................................11
Hình 2 Minh họa về phương pháp đặt hệ trục D-H:.....................................12
Hình 3 Khâu đế .........................................................................................16
Hình 4 Cách đặt Hệ trục khâu đế................................................................16
Hình 5 Khâu 1...........................................................................................17
Hình 6 Cách đặt hệ trục khâu 1 ..................................................................17
Hình 7 Khâu 2...........................................................................................18
Hình 8 Cách đặt hệ trục khâu tịnh tiến ( khâu 2)..........................................18
Hình 9 Khâu 3...........................................................................................19
Hình 10 Cách đặt hệ trục khâu quay (Khâu 3).............................................19
Hình 11 Khâu 4.........................................................................................20
Hình 12 Cách đặt hệ trục khâu quay (khâu 4)..............................................20
Hình 13 Miền làm việc theo phương X-Y...................................................28
Hình 14 Miền làm việc trong không gian Y-Z.............................................29
Hình 15 Miền làm việc trong không gian X-Z.............................................29
Hình 16 Miền làm việc vẽ trong không gian 3D ..........................................30
Hình 17 Quỹ đạo điểm cuối bài toán thuận kiểm tra ....................................32
Hình 18 Quỹ đạo mô phỏng thuận ..............................................................33
Hình 19 Đồ thị vận tốc, gia tốc điểm E theo phương x.................................34
Hình 20 Đồ thị vận tốc, gia tốc điểm E theo phương y.................................34
Hình 21 Đồ thị vận tốc gia tốc điểm E theo phương z..................................34
Hình 22 Đồ thị vận tốc, gia tốc khối tâm khâu 1 theo x................................35
Hình 23 Đồ thị vận tốc, gia tốc khối tâm khâu 1 theo y ...............................36
Hình 24 Đồ thị vận tốc, gia tốc khối tâm khâu 1 theo z................................36
Hình 25 Đồ thị vận tốc, gia tốc khối tâm khâu 2 theo x................................37

Hình 28 Đồ thị vận tốc, gia tốc khối tâm khâu 3 theo x ...............................38
Hình 31 Đồ thị vận tốc, gia tốc khối tâm khâu 3 theo x ...............................40
Hình 34 Vận tốc gia tốc góc khâu 1 theo x,y,z ............................................42
Hình 38 Quỹ đại điểm cuối theo quy luật cho trước.....................................47
Hình 39 Đồ thị kiểm tra .............................................................................48
Hình 40 Vẽ hai đồ thị trên cùng một miền ..................................................48
Hình 41 Đồ thị quy luật cho trước ..............................................................49
Hình 42 Đồ thị kiểm tra .............................................................................50
Hình 43 Vẽ hai đồ thị trên cùng một miền ..................................................51
Hình 44 Đồ thị q1......................................................................................52
Hình 45 Đồ thị q2......................................................................................53
Hình 46 Đồ thị q3......................................................................................54
Hình 47 Đồ thị q4......................................................................................55
Hình 48 Đồ thị vận tốc thực và vận tốc kiểm tra phương x ..........................56
Hình 49 Đồ thị vận tốc thực và vận tốc kiểm tra phương y ..........................57
Hình 50 Đồ thị vận tốc thực và vận tốc kiểm tra phương z...........................57
Hình 51 Quỹ đạo đoạn 1............................................................................63
Hình 52 Quỹ đạo đoạn 2............................................................................64

Hình 53 Quỹ đạo đoạn 3............................................................................64
Hình 54 Quỹ đạo đoạn 4............................................................................65
Hình 55 Quỹ đọa đi qua 5 điểm..................................................................65
Hình 56 Đồ thị vận tốc theo phương x........................................................67
Hình 57 Đồ thị vận tốc theo phương y........................................................68
Hình 58 Đồ thị vận tốc theo phương z ........................................................69
Hình 59 Mô phỏng quỹ đạo tròn.................................................................87
Hình 60 Mô hình động học Robotác thông số đầu vào:................................89
Hình 61 Mô tả sumlink giải động học ngược...............................................93
Hình 62 Đồ thị MoMen khâu 1...................................................................93
Hình 63 Đồ thị Lực khâu 2.........................................................................94
Hình 64 Đồ thị Momen khâu 3...................................................................94
Hình 65 Đồ thị Momen khâu 4 ...................................................................95
Hình 66 Đồ thị Momen khớp 1...................................................................95
Hình 67 Đồ thị Lực khớp 2 ........................................................................96
Hình 70 Mô tả simulink giải động học thuận...............................................98
Hình 71 Đồ thị tính toán và so sánh q1.....................................................103
Hình 75 Mô hình điều khiển tổng quát......................................................107
Hình 76 Mô hình điều khiển PID thiết kế Thiết kế: .................................107
Hình 77 Toàn bộ sơ đồ PID......................................................................109
Hình 78 Tín hiệu đặt................................................................................110
Hình 79 Khối tính Mô men, Lực ..............................................................110

Hình 80 Khối tính các biến khớp thực tế theo lực, mô men điều khiển .......111
Hình 81 Khối so sánh lực thực tế và tính toán...........................................111
Hình 82 Khối PID....................................................................................111
Hình 83 quỹ đạo điểm cuối bài toán thuận theo quỹ đạo tùy ý bên trên. .....141
Hình 84 Đồ thị vận tốc gia tốc điểm cuối..................................................142
Hình 85 Vận tốc gia tốc góc khâu 1 theo x,y,z ..........................................143
Hình 89 Kiểm tra quỹ đạo thẳng...............................................................147
Hình 90 Kiểm tra quỹ đạo tròn.................................................................148
Hình 91 Kiểm tra khớp 1 .........................................................................149
Hình 95 kiểm tra vận tốc phương x...........................................................151
Hình 96 kiểm tra vận tốc phương y...........................................................151
Hình 97 Kiểm tra vận tốc phương z..........................................................152
Hình 98 Đồ thị quỹ đạo và các điểm đã chọn............................................154

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 :
Theo DH tại mỗi khớp của robot đặt một hệ chục tọa độ quy ước về việc đặt
hệ trục tọa độ này như sau:
-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 songxi 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:
 : khoảng cách Oi-1 và Oi theo trục zi-1.
 : góc giữa 2 đường vuông góc chung. Là góc quay quanh trục zi-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 zi 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.
id
i
i
ia

Lặp được bảng DH sau:
Các biến khớp:𝑞 = [ 𝜃1 , 𝜃2 , 𝑑3 , 𝜃3] = [ 𝑞1 , 𝑞2 , 𝑞3 , 𝑞4]
Các thông số động học thiết kế:
𝑎1 =, 𝑎2 = 400, 𝑎3 = 400 (𝑚𝑚)
𝑑1 = 600 (𝑚𝑚)
Khâu Θ d a α
1 𝑞1 𝑑1 𝑎1 0
2 0 𝑞2 0 900
3 𝑞3 0 𝑎3 0
4 𝑞4 0 𝑎4 0
1.2 Thiết kế 3D Robottrên Solidworks.
1.2.1 Thiếtkế từng khâu và đặthệ trục.
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: 𝑂0 𝑥0 𝑦0 𝑧0
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: 𝑂1 𝑥1 𝑦1 𝑧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: 𝑂2 𝑥2 𝑦2 𝑧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 : 𝑂3 𝑥3 𝑦3 𝑧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: 𝑂4 𝑥4 𝑦4 𝑧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ấtfile.
- 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 𝐴𝑖
𝑖−1
từ hệ 𝑖 − 1 sang hệ 𝑖.
Tính ma trận chuyển từ hệ 0 sang hệ 𝑖 bằng công thức:
𝑇4
0
= 𝑇1
0
. 𝑇2
1
. 𝑇3
2
. 𝑇4
3
Ma trận biến đổi tọa độ thuần nhất Denavit – Hartenberg:
𝑇𝑖
𝑖−1
= [
cos( 𝜃) − sin( 𝜃). cos( 𝛼) sin( 𝜃)sin( 𝛼) 𝑎. cos( 𝜃)
sin( 𝜃) cos( 𝜃)cos( 𝛼) − cos( 𝜃)sin( 𝛼) 𝑎. sin( 𝜃)
0 sin( 𝛼) cos( 𝛼) 𝑑
0 0 0 1
] (2.1)
Ta quy ước như sau :
cos(rotx)= 𝑐 𝑥 sin(rotx) = 𝑠 𝑥
cos(roty)= 𝑐 𝑦 sin(roty) = 𝑠 𝑦

cos(rotz)= 𝑐 𝑧 sin(rotz) = 𝑠𝑧
C = [
𝑐 𝑦 𝑐 𝑧
−𝑐 𝑦 𝑠𝑧 𝑠 𝑦 𝑥 𝐸
𝑠 𝑥 𝑠 𝑦 𝑐 𝑧 + 𝑐 𝑥 𝑠𝑧
−𝑐 𝑥 𝑠 𝑦 𝑐 𝑧 + 𝑠 𝑥 𝑠𝑧
−𝑠 𝑥 𝑠 𝑦 𝑠𝑧 + 𝑐 𝑥 𝑐𝑧
𝑐 𝑥 𝑠 𝑦 𝑠𝑧 + 𝑠 𝑥 𝑐 𝑧
−𝑠 𝑥 𝑐 𝑦
𝑐 𝑥 𝑐 𝑦
𝑦𝐸
𝑧 𝐸
0 0 0 1
] (2.2)
Từ bảng D-H lập được thông qua tính toán maple ta tính được ma trận thuần
nhất các khâu:
Quy ước:
sin( 𝑞1) = 𝑠1,sin( 𝑞2) = 𝑠2,sin( 𝑞4) = 𝑠4
cos( 𝑞1) = 𝑐1,cos( 𝑞2) = 𝑐2,cos( 𝑞4) = 𝑐4
Khâu 0 sang khâu 1:
𝑇1
0
= [
𝑐1 −𝑠1 0 𝑎1 ∗ 𝑐1
𝑠1 𝑐1 0 𝑎1 ∗ 𝑠1
0
0
0 1
0 0
𝑑1
1
] (2.3)
𝑇2
1
= [
1 0 0 0
0 0 − 1 0
0
0
1 0
0 0
𝑞2
1
] (2.4)
𝑇3
2
= [
𝑐3 −𝑠3 0 𝑎3 ∗ 𝑐3
𝑠3 𝑐3 0 𝑎3 ∗ 𝑠3
0
0
0 1
0 0
0
1
] (2.5)
𝑇4
3
= [
𝑐4 −𝑠4 0 𝑎4 ∗ 𝑐4
𝑠4 𝑐4 0 𝑎4 ∗ 𝑠4
0
0
0 1
0 0
0
1
] (2.6)
Ma trận chuyển từ khâu 0 sang khâu 4 được tính theo công thức:
𝐴4
0
= 𝐴1
0
∗ 𝐴2
1
∗ 𝐴3
2
∗ 𝐴4
3
(2.7)

Note: Kết quả chi tiết ở phụ lục
Lập các phương trình động học:
𝑓1 = 𝑇4
0[1,4] − 𝐶[1,4]
𝑓2 = 𝑇4
0[2,4] − 𝐶[2,4]
𝑓3 = 𝑇4
0[3,4] − 𝐶[3,4]
𝑓4 = 𝑇4
0[3,3] − 𝐶[3,3]
𝑓5 = 𝑇4
0[1,3] − 𝐶[1,3]
𝑓6 = 𝑇4
0[1,2] − 𝐶[1,2]
𝑓7 = 𝑇4
0[1,1] − 𝐶[1,1]
𝑓8 = 𝑇4
0[2,1] − 𝐶[2,1]
𝑓9 = 𝑇4
0[2,2] − 𝑐[2,2]
𝑓10 = 𝑇4
0[2,3] − 𝐶[2,3]
𝑓11 = 𝑇4
0[3,1] − 𝐶[3,1]
𝑓12 = 𝑇4
0[3,2] − 𝐶[3,2]
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ình
lập được ở mục 2.1.1
𝑓1 = 𝑇4
0[1,4] − 𝐶[1,4]
𝑓2 = 𝑇4
0[2,4] − 𝐶[2,4]
𝑓3 = 𝑇4
0[3,4] − 𝐶[3,4]
Giải được tọa độ điểm cuối như sau:
𝑥 𝐸 = cos( 𝑞1). ((𝑎4 ∗ cos( 𝑞4) + 𝑎3) ∗ cos( 𝑞3) − 𝑠𝑖𝑛(𝑞4) ∗ sin(𝑞3) + 𝑎1)
𝑦𝐸 = sin( 𝑞1). ( 𝑎4 ∗ cos( 𝑞4) ∗ cos( 𝑞3) − 𝑎4 ∗ sin( 𝑞4) ∗ sin( 𝑞3) + 𝑎3∗cos(𝑞3)
+ 𝑎1)

𝑧 𝐸 = ((𝑎4 ∗ cos( 𝑞4) + 𝑎3) ∗ sin( 𝑞3) − 𝑠𝑖𝑛(𝑞4) ∗ cos(𝑞3)+ 𝑞2 + 𝑑1
Vận tốc điểm cuối:
Vận tốc điểm E: 𝑣𝑒 = 𝑞̇ 𝐸 = {
𝑥̇ 𝐸
𝑦̇ 𝐸
𝑧̇ 𝐸
Gia tốc điểm cuối:
Gia tốc điểm E: 𝑎 𝐸 = 𝑣̇ 𝐸 = {
𝑣̇ 𝑥𝐸
𝑣̇ 𝑦𝐸
𝑣̇ 𝑧𝐸
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ố
liệu cụ thể trích dẫn từ solidworks)
khâu1:
𝑢1 = [
−𝐿1𝑥
−𝐿1𝑦
−𝐿1𝑧
1
] = [
−315
0
−355
1
](𝑚𝑚)
khâu2:
𝑢2 = [
−𝐿2𝑥
−𝐿2𝑦
−𝐿2𝑧
1
][
0
0
−211
1
](𝑚𝑚)
khâu 3:
𝑢3 = [
−𝐿3𝑥
−𝐿3𝑦
−𝐿3𝑧
1
][
−197
0
0
1
](𝑚𝑚)
khâu 4:
𝑢4 = [
−𝐿4𝑥
−𝐿4𝑦
−𝐿4𝑧
1
][
−222
0
0
1
](𝑚𝑚)
Tọa độ khối tâm khâu trong hệ cơ sở được tính:

𝑟𝐶𝑖
0
= 𝑇𝑖
0
∗ 𝑢𝑖
𝑟𝐶1
0
= [
−258 ∗ cos(𝑞1)
−258 ∗ sin(𝑞1)
245
1
]
𝑟𝐶2
0
= [
−211∗ sin( 𝑞1) + 30 ∗ cos(𝑞1)
211∗ sin( 𝑞1)+ 30∗ cos(𝑞1))
600 + 𝑞2
1
]
𝑟𝐶3
0
= [
203 ∗ cos( 𝑞1)cos(𝑞3)+ 30∗ cos(𝑞1)
211 ∗ sin( 𝑞1) ∗ (203 ∗ cos(𝑞3) + 30 ∗ cos(𝑞1))
203 ∗ sin( 𝑞3)+ 600 + 𝑞2
1
]
𝑟𝐶4
0
=
[
2 ∗ cos( 𝑞1) ∗ (89 ∗ cos( 𝑞3) ∗ cos( 𝑞4) − 89 ∗ sin( 𝑞3) ∗ sin( 𝑞4) + 200∗ cos( 𝑞3) + 15)
2 ∗ sin( 𝑞1) ∗ (89 ∗ cos( 𝑞3) ∗ cos( 𝑞4) − 89 ∗ sin( 𝑞3) ∗ sin( 𝑞4) + 200∗ cos( 𝑞3) + 15)
(178 ∗ cos( 𝑞4) + 400) ∗ sin( 𝑞3) + 178 ∗ cos( 𝑞3) ∗ sin( 𝑞4) + 𝑞2 + 600
1
]
Vận tốc khối tâm khâu-ma trận jacobi tịnh tiến khối tâm khâu
𝑣𝑖 = 𝑟̇ 𝐶𝑖
0
= {
𝑥̇ 𝐶𝑖
𝑦̇ 𝐶𝑖
𝑧̇ 𝐶𝑖
Ji =
∂rGi
∂qj
=
[
∂xGi
∂q1
⋯
∂xGi
∂qj
∂yGi
∂q1
⋱
∂yGi
∂qj
∂zGi
∂q1
⋯
∂zGi
∂qj ]
Note: kết quả chi tết ở phụ lục
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ền
làm việc:

- Giới hạn các biến khớp:
𝑞1 ( −
𝜋
2
÷
𝜋
2
)
𝑞2 (50 ÷ 350)( 𝑚𝑚)
𝑞3 (−
𝜋
2
÷
𝜋
2
)
𝑞4 (−
𝜋
3
÷
𝜋
3
)
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)
𝑞1 = 0
𝑞2 = 50
𝑞3 =
2
3
∗ 𝜋 ∗ sin(
𝜋
12
∗ 𝑡) 𝑡 = 0 thì 𝑞3 = 0 𝑡 = 2 thì 𝑞3 =
𝜋
3
𝑞4 = −
2
3
∗ 𝜋 ∗ sin(
𝜋
12
∗ 𝑡) 𝑡 = 2 thì 𝑞4 = 0𝑡 = 2 thì 𝑞4 = −
𝜋
3
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 𝑡 = 0 thì [𝑞1, 𝑞2, 𝑞3, 𝑞4] = [0,50,0, 0]

𝑈𝐴 {
𝑥 𝐸 = 𝑎1 + 𝑎3 + 𝑎4 = 350+ 400 + 400 = 1150 (𝑚𝑚)
𝑦𝐸 = 0 (𝑚𝑚)
𝑧 𝐸 = 𝑑1 + 𝑞2 = 770+ 50 = 810 (𝑚𝑚)
- Tại 𝑡 = 2 ( 𝑠) thì [𝑞1, 𝑞2, 𝑞3, 𝑞4] = [0,50,
𝜋
3
,
𝜋
3
]
𝑈 𝐵 =
{
𝑥 𝐸 = 𝑎1 + 𝑎3 ∗ 𝑐𝑜𝑠(𝑞3) + 𝑎4 = 350 + 400 ∗ cos(
𝜋
3
) + 400 = 950 (𝑚𝑚)
𝑦𝐸 = 0 (𝑚𝑚)
𝑧 𝐸 = 𝑑1 + 𝑞2 + 𝑎3 ∗ sin( 𝑞3) = 770+ 50 + 400∗ sin(
𝜋
3
) = 1166 (𝑚𝑚)

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:
𝑡 = 0 ∶ 𝑈𝐴𝑠ố {
𝑥 𝐸 = 1150 (𝑚𝑚)
𝑦𝐸 = 0 (𝑚𝑚)
𝑧 𝐸 = 810 (𝑚𝑚)
𝑡 = 2 ∶ 𝑈 𝐵𝑠ố {
𝑥 𝐸 = 950 (𝑚𝑚)
𝑦𝐸 = 0 (𝑚𝑚)
𝑧 𝐸 = 1166 (𝑚𝑚)
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ì:
𝑞1 = 𝜋 ∗ sin(
𝜋
60
∗ 𝑡)
𝑞2 = 200
𝑞3 = 𝜋 ∗ sin(
𝜋
60
∗ 𝑡)
𝑞4 = 𝜋 ∗ sin(
𝜋
60
∗ 𝑡)
𝑡 = [0:0.04:4]

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 19 Đồ thị vận tốc, gia tốc điểm E theo phương x
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 23 Đồ thị vận tốc, gia tốc khối tâm khâu 1 theo y
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 27 Đồ thị vận tốc, gia tốc khối tâm khâu 2 theo z
Khâu 3
Hình 28 Đồ thị vận tốc, gia tốc khối tâm khâu 3 theo x

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

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

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

Khâu 3:

Khâu 4:

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ình giải góc Cardan (rotx roty rotz) mà ta đã chọn. Tính các đạo hàm của 3
phương trình trên thực hiện đáp ứng ta tính được 𝑥1 là 1 xấp xỉ tốt hơn 𝑥0:
𝑥1 = 𝑥0 −
𝑓( 𝑥0)
𝑓′( 𝑥0)
. (2.9)
𝑥0 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ỉ :
𝑥 𝑛 = 𝑥 𝑛−1 −
𝑓( 𝑥 𝑛−1)
𝑓′( 𝑥 𝑛−1)
. (2.10)
Ta chọn các giải các phương trình:
𝑓1 = 𝑇4
0[1,4] − 𝐶[1,4]
𝑓2 = 𝑇4
0[2,4] − 𝐶[2,4]
𝑓3 = 𝑇4
0[3,4] − 𝐶[3,4]
𝑓7 = 𝑇4
0[1,1] − 𝐶[1,1]
Mô phỏng số bài toán ngược (đường thẳng ):
Quỹ đạo đặt:
𝑥 𝐸𝑡 = 50 ∗ 𝑡 + 600.

𝑦𝐸𝑡 = 0.
𝑧 𝐸𝑡 = 100∗ t + 300.
𝑟𝑜𝑡 𝑧 =
1
3
𝜋
Bộ nghiệm đầu: thay 𝑡 = 0 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ến
khớp tương ứng, thay lại vào bài toán thuận để vẽ đồ thị quỹ đạo điểm
cuối.

Hình 39 Đồ thị kiểm tra
Lắp trùng 2 đồ thị này trên một miền duy nhất ta thấy trùng nhau nên kết quả
giải là chính xác
Hình 40 Vẽ hai đồ thị trên cùng một miền

Mô phỏng số bài toán ngược (đường tròn):
𝑥 𝐸𝑡 = 100 ∗ cos (
1
5
𝜋𝑡) + 400.
𝑦𝐸𝑡 = 0.
𝑧 𝐸𝑡 = 100∗ cos (
1
5
𝜋𝑡) + 1000..
𝑟𝑜𝑡 𝑧 =
1
3
𝜋
Bộ nghiệm đầu: [𝑞1, 𝑞2, 𝑞3, 𝑞4] = [0,147.22008,−1.1863995,.13920200]
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ến
khớp tương ứng, thay lại vào bài toán thuận để vẽ đồ thị quỹ đạo điểm
cuố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ột miền
.

Hình 43 Vẽ hai đồ thị trên cùng một miền
Kết luận:Bàitoá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àitoá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ị 𝑞1 đổi chiều thì vận tốc 𝑣 𝑞1 = 0.
- Tại các điểm 𝐶 trên đồ thị khi đồ thị 𝑣 𝑞1 đổi chiều thì vận tốc 𝑎 𝑞1 = 0.
b) Đồ thị đối với 𝒒 𝟐

Kiểm tra:
- Tại 𝐴, 𝐵, Đồ thị 𝑞2 đặt cực trị ( đổichiều tăng giảm) thì 𝑣 𝑞2 = 0.
- Tại 𝐶 Đồ thị 𝑣 𝑞2 đặt cực trị ( đổi chiều tăng giảm) thì 𝑎 𝑞2 = 0.

c) Đồ thị đối với 𝐪 𝟑
Kiểm tra:
- Tại 𝐴 Đồ thị 𝑞3 đặt cực trị ( đổichiều tăng giảm) thì 𝑣 𝑞3 = 0.
- Tại 𝐵, 𝐶 Đồ thị 𝑣 𝑞3 đặt cực trị ( đổi chiều tăng giảm) thì 𝑎 𝑞3 = 0.
Kết luận:Bài toán ngược giải ra 𝑞3 đúng.

d) Đồ thị đối với 𝒒 𝟒
Kiểm tra:
- Tại 𝐴 Đồ thị 𝑞4 đặt cực trị ( đổichiều tăng giảm) thì 𝑣 𝑞4 = 0.
- Tại 𝐵, 𝐶 Đồ thị 𝑣 𝑞4 đặt cực trị ( đổi chiều tăng giảm) thì 𝑎 𝑞4 = 0.
Kết luận:Bài toán ngược giải ra 𝑞4 đúng.

2.2.3 Kiểm tra tính chính xáccủ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:
vE = ∑
duE
dqi
n
i=1
∗ q̇
- 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
Hình 50 Đồ thị vận tốc thực và vận tốc kiểm tra phương z

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
toán động lực học ngược.
Quỹ đạo hình học đơn giản làm đầu vào kiểm tra mô phỏng bài toán nghịch, ta
chỉ cần tìm các tọa độ điểm E là hàm theo thời gian.
Đường thẳng đi qua 2 điểm: A ( 600,0 , 300)và B(900, 0, 900) chuyển động
chỉ trong mặt Oxz.
Phương trình đường thẳng đi qua A, B: 𝑧 = 2𝑥 + 9.
Chiếu lên 2 trục : 𝑥 = 𝑎1 ∗ 𝑡 + 𝑏1 , z = 𝑎2 ∗ 𝑡 + 𝑏2.
Thay các điều kiện: 𝑡 = 0: 𝑥 = 600, 𝑧 = 300. 𝑡 = 6 : 𝑥 = 900, 𝑧 = 900
Được quy luật điểm cuối: 𝑥 𝐸𝑡 = 100∗ 𝑡 + 600; 𝑦𝐸𝑡 = 0; 𝑧 𝐸𝑡 = 100 ∗ t + 300.
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: 𝑥 𝐸𝑡 = 100 ∗ cos( 𝜑) + 600.
𝑦𝐸𝑡 = 100∗ sin( 𝜑) , 𝑧 𝐸𝑡 = 200.
Thay các điều kiện: 𝑡 = 0: 𝑥 = 500, 𝑦 = 0, 𝑧 = 200.
𝑡 = 5: 𝑥 = 700, 𝑦 = 0, 𝑧 = 200.
Được quy luật điểm cuối: 𝑥 𝐸𝑡 = 100∗ cos (
1
5
𝜋𝑡) + 400;

𝑦𝐸𝑡 = 0; 𝑧 𝐸𝑡 = 100∗ sin (
1
5
𝜋𝑡) + 1000.
3.2 Thiết kế quỹ đạo đi qua 5 điểm:
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 𝑁 + 1 = 5 điểm thuộc quỹ đạo cần thiết kế ta sử dụng phương
pháp Spline 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:
𝑁 + 1 = 5 điểm , 𝑘 = 0,1,2,3, 4.
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:
𝑘 𝑢 𝐸(𝑥, 𝑦, 𝑧) 𝑞1[ 𝑘](𝑟𝑎𝑑) 𝑞2[𝑘](𝑚𝑚) 𝑞2[𝑘](𝑟𝑎𝑑) 𝑞4[𝑘](𝑟𝑎𝑑) 𝑡[𝑘](𝑠)
0 600,0,200 0 173.272 −1.4454 0.39827 0
1 650,0,250 0 213.708 −1.3181 0.27091 1
2 670,0,200 0 157.985 −1.2661 0.21890 2
3 700,0,210 0 167.220 −1.1863 0.13920 3
4 720,0,300 0 238.487 −1.1318 0.08463 5
2. Tính các bước thời gian:
ℎ 𝑘 = 𝑡 𝑘+1 − 𝑡 𝑘
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 𝐴−1
. 𝑟 = 𝑚
5. Giải tìm các hệ số theo công thức:
𝑎 𝑘 = 𝑞 𝑘 , 𝑏𝑘 =
𝑞 𝑘+1 − 𝑞 𝑘
ℎ 𝑘
−
ℎ 𝑘
2
. 𝑚 𝑘 −
ℎ 𝑘
6
. (𝑚 𝑘+1 − 𝑚 𝑘 )
𝑐 𝑘 =
𝑚 𝑘
2
𝑑 𝑘 =
1
6.ℎ 𝑘
. (𝑚 𝑘+1 − 𝑚 𝑘 )
- Quỹ đạo có điều khiển vận tốc đầu cuối:
- Vận tốc đặt : 𝑞̇0
𝑖
= [1,1,1,1] 𝑞̇5
𝑖
= [1,1, 1,1]
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 : 1,2,3, 4.
Với 𝑗 là trạng thái biên khớp tại điểm thứ 𝑘 : 0, 1,2,3, 4.
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 : 1,2,3, 4.
Với 𝑗 là trạng thái biên khớp tại điểm thứ 𝑘 : 0, 1,2,3, 4.
Note: kết quả chi tiết ở phụ lục
Đồ thị quỹ đạotừ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):

Đoạn 4 D (700, 0, 210) sang E (720, 0, 300):
Đồ 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:
Kiểm tra:
- Tại các điểm 𝐴, 𝐵, 𝐶 trên đồ thị khi đồ thị 𝑣 𝑞1 đổi chiều thì vận tốc
𝑎 𝑞1 = 0.
− 𝑣10 = 1 𝑣1𝑁 = 1
Kết luận:Kết quả thiết kế đốivới 𝑞1 là đúng.

Vận tốc q2
Hình 56 Đồ thị vận tốc theo phương x
Kiểm tra:
𝑎 𝑞2 = 0.

Vận tốc q3
Hình 57 Đồ thị vận tốc theo phương y
Kiểm tra:
𝑎 𝑞3 = 0.
− 𝑣30 = 1 𝑣3𝑁 = 1

Vận tốc q4
Hình 58 Đồ thị vận tốc theo phương z
Kiểm tra:
𝑎 𝑞4 = 0.
− 𝑣40 = 1 𝑣4𝑁 = 1

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êu
chuẩn kỹ thuật đồ họa được hình thành với mục đích định ra một giao diện lập
trì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ự trong
bấ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ương
thí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ính
khô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 thi
ngay lập tức. Trong đó mỗi lệnh phụ trách một hành động vẽ hoặc tạo ra các
hiệ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ều
hành, nhưng cung cấp các quy trình "glue" đặc biệt cho mỗi hệ điều hành, điều
nà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ằng
mộ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ác
phần cứng 3 chiều khác nhau phải tương thích. Ngay cả khi không thể ép phần

cứ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ạnh
phầ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, đường
thẳ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ữ graphics
pipeline). 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/CopyBody xuất hiện, ta chọn toàn bộ khối để di chuyển
 Hộp thoại Move/CopyBody xuất hiện, ta chọn toàn bộ khối để di chuyển
 Trên thanh công cụ của Solidwork ta chọn mục Reference Geometryrồi
chọn Coordinate system để kiểm tra hướng của các trục x, y và z.

 Hộp thoại Coordinate systemxuấ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 đổikhi xuất file .stl, ở mục Options… ta
tích chọn vào Do not translatestl outputdata to positivespace.
 Để kiểm tra lại chi tiết đã đặt đúng hệ trục tọa độ chưa, ta mở file
ConvertSTL2MBF

 Kích chọn OpenSTL và chọn file STL cần kiểm tra, tích vào Translate
to Orgin để xem hệ gốc của khâu có đúng theo DH không
 Khâu 0
 Khâu 1

 Khâu 2

 Khâu 3
 Khâu 4

Lắp ghép hoành chỉnh robot:
4.2 Khởi tạo code đọc mã STL từ Solidwoks để chạy trong môi trường
OpenGL
Từ các thông số tính toán robot thiết kế ta đặt giới hạn cho các khâu robot
 Khai báo số khớp robot
;num Elements
; 0 1 2 3
 Khai báo số khâu robot
[elements]
B0 B1 B2 B3 B4
T0 T1 T2 T3 T4
;
 Khai báo giới hạn biến của các khớp
;name min max init unit
[parameters]
q1 -3.141592654 3.141592654 0 "rad"
q2 50 400 350 "mm"

q3 -1.570796327 1.047197551 0 "rad"
q4 -1.047197551 1.047197551 0 "rad"
;
 Khai báo thông số của các khâu
[constants]
d1 770 "mm"
a1 350 "mm"
a3 400 "mm"
a4 400 "mm"
;
 Nhập các ma trận biến đổi tọa độ thuần nhất
;Transform matrixes
[T0]
1 0 0 0
0 1 0 0
0 0 1 0
0 0 0 1
[T1]
cos(q1) -sin(q1) 0 a1*cos(q1)
sin(q1) cos(q1) 0 a1*sin(q1)
0 0 1 d1
0 0 0 1
[T2]
cos(q1) 0 sin(q1) a1*cos(q1)
sin(q1) 0 -cos(q1) a1*sin(q1)
0 1 0 q2+d1
0 0 0 1

[T3]
cos(q1)*cos(q3) -cos(q1)*sin(q3) sin(q1) cos(q1)*(a3*cos(q3)+a1)
sin(q1)*cos(q3) -sin(q1)*sin(q3) -cos(q1) sin(q1)*(a3*cos(q3)+a1)
sin(q3) cos(q3) 0 a3*sin(q3)+q2+d1
0 0 0 1
[T4]
;
 Tạo code đọc file STL
;*X0 Y0 Z0 - translation of an element - not used at moment
;filename R
G B
X0 Y0
Z0 phi
theta - X0, Y0, Z0, phi, theta are not used at moment
[B0]
*0 0 0
"file_STLpart1.STL" 1
0 0
0 0
0 0
0
[B1]
*0 0 0
1 0
0 0

0 0
0
[B2]
*0 0 0
1 1
0 0
0 0
0
[B3]
*0 0 0
1 0
0 0
0 0
0
[B4]
*0 0 0
"file_STLpart5.STL"
0 1
1 0
0 0
0 0
;

4.3 Mô phỏng quỹ đạo:
Quỹ đạo tròn:
Hình 59 Mô phỏng quỹ đạo tròn
Quỹ đạothẳng:

CHƯƠNG 5: ĐỘNG LỰC HỌC
5.1 Động lực học tổng quát
Mục đích bài toán:
- Khảo sát các đặc trưng của Robot về lực.
- Bài toán thuận: Khảo sát sự phụ thuộc của luật biến khớp vào luật lực,
mômen động cơ mong muốn.
- Bài toán ngược: Khảo sát sự phụ thuộc của lực, mômen động cơ vào quy
luật biến khớp, quy luật điểm cuối mong muốn.
- Ứng dụng trong thực tế: Từ việc khảo sát bài toán động học giúp thiết kế
động cơ, các bộ hộp số phù hợp, đồng thời sử dụng kết quả cho bộ điều
khiển PID.
Phương pháp: Sử dụng phương pháp Lagrange 2 dạng ma trận.
Các quy ước và giả thiết của bài toán:
Quy ước:
- Tọa độ suy rộng biến khớp: 𝑞 = [𝑞1, 𝑞2, 𝑞3, 𝑞4] = [𝜃1, 𝜃2, 𝑑3, 𝜃4]
𝑞̇ = [𝑞̇1, 𝑞̇2, 𝑞̇3, 𝑞̇4]
- Để điều khiển Robottheo ý muốn tại mỗi khớp động cơ cần sinh ra một lực
(đối với khớp tịnh tiến) và Mô men ( đối với khớp quay) tương ứng ta gọi
lực và Mô men này là 𝑇 ( là một hàm 𝑓( 𝑞)).
- 𝑇 là dạng ma trận 1 cột 𝑛 hang với 𝑛 là số khớp của Robot.
Giả thiết :
- Trong phạm vi của bài toán này t quy định bỏ qua lực cản nhớ, lực ma sát
và các lực cản trở khác.
- Các thông số động lực học được lấy từ thiết kế Solidworks.

Hình 60 Mô hình động học Robotác thông số đầu vào:
Bảng mô tả vị trí khối tâm khâu theo hệ tọa gắn với khâu:
Khâu Tọa độ khối tâm
𝑥 𝑦 𝑧
1 −315 0 −355
2 0 0 −211
3 −197 0 0
4 −222 0 0
Bảng mô tả khôi lượng các khâu:
Khâu Khối lượng
1 375695.81
2 19228.01
3 10274.18
4 4995.68

Bảng mô tả tenxo quán tính các khâu:
Tenxo Khâu 1 2 3 4
𝐼𝑥𝑥 70913537660.51 1266568062.42 15548087.67 22578630.02
𝐼𝑥𝑦 −30308.84 1.43 −13.92 −5.95
𝐼𝑥𝑧 45637713750.71 −9.33 43897838.01 71527771.18
𝐼 𝑦𝑥 −30308.84 1.43 −13.92 −5.95
𝐼 𝑦𝑦 111298403977.68 16546547.34 571327977.53 344907553.76
𝐼 𝑦𝑧 −54365.51 −0.67 27.76 −13.48
𝐼𝑧𝑥 45637713750.71 −9.33 43897838.01 71527771.18
𝐼𝑧𝑦 −54365.51 −0.67 27.76 −13.48
𝐼𝑧𝑧 43741516898.29 1266441336.39 566781188.74 325540277.00
Các bước tính toán:
1. Tọa độ khối tâm trong hệ cơ sở:
𝑟𝐶𝑖
0
= 𝑇𝑖
0
∗ 𝑢𝑖
𝑟𝐶1
0
= [
−258 ∗ cos(𝑞1)
−258 ∗ sin(𝑞1)
245
1
]
𝑟𝐶2
0
= [
−211∗ sin( 𝑞1) + 30 ∗ cos(𝑞1)
211∗ sin( 𝑞1)+ 30∗ cos(𝑞1))
600 + 𝑞2
1
]
𝑟𝐶3
0
= [
203 ∗ cos( 𝑞1)cos(𝑞3)+ 30∗ cos(𝑞1)
211 ∗ sin( 𝑞1) ∗ (203 ∗ cos(𝑞3) + 30 ∗ cos(𝑞1))
203 ∗ sin( 𝑞3)+ 600 + 𝑞2
1
]
𝑟𝐶4
0
=

[
2 ∗ cos( 𝑞1) ∗ (89 ∗ cos( 𝑞3) ∗ cos( 𝑞4) − 89 ∗ sin( 𝑞3) ∗ sin( 𝑞4) + 200∗ cos( 𝑞3) + 15)
2 ∗ sin( 𝑞1) ∗ (89 ∗ cos( 𝑞3) ∗ cos( 𝑞4) − 89 ∗ sin( 𝑞3) ∗ sin( 𝑞4) + 200∗ cos( 𝑞3) + 15)
(178 ∗ cos( 𝑞4) + 400) ∗ sin( 𝑞3) + 178 ∗ cos( 𝑞3) ∗ sin( 𝑞4) + 𝑞2 + 600
1
]
2. Vận tốc khối tâm khâu-ma trận jacobi tịnh tiến khối tâm khâu
𝑣𝑖 = 𝑟̇ 𝐶𝑖
0
= {
𝑥̇ 𝐶𝑖
𝑦̇ 𝐶𝑖
𝑧̇ 𝐶𝑖
3. Tính các Jacobi tịnh tiến:
Ji =
∂rGi
∂qj
=
[
∂xGi
∂q1
⋯
∂xGi
∂qj
∂yGi
∂q1
⋱
∂yGi
∂qj
∂zGi
∂q1
⋯
∂zGi
∂qj ]
4. Tính các ma trận sóng:
𝜔̃1 = 𝐴̇1 ∗ 𝐴1
𝑇
𝜔̃2 = 𝐴̇2 ∗ 𝐴2
𝑇
𝜔̃3 = 𝐴̇3 ∗ 𝐴3
𝑇
𝜔̃4 = 𝐴̇4 ∗ 𝐴4
𝑇
5. Tính các Jacobi quay:
6. Tính ma trận khối lượng M:
7. Tính ma trận Concolin:
8. Tính các phương trình vi phân chuyển động:
𝑻 = 𝑴 ∗ 𝒒̈ + 𝑪 ∗ 𝒒̇ + 𝑮 − 𝑱 𝑬 ∗ 𝑭
Note: Toàn bộ kết quả được trình bày ở phần phụ lục

5.3. Mô phỏng số bài toán động lực học ngược:
Mục đích bài toán:Khảo sát 𝑇 (lực, Mô men) phụ thuộc vào các quy luật biến
khớp và quỹ đạo cho trước.
Đầu vào bài toán:
- Là các phương trình biến khớp theo t hoặc tập giá trị số của các biến khớp
1. Đường tròn:
𝑥 𝐸𝑡 = 100 ∗ cos (
1
5
𝜋𝑡) + 750.
𝑦𝐸𝑡 = 0.
𝑧 𝐸𝑡 = 100∗ sin (
1
5
𝜋𝑡) + 900.
Từ bài toán ngược ta tìm được các luật biến khớp.
Note: trong phụ lục
Từ bài toán ngược ta giải ra các tập giá trị biến khớp theo thời gian.
2. Đường thẳng:
𝑥 𝐸𝑡 = 50 ∗ 𝑡 + 600.
𝑦𝐸𝑡 = 0.
𝑧 𝐸𝑡 = 100 ∗ t + 300
Ta tìm được các phương trình theo thời gian của biến khớp:
𝑞1 = 0;
𝑞2 = 0.09118156680 ∗ 𝑡3
+ − 1.367723502∗ 𝑡2
+ 273.2728582
𝑞3 = 0.0001253046180∗ 𝑡3
− 0.00187956 ∗ 𝑡2
− 1.445468496
𝑞4 = −0.0001253046180 ∗ 𝑡3
+ 0.00187956∗ 𝑡2
+ 0.3982709
- Là các lực và Mô men tương ứng với đầu vào.
𝑻 = 𝑴 ∗ 𝒒̈ + 𝑪 ∗ 𝒒̇ + 𝑮 − 𝑱 𝑬 ∗ 𝑭

Sử dụng Simulinkgiải bàitoán động lực học ngược:
Hình 61 Mô tả sumlink giải động học ngược
Đầu ra bài toán (Đồ thị Lực và Mô men tương ứng):
1. Đường tròn:
Hình 62 Đồ thị MoMen khâu 1

Hình 63 Đồ thị Lực khâu 2
Hình 64 Đồ thị Momen khâu 3

Hình 65 Đồ thị Momen khâu 4
Hình 66 Đồ thị Momen khớp 1

Hình 67 Đồ thị Lực khớp 2

5.4 Mô phỏng bài toán động lực học thuận:
Mục tiêu: Khảo sát thiết lập mối quan hệ đầu vào là quy luật lực điều khiển
mong muốn và đầu và là quy luật biến khớp tương ứng
Đầu vào bài toán:
- Là các quy luật lực điều khiển tìm được ở bài toán ngược mục 5.3 ( lấy
trực tiếp kết quả số của bài toán động học ngược để giải vi phân tìm các
biến khớp).
Dùng Simulink để giải bài toán thuận có và không có PID:
Hình 70 Mô tả simulink giải động học thuận
Đầu ra bài toán: ( Đồ thị các biến khớp tương ứng với quỹ đạođặt tròn thẳng
của bài toán động học ngược):
- Ta tiến hành giải bài toán bằng các khối Simulink bằng 2 cách: có PID và
không có PID so sánh kết quả để lựa chọn các hệ số phù hợp cho bộ điều
khiển.

1. Đồ thị bài toán không dùng PID:
1.Đường thẳng:
Hình 71 đồ thị q 1
Hình 72 Đồ thị q 2

2.Đường tròn:

Nhận xét: Các đồ thị thực tế và tính toán lệch nhau khá lớn do các sai số tính
toán tích phân là lớn nên cần bổ xung bộPID để điều khiển được chính xác.

2. Đồ thị bài toán có dùng PID:
Note: Đường màu đỏ nét liền nhỏ là đồ thị 𝑞𝑖 tính toán được
Đường màu hồng nét đứt là 𝑞𝑖 đặt kí hiệu là 𝑞𝑑𝑖 (𝑞 đặt 𝑖) .
Hình 79 Đồ thị tính toán và so sánh q1 thẳng

2. Đường tròn:
Hình 83 Đồ thị tính toán và so sánh q1 tròn

CHƯƠNG 6: MÔ PHỎNG-THIẾT KẾ BỘ ĐIỀU KHIỂN
6.1 Chọn luật điều khiển PID tổng quát
Khi thiết kế thống điều khiển ta có thể bỏ qua động học của cơ cấu chấp
hành,quán tính của động cơ và bộ biến đổi.Như vậy từ 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
Ta chọn thiết kế hệ theo hệ thống phản hồi trong không gian khớp:
Hình 87 Mô hình điều khiển tổng quát
Hình 88 Mô hình điều khiển PID thiết kế Thiết kế:

Ta có phương trình động lực học :
𝑀( 𝑞) 𝑞̈ + 𝐶( 𝑞, 𝑞̇) 𝑞̇ + 𝑔( 𝑞) = 𝑈 + 𝐽 𝐸
𝑇
𝐹 (5.1)
Gọi H(q)=M(q) tránh nhầm với M là vector momen và 𝑉( 𝑞, 𝑞̇) = 𝐶( 𝑞, 𝑞̇) 𝑞̇ , 𝑀 = 𝑈
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ỹ đạo F = 0 như vậy phương trình động lực học được rút gọn như sau:
𝑀 = 𝐻( 𝑞) 𝑞 + 𝑉( 𝑞, 𝑞̇)̈ + 𝑔( 𝑞) (5.2)
Luật điều khiển :
𝑀 𝑑𝑘 = 𝐾𝑝( 𝑞 𝑑 − 𝑞) + 𝐾 𝑑( 𝑞̇ 𝑑 − 𝑞̇) = 𝐾𝑝 𝜀 + 𝐾 𝑑 𝜀̇ (5.3)
Trong đó : Kp = diag (Kp1, Kp2,….. Kpn) là ma trận đường chéo các hệ số
khuếch đại của từng khớp riêng biệt.
Kd= diag(Kd1, Kd2,….., Kdn) là ma trận đường chéo các hệ số
khuếch đại đạo hàm của từng khớp riêng biệt.
Với luật điều khiển này đã giả thiết thành phần momen trọng lực G(p) đã được
bù hoàn toàn.
Hệ thống điều khiển với cấu trúc bộ điều khiển như trên, ổn định tuyệt đối toàn
cục. Thực vậy chọn hàm Liapunov có dạng như sau:
𝑉̇𝐿 =
1
2
(𝜀̇ 𝑇
𝐾𝑝 𝜀 + 𝜀 𝑇
𝑘 𝑝 𝜀̇ + 𝑞̈ 𝑇
𝐻𝑞̇ + 𝑞̇ 𝑇
𝐻𝑞̇̇ + 𝑞̇ 𝑇
𝐻𝑞̈) (5.4)
Do tính chất đối xứng của các thành phần: 𝜀̇T.Kp. 𝜀 và 𝑞̇T.𝐻̇ . 𝑞̇ ta rút gọn được:
𝑉̇𝐿 = 𝜀̇ 𝑇
𝐾𝑝 𝜀 +
1
2
𝑞̇ 𝑇
𝐻𝑞̇̇ + 𝑞̇ 𝑇
𝐻𝑞̈ (5.5)
Từ phương trình động lực học với giả thiết không có thành phần momen trọng
lực G(q), nhận được phương trình sau :
𝐾𝑝 𝜀 +
1
2
𝑞̇ 𝑇
𝐻𝑞̇̇ + 𝑞̇ 𝑇[ 𝑀 − 𝑉( 𝑞, 𝑞̇)] (5.6)

Sử dụng thuộc tính của phương trình động lực học và áp dụng luật điều khiển
(5.1) ta có:
𝐾𝑝 𝜀 +
1
2
𝑞̇ 𝑇
𝐻𝑞̇̇ − 𝑞̇ 𝑇
𝐶( 𝑞, 𝑞̇) 𝑞̇ (5.7)
Trong đó:
𝑉( 𝑞, 𝑞̇) = 𝐶( 𝑞, 𝑞̇) (5.8)
Do ma
1
2
𝐻̇ -C là ma trận đối xứng ngược suy ra: 𝑞̇T.(
1
2
𝐻̇ -C).𝑞̇=0
𝑉̇𝐿 = −𝑞̇ 𝑇
𝐾 𝑑 𝑞̇ ≤ 0 (5.9)
Từ (6.2) ,(6.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 L phụ 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.
6.2 Mô phỏng PID bằng Simulink
Hình 89 Toàn bộ sơ đồ PID

Khối tín hiệu đặt là các quy luật biến khớp tính từ quỹ đạo ta mong muốn
thông qua bài toán động học ngược.
Hình 90 Tín hiệu đặt
Hình 91 Khối tính Mô men, Lực

Hình 92 Khối tính các biến khớp thực tế theo lực, mô men điều khiển
Hình 93 Khối so sánh lực thực tế và tính toán
Hình 94 Khối PID

6.3 Mô phỏng Robot SIMMECHANICS
6.3.1 Cài đặtSIMMECHANICS:
 Truy cập trang chủ của mathworks -> Chọn phần Simscape Multibody -> Chọn
bản Simmechanics tương ứng với bản matlab.
o Mở phần mềm Matlab (dưới dạng Run as adminnistrastor) > Mở đến phần
chứa file ‘.zip’ vừa tải > Tại cửa sổ command window gõ lệnh
Installs_addon(‘smlink.r2016a.win64.zip’) > Nhấn enter.
o Sau khi hoàn thành các bước trên > Nhấn Smlink_linksw, hoàn thành phần
cài đặt Simmechnics trên matlab.
.
Hình 95 intall vào matab

 Mở phần mềm solid để cài đặt Simmechanics trên solid. Vào tools > Add-ins>
chọn Simmechanics>enter.
6.3.2 Chuyển đổi mô hình từ SOLIDWORKSsang MATLAB
 Mở phần mềm solid lên, chọn vào mô hình muốn thêm > Nhấn vào tools >
Chọn simscape Multibody Link > Chọn simscape Multibody fist Generation
Hình 96 intall vào solidworks

Hình 97 Xuât mô hình
 Nhấn Enter, trên màn hình xuất hiện hình ảnh về tên file chúng ta sẽ lưu file
xuất 3D từ Solids sang simmechanics. Sau đó ấnSave để quá trình xuất mô
hình tiếp tục.
Chú ý lưu file có đuôi là (*.xml).
Hình 98 Đuôi file và lưu trữ
Sau khi quá trình hoàn tất, chúng ta mở phần mềm matlab lên.
 Sau khi mở matlab, vào phần file đã lưu file vừa thêm bên solid lên (ở
đường dẫn) .Ở ô command window gõ mech_import(‘tên file.xml’) để bắt
đầu mở file vừa xuất ra.

Hình 99 Gõ mech_import(‘tên file.xml’) ở phần command window
Hình 100 Sau khi hoàn thành
6.3.3 Mô phỏng:
6.3.3.1Thêm các khối chức năng :
Sau khi xuất được file từ soliowork sang matlab ta mở file đã chuyển đổi bằng
matlab Simulink.

VD: Ở đây đặt tên là đường thẳng.
Ta tiến hành gọi các khối chức năng cần thiết:
1. Nhấn tổ hợp phím Ctrl+shifl+L hoặc chòn như hình hoặc click vào biểu
tượng Library trên thanh công cụ:

2. Trên cửa sổ Simulinks Library browser: nhấp vào ô tìm kiếm và gõ tên các
khối cần thiết sử dụng:
3. Các khối cần sử dụng và chức năng chính:
 Khối rootground: là khối mô tả gốc tọa độ. Nêu vị trí của mô hình trong hệ
tọa độ. Đơn vị có thể là mm , m ,inch …

Hình 101 Khối rootground
 Khối env: khối mô tả gia tốc trong trường > Sửa cho đúng với chiều gốc tọa
độ của robot tại ô Gravity vector.
Hình 102 Khối machine environment.
 Các khối body: Mô tả các khâu, tên của các khối body được đặt theo tên của
các khâu đã đặt trong solidworks

Hình 103 Khối body.
Trong đó :
o Khối CG: Mô tả vị trí của trọng tâm của khâu đối với hệ tọa độ gốc.
o Khối CS2: Mô tả vị trí của điểm kết nối giữa khâu mô tả và khâu trước đó
đối với hệ tọa độ cố định.
o Khối CS3: Mô tả vị trí của điểm kết nối giữa khâu mô tả và khâu sau đó đối
với hệ tọa độ cố định.
o Dưới đây là các khối body của các khâu:
Hình 104 Khối body khâu 1

Hình 105 Khối body khâu thân
Hình 106 Khối body khâu tịnh tiến

Hình 107 Khối body khâu tay quay thứ nhất
Hình 108 Khối body khâu tay quay thứ hai

 Khối Weid: là khớp hàn cứng của khâu đế đối với nền.
Hình 109 Khối weld.
 Khối revolute : Mô tả khớp quay. Trong đó
o Current base: Mô tả khâu cố định: khâu sẽ đứng yên khi khớp chuyển
động.
o Current follower: Mô tả khâu quay: khâu sẽ quay khi khớp chuyển
động.
o Number of sensor /Actuator ports:Mô tả các nhánh được gắn trên
khớp, số lượng này được tự í thêm, người ta thường gắn động cơ và cả
biến vào khớp, nên đây sẽ thêm số lượng các đầu vào đầu ra ở khớp.
Hình 110 Khớp quay.

 Khối prismatic : Khớp tịnh tiến
o Current base : Mô tả khâu cố định : khâu sẽ đứng yên khi khớp chuyển động
.
o Current follower : Mô tả khâu quay : khâu sẽ quay khi khớp chuyển động .
o Number of sensor/actuator ports: Mô tả các nhánh được gắn trên khớp, số
lượng này được tự í thêm, người ta thường gắn động cơ và cả biến vào
khớp, nên đây sẽ thêm số lượng các đầu vào đầu ra ở khớp.
Hình 111 Khớp tịnh tiến
 Các khối chức năng khác:
o Khối joint actuator: mô tả các tín hiệu vận tốc, gia tốc, vị trí trong phần
“actuato with: motion” của các khớp.
- Đơn vị đo: rad, dev, rev,…
- Khối joint actuator thường được gắn vào bên trái của khớp.
Hình 112 Khối joint actuator

o Khối joint sensor: là khối dùng để đo các tín hiệu ra của khớp gồm vị trí,
vận tốc, gia tốc, lực.
Hình 113 Khối joint sensor
o Khối spoce:là khối đo các tín hiệu của các khối sensor.
6.3.3.2Kết nối các khối và chạy mô phỏng:
a) Kết nối các khối
Các khối được đánh số và giải thích cách kết nối và chức năng tương ứng theo
số đánh.

Khái quát về các khối:
1- Khối tín hiệu đặt: Khối chứa các hàm đầu vào và tính toán các tọa độ vận
tốc gia tốc của từng khớp để lấy tính hiệu đó điều khiển các khớp của
Robot.
2- Khối Substeam: chứa các khối tiếp nhận tín hiệu điều khiển và phản hồi.
3- Khối chứa các khâu cố định đầu cuối: Khối này không quan trọng vì chỉ là
gộp các thành phần ko chuyển động vào một khối hoàn toàn có thể để
nguyên như lúc xuất.
Chi tiết kế nối của từng khối
- Khối tín hiệu đặt: (số 1)
4- Khối đặt chạy theo thời gian
5- Khối chứa các hàm của biến khớp theo thời gian ( q, dq, ddq)có thể kết hợp
1 khối giải bài toán ngược để chỉ cần nhập quỹ đạo điểm cuối thì sẽ tự tính
được các q, dq, ddq.
6- Khôi PID là khối t đưa toàn bộ bộ PID đã thiết kế phía trên vào
7- Các đầu ra cho từng khớp sẽ nối với đầu In1 của các khối số 2.

-Khối substeam (số 2):
1- Là điểm tiếp nhận tín hiệu vào nối vơi chân In1 chân này
sẽ nối với chân tín hiệu ra của khối tín hiệu đặt. ( có thể tự đặt tên bằng cách
nháy con chỏ trái vào chữ In1) để đặt theo ý giúp dễ phân biệt các chân.
2- Khối PID Controller (2DOF) : khối này sẽ nhận tín hiệu điều khiển và tín
hiệu phản hồi từ Joint Sensor để điều chỉnh 1 lần nữa giúp hoạt động chính
xác
3- Khối Sensor là khối tác động điều khiển quay theo tín hiệu
4- Conn1: Đầu ra nối với chân nhận tín hiệu của khớp (mô tả chân nhận tính
hiệu điều khiển: )
5- Conn2: Đầu vào nối với chân phẩn hồi từ khớp ( chân đối diện với chân
nhận tín hiệu)
6- Khối Joint Sensor: thu thập tín hiệu phản hồi ( cảm biến).
b) Tiến hành mô phỏng:
Nhập các pt đã tìm được vào khối đầu vào

Hình 114 Khối tín hiệu đặt
Hình 115 Mô hình khối Simulink mô phỏng chạy cho đường thẳng
Hình ảnh chạy môphỏng:
o Đường thẳng:Ấn nút Run trên phần toolbox của simmechanics,
cho thời gian chạy là 15 giây, để quá trình mô phỏng hoạt động
ta thu được kết quả.
Hình 116 Quá trình hoạt động của mô hình Robot

Hình 117 Đồ thị quá trình hoạt động nhìn theo mặt XZ
o Đường tròn: Ấn nút Run trên phần toolbox của simmechanics,
cho thời gian chạy là 15 giây, để quá trình mô phỏng hoạt động
ta thu được kết quả.
Hình 118 Quá trình hoạt động của mô hình Robot

Hình 119Đồ thị quá trình hoạt động nhìn theo mặt XY
Hình 120 Đồ thị quá trình hoạt động nhìn theo mặt XY
6.3.4 Sửa lỗi đặt trục trên Matlab 2018a
 Tìm hiểu Matlab R2018a
o Các bước tải và cài đặt giống như phần Matlab 2016a

 Xuất file từ Soliwork: Mở phần mềm solid lên, chọn vào mô hình muốn
thêm > Nhấn vào tools > Chọn simscape Multibody Link > Export>
simscape Multibody…
Hình 6.3. Cách thêm mô hình từ solid sang matlab.
 Nhấn Enter, trên màn hình xuất hiện hình ảnh về tên file chúng ta sẽ lưu file
xuất 3D từ Solids sang simmechanics. Sau đó ấnSave để quá trình xuất mô
hình tiếp tục.
Chú ý lưu file có đuôi là (*.step; hoặc *.xml)
.
 Sau khi mở matlab, vào phần file đã lưu file vừa thêm bên solid lên (ở
đường dẫn) .Ở ô command window gõ smimport(“tên file.xml”) để bắt đầu
mở file vừa xuất ra

Hình 6.5. Gõ smimport(“tên file.xml”) ở phần command window.
Hình 6.6. Các khối Simulink nhận được sau khi thực hiện lện smimport
 Như vậy chúng ta đã xuất thành công mô hình 3D từ solidwork sang
simmechanics trong matlab. Trong mimmechanis mô hình 3D sẽ được biểu
diễn bằng các khối Simulink.
 Note : phải chạy filematlabtrong thư mụclưu filevừa xuấttừ Soliwork
sang
Các khối chức năng sử dụng trong simmechanics

o Khối env: khối mô tả gia tốc trong trường > Sửa cho đúng với chiều
gốc tọa độ của robottại ô Gravity vector.
Hình 6.8. Khối machine
environment.
o Khối Revolute dung để diển tả đây là khớp quay
Hình 6.. Khối Revolute

 Trong khối Revolute ta click vào mục Actuation chỉnh các gia trị như
hình vẽ, bước này chúng ta đã tạo ra 1 cổng đầu vào cho khớp
Hình 6. Tạo đầu vào cho khối
o Khối Prismatic dung để diển tả đây là khớp tịnh tiến
Hình 6.. Khối Prismatic
 Trong khối Prismatic ta click vào mục Actuation chỉnh các gia trị
như hình vẽ, bước này chúng ta đã tạo ra 1 cổng đầu vào cho khớp

Hình 6. Tạo đầu vào cho khối
o Khối Simulink PS Converter : chuyển tín hiệu vật lí thành tín hiệu điện ,
trong khối này đã được tích hợp tính cho ta vận tốc, gia tốc khi ta cho giá trị
đầu vào là quỹ đạo
Hình 6. Khối
Simulink PS Converter
 Trong khối này ta Click vào phần Input Handing rồi chỉnh lại các giá
trị như hình vẽ

Hình 6. Khối Simulink PS Converter
o Click vào Block than_1_RIGID ta mở được các khối là các hệ
trục tọa độ đóng vai trò làm các hệ trục tọa độ chuyển tại đây ta có thể điều
chỉnh lại vị trí các hệ trục tọa độ theo yêu cầu
Hình 6. Các khối chỉnh hệ trục tọa độ

o Khối Matlab Function
 khối này được dung để truyền các giá trị đầu vào mà ta mong muốn
dưới dạng file .m

CHƯƠNG 7: KẾT QUẢ VÀ ĐỒ THỊ
7.1 Mô tả vị trí các code tính toán.
1. File tính toán các phương trình dạng tổng quát ( dạng chữ)
3. PATH: LẬP TRÌNH MÔ PHỎNG -NHÓM 1-code
4. NAME: code chữ ( động học + động lực học).
Các mục tính toán được đánh số như sau:

2. File tính toán số ( các bộ giá trị và đồ thị)
6. NAME: code số ( động học +động lực học ngược)
3. File tính động lực học thuận ( không có PID )
8. NAME: code động lực học thuận không PID
7.2 Phần động học tổng quát :

1. Bảng DH và các chiều dài động học:
Các biến khớp:𝑞 = [ 𝜃1 , 𝜃2 , 𝑑3 , 𝜃3] = [ 𝑞1 , 𝑞2 , 𝑞3 , 𝑞4]
Các thông số động học thiết kế:
𝑎1 =, 𝑎2 = 400, 𝑎3 = 400 (𝑚𝑚)
𝑑1 = 600 (𝑚𝑚)
Khâu Θ d a α
1 𝑞1 𝑑1 𝑎1 0
2 0 𝑞2 0 900
3 𝑞3 0 𝑎3 0
4 𝑞4 0 𝑎4 0
9. Các phương trình quỹ đạo điểm cuối:

𝑥 𝐸 = cos( 𝑞1). ((𝑎4 ∗ cos( 𝑞4) + 𝑎3) ∗ cos( 𝑞3) − 𝑠𝑖𝑛(𝑞4) ∗ sin(𝑞3) + 𝑎1)
𝑦𝐸 = sin( 𝑞1). ( 𝑎4 ∗ cos( 𝑞4) ∗ cos( 𝑞3) − 𝑎4 ∗ sin( 𝑞4) ∗ sin( 𝑞3) + 𝑎3∗cos(𝑞3)
+ 𝑎1)
𝑧 𝐸 = ((𝑎4 ∗ cos( 𝑞4) + 𝑎3) ∗ sin( 𝑞3) − 𝑠𝑖𝑛(𝑞4) ∗ cos(𝑞3)+ 𝑞2 + 𝑑1
7.3 Độ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):
Đầu vào
𝑞1 = 0 𝑞2 = 50 𝑞3 =
2
3
∗ 𝜋 ∗ sin(
𝜋
12
∗ 𝑡) 𝑞4 = −
2
3
∗ 𝜋 ∗ sin(
𝜋
12
∗ 𝑡)
Đầu ra:
3. Thiết kế mô phỏng theo quỹ đạo bất kì:
Đầu vào:
𝑞1 = 𝜋 ∗ sin(
𝜋
60
∗ 𝑡)
𝑞2 = 200
𝑞3 = 𝜋 ∗ sin(
𝜋
60
∗ 𝑡)

𝑞4 = 𝜋 ∗ sin(
𝜋
60
∗ 𝑡)
𝑡 = [0:0.04:4]
Đầu ra:
1. Quỹ đạo điểm cuối
Hình 121 quỹ đạo điểm cuối bài toán thuận theo quỹ đạo tùy ý bên trên.
2. Vận tốc gia tốc điểm cuối

Hình 122 Đồ thị vận tốc gia tốc điểm cuối
3. Vận tốc góc gia tốc góc các khâu:

Khâu 1:
Khâu 2:

Khâu 3:

Khâu 4:

7.4 Động học ngược
Mô phỏng số bài toán ngược (đường thẳng ):
Đầu vào: Quỹ đạođặt điểm cuối đi theo đường thẳng
𝑥 𝐸𝑡 = 50 ∗ 𝑡 + 600.
𝑦𝐸𝑡 = 0.
𝑧 𝐸𝑡 = 100∗ t + 300.
Đầu ra: Giá trị các biến khớp, thay lại vào phương trình tọa độ khâu cuối vẽ
đồ thị màu xanh. Vẽ cùng đồ thị thực từ quỹ đạo đặt đường chấm màu đỏ để kiểm
tra.
Hình 127 Kiểm tra quỹ đạo thẳng

Mô phỏng số bài toán ngược (đường tròn):
Đầu vào:
𝑥 𝐸𝑡 = 200 ∗ cos (
1
5
𝜋𝑡) + 600.
𝑦𝐸𝑡 = 0.
𝑧 𝐸𝑡 = 200∗ sin (
1
5
𝜋𝑡) + 1000.
𝑟𝑜𝑡 𝑧 =
1
3
𝜋
Đầu ra:
Hình 128 Kiểm tra quỹ đạo tròn

Các biến khớp - vận tốc – gia tốc của mỗi khớp:
Hình 129 Kiểm tra khớp 1

Kiểm tra vận tốc khâu cuối thực và giải ngược:
Hình 133 kiểm tra vận tốc phương x
Hình 134 kiểm tra vận tốc phương y

Hình 135 Kiểm tra vận tốc phương z

7.5 Thiết kế quỹ đạo
Đầu vào: Thiết kế quỹ đạo trơn điều khiển vận tốc đi qua 5 điểm bất kì.
𝑘 𝑢 𝐸(𝑥, 𝑦, 𝑧) 𝑞1[ 𝑘](𝑟𝑎𝑑) 𝑞2[𝑘](𝑚𝑚) 𝑞2[𝑘](𝑟𝑎𝑑) 𝑞4[𝑘](𝑟𝑎𝑑) 𝑡[𝑘](𝑠)
0 600,0,200 0 173.272 −1.4454 . 39827 0
1 650,0,250 0 213.708 −1.3181 . 27091 1
2 670,0,200 0 157.985 −1.2661 . 21890 2
3 700,0,210 0 167.220 −1.1863 . 13920 3
4 720,0,300 0 238.487 −1.1318 . 08463 5
Đầu ra
Phương trình q1:
Phương trình q2
Phương trình q3:

Phương trình q4:
Đồ thị:
Hình 136 Đồ thị quỹ đạo và các điểm đã chọn

7.6 Động lực học ngược
Đầu vào:
1. Đường tròn:
𝑥 𝐸𝑡 = 100 ∗ cos (
1
5
𝜋𝑡) + 750.
𝑦𝐸𝑡 = 0.
𝑧 𝐸𝑡 = 100∗ sin (
1
5
𝜋𝑡) + 900.

𝑥 𝐸𝑡 = 50 ∗ 𝑡 + 600.
𝑦𝐸𝑡 = 0.
𝑧 𝐸𝑡 = 100 ∗ t + 300
Đầu ra:
1. Đường tròn:

7.7 Động lực học thuận:
Đầu vào: Các lực, Mô men tính được từ bài toán động lực học ngược
Bài toán không dùng PID:
Bài toán có dùng PID:
1. Đường thẳng

2. Đường tròn

PHỤ LỤC
0. Các phương trình động học tìm được:

1. Ma trận chuyển đối với khâu cố định

2. Các hàm tọa độ khâu cuối theo thời gian (quy luật bất kì động học thuận):
3. Các vận tốc tốc khâu cuối:

4. Các ma trận khối tâm khâu với hệ cơ sở:

5. Các ma trận Jacobi tịnh tiến khối tâm:

6. Các ma trận sóng:

7. Các ma trận vận tốc góc từng khâu
8. Các ma trận gia tốc góc:
9. Các ma trận Jacobi quay

10. Ma trận khối lượng M:
11. Ma trận C:
12. Các phương trình vi phân chuyển động:

Đề tài: Đồ án bài tập mô phỏng hệ cơ điện tử, HAY, 9đ

Recommended

Recommended

More Related Content

What's hot

What's hot (20)

Similar to Đề tài: Đồ án bài tập mô phỏng hệ cơ điện tử, HAY, 9đ

Similar to Đề tài: Đồ án bài tập mô phỏng hệ cơ điện tử, HAY, 9đ (20)

More from Dịch vụ viết bài trọn gói ZALO: 0909232620

More from Dịch vụ viết bài trọn gói ZALO: 0909232620 (20)

Recently uploaded

Recently uploaded (20)

Đề tài: Đồ án bài tập mô phỏng hệ cơ điện tử, HAY, 9đ