1.5 Cơ sở lý thuyết khảo sát động học robot
1.5.2 Thiết lập hệ phương trình động học của robot cấu trúc liên tục
Để làm cơ sở cho việc khảo sát động học robot tác hợp MRM trong chương sau, ở đây dẫn ra việc thiết lập phương trình động học cơ bản cho mô
--- 28
a) Cách xây dựng hệ trục tọa độ
Áp dụng phương pháp ma trận Denavit-Hartenberg, trên hình 1.13 dẫn ra các hệ tọa độ gắn với các khâu của robot cùng với các tọa độ suy rộng tương ứng với góc quay tại các khớp. Khi biểu diễn chuyển động quay và tịnh tiến giữa các khâu với nhau, Denavit Hartenberg đưa ra phương pháp xây - dựng liên tiếp các hệ tọa độ gắn với các khâu của mạch động học. Ý nghĩa của phép biểu diễn này là xây dựng được các ma trận biến đổi tọa độ thuần nhất cỡ 4x4 biểu diễn vị trí và hướng của hệ tọa độ gắn vào một khâu đối với hệ tọa độ gắn với khâu trước đó. Điều này cho thấy có thể biểu diễn liên tiếp các tọa độ và hướng của bàn kẹp từ hệ tọa độ gắn vào khâu cuối với hệ tọa độ gắn vào giá cố định. Các hệ tọa độ được xây dựng là hệ tọa độ Đề Các vuông góc, mà hệ tọa độ (xi, yi, zi) gắn với khâu thứ (i) tại khớp động thứ (i+1) . Để xây dựng các hệ tọa độ gắn với các khâu, Theo Denavit-Hartenberg:
1) Bắt đầu từ khâu đế, đánh số các khâu và các khớp theo thứ tự. Đế được đánh số là khâu 0, khâu cuối cùng là đầu tác động. Trừ đế và khâu cuối cùng, mỗi khâu đều có hai khớp. Khớp i nối khâu i với khâu i-1
2) Vẽ các đường vuông góc chung giữa từng cặp trục khớp liên tiếp. Trừ trục khớp thứ nhất và cuối cùng, mỗi trục khớp đều có hai đường vuông góc chung, một với trục khớp (i 1) và một với trục khớp (i+1).-
3) Thiết lập hệ tọa độ cơ sở gắn vào giá cố định sao cho trục zo thẳng hàng với trục khớp thứ nhất, trục xo vuông góc với trục zo và trục yo tạo với xo và zo
hệ qui chiếu thuận.
4) Thiết lập hệ tọa độ đầu tác động n sao cho trục xn vuông góc trục khớp cuối cùng, trục zn thường được chọn theo chiều tiếp cận của đầu tác động.
5) Đặt hệ tọa độ ĐềCác cho tất cả các khâu như sau:
-Trục zi-1 chọn dọc theo hướng của trục khớp động thứ i.
-Trục xi-1 được chọn dọc theo đường vuông góc chung của hai trục zi-2 và zi-1, hướng đi từ trục zi-2 sang trục zi-1. Nếu trục zi-2 cắt trục zi-1thì hướng của trục xi-1được chọn tùy ý.
--- 29
- Gốc tọa độ Oi-1 được chọn tại giao điểm của trục xi-1 và zi-1. - Trục yi-1được chọn sao cho hệ (Oxyz)i-1là hệ qui chiếu thuận.
Ngoài ra trục xo và trục zn được chọn tùy ý (thường được chọn như cách đã trình bày ở trên).
Cuối cùng xác định các thông số các khâu và các biến khớp.
Như vậy, với robot gồm sáu khâu động cấu trúc liên tục, trên hình 1.3 chỉ ra các hệ tọa độ (x1, y1, z1), (x2, y2, z2),… (x6, y6, z6).
Hình 1.13 Xây dựng h t a các âu ệ ọ độ kh b) Các tham số động học Denavit – Hartenberg
Khi các khâu được dẫn động bởi các động cơ đặt tại các khớp, khâu thứ (i) sẽ chuyển động tương đối với khâu thứ (i 1) và hệ tọa độ gắn với khâu - trước đó. Với cách chọn các hệ tọa độ như trên hình 1.13, quan hệ về vị trí và hướng giữa hai hệ tọa độ liên tiếp được xác định bởi bốn tham số sau:
di : là khoảng cách khớp, là dịch chuyển tịnh tiến dọc theo trục zi-1 để gốc
--- 30
ai : độ dài khâu, là dịch chuyển tịnh tiến dọc theo trục xi để điểm O’i
chuyển đến điểm Oi.
θi : góc khớp, là góc quay quanh trục zi-1 để trục xi-1 chuyển đến trục xi’
(xi’//xi)
α i : góc xoắn của khâu, là góc quay quanh trục xi sao cho trục z’i-1 chuyển đến trục zi.
Với Oilà giao điểm của trục xivà trục zi., Oi’là giao điểm của trục xivà trục zi- 1.
Đối với khớp quay, ai, di, α i là hằng, θi là biến, đo vị trí tương đôí của khâu i so với khâu i 1. Đốivới khớp lăng trụ - ai, θi, α ilà hằng, dilà biến, đo vị trí tương đối của khâu i so với khâu i-1.
Với mô hình robot như hình 1.13 ta xác định tham số cho các phép biến đổi liên tiếp các hệ tọa độ và lập bảng, ta có “Bảng tham số động học Denavit- Hartenberg”
Khớp θ d a α
1 θ1 d1 -a1 π /2
2 θ2 0 a2 0
3 θ3 0 a3 -π /2
4 θ4 d4 0 π /2
5 θ5 0 0 -π /2
6 θ6 d6 0 0
Bảng 1.3 Bảng tham số động học D-H c) Ma trận Denavit – Hartenberg
Ta có thể chuyển tọa độ khâu (Oxyz)i−1 sang hệ tọa độ khâu (Oxyz)i bằng bốn phép biến đổi cơ bản sau:
- Quay quanh trục zi-1 một góc θi
--- 31
- Dịch chuyển tịnh tiến dọc trục zi-1một đoạn di - Dịch chuyển tịnh tiến dọc trục ximột đoạn ai
- Quay quanh trục xi một góc αi
Mỗi phép biến đổi cơ bản trên được xác định bằng một ma trận quay hoặc tịnh tiến cơ bản dạng (1.1) gọi là các ma trận quay cơ bản thuần nhất hoặc ma trận tịnh tiến thuần nhất. Kết quả phép chuyển tọa độ khâu (Oxyz)i−1
sang hệ tọa độ khâu (Oxyz)i được biểu diễn bởi ma trận i−1Ai , là tích của bốn ma trận biến đổi cơ bản và có dạng như sau:
i−1A Rot zi = ( , ).θi Transz d Transx a Rot x( ).i ( ).i ( , )αi (1.3) Thay các ma trận biến đổi thuần nhất cơ bản ở trên vào công thức (1.3) ta
được
1
cos - sin cos sin sin co sin cos cos - cos sin sin 0 sin cos 0
i i i i i i i
i i i i i i i
i i
i i i
a A a
d
θ θ α θ α θ
θ θ α θ α θ
α α
− =
0 0 1
(1.4)
Ma trận i−1Ai được xác định bởi công thức (1.4) được gọi là ma trận Denavit-Hartenberg. Đó là ma trận chuyển tọa hệ qui chiếu (Oxyz)i−1 về hệ qui chiếu (Oxyz)i. Với phép mô tả dịch chuyển của các khâu từ khâu 1 đối với giá cố định, khâu 2 đối với khâu 1,… cuối cùng là khâu thao tác đối với khâu trước đó, vị trí và hướng của khâu cuối trong hệ tọa độ cố định được xác định bởi tích các ma trận dạng (1. ). Áp dụng liên tiếp các phép biến đổi trên đối 4 với robot n khâu ta được ma trận xác định vị trí của khâu thao tác (bàn kẹp) của robot
oAn = oA A1. ....1 2 n−1An (1.5) Theo sơ đồ động học của robot, vị trí và hướng của điểm tác động cuối
của robot đã đuợc xác định bởi ma trận D-H oAn như trên. Từ các ma trận oAn ta nhận thấy sự thay đổi vị trí và hướng của robot ở mỗi thời điểm được xác
--- 32
robot. Thường đó là các tham số biểu diễn sự dịch chuyển của các khâu, được thực hiện bởi các dẫn động đặt tại các khớp, gọi là các tọa độ điều khiển, ký hiệu là qivới i=1,n. Khi đó
( 1, )
i q ii n
θ = = .
Gọi hệ tọa độ gắn vào khâu cuối là x y zp p p. Ta có ma trận oAn được viết dưới dạng
1 1
1 2
( ) ( )
. ....
1
o o
p p
o o n
n n
T
C q R q
A A A A
O
−
= =
(1.6)
Ở đây
11 12 13
21 22 23
31 32 33
( ) ( ) ( )
( ) ( ) ( ) ( )
( ) ( ) ( )
o p
c q c q c q C q c q c q c q c q c q c q
=
(1.7)
Là ma trận côsin chỉ hướng của hệ tọa độ khâu cuối x y zp p p đối với hệ tọa độ cơ sở cố định x y zo o o và được xác định từ sơ đồ động học, do vậy các phần tử của nó là hàm của các tọa độ q ii( 1, )= n .
Và: oR qp( ) [ ( ) ( ) ( )]= x q y q z qp p p T (1.8) Là véctơ xác định vị trí gốc tọa độ của hệ tọa độ x y zp p p so với hệ x y zo o o. Các phần tử của nó cũng là hàm của các tọa độ q ii( 1, )= n .
d) Xác định ma trận trạng thái của khâu cuối
Xuất phát từ yêu cầu công nghệ, khâu cuối (khâu thao tác) của robot phải được dịch chuyển trong không gian theo một quỹ đạo xác định trước, còn gọi là quỹ đạo chương trình. Khi đó, vị trí và hướng của khâu tác động cuối chính là vị trí và hướng của hệ tọa độ x y zp p p, được xác định phụ thuộc vào dạng quỹ đạo chương trình, được biểu diễn bởi ma trận oAfi. Chỉ số fi chỉ ra rằng ma trận này được xác định tại mỗi điểm thuộc quỹ đạo chương trình i f.
--- 33
Sử dụng 3 góc quay Cardan với 3 phép quay cơ bản quanh trục x, y, z lần lượt 3 góc φ, ψ, θ và tịnh tiến theo 3 trục tọa độ x, y, z khoảng cách lần lượt là xp, yp,zpta xác định được oAfi như sau:
oAfi =Rot x( , ).ϕ Rot y( , ).ψ Rot z( , ).θ Trans x Trans y Trans zx( ).p y( ).p z( )p
( ). ( ) ( ). ( ) ( )
( ). ( ). ( ) ( ). ( ) ( ). ( ). ( ) ( ). ( ) ( ). ( ) ( ). ( ). ( ) ( ). ( ) ( ). ( ). ( ) ( ). ( ) ( ). ( )
0 0 0 1
p p o fi
p
c c c s s x
s s c c s s s s c c s c y
A c s c s s c s s s c c c z
ψ θ ψ θ ψ
ϕ ψ θ ϕ θ ϕ ψ θ ϕ θ ϕ ψ
ϕ ψ θ ϕ θ ϕ ψ θ ϕ θ ϕ ψ
−
+ − + −
=− + + −
(1.9)
Với c = cos; s = sin.
rotx = φ, roty= ψ, rotz = θ.
Ta viết lại (1. ) như sau:9
( ) ( )
1
o o
p p
o fi
T
C t R t
A
O
=
(1.10)
Ở đây
11 12 13
21 22 23
31 32 33
( ) ( ) ( )
( ) ( ) ( ) ( )
( ) ( ) ( )
o p
c t c t c t
C t c t c t c t
c t c t c t
=
(1.11)
Các phần tử của nó được xác định từ quỹ đạo chương trình, là hàm của thời gian (t).
Và: oR tp( ) [ ( ) ( ) ( )]= x t y t z tp p p T (1.12) Với các phần tử là hàm của thời gian (t).
e) Thiết lập hệ phương trình động học của robot
Ta có ma trận oAn mô tả bởi hệ thức (1. ) và ma trận 6 oAfi mô tả bởi hệ thức (1.10) đều xác định vị trí và hướng của khâu tác động cuối trong hệ tọa độ cơ sở. Do vậy ta có:
oAn = oAfi (1.13)
--- 34
Từ (1.1 ) ta nhận được:3
( ) ( )
( ) ( )
o o
p p
o o
p p
R q R t
C q◊ C t◊
=
=
(1.14)
Ở đây hệ phương trình đầu xác định bởi ba phương trình chỉ sự ràng buộc về vị trí của điểm tác động cuối x q y q z qp( ), ( ), ( )p p và x t y t z tp( ), ( ), ( )p p .
Hệ phương trình sau xác định bởi ba phương trình có được từ so sánh giữa các phần tử của hai ma trận côsin chỉ hướng oC qp( ) và oC tp( ) tùy ý sao cho không cùng nằm trên một hàng hay một cột. Ta có thể viết (1.1 ) dưới dạng:4
1 2 3
4 11 11
5 22 22
4 33 33
( ) ( )
( ) ( )
( ) ( )
( ) ( ) ( ) ( ) ( ) ( )
o o
p p
o o
p p
o o
p p
f x q x t f y q y t f z q z t f c q c t f c q c t f c q c t
= −
= −
= −
= −
= −
= −
(1.15)
Đặt:
1 2 3 4 5 6
[ ]T
f = f f f f f f (1.16)
1 2 3 4 5 6
[ , , , , , , , , ,p p p , , ]T
X = q q q q q q x y z rotx roty rotz (1.17)
Ta nhận được f X( ) 0= (1.18)
Hệ phương trình (1.1 ) được gọi là hệ phương trình động học cơ bản của 8 robot. Hệ phương trình này cho phép ta thực hiện việc khảo sát bài toán động học của robot.
--- 35
CHƯƠNG 2