ĐỘNG lực học và điều KHIỂN ROBOT

Trờng đại học bách khoa hà nội
Khoa cơ khí - Bộ môn cơ HọC ứng dụng





đồ án tốt nghiệp
chuyên ngành: cơ tin kỹ thuật

Đề tài:
động lực học và điều khiển Robot

Giáo viên hớng dẫn :
Sinh viên thực hiện
Lớp

gs.tskh.nguyễn văn khang
Ths. Đỗ Thành Trung
: nguyễn viết quang
: cơ tin A - k45

Hà Nội 5/2005

Chơng 1
Cơ sở Rôbốt công nghiệp
I. Tổng quan về rôbốt
1.1 Sự ra đời của rôbốt công nghiệp


Nhu cầu nâng cao chất lợng sản phẩm và nâng cao năng suất lao

động đòi hỏi phải ứng dụng rộng rãi các phơng tiện tự động sản xuất. Chính
điều đó đã tạo ra những dây chuyền thiết bị tự động có tính linh hoạt cao.
Nó đang dần thay thế các máy truyền thống chỉ đáp ứng một công việc nhất
định. Trong khi đó thị trờng luôn đòi hỏi những thay đổi về mẫu mã, chất lợng , kích thớcvì thế nhu cầu ứng dụng rôbốt để tạo ra các hệ thống sản
xuất tự động có tính linh hoạt cao ngày càng tăng.
Theo ISO thì rô bốt công nghiệp là một tay máy đa mục tiêu, có một số
bậc tự do, dễ dàng lập trình, điều khiển trợ động, dùng để gắp phôi, dụng cụ
hoặc các vật dụng khác. Do chơng trình thao tác có thể thay đổi nên thực
hiện nhiều nhiệm vụ đa dạng .
Có một cách định nghĩa khác: Rôbốt công nghiệp có thể đợc hiểu là những
thiết bị tự động linh hoạt, bắt chớc đợc các chức năng lao động công nghiệp
của con ngời. Nh vậy theo cách định nghĩa này thì:
Rôbốt là thiết bị tự động linh hoạt: là nói đến khả năng thao tác với nhiều
bậc tự do,đợc điều khiển trợ động và lập trình thay đổi đợc.
Rôbốt có khả năng bắt chớc chức năng lao động của con ngời: nói đến sự
không hạn chế từ chức năng lao động chân tay đơn giản đến trí khôn nhân
tạo, tuỳ thuộc vào công việc mà nó đảm nhiệm.
1.2 ứng dụng rôbốt công nghiệp
Mục tiêu ứng dụng rôbốt công nghiệp nhằm góp phần nâng cao năng
suất lao động, giảm giá thành, nâng cao chất lợng sản phẩm và khả năng
cạnh tranh của sản phẩm, đồng thời cải thiện điều kiện làm việc đặc biệt là
trong những môi trờng độc hại ảnh hởng đến sức khoẻ của ngời lao động.
Việc ứng dụng rôbốt vào trong quá trình sản xuất tuỳ thuộc vào yêu cầu,
điều kiện và nhiệm vụ, chức năng của nơi sản xuất. Việc u tiên đầu t trớc
hết phải nhằm để đồng bộ hoá cả hệ thống thiết bị, rồi tự động hoá và rô bốt
hóa chúng khi cần thiết.
Ngày nay, việc ứng dụng rôbốt rất rộng rãi. Có thể kể ra một số lĩnh vực nh:
Kĩ nghệ đúc.
Trong ngành gia công áp lực.
Các quá trình hàn và nhiệt luyện.

Trong lĩnh vực gia công và lắp ráp(tháo lắp phôi và sản phẩm cơ khí).

II. Các phép biến đổi toạ độ cơ bản
2.1 Ma trận côsin chỉ hớng
2.1.1 Định nghĩa ma trận côsin chỉ hớng của vật rắn


r r r
r r r
Cho vật rắn B và hệ quy chiếu Ro = { e1(0) , e2(0) , e3(0) } . Trong đó e1(0) , e2(0) , e3(0)

là ba véc tơ đơn vị trên các trục Ox 0, Oy0, Oz0. Ta gắn chặt vào vật rắn B
r r r
r r r
một hệ quy chiếu R = { e1 , e2 , e3 } với e1 , e2 , e3 là ba véc tơ đơn vị trên các trục

Ax, Ay, Az.
z0

B

r
e3

r O
e1(0)

x

y

r
r e2
e1

x

r
e3(0)

Định

A

y

r
e2(0)

0

Hình 1.1
nghĩa: Ma trận vuông cấp ba
r r r r
e1(0) e1 e1(0) e2
r r r r
A = e2(0) e1 e2(0) e2
er3(0) er1 er3(0) er2

0

r r
e1(0) e3
r r
e2(0) e3
r r
e3(0) e3

(1.1)
đợc gọi là ma trận côsin chỉ hớng của vật rắn B đối với hệ quy chiếu R0.
Ta đa vào các kí hiệu
r r
r r
aij = ei(0) e j = cos(ei(0) , e j ),

(1.2)

(i, j = 1, 2,3)

thì ma trận côsin chỉ hớng (1.1) có dạng
a11
A = a21
a31

a12
a22
a31

a13

a23
a33

(1.3)

Từ định nghĩa trên ta có các hệ thức liên hệ
r
r
r
r
e1 = a11e1(0) + a21e2(0) + a31e3(0)
r
r
r
r
e2 = a12e1(0) + a22e2(0) + a32 e3(0)
r
r
r
r
e3 = a13e1(0) + a23e2(0) + a33e3(0)

(1.4)
r

Nếu ta kí hiệu e j là ma trận cột gồm các phần tử của véc tơ e j trong hệ quy
chiếu R0.
a11
a12
a13

e1 = a21 , e2 = a22 , e3 = a23 ,
a31
a32
a33

Thì ma trận côsin chỉ hớng (1.3) có dạng

(1.5)


A = [ e1 , e2 , e3 ]

(1.6)

Ma trận côsin chỉ hớng A còn đợc gọi là ma trận quay của vật rắn.

2.1.2 Các ma trận quay cơ bản
Ta qui ớc hớng quay dơng là hớng quay ngợc chiều kim đồng hồ nh hình vẽ
1.2

z

O

y

x

Hình 1.2
Các phép quay quanh trục x, y, z của hệ toạ độ vuông góc Oxyz đợc gọi là
phép quay cơ bản
Ta tìm ma trận quay của phép quay quanh trục x 0 một góc . Theo công
thức định nghĩa (1.1) ta có:
r r r (0) r r (0) r
z0
(0)

e1 e1 e1 e2
r (0) r r (0) r
A xo ( ) = e2 e1 e2 e2
er3(0) er1 er3(0) er2


e1 e3
r r
e2(0) e3
r r
e3(0) e3
(1.7)
0

0
1
= 0 cos( ) sin( )
0 sin( ) cos( )

z

r
e3

r
e3(0)
r
e2
r
O
e2(0)

y


y0


Hình 1.3
Ma trận (1.7) đợc gọi là ma trận quay của phép quay cơ bản quant truc x0.
Bằng cách tơng tự, xác định đợc ma trận quay cơ bản quanh trục y0.

z0

cos( ) 0 sin( )
A yo ( ) = 0
1
0 (1.8)
sin( ) 0 cos( )

z

r
e3(0) r
e3r
e1(0)
O r
e1

x0


x

Hình1.4
Bằng cách tơng tự, xác định đợc ma trận quay cơ bản quanh trục z0.
y0
cos( ) sin( ) 0
A zo ( ) = sin( ) cos( ) 0 (1.9)
0
0
1

y

r
e2

r
e2(0)
r
e1
r
O
e1(0)

x

Hình 1.5
Từ (1.7)& (1.8)& (1.9) ta dễ dàng tính đợc
det A xo ( ) = det A yo ( ) = det A zo ( ) = 1

(1.10)
2.2 Các góc Euler
Vị trí của vật rắn B quay quanh O cố định đợc xác định bởi hệ qui chiếu
động Oxyz (gắn chặt vào vật rắn B) đối với hệ qui chiếu cố định (Oxyz) 0
hình vẽ (1.6).

x0


Giả sử giao của mặt phẳng Oxy và mặt phẳng Oxy là trục OK. Trục OK này
gọi là đờng nút.
Ta đa vào các kí hiệu sau:

Góc giữa trục Ox0 và OK là

z0

z



Góc giữa trục OK và Ox là

y

Góc giữa trục Oz0 và Oz là
OO0

x0


K

y0


x

Hình 1.6
Ba góc , , đợc gọi là ba góc Euler. Nh thế vị trí của vật rắn B đối với
hệ qui chiếu cố định đợc xác định bởi ba toạ độ suy rộng , , . Khi xác
định vị trí của vật rắn bằng các góc Euler, ta có thể quay hệ qui chiếu cố
định (Oxyz)0 sang hệ qui chiếu động bằng ba phép quay Euler nh sau:

Quay quanh trục Oz0 một góc , quay quanh trục OK một góc , quay
quanh trục Oz một góc .
Các ma trận quay ứng với phép quay Euler là:
cos( ) sin( ) 0
A zo ( ) = sin( ) cos( ) 0
0
0
1

(1.11)
0
0
1

A K ( ) = 0 cos( ) sin( )
0 sin( ) cos( )

(1.11)


cos(ϕ ) − sin(ϕ ) 0 
A z (ϕ ) =  sin(ϕ ) cos(ϕ ) 0 
 0
0
1 

(1.12)
Ma trËn quay Euler ®îc x¸c ®Þnh nh sau:
A E = A zo (ψ ) A K (θ ) A z (ϕ )

TÝnh to¸n cô thÓ ta cã:
0
0  cos(ϕ ) − sin(ϕ ) 0 
cos(ψ ) − sin(ψ ) 0  1



A E =  sin(ψ ) cos(ψ ) 0  0 cos(θ ) − sin(θ )   sin(ϕ ) cos(ϕ ) 0 
 0
0
1   0 sin(θ ) cos(θ )   0
0
1 
 cos(ψ ) cos(ϕ ) − sin(ψ ) cos(θ )sin(ϕ ) − cos(ψ )sin(ϕ ) − sin(ψ ) cos(θ ) cos(ϕ ) sin(ψ )sin(θ ) 
A E =  sin(ψ ) cos(ϕ ) + cos(ψ ) cos(θ )sin(ϕ ) − sin(ψ )sin(ϕ ) + cos(ψ ) cos(θ ) cos(ϕ ) − cos(ψ )sin(θ ) 


sin(ϕ )sin(θ )
cos(ϕ )sin(θ )
cos(θ )

2.3 C¸c gãc Cardan
NÕu sö dông c¸c gãc quay Cardan ®Ó quay hÖ (Oxyz) 0 sang hÖ to¹ ®é ®éng
(Oxyz) th× ma trËn quay Cardan ®îc x¸c ®Þnh nh sau:
A C = A x (α ) A y ( β ) A z (γ )

z1
z2=z3

β α

γ

O

x0=x1
x2

γ
x3

Trong ®ã

y3

γ

α

H×nh 1.7

z0

α

y1=y2

β

y0

0
0
1

A x ( ) = 0 cos( ) sin( )
0 sin( ) cos( )

Là ma trận ứng với phép quay quanh trục x 0 một góc để chuyển (Oxyz)0
chuyển sang (Oxyz)1.
cos( ) 0 sin( )
A y ( ) = 0
1
0
sin( ) 0 cos( )

Là ma trận ứng với phép quay quanh trục y1 một góc để chuyển (Oxyz)1
chuyển sang (Oxyz)2.
cos( ) sin( ) 0
A z ( ) = sin( ) cos( ) 0
0
0
1

Là ma trận ứng với phép quay quanh trục z2 một góc để chuyển (Oxyz)2
chuyển sang (Oxyz)3. ở đây các véc tơ đơn vị của hệ (Oxyz) 3 trùng với các
véc tơ đơn vị của (Oxyz)1.
Khi đó
cos( ) cos( )

cos( )sin( )
sin( )



A C = sin( )sin( ) cos( ) + cos( )sin( ) sin( )sin( )sin( ) + cos( ) cos( ) sin( ) cos( )
cos( )sin( ) cos( ) + sin( )sin( ) cos( ) sin( )sin( ) + sin( ) cos( ) cos( ) cos( )

2.4 Các toạ độ thuần nhất
Các phép quay thuần tuý là đủ để xác định hớng của bàn kẹp. Nhng việc
xác định vị trí của bàn kẹp trong hệ toạ độ gắn với giá cố định là cha xác
định đợc. Ta phải sử dụng một phép biến đổi khác, đó là phép tịnh tiến.
Phép tịnh tiến có đặc điểm khác phép quay là: trong phép quay, gốc của hệ
toạ độ động luôn trùng với gốc hệ toạ độ cố định. Tuy vậy, gốc của hệ toạ
độ động cần đợc dịch chuyển so với gốc cố định.
2.4.1 Các toạ độ thuần nhất
z
Ta xét trong không gian làm việc
có số chiều lớn hơn. Hệ toạ độ
P
trong không gian 4 chiều.
Vị trí của điểm P ở trong hệ toạ độ
ba chiều Oxyz đợc xác định bởi véc tơ sau:
r
r
r
r
r = xe1 + ye2 + ze3

r

e3
r
e1

x

r
r

O

r
e2

y


Hình 1.8
Giả sử là một đại lợng vô hớng khác không tuỳ ý. Khi đó hệ toạ độ thuần
nhất của điểm P đợc định nghĩa bởi hệ thức sau:
r = [ x y z ]

T

Trong cơ học kỹ thuật (đặc biệt trong lĩnh vực rôbốt) ngời ta thờng chọn
=1. Khi đó toạ độ thuần nhất bốn chiều của điểm P đợc mở rộng từ toạ độ
vật lý ba chiều của điểm P bằng cách thêm vào thành phần thứ t nh sau:
r = [ x y z 1]

T

Nh vậy, nhờ khái niệm toạ độ thuần nhất trong không gian 4 chiều ta có thể
chuyển bài toán cộng ma trận cột trong không gian ba chiều sang bài toán
r
nhân ma trận trong không gian bốn chiều. Cho ar và b là hai véc tơ trong
không gian ba chiều, ta có:
a1 b1 a1 + b1
a + b = a2 + b2 = a2 + b2
a3 b3 a3 + b3

Ta chuyển phép tính cộng bằng phép nhâ hai ma trận nh sau:
a1 + b1 1
a + b 0
2 2 =
a3 + b3 0


1 0

0
1
0
0

0 a1 b1
0 a2 b2
1 a3 b3

0 1 1

2.4.2 Ma trận biến đổi toạ độ thuần nhất
Nếu một điểm vật lý trong không gian 3 chiều đợc biểu diễn bằng toạ độ
thuần nhất, và muốn thay đổi từ hệ toạ độ này sang hệ toạ độ khác. Ta sử
dụng ma trận biến đổi thuần nhất cỡ 4x4.
Xét vật rắn B chuyển động trong hệ quy chiếu cố định (Oxyz) 0. Lấy điểm A
nào đó của vật rắn B và gắn chặt vào vật rắn hệ quy chiếu Axyz (hình ) Lấy
P là một điểm bất kỳ thuộc vật rắn B. Trong hệ toạ độ vật lý (Oxyz)0 ta có:


r r r
rP = rA + s AP

Viết dới dạng ma trận phơng trình trên có dạng:
xP(0) x A(0) a11 a12
(0) (0)
yP = y A + a21 a22
zP(0) z A(0) a31 a32




a13 sx
a23 s y
a33 sz

Trong đó
A là ma trận côsin chỉ hớng của vật rắn B.
r
sx , s y , sz là các toạ độ của véc tơ s AP trong hệ quy chiếu Axyz

Nếu sử dụng các toạ độ thuần nhất phơng trình trên có thể viết dới dạng sau:
xP(0) a11 a12
(0)
yP = a21 a22
zP(0) a31 a32


0
1 0

a13
a23
a33
0

x A(0) sx

y A(0) s y
z A(0) sz

1 1

Định nghĩa: Ma trận
a11

a
T = 21
a31

0

a12
a22
a32
0

a13
a23
a33
0

x A(0)

y A(0)

z (0)
A

1

đợc gọi là ma trận chuyển toạ độ thuần nhất của điểm P trong hệ Axyz sang
hệ (Oxyz)0.
Tổng quát ma trận biến đổi thuần nhất có dạng nh sau:
R
T= T


p

Trong đó
Ma trận con R 3x3 ở góc bên trái của T là ma trận quay. Véc tơ p cỡ 3x1 ở
góc bên phải của T là véc tơ tịnh tiến nó biểu thị toạ độ của điểm gốc hệ toạ
độ
động (Axyz) trong hệ toạ độ cố định (Oxyz) 0. là số tỷ lệ khác
không. Véc tơ hàng T cỡ 1x3 là véc tơ phức hợp.
Nh vậy, việc sử dụng ma trận thuần nhất trong phép biến đổi toạ độ tỏ ra có
nhiều u điểm, vì trong ma trận T cỡ 4x4 bao gồm cả thông tin về sự quay và
cả về dịch chuyển tịnh tiến.
2.4.3 Các ma trận quay cơ bản thuần nhất và ma trận tịnh tiến thuần
nhất
Các ma trận quay cơ bản trong mục ma trận côsin chỉ hớng mở rộng ra
trong hệ toạ độ thuần nhất bốn chiều có dạng nh sau:


0
0
1
0 cos( ) sin( )
A x ( ) = Rot ( x, ) =
0 sin( ) cos( )

0
0
0
cos( )
0
A y ( ) = Rot ( y , ) =
sin( )

0

0
0
0

1

0 sin( ) 0
1
0
0
0 cos( ) 0

0
0
1

cos( ) sin( )
sin( ) cos( )
A z ( ) = Rot ( z , ) =
0
0

0
0

0
0

1
0

0
0
0

1

Cột thứ 4 của các ma trận 4x4 trên có 3 phần tử đầu đều bằng 0. Vì ở đây
không có sự tịnh tiến.
Các ma trận này gọi là các ma trận quay thuần nhất cơ bản. Ma trận quay
thuần nhất phức hợp đợc xây dựng dựa trên các ma trận quay thuần nhất cơ
bản.
Ngoài ra, ta đa vào khái niệm ma trận tịnh tiến thuần nhất có dạng nh sau:
1
0
Trans (a, b, c) =
0

0

0
1
0
0

0
0
1

0

a
b
c

1

Trong đó
Ta thực hiện chuyển động tịnh tiến theo trục toạ độ x một đoạn a, theo trục
toạ độ y một đoạn b, theo trục toạ độ z một đoạn c.
Ma trận quay R cỡ 3x3 là ma trận đơn vị.
2.4.4 Phép quay theo 3 góc Euler
Các ma trận quay của các góc Euler mở rộng ra trong hệ toạ độ thuần nhất
bốn chiều có dạng nh sau:
cos( ) sin( )
sin( ) cos( )
A zo ( ) =
0
0

0
0

0
0
1
0

0

0
0

1

(1.11)
0
0
1
0 cos( ) sin( )
A K ( ) =
0 sin( ) cos( )

0
0
0

(1.11)

0
0
0

1


cos( ) sin( )
sin( ) cos( )
A z ( ) =
0

0

0
0

0
0
1
0

0
0
0

1

(1.12)
Ma trận biểu diễn phép quay theo ba góc Euler, gọi tắt là phép quay Euler
nhận đợc bằng cách nhân 3 ma trận quay ở trên
A E = A zo ( ) A K ( ) A z ( )

Vậy ta có:
A E = R ( , , )
cos( ) cos( ) cos( ) sin( )sin( ) sin( ) cos( ) cos( ) cos( ) sin( ) sin( ) cos( )
cos( ) cos( ) sin( ) + sin( ) cos( ) sin( ) cos( )sin( ) + cos( ) cos( ) sin( )sin( )
=

sin( ) cos( )
sin( ) sin( )
cos( )

0
0
0


III. Quy tắc Denavít Hatenberg
Mô hình tay máy có thể mô hình hoá bởi một chuỗi các vật rắn đợc gắn với
nhau bởi các khớp. Mục đích của phần này là xác định hệ toạ độ gắn cho
từng khâu. Đó là một trong các bớc chuẩn bị để lập phơng trình chuyển
động cho khâu thao tác (bàn kẹp) của rôbốt.
3.1 Các tham số động học Denavít Hatenberg
Quan hệ giữa vị trí và hớng của 2 khâu kế tiếp có thể xác định bởi hai tham
số khớp.

Khớp
k
dk
xk-1

Khâu k1

z

k-1

k

Khâu
k

xk

xk1

0
0
0

1


Hình1.9 Góc khớp và
khoảng cách giữa các
khớp d
ở đây khớp k nối khâu k-1 với khâu k . Các tham số gắn với khớp k đợc xác
định đối với trục zk-1. Đó là trục của khớp k. Tham số thứ nhất k gọi là góc
khớp, góc khớp k là góc quay quanh trục zk-1 để trục xk-1 quay đến song
song với với trục xk. Tham số thứ hai dk gọi là khoảng cách khớp. Đó là
khoảng tịnh tiến dọc trục zk-1 để trục xk-1 đến gặp trục xk. Vậy k là góc quay
quanh khớp k, trong khi dk là khoảng cách tịnh tiến dọc theo trục của khớp
k. Mỗi một khớp đều có một tham số là không đổi và tham số khác là biến
đổi. Biến số khớp phụ thuộc vào kiểu của khớp, xem bảng (1.1).
Tham số tay máy

Khớp quay (R)

Khớp tịnh tiến(T)

Ký

hi
ệ
u
Góc khớp
Khoảng cách khớp
Độ dài khâu
Góc xoắn khâu


d
a


biến
const
const
const

const
biến
const
const


Bảng 1.1 Tham số động
học
Đối với khớp quay thì góc khớp k là biến còn khoảng cách khớp dk là
không đổi. Đối với khớp tịnh tiến thì khoảng cách d k là biến còn góc khớp
k là không đổi.
Nh vậy, có một khớp nối với các khâu kề nó, có một khâu giữa hai khớp liên

tiếp. Quan hệ về vị trí và hớng trên trục của hai biến khớp liên tiếp đợc xác
định bởi hai biến số khâu, xem hình vẽ ().
ở đó khâu k nối với khớp k và khớp k+1. Các tham số gắn liền với khâu k đợc xác định theo xk. Tham số khâu đầu tiên là a k, ak gọi là độ dài khâu. Nó
là khoảng cách tịnh tiến dọc theo trục x k để trục zk-1 đến gặp trục zk. Tham
số khâu thứ hai là k gọi là góc xoắn của khâu. Nó là góc quay của khâu
quanh xk để trục zk-1 song song với trục zk.
z
k

k

ai

yk

Khâu
k
zk-1

Khớp k

zk

Khớp k+1

xk


Hình1.10 Độ dài khâu và
góc xoắn của khâu

Khác với hai tham số khớp, hai tham số khâu luôn không đổi và xác định
một phần của quá trình thiết kế cơ khí. Đối với rôbốt công nghiệp thì góc
xoắn của khâu thờng là /2. Đôi khi trục của khớp k và trục khớp k-1 là
giao nhau trong trờng hợp đó thì tham số độ dài của khâu k là bằng không.
3.2 Véc tơ pháp tuyến, véc tơ trợt, véc tơ tiếp cận
Trong cơ cấu chấp hành của rôbốt thờng là một cơ cấu hở, gồm một chuỗi
các khâu nối với nhau bằng các khớp. Các khớp động này là khớp quay(R)
hoặc tịnh tiến (T). Để rôbốt có thể thao tác linh hoạt, cơ cấu chấp hành của
nó phải cấu tạo sao cho điểm mút của khâu cuối cùng đảm bảo dễ dàng di
chuyển theo một quỹ đạo nào đó, đồng thời khâu này có một định hớng
nhất định theo yêu cầu. Khâu cuối cùng thờng là bàn kẹp hoặc là khâu gắn
liền với dụng cụ làm việc. Điểm mút của khâu cuối cùng là điểm đáng quan
tâm nhất vì đó là điểm tác động của rôbốt lên đối tác và gọi là điểm tác
động cuối. Chính điểm này cần quan tâm không những vị trí nó chiếm trong
không gian mà cả hớng tác động của khâu cuối đó. Các khớp và khâu của
tay máy đợc đánh số lần lợt bắt đầu từ giá đỡ cố định, đó là khâu 0, và kết
thúc ở bàn kẹp, đó là khâu thứ n. Đối với rôbốt n bậc tự do thì có n+1 khâu
và có n khớp, khớp k nối khâu k-1 với khâu k. Ta phải đặt hệ toạ độ vào các
khâu của rôbốt, đặc biệt là phải đặt vào khâu cuối cùng là bàn kẹp. Hớng
của bàn kẹp có thể đợc diễn tả bằng ma trận quay R=[r 1 r2 r3]T. Trong đó 3


cột của R tơng ứng với các véc tơ pháp tuyến, véc tơ trợt, véc tơ tiếp cận
(hình).
Véc tơ tiếp cận có hớng tiếp cận với đối tác
Véc tơ trợt xác định theo hớng đóng mở bàn kẹp
Véc tơ pháp tuyến trực giao với mặt tạo bởi hai véc tơ r 2 và r3 có chiều sao
cho r1 r2, r3 làm thành hệ quy chiếu thuận.
Việc định hớng khâu cuối có thể thực hiện theo phép quay Roll Pitch
Yaw hay một số phép quay khác.

3.3 Biểu diễn Denavít Hatenberg
Denavít Hatenberg (1955) đã đa ra cách biểu diễn các hệ toạ độ trên một
dãy các khâu. Đặt Ok là hệ toạ độ gắn với khâu thứ k. Hệ tọa độ đợc Ok gắn
vào điểm cuối khâu k, (với 0kn). Nh vậy, hệ toạ độ cuối cùng đợc đặt vào
bàn kẹp. Các hệ toạ độ gắn vào khâu đợc xác định nh sau:
1. Trục zk-1 đợc chọn dọc theo hớng của trục khớp động thứ i.
2. Trục xk-1 đợc chọn dọc theo đờng vuông góc chung của hai trục zk-2 và
zk-1, hớng đi từ trục zk-2 sang zk-1. Nếu trục zk-1 cắt trục zk-2 thì hớng của
xk-1 trục đợc chọn tùy ý.
3. Gốc tọa độ Ok-1 đợc chọn tại giao điểm của trục xk-1 và trục zk-1.
4. Trục yk-1 đợc chọn sao cho hệ (Oxyz)k-1 là hệ quy chiếu thuận. Với
cách chọn hệ trục tọa độ nh trên, nhiều khi các hệ tọa độ khâu
(Oxyz)k-1 không xác định một cách duy nhất. Vì vậy ta cần có một số
bổ sung thích hợp sau:
5. Đối với hệ tọa độ (Oxyz) 0 theo qui ớc trên ta mới chỉ chọn đợc trục z0
, còn trục x0 cha có trong qui ớc trên. Ta có thể chọn trục x0 một cách
tùy ý.
6. Đối với hệ tọa độ (Oxyz)n do không có khớp n+1, nên theo qui ớc
trên ta không xác định đợc trục zn. Trục zn không đợc xác định duy
nhất, trong khi trục xn lại đợc chọn theo pháp tuyến của trục zn-1.
Trong trờng hợp này, nếu khớp n là khớp quay ta nên chọn trục song
song với trục. Ngoài ra ta có thể chọn tùy ý sao cho hợp lý.
7. Khi hai trục zk-2 và zk-1 song song với nhau, giữa hai trục này có nhiều
đờng pháp tuyến chung, ta có thể chọn trục z n-1 hớng theo pháp tuyến
chung nào cũng đợc.
8. Khi khớp thứ k là tịnh tiến, về nguyên tắc ta có thể chọn z k-1 một cách
tùy ý. Tuy nhiên trong nhiều trờng hợp ngời ta thờng chọn zk-1 dọc
theo trục của khớp tịnh tiến này.
Hình 1.11 và 1.12 minh họa cách chọn các hệ tọa độ khâu theo ý tởng của
Denavit Hatenberg.

i

Kh©u i+1

zi-1
θi

zi-2

Khíp Gi+1
Kh©u i
α

θ i−1

yi

ι

xi

Khíp Gi

Kh©u i-1

Oi

ai

Khíp Gi -1

yi -1
x'i

di
θi

Oi -1

xi-1

H×nh 1.11 BiÓu diÔn h×nh häc th«ng sè Denavit Hartenberg cña khíp tÞnh
tiÕn
i

Kh©u i+1

zi-1
θi

zi-2

Khíp Gi+1
Kh©u i
α

θi−1
Kh©u i-1

ι

yi

Khíp Gi

Oi

ai
Khíp Gi -1

yi -1
x'i

di
θi
Oi -1

xi-1

xi


Hình 1.12 Biểu diễn hình học thông số Denavit Hartenberg của khớp quay
Vị trí của hệ tọa độ khâu (Oxyz) k đối với hệ tọa độ khâu đợc xác định bởi
bốn tham số Denavit Hatenberg nh sau:
k : góc quay trục xk-1 quanh trục zk-1 đến trục xk (xk // xk)
d k : dịch chuyển tịnh tiến dọc trục zk-1 đến gốc tọa độ Ok-1 chuyển đến

Ok, giao điểm của trục xk và trục zk-1.

ak : dịch chuyển tịnh tiến dọc trục x k để điểm Ok chuyển đến điểm

Ok.
k : góc quay quanh trục xk sao cho trục zk-1 chuyển đến trục zk.

Trong bốn tham số trên, các tham số ak và k luôn luôn là các hằng số, độ
lớn của chúng phụ thuộc vào hình dáng và sự ghép nối các khâu thứ k-1 và
thứ k. Hai tham số còn k lại d k và , một là hằng số một là biến số phụ
thuộc vào khớp k là quay hay là tịnh tiến. Khi khớp k là khớp quay thì k là
biến số, còn d k là hằng số. Khi khớp k là khớp tịnh tiến thì d k là biến số,
còn k là hằng số.
3.3.1 Ma trận Denavít Hatenberg
Ta có thể chuyển toạ độ khâu(Oxyz) k-1 sang hệ toạ độ khâu (Oxyz) k bằng
bốn phép biến đổi cơ bản sau:
Quay quanh trục zk-1 một góc k
Dịch chuyển tịnh tiến dọc trục zk-1 một đoạn d k
Dịch chuyển tịnh tiến dọc trục xk một đoạn ak
Quay quanh trục xk một góc k
Ma trận của phép biến đổi, kí hiệu là Tk, là tích của bốn ma trận biến đổi cơ
bản và có dạng nh sau:
cos( k ) sin( k )
sin( ) cos( )
k
k
Tk =
0
0

0
0

0
0
1
0

0 1
0 0
0 0

1 0

0
1
0
0

0 0 1
0 0 0
1 d k 0

0 1 0

0
1
0
0

0 ak 1
0

0


0 0 0 cos( k ) sin( k )
1 0 0 sin( k ) cos( k )

0 1 0
0
0

cos( k ) sin( k ) cos( k ) sin( k ) sin( k ) ak co s( k )
sin( ) co s( ) cos( ) cos( ) sin( ) a sin( )
k
k
k
k
k
k
k
Tk =
0

sin( k )
co s( k )
dk


0
0
1

0


Ma trận Tk đợc xác định bởi công thức trên đơch gọi là ma trận Denavít
Hatenberg.
Ma trận Denavít Hatenberg Tk là ma trận côsin chỉ phớng của hệ quy
chiếu (Oxyz)k-1 đối với hệ quy chiếu (Oxyz)k. Chính xác hơn ta phải kí hiệu

0
0
0

1


ma trận này bằng Tk( k 1) . Để đơn giản cách viết sau này sử dụng kí hiệu Tk
với nghĩa Tk( k 1) , còn A n đợc dùng với nghĩa Tn(0)
3.3.2 Phơng trình xác định vị trí khâu thao tác (bàn kẹp) của rôbốt
Khi các toạ độ khâu đợc biểu diễn sử dụng các tham số (D-H) ta có thể tính
đợc toạ độ thứ k đến k-1 sử dụng ma trận biến đổi toạ độ thuần nhất. Bằng
cách nhân các ma trận biến đổi toạ độ với nhau. Ta nhận đợc ma trận biến
đổi toạ độ phức hợp từ toạ độ bàn kẹp và toạ độ cơ sở. Ma trận biến đổi toạ
độ thuần nhất phức hợp này gọi là ma trận bàn tay máy.
Nhắc lại ba tham số động học xuất hiện trong ma trận biến đổi toạ độ Tk là
không đổi, trong khi tham số chính là biến khớp. Biến khớp đôi khi là (khớp
quay) và đôi khi là (khớp tịnh tiến). Trong trờng hợp đó ta giới thiệu về kiểu
tham số khớp định nghĩa nh sau:
1
k =
0

khớp k quay
khớp k tịnh tiến

Vậy k là hàm nhị phân về kiểu của khớp. Ta có thể định nghĩa biến tại khớp
k là qk nh sau:
qk = k k + (1 k )d k

Nh vậy, ma trận biến đổi toạ độ khâu thuần nhất thứ k Tk là hàm của qk với
0kn. Ta phải biểu diễn vị trí và hớng của bàn kẹp trong hệ toạ độ gắn với
giá đỡ. Nếu Tn(0) biểu diễn phép biến đổi từ toạ độ khâu n về toạ độ khâu 0
thì
A n = Tn(0) = T1(0) .T2(1) ...Tn( n 1) = T1.T2 ...Tn

Phơng trình trên xác định vị trí khâu thao tác (bàn kẹp) của rôbốt.
Ta biểu diễn ma trận đó dới dạng sau đợc gọi là phơng trình bàn tay máy.
R (q)
T0( n ) =
0

p(q)

1

Trong đó
Ma trận R (q) cỡ 3x3 xác định hớng của bàn kẹp.
Véc tơ cột p(q) cỡ 3x1 xác định vị trí bàn kẹp trong toạ độ cơ sở.
3.4 áp dụng quy tắc Denavít Hatenberg cho một vài mô hình rôbốt

3.4.1 Cho rôbốt phẳng hai bậc

tự do nh hình vẽ


2

y0 y2

P(x,y,z)
y1

a2

2

x1

a1
1

x0

Hình 1.13
Ta có bảng tham số động học Denavít Hatenberg nh sau:
Trục
1
2

k

q1

q2

dk

ak

0
0

a1
a2

k

0
0

áp dụng bảng tham số động học Denavít Hatenberg vào biểu thức () sau
đó nhân các ma trận biến đổi toạ độ khâu ta đợc
C1
S
A 2 = T1T2 = 1
0

0
C12
S
= 12
0

0

S1
C1
0
0

0 a1C1 C2 S 2
0 a1S1 S2 C2
1
0 0
0

0 1 0
0
S12 0 a1C1 + a2C2
C12 0 a1S1 + a2 S2

0
1
0

0
0
1


0 a2C 2
0 a2 S2
1

0

0
1

Ba thành phần đầu tiên của cột cuối cùng của ma trận bàn tay máy là toạ độ
của bàn kẹp trong hệ toạ độ cơ sở. Vậy toạ độ bàn kẹp là:
p = [ a1C1 + a2C2 a1S1 + a2 S2 0]

T

3.4.2 Cho rôbốt phẳng ba bậc
tự do nh hình vẽ
Hình 1.14 là sơ đồ rôbốt không gian ba bậc tự do.


y1
z'1

P(x,y,z)
y2

x'2

d1

3

y3
a3

3

x2

a2
2

x1

z0

y0

1

x'1
0

Hình 1.14
Ta có bảng tham số động học Denavít Hatenberg nh sau:
Trục

k

dk

ak

k

1

q1

d1

0

/2

2

q2

0

a2

0

3

q3

0

a2

0

áp dụng bảng tham số động học Denavít Hatenberg vào biểu thức () sau
đó nhân các ma trận biến đổi toạ độ khâu ta đợc
C1
S
A 3 = T1T2 T3 = 1
0

0

0 S1
0 C1
1 0
0 0

C1C23
S C
= 1 23
S 23

0

0 C2
0 S 2
d1 0

1 0

C1S 23
S1S 23

C23
0

S1
C1
0
0

S2
C2
0
0

0 a 2 C 2 C3
0 a2 S 2 S3
1
0 0

0
1 0
C1 (a3C23 + a2C2 )
S1 (a3C23 + a2C2 )
a3 S 23 + a2 S 2 + d1

1


S3 0 a3C3
C3 0 a3 S3
0 1

0

0 0
1

Ba thành phần đầu tiên của cột cuối cùng của ma trận bàn tay máy là toạ độ
của bàn kẹp trong hệ toạ độ cơ sở. Vậy toạ độ bàn kẹp là:
p = [ C1 ( a3C23 + a2C2 ) S1 (a3C23 + a2C2 ) a3 S 23 + a2 S2 + d1 ]

T

IV. Phân tích động học hệ nhiều vật bằng phơng pháp Jacobi
4.1 Thông số định vị vật rắn


Khảo sát cơ hệ gồm p vật rắn, chụi liên kết, giữ, dừng, lý tởng có f bậc
tự do. Gọi q=[q1,q2,,qf]T là véc tơ toạ độ suy rộng đủ của cơ hệ, thông thờng đó là các toạ độ khớp của Robot.
zi

zo

yi
M

ki

Ri
ii

R

o

Hình1.15
Trên
góc
với giá

k0
i0

rOi

Oi

ji

Bi

xi

O j0

yo

mỗi vật rắn Bi ta gắn chặt vào nó một hệ trục toạ độ vuông
xo (Oxyz)i. Khi i=0, hệ toạ độ (Oxyz)0 là hệ toạ độ cố định
hay còn gọi là hệ toạ độ quán tính.

Hệ toạ độ (Oxyz)0 có các vectơ đơn vị trên các trục là i 0 , j0, k0. Hệ toạ độ
(Oxyz)i có các vectơ đơn vị là ii ,j i, ki.

Vị trí của vật rắn Bi đợc xác định nếu ta biết đợc vectơ định vị gốc toạ độ
Oi, là hàm của vectơ toạ độ suy rộng q:
rOi=rOi(q)
Và biết đợc ma trận quay Ai(q) từ hệ R0 sang hệ Ri (hay đợc gọi là ma trận
côsin chỉ hớng của vật rắn Bi đối với hệ quy chiếu R0).
cos(i0 , ii ) cos(i0 , ji ) cos(i0 , ki )
Ai = cos( j0 , ii ) cos( j0 , ji ) cos( j0 , ki )
cos(k0 , ii ) cos(k0 , ji ) cos(k0 , ki )

4.2 Vận tốc góc của vật rắn

z1

z0
M

Véc tơ vận tốc góc i của vật rắn Bi đợc biểu diễn trong hệ qui chiếu R0 dạng:

r
s

i = xi i0 + yi j0 + zi k0 .

OOi

x0

x1

y1

y0


Hình 1.16
Xét chuyển động của điểm M, r0 là bán kính định vị của điểm M trong hệ
toạ độ (Oxyz)0, ri bán kính định vị của M trong hệ toạ độ (Cxyz) i. Với hai
hệ toạ độ có gốc trùng nhau OOi thì ta có:
r r r
ro ri so

Từ
r0=Airi
Ta suy ra:
ri=AiTr0
Vận tốc của điểm M
v=

dr0 d
&r = A
& AT r
= ( Ai ri ) = A
i i
i i 0
dt dt

Mặt khác ta có:
v= r%i 0

Trong đó ta đa vào toán tử sóng %(ma trận phản đối xứng)

0

%= z

y


z
0
x

y

x
0

Từ đó ta suy ra:
& AT
%i = A

i i

Nh vậy, để tìm vận tốc góc của vật rắn Bi ta xác định ma trận phản đối xứng
%theo công thức trên. Từ đó tìm đợc các thành phần của vectơ vận tốc góc
i
%
= %
xi = %
13 ; zi =
23 ; yi

12 ;

Để biểu diễn vectơ vận tốc góc i theo vectơ toạ độ suy rộng q&, ta đa vào
ma trận Jacobi JRi cỡ 3xf:


J
Ri(1,1)
i = J Ri q&= J Ri
( 2,1)
JR
i( 3,1)

J Ri

(1,2 )

L

J Ri

( 2,2)

L

J Ri

L

(3,2 )

q&1
q&
2
J Ri ,
( 2, f )
L
J Ri q&
( 3, f )
f
J Ri

(1, f )

Các phần tử của ma trận JRii đợc xác định nh sau:
JRii = hệ số của q&[ j ]

trong i [ k ]

(k=1,2,3; j=1,2,..,f),

Véc tơ định vị của điểm M trong hệ toạ độ cố định (Oxyz)0 là:
r r r
r = rOi + s

Hay
r = rOi + Ai Ri s ;

Với R s là véc tơ đại số của sr trong hệ qui chiếu động Ri.
i

zi

zo
M

yi

s

Ri

Oi

Bi

r
rOi

R
o

O

xi

Hình
Nh vậy

yo

1.17

xo
r=r(p)

Vận tốc của điểm M đợc xác định bằng đạo hàm bán kính định vị r theo
thời gian:
v=

f
dr
r
=
q&j = J Tiq&
dt j =1 q j

Trong đó JTi là ma trận Jacobi cỡ 3x f.


rx

q1
r
J Ti = y
q1

rz
q
1

rx
q2

L

ry
q2

L

rz
q2

L

rx
q f
ry

q f

rz
q f

Chơng 2
Động lực học rô bốt phẳng 3 bậc tự do
Việc tìm ra mô hình động lực học của tay máy đóng một vai trò quan
trọng trong việc mô phỏng chuyển động và phân tích cấu trúc của những
tay máy và việc thiết kế các thuật toán điều khiển. Mô phỏng chuyển động
của tay máy cho phép kiểm tra các chiến lợc điều khiển và việc lập trình

quỹ đạo chuyển động mà không cần sử dụng các hệ vật lý cụ thể. Việc
phân tích mô hình động lực học có thể rất hữu ích cho những thiết kế cơ
khí đầu tiên của những tay máy. Việc tính toán lực và môment cần thiết cho
việc thực hiện những chuyển động điển hình sẽ cung cấp những thông tin
hữu ích cho việc thiết kế các khớp, các hộp truyền động, hộp phát động.
Hai dạng phơng trình thờng đợc dùng để mô tả chuyển động của các
Rôbốt công nghiệp là dạng phơng trình Newton-Euler và dạng phơng trình
Lagrange. Sử dụng phơng trình sau thờng cho kết quả khá gọn và đẹp đẽ do
chọn các toạ độ suy rộng đủ. Tuy nhiên đối với phơng pháp sau việc tính
toán các đại lợng động lực nh động năng, lực suy rộng trong nhiều trờng
hợp gặp nhiều khó khăn, đặc biệt khi số bậc tự do lớn hoặc có nhiều khớp
không gian phức tạp. Phơng pháp đầu có thuận lợi trong việc lập các phơng
trình động lực nhng không dễ dàng khi viết các phơng trình liên kết đặc
biệt số phần tử tham gia vào hệ lớn.
I. Thiết lập phơng trình vi phân chuyển động của rôbốt
1.1 Biểu thức động năng của hệ nhiều vật
Xem tay máy nh một hệ gồm n vật rắn. Tổng thế năng của toàn hệ
bằng tổng của các thành phần liên quan đến chuyển động của mỗi khâu và
các thành phần liên quan tới chuyển động của mỗi khớp dẫn động.
n

T = (Ti + Tmi )
i =1

Trong đó:
Ti

là động năng của vật rắn thứ Bi.

(2.1)