Đồ án giải bài toán động học Robot Công nghiệp bằng phương pháp ma trận Jacobi

TRƯỜNG ĐẠI HỌC KTCN
KHOA CƠ KHÍ

CỘNG HỊA XÃ HỘI CHỦ NGHĨA VIỆT NAM
Độc lập – Tự do – Hạnh phúc
-------------ĐỒ ÁN MÔN HỌC

MÔN HỌC: THIẾT KẾ RÔ BỐT CÔNG NGHIỆP
BỘ MÔN: CƠ ĐIỆN TỬ
Sinh viên: Vũ Văn Quân Mã số sinh viên: K175520114108 Lớp: K53CĐT.02
Đỗ Bảo Thịnh Mã số sinh viên: K175520114120 Lớp: K53CĐT.02
Ngành: Cơ điện tử
Ngày giao đề: .........................................Ngày hồn thành: .......................................
1.Tên đề tài: Giải bài tốn động học Robot Công nghiệp bằng phương
pháp ma trận Jacobi . (Mã số: IR-04).
2. Nội dung thuyết minh tính tốn:
Nhiệm vụ đồ án bao gồm:
 Tổng quan về đối tượng thiết kế.
 Giải quyết bài toán động học thuận, động học ngược.
 Giải quyết bài tốn lập trình quỹ đạo làm việc.
 Mơ phỏng động học q trình làm việc.
3. Các bản vẽ, chương trình và đồ thị
 Thuyết minh, chương trình và đồ thị mơ phỏng.

TRƯỞNG BỘ MƠN

GIÁO VIÊN HƯỚNG DẪN

ThS. Nguyễn Ngọc Hà

NHẬN XÉT CỦA GIÁO VIÊN HƯỚNG DẪN

1


.......................................................................................................................................
.......................................................................................................................................
.......................................................................................................................................
.......................................................................................................................................
.......................................................................................................................................
.......................................................................................................................................
Thái Nguyên, ngày….tháng…..năm 20....
GIÁO VIÊN HƯỚNG DẪN
(Ký ghi rõ họ tên)

NHẬN XÉT CỦA GIÁO VIÊN CHẤM
.......................................................................................................................................
.......................................................................................................................................
.......................................................................................................................................
.......................................................................................................................................
.......................................................................................................................................
.......................................................................................................................................
Thái Nguyên, ngày….tháng…..năm 20....
GIÁO VIÊN CHẤM
(Ký ghi rõ họ tên)

Lời nói đầu
Cho tới nay đã có hàng vạn robot ra đời và đang làm việc trong nhiều lĩnh vực sản
xuất và đời sống – xã hội ở khắp các quốc gia trên thế giới. Ngày nay tên gọi
2

Robot đã khá phổ biến trong nhân dân. Robot đã và đang trở thành nguồn lực lao
động với năng suất và chất lượng cao trong nhiều lĩnh vực như: công nghiệp, nơng
nghiệp, y học , an ninh quốc phịng v…v...Trong số đó robot cơng nghiệp đóng vai
trị rất quan trọng.
Robot nói chung và robot cơng nghiệp nói riêng là sự tích hợp giữa các lĩnh vực cơ
học - điện – điện tử và kĩ thuật điều khiển. Có thể nói robot là một trong số các loại
hình sản phẩm trí tuệ cao cấp trong thời đại ngày nay.
Bài thuyết trình của chúng em sau đây gồm 4 phần chính:
 Tổng quan về robot cơng nghiệp
 Giải quyết bài tốn động học thuận, động học ngược.
 Giải quyết bài toán lập trình quỹ đạo làm việc.
 Mơ phỏng động học q trình làm việc.
Chúng em xin chân thành cảm ơn các thầy, cơ đã nhiệt tình giúp đỡ chúng em giải
đáp những khó khăn, thắc mắc trong q trình làm bài.

Chương 1: TỔNG QUAN VỀ ROBOT CÔNG NGHIỆP
1.1 Sự ra đời của Robot
Một trong những ước mơ to lớn của con người đó là: Làm thế nào để tạo ra được
những con người bằng máy móc để giúp con người trong cuộc sống. Mãi đến năm
2021 người ta mới thấy một con rối xuất hiện trên sân khấu ở Tiệp Khắc trong vở
3


kịch “Rossum’s Universal Robot ” của nhà viết kịch viễn tưởng Karel Capek. Trong
vở kịch này tác giả đã sử dụng một Robota với khả năng biểu diễn rất linh hoạt thông
qua sự điều khiển của các nghệ sĩ xiếc. Có lẽ cũng chính từ đó đã gợi cho các nhà
sáng chế nghĩ tới việc tạo ra một tay máy hoặc người máy. Nó có kết cấu giống với
bàn tay con người để thực hiện những nhiệm vụ trong lao động, sản xuất, đặc biệt
trong những môi trường khắc nghiệt,..
1.2 Tình hình phát triển của Robot

-Vào những thập kỉ đầu tiên của thế kỉ XX, hệ thống điều khiển từ xa xuất hiện và
phát triển nhanh chóng. Nhờ đó các tay máy điều khiển bằng tay cũng ra đời. Tuy
nhiên trong giai đoạn này khả năng hoạt động của các tay máy còn bị hạn chế như độ
linh hoạt, khả năng mang tải, khả năng di chuyển,…
-Đến những năm 1950 với việc xuất hiện và phát triển hệ thống điều khiển tự động
theo chương trình số, các cơ cấu điều phối vơ cấp, các hệ điện tốn, nhờ đó hình
thành hệ điều khiển NC. Sự kết hợp khéo léo giữa NC và hệ thống điều khiển từ xa ra
đời một hệ thống máy tự động ở trình độ cao được gọi là “người máy”. Năm 1961
người máy xuất hiện đầu tiên ở Mỹ phục vụ trong lĩnh vực công nghiệp. Sau Mỹ đến
các nước Anh 1967, Thụy Điển, Nhật Bản 1968.
-Thị trường Robot ngày càng sơi động, loại hình Robot cũng ngày càng đa dạng và
phong phú. Trong đó phải kể đến các nước phát triển như Nhật Bản, Hàn Quốc, Mỹ,
Đức, Italia,Pháp, Anh,….Robot công nghiệp trong những năm gần đây phát triển
ngày càng nhiều và được ứng dụng rộng rãi trong các hệ thống sản xuất tự động linh
hoạt.
- Do yêu cầu phát triển về tự động hóa ngày càng cao, quy mô sản xuất ngày càng lớn
nên không những Robot chỉ được sử dụng trong công nghiệp mà còn còn được sử
dụng trong đời sống xã hội, an ninh quốc phịng nên robot cũng càng ngày càng hồn
thiện hơn
-Ngày nay robot đã tạo ra một lực lượng lao động mới trong xã hội với năng suất và
chất lượng ngày càng cao
1.3 Ứng dụng của robot
1.3.1 Ứng dụng của robot công nghiệp
Robot được sử dụng trong nhiều lĩnh vực sản xuất và đời sống của con người, trong
đó robot cơng nghiệp đóng vai trị rất quan trọng và nó được ứng dụng trong nhiều
lĩnh vực công nghiệp như:
- Phục vụ máy NC và các hệ thống tự động linh hoạt
- Đúc,
- Phun, phủ
- Tự động hàn (Hình 1.0)

- Lắp ráp, đóng gói (Hình 1.1)
4


- Đảm nhận thực hiện cấp phôi phục vụ các nguyên công trong các dây chuyền sản
xuất tự động
- Chế tạo máy
- Kiểm tra
- Sơn
- Bảo vệ

Hình 1.0: Robot hàn

Hình 1.1: Robot lắp ráp
1.3.2 Tình hình ứng dụng robot cơng nghiệp trên thế giới
Việc sử dụng robot ở các nước công nghiệp phát triển như Nhật, Mỹ, Đức, Italy,
Pháp, Trung Quốc, Anh rất phổ biến và tốc độ phát triển hàng năm rất nhanh, theo
thống kê chưa đầy đủ tỷ lệ phân bố phạm vi ứng dụng robot công nghiệp trong các
lĩnh vực như sau: Oto 31%; thiết bị điện 16%; dầu khí, hóa thực phẩm 12%; thiết bị
5


viễn thơng 8%; máy cơng cụ 6%; kim khí 4%; máy tính 3% và các lĩnh vực dầu
khống 20%
1.4 Cấu trúc
Một robot thường gồm có 3 bộ phận chính:

-

 Tay máy

 Bộ phận dẫn động
 Bộ phận điều khiển
Tay máy: Tay máy đóng vai trị là một bộ phận chấp hành của robot. Tay máy
được mô phỏng giống bàn tay của con người. giữa cánh tay và bàn tay có
nhiều khâu và khớp trung gian, số lượng các khâu và khớp để tạo nên một tay
máy phụ thuộc nhiều vào nhiệm vụ, chức năng của robot. Tay máy có nhiều
bậc tự do, có khả năng thực hiện các thao tác trong mặt phẳng hoặc trong
không gian với một phạm vi nhất định

Hình 1.2: Hình ảnh tay máy cơng nghiệp
-

Bộ phận dẫn động: các động cơ dùng để dẫn động robot thường là các loại
động cơ sau đây:
 Động cơ điện
 Động cơ điện xoay chiều
 Động cơ điện một chiều
 Động cơ bước
 Động cơ khí nén
 Động cơ thủy lực
6


Trong đó động cơ khí nén, thủy lực được sử dụng trong trường hợp yêu cầu công suất
lớn, tốc độ chậm mà các loại động cơ điện không đáp ứng được. Có một số loại robot
cơng nghiệp người ta kết hợp cả hai loại động cơ thủy lực và động cơ điện
Tuy nhiên khi sử dụng hệ thống dẫn động robot bằng thủy lực hay khí nén, thì kết
cấu cơng kềnh và phức tạp hơn so với động cơ điện do có các bộ phận kèm theo hệ
thống ống dẫn, van điều tiết, và hệ thống máy bơm thủy lực,…
-

Bộ phận điều khiển: Để điều khiển các hoạt động của Robot, bộ phận điều
khiển trong robot hoạt động giống như bộ não của con người. Bộ phận điều
khiển thường được thơng qua một hệ thống chương trình điều khiển- mỗi
chương trình được đảm nhận một nhiệm vụ cụ thể

1.5 Phân loại Robot
Để phân loại robot có thể dựa trên những cơ sở kĩ thuật khác nhau, dưới đây trình bày
một vài phương pháp phân loại chủ yếu
1.5.1 Phân loại theo khơng gian hoạt động của tay máy
Để tìm khơng gian hoạt động của tay máy trước hết phải gắn tay máy với một hệ trục
tọa độ gốc hay còn gọi là hệ tọa độ chuẩn oxyz và lấy hai hình thức chuyển động
nguyên thủy làm chuẩn đó bao gồm:
 Chuyển động thẳng theo hướng x, y, z trong không gian ba chiều để tạo
nên các hình khối có góc cạnh được kí hiệu là P
 Chuyển động quay quanh các trục x, y, z được kí hiệu là R
Với ba bậc tự do, robot sẽ hoạt động trong các trường cơng tác tùy thuộc tổ
hợp P và R ví dụ :
- PPP trường công tác là hộp chữ nhật hoặc lập phương
- RPP trường công tác là khối trụ
- RRP trường công tác là khối cầu
- RRR trường công tác là khối cầu

1.5.2 Phân loại theo phương pháp điều khiển
Có nhiều cách phân loại hệ thống điều khiển tự động, thơng thường có 3 loại :
7


 Điều khiển hở
- Điều khiển hở, dùng truyền động bước ( động cơ điện hoặc đơng cơ thủy lực,

khí nén,...) mà quãng đường hoặc góc dịch chuyển tỉ lệ với số xung điều
khiển. Kiểu này đơn giản, nhưng đạt độ chính xác thấp.
 Điều khiển kín – điều khiển servo
- Điều khiến kín ( điều khiển kiểu servo ), sử dụng tín hiệu phản hồi vị trí để
tăng độ chính xác điều khiển. Có 2 kiểu điều khiển servo: điều khiển điểm và
điều điều khiển theo đường ( contour )
 Hệ thống điều khiển hỗn hợp
- Trên cơ sở hai hệ thống điều khiển mở và kín người ta kết hợp lại thành hệ
thống điều khiển hỗn hợp
1.5.3 Phân loại theo ứng dụng
Tùy thuộc vào công dụng của robot, người ta có thể phân robot ra nhiều loại:
- Lắp ráp cơ khí
- Hàn
- Vận chuyển
- Phẫu thuật
- Lau nhà
- Các loại khác
1.6 Nhiệm vụ của bài toán thiết kế
- Xác định được Robot phục vụ cho cơng việc gì ? Để hình thình ý tưởng vềo hình
dáng cấu tạo, cấu trúc của Robot hay Tay máy
- Thực hiện các yêu cầu về thiết kế Robot thực hiện được các yêu cầu đặt ra như : Vật
liệu thiết kế, máy móc ,vị trí làm việc của Robot, số bậc tự do ,giá thành lắp ráp thiết
kế…
1.7 Nội dung
a. Phân tích và chọn cơ cấu
+ số bậc tự do cần thiết
+ các phương án thiết kế và cấu trúc các khâu khớp
+ phân tích và thiết kế cấu trúc đã chọn ( với cơ cấu <4DOF thì sẽ dễ thiết kế nhưng
lại kém linh hoạt , cơ cấu 4,5,6 DOF linh hoạt nhưng thiết kế khó và giảm dần về độ
cứng vững)

b. Khảo sát bài toán ĐH Thuận
+ vẽ cơ cấu
+ đặt các hệ trục toạ độ cơ sở và địa phương tuân theo quy tắc Denavit Hatenberg
+ lập bảng D_H với cơ cấu vừa thiết kế
8


+ dạng tổng quát ma trận cho các khâu
+ Thiết lập các phương trình động học Robot
c. Giải bài tốn ĐH Ngược
+ Phương pháp Số (cho kết quả gần đúng)
+ Phương pháp giải tích ( cho kết quả chính xác)
1.8 Các công cụ hỗ trợ
+ Matlab , Exce

Chương 2: Những kiến thức cơ sở
2.1 Ma trận cosin chỉ hướng
2.1.1 Định ngĩa ma trận cosin chỉ hướng của vật rắn
r (0) r (0) r (0)

r (0) r (0) r (0)

Cho vật rắn B và hệ quy chiếu R0 = { e1 , e 2 , e3 }. Trong đó e1 , e2 , e3 là ba

r
e
vector trên các trục Ox0 , Oy0, Oz0. Ta gắn chặt vào vật rắn một hệ quy chiếu R= { 1 ,
r
r
r r r

e2 , e3 } với e1 , e 2 , e3 là ba vector đơn vị trên các trục Ax, Ay, Az (Hình 2.1).

Z0

Z
Y

Z1
e2

e3
A
e(0)
3

X1

e1

Y1

X

O
e1(0)

B

e(0)
2

X0

Hình 2.1
9

Y0


2.1.2 Định nghĩa : Ma trận vuông cấp ba

r (0) r
�
e1 .e1
�
r (0) r
�
e2 .e1
�
r (0) r
�
e3 .e1
A= �

r (0) r
e1 .e 2
r (0) r
e2 .e2
r(0) r
e3 e 2

r(0) r
e1 .e3 �
r(0) r �
e2 .e3 �
r(0) r �
e3 .e3 �
�

(2.1)

Được goi là ma trận cosin chỉ hướng cả vật rắn B với hệ quy chiếu R0. Nếu ta đưa
vào ký hiệu
r (0) r
r (0) r
e
i .e j  cos(e i .e j )
aij =
(i, j = 1, 2, 3)
(2.2)
Thì ma trận cosin chỉ hướng (2.1) có dạng

A=

a11
�
�
a12
�
�

a13
�

a21
a22
a23

a31 �
a32 �
�
a33 �
�

(2.3)

Từ định nghĩa trên, trong hệ quy chiếu R0 ta có các hệ thức liên hệ

r
r (0)
r (0)
r (0)
e1  a11.e1  a21.e2  a31.e3

r
r (0)
r (0)
r (0)
e 2  a12 .e1  a22 .e 2  a32 .e 3
r
r (0)

r (0)
r (0)
e3  a13 .e1  a23 .e 2  a33 .e3

(2.4)

r
e
Nếu ta ký hiệu ei là ma trận hàng gồm các phần tử của vector i trong hệ quy chiếu

R0.

r
e1   a11 a21 a31 

,

r
e2   a12

a22

a32 

,

r
e3   a13 a23

a33 

(2.5)
Thì ma trận cosin chỉ hướng (2.3) có dạng

A=

e1 �
�
�
e2 �
�
�
�
e
�3 �
�

Ma trận cosin chỉ hướng A còn được gọi là ma trận quay của vật rắn.
2.1.2 Một số tính chất cơ bản của ma trận cosin chỉ hướng
a) Tính chất 1 : Ma trận cosin chỉ hướng A là ma trận trực giao
Theo công thức (2.6)

10

(2.6)


e1 �
�
A�

e2 �
�
�
�
e3 �
�
�

Vậy ma trận cosin chỉ hướng A là ma trận có 3 hàng là ba vector trực chuẩn. Do đó A
là ma trận trực giao.
Hệ quả : Trong 9 thành phần cơ bản của ma trận cosin chỉ hướng có 3 thành phần độc
lập.
Do tính chất của ma trận cosin chỉ hướng là ma trận trực giao nên A.AT =E .Từ đó
nhận được 6 phương trinh liên hệ giữa các thành phần của ma trận cosin chỉ hướng
như sau:
2
a112  a21
 a312  1
2
2
a122  a22
 a32
1
2
2
a132  a23
 a33
1

a11.a12  a21.a22  a31.a32  0

a11.a13  a21.a23  a31.a33  0

a12 .a13  a22 .a23  a32 .a33  0
Do vậy chỉ có ba thành phần chỉ hướng là độc lập.
b) Tính chất 2: Định thức của ma trận cosin chi hướng
det (A) = 1.
Từ hệ thức A.AT = E ta suy ra
det (A.AT) = det(A).det(AT) =det(E) = 1
Do: det(A) = det(AT) nên ta có det(AT) = �1. Ta có thể chứng minh det (A) = 1.
c)Tính chất 3: Ma trận cosin chỉ hướng có ít nhất 1 trị riêng λ1=1.
2.1.3 Ý nghĩa của ma trận cosin chỉ hướng của vật rắn
Xét 2 hệ quy chiếu R0 và R có cùng gốc O. Trong đó hệ quy chiếu R0 = Ox0y0z0 là hệ
quy chiếu cố định , hệ quy chiếu R = Oxyz gắn liền với vật rắn B. Lấy mộtuuđiểm
P
u
r r
bất kì thuộc vật rắn B. Vị trí của điểm P được xác định bởi vector định vị OP  r p .
(Hình 2.2)

11


Z0
Z

e(0)
3

e3

Y
P

e2

e1
B

(0)
e1

X

Y0

e(0)
2

X0

Hình 2.2
Kí hiệu các tọa độ của điểm P trong hệ quy chiếu động Oxyz là x p, yp, zp, các tọa
x (0)
y (0)
z (0)
p
p
độ của điểm P trong hệ quy chiếu cố định Ox0y0z0 là
,
, p .

Ta có hệ thức sau:

r
r
r
r
(0) (0)
(0) (0)
(0) (0)
r p  x p .e1  y p .e 2  z p .e3

(2.7)

r
r
r
r
r p  x p .e1  y p .e 2  z p .e3
(2.8)
Thế các biểu thức (2.4) vào hệ thức (2.8) ta được

r
r(0)
r (0)
r(0)
r(0)
r(0)
r(0)
r (0)
r(0)

r(0)
r p  x p (a11.e1  a21.e 2  a31.e3 )  y p (a12 .e1  a22 .e 2  a32 .e3 )  z p (a13.e1  a23.e 2  a33 .e3 )

(2.9)
Hay :

r
r (0)
r(0)
r (0)
r p  (a11.x p  a12 . y p  a13 .z p ).e1  (a21.x p  a22 . y p  a23 .z p ).e 2  (a31.x p  a32 . y p  a33 .z p ).e 3
(2.10)

So sánh các biểu thức (2.7) và (2.10) ta suy ra hệ phương trình

x (0)
p  a11 .x p  a12 . y p  a13 .z p
y (0)
p  a21 .x p  a22 . y p  a23 .z p

z

(0)
p

 a31.x p  a32 . y p  a33 .z p

Hệ phương trình (2.11) có thể viết lại dưới dạng ma trận như sau
12

(2.11)


�
� �
x (0)
a11
p
�(0) � �
y p � �
a21
�
(0)
�
zp �
a31
�
�
� �

a12
a22
a32

a13 ��
xp �
� �
a23 �
.�
yp �

�
a33 �
zp �
��
�
�

(2.12)

Từ hệ phương trình (2.12) ta rút ra kết luận sau: Ma trận cosin chỉ hướng A biến
đổi các tọa độ của điểm P bất kỳ thuộc vật rắn trong hệ quy chiếu động Oxyz sang
các tọa độ của điểm P trong hệ quy chiếu cố định O0x0y0z0.
2.2 Các ma trận quay cơ bản
Ta quy ước hướng quay dương là hướng quay ngược chiều kim đồng hồ như hình
vẽ (2.3).
Z



O



Y



X

Hình 2.3

Các phép quay quanh trục x, y,z của hệ tọa độ vng góc Oxyz được gọi là phép
quay cơ bản.
Ta tìm ma trận của phép quay quanh trục x0 một góc φ (Hình 2.4).

Z0
Z

e(0)
3

e3

Y

e2


O

e(0)
2
Hình 2.4

Theo cơng thức định nghĩa (2.1) ta có
13

Y0


r (0) r

�
e1 .e1
�
r (0) r
�
e 2 .e1
�
r (0) r
�
e3 .e1
Ax0 (φ) = �

Ax0 (φ) =

r (0) r
e1 .e 2
r (0) r
e 2 .e 2
r (0) r
e3 .e 2

1
0
�
�
0 cos
�
�
0 sin 
�

r (0) r
e1 .e3 �
r (0) r �
e 2 .e3 �
r (0) r �
e3 .e3 �
�

(2.13)

0

�
 sin  �
�
cos �
�

(2.14)

Ma trận (2.14) được gọi là ma trận quay của phép quay cơ bản quanh trục x 0.
Bằng cách tương tự, ta xác định được các ma trận quay cơ bản quanh các trục y 0 và z0
(Hình 2.5)
cos  sin  0 �
�cos 0 sin  �
�
� 0
�
�

1
0 �
sin  cos 0 �
�
�
�
�
 sin 0 cos �
0
1�
� , Az0 (θ) = �
�0
�
Ay0 () = �
(2.15)
Từ các công thức (2.14) và (2.15) ta dễ dàng tính được
det Ax0 (φ) = det Ay0 () = det Az0 (θ)
(2.16)
Z0

Y0

Y

Z

e2

X

X

e(0)
2

e3

e(0)
3
e1

e1





X0

X0

e1(0)

e1(0)

Hình 2.5
Tuy nhiên nếu chỉ dùng các phép quay cơ bản riêng lẻ thì có nhiều trường hợp ta
khơng đạt được hướng và vị trí theo mong muốn. Muốn vậy ta phải sử dụng phép
quay tổng hợp. Khi một số ma trận quay cơ bản được nhân với nhau, ma trận kết quả
biểu diễn một trình tự các phép quay xung quanh một vetor đơn vị. Một phép quay

như thế gọi là phép quay tổng hợp. Ma trận kết quả đó được gọi là ma trận quay tổng
hợp. Nhờ sử dụng phép quay tổng hợp mới có thể đạt được hướng và vị trí mong
muốn. Như đã biết với phép quay cơ bản được biểu diễn bằng một ma trận. Tuy
nhiên do phép nhân ma trận khơng có tính chất giao hốn, thường A.B ≠B.A. vì thế
trình tự các phép quay cơ bản khác nhau sẽ tạo ra phép quay tổng hợp khác nhau.
Hơn nữa khi thực hiện phép quay thì trục của hai hệ tọa độ khơng cịn trùng nhau, vì
thế phép quay kế tiếp có thể thực hiện hoặc là quanh vetor đơn vị của trục trên hệ cố
14


định hay quanh vector đơn vị của trục trên hệ vừa bị quay. Để giải quyết vấn đề này
người ta thường dùng thuật toán sau:
Khời gán ma trận quay ban đầu R= E.E là ma trận đơn vị. Điều này đảm bảo cho
hệ cố định và hệ bị quay trùng nhau.
Nếu hệ bị quay quay một góc θ quanh vector đơn vị k của hệ cố định thì nhân
Rk(θ) ở trước R. Tức là R= Rk (θ).E.
Nếu hệ bị quay quay một góc θ quanh vector đơn vị k của chính nó thì nhân Rk (θ) ở
sau R. Tức là R = E.Rk(θ).
Tiếp tục làm như vậy nếu có nhiều phép quay cơ bản nữa.
2.3 Các tọa độ thuần nhất và ma trận biến đổi tọa độ thuần nhất
Khái niệm tọa độ thuần nhất được Dennavit – Hartenberg đưa ra năm 1955, và
hiện nay đang được dùng rất rộng rãi trong tính tốn động học robot.
Các tọa độ thuần nhất
Định nghĩa: Cho X ={ x1, x2…xn } là một điểm trong không gian n chiều Rn.
Tập hợp (n+1) phần tử (y1, y2…yn, yn+1) với (yn+1 ≠ 0) và:

yn
y1
y2
 x1

 x2
 xn
yn 1
y
y
; n 1
; … n 1
(2.17)
Gọi là tọa độ thuần nhất của X. Trong kỹ thuật người ta thường chọn (yn+1=1).
Vậy điể m P (x, y, z) trong tọa độ vật lý R3 được biểu diễn trong tọa độ thuần nhất R4
như sau :
P = [x, y, z]T
p = [x, y ,z, 1]T
Trong R3
Trong tọa độ thuần nhất R4
Nhờ khái niệm tọa độ thuần nhất trong không gian 4 chiều ta có thể chuyển bài tốn
cộng ma trận cột trong khơng gian ba chiều sang bài tốn nhân ma trận trong không

r
r
b
gian bốn chiều, Cho a và
là hai vector trong khơng gian ba chiều, ta có :
a1 � �
b1 � �
a1  b1 �
�
�
�
�

a2 �
b2 �
a2  b2 �
�
� �
� �
�
�
a3 �
b3 �
a3  b3 �
�
��
�
� �
�
�

a+b =
(2.18)
Ta chuyển phép tính cộng (2.18) bằng phép tính nhân hai ma trận như sau:
1 0 0 a1 ��
b1 �
�a1  b1 � �
�
�
�
�
�
a b

0 1 0 a2 b2 �
�2 2 � �
�� �
�
a3  b3 � �
0 0 1 a3 ��
b3 �
�
� �
�� �
0 0 0 1 ��1 �
� 1 � �
(2.19)
2.3.1 Ma trận biến đổi tọa độ thuần nhất
Xét một vật rắn B chuyển động trong hệ quy chiếu cố định Ox0y0z0. 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ệ quy chiếu Axyz (Hình 2.6). Lấy P
là một điểm bất kỳ thuộc vật rắn B. Trong hệ tọa độ vật lý Ox0y0z0 ta có:
15


ur ur uuur
rp  rA  sAP

(2.20)

Phương trình (2.20) có thể viết dưới dạng ma trận như sau:

�
� �
x (0)

xA(0) � �
r11
p
�(0) � �(0) � �
y p � �
y A � �
r21
�
�
� �
z (0)
z A(0) �
r31
p � �
�
�
��

r12
r22
r32

r13 �
sx �
�
�
r23 �
sy �
�
�

�
�
r33 �
sz �
�
�
�

(2.21)
Trong đó:

R=

r11
�
�
r21
�
�
r31
�

r12
r22
r32

r13 �
r23 �
�
r33 �

�

là
uuuma
r trận cosin chỉ hướng của vật rắn B.
Sx, Sy, Sz là tọa độ của vetor S AP trong hệ quy chiếu Axyz.
Nếu sử dụng hệ các tọa độ thuần nhất, phương trình (2.12) có thể được viết lại dưới
dạng

�
� �
x (0)
r11
p
�(0) � �
yp � �
r21
�

�
� �
z (0)
r31
p
� � �
�1 � �0

r12

r13

r22
r32
0

r23
r33
0

sx �
x A(0) �
�
�
�
�
s
y A(0) �
y
��
�
�
sz �
z (0)
A
�
��
1�
1 �
�

(2.22)
Z0

Z
e3

ra
(0)
e1

SAP

A

rP

e(0)
3

O

Y

P
e2

e1
X

e(0)

2

Y0

X0

Hình 2.6
Định nghĩa: Ma trận

�
r11
�
r21
�
�
r31
�
0
H = �

r12

r13

r22

r23

r32

r33

0

0

x A(0) �
�
y A(0) �
z A(0) �
�
1 �

(2.23)
được gọi là ma trận chuyển tọa độ thuần nhất của điểm P từ hệ Axyz sang hệ Ox 0y0z0.
16


Hoặc viết rút gọn lại:

R a�
�
�
0 1�
�
H= �
(2.23a)
Trong đó: a – ma trận trận 3x1 biểu thị tọa độ của điểm gốc hệ tọa độ
A so với điểm gốc tọa độ O.
Khi ta muốn có một phép biến đổi ngược lại, tức là muốn tìm ma trận chuyển tọa

độ thuần nhất của điểm P từ hệ Ox0y0z0 sang hệ Axyz. Ma trận chuyển tương ứng sẽ
có dạng
�
RT a '�
�
�
0 1�
-1
�
H=H =
(2.23b)
Trong đó: a’ – ma trận 3x1 biểu thị tọa độ của điểm gốc hệ tọa độ O so với điểm
gốc tọa độ A.
Như vậy ứng dụng ma trận thuần nhất trong phép biến đổi tọa độ tỏ ra có nhiều ưu
điểm, bởi vì trong ma trận 4x4 bao gồm cả thông ton về sự quay và cả về dịch chuyển
tịnh tiến.
Các ma trận quay cơ bản (2.14), (2.15) mở rộng ra trong hệ tọa độ thuần nhất bốn
chiều có dạng như sau
1
0
�
�
0 cos
�
�
0 sin 
�
0
0
Ax0 (φ) = �

�cos 0
� 0
1
�
�
 sin 0
�
0
A () = � 0
y0

cos 
�
�
sin 
�
�0
�
Az0 (  ) = � 0

0
 sin 
cos
0

0�
0�
�
0�
�

1�
sin 0 �
0
0�
�
cos 0 �
�
0
1�

 sin 
cos 
0
0

0
0
1
0

0�
0�
�
0�
�
1�

(2.24)

(2.25)

(2.26)

Ngoài ra ta đưa khái niệm ma trận tịnh tiến thuần nhất có dạng:
1
�
�
0
 a, b, c    �
�
0
�
0
�
T

0 0 a�
1 0 b�
�
0 1 c�
�
0 0 1�

(2.27)
Trong đó ta thực hiện chuyển động tịnh tiến theo trục tọa độ x một đoạn là a, theo
trục tọa độ y một đoạn b, theo trục tọa độ z một đoạn c.
2.4 Các tham số động học và ma trận Denavit – Hartenberg
2.4.1 Các tham số động học Denavit – Hartenberg
Về phương diện cơ học, robot công nghiệp là hệ nhiều vật rắn ghép nối với nhau
bằng các khớp, chủ yếu là các khớp quay và khớp tịnh tiến. Có nhiều phương pháp

17


tính tốn động học robot cơng nghiệp. Ở đây ta giới hạn trình bày phương pháp ma
trận Denavit – Hartenberg tính tốn động học robot. Ta chỉ xét 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ệ vị trí giữa hai
khâu kế tiếp nhau có thể được xác định bời hai tham số khớp (hình 2.7).

Hình 2.7
Trên hình (2.7) khâu thứ (i-1) nối với khâu thứ i bằng khớp i. Trục Zi-1 được chọn là
trục khớp của 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 xi-1 quanh trục zi-1 đến trục xi’// xi. Tham số thứ hai di, là khoảng cách giữa trục
xi’ và trục xi. Nếu khớp i là khớp quay thì θi là biến, cịn di là hằng số.nếu khớp i là
tịnh tiến thì khoảng cách di biến, cịn θi là hằng số.
Đối với các robot công nghiệp, Denavit- Hartenberg (1955) đã đưa ra cách chọn hệ
trục tọa độ như sau:
1. Trục Zi-1được chọn theo hướng của trục khớp động thứ i.
2. Trục Xi-1 được chọn dọc theo đường vuông góc chung của hai trục Z i-2 và Zi-1 ,
hướng đi từ trục Zi-2 sang trục Zi-1 .Nếu trục Zi-1 cắt trục Zi-2 thì hướng của trục Xi1 được chọn tùy ý.
3. Gốc tọa độ Oi-1 được chọn tại giao điểm của trục Xi-1 và trục Zi-1.
4. Trục Yi-1 được chọn sao cho hệ (Oxyz) i-1 là hệ quy chiếu thuận.Với cách chọn hệ
tọa độ như trên, đôi khi các hệ tọa độ 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ổ xung thích hợp như sau:

18


Hình 2.8: Biểu diễn thơng số Denavit – Hartenberg
5. Đối với hệ tọa độ (Oxyz)n , theo quy ước trên ta mới chỉ chọn được trục Z 0, còn
trục x0 chưa có trong quy ướ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 quy ước trên ta có
khơng xác định được trục z n. 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 z n-1. Trong trường hợp này, nếu
khớp n là khớp quay ta nên chọn trục z n song song với trục zn-1.Ngồi ra ta có thể
chọn tùy ý sao cho hợp lý.
7. Khi hai trục zi-2 và zi-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 xi-1 hướng theo 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 zi-1 một cách tùy
ý. Tuy nhiên trong nhiều trường hợp người ta thường chọn theo trục zi-1 dọc theo
trục của khớp tịnh tiến này.
Vị trí của hệ tọa độ khâu (Oxyz)i đối với hệ tọa độ khâu (Oxyz)i-1 được xác định bởi
bốn tham số Denavit – Hartenberg θi,, di, αi, và ai như sau:
θi: Góc quay quanh trục z để trục x chuyển đến trục xi’
di: Dịch chuyển tịnh tiến dọc theo trục z để gốc tọa độ O chuyển đến O’, giao điểm
của trục xi và trục zi-1
ai: Dịch chuyển tịnh tiến dọc theo trục xi để điểm Oi’ chuyển đến điểm Oi.
αi: Góc quay quanh trục xi sao cho trục zi-1 ‘ chuyển đến trục zi.
Trong bốn tham số trên, các tham số ai 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 các khâu thứ i-1 và thứ i. Hai tham số
còn lại θi,, và di, một là hằng số một là biến số phụ thuộc vào khớp i là khớp quay hay
khớp tịnh tiến. Khi khớp i là khớp quay thì θi là biến cịn di là hằng số. Khi khớp i là
khớp tịnh tiến thì di là biến cịn θi là hằng số.
2.4.2 Ma trận Denavit – Hartenberg
Ta có thể chuyển đổi tọa độ khâu (Oxyz)i-1 sang hệ tọa độ khâu (Oxyz)i bằng bốn
phép biến đổi ma trận cơ bản như sau.
19


1

Quay quanh trục zi một góc θi.

2

Dịch chuyển tịnh tiến dọc trục zi-1 một đoạn di.

3

Dịch chuyển tịnh tiến dọc trục xi một đoạn ai.

4

Quay quanh trục xi một góc αi.

Ma trận quay của phép biến đổi, ký hiệu là H, tích của bốn ma trận biến đổi cơ bản
có dạng như sau:
cosi
�
�
sin 
Hi  � i
�0
�
�0

 sin  i

0

cos  i

0

0

1

0

0

0 ��
1
�
�
0 0
�
.�
0 ��
0
��
1 ��
0

cosi
�
�
sin 
Hi  � i
�0

�
�0

0

0

1

0

0

1

0

0

0 ��
1
�
�
0
0
�
.�
di ��
0
��

1 ��
0

 sin i cos  i
cos  i cos  i
sin  i
0

0

0

1

0

0

1

0

0

ai ��
1
0
�
�
0

0 cos i
�
.�
0 ��
0 sin  i
��
1 ��
0
0

sin i sin  i
 cos i sin  i
cos i
0

0
 sin  i
cos i
0

0�
0�
�
0�
�
1�

acosi �
a sin i �
�

di �
�
1 �

(2.28)
Định nghĩa: Ma trận Hi được xác định bởi công thức (2.23) được gọi là ma trận
Denavit – Hartenberg.
Ma trận Denavit – Hartenberg Hi là ma trận chuyển tọa độ của hệ qui chiếu (Oxyz)i-1
đối với hệ quy chiếu (Oxyz)i. Chính xác hơn ta phải kí hiệu ma trận này bằng i-1Hi để
đơn giản trong cách viết sau này, ta sử dụng kí hiệu Hi với nghĩa i-1Hi , còn Ti đưuọc
dùng với nghĩa 0Hi.

20


CHƯƠNG 3: ĐỘNG HỌC ROBOT
Xét về phương diện cơ học, robot công nghiệp là hệ nhiều vật rắn ghép nối với
nhau bằng các khớp, chủ yếu là các khớp quay và các khớp tịnh tiến. Cơ cấu chấp
hành của robot 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. Để robot có thể thao tác linh hoạt, cơ cấu chấp hành của nó phải 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 nào đó theo yêu cầu.
Khâu cuối cù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 robot lên đối
tác và được gọi là “điểm tác động cuối”. Ở điểm này cần quan tâm đến cả hướng tác
động và vị trí của nó trong khơng gian.
Gán vào “điểm tác động cuối” này là một hệ tọa độ động thứ n và gắn cố định
với mỗi khâu động một hệ tọa độ động khác, còn gắn liền với giá đỡ hệ tọa độ cố
định. Ta đánh số các ký hiệu các hệ này từ 0 đến n, bắt đầu từ giá cố định, khi khảo
sát chuyển động của robot cần biết định vị và định hướng tại điểm tác động cuối

trong mọi thời điểm. Nhiều khi lại cần biết cả vận tốc và gia tốc chuyển động của
robot tại điểm tác động cuối đó cũng như tại các điểm khác trên robot. Đó là nội dung
quan trọng của bài toán động học robot. Chúng được xây dựng trên cơ sở thiết lập
các mối quan hệ giữa các hệ tọa độ động nói trên so với hệ tọa độ cố định.
Trong bài toán động học thuận. Trạng thái của robot tại điểm tác động cuối hoàn
toàn xác định bằng sự định vị và định hướng tại điểm tác động cuối đó. Biểu thị sự
định vị và định hướng đó bằng ma trận trận trạng thái cuối TE như sau:
nx
�
�
n
TE  �y
�
nz
�
�0

sx
sy
sz
0

ax
ay
az
0

px�
py�
�

pz�
�
1 �

(3.1)
Trong đó:
n: Véc tơ pháp tuyến.
s: Véc tơ có hướng đường trượt đóng mở bàn kẹp.
a: Véc tơ có hướng trượt tiếp cận với đối tác.
On-1
Z0

Yn
S
On
a
Zn
n

O0
Y0

Hình 3.1
21

Xn


Một điểm bất kì trong hệ tọa độ thứ i xác định bằng vector ri và trong hệ tọa độ cố
định được xác định bằng vector r0, áp dụng với các kết quả thu được ở phần trên ta có

r0 = H1.H2…Hi.ri
(3.2)
Hay
r0 = Ti.ri
(3.3)
Ti = H1.H2…Hi
(3.4)
Trong đó Hi mơ tả vị trí và hướng của khâu thứ nhất so với giá cố định, Ai mơ tả vị
trí và hướng của khâu thứ i so với khâu thứ i-1. Như vậy T i mơ tả vị trí của khâu i so
với giá cố định.
Nhiệm vụ của bài toán động lực học thuận là tìm mối quan hệ của các thơng số về vị
trí của điểm tác động cuối và hướng của khâu cuối cùng của robot (bàn kẹp). Vậy từ
công thức (3.4) trong trường hợp (i=n), với n là số hiệu của toạ độ gắn liền với điểm
tác động cuối E thì ta có:
Ti = H1.H2…Hn

(3.5)

Kết hợp với cơng thức (3.1) ta có:
nx
�
�
n
Tn    TE    H1.H 2 ...H n   �y
�
nz
�
�0

sx

sy
sz
0

ax
ay
az
0

px �
py �
�
pz �
�
1�

(3.6)

Phương trình (3.6) gọi là phương trình động học cơ bản của robot.

CHƯƠNG 4: GIẢI BÀI TOÁN ĐỘNG HỌC THUẬN CỦA ROBOT CÔNG NGHIỆP
4.1 Robot 2 khâu
22


X2
Y2
Y0

P(x,y,z)

Y1

a2

2

X1

a1

1
X0

Xác định bảng thông số Denavit – Hartenberg và xác định toạ độ điểm P. Từ hình vẽ
ta thiết lập bảng thông số Denavit – Hartenberg

Khâu

θi

di

ai

αi

1

q1 = θ1

0

a1

0

2

q2 = θ2

0

a2

0

Ma trận chuyển DH:
cos(q1 +q 2 )  sin (q1 +q 2 ) 0 a2 cos(q1 +q 2 )+a1cos( q1 ) �
�
�
sin (q1 +q 2 ) cos(q1 +q 2 ) 0 a2 sin (q1 +q 2 )+a1 sin( q1 ) �
�
�
DH 
� 0
�
0
1
0
�

�
0
0
1
� 0
�
Tọa độ bàn kẹp là:
Px = a2cos(q1+q2) + a1cos(q1)
Py = a2sin(q1+q2) + a1sin(q1
4.2 Robot 3 khâu

23


X3
Y3
Y1

P(x,y,z)
Y2

3

a3

X2

a2

2

d1

X1

z0

X'0

1
X0

Hình 4.2
Từ hình vẽ và bảng tham số động học Denavit – Hartenberg:
Trục
θi
di
ai
1
q1 = θ1
d1
0

αi
 /2

2
3

0

0

q2 = θ2
q3 = θ3

0
0

a2
a3

Ma trận DH là:

cos(q2 + q3)*cos(q1) -sin(q2 + q3)*cos(q1)
�
�
cos(q2 + q3)*sin(q1) -sin(q2 + q3)*sin(q1)
DH  �
�
0
0
�
0
0
�

0 cos(q1)*(a3*cos(q2 + q3) + a2*cos(q2)) �
0 sin(q1)*(a3*cos(q2 + q3) + a2*cos(q2)) �
�
0

a3*sin(q 2  q3)  a2*sin( q2)  d1 �
�
0
1
�

Toa do ban kep la:
Px  cos( q1 )(a3 cos(q2  q 3 )  a2 cos(q2 ))

Py  sin(q1 )(a3 cos(q2  q 3 )  a2 cos(q2 ))
Pz  a3 sin(q2  q 3 )  a2 sin(q2 )  d1 )

4.3 tay máy 4 bậc tự do
24


Bảng DH:
1
2
3
4

Rz,ϴ
ϴ1
ϴ2
ϴ3

Tz,d
d1
0

0
0

Tx,a
0
a2
a3
a4

Ma trận DH là =***=

sin(q1)*(a1 + a3*cos(q2 + q3) + a2*cos(q2) + a4*cos(q2 + q3 + q4))

25

Rx,α
90
0
0
0