BỘ LAO ĐỘNG – THƯƠNG BINH XÃ HỘI
ĐẠI HỌC SƯ PHẠM KỸ THUẬT VĨNH LONG

KHĨA LUẬN TỐT NGHIỆP ĐẠI HỌC
CƠNG NGHỆ KỸ THUẬT ĐIỀU KHIỂN VÀ TỰ ĐỘNG HĨA

MƠ PHỎNG VÀ ĐIỀU KHIỂN
CÁNH TAY ROBOT 5 BẬC

CBHD: ThS. MAI NHẬT THIÊN
Sinh viên:
1

Họ và tên: CHÂU TRUNG TÍN
MSSV: 16007030

2

Họ và tên: NGUYỄN TẤN THÀNH
MSSV: 16007016

Vĩnh Long – năm 2019


LỜI CẢM ƠN
Để hồn thành khóa luận tốt nghiệp và kết thúc khóa học, với tình cảm chân
thành, chúng em xin bày tỏ lịng biết ơn sâu sắc tới tồn thể cán bộ, nhân viên,
giảng viên trường Đại học Sư phạm Kỹ thuật Vĩnh Long nói chung và quý giảng
viên khoa Điện - Điện tử nói riêng đã tạo điều kiện cho chúng em có mơi trường
học tập tốt trong suốt thời gian chúng em học tập, nghiên cứu tại trường.
Chúng em xin gửi lời cảm ơn tới thầy Mai Nhật Thiên đã giúp đỡ chúng em

trong suốt quá trình nghiên cứu và trực tiếp hướng dẫn để chúng em hồn thành
khóa luận tốt nghiệp này. Bên cạnh đó, chúng em cũng cảm ơn sự hỗ trợ, động viên
của gia đình và bạn bè chính là động lực to lớn để chúng em cố gắng thật tốt.
Chúng em xin chân thành cảm ơn!


NHẬN XÉT
(của giảng viên hướng dẫn)
 Ý thức thực hiện:

………………………………………………………………………………………..
.
………………………………………………………………………………………..
.
………………………………………………………………………………………..
.
………………………………………………………………………………………..
.
 Nội dung thực hiện:

………………………………………………………………………………………..
.
………………………………………………………………………………………..
.
………………………………………………………………………………………..
.
………………………………………………………………………………………..
.
 Hình thức trình bày:


………………………………………………………………………………………..
.
………………………………………………………………………………………..
.
………………………………………………………………………………………..
.
………………………………………………………………………………………..
.
 Tổng hợp kết quả:

………………………………………………………………………………………..
.
………………………………………………………………………………………..
.


………………………………………………………………………………………..
.
………………………………………………………………………………………..
.
Tổ chức báo cáo trước hội đồng
Tổ chức chấm thuyết minh
Vĩnh Long, ngày…tháng…năm
2019
Giảng viên hướng dẫn

MAI NHẬT THIÊN
DANH MỤC CÁC TỪ VIẾT TẮT
ST
T

1
2
3
4
5
6

Chữ viết tắt
DOF
DH
parameters
FK
IK
c
s

Chữ viết đủ

Dịch nghĩa

Degree of Freedom
Denavit-Hartenberg parameters

Bậc tự do
Thông số DH

Forward Kinematic
Inverse Kinematic
Cosine
Sine


Động học thuận
Động học nghịch
Hàm Cosin
Hàm Sin


DANH MỤC CÁC BẢNG


DANH MỤC HÌNH ẢNH


MỤC LỤC


LỜI NĨI ĐẦU
Trong thời đại của cơng nghiệp hóa, robot được ứng dụng vào hầu hết các
quá trình sản xuất công nghiệp. Việc sử dụng robot giúp cho hoạt động sản xuất
được nhanh chóng, chính xác và mang lại hiệu quả kinh tế cao. Một trong những
robot phổ biến nhất trong sản xuất là cánh tay robot. Cánh tay robot trong hầu hết
các trường hợp được lập trình và sử dụng để thực hiện các nhiệm vụ cụ thể, phổ
biến cho sản xuất, chế tạo và các ứng dụng công nghiệp.
Nắm bắt được sự hữu ích cũng như yêu cầu cấp thiết về việc phát triển
Robot, nhóm đã chọn đề tài “Mô phỏng và điều khiển cánh tay robot 5 bậc” để
nghiên cứu và phát triển khóa luận tốt nghiệp.
Khóa luận tốt nghiệp chúng em đi từ xây dựng hàm tốn, mơ phỏng sau đó
điều khiển thực tế, các bước được thực hiện lần lượt qua các chương:
Chương 1: Tổng quan
Chương 2: Xây dựng hàm toán cánh tay robot 5 bậc

Chương

3: Mơ

phỏng

bằng

phần

mềm

EasyRob,

Solidwork,

Matlab/Simulink điều khiển cánh tay robot.
Chương 4: Lập trình Arduino điều khiển cánh tay robot bằng Smartphone
thông qua giao tiếp Bluetooth; trình điều khiển thơng qua giao tiếp USB.
Chương 5: Kết quả

8


Chương 1

TỔNG QUAN

1.1 Cánh tay Robot:
 Giới thiệu cánh tay robot:


Cánh tay Robot là một trong những robot phổ biến nhất trong nền công
nghiệp sản xuất. Cánh tay robot trong hầu hết các trường hợp được lập trình và sử
dụng để thực hiện các nhiệm vụ cụ thể, phổ biến cho sản xuất, chế tạo và các ứng
dụng cơng nghiệp.

Hình 1.1 Cánh tay robot lắp ráp ô tô tại nhà máy Vinfast
Cánh tay robot là một thiết bị hoạt động theo cách tương tự như cánh tay
người, với một số khớp có thể di chuyển dọc theo trục hoặc có thể xoay theo một số
hướng nhất định. Trên thực tế, một số cánh tay robot được cấu tạo và lập trình hoạt
động với cách bắt chước các chuyển động chính xác của cánh tay con người.
Chúng cũng giống như cánh tay của chúng ta, với một cổ tay, cẳng tay,
khuỷu tay và khớp vai. Tuy nhiên, cánh tay robot 5 bậc tự do, được cấu tạo từ các
trục và khớp nối cho phép nó di chuyển, cử động theo vị trí hoặc tọa độ mong
muốn.
 Ưu điểm cánh tay Robot:
•

Các chuyển động chính xác và lặp lại giống nhau.

•

Hoạt động xun suốt; trong mơi trường độc hại, nguy hiểm.

•

Khả năng tự động hóa và hoạt động theo module tốt.
9



1.2 Phần mềm hỗ trợ mơ phỏng và lập trình:
1.2.1 Phần mềm MATLAB:
MATLAB (matrix laboratory) là phần mềm cung cấp mơi trường tính tốn
số và lập trình, do cơng ty MathWorks thiết kế. MATLAB cho phép tính tốn số với
ma trận, vẽ đồ thị hàm số hay biểu đồ thông tin, thực hiện thuật toán, tạo các giao
diện người dùng và liên kết với những chương trình máy tính viết trên nhiều ngơn
ngữ lập trình khác.
Với thư viện Toolbox, MATLAB cho phép mơ phỏng tính tốn, thực nghiệm
nhiều mơ hình trong thực tế và kỹ thuật.
1.2.2 Phần mềm Proteus:
Proteus Design Suite là bộ công cụ phần mềm độc quyền được sử dụng chủ
yếu cho tự động hóa thiết kế điện tử. Phần mềm được sử dụng chủ yếu bởi các kỹ
sư thiết kế điện tử và kỹ thuật viên để tạo sơ đồ và bản in điện tử để sản xuất bảng
mạch in. Bên cạnh đó Proteus cũng hỗ trợ người dùng mơ phỏng mạch hỗ trợ cho
việc trình diễn, kiểm chứng,...
1.2.3 Phần mềm SolidWorks:
SolidWorks là phần mềm thiết kế 3D, phần mềm cũng cho phép mô phỏng
các hoạt động của các vật thể hoặc khối vật thể. Đặc biệt, phần mềm có khả năng
liên kết các phần mềm tính tốn để mơ phỏng chính xác các chuyển động vật thể
theo các hàm toán học,.. như MATLAB.
1.2.4 Phần mềm EasyRob:
EasyRob là công cụ mô phỏng robot sử dụng đồ hoạ trong khơng gian 3
chiều (3D) và các hình ảnh có thể hoạt động được. Một hệ thống 3D-CAD đơn giản
cho phép tạo ra các khối hình học cơ bản như khối trụ, khối cầu, khối chữ nhật, khối
tam giác, khối hình thang,... để vẽ kết cấu của robot. Trong EasyRob chúng ta có thể
dùng chuột để quay hoặc tịnh tiến robot đến một tọa độ tuỳ ý. EasyRob cũng có các
chức năng phóng to, thu nhỏ đối tượng vẽ như nhiều phần mềm thiết kế khác…
10



1.2.5 Phần mềm Python:
Python là một ngơn ngữ lập trình bậc cao cho các mục đích lập trình đa năng,
do Guido van Rossum tạo ra và lần đầu ra mắt vào năm 1991. Python được thiết kế
với ưu điểm mạnh là dễ đọc, dễ học và dễ nhớ. Python là ngơn ngữ có hình thức rất
trực quan, cấu trúc rõ ràng, thuận tiện cho người mới học lập trình.
1.3 Cấu trúc mơ hình cánh tay robot 5 bậc:
1.3.1 Bo Arduino:
Bo Arduino Uno là một bo vi điều khiển nguồn mở dựa trên vi điều khiển
Microchip ATmega328P và được phát triển bởi Arduino, được trang bị các bộ chân
đầu vào/đầu ra kỹ thuật số và tương tự có thể được giao tiếp với các bo mở rộng
khác nhau và các mạch khác.

Hình 1.2 Chi tiết thành phần trên Arduino Uno R3
 Thông số kỹ thuật:

Bảng 1.1 Thông số kỹ thuật Arduino Uno R3
Vi điều khiển

ATmega328 họ 8 bit

Điện áp hoạt động

5V DC (chỉ được cấp qua cổng USB)

Tần số hoạt động

16 MHz

Dòng tiêu thụ


khoảng 30mA
11


Điện áp vào khuyên dùng

7-12V DC

Điện áp vào giới hạn

6-20V DC

Số chân Digital I/O

14 (6 chân hardware PWM)

Số chân Analog

6 (độ phân giải 10bit)

Dòng tối đa trên mỗi chân I/O 30mA
Dòng ra tối đa (5V)

500 mA

Dòng ra tối đa (3.3V)

50 mA

Bộ nhớ flash


32 KB (ATmega328) với 0.5 KB dùng bởi
bootloader

SRAM

2 KB (ATmega328)

EEPROM

1 KB (ATmega328)

1.3.2 Các linh kiện, vật tư của mơ hình:


Nguồn:
Bình Acquy
Thơng số kỹ thuật:
Kích thước: Dài 70mm x rộng 47mm x cao100mm
Điện áp: 6V
Dung lượng: 5Ah

Hình 1.3 Acquy Globe 6V-5Ah
 Mạch giảm áp:
Thông số kỹ thuật:
Điện áp đầu vào: 5 32 VDC
Điện áp đầu ra: điều chỉnh 1.25 30VDC
Dịng ra tối đa: 5A (trung bình 3.5A)
Độ gợn sóng: 30mV
Hình 1.4 Mạch giảm áp DC XL4005

 Module Arduino UNO Shield:
Hình 1.5 Bo Arduino Shield
mở rộng cho Arduino Uno
12


Bo có tính năng như một đế ra chân các chuẩn giao tiếp Digital, Analog,
UART, I2C, LCD, Servo... để kết nối các module, cảm biến để thực hiện nhiều
chức năng khác nhau.


Bo Bluetooth HC-05:
Thông số kỹ thuật:
Điện áp hoạt động: 3.3 5 VDC
Mức điện áp chân giao tiếp: tương thích 3.3 VDC
Dòng điện hoạt động: Ghép nối 30 mA, truyền nhận 8
mA

Tốc độ Baud 4800,9600,19200,38400,57600,115200
Tần số: 2.4 GHz
Hình 1.6 Bo Bluetooth HC-05
 Mơ hình cánh tay robot 5 bậc:

Hình 1.7 Mơ hình cánh tay robot 5 bậc
Vật liệu:
- Khung cánh tay được gia công bằng hợp kim nhôm và sơn tĩnh điện.
- Các động cơ servo MG996R chịu được nhiệt khi bị kẹt.

13



Chương 2
XÂY DỰNG HÀM TOÁN CÁNH TAY ROBOT 5 BẬC
2.1 Kiến thức chung:
Động học là mô tả chuyển động mà không xét đến các lực gây ra. Động học
nghiên cứu vị trí, vận tốc, gia tốc và các dẫn xuất cao hơn của biến vị trí. Đối với
robot động học được chia thành hai dạng: động học thuận và động học nghịch.
- Động học thuận sẽ xác định tay Robot sẽ ở đâu nếu biết tất cả các khớp.
- Động học nghịch sẽ tính tốn giá trị mỗi biến số của khớp nếu xác định vị
trí và hướng mong muốn của bộ phận đầu cuối.
Do đó, động học thuận được định nghĩa là chuyển đổi từ không gian khớp
nối sang không gian Descartes. Trong khi, động học nghịch được định nghĩa là
chuyển đổi từ không gian Descartes sang không gian khớp nối.
2.2 Mơ hình tốn động học thuận:
Vấn đề động học thuận là các biến số chung của robot, xác định vị trí và
hướng của bộ phận đầu cuối. Vì mỗi khớp có một mức độ tự do riêng nên hoạt động
của mỗi khớp có thể được mơ tả bằng một số duy nhất, tức là q 1,q2….,qn, góc quay
là khớp xoay. Mục tiêu của phân tích động học thuận là xác định giá trị của các biến
khớp.
Giả sử một robot có n+1 liên kết được đánh số từ 0 đến n, bắt đầu từ đế của
robot, được lấy là liên kết 0. Các khớp được đánh số từ 1 đến n và z i là một vectơ
đơn vị dọc theo trục trong không gian, kết nối khớp i-1 và i. Biến khớp thứ i được
ký hiệu là q, trong trường hợp khớp quay trịn, q là góc quay, nếu trường hợp khớp
hình lăng trụ, q là khoảng tịnh tiến. Mỗi khung tọa độ được gắn chặt vào từng liên
kết, chọn các khung từ 1 đến n sao cho khung i được gắn chặt vào liên kết i. Hình
2.1 minh họa ý tưởng gắn khung vào các liên kết trong trường hợp robot 5 bậc (bỏ
qua tay gấp).
Ma trận đồng nhất được xác định để biến đổi tọa độ của một điểm từ khung i
sang khung i-1. Ma trận không phải là hằng số, vì thế ma trận sẽ thay đổi khi cấu
14



hình của robot được thay đổi. Tuy nhiên, giả định rằng tất cả các khớp đều quay
vòng hoặc tịnh tiến có nghĩa là một hàm chỉ có một biến khớp duy nhất, cụ thể là qi.

Ti i −1 = Ti i −1 (q i )

(1)

Hình 2.1 Sự phân bố khung cánh tay robot 5 bậc
Ti i −1

là ma trận đồng nhất được xác định để biến đổi tọa độ của một điểm từ

khung i sang khung i-1. Ma trận

Ti i −1

khơng phải là hằng số, vì thế thay đổi khi cấu

hình của robot được thay đổi. Tuy nhiên, giả định rằng tất cả các khớp đều quay
vịng hoặc hình lăng trụ có nghĩa là

Ti i −1

là một hàm chỉ có một biến khớp duy nhất,

cụ thể là qi.

Ti i −1 = Ti i −1 (q i )


(1)

Ma trận đồng nhất biến đổi tọa độ của một điểm từ khung i sang khung j
được ký hiệu là

Ti j (i > j )

. Biểu thị vị trí và hướng của bộ phận tác động cuối đối
15


với khung quán tính hoặc khung đế bằng một vectơ ba chiều
3x3

Rn0

d n0

và ma trận xoay

, tương ứng, xác định ma trận đồng nhất

 Rn0
T =
0

d n0 

1


0
n

(2)

Sau đó, vị trí và hướng của bộ phận đầu cuối trong khung quán tính được đưa ra bởi

Tn0 (q1 , q2 ,...., qn ) = T10 ( q1 )T21 (q1 )...Tnn −1 ( qn )

(3)

Mỗi phép biến đổi đồng nhất Tii-1 có dạng

Ti

i −1

 Rii −1
=
 0

dii −1 

1 

(4)

Vì thế


Ti = T ...Ti
j

Ma trận

R ij

j
i +1

i −1

 Ri j
=
0

di j 

1

biểu thị hướng của khung i so với khung j (i> j) và được cho bởi các

phần quay của ma trận

T ji

(i> j) là

Ri j = Ri +j 1...R ii −1
Các vectơ


d ij

(6)

(i> j) được cho đệ quy theo công thức

di j = di +j 1 + R ij−1 d ii −1
•

(5)

(7)

Quy định của các khung tọa độ:
16


Cánh tay robot có năm khớp xoay và tay gắp di chuyển như trong Hình 2.1.
Khớp 1 biểu thị vai và trục chuyển động của nó là z 1. Khớp này cung cấp chuyển
động góc θ1 xoay quanh trục z1 trong mặt phẳng x1y1. Khớp 2 được xác định là
Cánh tay trên và trục của nó vng góc với trục 1 khớp. Nó cung cấp một chuyển
động góc θ2 xoay quanh trục z2 trong mặt phẳng x2y2. trục z3 của khớp 3 (cẳng tay)
và khớp 4 (cổ tay) song song với trục 2 khớp z; chúng cung cấp các chuyển động
góc θ3 và θ4 trong các mặt phẳng x3y3 và x4y4 tương ứng.
Khớp 5 được xác định là vòng quay tay gắp. Trục z 5 của nó thẳng đứng với
trục z4 và nó cung cấp θ5 chuyển động góc trong mặt phẳng x 5y5. Toạ độ khung của
khớp trong hình 2.2.
Tay robot 5 bậc được mơ tả hồn tồn trong khơng gian bởi vị trí của nó đối
với khung tham chiếu và hướng của nó.

Hình 2.2 Các khung tọa độ của Cánh tay robot 5

bậc
Nhiều phương pháp

có
thể
được sử dụng trong tính tốn động
học

thuận. Phân tích Denavit-Hartenberg là một trong những phương
pháp được sử dụng nhiều nhất, trong phương pháp này, động
học thuận được xác định từ một số tham số phải được xác định, tùy
thuộc vào từng cơ chế.
Tuy nhiên, nó đã được chọn để sử dụng phép biến đổi đồng nhất
ma trận. Phân tích để xác định một phép biến đổi tọa độ giữa

hai

khung, trong đó vị trí và hướng được cố định với khung kia, có

thể sử dụng các phép toán biến đổi đồng nhất sơ cấp. Các tham số DH của cánh tay
robot này được xác định cho các khung được gán trong Bảng 2.1
i
1
2
3

Bảng 2.1 Thông số DH cho cánh tay robot 5 bậc
αi-1

ai-1
di
0
0
d1
90
0
0
0
a3
0
17

θi
θ 1*
θ 2*
θ 3*


4
0
a4
0
(θ 4-90)*
5
-90
0
d5
θ 5*
6

0
0
0
Gripper
Bằng cách thay thế các tham số từ Bảng 1 thành phương trình (2.4), ma trận
biến đổi từ T1 đến T6 có thể thu được như hình dưới đây. Ví dụ: T 1 hiển thị phép
biến đổi giữa các khung 0 và 1 (chỉ định Ci là cosθi và Si là sinθi, v.v.).

cθ1

s
T10 =  θ1
0

 0

− sθ1

 cθ2

0
T21 = 
 sθ
 2
 0

− sθ2

cθ3


s
2
T3 =  θ3
0

 0

− sθ3

cθ4

s
T43 =  θ4
0

 0

− sθ4

cθ1
1
0

0
cθ2
0

cθ3
0
0


cθ4
0
0

0

0 0
0 d1 

0 1 
0

0

−1 0 
0 0

0 1 

(8)

0

0 a3 

0 0
1 0

0 1 


0 a4 

0 0
1 0

0 1 

18

(9)

(10)

(11)


cθ5

0
4
T5 = 
 sθ
 5
 0

TGripper

cθ5


s
=  θ5
0

 0

− sθ5
0
cθ5
0

− sθ5
cθ5
0
0

0

1 d5 
0 0

0 1 
0

(12)

0 0

0 0
1 0


0 1 

(13)

Sử dụng các giá trị trên của ma trận biến đổi; các phép biến đổi liên kết được
nối (nhân với nhau) để tìm phép biến đổi đơn liên quan đến khung (5) với khung
(0):

 nx
n
0
0 1 2 3 4
T5 = T1 T2 T3 T4 T5 =  y
 nz

0

ox
oy
oz
0

ax
ay
az
0

dx 
d y 

dz 

1

(14)

Phép biến đổi được đưa ra bởi phương trình (14) là hàm của cả 5 biến khớp.
Từ vị trí khớp robot, vị trí và hướng của Cartesian của liên kết cuối cùng có thể
được tính bằng phương trình trên (14).
Ba cột đầu tiên trong ma trận đại diện cho hướng của các hiệu ứng cuối,
trong khi cột cuối cùng đại diện cho vị trí của các hiệu ứng cuối.
Mô phỏng và giao diện của 5 hướng robot và vị trí của cánh tay robot 5DOF có thể được tính theo các góc của khớp bằng cách sử dụng:

nx = ((c1c2 c3 − c1s2 s3 )c4 + (−c1c2 s3 − c1s2 c3 ) s4 )c5 + s1s5
n y = ((s1 c2 c3 − s1s2 s3 )c4 + (− s1c2 s3 − s1s2 c3 ) s4 )c5 + c1s5
nz = ((s 2 c3 + c2 s3 )c4 + (− s2 s3 + c2c3 ) s4 )c5
(15)

19


ox = −((c1c 2 c3 − c1s2 s3 )c4 + ( −c1c2 s3 − c1s2c3 ) s4 ) s5 + s1c5
o y = −((s1c 2 c3 − s1s2 s3 )c4 + (− s1c2 s3 − s1s2 c3 ) s4 )s5 − c1c5
oz = ((c 2 c3 − s2 s3 ) s 4 ) s5 − (s 2 c3 + c2 s3 ) c 4 )

(16)

ax = −(c1c2 c3 − c1s2 s3 ) s4 + (−c1c2 s3 − c1s2 s3 ) c 4
a y = −(s 2 c2 c3 − s1s2 s3 ) s4 + (− s1c2 s3 − s1s2 c3 ) c4
az = −(c2 c3 − s2 s3 ) c4 − ( s2 c3 + c2 s3 ) s4


(17)

d x = (−(c1c2c3 − c1s2 s3 ) s4 + (−c1c2 s3 − c1s2 s3 )c4 )d 5 + (c1c2c3 − c1s2 s3 ) a 4 + c1c2a3
d y = (−(s1c2c3 − s1s2 s3 )s4 + (− s1c2 s3 − s1s2c3 )c4 )d 5 + (s1c2c3 − s1s2 s3 ) a 4 + s1c2 a3
d z = (−( s2c3 − c2 s3 ) s4 + (− s2 s3 + c2c3 )c4 )d 5 + (s2c3 − c2 s3 ) a 4 + s2 a3 + d1

(18)

2.3 Động học nghịch:
Phân tích động học nghịch (IK) xác định các góc khớp cho vị trí và hướng
mong muốn trong không gian Cartesian. Tổng ma trận biến đổi trong phương trình
(14) sẽ được sử dụng để tính tốn phương trình động học nghịch đảo. Động học
nghịch là vấn đề khó khăn hơn so với động học thuận.
Giải pháp của động học nghịch phức tạp hơn động học thuận và khơng có bất
kỳ phương pháp giải pháp phân tích tồn cầu nào. Mỗi người thao tác cần một
phương pháp cụ thể xem xét cấu trúc hệ thống và các hạn chế. Có hai cách tiếp cận
giải pháp là hình học và đại số được sử dụng để tạo ra giải pháp động học nghịch
đảo.
2.3.1 Phương pháp hình học:
Sử dụng chế độ IK-Cartesian, người dùng chỉ định vị trí mục tiêu mong
muốn của gắp trong khơng gian Cartesian là (x, y, z) trong đó z là chiều cao và góc
của bộ gắp so với mặt đất (xem Hình 2.4). Hằng số ψ cho phép người dùng di
chuyển các đối tượng mà khơng thay đổi hướng của đối tượng.
Ngồi ra, bằng cách giữ cố định ở vị trí hoặc giữ cố định cổ tay so với phần
còn lại của cánh tay, các phương trình động học nghịch có thể được giải quyết ở
dạng kín.
20



Chiều dài d1, a3, a4 và d5 tương ứng với chiều cao cơ sở, chiều dài cánh tay
trên, chiều dài cẳng tay và chiều dài gắp tương ứng là không đổi. Các góc θ 1, θ2, θ3,
θ4 và θ5 tương ứng với xoay vai, cánh tay trên, cẳng tay, cổ tay và đầu ra tương ứng.
Các góc này được cập nhật khi vị trí đã chỉ định trong khơng gian thay đổi. Để giải
quyết các góc khớp của cánh tay, θ 1:4 cho vị trí mong muốn (x, y và z) và ψ được
định trước.
Từ Hình 2.3, suy ra θ1 = atan2(y, x ) và khoảng cách xuyên tâm được chỉ định từ đế
d có liên quan đến x và y bởi:

Hình 2.3 Hệ trục đơn của Robot.

d = xd2 + yd2
xd = d cos(θ1 )
yd = d sin(θ1 )

(19)

Khi chuyển sang chế độ xem phẳng trong Hình 2.4, mối quan hệ giữa các
góc khớp θ2, θ3 và θ4 như sau:

ψ = θ 2 + θ3 + θ 4

(20)

Vì ψ được cho, nên có thể tính khoảng cách xun tâm và chiều cao của
khớp cổ tay:

r4 = zd − a5 cos(ψ )
z4 = zd − a5 sin(ψ )
hoặc


(21)
21


r4 = a3 cos(θ 2 ) + a4 cos(θ 2 + θ3 )
z4 = a3 sin(θ 2 ) + a4 sin(θ 2 + θ3 ) + d1
Để xác định θ2 và θ3. Đầu tiên, tìm β và s (từ Hình 2.4) sử dụng định luật
cosin:

β = a tan 2( s 2 + a32 − a42 , 2a3 s)
α = a tan 2( z4 − d1 , r4 )
s = ( z4 − d1 ) 2 + r42

(22)

- Với các giá trị trung gian này, có thể tìm thấy các giá trị góc cịn lại là:

θ2 = a ± β
θ 3 = a tan 2(s 2 − a32 − a42 , 2 a 3a 4 )
θ 4 = ψ − θ 2 − θ3

(23)

Hình 2.4 Xem trên mặt phẳng của cánh tay robot 5 bậc.
2.3.2 Phương pháp đại số:
Sử dụng các kết quả X, Y và Z nhận được trong các động học trực tiếp:

x = c1 [ a3c2 + a4 c23 + d5c234 ]


y = s1 [ a3c2 + a4 c23 + d5c234 ]

z = [ d5 c234 + a4 s23 + a3 s2 ] + d1

(24)
(25)
(26)

Phương trình đơn giản hóa được nhận khi bình phương phương trình 24 và
25:
22


⇒ a3c2 + a4 c23 = ± x 2 + y 2 − d 5c234

(27)

Chuyển động khớp đầu tiên, được xác định θ1, có thể được tính bằng các
tham số hình học:

θ1 = a tan 2( y, x)
Tìm θ3 bằng cách sử dụng phương trình (26):

⇒ a3 s2 + a4 s23 = z − d5 s234 − d1

(28)

Bình phương phương trình (27) và (28):
( z − d5 s234 − d1 )2 + (± x 2 + y 2 − d5c234 )2 − a32 − a42
⇒ c3 =

2a3 a4

(29)

cψ = c234 , sψ = s234
trong khi
c234 = cos(θ 2 + θ 3 + θ 4 ), s234 = sin(θ 2 + θ3 + θ 4 )
⇒ c3 =

( z − d5 sψ − d1 ) 2 + ( ± x 2 + y 2 − d 5cψ ) 2 − a32 − a42
2a3 a4

(30)

s3 = ± 1 − c32
⇒ θ3 = a tan 2( s3 , c3 )

(31)

Sau khi tính θ3, có thể tìm θ2 bằng cách:

θ2 = α − β
α = a tan 2( z − d5 sψ − d1 , ± x 2 + y 2 − d 5cψ )
β = a tan 2( a4 s3 , a3 + a4 c3 )

(32)

θ 2 = a tan 2( z − d5 sψ − d1 , ± x 2 + y 2 − d5cψ ) − a tan 2(a4 s3 , a3 + a4c3 )

(33)


θψ = θ 2 + θ3 + θ4

(34)

θ 4 = θψ − θ 2 − θ3

(35)

Để tìm θ5, ta sử dụng tổng ma trận biến đổi trong phương trình (14):
23


s5 = s1r11 − c1r21
c5 = s1r12 − c1r22
r11 = ((c1c2 c3 − c1s2 s3 )c4 + (−c1c2 s3 − c1s2c3 ) s4 )c5 + s1s5
r12 = −((c1c2 c3 − c1s2 s3 )c4 + (−c1c2 s3 − c1s2 c3 ) s4 )s5 + s1s5
r21 = ((s1 c2 c3 − s1s2 s3 )c4 + (− s1c2 s3 − s1s2c3 )s4 )c5 − c1s5
⇒ θ 5 = a tan 2( s5 , c5 )

(36)
(37)

Một giải pháp đại số khác bằng cách sử dụng tổng ma trận biến đổi trong
phương trình (14), có thể tìm thấy giải pháp động học nghịch cho một cánh tay
robot 5 bậc.

 r11 r12
r r
1

2
3
4
5
T = T0 * T1 * T2 * T3 * T4 = G =  21 22
 r31 r32

0 0

r13
r23
r33
0

x
y 
z

1

(38)

Để tìm giải pháp động học nghịch cho khớp đầu tiên θ 1 như là một hàm của
các phần tử đã biết của

end − effector
Tbase

nghịch đảo chuyển đổi liên kết được điều chỉnh lại


như thể hiện trong phương trình (39):
−1

−1

T01  * T05 = T01  * T01  * T12 * T23 * T34 * T45

(39)

−1

trong đó

T01  * T01  = I

I là ma trận danh tính. Trong trường hợp này, phương
−1

trình trên được đưa ra bởi

T01  * T05 = T12 * T23 * T34 * T45 ;

giải pháp của phương trình

này được giải thích trong phương trình (40).

c1r12 + s1r22
 c1r11 + s1r21
−s r + c r −s r + c r
1 12

1 22
 1 11 1 21

r31
r32

0
0


c1r13 + s1r23
− s1r13 + c1r23
r33
0

24

c1 x + s1 y  α11 α12 α13 α14 
− s1 x + c1 y  α 21 α 22 α 23 α 24 
=
z − d1  α 31 α 32 α 33 α 34 
 

1
0
0
1 
 0

(40)



α11 = ((c2c3 − s2 s3 )(c4 − 1) + (− c 2 s3 − s2c3 ) s4 )c5
α12 = −((c2c3 − s2 s3 )(c4 − 1) + ( − c 2 s3 − s2c3 ) s4 ) s 5
α13 = −(c2c3 − s2 s3 ) s 4 + ( − c 2 s3 − s2 c3 ) c 4
α14 = ( −(c2c3 − s2 s3 ) s 4 + ( − c 2 s3 − s2c3 ) c4 ) d 5 + (c2c3 − s2 s3 )a4 + c2 a3
α 21 = − s5
α 22 = −c5
α 23 = 0
α 24 = 0
α 31 = ((s 2 c3 + c2 s3 )(c4 − 1) + (c2 c3 − s2 s3 ) s4 )c5
α 32 = −((s 2 c3 + c2 s3 )(c4 − 1) + (c2c3 − s2 s3 )s4 ) s5
α 33 = −((s 2 c3 + c2 s3 ) s 4 + (c2 c3 − s2 s3 ) c 4
α 34 = (−(s 2 c3 + c2 s3 ) s 4 + (c2c3 − s2 s3 ) c 4 ) d 5 + ( s2c3 + c2 s3 )a4 + s2 a3
 (c23 (c4 − 1) − s234 )c5

− s5
=
(s 23 (c4 − 1) − c23 s)c5

0


−(c23 (c4 − 1) − s234 ) s5
−c5

− s234
0

−(s 23 (c4 − 1) + c23 s)c5

0

c234
0

− s234 d5
0




c234 d5 + s23a4 + s2 a3 

0


(41)

Từ phương trình (40), (41):

c1 y − s1 x = 0
⇒ θ1 = a tan 2( y, x)

(42)

Để tìm các biến khác, các phương trình sau đây được lấy theo cách tương tự.
−1

T01 * T12  * T05 = T23 * T34 * T45
−1


T01 * T12 * T23  * T05 = T34 * T45
−1

T01 * T12 * T23 * T34  * T05 = T45
Bây giờ, có thể tính θ2 từ phương trình trên:

c1 x + s1 y = − s234 d5 + c23a4 + c2 a3
c1 x + s1 y = − s234 d5 + c2 (c3 a4 + a3 )
c2 =

c1 x + s1 y + s234 d5
(c3 a4 + a3 )

25