báo-cáo-mô-hình-hóa-nhóm-15.pdf

ĐẠI HỌC BÁCH KHOA HÀ NỘI
TRƯỜNG CƠ KHÍ
BÁO CÁO BÀI TẬP LỚN
MÔN HỌC: MÔ HÌNH HÓA CÁC HỆ CƠ ĐIỆN TỬ
ĐỀ TÀI:
Áp dụng phương pháp mô hình hóa, xây dựng mô hình robot mài phẳng
Giảng viên hướng dẫn: PGS.TS Phan Bùi Khôi
ThS. Nguyễn Văn Quyền
Nhóm sinh viên: Nhóm 15 Mã số sinh viên
1. Trần Việt Dũng 20184408
2. Hoàng Thiên Huệ 20184374
Hà Nội, 8/2022

2
THÀNH VIÊN VÀ PHÂN CÔNG CÔNG VIỆC
Họ và tên MSSV Phân công Đánh giá
Trần Việt Dũng 20184408 Thiết kế 3D
Thiết kế quỹ đạo
Tính toán động học, động lực học
Mô phỏng Robot trên openGL
Lập trình Maple, Matlab và simulink
Viết báo cáo
10/10
Tích cực
Hoàng Thiên Huệ 20184466 Làm báo cáo, làm Slide
Tính toán động lực học, động học thuận,
ngược
Xây dựng mô hình
Tìm hiểu và đưa ra tài liệu tham khảo
9/10
Tích cực

3
Mục lục
CHƯƠNG 1: PHÂN TÍCH, THIẾT KẾ CẤU TRÚC 3D............................... 5
1.1 Phân tích cấu trúc.......................................................................................... 5
1.2 Thiết kế 3D Robot trên Solidworks. ............................................................. 8
1.2.1 Thiết kế từng khâu và đặt hệ trục............................................................. 8
1.2.2 Lắp ghép thành mô hình 3D................................................................... 10
CHƯƠNG 2: PHẦN ĐỘNG HỌC................................................................... 11
2.1 Động học thuận ........................................................................................... 11
2.1.1 Tìm các phương trình động học cần thiết. .......................................... 11
2.1.2 Phương trình quỹ đạo điểm cuối, các vận tốc, gia tốc........................ 13
2.1.3 Mô phỏng động học thuận................................................................... 15
2.2 Động học ngược........................................................................................ 18
CHƯƠNG 3: THIẾT KẾ QUỸ ĐẠO.............................................................. 24
CHƯƠNG 4: MÔ PHỎNG OPEN GL............................................................ 26
4.1 Giới thiệu chung về OpenGL...................................................................... 26
4.2 Mô phỏng quỹ đạo ...................................................................................... 32
CHƯƠNG 5: ĐỘNG LỰC HỌC ..................................................................... 33
5.1 Động lực học tổng quát............................................................................... 33
5.2 Thiết lập phương trình vi phân chuyển động của robot.............................. 38
5.3. Mô phỏng số bài toán động lực học ngược:............................................... 45
5.4 Mô phỏng bài toán động lực học thuận: ..................................................... 48
CHƯƠNG 6: MÔ PHỎNG-THIẾT KẾ BỘ ĐIỀU KHIỂN ......................... 49
6.1 Chọn luật điều khiển PD tổng quát............................................................. 49
6.2 Mô phỏng PID bằng Simulink .................................................................... 52

4
PHỤ LỤC........................................................................................................... 61
KẾT LUẬN ........................................................................................................ 63
TÀI LIỆU THAM KHẢO ................................................................................ 63

5
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
 Mô hình cấu trúc 3D
Hình 1 Mô hình Robot 3D

6
 Mô hình hóa sơ đồ động học
Hình 2 Mô hình động học ROBOT
 Sử dụng phương pháp DH: Theo DH tại mỗi khớp của robot đặt một hệ trụ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 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.

7
Hình 3 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.
 Lập được bảng DH sau:
Các biến khớp: 𝑞 = [𝜃 , 𝜃 , 𝜃 , 𝜃 ] = [𝑞 , 𝑞 , 𝑞 , 𝑞 ]
Các thông số động học thiết kế:
𝑑 = 180 (𝑚𝑚), 𝑎 = 200 (𝑚𝑚), 𝑎 = 289.12 (𝑚𝑚), 𝑎 = 31.5 (𝑚𝑚)
Khâu θ d a α
1 𝜃 𝑑 0 90
2 𝜃 0 𝑎 0
3 𝜃 0 𝑎 0
4 𝜃 0 𝑎 0
i
d
i

i

i
a

8
1.2 Thiết kế 3D Robot trên Solidworks.
1.2.1 Thiết kế từng khâu và đặt hệ trục.
-Hệ trục đặt tại khâu 0 khớp 1: 𝑂 𝑥 𝑦 𝑧
Hình 02 Đặt hệ trục khâu đế
Hình 5 Đặt hệ trục khâu 1

9
-
-Hệ trục đặt tại điểm lắp dụng cụ khâu 4: 𝑂 𝑥 𝑦 𝑧
Hình 8 Đặt hệ trục khâu cuối

10
1.2.2 Lắp ghép thành mô hình 3D
Hình 9 Mô hình sau lắp ghép 3D

11
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ệ 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ìm 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ệ 𝑖 − 1 sang hệ 𝑖.
Tính ma trậ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:
𝐴 =
cos(𝜃) − sin(𝜃) . cos(𝛼) sin(𝜃) sin(𝛼) 𝑎. cos(𝜃)
sin(𝜃) cos(𝜃) cos(𝛼) − cos(𝜃) sin(𝛼) 𝑎. sin(𝜃)
0 sin(𝛼) cos(𝛼) 𝑑
0 0 0 1
(2.1)
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: Các biến khớp: 𝑞 = [𝜃 , 𝜃 , 𝜃 , 𝜃 ] = [𝑞 , 𝑞 , 𝑞 , 𝑞 ]
 Khâu 0 sang khâu 1:
𝐴 =
cos(𝑞 ) 0 sin(𝑞 )
sin(𝑞 ) 0 −cos(𝑞 )
0
0
1
0
0
0
0
0
𝑑
1

12
𝐴 =
cos(𝑞 ) −sin(𝑞 ) 0
sin(𝑞 ) cos(𝑞 ) 0
0
0
0
0
1
0
𝑎 . cos(𝑞 )
𝑎 . sin(𝑞 )
0
1
𝐴 =
0
0
0
0
1
0
𝑎 . cos(𝑞 )
𝑎 . sin(𝑞 )
0
1
𝐴 =
0
0
0
0
1
0
𝑎 . cos(𝑞 )
𝑎 . sin(𝑞 )
0
1
 Ma trận chuyển từ khâu 0 sang khâu 4 được tính theo công thức:
𝐴 = 𝐴 ∗ 𝐴 ∗ 𝐴 ∗ 𝐴

13
2.1.2 Phương trình quỹ đạo điểm cuối, các vận tốc, gia tốc
 Giải được tọa độ điểm cuối như sau:
 Tiến hành ràng buộc hướng khâu cuối:
-Hướng khâu cuối hướng xuống dưới nên ta có:
𝛅 =𝜃 + 𝜃 + 𝜃 = 𝑞 + 𝑞 + 𝑞 = −
-Từ đó ta được tọa độ điểm tác động cuối E sau ràng buộc chỉ còn phụ thuộc vào
3 biến khớp 𝑞 , 𝑞 , 𝑞

14
 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: 𝑎 = 𝑣̇ =
𝑣̇
𝑣̇
𝑣̇
 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:
𝑞 −
𝜋
2
÷
𝜋
2
𝑞 0 ÷
2. 𝜋
3
𝑞 −
8. 𝜋
9
÷ 0
- 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 3 Miền làm việc theo phương X-Y

15
Hình 4 Miền làm việc trong không gian Y-Z
Hình 5 Miền làm việc vẽ trong không gian 3D
2.1.3 Mô phỏng động học thuận
 Thiết kế mô phỏng theo quỹ đạo bất kì
- Chọn các quy luật biến khớp bất kì: ( thuộc giới hạn biến khớp)
𝑞 (𝑡) = 0.2 − 1.1 ∗ 𝑐𝑜𝑠(3 ∗ 𝑡):
𝑞 (𝑡) = 1.2 + 0.8 ∗ 𝑠𝑖𝑛(3 ∗ 𝑡):
𝑞 (𝑡) = −1.5 − 0.5 ∗ 𝑐𝑜𝑠(3 ∗ 𝑡):

16
- Chạy hết 1 chu kì T = = 2.1(𝑠), ta có
𝑞 𝜖(−0.9 ÷ 1.3) (𝑟𝑎𝑑)
𝑞 𝜖(−0.4 ÷ 2) (𝑟𝑎𝑑)
𝑞 𝜖(−2 ÷ −1) (𝑟𝑎𝑑)
-Thuộc giới hạn biến khớp nêu trên
-Tiến hành thực hiện với t đi hết 1 chu kì từ 0s đến 2.1s
 Quỹ đạo điểm E
Hình 6 Quỹ đạo mô phỏng thuận
 Vận tốc, gia tốc điểm E:

18
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.
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 xỉ ở đây là 3 phương
trình giải như 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 𝑥 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:
⎩
⎪
⎨
⎪
⎧𝑓 = 𝑥 −
𝑎 . cos (−𝑞 + 𝑞 − 𝑞 )
2
+
𝑎 . cos (𝑞 + 𝑞 + 𝑞 )
2
+
𝑎 . cos (𝑞 − 𝑞 )
2
+
𝑎 . cos (𝑞 + 𝑞 )
2
𝑓 = 𝑦 −
𝑎 . 𝑠𝑖𝑛 (𝑞 + 𝑞 + 𝑞 )
2
+
𝑎 . sin (−𝑞 + 𝑞 − 𝑞 )
2
+
𝑎 . 𝑠𝑖𝑛(𝑞 + 𝑞 )
2
+
𝑎 . sin (𝑞 − 𝑞 )
2
𝑓 = 𝑧 − [𝑑 + 𝑎 𝑠𝑖𝑛( 𝑞 ) + 𝑎 𝑠𝑖𝑛( 𝑞 + 𝑞 ) − 𝑎 ]
Bài toán khi biết được xE(t), yE(t), zE(t) tại mỗi thời điểm t ta sẽ tìm được vector
q=[q1, q2, q3]T
tại mỗi thời điểm đó.
Ta lấy giá trị sát giá trị đầu để tiến hành quá trình lặp Newton-Raphson.
Quá trình lặp dừng lại khi sai số ở lần k+1 với lần k nhỏ hơn giá trị cho phép.
Dưới đây là 1 doạn chương trình viết bằng maple sử dụng phương pháp
Newton-Raphson lưu kết quả tính toán ra file txt.

19
Mô phỏng số bài toán ngược (đường thẳng):
Quỹ đạo đặt:
𝑥 = 0.32
𝑦 = 0.16.
𝑧 = 0.17 ∗ (𝑡^3) − 0.255 ∗ (𝑡^2) + 0.275
𝑡 𝑐ℎạ𝑦 𝑡ừ 0 đế𝑛 1
Bộ nghiệm đầu: Được tính từ mô hình thực trên Solidwork ứng với tọa độ điểm
ban đầu.
-Mate điểm cuối E với điểm đầu của đoạn thẳng cần tìm tọa độ (0.32;0.16;0.275)
-Tiến hành đo các góc 𝜃 ; 𝜃 ; 𝜃 ứng với mô hình trong solid

20
-Góc 𝜃 = 0.46 𝑟𝑎𝑑
-Góc 𝜃 = 1.19 𝑟𝑎𝑑

21
-Góc 𝜃 = −(𝜋 − 1.56) ≈ −1.58 𝑟𝑎𝑑
Từ đó ta được bộ nghiệm đầu:
(𝜃 ; 𝜃 ; 𝜃 ) = (0.46; 1.19; −1.58)
- Tiến hành giải bài toán động học ngược, chương trình lập trình sẵn bằng
Maple với bộ tham số
- Nghiệm đầu (𝜃 ; 𝜃 ; 𝜃 ) = (0.46; 1.19;-1,58)
- Thời gian t từ 0 đến 1s, bước nhảy dt= 0.01s
- Ta tìm được vector q = [q1, q2, q3]T
tại mỗi thời điểm t.
…

22
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 7 Quỹ đạo đ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 8 Đồ thị kiểm tra

23
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 9 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.

24
CHƯƠNG 3: THIẾT KẾ QUỸ ĐẠO
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 cuối E là hàm theo thời gian.
Hình 21 Quỹ đạo dự kiến cho ROBOT
Đường thẳng đi qua 4 điểm:
Đ𝑖ể𝑚 𝐸(𝑥, 𝑦, 𝑧) 𝑡(𝑠)
𝐴 0.32; 0.16; 0.275 0
𝐵 0.32; 0.16; 0.19 1
𝐶 0.32; −0.16; 0.19 3
𝐷 0.26; −0.16; 0.19 4

25
Đường thẳng đi qua A, B được quy luật điểm cuối: với 0≤t≤1
𝑥 = 0.32
𝑦 = 0.16
𝑧 = 0.17 ∗ 𝑡 − 0.255 ∗ 𝑡 + 0.275
Đường thẳng đi qua B, C được quy luật điểm cuối: với 1≤t≤3
𝑥 = 0.32
𝑦 = 0.08 ∗ 𝑡 − 0.48 ∗ 𝑡 + 0.72 ∗ 𝑡 − 0.16
𝑧 = 0.19
Đường thẳng đi qua C, D được quy luật điểm cuối: với 3≤t≤4
𝑥 = 0.12 ∗ 𝑡 − 1.26 ∗ 𝑡 + 4.32 ∗ 𝑡 − 4.54
𝑦 = −0.16
𝑧 = 0.19
Đồ thị toàn bộ quỹ đạo đi qua 4 điểm:
Hình 22 Quỹ đọa đi qua 04 điểm

26
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ý.

27
- 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:
 Sử dụng lệnh Move/Copy Body trong solid để di chuyển hệ tọa độ tương
ứng với mô hình theo phương pháp Denavit – Hartenberg (DH)
 Hoàn thành việc đặt hệ trục tọa độ cho khâu 0
 Khâu 1
 Khâu 2

28
 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++ 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.

29
 Để 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
4

32
Lắp ghép hoành chỉnh robot:
4.2 Mô phỏng quỹ đạo
Quỹ đạo qua nhiều điểm lấy từ file txt (tính toán từ bài toán ngược)

33
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: 𝑞 = [𝑞 , 𝑞 , 𝑞 , 𝑞 ] = [𝜃 , 𝜃 , 𝜃 , 𝜃 ]
𝑞̇ = [𝑞̇ , 𝑞̇ , 𝑞̇ , 𝑞̇ ]
- Để điều khiển Robot theo ý 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 𝑓(𝑞)).
- U là dạng ma trận 1 cột 𝑛 hàng với 𝑛 là số khớp của Robot.
 Giả thiết:
- Trong phạm vi của bài toán này ta quy định bỏ qua lực cản, 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.
-

34
 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 (m)
𝑥 𝑦 𝑧
1 -0.023 -0.059 0.003
2 -0.120 0 0.001
3 -0.204 0.027 0
4 -0.008 0 0
 Bảng mô tả khối lượng các khâu:
Khâu Khối lượng (kg)
1 6.062
2 2.644
3 2.340
4 0.2
 Bảng mô tả ma trận momen quán tính các khâu(kg*𝑚 ):
Tenxo Khâu 1 2 3 4
𝐼 0.022 0.001 0.001 0
𝐼 0.002 0 -0.002 0
𝐼 -0.001 0 0 0
𝐼 0.002 0 -0.002 0
𝐼 0.024 0.014 0.020 0
𝐼 0 0 0 0
𝐼 -0.001 0 0 0
𝐼 0 0 0 0
𝐼 0.022 0.014 0.021 0

35
Các bước tính toán:
1. Tọa độ khối tâm trong hệ cơ sở:
𝑟 =
−0.023 ∗ cos(q1) + 0.003 ∗ sin(q1)
−0.023 ∗ sin(q1) − 0.003 ∗ cos(q1)
0.121
1
𝑟 =
0.08 ∗ cos(q1) ∗ cos(q2) + 0.001 ∗ sin(q1)
0.08 ∗ sin(q1) ∗ cos(q2) − 0.001 ∗ cos(q1)
0.08 ∗ sin(q2) + 0.18
1
𝑟 =
𝑟 =
2. Tính các Jacobi tịnh tiến ở khối tâm:
Ma trận Jacobi tịnh tiến được tính bằng đạo hàm của tọa độ vị trí khối tâm
của các khâu theo các biến khớp:

36
J =
∂r
∂q
=
⎣
⎢
⎢
⎢
⎢
⎢
⎡
∂x
∂q
⋯
∂x
∂q
∂y
∂q
⋱
∂y
∂q
∂z
∂q
⋯
∂z
∂q ⎦
⎥
⎥
⎥
⎥
⎥
⎤
𝐽 = =
0.023 ∗ sin(q1) + 0.003 ∗ cos(q1) 0 0 0
−0.023 ∗ cos(q1) + 0.003 ∗ sin(q1) 0 0 0
0 0 0 0
𝐽 = =
𝐽 = =
𝐽 = =
3. Tính các ma trận sóng:
𝜔 = 𝑅 ∗ 𝑅̇
𝜔 = 𝑅 ∗ 𝑅̇
𝜔 = 𝑅 ∗ 𝑅̇
𝜔 = 𝑅 ∗ 𝑅̇

37
- Xác định vận tốc góc các khâu:
(Với dp[i] là đạo hàm của biến khớp)
𝜔 =
𝜔 =
𝜔 =
𝜔 =
T

38
4. Tính các Jacobi quay:
Ma trận Jacobi quay được tính bằng đạo hàm của vận tốc góc của các khâu theo
các đạo hàm của biến khớp:
 𝐽 = ̇
=
0 0 0 0
1 0 0 0
0 0 0 0
 𝐽 = ̇
=
sin(q2) 0 0 0
cos(q2) 0 0 0
0 1 0 0
 𝐽 = ̇
=
sin(q2 + q3) 0 0 0
cos(q2 + q3) 0 0 0
0 1 1 0
 𝐽 = ̇
=
sin(q2 + q3 + q4) 0 0 0
cos(q2 + q3 + q4) 0 0 0
0 1 1 1
5.2 Thiết lập phương trình vi phân chuyển động của robot
- Thiết lập phương trình vi phân chuyển động của Robot:
𝑀(𝑞)𝑞̈ + 𝐶(𝑞, 𝑞̇)𝑞̇ + 𝐺(𝑞) + 𝑄(𝑞) = 𝑈
 Trong đó:
 ( )
M q là: ma trận khối lượng suy rộng.
 ( , )
C q q
 là: ma trận Coriolis.
 ( )
G q là: ma trận đặc trưng cho các lực có thế tác dụng vào robot
 Q(q) là: ma trận đặc trưng cho các ngoại lực không thế tác dụng vào
robot.
 Ulà: ma trận các lực và momen điều khiển.
 𝑞̈ : gia tốc của biến khớp (kí hiệu trong bài ddq[i])
 𝑞̇ : đạo hàm của biến khớp (kí hiệu trong bài dp[i])
- Xác định được từng ma trận khối lượng suy rộng cho từng khâu:
 𝑀 (𝑞) = 𝐽 .𝑚 . 𝐽 + 𝐽 . 𝐼 . 𝐽
 𝑀 (𝑞) = 𝐽 .𝑚 . 𝐽 + 𝐽 . 𝐼 . 𝐽
 𝑀 (𝑞) = 𝐽 .𝑚 . 𝐽 + 𝐽 . 𝐼 . 𝐽
 𝑀 (𝑞) = 𝐽 .𝑚 . 𝐽 + 𝐽 . 𝐼 . 𝐽

39
- Ma trận khối lượng của cả hệ là:
M(q) = 𝑀 (𝑞) + 𝑀 (𝑞) + 𝑀 (𝑞) + 𝑀 (𝑞) =
𝑚 𝑚 𝑚 𝑚
𝑚 𝑚 𝑚 𝑚
𝑚 𝑚 𝑚 𝑚
𝑚 𝑚 𝑚 𝑚
 𝑚 =
0.1222691273 + 0.00094*cos(q4 + q3 + 2*q2) + 0.00094*cos(q4 + q3) +
0.001358864*cos(q4 + 2*q3 + 2*q2) - 0.012636*sin(q3 + 2*q2) +
0.05140096*cos(q3 + 2*q2) + 0.000055225*cos(2*q4 + 2*q3 + 2*q2) +
0.0657608*cos(2*q2) - 0.0073778816*sin(2*q3 + 2*q2) +
0.02548324228*cos(2*q3 + 2*q2) + 0.001358864*cos(q4) -
0.012636*sin(q3) + 0.05140096*cos(q3)
 𝑚 =0.1890102547 + 0.00188*cos(q4 + q3) + 0.002717728*cos(q4) -
0.025272*sin(q3) + 0.10280192*cos(q3)
 𝑚 =0.05648865458 + 0.002717728*cos(q4)
 𝑚 =0.00011045
 𝑚 = 𝑚 = -0.00021152*sin(q2)
 𝑚 = 𝑚 =0.05648865458 + 0.00094*cos(q4 + q3) +
0.002717728*cos(q4) - 0.012636*sin(q3) + 0.05140096*cos(q3)
 𝑚 = 𝑚 =0.00094*cos(q4 + q3) + 0.00011045 + 0.001358864*cos(q4)
 𝑚 = 𝑚 = 𝑚 = 𝑚 = 0
 𝑚 = 𝑚 =0.00011045 + 0.001358864*cos(q4)

40
- Xác định ma trận lực quán tính Coriolis và quán tính li tâm
𝑐 = ∑ (𝑘, 𝑙, 𝑗). 𝑞̇ . 𝑞̇ . Trong đó: (𝑘, 𝑙, 𝑗) = + −
 𝑐 =
(-0.00188*sin(q4 + q3 + 2*q2) - 0.002717728*sin(q4 + 2*q3 + 2*q2) - 0.025272*cos(q3 + 2*q2) -
0.10280192*sin(q3 + 2*q2) - 0.00011045*sin(2*q4 + 2*q3 + 2*q2) - 0.1315216*sin(2*q2) -
0.0147557632*cos(2*q3 + 2*q2) - 0.05096648458*sin(2*q3 + 2*q2))*dq[1]*dq[2] + (-
0.00094*sin(q4 + q3 + 2*q2) - 0.00094*sin(q4 + q3) - 0.002717728*sin(q4 + 2*q3 + 2*q2) -
0.012636*cos(q3 + 2*q2) - 0.05140096*sin(q3 + 2*q2) - 0.00011045*sin(2*q4 + 2*q3 + 2*q2) -
0.0147557632*cos(2*q3 + 2*q2) - 0.05096648458*sin(2*q3 + 2*q2) - 0.012636*cos(q3) -
0.05140096*sin(q3))*dq[1]*dq[3] + (-0.00094*sin(q4 + q3 + 2*q2) - 0.00094*sin(q4 + q3) -
0.001358864*sin(q4 + 2*q3 + 2*q2) - 0.00011045*sin(2*q4 + 2*q3 + 2*q2) -
0.001358864*sin(q4))*dq[1]*dq[4] - 0.00021152*cos(q2)*dq[2]^2
 𝑐 =
((0.00188*sin(q4 + q3 + 2*q2) + 0.002717728*sin(q4 + 2*q3 + 2*q2) + 0.025272*cos(q3 + 2*q2) +
0.10280192*sin(q3 + 2*q2) + 0.00011045*sin(2*q4 + 2*q3 + 2*q2) + 0.1315216*sin(2*q2) +
0.0147557632*cos(2*q3 + 2*q2) + 0.05096648458*sin(2*q3 + 2*q2))*dq[1]^2)/2 + (-
0.00188*sin(q4 + q3) - 0.025272*cos(q3) - 0.10280192*sin(q3))*dq[2]*dq[3] + (-0.00188*sin(q4 +
q3) - 0.002717728*sin(q4))*dq[2]*dq[4] + ((-0.00188*sin(q4 + q3) - 0.025272*cos(q3) -
0.10280192*sin(q3))*dq[3]^2)/2 + (-0.00188*sin(q4 + q3) - 0.002717728*sin(q4))*dq[3]*dq[4] + ((-
0.00188*sin(q4 + q3) - 0.002717728000*sin(q4))*dq[4]^2)/2
 𝑐 =
((0.00094*sin(q4 + q3 + 2*q2) + 0.00094*sin(q4 + q3) + 0.002717728*sin(q4 + 2*q3 + 2*q2) +
0.012636*cos(q3 + 2*q2) + 0.05140096*sin(q3 + 2*q2) + 0.00011045*sin(2*q4 + 2*q3 + 2*q2) +
0.0147557632*cos(2*q3 + 2*q2) + 0.05096648458*sin(2*q3 + 2*q2) + 0.012636*cos(q3) +
0.05140096*sin(q3))*dq[1]^2)/2 + ((0.00188*sin(q4 + q3) + 0.025272*cos(q3) +
0.10280192*sin(q3))*dq[2]^2)/2 - 0.002717728*sin(q4)*dq[2]*dq[4] -
0.002717728*sin(q4)*dq[3]*dq[4] - 0.001358864*sin(q4)*dq[4]^2
 𝑐 =
((0.00094*sin(q4 + q3 + 2*q2) + 0.00094*sin(q4 + q3) + 0.001358864*sin(q4 + 2*q3 + 2*q2) +
0.00011045*sin(2*q4 + 2*q3 + 2*q2) + 0.001358864*sin(q4))*dq[1]^2)/2 + ((0.00188*sin(q4 + q3)

41
+ 0.002717728*sin(q4))*dq[2]^2)/2 + 0.002717728*sin(q4)*dq[2]*dq[3] +
0.001358864*sin(q4)*dq[3]^2
- Tính thế năng:
= 𝑚 𝑔𝑧 + 𝑚 𝑔𝑧 + 𝑚 𝑔𝑧 + 𝑚 𝑔𝑧
+ Chiếu lên trục Z với gốc thế năng chọn tại gốc khâu 0 có chiều hướng lên ta sẽ
có được:
∏ =
0.2*g*(0.0235*(sin(q2)*cos(q3) + cos(q2)*sin(q3))*cos(q4) + 0.0235*(-sin(q2)*sin(q3) +
cos(q2)*cos(q3))*sin(q4) + 0.180 + 0.28912*sin(q2)*cos(q3) + 0.28912*cos(q2)*sin(q3) +
0.200*sin(q2)) + 2.340*g*(0.08512*sin(q2)*cos(q3) + 0.08512*cos(q2)*sin(q3) -
0.027*sin(q2)*sin(q3) + 0.027*cos(q2)*cos(q3) + 0.180 + 0.200*sin(q2)) + 2.644*g*(0.080*sin(q2)
+ 0.180) + 0.733502*g
- Lực suy rộng của lực có thế (trọng lực):
G(q) =
∏
=
𝑔
𝑔
𝑔
𝑔
Với
 𝑔 = 0
 𝑔 =
0.2*g*(0.0235*(-sin(q2)*sin(q3) + cos(q2)*cos(q3))*cos(q4) + 0.0235*(-cos(q2)*sin(q3) -
sin(q2)*cos(q3))*sin(q4) + 0.28912*cos(q2)*cos(q3) - 0.28912*sin(q2)*sin(q3) + 0.200*cos(q2)) +
2.340*g*(0.08512*cos(q2)*cos(q3) - 0.08512*sin(q2)*sin(q3) - 0.027*cos(q2)*sin(q3) -
0.027*sin(q2)*cos(q3) + 0.200*cos(q2)) + 0.211520*g*cos(q2)
 𝑔 =
0.2*g*(0.0235*(-sin(q2)*sin(q3) + cos(q2)*cos(q3))*cos(q4) + 0.0235*(-cos(q2)*sin(q3) -
sin(q2)*cos(q3))*sin(q4) + 0.28912*cos(q2)*cos(q3) - 0.28912*sin(q2)*sin(q3)) + 2.340*g*(-
0.08512*sin(q2)*sin(q3) + 0.08512*cos(q2)*cos(q3) - 0.027*sin(q2)*cos(q3) -
0.027*cos(q2)*sin(q3))
 𝑔 =
0.2*g*(-0.0235*(sin(q2)*cos(q3) + cos(q2)*sin(q3))*sin(q4) + 0.0235*(-sin(q2)*sin(q3) +
cos(q2)*cos(q3))*cos(q4))

42
- Xác định lực suy rộng của lực không thế - ma trận Q:
 Lực suy rộng ứng với tọa độ suy rộng 𝑞
𝑄 = 𝐽 . 𝐹 + 𝐽 . 𝑀
 Chỉ số k ứng với hàng k của các ma trận Jacobi
 Giả thiết khâu thao tác cuối E chịu tác dụng của ngoại lực 𝐹 = 𝐹 , 𝐹 , 𝐹 và
momen 𝑀 = 𝑀 , 𝑀 , 𝑀
 Ta đi tính ma trận chuyển vị của ma trận jacobi tịnh tiến và quay của điểm
thao tác cuối E
 𝐽 =
 𝐽 = ̇
 Xác định các thành phần: Q =
𝑄
𝑄
𝑄
𝑄
 𝑄 = 𝐽 . 𝐹 + 𝐽 . 𝑀 =
(0.0315*(-sin(q1)*cos(q2)*cos(q3) + sin(q1)*sin(q2)*sin(q3))*cos(q4) +
0.0315*(sin(q1)*cos(q2)*sin(q3) + sin(q1)*sin(q2)*cos(q3))*sin(q4) -
0.28912*sin(q1)*cos(q2)*cos(q3) + 0.28912*sin(q1)*sin(q2)*sin(q3) -
0.200*sin(q1)*cos(q2))*Fx + (0.0315*(cos(q1)*cos(q2)*cos(q3) -
cos(q1)*sin(q2)*sin(q3))*cos(q4) + 0.0315*(-cos(q1)*cos(q2)*sin(q3) -
cos(q1)*sin(q2)*cos(q3))*sin(q4) + 0.28912*cos(q1)*cos(q2)*cos(q3) -
0.28912*cos(q1)*sin(q2)*sin(q3) + 0.200*cos(q1)*cos(q2))*Fy + sin(q2 +
q3 + q4)*Mx + cos(q2 + q3 + q4)*My
 𝑄 = 𝐽 . 𝐹 + 𝐽 . 𝑀 =
(0.0315*(-cos(q1)*cos(q2)*sin(q3) - cos(q1)*sin(q2)*cos(q3))*cos(q4) +
0.0315*(cos(q1)*sin(q2)*sin(q3) - cos(q1)*cos(q2)*cos(q3))*sin(q4) -
0.28912*cos(q1)*sin(q2)*cos(q3) - 0.28912*cos(q1)*cos(q2)*sin(q3) -
0.200*cos(q1)*sin(q2))*Fx + (0.0315*(-sin(q1)*cos(q2)*sin(q3) -
sin(q1)*sin(q2)*cos(q3))*cos(q4) + 0.0315*(-sin(q1)*cos(q2)*cos(q3) +
sin(q1)*sin(q2)*sin(q3))*sin(q4) - 0.28912*sin(q1)*sin(q2)*cos(q3) -
0.28912*sin(q1)*cos(q2)*sin(q3) - 0.200*sin(q1)*sin(q2))*Fy + (0.0315*(-
sin(q2)*sin(q3) + cos(q2)*cos(q3))*cos(q4) + 0.0315*(-cos(q2)*sin(q3) -

43
sin(q2)*cos(q3))*sin(q4) + 0.28912*cos(q2)*cos(q3) -
0.28912*sin(q2)*sin(q3) + 0.200*cos(q2))*Fz + Mz
 𝑄 = 𝐽 . 𝐹 + 𝐽 . 𝑀 =
(0.0315*(-cos(q1)*cos(q2)*sin(q3) - cos(q1)*sin(q2)*cos(q3))*cos(q4) +
0.0315*(cos(q1)*sin(q2)*sin(q3) - cos(q1)*cos(q2)*cos(q3))*sin(q4) -
0.28912*cos(q1)*sin(q2)*cos(q3) - 0.28912*cos(q1)*cos(q2)*sin(q3))*Fx +
(0.0315*(-sin(q1)*cos(q2)*sin(q3) - sin(q1)*sin(q2)*cos(q3))*cos(q4) +
0.0315*(-sin(q1)*cos(q2)*cos(q3) + sin(q1)*sin(q2)*sin(q3))*sin(q4) -
0.28912*sin(q1)*sin(q2)*cos(q3) - 0.28912*sin(q1)*cos(q2)*sin(q3))*Fy +
(0.0315*(-sin(q2)*sin(q3) + cos(q2)*cos(q3))*cos(q4) + 0.0315*(-
cos(q2)*sin(q3) - sin(q2)*cos(q3))*sin(q4) + 0.28912*cos(q2)*cos(q3) -
0.28912*sin(q2)*sin(q3))*Fz + Mz
 𝑄 = 𝐽 . 𝐹 + 𝐽 . 𝑀 =
(-0.0315*(cos(q1)*cos(q2)*cos(q3) - cos(q1)*sin(q2)*sin(q3))*sin(q4) +
0.0315*(-cos(q1)*cos(q2)*sin(q3) - cos(q1)*sin(q2)*cos(q3))*cos(q4))*Fx +
(-0.0315*(sin(q1)*cos(q2)*cos(q3) - sin(q1)*sin(q2)*sin(q3))*sin(q4) +
0.0315*(-sin(q1)*cos(q2)*sin(q3) - sin(q1)*sin(q2)*cos(q3))*cos(q4))*Fy +
(-0.0315*(sin(q2)*cos(q3) + cos(q2)*sin(q3))*sin(q4) + 0.0315*(-
sin(q2)*sin(q3) + cos(q2)*cos(q3))*cos(q4))*Fz + Mz
- Ta xác định được phương trình vi phân chuyển động:
M(q).𝑞̈+C(q,𝑞̇).𝑞̇+G(q)+Q(q) =
𝑈
𝑈
𝑈
𝑈
- 𝑈 =

45
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.
Ý nghĩa:
- Từ bài toán động lực học ngược, khi đã biết các thông số các khâu và phương
trình chuyển động của chúng; ta có thể tìm ra các momen cần thiết (cung cấp cho
quá trình điều khiển) để có thể điều khiển robot đi theo quỹ đạo chuyển động như
mong muốn mà không phải thực hiện các bước thử lần lượt với từng động cơ của
khớp.
Đầu vào bài toán:
Ta có các phương trình theo thời gian của biến khớp:
𝑞 = 0.2 − 1.1 ∗ 𝑐𝑜𝑠(3 ∗ 𝑡)
𝑞 = 1.2 + 0.8 ∗ 𝑠𝑖𝑛(3 ∗ 𝑡)
𝑞 = −1.5 − 0.5 ∗ 𝑐𝑜𝑠(3 ∗ 𝑡)
𝑞 = −
𝜋
2
− 𝑞2 − 𝑞3
 Giả thiết khâu thao tác cuối E chịu tác dụng của ngoại lực 𝐹 = [10,10,30]
và momen 𝑀 = [0,0,20]
- Có các lực và Mô men tương ứng với đầu vào.
𝑼 = 𝑀(𝑞)𝑞̈ + 𝐶(𝑞, 𝑞̇)𝑞̇ + 𝐺(𝑞) + 𝑄(𝑞)

46
Đầu ra bài toán (Đồ thị Lực và Mô men tương ứng);
- Thời gian hoạt độ từ 0->2.1s
Hình 10 Đồ thị MoMen khâu 1
Hình 11 Đồ thị MoMen khâu 2

47
Hình 12 Đồ thị Momen khâu 3
Hình 13 Đồ thị Momen khâu 4

48
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 ở phần trước (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).
Đầu ra bài toán: (Đồ thị các biến khớp tương ứng với quỹ của bài toán động
học ngược)
Bài toán động lực học thuận tương ứng mô tả trong chương 6

49
CHƯƠNG 6: MÔ PHỎNG-THIẾT KẾ BỘ ĐIỀU KHIỂN
6.1 Chọn luật điều khiển PD tổng quát
Ở những phần trước ta đã tiến hành tính toán, xác định quy luật biến thiên của
các biến khớp theo thời gian, tương ứng với quỹ đạo công tác của robot theo yêu cầu.
Phần này sẽ trình bày việc điều khiển robot sao cho chúng có thể thực hiện được
đúng các chuyển động mong muốn.
Xuất phát trực tiếp từ hệ phương trình vi phân chuyển động đã được nghiên cứu
trong phần Động lực học hệ robot. Phương pháp điều khiển áp dụng là phương pháp
điều khiển lực (mômen) thường được sử dụng để điều khiển cho mô hình của nhóm.
Mục tiêu của bài toán điều khiển là làm sao cho robot bám theo quỹ đạo đã được
thiết kế. Các phần tử dẫn động làm việc theo cách nhận lệnh điều khiển và sinh ra
lực (momen). Momen đầu ra sẽ được sử dụng để tính toán momen mong muốn tiếp
theo.
Vấn đề cốt lõi của việc thiết kế bộ điều khiển robot là làm thế nào để bảo đảm
rằng bộ điều khiển được thiết kế sẽ đáp ứng tốt các yêu cầu làm việc cho trước. Tiêu
chí cơ bản quan trọng nhất là hệ phải đảm bảo ổn định. Nghĩa là đảm bảo thời gian
quá độ, độ quá điều chỉnh và sai số quỹ đạo đủ nhỏ theo yêu cầu đặt ra cho dù hệ có
phải chịu tác động của một số nhiễu trong suốt quá trình làm việc.
Trước hết ta có mô hình toán học của hệ thống đã được xây dựng từ các phần
trước là hệ phương trình vi phân động học:
𝑀(𝑞)𝑞̈ + 𝐶(𝑞, 𝑞̇)𝑞̇ + 𝐺(𝑞) + 𝑄(𝑞) = 𝜏
Trong đó vế trái của phương trình là mô hình với các tham số của robot, vế
phải là momen mà bộ đều khiển tác động lên robot. Momen này được sinh ra từ bộ
điều khiển với giá trị thỏa mãn yêu cầu đã nêu ở trên.

50
Chúng ta phải xét xem cấu trúc của bộ điều khiển như thế nào thì có thể đáp
ứng mục tiêu thiết kế. Trước hết chúng ta chia bộ điều khiển thành hai phần: một
phần dựa trên mô hình và một phần dựa trên phản hồi.
  
 

Trong đó α, β là các hàm được chọn lựa sao cho nếu τ với tư cách là đầu vào
mới của hệ.
Với cấu trúc này của luật điều khiển, phương trình của hệ trở thành:
𝑀(𝑞)𝑞̈ + 𝐶(𝑞, 𝑞̇)𝑞̇ + 𝐺(𝑞) + 𝑄(𝑞) = 𝛼𝜏′
+ 𝛽
Với τ là thành phần đầu vào thì α, β có thể được chọn
𝛼 = 𝑀(𝑞)
𝛽 = 𝐶(𝑞, 𝑞̇)𝑞̇ + 𝐺(𝑞) + 𝑄(𝑞)
Sau khi chọn được α, β thì phương trình còn lại là:
q 
 

Với luật điều khiển phản hồi vòng kín như sau:
𝜏′
= 𝑞̈ + 𝐾 (𝑞̇ − 𝑞̇) + 𝐾 (𝑞 − 𝑞)
Trong đó các giá trị đầu vào là quy luật vị trí, quy luật vận tốc, quy luật gia tốc
mong muốn đã được tính toán trong phần thiết kế quỹ đạo:
( )
( )
( )
d
d
d
q t
q t
q t







Và các hệ số 𝐾 , 𝐾 là các ma trận đường chéo vuông cấp n (n là số tham số
động học của mô hình robot), phần sau chúng ta sẽ xem xét chi tiết việc chọn các hệ
số 𝐾 , 𝐾 này theo các điều kiện ràng buộc và mục tiêu bài toán.
Ta tìm được biểu thức đặc trưng cho hệ điều khiển vòng kín robot:
𝐸̈ + 𝐾 𝐸̇ + 𝐾 𝐸 = 0

51
 Với: E, Ė , Ë : là kết quả so sánh giữa tín hiệu đầu ra và tín hiệu đầu vào.
𝐸 = 𝑞 − 𝑞
𝐸̇ = 𝑞̇ − 𝑞̇
𝐸̈ = 𝑞̈ − 𝑞̈
Hình 14 Mô hình điều khiển PD thiết kế
Với các giá trị:
 𝜏 = 𝑞̈d + Kv Ė + Kp E
 Kv: ma trận đường chéo khuếch đại đạo hàm, Kp ma trận khuếch đại vị trí
 E, Ė , Ë : là kết quả so sánh giữa tín hiệu đầu ra và tín hiệu đầu vào.
 qd, 𝑞̇ , 𝑞
̈ d: là các tín hiệu đầu vào.
 q, 𝑞̇: Là các giá trị đầu ra
Với bộ điều khiển PD:
- Tăng tốc độ hội tụ -> tăng Kv
- Nâng cao độ chính xác -> tăng Kp
- Kv, Kp lớn quá -> giảm độ ổn định, giảm chất lượng quá trình quá độ (tăng
độ quá điều chỉnh, tăng thời gian quá độ),…
- Ngoài ra còn ảnh hưởng của thành phần mô men trọng lực,… -> Bộ điều
khiển PID

52
6.2 Mô phỏng PID bằng Simulink
Hình 15 Toàn bộ sơ đồ PID

53
 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 16 Tín hiệu đặt
Với
1: Giá trị vị trí đầu vào của các khâu 1, 2, 3, 4.
2: Giá trị vận tốc đầu vào của các khâu 1,2, 3, 4.
3: Giá trị gia tốc đầu vào của các khâu 1, 2, 3, 4.
 Khối tính Mô men, Lực
Với u chính là 𝜏
Hình 17 Khối tính Mô men, Lực

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

56
 Ta được các đồ thị biểu diễn góc quay các khớp thực tế và đặt trước

58
 Thiết kế bộ điều khiển cho quỹ đạo điểm – điểm
Quỹ đạo đi từ điểm A tới điểm B có các thông số như sau:
 A(0.32;0.16;0.275)
với tọa độ biến khớp (𝜃 ; 𝜃 ; 𝜃 ; 𝜃 ) = (0.46; 1.19; −1.39; −1.37)
 B(0.32;0.16;0.19)
với tọa độ biến khớp (𝜃 ; 𝜃 ; 𝜃 ; 𝜃 ) = (0.46; 1.05; −1.52; −1.10)
Tương tự như bộ điều khiển bám quỹ đạo nhưng ta thay thông số đầu vào và
nghiệm đầu
Hình 32 Đầu vào tọa độ, vận tốc, gia tốc khớp ứng với điểm A
Hình 33 Tọa độ, vận tốc khớp ứng với điểm B

59
 Ta được các đồ thị biểu diễn góc quay các khớp thực tế và đặt trước

61
PHỤ LỤC
-Lấy được khối lượng, tọa độ khối tâm và momen quán tính của các khâu từ solid

62
Phương trình xác định vị trí
x = f(q) (1.26)
Đạo hàm hai vế phương trình (1.26) theo t, ta được
𝒙̇ =
𝝏𝒇
𝝏𝒒
𝒒̇ = 𝑱(𝒒)𝒒̇ (1.27)
Trong đó: J(q) =
𝝏𝒇
𝝏𝒒
= ⋯ ⋯
⋯
⋯ ⋯
⋯
(1.28)
- Khi m = n ta có bài toàn có cấu trúc động học cân bằng x, q, f ∈ 𝑹𝒏
.
Từ (1.27) ta suy ra:
𝒒̇ (𝒕) = 𝑱 𝟏
(𝒒(𝒕))𝒙̇ (𝒕) (1.29)
Đạo hàm (1.27) theo t, ta được:
𝒙̈ (𝒕) = 𝑱̇[𝒒(𝒕)]𝒒̇ (𝒕) + 𝑱[𝒒(𝒕)]𝒒̈ (𝒕) (1.30)
Từ (1.30) suy ra:
J[q(t)]𝒒̈ (𝒕) = 𝒙̈ (𝒕) − 𝑱̇[𝒒(𝒕)]𝒒̇ (𝒕) (1.31)
Từ (1.31) suy ra được:
𝒒̈ (𝒕) = 𝑱 𝟏[𝒒(𝒕)][𝒙̈ (𝒕) − 𝑱̇[𝒒(𝒕)]𝒒̇ (𝒕)] (1.32)
Công thức (1.29) và (1.32) cho phép ta xác định được vecto vận tốc suy rộng, gia
tốc suy rộng khi biết q(t) tại thời điểm khảo sát và các quy luật 𝒙(𝒕), 𝒙̇ (𝒕), 𝒙̈ (𝒕).

63
KẾT LUẬN
1. Những kết quả có được
Dựa trên các phần mềm đa năng MATLAB và MAPLE đã xây dựng các chương
trình tính toán động học thuận, động học ngược, động lực học và điều khiển trượt
Robot chuyển động theo một quỹ đạo được định sẵn.
Tìm được mối liên hệ giữa các biến khớp và vị trí toạ độ điểm cuối từ đó xây
dựng ra được quỹ đạo chuyển động thích hợp.
2. Định hướng phát triển tương lai
Các kết quả có được sẽ là bước đệm cho tư duy về thiết kế do vậy ý tưởng của
nhóm trong tương lai sẽ cho sản phẩm robot mài phẳng hoàn thiện và sử dụng phổ
biến trong ngành công nghiệp nước nhà.
TÀI LIỆU THAM KHẢO
[1] PGS.TS. Phan Bùi Khôi, Bài giảng Tính toán thiết kế robot, ĐH Bách
khoa Hà Nội
[2] PGS.TS. Phan Bùi Khôi, Bài giảng Robotics (2009), ĐH Bách khoa Hà
Nội
[3] GS. TSKH. Nguyễn Văn Khang, Cơ sở Robot công nghiệp
[4] GS. TSKH. Nguyễn Thiện Phúc, Robot công nghiệp
[5] GS. TSKH. Nguyễn Văn Khang, Động lực học hệ nhiều vật
[6] TS. Nguyễn Quang Hoàng, Cơ sở Matlab và Simulink

báo-cáo-mô-hình-hóa-nhóm-15.pdf

Recommended

Recommended

More Related Content

What's hot

What's hot (20)

Similar to báo-cáo-mô-hình-hóa-nhóm-15.pdf

Similar to báo-cáo-mô-hình-hóa-nhóm-15.pdf (20)

báo-cáo-mô-hình-hóa-nhóm-15.pdf