Đồ án Cơ điện tử

Mục lục

1. GIỚI THIỆU ROBOT PUMA

2. SƠ ĐỒ ĐỘNG VÀ HỆ TỌA ĐỘ

3. PHƯƠNG TRÌNH ĐỘNG HỌC ROBOT

4. PHƯƠNG TRÌNH ĐỘNG HỌC NGƯỢC ROBOT

Nhóm sinh viên thực hiện:

Đồ án Cơ điện tử

Đồ án Cơ điện tử

1. GIỚI THIỆU
Ra đời cách đây nửa thế kỷ, robot công nghiệp đã có những phát triển
vượt bậc. Nhiều nước trên thế giới sớm áp dụng mạnh mẽ kỹ thuật robot vào
sản xuất và nó đã đem lại những hiệu quả to lớn về kinh tế và kỹ thuật, nâng
cao năng suất lao động, tăng chất lượng và khả năng cạnh tranh của sản phẩm,
cải thiện điều kiện làm việc của công nhân...
Đối với nước ta, kỹ thuật robot vẫn còn là vấn đề khá mới mẻ, nhất là
việc nghiên cứu thiết kế, chế tạo robot. Nội dung nghiên cứu nhằm chế tạo
một robot sáu bậc tự do, kiểu robot PUMA –Programmalbe Univesal
Machine for Assembly, sử dụng card điều khiển LAB-PC+ để điều khiển các
động cơ bước dẫn động các khớp.

2. SƠ ĐỒ ĐỘNG VÀ HỆ TỌA ĐỘ CỦA ROBOT PUMA

Robot PUMA là robot có 6 bậc tự do, cấu hình RRRRRR.

Sơ đồ động và hệ tọa độ gắn trên các khâu của robot như sau:

Hình 1: Hệ tọa độ gắn trên các khâu của robot.

BẢNG THÔNG SỐ DENAVIT - HARTENBERG (DH)

Nhóm sinh viên thực hiện:

Đồ án Cơ điện tử

Khâu θ i α i ai di
π
1 θ 1* − 0 d1
2

2 θ 2* 0 a2 d2
π
3 θ 3* a3 0
2
π
4 θ 4* − 0 d4
2
π
5 θ 5* 0 0
2

6 θ 6* 0 0 d6

Các thông số cơ bản của Robot PUMA

+ Chọn a1 = …mm; a2 = …mm;
a3max = …mm.
+ Giới hạn chuyển động cuả các khâu:
- Khâu 1: …≤ θ 1≤ …
- Khâu 2: …≤ θ 2 ≤ …
- Khâu 3:
- Khâu 4:
- Khâu 5:
- Khâu 6:

Nhóm sinh viên thực hiện:

Đồ án Cơ điện tử

3. PHƯƠNG TRÌNH ĐỘNG HỌC ROBOT

+ Để mô tả mối quan hệ về hướng và vị trí của hệ tọa độ gắn trên hai
khâu liền kề nhau (Khâu thứ i và khâu i-1) ta dùng các ma trận A i ; được biểu
diễn bởi các phép biến đổi:
Ai = Rot(z, θ 0). Trans(a,0,0). Trans(0,0,d). Rot(x, α )
Hay
cos θ − sin θ cos α sin θ sin α a cos θ 
sin θ cos θ cos α − cos θ sin α a sin θ 
Ai =  0 sin α cos α d 

 
 0 0 0 1 

Qui ước viết tắt các hàm lượng giác như sau:
Ci = cosθ i; Si = sinθ i; Cij = cos(θ i+θ j); Sij = sin(θ i+θ j);.......
Ta có:
C1 0 − S1 0  C2 − S2 0 a2 C2 
S 0 C1 0  S C2 0 a1 S2 
A1 =  A2 = 
1 2
; ;
0 −1 0 d1  0 0 1 d2 
   
0 0 0 1 0 0 0 1 
C3 0 S3 a3 C3  C4 0 − S4 0
 S 0 −C a3 S3  S 0 C4 0 
A3 =  A4 = 
3 3 4
; ;
0 1 0 0  0 −1 1 d4 
   
0 0 0 1  0 0 0 1
C5 0 S5 0 C6 − S6 0 0
 S 0 −C 0  S C6 0 0 
A5 =  A6 = 
5 5 6
; ;
0 1 0 0 0 0 1 d6 
   
0 0 0 1 0 0 0 1

Tích các ma trận Ai được gọi là ma trận T:

5
T6 = A6; 4T6 = A5. A6 ; 3T6 = A4.A5.A6;
2
T6= A3.A4.A5.A6 ; 1T6 = A2.A3.A4.A5.A6 ;
T6 = A1.A2.A3.A4.A5.A6 ;
Ta còn có ma trận trạng thái cuối:
 nx sx ax px 
n sy ay py 
TE = 
y

nz sz az pz 
 
0 0 0 1

Ma trận TE mô tả hướng và vị trí của

r r r
hệ toạ độ gắn trên khâu chấp hành
cuối đối với hệ tọa độ gốc. Trong đó: n, s, a là các véctơ chỉ phương của hệ

tọa độ gắn trên khâu chấp hành cuối, p là véctơ điểm chỉ vị trí của gốc hệ tọa
độ gắn trên khâu chấp hành cuối.

Nhóm sinh viên thực hiện:

Đồ án Cơ điện tử

a-vector có hướng tiếp cận (approach) đối tác

s-vector có hướng đường trượt (sliding) đóng mở bàn kẹp
hoặc phương nắm bắt(occupation) kí hiệu là o
n- vector pháp tuyến (normal)
************ ***********************
Ma trân chuyển trạng thái
5
T6 = A6 mô tả hướng và vị trí của khâu thứ 6 so với khâu thứ 5:
 C6 − S 6 0 0 
S C6 0 0 
5
T6 =A6 =  6
;
0 0 1 d6 
 
0 0 0 1
C5 .C6 −C5 S6 S5 S5 .d6 
 S .C − S S C −C5 .d6 
T6 = A5. A6 = 
4 5 6 5 6 5

S6 C6 1 0 
 
 0 0 0 1 

3
T6 = A4.A5.A6=

2
T6 = A3.A4.A5.A6
[1,1]=
[1,2]=
[1,3]=
[1,4]=
[2,1]=
[2,2]=
[2,3]=
[2,4]=
[3,1]=
[3,2]=
[3,3]=
[3,4]=
1
T6 = A2.A3.A4.A5.A6
[1,1] = C23 *(C4C5C6 − S4 S6 ) − S23 S5 C6
[1.2] = −C23 *(C4 C5 S6 + S4 C6 ) + S5 S6 S23
[1.3] = C23C4C5 + S23C5
[1, 4] = C23 *(C4 S5 d6 + a3 ) + S23 *(C5 d6 + d4 ) + a2 C2
[2,1] = S 23 *(C4 C5 C6 − S4 S6 ) + C23 S5 C6

Nhóm sinh viên thực hiện:

Đồ án Cơ điện tử

[2, 2] = − S 23 *(C4C5 S6 + S4 C6 ) − C23 S5 S6

[2,3] = S 23C4 S5 − C23C5
[2, 4] = S 23 *(C4 S5 d6 + a3 ) − C23 *(C5 d6 + d4 ) + a2 C2
[3,1] = S 4C5C6 + C4 S6
[3, 2] = − S 4C5 S6 + C4 C6
[3.3] = S 4 S5
0
T6 = A1.A2.A3.A4.A5.A6
Tu ma trận trên ta có hệ phương trình động học của Robot PUMA như sau:
nx =
ox =

ax =
px =

ny =
oy =

ay =

py =

nz =

oz =

az =

pz =

4. PHƯƠNG TRÌNH ĐỘNG HỌC NGƯỢC ROBOT PUMA

Trong thực tế, thường ta biết trước vị trí và hướng mà khâu chấp hành
cuối của robot cần đạt đến. Điều ta cần biết là giá trị của các biến khớp (góc
quay) tại mỗi thời điểm đó. Giải hệ phương trình (1), khi biết trước hướng và
vị trí của hệ tọa độ gắn trên khâu chấp hành cuối, ta sẽ xác định được tệp

Nhóm sinh viên thực hiện:

Đồ án Cơ điện tử

nghiệm (θ 1, θ 2, θ 3, θ 4,θ 5, θ 6) là giá trị của các biến khớp. Các phương
   
trình xác định giá trị các biến khớp thông qua các véctơ n , o, a, p được
gọi là hệ phương trình động học ngược của robot.

Tham khảo bài giảng của GS.TSKH Nguyễn Văn Khang trong bài giải về
việc giải một bài toán động học ngược bằng phương pháp số.

Gọi trong đó là tọa độ suy rộng, Trong đó

là vị trí của khâu thao tác. Giữa và liên hệ vởi nhau bằng
phương trình sau: . Bài toán động học ngược được phát biểu như sau:
biết tìm .Tức là:

Khi đó xảy ra 3 trường hợp:

+ m=n gọi là Robot có cấu trúc động học cân bằng (chuẩn). Phương
trình có thể có nghiệm duy nhất tùy thuộc vào cấu trúc của hệ.
+m<n : Robot có cấu trúc dư dẫn động. Khi đó bài toán có nhiều
nghiệm, do vậy người ta đưa vào các điều kiện rằng buộc cho khâu thao tác.
+m>n : Để bài toán có nghiệm thì cần có các rằng buộc điều kiện về tọa
độ suy rộng.
*Lưu ý:
Bài toán động học ngược là rất khó, đối với tay máy 6 khớp có ba khớp
cuối đồng quy tại một điểm ( with 6 DOF in which 3 consecutive axes
intersect at a point), ta có thể tách bài toán động học ngược thành hai bài toán
đơn giản hơn là động học ngược vị trí và động học ngược hướng

+ [1]Tìm vị trí giao điểm các trục cổ tay (tâm cổ tay)

+ [2]Tìm hướng của cổ tay.

Ta biểu diễn (4.2) thành hai hệ phương trình như sau:

0
R6 ( q1 ,K , q6 ) = R
(4.5)
0
O6 (q1 ,K , q6 ) = O

trong đó O và R là hướng và vị trí của dụng cụ, được biểu diễn đối với hệ
tọa độ cố định bên ngoài (world coordinate system). Ta phải giải bài toán trên
đối với các ẩn q1 ,  , q6 .

Nhóm sinh viên thực hiện:

Đồ án Cơ điện tử

Pỉeper’Solution ứng dụng cho cơ cấu 6 khớp quay có 3 khớp cuối giao
nhau có phương pháp giải như sau:
Vị trí của tâm cổ tay, pc xác định qua vị trí công cụ (The given tool
position) và phương của Tool pointing ( Z 6 ).Do đó vị trí của tâm cổ tay phụ
thuộc vào 3 biến khớp đầu tiên.
The relative wrist oriention R36
Các biến khớp θ 4 ,θ5 ,θ6 xác định từ ma trận định hướng cổ tay (The arm
orientation) R03 và ma trận định hướng công cụ (The given tool orientation)
R60 .

+ Ma trân trạng thái (The given tool pose) T60

+ Solve porions của động học ngược để tìm ra R30 (θ1 ,θ2 ,θ3 ) và
R63 (θ 4 , θ5 , θ6 ) .

+ Định vị trí của tâm cổ tay Oc có tọa độ cho trước như sau:
0 
0
OC = O − d 6 .R. 0  trong đó O và R là hướng và vị trí của dụng cụ,
1 
được biểu diễn đối với hệ tọa độ cố định bên ngoài (world coordinate
system). Pc as d 6 (cột cuối cùng của T60 ) –
d 6 (tool offset length)* Z 6 (3 cột của ma trận T60 )

+ Thiết lập Pc = cột cuối cùng của R30 (θ1 ,θ2 ,θ3 ) để tìm ra các biến khớp
θ1 , θ 2 , θ3

Tính R63 = R30 R60 sau khi đã thay giá trị của các biến khớp vào θ1 ,θ 2 ,θ3 vào
−1

R30 (θ1 , θ 2 ,θ3 )

Nhóm sinh viên thực hiện:

Đồ án Cơ điện tử

+So sánh R63 và R63 (θ 4 ,θ5 ,θ6 ) để rút ra θ 4 ,θ5 ,θ6

Tọa độ của điểm C trong hệ tọa độ R6 :
 0 
 
6  0 
rC = − d 
 6
 t 

Tọa độ của C trong hệ tọa độ R0 :

n sx ax p x   0   − a x . d6 + x p 
 x    
n sy ay p y   0   − a y . d6 + y p 
r = T . 6
r =  y   =   (*)
C E E 
n sz az p z   − d 6   − a z . d6 + z p 
 z    
 0 0 0 1   1   1 

Ma trận chuyển từ hệ tọa độ 0 → 3:

C C − S1 C1 . S23 C1 ( a3 .C23 + a2 .C2 )− S1 . d 2 
 1 23 
 S C1 S1. S23 S1 ( a3 .C23 + a2 .C2 ) − C1 . d 2 
0
R A A A
3= 1. 2. 3 =  1
 −S C23 − a3 . S23 − a2 . S 2 + d1


0
 23 
 0 0 0 1 

⇒ Tọa độ của C trong R0 :

C . S . d + a .C C + a .C .C − S . d 
 1 23 4 3 1 23 2 1 2 1 2 
 S . S . d4 + a3 . S C23 + a2 . S1 .C2 + C1 . d 2 
rC =0R r 4
3. C = 

1 23 1  (**)

d 4 .C23 − a3 . S 23 − a2 . S 2 + d1
 
 t 

Đối chiếu (*) và (**) ta được hệ 3 phương trình 3 ẩn :

C1 ( S 23 .d 4 + a3.C 23 + a 2.C 2 ) − S 1.d 2 = x p − ax .d 6
S1 ( S 23 .d 4 + a3.C 23 + a 2.C 2 ) + C 1.d 2 = y p − a y .d 6
d 4 .C23 − a3.S 23 − a 2.S 2 = z p − a z .d 6 − d 1
Nhân (2) với cos θ1 trừ đi (1) nhân với sin θ1 :
d 2 = ( y p − a y d6 )cosθ1 − ( x p − ax d6 )sinθ1
 d2 
⇒ θ1 = ± Ar cos   − A tan 2( x p − a x d6 , y p − a y d6 )
 ( y p − a y d 6 ) + ( xp − ax d6 )2
2 
 
Nhân (2) với sin θ1 cộng với (1) nhân vói cos θ1 và kết hợp với (3) ta có
hệ [@]

Nhóm sinh viên thực hiện:

Đồ án Cơ điện tử

S 23 .d 4 + a3.C 23 + a 2.C 2 = ( y p − a y .d 6) sin θ + ( x p − a x .d 6)cosθ 1 = Px

C23 .d 4 − a3.S 23 − a 2.S 2 = z p − az .d 6 − d 1 = Py
Bình phương 2 vế cộng lại ta được:
d 42 + a32 + a22 + 2d4 a2 sin θ3 + 2a2 a3 cosθ3 = Px2 + Py2
⇒ 2d 4 a2 sin θ3 + 2a2 a3 cosθ3 = Px2 + Py2 − (a32 + a22 + d42 )

 Px2 + Py2 − ( a32 + a22 + d42 ) 

⇒ θ3 = ± Arc cos   + A tan 2(d4 a2 , a2 a3 )
 2 a2 d 4 + a3
2 2 
 
Khai triển hệ phương trình liên kết [@] ta được:
(a3 .C3 + a2 + S3 .d4 )C2 + (C3 .d4 − a3 .sin θ3 )sin θ2 = Px
(C3 .d 4 − a3 .S3 )cosθ2 − (a2 + S3 .d4 + a3 .cosθ3 ) sin θ2 = Py

∆ = −  (a3 .C3 + a2 + S3 .d4 ) 2 + (C3 .d4 − a3 .S3 )2 

∆ c = −  (a3 .C3 + a2 + S3 .d4 ) Px + (C3 .d4 − a3 .S3 ) Py 
∆ s = − (d 4 .C3 − a3 S3 ) Px − (a2 + d4 S3 + a3 .C3 ) Py 

∆ s ∆c
θ 2 = A tan 2( , )
∆ ∆
⇒ θ 2 = A tan 2 (d4 C3 − a3 S3 ) Px − (a2 + d4 S3 + a3 C3 ) Py , (a2 + d4 S3 + a3 C3 ) Px + (d4 C3 − a3 S3 ) 

 C4 0 − S4 0 C5 0 S5 0  C6 − S6 0 0
S 0 C4 0  S 0 −C5 0  S C6 0 0 
Ta có R6 = A4 A5 A6 =   5  6
3 4

0 −1 1 d4  0 1 0 0 0 0 1 d6 
     
0 0 0 1 0 0 0 1 0 0 0 1

Mặt khác:
A1 A2 A3 A4 A5 A6 = TE ⇔ R30 .R63 = TE ⇒ R63 = R30( −1) .TE

C1C23nx + S1C23 ny − S23 nz C1C23 sx + S1 C23 sy − S23 sz C1 C23 ax + S1 C23 ay − S23 az f14 
 − S1nx + C1ny − S1 sx + C1 sy − S1 ax + C1 ay f24 
 
 C1S 23nx + S1 S23 ny − C23 nz C1 S23 sx + S1 S23 sy − C23 sz C1 S23 ax + S1 S23 ay − C23 az f34 
 
 0 0 0 1 
Chỉ cần quan tâm tới phần định hướng tức là ma trận [3,3].
So sánh các phần tử của 2 ma trận [3,3] :

Nhóm sinh viên thực hiện:

Đồ án Cơ điện tử

R63 [ 3,3] ⇒ cos θ5 = S 23 (C1ax + S1a y ) − C23 az

⇒ θ5 = ± Arc cos  S 23 (C1ax + S1a y ) − C23 az 

Nếu sin θ5 ≠ 0 ⇒
R63 [ 2,3] ⇒ sin θ4 *sin θ5 = − S1 * ax + C1 * a y
− S1 * ax + C1 * a y
⇒ sin θ 4 =
sin θ5
R [ 1,3] ⇒ cosθ4 *sin θ5 = C23 (C1 * ax + S1 * a y ) − S23 az
3
6

C23 (C1 * ax + S1 * a y ) − S23 az

⇒ cosθ 4 =
sin θ5
− S1 * ax + C1 * a y C23 (C1 * ax + S1 * ay ) − S23 az
θ 4 = A tan 2( , )
sin θ5 sin θ5

R63 [ 3, 2] ⇒ sin θ6 *sin θ5 = C1 S23 sx + S1 S23 sy + C23 sz

C1S 23 sx + S1S23 s y + C23 sz
⇒ sin θ 6 =
sin θ5
R63 [ 3,1] ⇒ −cosθ6 *sin θ5 = C1 S23 nx + S1 S23 ny + C23 nz
C1S23nx + S1 S23 ny + C23 nz
⇒ cosθ 6 = −
sin θ5
S23 (C1sx + S1 s y ) + C23 sz S23 (C1 nx + S1 ny ) + C23nz
θ 6 = A tan 2( ,− )
sin θ5 sin θ5

Phu luc

Định nghĩa hàm Atan2(y,x):

+Với y≠0:

Nhóm sinh viên thực hiện:

Đồ án Cơ điện tử

Trong đó là một góc trong khoảng [0,π/2) tương tự như và sgn là hàm dấu(sign
function):

+và

Chú ý:

• Tập giá trị của Atan2 là khoảng (-π,π], có thể khại triển thành [0,2π) bằng cách thêm π vào già trị
âm.

• Thông thường, atan2(0,0) không được định nghĩa.

o Trong hàm của C, and most other computer implementations, are designed to reduce the
effort of transforming cartesian to polar coordinates and so always define atan2(0,0). On
implementations without signed zero, or when given positive zero arguments, it is normally
defined as 0. It will always return a value in the range [-π,π] rather than raising an error or
returning a NaN (Not a Number).

Number of Solutions:
It depends on
•Number of Joints

•Range of Motion
General Mechanism with 6 d.o.f.Number of solutions ≤ 16

•Link Parameters
e.g. 6-revolute-joint manipulator
if all ai≠ 0 Number solutions ≤ 16
if a3 = a5 = 0 Number solutions ≤8
if a1 = a3 = a5 = 0 Number solutions ≤4

•Range of Motion
General Mechanism with 6 d.o.f.Number of solutions < 16.Main Results:
General 6R open-chain 16 solutions
General 5RP open-chain 16 solutions
General 4R2P open-chain 8 solutions
General 3R3P open-chain 2 solutions

Nhóm sinh viên thực hiện:

Đồ án Cơ điện tử

Special conditions in the structure [such as intersecting or parallel axes] cause the general number of
solutions to reduce.There exist open-chain manipulators with 16, 14, 12, 10, 8, 6, 4, 2 solutions.
Ex:PUMA 560 with 8 Solutions
θ4 → θ4 + 180
0

θ5 → -θ5
θ6 → θ6 + 1800

TÀI LIỆU THAM KHẢO

[1] Nguyễn Thiện Phúc, Robot công nghiệp, NXB Khoa học và Kỹ thuật,
Hà Nội, 2004.
[2] Nguyễn Văn Khang, Động lực học hệ nhiều vật ,NXB Khoa học và Kỹ
thuật
[3] www.google.com

Nhóm sinh viên thực hiện: