rbcn2011 Chuong2 9568

Bài 4 PHƯƠNG TRÌNH ĐỘNG HỌC
VÀ ĐỘNG LỰC HỌC CỦA ROBOT
Giải quyết hai bài toán động học tay máy: bài toán động
học thuận, bài toán động học ngược
ROBOT CÔNG NGHIỆP
Muốn điều khiển cánh

A tay robot di chuyển từ A
đến B?
Muốn điều khiển cánh

tay robot di chuyển từ A
đến C theo một quỹ đạo
Mô hình robot 6 bậc tự do
xác định?
3.1 Các hệ tọa độ cơ sở - Ma trận quay

3.1.1 Các hệ tọa độ cơ sở
a/ Hệ tọa độ cố định
Là hệ tọa độ gắn với giá
đỡ không chuyển động.
b/ Hệ tọa độ tương đối

Là hệ tọa độ gắn với mỗi
khâu chuyển động trong hệ
vật
c/ Quy tắc bàn tay phải
Xác định phương, chiều của các trục tọa độ
Phát biểu quy tắc: Dùng bàn tay

phải, nắm 2 ngón út và áp út lại, 3
ngón cái, trỏ và giữa xoè ra vuông
góc nhau. Chọn ngón cái chỉ phương
và chiều của trục z, thì ngón trỏ sẽ
chỉ phương và chiều của trục x và
ngón giữa sẽ chỉ phương và chiều
của trục y.
x
z
y
z
x y
d/ Véc tơ đơn vị và véc tơ định vị:

* Véc tơ đơn vị: z
Là một bộ gồm 3 véc tơ, từng đôi z

một vuông góc với nhau, có giá trị
P(x,y,z)
là 1 đơn vị, có phương và chiều
trùng với phương và chiều của các
rp
trục tọa độ ez
* Véc tơ định vị O ey y
Là một véc tơ dùng để ex x

xác định vị trí của một
x y
điểm thuộc vật rắn so
với gốc tọa độ
e/ Quan hệ giữa hệ toạ độ cố định và hệ toạ độ tương đối

i
ez i
P
(0)
(0)
ey rp
(i)
i
ex rp
(0) ex
(0) (0)
0i
r
(0) 0i
ey
(0)
0
ez
(0)
r
(0) P  r
(0) 0 i  i
(0) R ( i ) r P (2.1) (i ) rp (0)
i
RT ( (0) r P  (0) r0 i ) (2.2)
z1
x2
z2
x1 O2
O1
y1
z r
(0) 01
r
(0) 02
y2
x
O
y
2.1.2 Ma trận quay

a/ Khái niệm
Ma trận quay của một hệ toạ độ là một ma trận được tạo bởi toạ
độ của các véc tơ đơn vị của hệ tọa độ đó
(0)
i
R   (0)i ex i
e
(0) y e 
i
(0) z
Ma trận quay đặc trưng cho hướng của hệ toạ độ vật so với hệ toạ độ cố
định
b/ Các phép quay sơ cấp

i
- Phép quay quanh trục x
R
(0) x
( )   (0)i ex i
e
(0) y e 
i
(0) z
z
P
(0)
zP 1 0 0 
 
i
(0) xR ( )   0 cos   sin  
 0 sin  cos  
zi  
yi
w v
i i
ey
r  r  i
(0) P (0) 0i (0) R ( i ) rP
ez
Ø
yP y x  1 0 0  u 
x=xi (0)  y    0 cos   sin   v 
    
 z   0 sin  cos    w
x  u 
   
(0) rP   y  ( i ) rP   v 
 z   w
* Phép quay quanh trục y z P

zP
 cos 0 sin 
 
(0) R y ( )  
i
0 1 0  zR
yR
  sin 0 cos 
 ez
ey
y=yR xP x
* Phép quay quanh trục z y P

yP
cos   sin  0
Rz ( )   sin  cos  0 zR
yR
 0 0 1
ez
ey
z=zR xP x
c/ Các phép quay tổng hợp

Là phép quay được hình thành từ các phép quay sơ cấp liên
tiếp nhau
* Lưu ý: Trong các phép quay hữu hạn, thứ tự thực hiện các phép
quay khác nhau cho ta các hướng khác nhau
 / 2  y 0  y
  0     / 2   / 2 
 0  z 0      / 2 
z  0 
x z x
 / 2 
  0 
y z
x  0 
 / 2 
0       / 2 
   / 2   0 
z y y
0 
x x
Ví dụ minh họa:
Xác định vị trí của điểm làm l1 P
việc P thuộc khâu quay R so

với giá đỡ cố định. Biết P có O1
tọa độ (u,v) trong hệ tọa độ

tương đối gắn liền với khâu r
(0) P
R, vị trí của khớp quay R so l0 z
với giá đỡ cố định là (x0,z0),

góc
 x O
Theo công thức 2.1:
r
(0) ( P )   i
(0) 0 i (0) R ( i ) rP
r x1 l1
z1
 x   x0  1  0 
 z    l   (0) R l 
   0  1
O1
Ma trận quay:
 sin   cos  
r
(0) P
1
R   e
1 1

e  
(0) (0) x (0) z
 cos  sin   l0 z
Vị trí của điểm P so với giá đỡ cố định:
 x   0   sin   cos    0 
 z   l   cos  sin   l 
   0    1 x O
Bài tập:
l2
Bài 1: P
Xác định vị trí của điểm P l1

R2
trong hệ tọa độ cố định biết:
l 0
 750mm R1
l 1
 400mm
l 2
 350mm
l0
 1
 300
 2
 450
Bài 2:
l2
Xác định phương án
P
điều chỉnh các góc
quay  1 , 2 l1
R2
để điểm làm việc Q
của Robot di chuyển
R1
từ vị trí điểm P đến
vị trí điểm Q(-
600,800)
l0
2.2 Phép biến đổi thuần nhất

2.2.1 Mô tả tối thiểu của hướng
- Ma trận quay gồm 9 thành phần nhưng có 6 điều kiện ràng
buộc nên chỉ có 3 thành phần độc lập
- Việc dùng 3 tham số độc lập để mô tả hướng được gọi là sự
mô tả tối thiểu MRO (Minimal Representation of Orientation)
- Có nhiều bộ tham số khác nhau cho MRO nhưng thường
dùng là góc Euler và góc RPY
 r11 r12 r13 


R  r21 r22 r23 
r31 r32 r33 
2.2 Phép biến đổi thuần nhất
a, Góc Euler:
Góc ơle hình thành mô tả hướng tối thiểu bằng cách
tổ hợp các thành phần độc lập tuyến tính của ma trận
quay trong hệ tọa độ hiện thời (ba lần quay quanh ba
trục của ba hệ quy chiếu khác nhau). Tùy theo cách tổ
hợp cụ thể 3 thành phần độc lập từ 9 thành phần ban
đầu có thể đạt được 12 bộ góc ơle khác nhau.
b, Góc Roll – pitch – Yaw:

Góc RPY hình thành MRO bằng cách tổng hợp các phép quay
thành phần trong hệ tọa độ cố định
Góc RPY là bộ góc EULER theo trình tự (zyx)
c c c s s  s c c s c  s s 
 
RRPY  R( z , ) R( y, ) R( x, )   s c s s s  c c s s c  c s 
  s c s c c 
 
2.2.2 Ma trận chuyển đổi thuần nhất

Ma trận chuyển đổi thuần nhất một điểm từ hệ thứ I về hệ
thứ i-1:
Ma trận chuyển đổi thuần nhất một điểm từ hệ tọa độ thứ I

về hệ thứ i-1:
2.3 Bài toán động học thuận

Nhiệm vụ của bài toán thuận là khi cho trước các biến khớp phải
xác định vị trí và định hướng của tất cả các khâu trên cánh tay,
thông thường nếu không khống chế quỹ đạo của các khâu trên
cánh tay nhằm tránh va chạm với các đổi tượng khác trong vùng
làm việc, người ta thường chỉ xác định vị trí và định hướng của
khâu sau cùng.
Trên cánh tay có các khâu và các khớp tổ hợp với nhau mà tạo
thành, cánh tay có hai hình thức cơ bản, có thể chuỗi động hình
thành nên nó là kín, hoặc hở.
Các khâu và các khớp được mô tả qua các thông số được chia ra
hai loại, các thông số không thay đổi (chiều dài khâu) gọi là tham
số. Các thông số thay đổi (góc quay của khâu, lượng di chuyển dài
của khâu tịnh tiến) gọi là biến khớp.
Trong kĩ thuật robot sử dụng phổ biến hai loại khớp thấp là quay
và tịnh tiến, khớp cầu được tổ hợp từ ba khớp quay có đường trục
quay giao nhau tại một điểm.
2.3.1 Biến đổi thuần nhất các hệ trục tọa độ theo quy tắc D-H
Bộ thông số Denavit-Hartenberg (DH) :
- Chọn trục Zn : trục Zn nằm dọc theo trục khớp thứ n+1.
- Trục Xn : nằm pháp tuyến chung (đường vuông góc
chung) hướng từ trục khớp n đến trục khớp n+1.
- Góc quay cùng chiều KĐH : dương , ngược KĐH: âm
Bộ thông số Denavit-Hartenberg (DH) :

Bảng thông số động học ĐH:

Gồm hàng là các thông số DH và cột là các khâu
Ma trận ĐH và phương trình động học:
cos( j )  cos( j ).sin( j ) sin( j).sin( j) ak .cos( j) 

 sin( j ) cos( j ).cos( j)  sin( j).cos( j) a .sin( j) 
Aj 1 
j  k 
 0 sin( j ) cos( j ) dj 
 
 0 0 0 1 
Các bước lập phương trình động học theo quy tắc ĐH:
-B1: Gắn hệ tọa độ cố định và các hệ tọa độ tương đối
-B2: Xác định bộ tham số ĐH
-B3: Thiết lập bảng tham số ĐH
-B4: Tính các ma trận ĐH
-B5: Xây dựng phương trình động học.

Ví dụ 1: Cơ cấu 3 khâu phẳng

Ví dụ 1: Cơ cấu 3 khâu phẳng
 ci  s i 0 a i ci 
s c 0 ai si 
Aii 1 (i )   i i
0 0 1 0 
 
0 0 0 1 
c123  s123 0 a1c1  a2 c12  a3c123 

s c123 0 a1s1  a2 s12  a3 s123 
T3 (q)  A1 A2 A3 
0 0 1 2  123
0 0 1 0 
 
0 0 0 1 
c123  cos(1   2  3 )
Ví dụ 2: Cơ cấu RRT
2.3.2. Vùng hoạt động của phần công tác:

Tập hợp các điểm mà tay máy có khả năng định vị và định hướng
phần công tác thỏa mãn yêu cầu công việc tạo thành một hoặc vài
miền liên tục, miền đó được gọi là miền công tác, hay vùng làm việc,
Những điểm thuộc vào vùng làm việc mà tay máy không thể đạt
được định vị ở đó do các lí do kết cấu gọi là lỗ trống.
Vùng làm việc của tay máy là một thông số quan trọng của nó, thể
tích và hình dạng của vùng làm việc phụ thuộc vào kết cấu của tay
máy và giới hạn của các biến khớp.
Đôi khi người ta có phân biệt vùng với tới và vùng với tới có định
hướng, để biểu diễn được vùng làm việc, xác định phần với tới có
đinh hướng và với tới không định hướng cần có các kĩ năng toán học,
và trên cơ sở đặc điểm cụ thể của từng loại tay máy.
2.4. Bài toán động học ngược của tay máy:

Bài toán thuận động học nhằm xác định định vị và
định hướng của phần công tác khi cho trước các biến
khớp. Bài toán ngược cho trước vị trí và định hướng
của khâu tác động sau cùng đòi hỏi phải xác định bộ
thông số tọa độ suy rộng để đảm bảo chuyển động cho
trước của phần công tác.
Đối với tay máy có kết cấu dạng chuỗi động hở, nếu
cho trước bộ thông số biến khớp thì vị trí và định
hướng của phần công tác xác định duy nhất, điều này
không đúng với các tay máy có cấu trúc dạng chuỗi
động kín.
Đối với các tay máy dạng chuỗi động hở, ứng với một bộ
thông số mô tả định vị và định hướng của phần công tác
khi giải bài toán ngược có thể xảy ra các trường hợp:
 Có thể có nhiều lời giải khác nhau;
 Các phương trình đồng nhất thức thường có dạng phi
tuyến, siêu việt, thường không cho lời giải đúng;

 Có thể gặp nghiệm vô định vì có các liên kết thừa
giống kiểu kết cấu siêu tĩnh;

 Có thể có lời giải toán học, song lời giải này không
chấp nhận được về mặt vật lí do các yếu tố về kết cấu

của cấu trúc không đáp ứng được.
Nhìn chung khi số bậc tự do càng lớn thì bài

toán ngược càng khó giải, số nghiệm toán học lại
càng nhiều, khi đó để chọn được nghiệm điều
khiển đòi hỏi phải loại bỏ các nghiệm không phù
hợp dựa trên cơ sở các ràng buộc về giới hạn hoạt
động của các khớp.
Việc lựa chọn phương pháp để giải bài toán
ngược cũng là một vấn đề, cho đến nay không có
phương pháp tổng quát nào có thể áp dụng cho tất
cả các robot.
2.4.1. Cơ cấu ba khâu phẳng:
c123  s123 0 a1c1  a2 c12  a3c123 

s c123 0 a1s1  a2 s12  a3 s123 
T3 (q)  A1 A2 A3 
0 0 1 2  123
0 0 1 0 
 
0 0 0 1 
2.4.1. Cơ cấu ba khâu phẳng:
Dựa trên kết quả đã triển khai ở bài toán thuận, ta đã có phương trình động học
của tay máy này dưới dạng ma trận đồng nhất :
c123  s123 0 a1c1  a 2 c12  a 3c123 

s c123 0 a1s1  a 2s12  a 3s123 
T3 (q)  A1 A 2 A 3 
0 0 1 2  123
 0 0 1 0 
 
 0 0 0 1 
Ma trận định vị và định hướng phần tác động sau cùng trên cánh tay được cho
trước trong bài toán ngược dưới dạng như sau:
 a11 a12 a13 a14 
a a22 a23 a24 
A   21
 a31 a32 a33 a34 
 
a41 a42 a43 a44 
Nhiệm vụ của bài toán ngược phải xác định một bộ công thức tính 1 ;2 ;3 dựa
trên các đồng nhất thức tạo ra từ hai ma trận trên.
Vì biến số nằm trong góc nên nếu giải trực tiếp hệ phương trình mô tả
định vị và định hướng là không thể. Hãy xem hệ thiết lập được trên 2
điều kiện này:
 sin(1  2  3 )  a12
0  a
 13

0  a 23

a1 cos(1 )  a2 cos(1  2 )  a3 cos(1  2  3 )  a14
a1 sin(1 )  a 2 sin(1  2 )  a3 sin(1  2  3 )  a24


0  a34
Ba phương trình đầu của hệ mô tả định hướng của khâu sau cùng, ba
phương trình sau mô tả định vị của khâu sau cùng. Vì hệ suy biến nên
thực chất còn ba phương trình, ba ẩn:
 sin(1  2  3 )  a12

a1 cos(1 )  a2 cos(1  2 )  a3 cos(1  2  3 )  a14
a sin( )  a sin(   )  a sin(     )  a
1 1 2 1 2 3 1 2 3 24
2.4.2. Cơ cấu cầu
c1c2  s1 c1s2 c1s2 d 3  s1d 2 

s c c s s s s d  c d 
T3 (q)  A1 A2 A3 
0 0 1 2  1 2 1 1 2 1 2 3 1 2
  s 2 0 c2 c2 d 3 
 
 0 0 0 1 
2.4.2. Cơ cấu cầu:
Phương trình động học cơ cấu cầu đã xác định trong bài toán
thuận, nếu tổng quát bài toán ngược đòi hỏi đáp ứng cả định vị và
định hướng của điểm quản lí, sẽ phải giải hệ 6 phương trình (ba định
vị, ba định hướng) để xác định các biến khớp .
Xuất phát từ phương trình động học trong bài toán thuận:
c1c2  s1 c1s2 c1s2 d 3  s1d 2 
s c c1 s1s2 s1s2 d 3  c1d 2 
T30 (q )  A10 A21 A32   1 2
  s2 0 c2 c2 d 3 
 
 0 0 0 1 
Ma trận mô tả định vị, định hướng của phần công tác biết trước:
 a11 a12 a13 a14 
a a a a 
A   21 22 23 24 
 a31 a32 a33 a34 

 
a
 41 a 42 a 43 a 44 
2.4.2. Cơ cấu cầu
Hệ 6 phương trình ba ẩn như sau:
a12   s1
a  c s
 13 1 2

a 23  s1 s 2

c1 s 2 d 3  s1d 2  a14
s1 s 2 d 3  c1d 2  a 24


c2 d 3  a34
Chúng ta thấy ba phương trình đầu mô tả định hướng của
phần làm việc vì vậy không liên quan gì đến tầm với d 3,
mà chủ yếu liên quan đến hai bậc tự do quay. Ngược lại, ba
phương trình sau mô tả định vị nên liên quan chặt chẽ đến
tầm với d3.
Việc giải hệ phương trình và tìm nghiệm có thể

được thực hiện bằng các công cụ toán học thuần
túy hoặc sử dụng các công cụ phần mềm và máy
tính.
Bài toán ngược kết thúc phần xác định nghiệm
toán học, cần tiếp tục căn cứ vào các yêu cầu cụ
thể chọn nghiệm điều khiển.
Industrial Robot
Thank you very much!
Me. Nhu Quy Tho
Lecturer of Hanoi University of Industry
Faculty: Mechanical Engineering
Dept.: Mechatronics
Phone: 04.37655121(Ext 324) Mobile: 0947593636
Email: nhuquytho@haui.edu.vn
Homepage: www.haui.edu.vn

rbcn2011 Chuong2 9568

Uploaded by

Document Information

Original Title

Copyright

Available Formats

Share this document

Share or Embed Document

Sharing Options

Did you find this document useful?

Is this content inappropriate?

Copyright:

Available Formats

rbcn2011 Chuong2 9568

Uploaded by

Copyright:

Available Formats

Bài 4 PHƯƠNG TRÌNH ĐỘNG HỌC

VÀ ĐỘNG LỰC HỌC CỦA ROBOT

Muốn điều khiển cánh

Muốn điều khiển cánh

3.1 Các hệ tọa độ cơ sở - Ma trận quay

b/ Hệ tọa độ tương đối

c/ Quy tắc bàn tay phải

Xác định phương, chiều của các trục tọa độ

Phát biểu quy tắc: Dùng bàn tay

d/ Véc tơ đơn vị và véc tơ định vị:

Là một bộ gồm 3 véc tơ, từng đôi z

Là một véc tơ dùng để ex x

e/ Quan hệ giữa hệ toạ độ cố định và hệ toạ độ tương đối

2.1.2 Ma trận quay

b/ Các phép quay sơ cấp

* Phép quay quanh trục y z P

* Phép quay quanh trục z y P

c/ Các phép quay tổng hợp

Xác định vị trí của điểm làm l1 P

việc P thuộc khâu quay R so

tọa độ (u,v) trong hệ tọa độ

R, vị trí của khớp quay R so l0 z

với giá đỡ cố định là (x0,z0),

Theo công thức 2.1:

Vị trí của điểm P so với giá đỡ cố định:

Xác định vị trí của điểm P l1

2.2 Phép biến đổi thuần nhất

 r11 r12 r13 

2.2 Phép biến đổi thuần nhất

b, Góc Roll – pitch – Yaw:

2.2.2 Ma trận chuyển đổi thuần nhất

Ma trận chuyển đổi thuần nhất một điểm từ hệ tọa độ thứ I

2.3 Bài toán động học thuận

Bộ thông số Denavit-Hartenberg (DH) :

Bảng thông số động học ĐH:

Ma trận ĐH và phương trình động học:

cos( j )  cos( j ).sin( j ) sin( j).sin( j) ak .cos( j) 

-B1: Gắn hệ tọa độ cố định và các hệ tọa độ tương đối

-B2: Xác định bộ tham số ĐH

-B3: Thiết lập bảng tham số ĐH

-B4: Tính các ma trận ĐH

-B5: Xây dựng phương trình động học.

Ví dụ 1: Cơ cấu 3 khâu phẳng

Ví dụ 1: Cơ cấu 3 khâu phẳng

c123  s123 0 a1c1  a2 c12  a3c123 

2.3.2. Vùng hoạt động của phần công tác:

2.4. Bài toán động học ngược của tay máy:

 Các phương trình đồng nhất thức thường có dạng phi

tuyến, siêu việt, thường không cho lời giải đúng;

giống kiểu kết cấu siêu tĩnh;

chấp nhận được về mặt vật lí do các yếu tố về kết cấu

Nhìn chung khi số bậc tự do càng lớn thì bài

c123  s123 0 a1c1  a2 c12  a3c123 

c123  s123 0 a1c1  a 2 c12  a 3c123 

c1c2  s1 c1s2 c1s2 d 3  s1d 2 

 a31 a32 a33 a34 

Việc giải hệ phương trình và tìm nghiệm có thể

You might also like