Đồ án thiêt kế ROBOT SCORBOT ER4U

KHOA CƠ KHÍ Cộng hòa xã hội chủ nghĩa Việt Nam
Bộ môn Cơ điện tử Độc Lập – Tự Do – Hạnh Phúc
o0o

Đề tài: thiết kế robot SCORBOT-ER 4U trên hệ thống sản xuất tích hợp opencim

NỘI DUNG ĐỒ ÁN
1. Tổng quan về robot sẽ thiết kế
2. Tính toán động học
3. Tính toán động lực học(hoặc quỹ đạo chuyển động)
4. Mô phỏng robot
Ngày giao đề: …………………………………
Ngày hoàn thành: …………………………………
Trưởng Bộ Môn Giáo viên hướng dẫn
(Ký tên) (Ký tên)

LỜI NÓI ĐẦU

1
Khi xét về Robot chúng ta đặt ra các bài toán:động học Robot,động lực học
Robot và điều khiển Robot.đây là những bước cơ sở ban đầu hết sức quan trọng
trước khi thiết kế Robot.
Với sự phát triển như vũ bảo của công nghệ thông tin như ngày nay, rất
nhiều lĩnh vực trong cơ khí đả tận dụng được sự phát triển này để tao ra sự nhảy
vọt, trong đó có Robot công nghiệp.
Robot công nghiệp là môn học xương sống của ngành Cơ điện tử, đồ án
môn học bước đầu làm quen với việc thiết kế là kỹ năng cơ bản của người kỹ sư.
Trong số rất nhiều các sản phẩm Robot công nghiệp ứng dụng trong công nghiệp
thì robot là thiết bị rất điển hình về mức độ tích hợp và được coi là một tế bào
của hệ thống tự động linh hoạt. Trên quan điểm hệ thống điều khiển số nhiều
trục đồng thời, các hiểu biết về robot công nghiệp cũng đúng với các phần tử

khác như máy công cụ điều khiển số, trung tâm gia công…
Để thực hiện được đồ án này em xin chân thành cảm ơn cô Lê Thị Thu Thủy
và bộ môn cơ điện tử đả cho chúng e định hướng trong quá trình thực hiện đồ án
này.
Thái Nguyên, ngày 20 tháng 9 năm 2014
2
MỤC LỤC
Chương 1: TỔNG QUAN VỀ ROBOT CÔNG NGHIỆP
1.1.Lý do chọn đề tài
Robot ngày càng được ứng dụng rộng rãi trong các nghành công nghiệp trên
các dây truyền sản xuất tự động linh hoạt, dần thay thế cho con người ở nhiều vị
trí khác nhau.
Robot cần thực hiện các thao tác chính xác hoặc phối hợp chính xác với các
thiết bị khác theo nhịp sản xuất do đó nó đòi hỏi công nghệ sản xuất hiện đại,
tiên tiến và linh hoạt.
3
Hệ thống sản xuất tích hợp Cim (computer integrated manufacturing) là một
hệ thống sản xuất tự động hiện đại nó đang được ứng dụng phổ biến ở các nước
phát triển, tuy nhiên ở việt nam thì đang còn hạn chế, do vậy mục đích của đề
tài này nhằm giới thiệu công nghệ Cim, nhằm thúc đẩy việc ứng dụng công nghệ
Cim vào việt nam.
Phần mềm quản lý OpenCIM có thể thử nghiệm với tất cả các khí cạnh của
quản lý Cim với hệ thống phần mềm cho phép chúng ta sử dụng thử và kiểm tra
của các thay đổi về sơ đồ phân bố của phân xưởng hoặc thay đổi về phương
pháp điều khiển Cim trên hành vi xử lý hệ thống.Hệ thống Opencim có thể phối
hợp với nhiều máy móc và robot khác nhau đang sử dụng các bộ điều khiển thiết
bị, các bộ điều khiển thiết bị có thể do hãng cung cấp hoặc do người thiết kế cải
tiến cho phù hợp với bất kỳ loại máy móc nào.Hệ thống opencim cung cấp cửa
sổ hoạt động đầy đủ, một cách gần gũi với giải pháp MRP cho người dùng để tạo
ra sản phẩm bán tự động hoàn toàn. mặt khác, hệ thống này được sử dụng ứng

dụng chung, do vậy người dung đả có những hiểu biết nhất định có thể tạo ra các
sửa đổi khác nhau hoặc thay thế một modun opencim và tạo ra modun cho riêng
mình.
1.2.Sơ lược về lịch sử phát triển
Thuật ngữ robot được sinh ra từ trên sân khấu, không phải trong phân
xưởng sản xuất. Những robot xuất hiện lần đầu tiên trên ở trên NewYork vào
ngày 09/10/1922 trong vở “Rossum’s Universal Robot” của nhà soạn kịch người
Tiệp Karen Kapek viết năm 1921, còn từ robot là cách gọi tắt của từ robota -
theo tiếng Tiệp có nghĩa là công việc lao dịch.
Những robot thực sự có ích được nghiên cứu để đưa vào những ứng dụng
trong công nghiệp thực sự lại là những tay máy. Vào năm 1948, nhà nghiên cứu
Goertz đã nghiên cứu chế tạo loại tay máy đôi điều khiển từ xa đầu tiên, và cùng
năm đó hãng General Mills chế tạo tay máy gần tương tự sử dụng cơ cấu tác
4
động là những động cơ điện kết hợp với các cử hành trình. Đến năm 1954,
Goertz tiếp tục chế tạo một dạng tay máy đôi sử dụng động cơ servo và có thể
nhận biết lực tác động lên khâu cuối. Sử dụng những thành quả đó, vào năm
1956 hãng General Mills cho ra đời tay máy hoạt động trong công việc khảo sát
đáy biển. Năm 1968 R.S. Mosher, thuộc hãng General Electric, đã chế tạo một
thiết bịbiết đi có bốn chân, có chiều dài hơn 3m, nặng 1.400kg, sử dụng động cơ
đốt trong có công suất gắn 100 mã lực (hình 1.1)
Hình 1.1 robot 4 chân và xe tự hành thám hiểm bề mặt trăng
Cũng trong lĩnh vực này, một thành tựu khoa học công nghệ đáng kể đã đạt
được vào năm 1970 là xe tự hành thám hiểm bề mặt của mặt trăng Lunokohod 1
được điều khiển từ trái đất (hình 1.1).
Viện nghiên cứu thuộc Trường Đại học Stanford vào năm 1969 đã thiết kế
robot Shakey di động tinh vi hơn để thực hiện những thí nghiệm về điều khiển
sử dụng hệ thống thu nhận hình ảnh để nhận dạng đối tượng (hình 1.2). Robot
này được lập trình trước để nhận dạng đối tượng bằng camera, xác định đường
đi đến đối tượng và thực hiện một số tác động trên đối tượng.

5
Hình 1.2- Robot Shakey-robot đầu tiên nhận dạng đối tượng bằng camera
Năm 1952 máy điều khiển chương trình số đầu tiên ra đời tại Học Viện
Công nghệ Massachusetts (Hoa Kỳ). Trên cơ sở đó năm 1954, George Devol đã
thiết kế robot lập trình với điều khiển chương trình số đầu tiên nhờ một thiết bị
do ông phát minh được gọi là thiết bị chuyển khớp được lập trình. Joseph
Engelberger, người mà ngày nay thường được gọi là cha đẻ của robot công
nghiệp, đã thành lập hãng Unimation sau khi mua bản quyền thiết bị của Devol
và sau đó đã phát triển những thế hệ robot điều khiển theo chương trình. Năm
1962, robot Unmation đầu tiên được đưa vào sử dụng tại hãng General Motors;
và năm 1976 cánh tay robot đầu tiên trong không gian đã được sử dụng trên tàu
thám hiểm Viking của cơ quan Không Gian NASA của Hoa Kỳ để lấy mẫu đất
trên sao Hoả.
Trong hoạt động sản xuất, đa số những robot công nghiệp có hình dạng của
“cánh tay cơ khí”, cũng chính vì vậy mà đôi khi ta gặp thuật ngữ người máy -
tay máy trongnhững tài liệu tham khảo và giáo trình về robot.
1.3.Một số định nghĩa về robot
6
Robot công nghiệp là một tay máy tự động linh hoạt,được sử dụng để thay
thế từng phần hoặc toàn bộ các hoạt động cơ bắp và hoạt động trí tuệ của con
người với nhiều khả năng thích ngi khác nhau.
Viện Nghiên cứu robot Hoa Kỳ đưa ra một định nghĩa về robot như sau:
“Robot là một tay máy nhiều chức năng, thay đổi được chương trình hoạt động,
được dùng để di chuyển vật liệu, chi tiết máy, dụng cụ hoặc dùng cho những
công việc đặc biệt thông qua những chuyển động khác nhau đã được lập trình
nhằm mục đích hoàn thành những nhiệm vụ đa dạng” (Schlussel, 1985).
Định nghĩa của M.P.Groover về robot không dừng lại ở tay máy mà mở rộng
ra cho nhiều đối tượng khác có những đặc tính tương tự như con người như là
suy nghĩ, có khả năng đưa ra quy định và có thể nhìn thấy hoặc cảm nhận được
đặc điểm của vật hay đối tượng mà nó phải thao tác hoặc xử lý. Theo

Artobolevski I.I., Vorobiov M.V. và các nhà nghiên cứu thuộc trường phái khối
SEV trước đây thì phát biểu rằng: “Robot công nghiệp là những máy hoạt động
tự động được điều khiển theo chương trình để thực hiện việc thay đổi vị trí của
những đối tượng thao tác khác nhau với mục đích tự động hoá các quá trình sản
xuất”.
Robot công nghiệp là một tay máy vạn năng nhiều trục(số trục ít nhất là
3),chuyển động của cơ cấu chấp hành được điều khiển tự động (định nghĩa ttheo
tiêu chuẩn ISA TR 8373-1991)
Robot công nghiệp là một tay máy vạn năng hoạt động theo chương trình
và có thể lập trình lại để hoàn thành và nâng cao hiệu quả hoàn thành các nhiệm
vụ khác nhau trong công nghiệp,như vận chuyển nguyên vật liệu,chi tiết dụng cụ
hoặc các thiết bị chuyên dùng khác(định nghĩa theo đề xuất của viện nghiên cứu
robot của Mỹ).
7
1.4.Một số ứng dụng của Robot công nghiệp
1.4.1.Mục tiêu ứng dụng
Mục tiêu ứng dụng của robot công nghiệp là thay thế từng phần hoặc toàn
bộ lao động nặng nhọc của con người hoặc thay thế con người khi phải làm việc
trong điều kiện kỹ thuật,môi trường đặc biệt, đồng thời mang lại năng suất, chất
lượng và hiệu quả kinh tế cao.
Robot được sử dụng trong nhiều lĩnh vực sản xuất và đời sống của con
người trong đó robot công nghiệp đóng vai trò rất quan trọng và nó được ứng
dụng trong nhiều lĩnh vực công nghiệp như:
-phục vụ máy NC và các hệ thống tự động linh hoạt
-đúc,phun phủ,lắp ráp và đóng gói
-tự động hàn
-đảm nhận thực hiện cấp phôi phục vụ các nguyên công trong các dây
truyền sản xuất tự động
-chế tạo máy
-kiểm tra

-sơn
-bảo vệ
1.4.2.Phạm vi ứng dụng
Tình hình ứng dụng robot công nghiệp trên thế giới:nhìn chung viêc sử dụng
robot công nghiệp ở các nước phát triển như:Nhật, Mỹ, Italy, Pháp, Anh là rất
phổ biến và tốc độ phát triển hàng năm rất nhanh, theo thống kê chư đầy đủ tỷ lệ
phân bố phạm vi ứng dụng robot trong các lĩnh vực như sau:ô tô 31%;thiết bị
điện 16%;dầu khí,hóa thực phẩm 12%;thiết bị viễn thông 8%;máy công cụ
6%,kim khí 4%;máy tính 3% và các lĩnh vực khác chiếm khoảng 20%.
8
1.5.Tổng quan về SCORBOT ER-4U
1.5.1.Scorbot er-4u
Robot ScorBot ER-4U là một hệ thống hoạt động linh hoạt và đáng tin cậy
thích hợp cho việc giáo dục đào tạo robot công nghiệp, cánh tay robot ScorBot
ER-4u có thể được đặt lên bàn, bệ hoăc slidebase tuyến tính.
1.5.2.Chức năng của scorbot
Scorbot ER-4U là một robot mạnh mẽ sử dụng rễ ràng và hiệu quả cao, nó
được sử dụng rộng rãi trong các lĩnh vực sản xuất kỹ thuật như: vận chuyển,lắp
đặt ,nâng hạ,di chuyển các đối tượng…
Tốc độ được lặp lại, làm cho nó có thể thích hợp với các hoạt động độc lập và
sử dụng tích hợp trong các ứng dụng Workcell tự động như robot hàn,máy CNC
và các hoạt động chăm sóc FMS khác.
1.5.3.Thông số kỹ thuật
Một số thông số kỹ thuật:
-Bộ điều khiển:bộ điều khiển usb
-Cân nặng:10,8 kg
-Bán kính hoạt động: 610 mm
-Tải trọng tối đa: 2,5 kg
-Tốc độ tối đa: 2,2 lbs
-Đo kích thước của đối tượng bằng cảm biến Gripper và phần mềm.

-Nhiệt độ môi trường hoạt động: 2°-40°C
-Truyền động bởi 12 động cơ servo VDC
-Có 5 trục quay và 1 trục kẹp
Một số hình ảnh Robot SCORBOT ER -4U
9
Hình 1.3. SCORBOT ER-4U
1.6.Kết luận
Qua việc nghiên cứu đề tài, cho chúng ta biết thêm về sự phát triển và tầm
quan trọng của robot công nghiệp. Cho chúng ta biết thêm về hệ thống sản xuất
tiên tiến Opencim đề tài đả khái quát hóa và xây dựng được mô hình làm việc
cho hệ thống OpenCIM phục vụ các chương trình đào tạo,nghiên cứu khoa học
và chuyển giao công nghệ.
-Thiết lập được mô hình làm việc cho hệ thống Cim
-Thiết kế-tích hợp mô hình làm việc cho hệ thống
-Quản lý, điều khiển và vận hành cho hệ thống
-Ứng dụng mô hình vào sản xuất thực tế
10
Chương 2: TÍNH TOÁN ĐỘNG HỌC
2.1.Cơ sở lý thuyết
Động học nghiên cứu chuyển động nhưng không xét đến các lực hoặc các
mômen gây ra chuyển động. Động học chỉ xét vị trí, vận tốc, gia tốc và các đạo
hàm cấp cao của các biến vị trí theo thời gian hoặc theo các biến khác. Do đó
động học chỉ đề cập các tính chất hình học và thời gian của chuyển động. Các
biến khớp của cơ cấu chấp hành liên quan đến vị trí và định hướng của đầu tác
động theo các ràng buộc của các khớp đó. Các quan hệ động học này là tiêu
điểm nghiên cứu động học các cơ cấu chấp hành robot. Nghiên cứu này có thể
tiếp cận theo hai quan điểm giải tích động học và tổng hợp động học, hai quan
điểm này có liên quan chặt chẽ với nhau.
11
Khi xét về robot chúng ta đặt ra các bài toán: động học robot, động lực học

robot và điều khiển robot. Bài toán động học robot là cơ sở đầu vào cho bài toán
động lực học robot và điều khiển robot, bài toán động học robot gồm có 2 bài
toán động học thuận và bài toán động học ngược.
Nhiệm vụ của bài toán thuận là khi cho trước các biến khớp phải xác định
vị trí và định hướng của tất cả các khâu trên cánh tay, thông thường nếu khống
chế quỹ đạo của các khâu trên cánh tay nhằm tránh va chạm với các đối tượng
khác trong vùng làm việc, người ta thường chỉ xác định vị trí và định hướng
của khâu sau cùng.
Còn đối với bài toán động học ngược là bài toán cho trước hướng và vị trí
của mục tiêu mà bàn kẹp kẹp tới, nhiệm vụ của bài toán là xác định bộ giá trị
biến khớp cuối cùng, bài toán động học ngược có vai trò rất quan trọng vì nó
là thông số đầu vào cho bài toán động lực học.
2.2.Khảo sát động học
Sau khi việc mô hình hóa hoàn thành cần đề xuất được phương pháp và
công cụ thích hợp để khảo sát được mô hình đó một cách hiệu quả, với bài toán
động học robot có hai phương pháp cơ bản là phương pháp số và phương pháp
giải tích.
- Phương pháp giải tích cho phép rút nghiệm dưới dạng công thức toán trong đó
tọa độ suy rộng là hàm của các tọa độ thực và các tham số DH của robot, khi tọa
độ thực cập nhật tọa độ suy rộng cập nhật theo, điều này rất thuận tiện cho việc
lập trình, tuy nhiên có hai vấn đề có bản làm cho lời giải dưới dạng giải tích ít
được sử dụng, thứ nhất là không phải robot nào cũng có lời giải dưới dạng giải
tích, thứ hai là nếu robot có lời giải dưới dạng giải tích chưa chắc đã tìm được
lời giải đó vì nó đòi hỏi các kỹ năng làm toán sơ cấp, cụ thể là các nhận định
chính xác khi giải hệ phương trình liên kết của robot vốn rất dài và phức tạp.
12
- Phương pháp số là phương pháp tìm ra nghiệm gần đúng của bài toán bằng
cách thực hiện nhiều vòng lặp, nghiệm của của mỗi vòng lặp (mỗi lần giải) là
một bộ số thực, khác với phương pháp giải tích, phương pháp số luôn tìm ra
nghiệm chỉ cần cấu hình robot đáp ứng được và điểm yêu cầu nằm trong vùng

làm việc.
2.3.Bài toán giải tích động học
Giải tích động học nghiên cứu về đạo hàm, vi phân, tích phân của các
chuyển động tương đối. Có hai kiểu bài toán giải tích động học là động học
thuận và động học ngược. Trong lập trình cơ cấu chấp hành, tập hợp các vị trí và
định hướng mong muốn, các đạo hàm thời gian của vị trí và định hướng của đầu
tác động, được chuyên biệt trong không gian. Vấn đề là tìm mọi tập hợp khả dĩ
của các biến khớp động, là các đạo hàm thời gian tương ứng của chúng để đầu
tác động đạt vị trí và định hướng mong muốn với các đặc tính chuyển động theo
yêu cầu. Vấn đề này được gọi là động học ngược. Mặt khác, đôi khi các biến
khớp động và các đạo hàm thời gian của chúng có thể nhận được từ các cảm
biến lắp trong khớp, từ đó có thể tìm mọi tập hợp khả dĩ về vị trí và định hướng
của đầu tác động và các đạo hàm thời gian tương ứng của chúng. Điều này được
gọi là động học thuận. Vấn đề về động học thuận và động học ngược đều có thể
giải quyết bằng nhiều phương pháp giải tích, chẳng hạn giải tích véc tơ hình học,
đại số ma trận,…
2.4.Bài toán tổng hợp động học
Tổng hợp động học là quá trình ngược của giải tích động học. Trong trường
hợp này cần thiết kế cơ cấu chấp hành có các tính chất động học mong muốn.
Đặc biệt, với tập hợp các vị trí và định hướng của đầu chấp hành đã biết và các
đạo hàm của chúng trong không gian, cần xác định các biến khớp động tương
ứng, kiểu loại và dạng hình học của cơ cấu chấp hành. Vấn đề tổng hợp động
13
học có thể được chia thành ba giai đoạn liên quan là tổng hợp kiểu loại, tổng
hợp số lượng và tổng hợp chiều.
2.5.Bài toán động học thuận của Robot SCORBOT ER-4U
2.5.1.Thiết lập phương trình động học của tay máy
Phương trình động học tay máy được xây dựng bằng nhiều phương pháp.
nhưng đối với bài toán này sẽ áp dụng phương pháp xây dựng bài toán theo
phương pháp của Denavit- Hartenberg (DH). Việc gắn hệ tọa độ với các khâu

có vai trò rất quan trọng khi thiết lập hệ phương trình động học của robot. Và
việc xác định các hệ tọa độ cần phải phù hợp với phép biến đổi của ma trận Ai
để có thể sử dụng được bộ thông số DH. Vì thế, khi xác định các hệ tọa độ cần
lưu ý các điểm sau:
- Trục zi phải chọn cùng phương với trục khớp động i+1 .
- Các hệ tọa độ phải tuân theo quy tắc bàn tay phải.
Ta được ma trận tổng hợp tổng quát theo D-H là:
A
i
= R
(z,α)
T
(z,d)
T
(x,a)
R
(x,β)
= (2.1)
2.5.2.Thiết lập phương trình động học cho scorbot
Sơ đồ động học và các tọa độ được đặt lên các khớp của SCORBOT ER-4U như
hình 2.1
14

X0
Z0
Z1
X1
X2
Z3
X4

X3
Z4
d1
a1
a
2
a
3
Y5
Z5
d
5
x5
Z2
Hình 2.1 sơ đồ cấu trúc robot SCORBOT-ER 4U
Bảng thông số D-H của SCORBOT-ER 4U
R
(zi,α)
T
(zi,d)
T
(x(i+1),a)
R
(x(i+1),β)
1 (α
1
) d
1
a
1

90°
2 (α
2
) 0 a
2
0°
3 (α
3
) 0 a
3
0°
4 (α
4
) 0 0 90°
5 (α
5
) d
5
0 0°
Từ bảng thông số D-H thay vào 2.1 ta được các ma trận A
i
(ma trận chuyển từ
tọa độ 0 sang tọa độ i) như sau:
A
1
0
=
A
2
1

=
A
3
2
=
A
4
3
=
A
5
4
=
15
Trong đó : :C
1
=cosα
1
,C
2
=cosα
2
,C
3
=cosα
3
,C
4
=cosα
4

S
1
=sinα
1
, S
2
= sinα
2
,S
3
= sinα
3
,S
4
= sinα
4
Vậy ma trận mô tả hướng và vị trí của bàn kẹp là:
T
5
0
=A
1
0
A
2
1
A
3
2
A

4
3
A
5
4
=
[ ((cos(q1)*cos(q2)*cos(q3)-
cos(q1)*sin(q2)*sin(q3))*cos(q4)+(-cos(q1)*cos(q2)*sin(q3)-
cos(q1)*sin(q2)*cos(q3))*sin(q4))*cos(q5)+sin(q1)*sin(q5),
-((cos(q1)*cos(q2)*cos(q3)-cos(q1)*sin(q2)*sin(q3))*cos(q4)+(-
cos(q1)*cos(q2)*sin(q3)-
cos(q1)*sin(q2)*cos(q3))*sin(q4))*sin(q5)+sin(q1)*cos(q5),
(cos(q1)*cos(q2)*cos(q3)-cos(q1)*sin(q2)*sin(q3))*sin(q4)-(-
cos(q1)*cos(q2)*sin(q3)-cos(q1)*sin(q2)*cos(q3))*cos(q4),
100*(cos(q1)*cos(q2)*cos(q3)-cos(q1)*sin(q2)*sin(q3))*sin(q4)-100*(-
cos(q1)*cos(q2)*sin(q3)-
cos(q1)*sin(q2)*cos(q3))*cos(q4)+220*cos(q1)*cos(q2)*cos(q3)-
220*cos(q1)*sin(q2)*sin(q3)+220*cos(q1)*cos(q2)+70*cos(q1)]
[ ((sin(q1)*cos(q2)*cos(q3)-
sin(q1)*sin(q2)*sin(q3))*cos(q4)+(-sin(q1)*cos(q2)*sin(q3)-
sin(q1)*sin(q2)*cos(q3))*sin(q4))*cos(q5)-cos(q1)*sin(q5),
-((sin(q1)*cos(q2)*cos(q3)-sin(q1)*sin(q2)*sin(q3))*cos(q4)+(-
sin(q1)*cos(q2)*sin(q3)-sin(q1)*sin(q2)*cos(q3))*sin(q4))*sin(q5)-
cos(q1)*cos(q5),
(sin(q1)*cos(q2)*cos(q3)-sin(q1)*sin(q2)*sin(q3))*sin(q4)-(-
sin(q1)*cos(q2)*sin(q3)-sin(q1)*sin(q2)*cos(q3))*cos(q4),
100*(sin(q1)*cos(q2)*cos(q3)-sin(q1)*sin(q2)*sin(q3))*sin(q4)-100*(-
sin(q1)*cos(q2)*sin(q3)-
sin(q1)*sin(q2)*cos(q3))*cos(q4)+220*sin(q1)*cos(q2)*cos(q3)-
220*sin(q1)*sin(q2)*sin(q3)+220*sin(q1)*cos(q2)+70*sin(q1)]

16
[ ((sin(q2)
*cos(q3)+cos(q2)*sin(q3))*cos(q4)+(-
sin(q2)*sin(q3)+cos(q2)*cos(q3))*sin(q4))*cos(q5),
-((sin(q2)*cos(q3)+cos(q2)*sin(q3))*cos(q4)+(-
sin(q2)*sin(q3)+cos(q2)*cos(q3))*sin(q4))*sin(q5),
(sin(q2)*cos(q3)+cos(q2)*sin(q3))*sin(q4)-(-
sin(q2)*sin(q3)+cos(q2)*cos(q3))*cos(q4),
364+100*(sin(q2)*cos(q3)+cos(q2)*sin(q3))*sin(q4)-100*(-
sin(q2)*sin(q3)+cos(q2)*cos(q3))*cos(q4)+220*sin(q2)*cos(q3)+220*cos(q2)*sin(
q3)+220*sin(q2)]
[
0,
0,
0,
1]
Như vậy từ ma trận T
5
0
ta nhận được hệ phương trình động học thuận của
robot scorbot er 4u như sau:
n
x
=((cos(q1)*cos(q2)*cos(q3)-cos(q1)*sin(q2)*sin(q3))*cos(q4)+(-
cos(q1)*cos(q2)*sin(q3)-
cos(q1)*sin(q2)*cos(q3))*sin(q4))*cos(q5)+sin(q1)*sin(q5)
n
y
=((sin(q1)*cos(q2)*cos(q3)-sin(q1)*sin(q2)*sin(q3))*cos(q4)+(-
sin(q1)*cos(q2)*sin(q3)-sin(q1)*sin(q2)*cos(q3))*sin(q4))*cos(q5)-

cos(q1)*sin(q5)
n
z
=((sin(q2)*cos(q3)+cos(q2)*sin(q3))*cos(q4)+(-
sin(q2)*sin(q3)+cos(q2)*cos(q3))*sin(q4))*cos(q5)
s
x
=-((cos(q1)*cos(q2)*cos(q3)-cos(q1)*sin(q2)*sin(q3))*cos(q4)+(-
cos(q1)*cos(q2)*sin(q3)-
cos(q1)*sin(q2)*cos(q3))*sin(q4))*sin(q5)+sin(q1)*cos(q5)
s
y
=-((sin(q1)*cos(q2)*cos(q3)-sin(q1)*sin(q2)*sin(q3))*cos(q4)+(-
sin(q1)*cos(q2)*sin(q3)-sin(q1)*sin(q2)*cos(q3))*sin(q4))*sin(q5)-
cos(q1)*cos(q5)
17
s
z
=-((sin(q2)*cos(q3)+cos(q2)*sin(q3))*cos(q4)+(-
sin(q2)*sin(q3)+cos(q2)*cos(q3))*sin(q4))*sin(q5)
a
x
=(cos(q1)*cos(q2)*cos(q3)-cos(q1)*sin(q2)*sin(q3))*sin(q4)-(-
cos(q1)*cos(q2)*sin(q3)-cos(q1)*sin(q2)*cos(q3))*cos(q4)
a
y
=(sin(q1)*cos(q2)*cos(q3)-sin(q1)*sin(q2)*sin(q3))*sin(q4)-(-
sin(q1)*cos(q2)*sin(q3)-sin(q1)*sin(q2)*cos(q3))*cos(q4)
a
z

=(sin(q2)*cos(q3)+cos(q2)*sin(q3))*sin(q4)-(-
sin(q2)*sin(q3)+cos(q2)*cos(q3))*cos(q4)
p
x
=100*(cos(q1)*cos(q2)*cos(q3)-cos(q1)*sin(q2)*sin(q3))*sin(q4)-100*(-
cos(q1)*cos(q2)*sin(q3)-
cos(q1)*sin(q2)*cos(q3))*cos(q4)+220*cos(q1)*cos(q2)*cos(q3)-
220*cos(q1)*sin(q2)*sin(q3)+220*cos(q1)*cos(q2)+70*cos(q1)
p
y
=100*(sin(q1)*cos(q2)*cos(q3)-sin(q1)*sin(q2)*sin(q3))*sin(q4)-100*(-
sin(q1)*cos(q2)*sin(q3)-
sin(q1)*sin(q2)*cos(q3))*cos(q4)+220*sin(q1)*cos(q2)*cos(q3)-
220*sin(q1)*sin(q2)*sin(q3)+220*sin(q1)*cos(q2)+70*sin(q1)
p
z
=364+100*(sin(q2)*cos(q3)+cos(q2)*sin(q3))*sin(q4)-100*(-
sin(q2)*sin(q3)+cos(q2)*cos(q3))*cos(q4)+220*sin(q2)*cos(q3)+220*cos(q2)*
sin(q3)+220*sin(q2)
2.5.3.Kêt luận
Để biết được hướng và vị trí của bàn kẹp ta chỉ việc thay các biến khớp α
1
,
α
2
, α
3
, α
4
,α

5
và các tham số d
1
,d
5
,a
1
,a
2
,a
3
vào hệ phương trình động học thuận.
(d
1
=364mm,a
1
=70mm, a
2
=a
3
=220mm, d
5
=100mm)
2.6.Bài toán động học ngược Robot SCORBOT ER-4U
2.6.1.Đặt bài toán
Cho vị trí và hướng của bàn kẹp tức là biết ma trận T
E
. Cần phải xác định
các biến khớp αi(i=1 5) theo vị trí và hướng bàn kẹp.
18

2.6.2.Giải bài toán động học ngược cho scorbot.
Từ ma trận T
0
5
và T
E
ta được hệ phương trình động học nghịch:
(3.1)
Với a13, a33, a14, a24, a34 là các tọa độ thực đã biết
Và ,ax,ay,px,py,pz lấy từ bài toán động học thuận
Ta ứng dung gói công cụ solver trên exel để giải bài toán đông học ngươc:
Giả sử rằng đã cấu trúc được bài toán động học ngược, ở đây chỉ bàn đến
cách giải, có rất nhiều phương pháp số, trong số đó có những phương pháp rất
khó vận dụng, đòi hỏi phải có kỹ năng lập trình, dưới đây giới thiệu một phương
pháp không cần lập trình, rất dễ hiểu và hiệu quả cao, có thể ứng dụng giải quyết
bài toán động học ngược cho bất cứ robot nào.
Giải thuật được sử dụng ở đây là phương pháp giảm gradient tổng quát về
bản chất là một phương pháp có sử dụng đạo hàm. Do các tìm kiếm được thực
hiện theo hướng hàm giảm giá trị mạnh nhất, là hướng ngược với hướng của véc
tơ gradient nên kết quả được cải thiện mạnh nhất sau mỗi vòng lặp. Chương
trình ứng dụng cụ thể là gói Solver được tích hợp kèm theo Excel của MS
OFFICE. Chương trình này sẵn có trên bất cứ máy tính nào, tuy nhiên solver là
gói tùy chọn trong khi cài đặt nên nếu không lựa chọn cài đặt ngay từ đầu có thể
cần cài bổ xung khi muốn sử dụng.
Giải hệ phương trình (3.1) trên exel ta được các góc αi(i=1 5) tương ứng:
19
Hình 2.2.Kết quả bài toán ngược giải trên excell
2.6.3.Kết luận
Khi bài toán cho vị trí và hướng của mục tiêu ta chỉ việc thay các tọa độ
thực vào dòng 2 trong chương trình exel ta sẽ được bộ thông số của các biến

khớp tương ứng ở dòng 4.(Hình 2.2)
20
Chương 3: NỘI SUY QUỸ ĐẠO CHUYỂN ĐỘNG
3.1.Nội suy quỹ đạo trong không gian khớp
3.1.1.Khái niệm về chi phí tính toán
Để hoàn thành một bài toán cần thực hiện các phép tính, việc này đòi hỏi
phần cứng máy tính dự trữ một lượng tài nguyên (ram, chip, ổ ứng ) nhất định
cho việc đó. Vì tài nguyên công nghệ thông tin tích hợp sẵn theo robot chỉ ở
mức trung bình nên việc sử dụng hợp lý để hoàn thành được bài toán đặt ra là
cần thiết.
Vì mục tiêu cần hoàn thành của bài toán động học là quỹ đạo chuyển động
trong không gian khớp, có hai cách để làm việc này khi sử dụng phương pháp số
giải bài toán động học ngược:
- Lấy số lượng điểm keypoint lớn sau đó nối giữa hai điểm kề nhau bằng đường
thẳng;
21
- Lấy điểm keypoint ở các vị trí quan trọng trên quỹ đạo trong không gian công
tác, giải bài toán ngược sau đó nội suy bằng đa thức bậc cao.
So sánh hai cách làm trên chi phí tính toán của việc giải lặp lại hệ phương
trình động học sẽ lớn hơn nên thường đi theo hướng thứ hai, dưới đây trình bày
nguyên tắc toán để xây dựng mô hình nội suy.
Ở đây chúng ta thực hiện bằng cách lấy điểm keypoint ở các vị trí quan trọng
trên quỹ đạo trong không gian công tác, giải bài toán ngược sau đó nội suy bằng
đa thức bậc cao.
3.1.2.Giới thiệu và cơ sở nội suy quỹ đạo
Giả sử robot đang chuyển phôi từ điểm A sang điểm B trong không gian làm
việc, biết tọa độ (X
E
,Y
E

,Z
E
) và hướng của khâu thao tác, thiết lập phương trình
nội suy của các biến khớp khi chuyển động từ A tới B.
Quỹ đạo là các đường cong dạng:
+Đa thức bậc 2 : R
(t)
= at
2
+bt+c
+Đa thức bậc 3 : R
(t)
= at
3
+bt
2
+ct+d
+Đa thức bậc cao : R
(t)
=at
n
+bt
n-1
+…+kt+m
Ta sử dụng dạng đa thức bậc 3 để nội suy.
Để thuận lợi cho điều khiển, nếu xác định đồ thị gia tốc không có bước nhảy
nó cần có bậc tối thiểu là bậc ba với mỗi phân tố trong trường hợp tổng quát.
Tính theo mức đó trên cơ sở quan hệ đạo hàm vận tốc cần có dạng bậc 2 và
chuyển vị có dạng đa thức bậc 3, một đa thức bậc 3 đầy đủ có 4 ẩn cần dựa vào
hai loại điều kiện biên trong đó:

22
* Điều kiện đi qua dùng cho hai đầu.
* Điều kiện vận tốc liên tục ( điều kiện liên tục tiếp tuyến hay liên tục loại một)
áp dụng cho hai đầu.
Trong đó điều kiện đi qua căn cứ vào kết quả bài toán ngược, điều kiện liên
tục vận tốc với giá trị vận tốc trung gian tại điểm chuyển tiếp tính toán theo quan
điểm dung hòa độ uốn của hai phân tố kề nhau, cụ thể:
Xét ba điểm lần lượt là
),(
1-1-1- iii
qtp
;
),(
iii
qtp
;
),(
111 +++ iii
qtp
lại gọi
1
,
+ii
kk
là hệ số góc
của hai dây cung nối ba điểm đó, công thức tính các hệ số góc các dây này cho bởi:
1-
1-
-
-

ii
ii
i
tt
qq
k =
;
ii
ii
i
tt
qq
k
-
-
1
1
1
+
+
+
=
Xét hai tình huống xảy ra:
Nếu
2
⇒)()(
1
1
+
+

+
==
ii
cii
kk
kksigksig
Nếu
0⇒)(≠)(
1
=
+ cii
kksigksig
Xét ba điểm theo thứ tự là:
),(
1-1-1- iii
qtp
;
),(
iii
qtp
;
),(
111 +++ iii
qtp
gọi hai đa thức bậc ba
đầy đủ sau đây là hai phân tố lần lượt đi qua hai điểm p
i-1
, p
i
và p

i
, p
i+1
:
R
(u)
= a
i
u
3
+b
i
u
2
+c
i
u+d
i
Trong đó u là thời gian quy đổi từ t là thời gian thực, cụ thể như sau, R
i
(u) là phân
tố đi qua hai điểm
),(
1-1-1- iii
qtp
;
),(
iii
qtp
công thức đổi biến:

1-
1-
-
-
ii
i
tt
tt
u =
trong đó vì
),(∈
1- ii
ttt
nên
)1,0(∈u
Với đoạn còn lại:
R
(u)
= a
i+1
u
3
+b
i+1
u
2
+c
i+1
u+d
i+1

23

ii
i
tt
tt
u
-
-
1+
=
Xét mô hình chuyển tiếp trơn giữa hai đoạn bậc ba nói trên, toàn bộ mô hình gồm 8
ẩn số cần có một lượng phương trình độc lập tương ứng:
- Điều kiện đi qua (sử dụng dữ liệu từ bài toán động học nghịch):
R
i(u=0)
=q
i-1
=d
i
(3.1)
R
i(u=1)
=q
i
=a
i
+b
i
+c

i
+d
i
(3.2)
R
i+1(u=0)
=q
i
=d
i+1
(3.3)
R
i+1(u=1)
=q
i+1
=a
i+1
+b
i+1
+c
i+1
+d
i+1
(3.4)

- Điều kiện liên tục vận tốc tại điểm chuyển tiếp :
R’
i(u=0)
=k
c-1

=c
i
(3.5)
R’
i(u=1)
=k
c
=3a
i
+2b
i
+c
i
(3.6)
R’
i+1(u=0)
=k
c
=c
i+1
(3.7)
R’
i+1(u=1)
=k
c+1
=3a
i+1
+2b
i+1
+c

i+1
(3.8)
Tập hợp tất cả các phương trình từ (3.1) đến (3.8) cho phép xác định chính xác 8 ẩn
giả định và vẽ được các đặc tính chuyển vị, vận tốc, gia tốc trơn đều trong khoảng
thời gian
),(
11- +ii
tt
.
3.1.3.Áp dụng nội suy quỹ đạo cho robot scorbot er-4u
Căn cứ vào kiểu dáng và kích thước vùng làm việc của robot, bố trí vị trí phôi
trong vùng làm việc theo sơ đồ sau:
24
Tram A
Tram B
9
0
364
70
2
2
0
2
2
0
100
3
0
0
200

3
0
0
OB
OA
Hình 3.1.Sơ đồ động học khi chuyển phôi từ trạm A sang trạm B
Căn cứ vào toàn bộ quá trình chuyển phôi từ tạm A sang trạm B cần lấy toàn
bộ chu trình tối thiểu cần 9 điểm keypoint ở các vị trí quan trọng trên quỹ đạo
trong không gian công tác được mô tả như sau:
P
1
: (Home) đây là điểm khởi xuất làm việc từ tư thế nghỉ mở kẹp cách vật
100mm (vật có đường kính là 40mm)
P
1
=
P
2
: Bàn tay dừng trên trạm A với vật đã được định vị đúng cách vật 50(mm).
P
2
=
P
3
:Bàn tay đi xuống 70(mm) theo phương thẳng đứng để kẹp chặt vật.
P
3
=
P
4

:Bàn tay nhấc vật lên 70(mm) theo phương thẳng đứng.
P
4
=
25