Phân tích động học thuận Robot bằng phương pháp ma trận DenavitHartenberg

Lời nói đầu
Cách mạng khoa học kỹ thuật trên thế giới đang phát triển với tốc độ rất nhanh,
không ngừng vơn lên những đỉnh cao mới, trong đó phải kể đến các thành tựu về kỹ
thuật tự động hoá sản xuất.
Nhu cầu thị trờng không ngừng nâng cao chất lợng sản phẩm, giảm giá thành,
đổi mới kết cấu, mẫu mã sản phẩm đòi hỏi sự linh hoạt của hệ thông tự động hoá sản
xuất. Nh vậy trong thời kì công nghiệp hoá,hiện đại hoá nh hiện nay Robot là một
trong những công cụ không thể thiếu đợc trong công nghiệp cũng nh trong mọi hoạt
động khác hàng ngày, có nhiều công việc con ngời không thể làm đợc ví dụ nh những
chỗ nguy hiểm (có thuốc độc ). Ngoài ra Robot còn thực hiện những thao tác hợp lý
hơn ngời thợ, ổn định suốt trong thời gian làm việc, nhanh chóng thay đổi theo yêu
cầu chất lợng Cùng với đời sống con ngời mỗi ngày càng đợc nâng cao. Do đó mỗi
ngày chi phí cho công nhân ngày càng lớn vì vậy Robot là công cụ không thể thiếu đ-
ợc để tăng năng suất, giảm giá thành tăng chất lợng cạnh tranh trong thời kì công
nghiệp hoá, hiện đại hoá đất nớc.
Theo hãng Fuji của nhật sử dụng Robot tăng năng suất
Theo hãng Fanue dùng Robot tăng nhịp độ sản xuất lên 3 lần và hoàn toàn không
mệt mỏi.
Bản đồ án tin học với đề tài Phân tích động học thuận Robot bằng phơng pháp
ma trận Denavit-Hartenberg đã hoàn thành.
Em xin chân thành cảm ơn GS,TS .Nguyễn Văn Khang và thầy Đỗ Thành Trung đã
hớng dẫn em trong quá trình thực hiện đồ án môn học này.

Đại Học Bách Khoa Hà Nội,12-2002
Sinh viên
Nguyễn Hữu Dĩnh
Phần I: Phần lý thuyết
I. Giới thiệu chung.
Sự ra đời của robot công nghiệp
Năm 1922 robot xuất hiện tập thơ do nhà Karel capek
Năm 1960 xuất hiện thiết bị robot công nghiệp do hãng AMF

Năm 1967 Nhật mua robot mỹ của hãng AMF
Năm 1990 Nhật có 40 công ty sản xuất robot có một số hãng lớn là Hitachi
Đồ án tin học trong cơ học Nguyễn Hữu Dĩnh lớp Cơtin-A
Hệ sản xuất linh hoạt dùng ngời máy công nghiệp là bớc phát triển mới của kỹ
thuật tự động hoá sản xuất, đồng thời là phơng thức rất thích hợp với qui mô vừa và
nhỏ. Do đó, xu hớng tạo ra những dây truyền và thiết bị tự động có tính linh hoạt cao
đang hình thành. Các thiết bị này đang thay thế các thiết bị tự động cứng chỉ đáp ứng
một việc nhất định trong lúc thị trờng luôn luôn đòi hỏi thay đổi về mặt hàng chủng
loại, về kích cỡ, về tính năngVì thế ngày càng tăng nhanh nhu cầu ứng dụng ngời
máy để tạo ra các hệ sản xuất tự động linh hoạt. Từ khi mới ra đời, robot công nghiệp
đã đợc áp dụng trong nhiều lĩnh vực dới góc độ thay thế sức ngời. Nhờ vậy nhiều dây
chuyền hoặc phân xởng đã đợc tổ chức lại, làm việc hai hoặc ba ca, năng suất và hiệu
quả tăng rõ rệt.
Ngời máy công nghiệp đợc dùng để thay thế con ngời trong những công việc
quá cực nhọc, nguy hiểm hoặc độc hại. Ngời máy công nghiệp còn đợc dùng để thực
hiện các công việc tuy không nặng nhọc về mặt thể lực nhng quá đơn điệu và rất dễ
gây mệt mỏi, nhầm lẫn. Một đặc điểm của Ngời máy công nghiệp là chúng cho phép
dễ dàng kết hợp những việc phụ và việc chính của một quá trình sản xuất thành một
dây truyền tự động. So với các phơng tiện tự động hoá khác, các dây chuyền tự động
dùng Ngời máy công nghiệp có nhiều u điểm nh dễ dàng thay đổi chơng trình làm
việc, khả năng tạo ra dây chuyền tự động từ các máy vạn năng và có thể tự động hoá
toàn phần. Khi sử dụng ngời máy công nghiệp vào các dây chuyền tự động, khâu
chuẩn bị kỹ thuật đợc rút ngắn đi rõ rệt.
II. Cấu trúc và một số đặc điểm của robot.
2.1.định nghĩa robot vàcác bộ phận cấu thành robot.
a. Định nghĩa:
Thuật ngữ robot có nhiều nghĩa khác nhau tuỳ theo nhiều ngời khác nhau. Có
rất nhiều định nghĩa về robot cùng tồn tại, chúng ta có thể tham khảo một số định
nghĩa nh sau:
Robot là một kết cấu cơ khí có hình dạng bất kỳ, đợc xây dựng để thực

hiện những công việc bằng tay của con ngời.
Robot là một máy, cơ cấu thờng gồm có một số các phân đoạn đợc nối với
các phân đoạn khác bằng khớp quay hay khớp trợt nhằm mục đích để gắp
hay để di chuyển các đối tợng, thờng có một số bậc tự do. Nó có thể đợc
điều khiển bởi một nguồn kích hoạt, một hệ điều khiển điện tử có thể lập
trình đợc hay một hệ thống Lô-gíc nào đó (ví dụ nh thiết bị cam hay dây,
v.v.).
b. Các bộ phận cấu thành robot:
2
Đồ án tin học trong cơ học Nguyễn Hữu Dĩnh lớp Cơtin-A
1. Đế 2. Thân 3. Vai 4. Tay trên.
5. Tay dới 6. Cổ tay 7. Bàn kẹp
Tay máy trong các robot là bộ phận chấp hành chủ yếu.
Nội tín hiệu: cho biết các thông tin về bản thân hoạt động bên trong của
robot( chủ yếu ở các khớp động: vị trí nào, tốc độ nào)
Ngoại tín hiệu: cho biết các tín hiệu ở môi trờng làm việc
Các bộ phận cấu thành có thể đợc kết cấu theo kiểu môđun: cụm các chi tiết
máy kết hợp với nhau có công dụng độc lập tơng đối, hợp thành một bộ phận nhỏ của
máy. Có thể tạo thành nhièu loại thiết bị từ các môđun cơ bản chuẩn.
2.2. Bậc tự do và toạ độ suy rộng.
Robot công nghiệp là loại thiết bị tự động nhiều công dụng. Hệ cơ của chúng
phải đợc cấu tạo sao cho bàn kẹp giữ vật kẹp theo một hớng nhất định nào đó và thực
hiện dễ dàng các chuyển dịch muôn mầu muôn vẻ. Muốn vậy cơ cấu chấp hành của
tay máy phải đạt đợc một số bậc tự do. Có lẽ đại lợng đầu tiên khi nghiên cứu động
học cơ cấu là số bậc tự do. Bậc tự do của cơ cấu là số thông số độc lập hay số thông
số cần cho trớc để vị trí của cơ cấu hoàn toàn xác định. Ta có thể thể tìm đợc một
công thức tổng quát tính bậc tự do của cơ cấu theo số khâu, số khớp và loại khớp tạo
thành cơ cấu.
Thờng thờng các khâu của cơ cấu này gọi là các khâu chấp hành, chúng đợc nối
ghép với nhau bằng các khớp quay và khớp tịnh tiến (khớp động học loại 5). Đối với

các cơ cấu này, số bậc tự do bằng số khâu chấp hành.
Trong trờng hợp chung có thể tính toán bậc tự do w theo công thức thông dụng
trong nguyên lý máy:

=
=
5
1
6
i
i
ipnw
n - số khâu động.
p
i
- số khớp loại i.
3
Đồ án tin học trong cơ học Nguyễn Hữu Dĩnh lớp Cơtin-A
Ví dụ: cho cơ cấu nh hình vẽ:
Số khâu động:
3=n
.
Số khớp loại 3:
2
3
=p
.
Số khớp loại 5:
1
5

=p
.
Vậy số bậc tự do của cơ cấu:
7)3251(366
5
1
=ì+ìì==

=i
i
ipnw
.
Các cấu hình khác nhau của cơ cấu tay máy trong từng thời điểm đợc xác định
bằng các toạ độ suy rộng. Chúng thờng là độ dịch chuyển dài hoặc dịch chuyển góc
của các khớp tịnh tiến hoặc khớp quay của tay máy. ở hình dới đây là sơ đồ động học
của một loại tay máy. Cơ cấu có 6 bậc tự do, trong 6 toạ độ suy rộng q
1
ữ q
6
, có q
2
là
dịch chuyển dài của khớp tịnh tiến giữa khâu 2 với khâu 1, còn lại là dịch chuyển góc
của các khớp quay. Các giá trị q
i
là các biến khớp, ta có:
iiiii
Sq )1(

+=

.
Trong đó:
1=
i

, nếu là khớp quay.
ii
q


0=
i

, nếu là khớp tịnh tiến.
ii
Sq
Đối với các cơ cấu nói chung nên lấy số bậc tự do
6

w
.
III. Toạ độ và các phép biến đổi hệ toạ độ.
4
Đồ án tin học trong cơ học Nguyễn Hữu Dĩnh lớp Cơtin-A
Bài toán động học robot bao gồm các bài toán về vị trí, bài toán về vận tốc, gia
tốc. Tuy nhiên, trong phạm vi đồ án tin học lần này ta chỉ đề cập đến việc giải bài
toán về vị trí. Trong bài toán về vị trí thì việc xác định vị trí vị trí và hớng của điểm
tác động cuối tại nhũng thời điểm khác nhau là vấn đề cốt lõi. Để có thể giải quyết đ-
ợc bài toán, thì nh ta đã biết robot là một hệ nhiều vật rắn ghép nối với nhau bằng các
khớp, chủ yếu là khớp quay và khớp tịnh tiến, do vậy cần phải xác định đợc các hệ toạ

độ gắn với các khâu của robot.
Các hệ toạ độ gắn với robot đợc xác định nh sau: chọn hệ toạ độ cố định gắn liền
với giá và các hệ toạ độ động gắn liền với các khâu động. Đánh số thứ tự các hệ toạ
độ này từ 0 đến n bắt đầu từ giá cố định.
3.1. Các phép biến đổi hệ toạ độ dùng ma trận.
3.1.1. Vector điểm và ma trận biến đổi 3x3.
Ta xét hai hệ toạ độ i và j nh sau: trong đó r
i
là bán kính của điểm M trong hệ
toạ độ thứ i, r
j
là bán kính của điểm M trong hệ toạ độ thứ j.
ta có nh sau:
T
iiii
zyxr ),,(=
;
T
jjjj
zyxr ),,(=
r
i
đợc gọi là các véc tơ điểm trong hệ toạ độ thứ i.
Để xác định ma trận biến đổi 3x3 ta xét một điểm M trong hai hệ toạ độ khác
nhau nh sau:
Trong hệ toạ độ (xyz):
5
Đồ án tin học trong cơ học Nguyễn Hữu Dĩnh lớp Cơtin-A
zxyyxxxyz
krjrirrr ì+ì+ì==

.
Trong hệ toạ độ (uvw):
wwvvuuuvw
krjrirrr ì+ì+ì==
.
)(
wwvvuuxx
krjririr ì+ì+ìì=
.
)(
wwvvuuyy
krjririr ì+ì+ìì=
.
)(
wwvvuuzz
krjririr ì+ì+ìì=
.










ì

ììì
ììì
ììì
=











w
v
u
wzvzuz
wyvyuy
wxvxux
z

y
x
r
r
r
kijiii
kijiii
kijiii
r
r
r
.
[ ]
ijuvwxyz
aRrRr =ì= ,
.
Mặt khác ta có:
xuux
iiuxii ì==ì ),cos(










=

),cos(),cos(),cos(
),cos(),cos(),cos(
),cos(),cos(),cos(
zuzuzu
yuyuyu
xwxvxu
R
.
( )
T
RR =
1
Ma trận R = [a
ij
] chính là ma trận biến đổi hệ toạ độ từ hệ (uvw) sang hệ (xyz).
3.1.2. Biến đổi hệ toạ độ dùng ma trận thuần nhất.
a. Các toạ độ thuần nhất
Vị trí của một điểm M ở trong hệ toạ độ ba chiều Oxyz đợc xác định nh sau
zxyyxxxyz
krjrirrr ì+ì+ì==
.
Giả sử w là một đại lợng vô hớng tuỳ ý. Khi đó toạ độ thuần nhất bốn chiều
thuần nhất của điểm M đợc định nghĩa bởi hệ thức
( )
T
zyx
wwwwr ,,,=
.
Trong kỹ thuật, ngời ta thờng chọn w = 1. Khi đó toạ độ thuần nhất bốn chiều
thuần nhất của điểm M đợc mở rộng từ các toạ độ vật lý ba chiều của điểm M băng

cách thêm vào thành phần thứ t nh sau
( )
T
zyx
rrrr 1,,,=
.
Nhờ khái niệm toạ độ thuần nhất trong không gian bốn chiều ta có thể chuyển
bài toán cộng ma trận trong không gian ba chiều sang bài toán nhân ma trận trong
không gian bốn chiều. Cho a và b là hai véctơ trong không gian ba chiều, ta có










+
+
+
=

+










=+
33
22
11
3
2
1
3
2
1
ba
ba
ba
b
b
b

a
a
a
ba
Ta chuyển phép cộng trên bằng phép nhân hai ma trận nh sau
6
Đồ án tin học trong cơ học Nguyễn Hữu Dĩnh lớp Cơtin-A












ì

=












+
+
+
11000
100
010
001
1
3
2
1
3
2
1
33

22
11
b
b
b
a
a
a
ba
ba
ba
b. Ma trận biến đổi toạ độ thuần nhất.
Xét một vật rắn B chuyển động trong hệ qui chiếu cố định Oxyz. Lấy một điểm
A nào đó của vật rắn B và gắn chặt vào vật rắn hệ qui chiếu Auvw. Lấy P là một điểm
bất kỳ thuộc vật rắn rắn B. trong hệ toạ độ vật lý ta có
r
P
= r
A
+s
AP
Sử dụng ngôn ngữ ma trận phơng trình trên có dạng

ì










+










=

w
v
u
A
A
A
P
P
P
s
s
s
aaa
aaa
aaa
z
y
x
z
y
x
333231
232221
131211
trong đó A là ma trận côsin chỉ phơng của vật rắn B, s

u
, s
v
, s
w
là toạ độ của vectơ
s
AP
trong hệ qui chiếu Auvw. Nếu sử dụng các toạ độ thuần nhất thì có thể viết lại ph-
ong trình trên dới dạng












ì

=












110001
333231
232221
131211
w
v
u
A
A
A
P
P

P
s
s
s
zaaa
yaaa
xaaa
z
y
x
nh vậy ta định nghĩa ma trận












=
1000
333231
232221
131211
A
A

A
zaaa
yaaa
xaaa
T
đợc gọi là ma trận chuyển toạ độ thần nhất của điểm P trong hệ Auvw sang hệ
Oxyz.
c. Các ma trận quay cơ bản thuần nhất và tịnh tiến thuần nhất
Phép quay quanh trục x một góc

( )













=
1000
0cossin0
0sincos0
0001

x
A
Phép quay quanh trục y một góc

7
Đồ án tin học trong cơ học Nguyễn Hữu Dĩnh lớp Cơtin-A
( )













=
1000
0cos0sin
0010
0sin0cos

y
A
Phép quay quanh trục z một góc

( )













=
1000
0100
00cossin
00sincos



z
A
Phép tịnh tiến dọc trục x một đoạn là p
x

, trục y một đoạn là p
y
, trục z một
đoạn là p
z
.
( )












=
1000
100
010
001
,,
z
y
x
p
p

p
cbas
3.2. Một số phép quay đặc biệt và ma trận biến đổi thuần nhất.
3.2.1. Các góc Euler và ma trận quay thuần nhất.
Ta có hình vẽ biểu thị ba góc quay Euler nh sau
Vị trí của vật rắn B quay quanh điểm O cố định đợc xác định bởi vị trí của 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 Ox
0
y
0
z
0
.
Giả sử giao của mặt phẳng Ox
0
y
0
và mặt phẳng Oxy là trục OK. Trục OK này đợc gọi
là đờng nút.
Ta đa vào các ký hiệu sau:
Góc giữa trục Ox
0
và OK là :

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

Góc giữa trục Oz
0
và Oz là :

8
Đồ án tin học trong cơ học Nguyễn Hữu Dĩnh lớp Cơtin-A
Ba góc

,,
đợc gọi là 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 roạ độ suy rộng

,,
. Phơng trình chuyển động
của vật rắn quay quanh một điểm cố định có dạng:
( )
t

=
;
( )
t

=
;
( )
t

=
.
Từ đó ta suy ra, vật rắn quay quanh một điểm cố định có ba bậc tự do.
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 Ox
0

y
0
z
0
sang hệ qui chiếu động Oxyz bằng ba phép quay Euler nh sau:
Quay hệ qui chiếu R
0


Ox
0
y
0
z
0
quanh trục Oz
0
một góc

để trục Ox
0
chuyển tới đờng nút OK. Với phép quay này hệ Ox
0
y
0
z
0
chuyển sang hệ
Ox
1

y
1
z
1
với Oz
0


Oz
1
.
Quay hệ qui chiếu R
1


Ox
1
y
1
z
1
quanh trục Ox
1


OK một góc

để Oz
0

Oz
1
chuyển tới trục Oz
2


Oz. Nh thế hệ qui chiếu Ox
1
y
1
z
1
chuyển sang hệ
qui chiếu Ox
2
y
2
z
2
với Ox
1


Ox
2

OK.
Quay hệ qui chiếu R
2

Ox
2
y
2
z
2
quanh trục Oz
2


Oz một góc

để trục
Ox
2


OK chuyển tới trục Ox. Với phép quay này hệ qui chiếu Ox
2
y
2
z
2
chuyển sang hệ qui chiếu Oxyz với Oz
2


Oz.

Nh thế bằng ba phép quay Euler quanh trục Oz
0
một góc

, quanh trục OK một
góc

, quanh trục Oz một góc

, hệ qui chiếu Ox
0
y
0
z
0
chuyển sang hệ qui chiếu
Oxyz.
Các ma trận quay ứng với các phép quay Euler có dạng
( )

=
1000
0100
00cossin
00sincos
0



z
A
( )













=
1000
0cossin0

0sincos0
0001



K
A
9
Đồ án tin học trong cơ học Nguyễn Hữu Dĩnh lớp Cơtin-A
( )













=
1000
0100
00cossin
00sincos

z
A
Bây giờ ta xác định ma trận quay hệ qui chiếu Ox
0
y
0
z
0
sang hệ qui chiếu
Oxyz( cũng là ma trận côsin chỉ hớng của hệ qui chiếu Oxyz đối với hệ qui chiếu
Ox
0
y
0
z
0
). Ta có:
( ) ( ) ( )

KKz
AAAR ìì=
0
),,(
.
( ) ( )
( ) ( )

++

=
1000
0
0
0
),,(




CSSSC
SSCCCCSCCCCS
CCSSCCCSSCCC
R
kí hiệu: Cx = cos(x), Sx = sin(x);
3.2.2. Các góc R-P-Y( Roll-Pitch-Yaw) và ma trận quay thuần nhất.
Việc định hớng khâu cuối có thể thực hiện theo các phép quay Roll-Pitch-Yaw
nh sau:

Quay quanh trục Zn một góc

.
Quay quanh trục Yn một góc

.
Quay quanh trục Xn một góc

.
Nh vậy ta có các ma trận quay ứng với các góc quay Roll-Pitch-Yaw :
( )













=
1000
0cossin0
0sincos0
0001

Xn
A
10
Đồ án tin học trong cơ học Nguyễn Hữu Dĩnh lớp Cơtin-A
( )













=
1000
0cos0sin
0010
0sin0cos



Yn
A

( )













=
1000
0100
00cossin
00sincos



Zn
A
Tơng tự nh đối với phép quay Euler, ta cũng có ma trận chuyển hệ qui chiếu nh
sau
( ) ( )
( ) ( )

+
+
=
1000
0
0
0
),,(




CCCCS
CCSSSSCCSSSSSS
CSCSCCSCSCCC
R
kí hiệu: Cx = cos(x), Sx = sin(x);
IV. Hệ ph ơng trình động học Robot.
4.1. Đặt vấn đề.

Robot là một chuỗi các khâu, chúng đợc nối với nhau bằng các khớp quay, khớp
tịnh tiến. Điểm tác động cuối cùng E thực hiện chuyển động dễ dàng linh hoạt theo
một quĩ đạo nào đó. Tuy nhiên điểm tác động cuối không những phải bám theo một
quĩ đạo nhất định mà còn phải giữ đợc một hớng nhát định để đảm bảo việc tiếp cận
với đối tợng công tác. Vậy điều khiển Robot là điều khiển điểm tác động cuối E di
chuyển theo một quĩ đạo cho trớc và đảm bảo hớng của điểm tác động cuối.
Trong giới hạn của đồ án này, chúng ta chỉ đi sâu vào việc thành lập hệ phơng
trình động học của robot khi biết các biến khớp, ta sẽ xác định đợc vị trí và hớng của
điểm tác động cuối (trạng thái của điểm tác động cuối).
4.2. Ma trận quan hệ.
Chọn hệ toạ độ cố định gắn liền với giá đỡ và các hệ hệ toạ động gắn với từng
khâu động. Ký hiệu các hệ toạ độ này từ 0 đến n, kể từ giá trị cố định trở đi.
Một điểm bất kỳ nào đó trong không gian đợc xác định trong hệ toạ độ thi bằng
bán kính véctơ r
i
và trong hệ toạ độ cố định x
0
, y
0
, z
0
đợc xác định bằng bán kính
véctơ r
0
.

ii
rAAAr
210
=

(4.1)
hoặc:
ii
rTr .
0
=
(4.2)
với
;
21 ii
AAAT =

ni , ,2,1=
(4.3)
11
Đồ án tin học trong cơ học Nguyễn Hữu Dĩnh lớp Cơtin-A
Trong đó ma trận A
i
mô tả vị trí và hớng của khâu đầu tiên, ma trận A
2
mô tả vị
trí hớng của khâu thứ hai so với khâu đầu; ma trận A
i
mô tả vị trí hớng của khâu thứ i
so với khâu thứ i-1.
Nh vậy, tích của các ma trận A
i
là ma trận T
i
mô tả vị trí và hớng của khâu thứ i

so với gía cố định. Thờng ký hiệu ma trận T với 2 chỉ số: trên và dới. Chỉ số dới để chỉ
khâu đang xét còn chỉ số trên để chỉ toạ độ đợc dùng để đối chiếu. ví dụ biểu thức
(4.3) có thể viết lại là:

iii
TATT .
1
1
0
==
(4.4)
với:
ii
AAAT
32
1
=
(4.5)
là ma trận mô tả vị trí và hớng của khâu thứ i so với khâu thứ nhất. Trong ký hiệu th-
ờng bỏ qua chỉ số trên nếu chỉ số đó bằng 0.
4.3. Bộ thông số động học Denavit-Hartenberg và ma trận Denavit-Hartenberg.
a. Các tham số động học Denavit-Hartenberg.
Nh đã giới hạn, trong đồ án này chỉ xét hệ các vật rắn nối ghép với nhau bằng
các khớp quay và các khớp tịnh tiến. Khi đó quan hệ giữa hai khâu kkế tiếp nhau có
thể xác định bởi hai tham số khớp, nh hình vẽ dới đây:
Trên hình trên khâu thứ i -1 nối với khâu i bằng khớp thứ i. Trục z
i-1
đợc chọn là
trục khớp của khớp thứ i. Tham số thứ nhất
i

, đợc gọi là góc khớp, là góc quay trục
x
i-1
quanh trục z
i-1
đến trục x
i
// x
i
. Tham số thứ hai d
i
, là khoảng cách giữa hai trục x
i
và trục x
i
. Nếu khớp i là khớp quay thì
i

là biến còn d
i
là hằng số. Nếu i là khớp tịnh
tiến thì khoảng cách d
i
là biến còn
i

là hằng số.
Đối với các robot công nghiệp Denavit-Hertenberg đa ra cách chọn hệ trục toạ
độ nh sau:

1. Trục z
i-1
đợc chọn dọc theo hớng của trục khớp động thứ i.
2. Trục x
i-1
đợc chọn dọc theo đờng vuông góc chung của hai trục z
i-2
và z
i-1
,
hớng đi từ trục z
i-2
đến z
i-1
. nếu trục z
i-1
cắt trục z
i-2
thì hớng của trục x
i-1
đ-
ợc chọn tuỳ ý.
12
Đồ án tin học trong cơ học Nguyễn Hữu Dĩnh lớp Cơtin-A
3. Gốc toạ độ đợc chọn trụcại giao điểm trục x
i-1
và trục z
i-1
.
4. Trục y

i-1
đợc chọn sao cho hệ toạ độ (Oxyz)
i-1
là hệ qui chiếu thuận. Với
cách chọn hệ toạ độ nh trên, nhiều khi hệ toạ độ khâu (Oxyz)
i-1
không đợc
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
nh sau:
5. Đối với hệ toạ độ (Oxyz)
0
theo qui ớc trên thì chỉ chọn đợc trục z
0
, ta có
thể chọn trục x
0
tuỳ ý.
6. Đối với hệ toạ độ (Oxyz)
n
do không co khớp thứ n+1, nên theo qui ớc trên
ta không xác định đợc trục z
n
. Trục z
n
không đựoc xác định một cách duy
nhất, trong khi trục x
n
lại đợc chọn theo pháp tuyến trục z
n-1
. Ngoài ra có

thể chọn tuỳ ý soa cho hợp lý.
7. Khi hai trục z
i-2
và trục z
i-1
song song với nhau, giữa hai trục có nhiều đ-
ờng pháp tuyến chung, ta có thể chọn trục x
i-1
theo hớng pháp tuyến chung
nào cũng đợc.
8. Khi khớp thứ i là khớp tịnh tiến, về nguyên tắc ta có thể chọn trục z
i-1
một
cách tuỳ ý. Tuy nhiên, trong nhièu trờng hợp ngời ta thờng chọn trục z
i-1
dọc theo trục của khớp tịnh tiến này.
Vị trí của hệ toạ độ khâu (Oxyz)
i
đối với hệ toạ độ khâu (Oxyz)
i-1
đợc xác định
bởi bốn tham số Denavit Hertenberg
i

, d
i
, a
i
,
i

nh sau:

i

: góc quay trục x
i-1
quanh trục z
i-1
đến trục x
i
(x
i
// x
i
).
d
i
: dịch chuyển tịnh tiến dọc theo trục z
i-1
đến gốc toạ độ O
i-1
chuyển đến O
i
,
giao điểm trục x
i
và trục z
i-1
.

a
i
: dịch chuyển tịnh tiến dọc theo trục x
i
để điểm O
i
chuyển đến điểm O
i
.

i

: góc quay quanh trục xi sao cho trục z
i-1
chuyển đến trục z
i
.
Dới đây là hình minh hoạ cách chọn các hệ toạ độ theo ý tởng của Denavit -
Hertenberg:
13
Đồ án tin học trong cơ học Nguyễn Hữu Dĩnh lớp Cơtin-A
Trong bốn tham số trên, các tham số a
i
và
i

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 giữa các khâu thứ i và khâu thứ i-
1. Hai tham số còn lại là d
i

và
i

, một là hằng số, một là biến phụ thuộc vào khớp i là
khớp tịnh tiến hay khớp quay. Khi khớp i là tịnh tiến thì d
i
là biến số, còn
i

là hằng
số. Khi khớp i là quay thì
i

là biến số, còn d
i
là hằng số.
b. Ma trận Denavit-Hartenberg.
Ta có thể chuyển toạ độ khâu (Oxyz)
i-1
sang hệ toạ độ khâu (Oxyz)
i
bằng bốn
phép biến đổi cơ bản nh sau:
Quay quanh trục z
i-1
một góc
i

.
Dịch chuyển tịnh tiến dọc trục z

i-1
một đoạn d
i
.
14
Đồ án tin học trong cơ học Nguyễn Hữu Dĩnh lớp Cơtin-A
Dịch chuyển tịnh tiến dọc trục x
i
một đoạn a
i
.
Quay quanh trục x
i
một góc
i

.
Ma trận của phép biến đổi, kí hiệu là Trục T
i
, là tích của bốn ma trận biến đổi có
bản và có dạng nh sau:
( )

=
1000
0100
00cossin
00sincos
ii
ii
iZ
R



,
( )

=
1000
100
0010
0001
i
iz
d
dT
( )













=
1000
0cossin0
0sincos0
0001
ii

ii
iX
R



,
( )












=
1000
0100
0010
001
i
ix
a
aT
Khi đó ta có:

( ) ( ) ( ) ( )
iXixiziZi
RaTdTRT

ììì=














=
1000
cossin0
sinsincoscoscossin
cossinsincossincos
iii
iiiiiii
iiiiiii
i
d
a

a
T



Định nghĩa: Ma trận T
i
đợc xác định bởi công thức trên đợc gọi là ma trận
Denavit-Hartenberg. Đó chính à ma trân côsin chỉ phơng của hệ qui chiếu (Oxyz)
i-1
đối với hệ qui chiếu (Oxyz)
i
. Chính xác hơn ta phải ký hiệu ma trận này bằng T
i
i-1
, để
đơn giản ta sử dụng ký hiệu T
i
với nghĩa T
i
i-1
, còn A
i
đợc dùng với nghĩa T
i
0
.
4.4. Phơng trình xác định vị trí khâu thao tác(bàn kẹp) của robot.
áp dụng liên tục phép biến đổi đối với hai khâu liên tiếp đối với robot n khâu ta
đợc phơng trình xác định vị trí khâu thao tác (bàn kẹp) của robot.

( ) ( ) ( ) ( ) ( )
n
n
nnn
TTTTTTTTTA
321
12
3
1
2
0
1
0
===

.
Phần II: Phần lập trình
Trong bản đồ án tin này ta sử dụng phần mềm Maple để thực hiện việc xây dựng ch-
ơng trình tính. Trong chơng trình này các thông số đầu vào là các bộ thông số
Denavit-Hertenberg và máy tính sẽ tự động tính toán và cho ra kết quả là các phơng
trình động học của robot.
Dới đây là chơng trình tính toán
-Xây dựng module giải bài toán động học thuận của Robot công nghiệp bằng
Maple
Robot:=module()
15
§å ¸n tin häc trong c¬ häc NguyÔn H÷u DÜnh líp C¬tin-A
#option package,'copyrigh(C) noname';
export DH,vantoc,giatoc;
#Tinh ma tran cua khau thu i

DH:=proc(x,y,z,t)
local a11,a12,a13,a14,
a21,a22,a23,a24,
a31,a32,a33,a34,
a41,a42,a43,a44,
A;
a11:=cos(x);
a12:=-sin(x)*cos(z);
a14:=t*cos(x);
a13:=sin(x)*sin(z);
a21:=sin(x);
a22:=cos(x)*cos(z);
a23:=-cos(x)*sin(z);
a24:=t*sin(x);
a31:=0;
a32:=sin(z);
a33:=cos(z);
a34:=y;
a41:=0;
a42:=0;
a43:=0;
a44:=0;
A:=array([[a11,a12,a13,a14],[a21,a22,a23,a24],[a31,a32,a33,a34],
[a41,a42,a43,a44]]);
print("ma tran Denavit-Hartenberg la:");
print(A);
RETURN(A);
end:
# ham tinh van toc cua diem thao tac
vantoc:=proc(A)

local i,j,px,py,pz,c1,c2,c3,Vp;
a:=array(1 4,1 4);
for i to 4 do
for j to 4 do
16
§å ¸n tin häc trong c¬ häc NguyÔn H÷u DÜnh líp C¬tin-A
a[i,j]:=A[i,j];
px:=a[1,4];
py:=a[2,4];
pz:=a[3,4];
c1:=diff(px,t);
c2:=diff(py,t);
c3:=diff(pz,t);
Vp:=sqrt(c1^2+c2^2+c3^2);
od;
od;
print("van toc cua diem tac dong cuoi cung:");
print(Vp);
RETURN(Vp);
end:
# ham tinh gia toc cua diem thao tac
giatoc:=proc(A)
local i,j,px,py,pz,c1,c2,c3,gt;
a:=array(1 4,1 4);
for i to 4 do
for j to 4 do
a[i,j]:=A[i,j];
px:=a[1,4];
py:=a[2,4];
pz:=a[3,4];

c1:=diff(px,t$2);
c2:=diff(py,t$2);
c3:=diff(pz,t$2);
gt:=sqrt(c1^2+c2^2+c3^2);
od;
od;
print("gia toc cua diem tac dong cuoi cung la:");
print(gt);
RETURN(gt);
end:
end module:
libname:="C:/Dinh4",libname;
march('create',libname[1],10):
17
Đồ án tin học trong cơ học Nguyễn Hữu Dĩnh lớp Cơtin-A
savelib('Robot');
Phần III: Phần gọi modul
Từ đờng dẫn của modul Robot trong C:/Dinh4
RESTART;
libname:="C:/Dinh4",libname:
With(Robot);
Sau đó gọi các hàm : DH,vantoc,giatoc ta sẽ đợc kết quả sẽ đợc trình bày trong các
thí dụ dới đây.
Phần IV: phần thí dụ lập trình
Một số ví dụ về việc thiết lập hệ toạ độ, bộ thông số và ma trận Denavit-Hertenberg
của một số robot điển hình.
Trong các thí dụ dới đây ta chủ yếu đi sâu vào việc thiết lập phơng trình động học
robot(bài toán vị trí)
Thí dụ 1: Rôbot có 2 bậc tự do
Xét một cơ cấu phẳng gồm hai khâu nh hình vẽ dới đây:

Từ hình vẽ ta dễ dàng thiết lập bảng các thông số động học D-H cho ví dụ này
nh sau:
Ma trận Denavit-Hertenberg có đợc bằng cánh gọi hàm DH
T[1]:=DH(q[1],0,0,a[1]);
T[2]:=DH(q[2],0,0,a[2]);
Kết quả tính toán bằng máy tính có đợc(do kết quả tính toán bằng máy tính quá cồng
kềnh nên ta dùng kí hiệu đã đợc định nghĩa ở dới)
18
Đồ án tin học trong cơ học Nguyễn Hữu Dĩnh lớp Cơtin-A













=
1000
0100
0
0
1111
1111
1

SaCS
CaSC
T













=
1000
0100
0
0
2222
2222
2
SaCS
CaSC
T
Vậy ma trận xác định phơng trình động học
with(linalg);
A[2]:=multiply(T[1],T[2]);

Kết quả là












+
+
=
1000
0100
0
0
122111212
122111212
2
SaSaCS
CaCaSC
A
Ba thành phần đầu tiên của cột cuối cùng của ma trận A
2
là các toạ độ điểm cuối cùng
P. Vậy ta có

( )
12211
0
CaCax
p
+=
,
( )
12211
0
SaSay
p
+=
.
0
)0(
=
p
z
Trong thí dụ này cũng nh trong các thí dụ sau ta sử dụng kí hiệu
11
cos qC =
,
11
sin qS =
,
22
cos qC =
,
22

sin qS =
)cos(
2112
qqC +=
,
)sin(
2112
qqS +=
+ Xác định bài toán vận tốc và gia tốc của khâu thao tác bàn kẹp cuối cùng
trên đã trình bày a
i
và
i
luôn luôn là hằngsố. Hai tham số còn lại
i
và d
i
,một là
hằng số một là biến phụ thuộc vào khớp quay hay khớp tịnh tiến. Nếu khớp i là
khớp quay thì
i
là biến, còn d
i
là hằng. Khi khớp i là khớp tịnh tiến thì d
i
là
biến , còn
i
là hằng số.
Vận tốc dài là:

V
p
=
222
)()()(

++
ppp
ZYX
để tính vận tốc dài ta gọi hàm vantoc
V[p]:=vantoc(A[2]);
Tơng tự để tính gia tốc ta cũng phải gọi hàm giatoc
a[p]:=giatoc(A[2]);
+ Gia tốc của điểm tác động cuối cũng chính là đạo hàm bậc hai của vị trí
a
p
=
22
2
)()()(

++
ppp
ZYX
Do biểu thức tính vận tốc,gia tốc trên máy tính là quá cồng kềnh do đó ta không đa ra
ở đây.
19
Đồ án tin học trong cơ học Nguyễn Hữu Dĩnh lớp Cơtin-A
Kết quả máy tính là:
> restart;

> libname:="C:/Dinh4",libname;
:= libname ,"C:/Dinh4" "C:\\PROGRAM FILES\\MAPLE 7/lib"
> with(Robot);
[ ], ,DH giatoc vantoc
> T[1]:=DH(q[1],0,0,a[1]);
"ma tran Denavit-Hartenberg la:"






















( )cos q
1

( )sin q
1
0 a
1
( )cos q
1
( )sin q
1
( )cos q
1
0 a
1
( )sin q
1
0 0 1 0
0 0 0 1
:= T
1
A
> T[2]:=DH(q[2],0,0,a[2]);
"ma tran Denavit-Hartenberg la:"

( )cos q
2
( )sin q
2
0 a
2
( )cos q
2
( )sin q
2
( )cos q
2
0 a
2
( )sin q
2
0 0 1 0

0 0 0 1
:= T
2
A
> with(linalg);
Warning, the protected names norm and trace have been redefined and unprotected
BlockDiagonal GramSchmidt JordanBlock LUdecomp QRdecomp Wronskian addcol, , , , , , ,[
addrow adj adjoint angle augment backsub band basis bezout blockmatrix, , , , , , , , , ,
charmat charpoly cholesky col coldim colspace colspan companion concat cond, , , , , , , , , ,
copyinto crossprod curl definite delcols delrows det diag diverge dotprod, , , , , , , , , ,
eigenvals eigenvalues eigenvectors eigenvects entermatrix equal exponential, , , , , , ,
extend ffgausselim fibonacci forwardsub frobenius gausselim gaussjord geneqns, , , , , , , ,
genmatrix grad hadamard hermite hessian hilbert htranspose ihermite indexfunc, , , , , , , , ,
innerprod intbasis inverse ismith issimilar iszero jacobian jordan kernel, , , , , , , , ,
laplacian leastsqrs linsolve matadd matrix minor minpoly mulcol mulrow, , , , , , , , ,
multiply norm normalize nullspace orthog permanent pivot potential randmatrix, , , , , , , , ,
randvector rank ratform row rowdim rowspace rowspan rref scalarmul, , , , , , , , ,
singularvals smith stackmatrix submatrix subvector sumbasis swapcol swaprow, , , , , , , ,
sylvester toeplitz trace transpose vandermonde vecpotent vectdim vector, , , , , , , ,
wronskian]
20
Đồ án tin học trong cơ học Nguyễn Hữu Dĩnh lớp Cơtin-A
> A[2]:=multiply(T[1],T[2]);
A
2
:=
( )cos q
1
( )cos q
2

( )sin q
1
( )sin q
2
( )cos q
1
( )sin q
2
( )sin q
1
( )cos q
2
0[ , , ,
+ ( )cos q
1
a
2
( )cos q
2
( )sin q
1
a
2
( )sin q
2
a
1
( )cos q
1
]

+ ( )sin q
1
( )cos q
2
( )cos q
1
( )sin q
2
( )cos q
1
( )cos q
2
( )sin q
1
( )sin q
2
0[ , , ,
+ + ( )sin q
1
a
2
( )cos q
2
( )cos q
1
a
2
( )sin q
2
a

1
( )sin q
1
]
[ , , , ]0 0 1 0
[ , , , ]0 0 0 1
>
Thí dụ 2: Phân tích động học Robot có 3 bậc tự do
Bảng thông số DH(Denavit-Hatenberg)
Trục

i
d
i
A
i

i
1 q
1
d
1
0
/2
2 q
2
0 A
2
0
3 q

3
0 A
3
0
Ma trận Denavit-Hartenberg là:













=
1000
010
00
00
1
11
11
1
d
CS
SC

T













=
1000
0100
0
0
2222
2222
2
SaCS
CaSC
T
21
P(
x
p,
y

p,
z
p
)

2

1

3
d
1
a
2
a
3
x
0
x
1
x
1
y
1
z
0
x
2
y
2

y
3
x
3
Đồ án tin học trong cơ học Nguyễn Hữu Dĩnh lớp Cơtin-A













=
1000
0100
0
0
3333
3333
3
SaCS
CaSC
T
Ma trận xác địnhphơng trình động học

++
+
+
=ìì=
1000
0
)(
)(
1222332323
2223311231231
2223311231231
3213
dSaSaCS
CaCaSCSSCS
CaCaCSCCCC
TTTA

phơng trình xác định vị trí của khâu thao tác

x
p
= cos(q
1
)[a
3
cos(q
2
+q
3
) + a
2
cos(q
2
)]
y
p
= sin(q
1
)[a
3
cos(q
2
+q
3
) + a
2
cos(q
2
)]

z
p
= a
3
sin(q
2
+q
3
) + a
2
sin(q
2
) +d
1
Kết quả máy tính:
> restart;
> libname:="C:/Dinh4",libname;
:= libname ,"C:/Dinh4" "C:\\PROGRAM FILES\\MAPLE 7/lib"
> with(Robot);
[ ], ,DH giatoc vantoc
> T[1]:=DH(q[1],d[1],Pi/2,0);
"ma tran Denavit-Hartenberg la:"

( )cos q
1
0 ( )sin q
1
0
( )sin q
1
0 ( )cos q
1
0
0 1 0 d
1
0 0 0 1
:= T

1
A
> T[2]:=DH(q[2],0,0,a[2]);
"ma tran Denavit-Hartenberg la:"






















( )cos q
2
( )sin q
2

0 a
2
( )cos q
2
( )sin q
2
( )cos q
2
0 a
2
( )sin q
2
0 0 1 0
0 0 0 1
:= T
2
A
> T[3]:=DH(q[3],0,0,a[3]);
"ma tran Denavit-Hartenberg la:"
22
§å ¸n tin häc trong c¬ häc NguyÔn H÷u DÜnh líp C¬tin-A























( )cos q
3
− ( )sin q
3
0 a
3
( )cos q
3
( )sin q
3
( )cos q
3
0 a
3
( )sin q
3
0 0 1 0

0 0 0 1
:= T
3
A
> with(linalg);
Warning, the protected names norm and trace have been redefined and unprotected
BlockDiagonal GramSchmidt JordanBlock LUdecomp QRdecomp Wronskian addcol, , , , , , ,[
addrow adj adjoint angle augment backsub band basis bezout blockmatrix, , , , , , , , , ,
charmat charpoly cholesky col coldim colspace colspan companion concat cond, , , , , , , , , ,
copyinto crossprod curl definite delcols delrows det diag diverge dotprod, , , , , , , , , ,
eigenvals eigenvalues eigenvectors eigenvects entermatrix equal exponential, , , , , , ,
extend ffgausselim fibonacci forwardsub frobenius gausselim gaussjord geneqns, , , , , , , ,
genmatrix grad hadamard hermite hessian hilbert htranspose ihermite indexfunc, , , , , , , , ,
innerprod intbasis inverse ismith issimilar iszero jacobian jordan kernel, , , , , , , , ,
laplacian leastsqrs linsolve matadd matrix minor minpoly mulcol mulrow, , , , , , , , ,
multiply norm normalize nullspace orthog permanent pivot potential randmatrix, , , , , , , , ,
randvector rank ratform row rowdim rowspace rowspan rref scalarmul, , , , , , , , ,
singularvals smith stackmatrix submatrix subvector sumbasis swapcol swaprow, , , , , , , ,
sylvester toeplitz trace transpose vandermonde vecpotent vectdim vector, , , , , , , ,
wronskian]
> A[3]:=multiply(T[1],T[2],T[3]);
A
3
:=
− ( )cos q
1
( )cos q
2
( )cos q
3

( )cos q
1
( )sin q
2
( )sin q
3
[ ,
− − ( )cos q
1
( )cos q
2
( )sin q
3
( )cos q
1
( )sin q
2
( )cos q
3
( )sin q
1
, ,
− + ( )cos q
1
( )cos q
2
a
3
( )cos q
3

( )cos q
1
( )sin q
2
a
3
( )sin q
3
( )cos q
1
a
2
( )cos q
2
]
− ( )sin q
1
( )cos q
2
( )cos q
3
( )sin q
1
( )sin q
2
( )sin q
3
[ ,
− − ( )sin q
1

( )cos q
2
( )sin q
3
( )sin q
1
( )sin q
2
( )cos q
3
− ( )cos q
1
, ,
− + ( )sin q
1
( )cos q
2
a
3
( )cos q
3
( )sin q
1
( )sin q
2
a
3
( )sin q
3
( )sin q

1
a
2
( )cos q
2
]
+ ( )sin q
2
( )cos q
3
( )cos q
2
( )sin q
3
− + ( )sin q
2
( )sin q
3
( )cos q
2
( )cos q
3
0[ , , ,
+ + + ( )sin q
2
a
3
( )cos q
3
( )cos q

2
a
3
( )sin q
3
a
2
( )sin q
2
d
1
]
[ , , , ]0 0 0 1
23
Đồ án tin học trong cơ học Nguyễn Hữu Dĩnh lớp Cơtin-A
>
Thí dụ 3:Robot SCARA.
Robot SCARA có bốn khâu nh hình vẽ dới đây. Đây là robot có kiểu RRTR.
Từ hình vẽ minh hoạ trên ta xác định đợc bộ thông số động học của Robot nh
sau:
Các ma trận biến đổi thuần nhất nh sau

=
1000
0100
0
0
1111
1111
1
SaCS
CaSC
T
,

=
1000
0100
0
0
2222
2222
2
SaCS
CaSC
T













=
1000
3100
0010
0000
3

d
T
,














=
1000
4100
00
00
44
44
4
d
CS
SC
T
ma trận tích là:

24
Đồ án tin học trong cơ học Nguyễn Hữu Dĩnh lớp Cơtin-A












++
++
+++
=ììì=
1000
)(100
0
0
134
11212412412412412
11212412412412412
4321
ddd
aSaSCCSSCSSC
aCaCCSSCSSCC
TTTTA

phơng trình xác định vị trí khâu thao tác là
11221
)cos()cos( aqaqqx
p
++=
11221
)sin()sin( aqaqqy
p
++=
134
)( dddz
p
++=
Thí dụ 4:Robot Stanford
Robot Stanford có 6 khâu với cấu hình RRTRRR.
Hệ toạ độ gắn lên các khâu của robot nh hình vẽ (kích thớc của khâu chấp hành cuối
có thể thay đổi khi gắn các công cụ khác nhau nên chọn O
6
trùng O
5
).
Bảng thông số DH(Denavit-Hartenberg) của robot Stanford nh sau:
Khâu

a
i
d
i
Khớp
1

1
*
-90
0
0 0 R
2

2
*
90
0
0 d
2
R
3 0 0 0 d
3
*
R
4

4
*
-90
0
0 0 R
5

5
*

90
0
0 0 R
6

6
*
0 0 0 R
(*: các biến khớp)
các ma trận T
i
của robot Stanford đợc xác định nh sau:
25
z
2
z
0
z
1
x
0
O
0
d
2
d
3
z
4
z

3,
z
5,
z
6
x
Hệ toạ độ của Robot Stanford