Đồ án Thiết kế hệ thống điều khiển Robot SCARA môn Thiết kế hệ thống cơ điện tử | Trường Đại học Bách Khoa Hà Nội

ĐẠI HỌC BÁCH KHOA HÀ NỘI SME.EDU - Mẫu 6.a TRƯỜNG CƠ KHÍ Học kỳ: 2 Khoa Cơ điện tử Năm học: 2022-2023
ĐỒ ÁN MÔN HỌC: THIẾT KẾ HỆ THỐNG ĐIỀU KHIỂN Mã HP: ME4336
Thời gian thực hiện: 15 tuần; Mã đề: 8
Ngày giao nhiệm vụ: …/…/20…; Ngày hoàn thành: …/…/20..
Họ và tên sv: Nguyễn Ngọc Trường MSSV: 20195211 Mã lớp: 717890 Chữ ký sv: ………. Ngày …/…/20… Ngày …/…/20… Ngày …/…/20…
ĐƠN VỊ CHUYÊN MÔN NGƯỜI RA ĐỀ CB Hướng dẫn
(ký, ghi rõ họ tên)
(ký, ghi rõ họ tên)
(ký, ghi rõ họ tên)
I. Nhiệm vụ thiết kế: Thiết kế hệ thống điều khiển cho robot SCARA II. Số liệu cho trước: 1. Tải trọng: 20 kg. 2. Tầm với: 1000 mm.
3. Độ chính xác lặp: (x, y) = ±0.01 mm, (z) = ±0.02mm.
4. Hành trình trục z: 400 mm
5. Vận tốc cực đại khâu tác động cuối: 2780 mm/s III. Nội dung thực hiện:
1. Phân tích nguyên lý và thông số kỹ thuật
- Tổng quan về hệ thống điều khiển
- Nguyên lý hoạt động của hệ thống điều khiển
- Xác định các thành phần của hệ thống điều khiển
2. Thiết kế hệ thống điều khiển -
Mô hình hóa và xác định hàm truyền -
Đánh giá tính ổn định của hệ thống -
Mô phỏng và phân tích, đánh giá các chỉ tiêu kỹ thuật của hệ thống -
Lựa chọn các thiết bị cho hệ thống điều khiển: cảm biến, thiết bị điều khiển, cơ cấu chấp hành -
Thiết kế sơ đồ mạch điện và mạch điều khiển (1 bản A0)
3. Lập trình điều khiển
- Lập trình điều khiển robot (1 chương trình điều khiển)
- Lập trình mô phỏng chuyển động (1 chương trình mô phỏng trên Simmechanics)
BẢNG SỐ LIỆU CÁC PHƯƠNG ÁN THIẾT KẾ Đề số
Thông số hình học các khâu
Khối lượng các khâu Vật (kg) nặng (kg) [L1,cm] [L2,cm] [L3,cm] M1 M2 M3 M 8 50 50 30 30 20 20 20 LỜI CẢM ƠN
Dưới sự hướng dẫn nhiệt tình của PSG.TS Bùi Hải Lê em đã vận dụng những kiến
thức đã được học trong chương trình đào tạo đại học, Trường Cơ Khí - ĐH Bách Khoa
Hà Nội để hoàn thành đề tài, nhờ đó em có thể hoàn thiện, tổng hợp kiến thức trước khi tốt nghiệp Đại Học.
Tuy nhiên với kiến thức chuyên môn còn hạn chế nên đề tài không thể tránh khỏi
những thiếu sót và chưa thể hoàn thiện một cách hoàn hảo như ý muốn mong đợi. Mong
được sự giúp đỡ và góp ý của các thầy để đồ án môn học của em có thể hoàn thiện hơn.
Để hoàn thành đồ án môn học này em xin trân trọng cảm ơn tất cả các thầy cô giáo
trong Đại Học Bách Khoa Hà Nội, nhất là các quý thầy cô trong Trường Cơ Khí, Khoa
Cơ điện tử, đã hướng dẫn em trong suốt kì học vừa qua. Em xin bày tỏ lòng biết ơn sâu
sắc tới thầy giáo hướng dẫn PSG.TS Bùi Hải Lê, người đã tận tình hướng dẫn và tạo
mọi điều kiện thuận lợi để em hoàn thành đồ án này.
Em xin chân thành cảm ơn!
Hà Nội, ngày ........ tháng .........năm.......... Sinh viên Trường Nguyễn Ngọc Trường MỤC LỤC
LỜI CẢM ƠN...................................................................................................................4
CHƯƠNG 1: PHÂN TÍCH NGUYÊN LÝ VÀ THÔNG SỐ KỸ THUẬT...................6
1.1. Tổng quan về Robot Scara....................................................................................7
1.1.1. Khái niêṃ ........................................................................................................7
1.1.2. Yêu cầu thiết kế..............................................................................................7
1.2. Nguyên lý hoạt động..............................................................................................7
1.2.1. Phân tích nguyên lý hoạt đông ̣ ......................................................................7
1.2.2 Các thông số kỹ thuật quan trọng của robot................................................8
CHƯƠNG 2: THIẾT KẾ HỆ THỐNG ĐIỀU KHIỂN..................................................9
2.1. Tính toán động học, động lực học.........................................................................9
2.1.1. Động học..........................................................................................................9
2.1.2. Tính toán động lực học.................................................................................19
2.2. Mô hình hóa, xác định hàm truyền, đánh giá tính ổn định..............................27
2.2.1. Mô hình hóa động cơ điện một chiều..........................................................28
2.2.2. Mô hình hóa và xác định hàm truyền các khâu.........................................33
2.2.3. Đánh giá tính ổn định, xác định sai lệch tĩnh.............................................39
2.3. Thiết kế bộ điều khiển PID.................................................................................44
2.4. Mô phỏng, phân tích và đánh giá các chỉ tiêu kỹ thuật của hệ thống..............50
2.5. Lựa chọn các phần tử cho hệ thống điều khiển.................................................58
2.6. Các mạch điện và ghép nối phần tử...................................................................62
CHƯƠNG 3: MÔ PHỎNG ĐIỀU KHIỂN ROBOT....................................................64
KẾT LUẬN.....................................................................................................................67
DANH MỤC TÀI LIỆU THAM KHẢO.......................................................................68
DANH MỤC HÌNH ẢNH
Hình 1. 1: Robot Scara........................................................................................................8
Hình 2. 1: Sơ đồ kết cấu các khâu robot...........................................................................11
Hình 2. 2: Vùng làm việc có thể với tới của robot............................................................15
Hình 2. 3: Vùng làm việc có thể với tới của robot (1).......................................................15
Hình 2. 4: Quỹ đạo gắp vật...............................................................................................17
Hình 2. 5: Mô hình Simulink thiết kế quỹ đạo chuyển động.............................................19
Hình 2. 6: Quỹ đạo biến khớp khâu 1...............................................................................19
Hình 2. 7: Quỹ đạo biến khớp khâu 2...............................................................................20
Hình 2. 8: Quỹ đạo biến khớp khâu 3...............................................................................20
Hình 2. 9: Thiết lập hệ tọa độ khối tâm.............................................................................21
Hình 2. 10: Sơ đồ khối robot hoàn chỉnh bao gồm hộp số................................................30
Hình 2. 11: Sơ đồ nguyên lý động cơ................................................................................32
Hình 2. 12: Sơ đồ khối mạch điện phần ứng.....................................................................32
Hình 2. 13: Sơ đồ khối phần momen điện từ....................................................................33
Hình 2. 14: Sơ đồ khối phần momen cân bằng trên động cơ.............................................33
Hình 2. 15: Sơ đồ khối động cơ ( Điên.áp -> vận tốc)......................................................34
Hình 2. 16: Sơ đồ khối động cơ ( Điện áp -> Góc quay)...................................................35
Hình 2. 17: Sơ đồ trục vít me- đai ốc bi............................................................................35
Hình 2. 18: Sơ đồ khối trục vít me- đai ốc bi....................................................................36
Hình 2. 19: Sơ đồ khối khâu 3..........................................................................................36
Hình 2. 20: Sơ đồ khối tính sai lệch tĩnh với đầu vào u(t)=1(t).........................................44
Hình 2. 21: Đồ thị đáp ứng khâu 1....................................................................................44
Hình 2. 22: Đồ thị đáp ứng khâu 2....................................................................................45
Hình 2. 23: Đồ thị đáp ứng khâu 3....................................................................................46
Hình 2. 24: Mô hình bộ điều khiển PID............................................................................47
Hình 2. 25: Sơ đồ khối bộ điều khiển PID........................................................................47
Hình 2. 26: Sơ đồ khối PID điều khiển động cơ...............................................................48
Hình 2. 27: Giao diện công cụ PID Tuner trên Matlab.....................................................48
Hình 2. 28: Đồ thi đáp ứng khâu 1 với bộ điều khiển PID................................................49
Hình 2. 29: Đồ thi đáp ứng khâu 2 với bộ điều khiển PID................................................50
Hình 2. 30: Đồ thi đáp ứng khâu 3 với bộ điều khiển PID................................................51
Hình 2. 31: Trình tự xây dựng mô hình hệ thống điều khiển............................................52
Hình 2. 32: Mô hình CAD của robot trong Matlab...........................................................52
Hình 2. 33: Khối 3D Scara robot......................................................................................53
Hình 2. 34: Mô hình hệ thống điều khiển toàn bộ robot...................................................54
Hình 2. 35: Khối Position Reference ( Thiết kế quỹ đạo )................................................54
Hình 2. 36: Khối động cơ DC...........................................................................................55
Hình 2. 37: Giá trị biến khớp khâu 1.................................................................................55
Hình 2. 38: Giá trị biến khớp khâu 2.................................................................................57
Hình 2. 39: Giá trị biến khớp khâu 3.................................................................................58
Hình 2. 40: Động cơ và bộ điều khiển hãng Yaskawa.....................................................60
Hình 2. 41: Thông số kỹ thuật của dòng động cơ SGMAV.............................................61
Hình 2. 42: Giao diện của bộ điều khiển SGDV-5R5A...................................................61
Hình 2. 43: Phương pháp xuất xung điều khiển servo ở chế độ điều khiển vị trí..............63
Hình 2. 44 Board vi điều khiển STM32F407 DISCOVERY............................................64
Hình 3. 1: Giao diện điều khiển robot scara......................................................................67
Hình 3. 2: Sơ đồ Simulink điều khiển robot......................................................................68
Hình 3. 3: Kết quả mô phỏng động học thuận...................................................................68
Hình 3. 4: Kết quả mô phỏng động học nghịch.................................................................69
Hình 3. 5: Robot về vị trí gốc............................................................................................69
CHƯƠNG 1: PHÂN TÍCH NGUYÊN LÝ VÀ THÔNG SỐ KỸ THUẬT
Trước khi thiết kế hệ thống điều khiển cho robot ta cần tìm hiểu về robot với các yêu
cầu như nguyên lý hoạt động của robot như thế nào, cấu tạo của robot về cơ khí và hệ thống điều khiển ra sao.
1.1. Tổng quan về Robot Scara 1.1.1. Khái niêṃ
Hình 1. 1: Robot Scara
SCARA là viết tắt của Selective Compliance Assembly Robot Arm hay Selective
Compliance Articulated Robot Arm. Có nghĩa là dễ dàng lựa chọn các khớp nối cánh tay robot
Năm 1981, Sankyo Seiki , Pentel và NEC đã trình bày một khái niệm hoàn toàn mới cho
robot lắp ráp. Robot được phát triển dưới sự hướng dẫn của Hiroshi Makino – giáo sư tại Đại học Yamanashi . 1.1.1
1.1.2. Yêu cầu thiết kế
• Cân bằng tối ưu về kinh tế và hiệu suất
• Sử dụng động cơ động cơ servo và các mạch điều khiển có sẵn.
• Sử dụng phần mềm có sẵn để điều khiển robot trên máy tính cá nhân. Phần mềm sử
dụng G-code để điều khiển robot
1.1.2.1. Hệ thống cơ khí
SCARA là một cấu hình tiêu chuẩn giữa các robot. Ở giai đoạn thiết kế sơ bộ, chúng ta có những điểm sau đây:
• Robot SCARA 3 bậc tự do. • Tải trọng 20 kg. • Tầm với 1000 mm.
• Độ chính xác lặp: (x, y) = ± 0,02 mm, (z) = ± 0,01 mm. Sử dụng bộ truyền vít me –
đai ốc cho khâu tịnh tiến.
1.1.2.2. Hệ thống điện tử
Các thiết bị điện tử liên quan phụ thuộc hoàn toàn vào loại động cơ được sử dụng. Để đạt
độchính xác cao, chúng ta có thể sử dụng động cơ bước hoặc động cơ servo trong bộ truyền
động chung. Do đó, phần chính của hệ thống điện tử liên quan đến việc thiết kế các mạch
điều khiển động cơ bước (servo) thích hợp. Ngoài phần mềm điều khiển được sử dụng, đây
là yếu tố quan trọng nhất, có thể ảnh hưởng lớn đến hiệu suất của robot. Ở đây, chúng ta chỉ
tập trung vào các trình điều khiển động cơ bước (servo).
Một số lựa chọn ban đầu cần phải chú ý là:
• Loại động cơ: Đơn cực hoặc lưỡng cực.
• Nguyên lý ổ đĩa: L/R hoặc PWM
• Chế độ hoạt động: Nửa bước hoặc toàn bước. Đặc trưng mô-men xoắn - tốc độ.
1.1.2.3. Phần mềm và điều khiển
Phần mềm điều khiển cần thực hiện được các nhiệm vụ sau:
• Đường dẫn liên tục trong không gian.
• Chuyển động xen kẽ (Joint-interpolated motion). • Nội suy cung tròn. • Nội suy tuyến tính.
Phối hợp chuyển động tuyến tính: tất cả các trục khởi động và dừng các bước chuyển
động cùng một thời gian.
1.2. Nguyên lý hoạt động
1.2.1. Phân tích nguyên lý hoạt đông ̣
Robot hoạt động dựa trên sự chuyển động quay của các động cơ điện ( động cơ bước,
động cơ servo ). Các động cơ này được đặt tại các khớp. Với khớp một và khớp hai, động cơ
liên kết với khớp xoay qua dây đai hoặc bánh răng để chuyền chuyển động. Khớp 3 là khớp
tịch tiến sử dụng bộ trục vit-me đai ốc bi để biến đổi chuyển động quay từ động cơ sang chuyển
động tịch tiến. Kết hợp với các cảm biến để giới hạn chuyển động cũng như đạt được vị trí, tốc độ chính xác.
Động cơ và cảm biến được điều khiển bằng hệ thống máy tính – controller thông qua
các trương trình phần mềm Matlab/Simulink, G-code,…
Tùy vào yêu cầu nhiệm vụ và chương trình đã được lập trình sẵn, bộ điều khiển sẽ xuất
tín hiệu điều khiển tới các cơ cấu chấp hành thực hiện chuyển động, thao tác (động cơ quay
làm cơ cấu quay, tay kẹp mở hay mỏ hàn bắt đầu hàn…)
Sau đó hệ thống cảm biến sẽ phản hồi tín hiệu lại cho bộ điều khiển để điều chỉnh và
xử lý những sai lệch (nếu có). Mục tiêu của việc sử dụng hệ thống điều khiển vòng kín (closed-
loop) là để triệt tiêu tín hiệu nhiễu và kiểm soát được hoạt động của robot, đảm bảo robot hoạt
động chính xác theo yêu cầu.
1.2.2 Các thông số kỹ thuật quan trọng của robot • Khâu điều khiển : 3
• Tầm với ( chiều dài cánh tay ) : 500mm + 500 mm
• Vùng làm việc o Trục quay khớp 1 : ± 155° o Trục quay khớp 2 : ± 145° o
Hành trình trục z khớp 3 : 400 mm
• Tốc độ tối đa o Khâu 1 : 450 °/s o Khâu 2 : 667 °/s o Khâu 3 : 2780 mm/s
• Tốc độ tổng hợp ( điểm làm việc ) : 11390 mm/s
• Chu kỳ thời gian ( với tải 2kg ) : 0,31s Tải trọng tối đa : 20kg Độ chính xác lặp :
o X-Y : ± 0,02 mm o Trục z khâu 3 : ± 0,01mm
CHƯƠNG 2: THIẾT KẾ HỆ THỐNG ĐIỀU KHIỂN
Robot Scara là một robot có tính ứng dụng cao nhất hiện nay. Trong công nghiệp robot được
sử dụng một cách tự động với một quỹ đạo được người thiết kế đặt ra sao cho phù hợp với
mục đích ứng dụng ví dụ như gắp thả linh kiện, lắp ráp linh kiện nhỏ, khắc lazer, in 3D, hàn,…
Với các quỹ đạo đó thì cần có một hệ thống điều khiển đảm bảo tính đáp ứng liên tục cho
robot. Nên trong chương này em sẽ thiết kế một quỹ đạo và xây dựng hệ thống điều khiển
robot với quỹ đạo đó.
2.1. Tính toán động học, động lực học
Đầu tiên, em tính toán động học cho robot để phục vụ cho việc khảo sát vùng làm việc để biết
được tầm với của robot, từ đó thiết kế một quỹ đạo trong tầm với đó. 2.1.1. Động học
Hình 2. 1: Sơ đồ kết cấu các khâu robot Bảng D-H Khâu θ d a α 1 0 2 0 3 0 0 0 -
2.1.1.1. Động học thuận và vùng làm việc của Robot
a. Động học thuận
Với bài toán động học thuận thì các biến khớp đã biết, yêu cầu tìm vị trí của khâu thao tác. Tham số động học: ¿ 22\* MERGEFORMAT (2.) Biến khớp:
q1=(−155∘÷155∘);q2=(−147∘÷147∘);q3=(0÷400mm) 33\* MERGEFORMAT (2.) Các ma trận D-H:
cos(q1) -sin(q1) 0 a1cos(q1) [ ] T01=sin(q1)
cos(q1) 0 a1sin(q1) 44\* 0 0 1 d1 0 0 0 1
cos(q2) -sin(q2) 0 a2cos(q2) [ ]
MERGEFORMAT (2.) T12=sin(q2) cos(q2) 0 a2sin(q2) 0 0 1 0 0 0 0 1 55\* MERGEFORMAT (2.) 1 0 0 0 0 1 0 0 [ ] T2 66\* 3= 0 0 1 −q3 0 0 0 1 MERGEFORMAT (2.)
Tìm vị trí và hướng điểm thao tác
- Ma trận thuần nhất khâu cuối:
T03=T01T12T23 c q1cq2 - sq1sq2 -cq1sq2 - sq1c q2 0
a2c(q1 + q2) + a1c(q1) [ ]
T03=s q1cq2 + cq1sq2 cq1c q2 - sq1sq2
0 a2s(q1 + q2) + a1s(q1) 77\* 0 0 1 −q3 + d1 + d3 0 0 0 1 MERGEFORMAT (2.)
Lưu ý: ở đây ký hiệu ‘c’ thay cho cos và ‘s’ thay cho sin.
- Ma trận biểu diễn hướng của điểm thao tác: cq1c q2 - sq1sq2 -cq1sq2 - sq1cq2 0 [ ] R30=sq1c q2 + c q1sq2 cq1cq2 - sq1sq2 0 88\* 0 0 1 MERGEFORMAT (2.)
- Vị trí điểm thao tác: x=a2cos(q1 + q2) + a1cos(q1)(1)
r03y=a2sin(q1 + q2) + a1sin(q1)(2) 99\* MERGEFORMAT (2.) z=−q3 + d1 + d2 (3)
- Vận tốc của điểm thao tác cuối: Vận tốc dài: V t30=¿ • • •
- a2sin(q1 + q2)q1 - a2sin(q1 + q2)q2- a1sin(q1)q1 • • • [ ] V 0
t3 =a2cos(q1 + q2)q1 + a2cos(q1 + q2)q2+ a1cos(q1)q1 ; 1010\* • −q3 MERGEFORMAT (2.) Vận tốc góc: 0 V ] q03=0 ; 1111\* • • q 1+q2 MERGEFORMAT (2.)
- Gia tốc điểm thao tác cuối: Gia tốc dài: a0t3=¿ 1212\ *
MERGEFOR MAT (2.) Gia tốc góc: ; 1313\ * MERGEFORMAT (2.)
Phương trình động học thuận của robot tham khảo:
x=500cos(q1 + q2) + 500cos(q1)
y=500sin(q1 + q2) + 500sin(q1) ¿ 1414\* { z=450−q3 MERGEFORMAT (2.)
b. Vùng làm việc có thể với tới của Robot
Với bài toán động học thuận thì các biến khớp đã biết, yêu cầu tìm vị trí của khâu thao tác. Tham số động học: ¿ 1515\* MERGEFORMAT (2.) Biến khớp:
q1=(−155∘÷155∘);q2=(−147∘÷147∘);q3=(0÷400mm ) 1616\* MERGEFORMAT (2.)
Hình 2. 2: Vùng làm việc có thể với tới của robot hệ Oxy
Hình 2. 3: Vùng làm việc có thể với tới của robot Oxz
2.1.1.2. Động học nghịch và bài toán thiết kế quỹ đạo trong không gian thao tác a. Động học nghịch
Với bài toán động học ngược thì vị trí của khâu thao tác xem như đã biết, yêu cầu tìm giá trị của các biến khớp.
Giả sử tọa độ điểm cuối là: xe [ ] re= ye 1717\* ze MERGEFORMAT (2.)
Phương trình tọa độ điểm cuối:
a2cos(q1 + q2) + a1cos(q1) = xe (1)
a2sin(q1 + q2) + a1sin(q1) = ye (2) 1818\* MERGEFORMAT (2.)
{ −q3 + d1 + d2=ze (3)
Từ phương trình (3) ta có: q3 = −ze+ d1+ d2 * MERGEFORMAT (2.) 1919\
Từ phương trình (1) và (2) ta có:
⇔q2=arccos( e + y2e2a−1aa222−a12) x 2
Hệ phương trình trở thành hệ 2 ẩn:
a2cos(q1 + q2) + a1cos(q1) = xe
{a 2sin(q1 + q2) + a1sin(q1) = ye {
⇔ (a2cos q2+a1)cosq1- a2sinq2sinq1 = xe 2020\
a2sinq2cos q1+(a2cosq2+a1)cosq1 = ye * MERGEFORMAT (2.)
Giải hệ phưong trình ta được hai ẩn:
cosq1=a1xe+a2(xe cos2 q22+ye sinq2) xe + ye sinq1=
a1 ye+a2(yecos2 q22−xesinq2) xe + ye ⇔q ) 1=arctan(cossin qq11 =arctan(
aa11xyee++aa22((xyeecoscosqq22−+yxeesinsinqq22)))(2.23)
Thay các tham số của robot ta có: q1=arctan( 500500xyee++500500((
{xy eecoscosqq22+−yxeesinsinqq22)))
xe2+ye2−5002−5002 (2.24) q2=arccos( 2.500.500 ) q3 = 450−ze
b. Thiết kế quỹ đạo làm việc
Để khảo sát quỹ đạo làm việc của bài toán đặt ra , ta chọn quỹ đạo làm việc gắp thả vật để khảo sát :
Hình 2. 4: Quỹ đạo gắp vật Tọa độ các điểm:
S (0.1; 0 ;0.45) là điểm khởi động của robot. A’(0 ;0.5; 0.35) A (0 ;0.5; 0.2) B (0.5 ; 0 ;0.2) B’ (0.5 ; 0 ; 0.35) Quỹ đạo SA’ A’A Dừng gắp AA’ A’B’ B’B Dừng thả BB’ mạch mạch Thời 2 2 1 2 3 2 1 2 gian(s)
Để thiết kế được quỹ đạo cho Robot, người ta thường dùng phương pháp xấp xỉ đa thức
bậc n hoặc dạng phối hợp. Trong nội dung báo cáo này nhóm sử dụng quỹ đạo theo đa thức
bậc 3. Vì dạng này vừa đơn giản trong thiết kế đồng thời thỏa mãn được các yêu cầu như :
Đảm bảo tính liên tục của vận tốc -> quỹ đạo không giật cục, gấp khúc, gây sốc trong quá trình hoạt động của robot.
+ Thiêt kế quỹ đạo trong không gian thao tác, quỹ đạo đường thẳng
Tổng quát : ta có phương trình đường thẳng đi qua 2 điểm và là : x−x 0 y−y0 z−z0
= = (2.25) xe−x0 ye−y0 ze−z0
Độ dời dịch chuyển của điểm thao tác E theo phương x,y,z là đa thức bậc 3 theo thời gian:
x=a0+a1t+a2t2+a32t3(2.26) {
x˙=a1+2a2t+3a3t y y y y y (2.28)
Với thời gian đặt cho quỹ đạo là te(s) để đi từ M đến N theo một đường thẳng. Với điều kiện đầu: ¿
Ta xác định được các hệ số như sau: ¿
Sơ đồ Simulink thiết kế quỹ đạo:
Hình 2. 5: Mô hình Simulink thiết kế quỹ đạo chuyển động Quỹ đạo các biến khớp:
Hình 2. 6: Quỹ đạo biến khớp khâu 1
Hình 2. 7: Quỹ đạo biến khớp khâu 2