Tính toán động học, động lực học tay máy nối tiếp theo phương pháp số

MỤC LỤC


2
DANH MỤC CÁC TỪ VIẾT TẮT
DH

Denavit Hartenberg

GUI

Graphical User Interface

GUIDE

Graphical User Interface Development Environment

CS

Coordinate system

CG

Center of gravity


3
DANH MỤC HÌNH ẢNH


4

MỞ ĐẦU
Trong sự nghiệp công nghiệp hóa và hiện đại hóa đất nước, khoa học và
công nghệ đóng vai trò hết sức quan trọng, đặc biệt là lĩnh vực Cơ điện tử và tự
động hóa các hệ thống sản xuất. Cùng với sự hội nhập, phát triển của khoa học
và công nghệ nước nhà, việc ứng dụng các loại robot nhằm mục đích nâng cao
năng suất, giải phóng sức lao động của con người ngày càng được quan tâm và
ứng dụng rộng rãi. Đi đôi với nó là nhu cầu đào tạo nguồn nhân lực chất lượng
cao, phục vụ cho công cuộc công nghiệp hóa và hiện đại hóa đất nước. Chính vì
thế, phần lớn các trường đại học hiện nay đã và đang nghiên cứu, mở rộng việc
đào tạo nhân lực ngành Cơ điện tử, để có thể khai thác, sử dụng, vận hành các hệ
thống robot và cao hơn nữa đó là nghiên cứu thiết kế các hệ thống robot made in
Việt Nam thay thế cho các hệ thống tự động nhập khẩu đắt tiền.
Từ thập niên 90 trở lại đây, các nghiên cứu ứng dụng robot công nghiệp
ngày càng nhiều, trong đó phải kể đến robot có cấu trúc nối tiếp. Để có thể xây
dựng bài toán điều khiển robot trước hết chúng ta cần phải giải bài toán động
học và động lực học của robot. Kết quả của các bài toán này là những tham số
đầu vào quan trọng cho tất cả các nghiên cứu tiếp theo về robot. Hiện tại, việc
giải lớp bài toán này phần lớn được thực hiện thủ công bằng tay đòi hỏi rất
nhiều kỹ năng tính toán, bên cạnh đó hiệu quả của việc tính toán theo phương
pháp này thấp, khả năng xảy ra sai sót rất lớn do khối lượng tính toán rất lớn,
các phép toán ma trận khá phức tạp, thường chỉ giải quyết được trong những
trường hợp đơn lẻ, cụ thể. Ở bộ môn Cơ học ứng dụng, Viện Cơ khí, Đại học
Bách khoa Hà Nội cũng đã xây dựng chương trình ROBOTDYN phục vụ cho
việc tự động hóa tính toán các bài toán trên. Tuy nhiên, đây là sản phẩm đặc thù
của họ, và việc tiếp cận khai thác phần mềm này là rất khó khăn, nếu không nói
là không thể đối với sinh viên.
Với mục đích tạo ra công cụ tự động hóa tính toán động học, động lực học,
và mô phỏng động học robot thuận tiện cho quá trình học tập, nghiên cứu của
sinh viên, cũng như quá trình giảng dạy của giáo viên, em chọn đề tài mang tên:

5
“Xây dựng chương trình tính toán động học, động lực học hệ tay máy hở tổng
quát”.
Do thời gian không cho phép, nên phạm vi của đồ án là các hệ tay máy hở
dưới 5 bậc tự do đã xác định trước cấu hình theo phương pháp DenavitHartenberg, và đồ án sẽ tập trung giải quyết các nội dung chủ yếu sau:
- Tìm hiểu phương pháp tính toán động học, động lực học tay máy phục vụ
cho việc xây dựng chương trình.
- Xây dựng chương trình tính toán động học, động lực học, mô phỏng động
học tay máy.
- Tìm hiểu phương pháp, cách thức mô hình hóa tay máy dựa vào gói công
cụ SimMechanics của Matlab và phần mềm đồ họa Solidworks.
Trong quá trình thực hiện đồ án này, do kiến thức về robot có hạn, thời gian
và tài liệu nghiên cứu còn ít nên đồ án của em vẫn còn nhiều thiếu sót, vì vậy rất
mong được sự góp ý của thầy cô và các bạn để đồ án được hoàn thiện hơn.
Em xin chân thành cảm ơn thầy Nguyễn Hoàng Long, thầy Nguyễn Hữu
Phúc và toàn thể các thầy cô trong bộ môn Robot đặc biệt & Cơ điện tử đã giúp
em hoàn thành đồ án này.

Hà Nội, ngày…tháng…năm 2012
Sinh viên thực hiện

CHƯƠNG 1: TỔNG QUAN VỀ TAY MÁY
1. Tay máy


6
Tay máy là phần cơ sở, quyết định khả năng làm việc của Robot công
nghiệp. Đó là cơ cấu cơ khí gồm các khâu, khớp, chúng hình thành cánh tay để

tạo ra các chuyển động cơ bản, cổ tay tạo nên sự khéo léo, linh hoạt và bàn tay
để trực tiếp hoàn thành các thao tác trên đối tượng.
2. Kết cấu của tay máy
Ý tưởng ban đầu của việc thiết kế và chế tạo tay máy là phỏng theo các cấu
tạo và chức năng của tay người.Về sau, đây không còn là điều kiện bắt buộc
nữa. Tay máy hiện nay rất đa dạng và nhiều loại với dáng vẻ khác rất xa do với
tay người. Tuy nhiên trong kỹ thuật robot người ta vẫn dùng các thuật ngữ quen
thuộc, như vai (Shoulder), cánh tay (Arm), cổ tay (Wrist), bàn tay (Hand) và các
khớp (Ariculation), … để chỉ tay máy và các bộ phận của nó.
Trong thiết kế và sử dụng tay, máy người ta thường quan tâm đến các thông
số có ảnh hưởng lớn đến khả năng làm việc của chúng, như:
- Sức nâng, độ cứng vững, lực kẹp của tay máy
- Tầm với hay vùng làm việc: kích thước và hình dáng vùng mà phần công tác có
thể vươn tới.
- Sự khéo léo, nghĩa là khả năng định vị và định hướng phần công tác trong vùng
làm việc. Thông số này liên quan đến số bậc tự do của phần công tác.
Các tay máy có đặc điểm chung về kết cấu là được cấu tạo bởi một số khâu
(Links), được nối với nhau nhờ các khớp (Joints), tạo thành chuỗi động học.
Một đầu của chuỗi động học được nối với giá (base), còn đầu kia nối với phần
công tác. Mỗi khâu hình thành cùng với khớp phía trước nó một cặp khâu–khớp.
Tùy theo kết cấu mà mỗi loại khớp đảm bảo cho khâu nối sau nó các khả năng
chuyển động nhất định.
Mỗi khớp được đặc trưng bởi 2 tham số:
- Các tham số không thay đổi trong quá trình làm việc của tay máy được
gọi là các tham số.
- Các tham số thay đổi khi tay máy làm việc, gọi là các biến khớp.
Hai loại khớp thông dụng trong kỹ thuật tay máy là khớp trượt và khớp
quay. Chúng đều là khớp có một bậc tự do.
3. Phân loại tay máy

7
Tuy theo số lượng và cách bố trí các khớp có thể tạo ra tay máy kiểu tọa độ
đề các, tọa độ trụ, tọa độ cầu, SCARA và kiểu tay người.
3.1. Tay máy kiểu tọa độ đề các:
Tay máy này còn được gọi là kiểu chữ nhật,
dùng 3 khớp trượt, cho phép phần công tác

thực hiện

một cách độc lập các chuyển động thẳng,
song song với 3 trục tọa độ. Vùng làm việc

của

tay máy có dạng hộp chữ nhật. Do sự đơn
giản về kết cấu, tay máy kiểu này có độ cứng
vững cao, độ chính xác được đảm bảo đồng
đều1.1:
trong
làmđềviệc,
Hình
Taytoàn
máy bộ
kiểuvùng
tọa độ
các
nhưng ít khéo léo.Vì vậy, tay máy kiểu đề các được dùng chủ yếu trong vận
chuyển và lắp ráp.
3.2.

Tay máy kiểu tọa độ trụ

Khác với tay máy kiểu đề các ở khớp đầu tiên : Dùng khớp quay
thay thế cho khớp tịnh tiến. Vùng làm việc của
nó có dạng trụ rỗng. Khớp trượt nằm ngang cho

phép

tay máy “thò” được vào khoảng rỗng nằm ngang.

Độ

cứng vững cơ học của tay máy trụ tốt, thích hợp

với tải

nặng, nhưng độ chính xác định vị và góc trong mặt

phẳng

nằm ngang giảm khi tầm với tăng.
3.3.

Tay máy kiểu tọa độ cầu

Hình 1.2:Tay máy kiểu tọa độ trụ


8

Khác kiểu trụ do khớp thứ 2 được thay thế bằng khớp quay. Nếu quỹ đạo
chuyển động của các phần công tác được mô
tả trong tọa độ cầu thì mỗi bậc tự do tương
ứng với khả năng chuyển động và vùng làm
việc của nó là khối cầu rỗng. Độ cứng vững
của loại tay máy này thấp hơn 2 loại tay máy
trên và độ chính xác định vị phụ thuộc vào
tầm với. Tuy nhiên, loại này có thể “ nhặt ” Hình 1.3: Tay máy kiểu tọa độ cầu
được cả vật dưới nền.
3.4.

SCARA

Đây là kiểu tay máy có cấu tạo đặc biệt, gồm 2 khớp quay và

1

khớp

tịnh tiến, nhưng cả 3 đều có trục song song vơi

nhau.

Kết cấu này làm tay máy cứng vững hơn theo

phương

ngang. Loại này chuyên dùng cho công nghệ lắp

ráp với

tải trọng nhỏ, theo phương thẳng đứng. Vùng làm

việc của

SCARA là một phần của hình trụ rỗng.
3.5. Tay máy kiểu tay người
Có cả 3 khớp là khớp quay, trong đó

Hình 1.4: Tay máy kiểu SCARA

trục thứ nhất vuông góc với 2 trục kia. Do
sự tương tác với tay người, khớp thứ 2 được
gọi là khớp vai, khớp thứ 3 gọi là khớp
khủy tay. Với kết cấu này, không có sự
tương ứng giữa khả năng chuyển động của
các khâu và số bậc tự do. Tay máy làm việc

Hình 1.5: Tay máy kiểu tay người
rất khéo léo, nhưng độ chính xác định vị phụ thuộc vị trí của phần công tác trong
vùng làm việc. Vùng làm việc của tay máy kiểu này gần giống một khối cầu.


9


10

CHƯƠNG 2: ĐỘNG HỌC VÀ ĐỘNG LỰC HỌC TAY MÁY
I.

ĐỘNG HỌC TAY MÁY
Động học tay máy nghiên cứu chuyển động các khâu của robot về phương

diện hình học, không quan tâm tới các lực và moment gây ra chuyển động .Động
học tay máy giải quyết 2 vấn đề:
- Bài toán động học thuận: Căn cứ vào các biến khớp để xác định vùng làm việc
của phần công tác và mô tả chuyển động của phần công tác trong vùng làm việc
của nó.
- Bài toán động học ngược: Xác định các biến khớp để đảm bảo chuyển động cho
trước của phần công tác.

Hình 2.1 Bài toán động học thuận, động học ngược
1. Bài toán động học thuận
Bài toán động học thuận nhằm mô tả thế (vị trí và hướng) của phần công
tác dưới dạng hàm số của biến khớp.
Giải quyết bài toán động học thuận của robot thực chất là chúng ta cho
trước cấu hình của robot và quy luật chuyển động của các khâu, từ đó chúng ta
xác định quy luật chuyển động của bàn kẹp (điểm tác động cuối): vị trí bàn kẹp,
vận tốc bàn kẹp, gia tốc chuyển động bàn kẹp, vận tốc góc, gia tốc góc v.v…
Để giải bài toán động học thuận ta có thể dùng phương pháp hình học giải
tích. Phương pháp này thường chỉ áp dụng cho các cơ cấu đơn giản. Để giải các
bài toán tổng quát ta cần một phương pháp giải chung. Có 2 phương pháp chính


11
được sử dụng phổ biến: Phương pháp Denavit-Hartenberg và phương pháp
Craig. Trong nội dung đồ án xin trình bày phương pháp Denavit-Hartenberg.
1.1.

Phép biến đổi thuần nhất và ma trận biến đổi thuần nhất

Cho 2 hệ quy chiếu R0 = {Ox0 y0 z0 } và R1 = {Ox1 y1 z1} như hình vẽ. Tọa độ
r
rp
điểm P trong hệ quy chiếu R0 là , trong hệ quy chiếu R1
là

r
uP .
Khi đó ta có:
r r r
rP = rA + u P

(1.1)

Trong hệ quy chiếu R0 biểu thức (2.1) có
r
r
r
rP(0) = rA(0) + u P(0)
(1.2)

dạng:

Hình 2.2
Gọi A là ma trận cosin chỉ hướng của hệ quy chiếu R1 đối với hệ quy chiếu R0.

Khi đó ta có hệ thức:

 a11 a12
A =  a21 a22
 a31 a32

a13 
a23 
a33 

(1)
u (0)
P = Au P

(1.3)

Thế (2.3) vào (2.2) ta được:
rP(0) = rA(0) + Au(1)
P
(1)
a13  uPx 
 (1) 
a23  u Py



a33  u (1) 
 Pz 
Nếu sử dụng khái niệm tọa độ thuần nhất ta có:

 xP(0)   x (0)
  a11 a12

A
 (0)   (0)  
 yP  =  y A  +  a21 a22
 z (0)   z (0)   a
a
 P   A   31 32
 xP(0)   a11 a12
 (0)  
 yP  =  a21 a22
 z (0)   a
a
 P   31 32
0
 1   0

Hay:

a13
a23
a33
0

(1)
x A(0)  u Px 
  (1) 
u
y (0)
A   Py 
 u (1) 
z (0)

A
  Pz 
1   1 

(1.4)

(1.5)

(1.6)


12
rP(0)   A
 = T
 1  0


rA(0)  u (1)
P
 
1  1 

(1.7)

Nếu ta đưa vào ma trận:
 a11 a12

a
a
T =  21 22

 a31 a32

0
0

a13
a23
a33
0


x (0)
A
(0) 
yA   A
=
 0T
z (0)
A

1 

rP(0) 

1 

(1.8)

Thì phương trình (2.7) có thể viết dưới dạng:
rP(0) = Tu (1)

P

(1.9)

1.2. Các ma trận quay cơ bản thuần nhất và ma trận tinh tiến thuần nhất
Các ma trận quay cơ bản trong không gian thuần nhất:
0
0
1
0 cos(ϕ ) − sin(ϕ )
TRx (ϕ ) = Rot ( x, ϕ ) = 
0 sin(ϕ ) cos(ϕ )

0
0
0

 cos(ψ )
 0
TRy (ψ ) = Rot ( y,ψ ) = 
 − sin(ψ )

 0

0
0 
0

1
(1.10)

0 sin(ψ )

0
1
0
0

0 cos(ψ ) 0 

0
0
1
(1.11)

cos(θ ) − sin(θ )
 sin(θ ) cos(θ )
TRz (θ ) = Rot ( z,θ ) = 
 0
0

0
 0

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

(1.12)

b)

c)
Hình 2.3: a,b,c

Các ma trận tịnh tiến

1
0
TTx (a ) = Trans ( x, a ) = 
0

0

a)

0
1
0
0

0
0
1
0

a
0


0

1
(1.13)

a)


13
1
0
TTy (b) = Trans ( y , b) = 
0

0

0
1

0
0

0
b

0

1
(1.14)

0

1

0

0

1
0
TTz (c ) = Trans ( x, c ) = 
0

0

0 0 0
1 0 0

0 1 c

0 0 1
(1.15)

b)

c)
Hình 2.4 a,b,c

Nếu tịnh tiến đồng thời trên các trục x, y ,z ta có ma trận thuần nhất:

1
0
TT (a, b, c) = Trans (a, b, c ) = 
0

0

0 0 a
1 0 b

0 1 c

0 0 1

(1.16)

1.3. Phương pháp Denavit-Hartenberg
1.3.1. Cách xác định hệ trục tọa độ
Xét các vật rắn nối tiếp nhau bằng các khớp quay và khớp tịnh tiến. Khi đó
quan hệ vị trí giữa các khâu kế tiếp có thể được xác định bởi 2 tham số khớp

Hình 2.5: Hình biểu diễn khớp và khâu
Trong hình trên, khâu i-1 và khâu i được nối với nhau bằng khớp i. Trục zi-1
được chọn là trục của khớp thứ i. Tham số thứ nhất θi , là góc quay trục xi-1
'
'
quanh trục zi-1 đến trục xi / / xi . Tham số thứ 2 là d i, là khoản 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à
khớp tịnh tiến thì khoảng cách di là biến, còn θi là hằng số.

Cách chọn các hệ trục tọa độ có gốc tại khớp thứ i:


14
1) Trục zi −1 được chọn dọc theo hướng của trục khớp động thứ i
2) Trục
zi − 2

xi −1 được chọn dọc theo đường vuông góc chung của 2 trục

zi −1

và

, hướng từ trục zi −2 đến trục zi −1 . Nếu trục zi −1 cắt trục zi −2 thì hướng của

trục xi −1 được chọn tùy ý miễn là vuông góc với trục zi −1 . Khi đó 2 trục
zi − 2

và zi −1 song song với nhau, giữa 2 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
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
5) Đối với hệ tọa độ (Oxyz) 0 theo quy ước trên ta chỉ chọn được trục z 0,
còn trục xo chưa có trong quy ước trên. Ta có thể chọn trục x 0 tùy ý,
miến là x0 và z0 vuông góc với nhau.
6) Đối với hệ trục (Oxyz) n, do không có khớp n+1, nên theo quy ước trên
ta không thể 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 không được chọn theo pháp tuyến của trục zn-1.
Trường hợp này, nếu khớp n là khớp quay, ta có thể chọn trục zn song
song với trục zn-1. Ngoài ra ta có thể chọn tùy ý sao cho hợp lý.
1.3.2. Các tham số động học Denavit-Hartenberg.

Hình 2.6: Sơ đồ thiết lập hệ tọa độ các khâu
Vị trí của hệ tọa dộ khớp (Oxyz)i , đối với hệ tọa độ khớp (Oxyz)i-1được
xác định bởi bốn tham số Denavit-Hartenberg:

θi , di , ai , α i như sau:

'
'
z
- θi : góc quay quanh trục i −1 để trục xi −1 chuyển đến trục xi ( xi / / xi )


15
-

di

: dịch chuyển tịnh tiến dọc theo trục zi −1 để gốc tọa độ Oi-1 chuyển đến

'
điểm Oi , giao điểm của trục xi và 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 ( zi' −1 / / zi ) chuyển đến trục zi

Do hệ trục tọa độ (Oxyz)i-1 gắn liền vào khâu thứ i-1, còn hệ tọa độ (Oxyz) i
gắn vào khâu thứ i, cho nên vị trí của khâu thứ i đối với khâu thứ i-1 được xác
định bởi 4 tham số Denavit-Hartenberg.
1.3.3. Ma trận Denavit-Hartenberg.
Ta có thể chuyển hệ tọa độ khớp (Oxyz) i-1 sang hệ tọa độ khớp (Oxyz) i bằng
bốn phép biến đổi cơ bản như sau:
- Quay quanh trục zi-1 một góc θi .
- Dịch chuyển tịnh tiến dọc trục zi-1 một đoạn di.
- Dịch chuyển tịnh tiến dọc trục xi một đoạn ai.
- Quay quanh trục xi một góc α i

Ma trận của phép biến đổi, ký hiệu H i là tích của bốn ma trận biến đổi cơ bản
và có dạng như sau:
Quay hệ tọa độ xung quanh trục zi −1 một góc θ1 ta được ma trận biến đổi:
cos(θi ) sin(θi )
 sin(θ ) cos(θ )
i
i
TRz = Rot ( z ,θi ) = 
 0
0

0
 0

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

(1.17)

Tịnh tiến theo trục zi một đoạn di ta được ma trận biến đổi:
1
0
TTz = Trans( z , di ) = 
0

0

0
1
0
0

0
0
1
0

0
0 
di 


1

Tịnh tiến theo trục xi một đoạn ai ta được ma trận biến đổi:

(1.18)


16
1
0
TTx = Trans( x, di ) = 
0

0

0
1
0
0

0
0
1
0

ai 
0 
0


1

(1.19)

Và quay quanh trục xi một góc α i ta được ma trận biến đổi
0
0
1
0 cos(α ) − sin(α )
i
i
TRx = Rot ( x, α i ) = 
0 sin(α i ) cos(α i )

0
0
0

0
0 
0

1

(1.20)

Ma trận biến đổi hệ tọa độ (Oxyz)i-1 thành hệ tọa độ (Oxyz)i được xác định
bằng tích của 4 ma trận biến đổi trên:
H i = TRz (θ i )TTz (d i )TTx (ai )TRx (α i )
cos θi

 sin θ
i
=
 0

 0

− sin θi cos α i
cos θi cos α i
sin θi
0

sin θ i sin α
− cos θi sin α i
cos α i
0

ai cos θ i 
ai sin θ i 
di 

1 

(1.21)

1.3.4. Phương trình xác định vị trí và hướng của khâu thao tác
Với ma trận trên ta có thể xác định được:
- Vị trí của gốc tọa độ của hệ tọa độ (Oxyz)i trong hệ tọa độ (Oxyz )i −1 trong
- Hướng của vật rắn gắn với hệ tọa độ (Oxyz )i trong hệ tọa tọa độ (Oxyz )i −1 trong.
- Bằng cách chuyển dần hệ quy chiếu (Oxyz ) n từ On về On−1 , On−2 ,... và cuối cùng

là hệ quy chiếu cố định (Oxyz )0 ta sẽ xác định được vị trí của gốc tọa độ On và
hướng của khâu thứ n trong hệ quy chiếu cố định

Hình 2.7: Tay máy n khâu
Áp dụng liên tiếp phép biến đổi đối với robot n khâu ta có :


17
Dn = H1H 2 ...H n
A
Dn =  n
0

rE(0) 

1 

Ma trận Dn cho biết vị trí của điểm định vị và hướng của khâu thao tác của robot
đối với hệ quy chiếu cố định.
2. Bài toán động học ngược
Bài toán động học thuận cho phép xác định thế của phần công tác, và có thể
cả vùng làm việc của nó theo quan hệ với các thông số động học của các cặp
khâu- khớp. Bài toán động học ngược nhằm xác định bộ thông số động học để
đảm bảo chuyển động cho trước của phần công tác. Các bài toán ngược thường
có các đặc điểm:
- Các phương trình có dạng phi tuyến và siêu việt, thường không cho lời
giải đúng.
- Có thể có nhiều lời giải.
- Có thể gặp nghiệm vô định, vì các liên kết thừa (giống như liên kết siêu
tĩnh).

- Có thể có nghiệm tìm được bằng toán học, nhưng lại không chấp nhận
được về mặt vật lý, do các ràng buộc về kết cấu.
Tính đa nghiệm của bài toán động học ngược không chỉ phụ thuộc vào các
biến khớp (số bậc tự do) mà cả vào số lượng các tham số khác không trong kết
cấu.
Việc tìm các nghiệm phù hợp đòi hỏi ở người thiết kế một trực giác về mặt
toán học và về kết cấu để dự đoán những đặc điểm hoặc khu vực khả dĩ giảm
được số nghiệm cần lựa chọn.
2.1.

Không gian thao tác và không gian cấu hình của Robot.

Xét mô hình robot như hình 2.13. Vị trí khâu thao tác được xác định bởi sáu
T
tham số px , p y , pz , ϕ ,ψ , θ . Trong đó p = [px , p y , pz ] xác định vị trí của điểm thao tác

T
E, còn α = [ϕ ψ θ ] xác định hướng của khâu thao tác. Trong số sáu tọa độ trên,


18
trong nhiều trường hợp chỉ quan tâm đến một
số tọa độ trong số đó, ký hiệu các tọa độ cần
quan tâm là x1 , x2 ,..., xm ( m ≤ 6) :
x = [ x1 , x2 ,..., xm ]

T

Các tọa độ x1 , x2 ,..., xm được gọi là các tọa độ
thao tác. Tập hợp các tọa độ thao tác tạo thành không

gian Tay
Euclide
chiều và
Hình 2.8:
máy 4mkhâu
được gọi là không gian thao tác của Robot.
¡

x

= {x| x i =f i (q),q ∈ ¡ q }

Ta sử dụng các tọa độ suy rộng q1 , q2 ,..., qn để xác định vị trí các khâu của robot.
Thông thường chúng là tọa độ các khớp. Như thế ta có:
q = [ q1 q2 ... qn ]

Các tọa độ

q1 , q2 ,..., qn

T

được gọi là các tọa độ khớp. Tập hợp các tọa độ

q1 , q2 ,..., qn tạo thành không gian Euclide n chiều gọi là không gian cấu hình của
Robot.
¡

q

= {q| q i min
2.2. Bài toán động học ngược
Khi giải bài toán động học thuận robot ta xác định được quan hệ:

x = f(q)

(1.22)

Từ phương trình trên ta suy ra một cách hình thức:

q = f -1 (x)
x = [ x1 , x2 ,..., xm ] , q = [ q1 , q2 ,..., qn ]
T

Trong đó:

(1.23)
T

Khi m = n ta quy ước gọi là robot cấu trúc không dư hoặc robot chuẩn. Nếu
m < n gọi là robot có cấu trúc dư hoặc robot dư dẫn động.
2.3.

Các phương pháp giải bài toán ngược
Các phương pháp giải bài toán động học ngược được phân loại thành 2

nhóm: các phương pháp giải tích và các phương pháp số.

19
• Phương pháp giải tích cho phép tìm ra kết quả q là biểu thức giải tích đối với x.
Các phương pháp này cho kết quả chính xác và nhanh chóng nhưng quá trình
tính toán, thành lập phương trình giải tích phức tạp và không có cách giải tổng
quát cho mọi robot.
• Phương pháp số là phương pháp tính gần đúng với sai số cho phép được sử
dụng với sự hỗ trợ của máy tính. Phương pháp này cho ta cách giải tổng
quát cho mọi robot, cho kết quả chính xác cần thiết nhưng đưa ra kết quả chậm.
2.3.1. Phương pháp giải tích
Với phương pháp này, khi giải bài toán động học thuận bằng phương pháp ma
trận Denavit-Hartenberg ta có ma trận biến đổi xác định vị trí của khâu thao tác
là:

 A n (q) rE(0) (q) 
Tn (q ) = Dn (q) = H1H 2 ...H n−1H n = 

1 
 0

(1.24)

Từ đó xác định được ma trận côsin chỉ hướng của khâu thao tác và véc tơ định
vị điểm thao tác E là các hàm của các tọa độ suy rộng. Nếu sử dụng phương
pháp ma trận Craig, ta có:
(0)
 A (q) rOn
(q) 
Tn (q ) = Cn (q) = K 1K 2 ...K n −1K n =  n

1 

 1

(1.25)

Từ đó ta xác định được ma trận côsin chỉ hướng của khâu thao tác. Véc tơ xác
định vị trí của điểm thao tác E có dạng:
(0)
rE(0) = rOn
+ A nu (0)
E

(1.26)

Mặt khác từ nhiệm vụ công nghệ của robot ta có ma trận cấu hình của khâu
thao tác (ma trận côsin chi hướng của khâu thao tác đó và véc tơ xác định vị trí
của điểm thao tác) dưới dạng hàm của các tọa độ thao tác:
 a11 ( x ) a12 ( x) a13 ( x)
 a ( x) a ( x) a x
22
23 ( )
Tn ( x) =  21
 a31 ( x ) a32 ( x) a33 ( x)

0
0
 0

a14 ( x) 
a24 ( x ) 
a34 ( x) 


1 

(1.27)


20
Từ đó ta có phương trình ma trận:

Tn (q) = Tn ( x)

(1.28)

Từ phương trình trên, sử dụng các phương pháp đại số và hình học ta có thể tìm
ra các hàm xác định các tọa độ khớp:

q = f -1 (x)

(1.29)

2.3.2. Phương pháp số
2.3.2.1. Xác định véc tơ vận tốc và véc tơ gia tốc suy rộng
Giả sử ta đã xác định được quan hệ giữa các tọa độ thao tác và các tọa độ
m
khớp: x = f(q), x ∈ ¡ , q ∈ ¡

m

(1.20)

Khi m < n robot có số bậc tự do n, lớn hơn số tọa độ xác định vị trí của khâu
thao tác. Khi đó robot được gọi là robot dư dẫn động. Đạo hàm 2 vế của biểu
thức (2.9) theo thời gian ta được:

x&=

∂f
q&= J (q)q&
∂q

(1.21)

Trong đó:
∂f1 
 ∂f1 ∂f1
 ∂q ∂q .... ∂q 
1
2
n

∂f 
J (q) =
= .............................
∂q 

∂
f
∂
f
∂

f
m
m
m


....
 ∂q1 ∂q2
∂qn  m×n

(1.22)

J(q) cỡ mxn được gọi là ma trận Jacobi. Giả sử hạng của ma trận J(q) là m.
Hệ phương trình (2.10) là hệ tuyến tính dư ẩn số với q&.Với một giá trị vận
tốc của khâu thao tác và một cấu hình của tay máy sẽ có vô số nghiệm là vận tốc
của vector suy rộng. Để giải hệ phương trình này cần sử dụng lý thuyết ma trận
nghịch đảo suy rộng
Định nghĩa ma trận nghịch đảo suy rộng:
m×n
Ma trận nghịch đảo suy rộng của ma trận A ∈ R là ma trận cỡ n x m kí hiệu

+
là A và được xác định theo biểu thức:


21
A + = A T (A.A T )-1

(1.23)

Ma trận nghịch đảo suy rộng có tính chất tương tự ma trận nghịch đảo trong
phép nhân ma trận. Khi m=n ta có A+ = A-1.
Như vậy ma trận nghịch đảo suy rộng là ma trận nghịch đảo tổng quát khi ma
trận không phải là ma trận vuông.
Áp dụng lý thuyết ma trận nghịch đảo suy rộng cho phương trình (1.21) ta
được :

q&(t ) = J + (q) x&(t )

(1.24)

Tiếp tục đạo hàm 2 vế của biểu thức (2.10) ta được :

&
&
x&
(t ) = J (q)q&
(t ) + J&(q)q&(t )

(1.25)

Từ đó suy ra phương trình xác định véc tơ gia tốc suy rộng ;

(

& )J + (q) x&(t )
&
q&
(t ) = J + (q)( &
x&

(t ) − J&(q)q&(t )) = J + (q) &
x&
(t ) − J(q

)

(1.26)

Như vậy ta có công thức xác định véc tơ vận tốc và gia tốc trong hệ tọa độ suy
rộng :

q&(t ) = J + (q).x&(t )

(

&(t ) = J + (q) &
q&
x&
(t ) − J&(q)J + (q) x&(t )

(1.27)

)

(1.28)

Trong trường hợp robot không dư dẫn động, trong 2 biểu thức trên ta thay ma
-1
+
trận tựa nghịch đảo J (q) bằng J (q) .

2.3.2.2.

Xác định véc tơ tọa độ suy rộng

Từ các công thức (12) và (13) cho phép ta xác định được véc tơ suy rộng và
véc tơ gia tốc suy rộng, nếu biết được q(t) tại thời điểm khảo sát và các quy luật

x(t ), x&(t ), &
x&
(t ) .
Đế xác định q(t) ta giả sử robot làm việc trong khoảng thời gian t=0 đến t=T.
Chia khoảng thời gian đó thành N khoảng bằng nhau :


22
∆t =

T
N

Như thế ta có :
tk +1 = tk + ∆t ( k = 0,1,..., N − 1)

Áp dụng khai triển Taylor hàm véc tơ q(t) ở lân cận giá trị t = tk ta được :

1
&
&(tk )(∆t ) 2 + ...
q(tk + ∆t ) = q(tk ) + q&(tk )∆t + q

2

(1.29)

Thế biểu thức (1.27) vào (1.29) và bỏ qua các vô cùng bé bậc cao ta được :

q(tk +1 ) ≈ q(tk ) + J + (q(tk )) x&(tk ) ∆t
Từ các công thức trên ta có sơ đồ thuật toán :

k = 0,1,..., N

(1.30)


23
Hình 2.9: Sơ đồ thuật toán xác định véc tơ tọa độ suy rộng
Kết quả thu được khá thô, vì vậy cần nâng cao độ chính xác của q(t) tại thời
điểm t = tk+1. Khi đó ta sử dụng thuật toán hiệu chỉnh gia lượng véc tơ tọa độ
suy rộng .
Phương pháp hiệu chỉnh gia lượng
k+1
Giả sử ta đã biết qk = q(t k ) . Trước tiên ta xác định giá trị gần đúng q%

q%k +1 = q k + J + (q k )x&k ∆t
Sau đó ta xác định véc tơ chính xác hơn của

(1.31)

qk +1 theo công thức :

q k +1 = q%k +1 + ∆q k +1

Ta cần xác định

∆q k +1 : Ta có

x k +1 = f (q k +1 ) = f (q%k +1 + ∆q k +1 ) = f (q%k +1 ) +

δf
(q%k +1 ) ∆q k +1 + ...
δq

Suy ra công thức gần đúng
J (q%k +1 )∆q k +1 ≈ x k +1 − f (q%k +1 )
J(q%k +1 )

(1.32)

(1.33)
(1.34)

là ma trận cỡ m x n, Δqk +1 là véc tơ có n phần tử. Do đó trong phương

trình trên, số ẩn lớn hơn số phương trình.
∆q k +1 = J + (q%k +1 )[x k +1 -f (q%k +1 )]

(1.35)

Sau đó ta lấy :
q%k +1 = q%k +1 + ∆q k +1

Nếu

∆q k +1 ≥ ε

thì thay (1.36) vào (1.35) và tiếp tục giải hệ phương trình.

Nếu

∆q k +1 < ε

thì dừng lại và lấy :

Biết được qk+1 ta suy ra được
Đánh giá sai số

q k +1 = q%k +1

&
&k +1
q&k +1 , q

(1.36)


24
Từ các yêu cầu công nghệ ta có x = x(t ) . Mặt khác từ bài toán động học thuận ta
có xk = f(q k ) . Do đó để đánh giá sai số của phương pháp ta đưa vào các công
thức xác định sai số của dich chuyển, vận tốc, gia tốc
e(tk ) = x(tk ) − x k = x(tk ) − f (q k )

e&(tk ) = x&(tk ) − x&k = x&(tk ) − J (q k )q&k
&
&
&k
e&(tk ) = &
x&(tk ) − &
x&k = &
x&(tk ) − J&(q k )q&k − J (q k )q

Độ lớn các chuẩn của véc tơ

e k , e&k (tk ), &
e&k (tk )

(1.37)

cho biết độ chính xác của

phương pháp số mà chúng ta sử dụng.
Sơ đồ thuật toán giải bài toán động học ngược theo phương pháp số có hiệu
chỉnh gia lượng véc tơ tọa độ suy rộng:


25

Hình 2.10: Sơ đồ thuật toán giải bài toán động học ngược