CHƯƠNG 2

ĐỘNG HỌC THUẬN VÀ NGƯỢC ROBOT NỐI TIẾP

Robot nối tiếp là chuỗi động học hở gồm nhiều khâu nối với nhau bởi khớp quay
hoặc trượt. Đế robot liên kết với hệ tọa độ trực chuẩn tham chiếu cố định, đầu công tác
nằm ở đầu cuối của chuỗi. Vị trí tương đối giữa hai khâu kế nhau phụ thuộc góc quay
θ hay độ dịch chuyển d gọi chung là biến khớp q. Động học thuận robot có mục đích là
tìm vị trí và hướng đầu công tác so với hệ tọa độ cố định khi các khớp chuyển động
còn động học ngược là tìm các biến khớp khi cho trước vị trí và hướng đầu công tác.
động học thuận (robot) đi tìm vị trí & hướng của đầu công tác dựa vào
các khớp chuyển động ! Còn động học ngược thì tìm biến khớp khi đã
2.1 MA TRẬN CHUYỂN ĐỔI có vị trí và hướng của đầu công tác !

2.1.1 Vị trí và hướng vật rắn trong không gian

Cho hệ tọa độ trực chuẩn O0xyz gọi là hệ tọa độ gốc (gọi tắt là hệ tọa độ 0) và
một vật rắn, liên kết với nó là hệ tọa độ vật O1xyz (gọi tắt là hệ tọa độ 1). Hệ tọa độ
được xác định theo qui tắc bàn tay phải. Các vector đơn vị của hai hệ lần lượt là x0, y0,
z0 và x1, y1, z1. Cho điểm P có tọa độ (px, py, pz) trong hệ tọa độ 0, ta biểu diển bằng
phương trình sau:

O0 P  p0  px x0  p y y0  pz z0 (2.1)
T
p0   px py pz  (2.2)

p0 có nghĩa là tọa độ điểm P trong hệ tọa độ 0.


Cho hai điểm A(ax, ay, az) và B(bx, by, bz), vector p0= AB điểm đầu là A điểm
cuối là B có thành phần trong hệ tọa độ 0 là
T
p0  bx  ax by  a y bz  az  (2.3)

Tọa độ điểm O1 trong hệ tọa độ gốc biểu diễn bởi vector p= O0O1 gọi là vector
tịnh tiến. Hướng hai hệ tọa độ liên hệ với nhau bởi phép quay
X1-->Xo
x0  x1, y0  y1, z0  z1 , biểu diễn bởi ma trận quay R01 3x3, mỗi phần tử là tích vô
hướng giữa hai vector đơn vị, và chính là cos của góc giữa hai vector

 x0 x1 x0 y1 x0 z1  Giá trị nằm trong khoảng -1 đến +1 (cos(x))

R0   y0 x1 y0 z1 
1 tích của 1 hàng (cột) = 0, module = 1
quay từ hệ trục 1 sang hệ trục 0 (1->0) y0 y1 (2.4)
 z0 x1 z0 y1 z0 z1  - Ký hieu giong sách Spong cũ
- Ký hieu ngược sách Spong mới
KỸ THUẬT ROBOT- TS NGUYỄN ĐỨC THÀNH
 CHƯƠNG 2
Cột thứ nhất, thứ hai, thứ ba lần lượt là hình chiếu của x1, y1, z1 trên hệ trục 0,
Hàng thứ nhất, thứ hai, thứ ba lần lượt là hình chiếu của x0, y0, z0 trên hệ trục 1,
Nếu hai hệ trục song song nhau R01 là ma trận đơn vị. (T = biểu diễn cho cột)

Cho điểm A, biểu thị bằng vector a1= [ax1 ay1 az1]T và a0= [ax0 ay0 az0]T lần lượt
trong hệ tọa độ 1 và 0, sự chuyển đổi giữa a1 và a0 được xác định bởi (2.5) (2.6)

tọa độ của điểm A trong hệ trục 0 a 0  p0  R01a1 (2.5)

p0: là vecto tịnh tiến giữa O và O1
a1: là tọa độ điểm A trên hệ trục 1
a1  p1  R10a 0 (2.6)
p1là tọa độ điểm gốc O0 trong hệ tọa độ 1 (O chiếu sang O 0 1

Hình 2.1 Qui ước bàn tay phải và dịch chuyển giữa hai hệ tọa độ

 x1 x0 x1 y0 x1z0 
R   y1 x0
0
1 y1 y0 y1z0  (2.7)
 z1 x0 z1 y0 z1z0 

Tích vô hướng có tính chất giao hoán, so sánh (2.4) và (2.7) ta nhận thấy

R01  ( R10 )T  ( R10 )1 (2.8)

Ma trận quay R01 có các đặc tính:

- det(R)=1
- mỗi cột hay hàng là vector đơn vị
- tích hai hàng hay hai cột là 0
- Mỗi cột là hình chiếu của vector đơn vị hệ tọa độ mới so với hệ tọa độ gốc
- Mỗi hàng là hình chiếu của vector đơn vị hệ tọa độ gốc so với hệ tọa độ mới

Như vậy một vật rắn trong không gian được biểu diễn so với hệ tọa độ tham
chiếu bởi vector vị trí và ma trận quay. Nếu hai gốc tọa độ trùng nhau ta có phép quay
quanh gốc (Hình 2.2a), là kết hợp của ba phép quay quanh trục.

Ví dụ 2.1: Xét Hình 2.2b, tìm phép quay trục 0 sang 1 và ngược lại.

26
ĐỘNG HỌC THUẬN VÀ NGƯỢC ROBOT NỐI TIẾP
Ta tìm hình chiếu của các vector đơn vị hệ trục 1 lên hệ trục 0
T T
 2 2  2 2
 , z1  0 1 0
T
x1   0  , y1   0 
 2 2   2 2 

Suy ra ma trận quay từ hệ trục 0 sang hệ trục 1

 2 2 
 0
 2 2 
R01   0 0 1
 
 2

2
0
 2 2 

Ma trận quay từ hệ trục 1 sang hệ trục 0 là

 2 2 
 0 
 2 2 
 2 2
R10  ( R01 )T   0  
 2 2 
 0 1 0 
 
 

Hình 2.2 Quay quanh gốc

Ma trận quay biểu diễn phép quay hệ trục tọa độ, ngoài ra còn dùng để biểu diển
phép quay một khối hay phép biến đổi tọa độ
27
 CHƯƠNG 2
Ví dụ 2.2: Xét Hình 2.3, điểm A có tọa độ a0=[-2 1 1]T, thực hiện phép quay
quanh trục z0 góc 180o, ma trận quay
x y z
 1 0 0  cos180 = -1

R01   0 1 0 
trục x và y quay 180 độ
còn z giữ nguyên (0,0,1)

 0 0 1 

Hình 2.3 Quay một khối 180o quanh trục z

Điểm A có tọa độ trong hệ 0 là

 1 0 0   2   2 
a 0  R01a 0   0 1 0   1    1
 0 0 1   1   1 

Ví dụ 2.3: Xét Hình 2.4 với hai hệ tọa độ, tọa độ điểm A trong hệ 1 là a1=[2 -1
1]T tìm tọa độ điểm A trong hệ tọa độ 0.

Hình 2.4 Tính tọa độ sau phép quay

 1 0 0   2   2 
a  R a   0 1 0  1    1
0 1 1
0
 0 0 1   1   1 

28
ĐỘNG HỌC THUẬN VÀ NGƯỢC ROBOT NỐI TIẾP
Ví dụ 2.4: Cho ba điểm O1, A, B trong hệ trục Oxyz, xét hệ trục O1x1y1z1 với trục
O1x1 theo phương O1A, trục O1z1 thẳng góc mặt phẳng O1AB, tìm ma trận R01
Ta tìm vector chỉ phương trục z1: z1 = O1A x O1B, z1 = z1/|| z1||,
Vector chỉ phương trục x1: x1 = O1A/|| O1A ||,
ko hiểu
Vector chỉ phương trục y1: y1 = z1 x x1
 x1 (1) y1 (1) z1 (1) 
R01   x1 (2) y1 (2) z1 (2)    x1 y1 z1 
 
 x1 (3) y1 (3) z1 (3) 

O1=[0;1;0];A=[1;0;0];B=[0;1;1]; tại sao ra được O1 A B ?

x1=A-O1;
x1=x1/norm(x1); norm(x1), chính là khoảng cách của điểm A s/v gốc O1, norm là 1 số ko âm
z1=cross(x1,(B-O1)); cross chính là tạo ra 1 vecto [3 biến] vuông góc với 2 vecto vào
z1=z1/norm(z1); Ở đây có nghĩa là vuông góc với O1A và O1B
y1=cross(z1,x1);
R=[x1 y1 z1]
R =
0.7071 0 -0.7071
-0.7071 0 -0.7071
0 1.0000 0

2.1.2 Các phép quay cơ bản

Cho một máy bay, hỏa tiễn hay chiếc thuyền đang chuyển động, ta gán trục z0
theo phương chuyển động (trục dọc), trục y0 trong mặt phẳng chuyển động (trục
ngang), trục x0 thẳng đứng, thẳng góc hai trục trên theo qui tắc bàn tay phải.

- Roll (góc lắc): quay góc φ quanh z0,

- Pitch (góc ngẩng) : quay góc θ quanh y0,
- Yaw (góc hướng): quay góc ψ quanh x0.

Có thể hình dung ba góc này theo tư thế bàn tay: một người đứng duổi thẳng
cánh tay và bàn tay nằm ngang, cánh tay cố định, bàn tay chuyển động, góc roll là góc
lật bàn tay sấp ngửa, góc pitch: quay bàn tay lên xuống, góc yaw: quay bàn tay sang
phải trái.
Phép quay Roll biểu diễn bởi ma trận R(z, φ)

cos   sin  0 
R( z, )   sin  cos  0  (2.9)
 0 0 1 

Trong mặt phẳng xy 2D ma trận quay là

cos   sin 
R()    (2.10)
 sin  cos  
29
 CHƯƠNG 2

Hình 2.5 Góc Roll Pitch Yaw

Phép quay Pitch biểu diễn bởi ma trận R(y, θ)

 cos  0 sin  
R( y, )   0 1 0  (2.11)
  sin  0 cos 

Phép quay Yaw biểu diễn bởi ma trận R(x, ψ)

1 0 0 
R( x, )  0 cos   sin   (2.12)
0 sin  cos  

Để đơn giản ta thường viết sφ thay sinφ, cφ thay coφ

2.1.3 Kết hợp phép quay

Phép quay quanh trục có hai loại là phép quay quanh trục cố định và phép quay
quanh trục hiện tại. Khi thực hiện nhiều phép quay liên tiếp ta tìm một ma trận quay
tổng hợp.
a/ Quay quanh trục hiện tại
Cho các hệ tọa độ 0, 1, 2, hệ tọa độ 1 suy từ hệ tọa độ 0 bởi ma trận R01 , hệ tọa
độ 2 suy từ hệ tọa độ 1 bởi ma trận R12 , điểm A trong không gian có tọa độ lần lượt a0,
a1, a2 trong ba hệ. Từ kết quả trước ta có

p0  R01 p1, p1  R12 p2

p0  R01 R12 p 2  R02 p 2
Suy ra (2.13)
R02  R01 R12

30
ĐỘNG HỌC THUẬN VÀ NGƯỢC ROBOT NỐI TIẾP

Phép quay R02 gọi là phép quay kết hợp hai phép quay liên tiếp trong hai hệ trục
khác nhau, do phép nhân ma trận không có tính giao hoán nên trật tự phép nhân là ma
trận quay thực hiện trước thì đặt ở bên trái ma trận quay thực hiện sau, ta gọi là phép
nhân sau.

Ví dụ 2.5: xét phép quay góc θ quanh trục hiện tại y0 rồi quay góc φ quanh trục
hiện tại z1, ma trận quay là

 c 0 s  c  s 0  cc cs s 

R  R( y, ) R( z, )   0 1 0   s c 0    s c 0 
  s 0 c  0 0 1    sc  ss c 

b/ Quay quanh hệ trục gốc cố định

Gỉa sử có điểm A trong hệ tọa độ gốc, ta dùng phép quay R01 quay A đến điểm
B, sau đó dùng phép quay R12 quay B đến điểm C, tọa độ điểm C trong hệ tọa độ gốc
là:

b  R01a , c  R12b
c = R12 R01a = R02a (2.14)
R02  R12 R01

Ma trận R02 là tích của phép nhân trước, phép quay thực hiện trước thì ma trận
quay đặt bên phải.

Hình 2.6 Quay quanh trục hiện tại

2.1.4 Phép quay YAW PITCH ROLL XYZ quanh trục cố định
Thực hiện ba phép quay theo thứ tự góc ψ quanh trục x0, góc θ quanh trục y0 và
góc φ quanh trục z0,

Ví dụ 2.6: Cho góc quay ψ= θ=45o , φ=90o, ta tính được

phi=90;psi=45;theta=45
cphi=cosd(phi);
cosd = cosi(x = độ); còn cos = cosin(x=rad) 31
 CHƯƠNG 2
sphi=sind(phi);
cpsi=cosd(psi);
RXYZfixed  R( z , ) R( y, ) R( x, ) cái này quay trước để bên phải

cos   sin  0   cos  0 sin   1 0 0 

  
  sin  cos  0   0 1 0  0 cos   sin  
 
 0 0 1    sin  0 cos  0 sin  cos  
cc css  sc csc  ss 
RXYZfixed   sc sss  cc ssc  cs  (2.15)
 
  s cs cc 

spsi=sind(psi);
ctheta=cosd(theta);
stheta=sind(theta);
R=[cphi*ctheta cphi*stheta*spsi-sphi*cpsi
cphi*stheta*cpsi+sphi*spsi;
sphi*ctheta sphi*stheta*spsi+cphi*cpsi sphi*stheta*cpsi-
cphi*cpsi;
-stheta ctheta*spsi ctheta*cpsi]
R =
0 -0.7071 0.7071
0.7071 0.5000 0.5000
-0.7071 0.5000 0.5000

Bây giờ cho ma trận quay R có 12 phần tử (2.16). Ta đi tìm ba góc φ θ ψ, đây là
bài toán động học quay đảo. So sánh (2.15) và (2.16), từ r21 và r11 suy ra (2.17)

 r11 r12 r13 

R   r21 r22 r23  (2.16)
 r31 r32 r33 

Nếu r21 và r11 không cùng 0 (θ ≠ ±900)

r21
tg  ,   atan2(r21 , r11 ) (2.17)
r11

*atan2(y,x) cho góc tính bằng rad mà tang là y/x, atan2d(y,x) cho góc tính bằng
độ mà tang là y/x. sử dụng arctan2 Mục đích để xét dấu ....

Bình phương r32 và r33 rồi cộng lại lấy căn ta được cosθ, xét thêm r31 ta được

r31
tg  ,   atan2(r31 , r322  r332 ) (2.18)
r r
2
32
2
33

32
ĐỘNG HỌC THUẬN VÀ NGƯỢC ROBOT NỐI TIẾP
Từ r32 và r33 suy ra Tính bằng atan2d() để ra độ hết

r32
tg  ,   atan2(r32 , r33 ) (2.19)
r33

Ví dụ 2.7: Cho ma trận chỉ phương hệ trục Ox1y1z1 so với Ox0y0z0

 0 0.7071 0.7071

R   0.7071 0.5 0.5 
 0.7071 0.5 0.5 

tính ba góc φ θ ψ
r11=0;
r21=0.7071;
r31=-0.7071;
r32=0.5;
r33=0.5;
phi=atan2d(r21,r11)
theta=atan2d(-r31,sqrt(r32*r32+r33*r33))
psi=atan2d(r32,r33)

phi = 90o, theta = 44.9997o, psi = 45o

Nếu r21 và r11 cùng 0 thì θ = ±900, ta có trường hợp suy biến, có vô số nghiệm
cho φ và ψ.

2.1.5 Phép quay RPY ZYX quanh trục hiện tại Roll Pitch Yaw

Phép quay quanh trục cố định khó thực hiện đối với bàn tay robot, giả sử có hệ
trục Ox0y0z0, ta thực hiện phép quay góc φ quanh z0, lúc này x0 → x1,y0→y1, tiếp tục
quay góc θ quanh y1, x1 → x2, z0→z2, cuối cùng quay góc ψ quanh x2.
Ma trận quay là
phép quay trục hiện tại, R = phép nhân sau, quay trước để bên trái

RZYXmobile  R( z, ) R( y, ) R( x, ) cái này làm trước để bên trái

cc css  sc csc  ss 

RZYXmobile   sc sss  cc ssc  cs  (2.20)
  s cs cc 

So sánh (2.15) và (2.20) ta thấy kết quả giống nhau. Ta nhận thấy qui tắc nhân
ma trận quay theo trục hiện tại thực hiện từ trái sang phải, phép quay thực hiện đầu
tiên dặt ở bên trái nhất (phép nhân sau).
Nếu có nhiều phép quay ta thực hiện tương tự Ví dụ 2.8 sau đây

33
 CHƯƠNG 2
Ví dụ 2.8: Tìm ma trận R thực hiện năm phép quay sau: quay góc 30o quanh trục
hiện tại x, góc 60o quanh trục hiện tại z, góc 45o quanh trục cố định z0, góc 90o quanh
trục hiện tại y, quay góc 180o quanh trục cố định x0.
Ta bắt đầu với R(x,30) sau đó theo thứ tự nhân phía trước nếu quay trong hệ trục
cố định và nhân phía sau nếu quay trong hệ trục hiện tại

R= R(x,160)R(z,45) R(x,30) R(z,60) R(y,90)

180

2.1.6 Phép quay góc Euler ZYZ quanh trục hiện tại
Ta thực hiện phép quay góc φ quanh trục z, góc θ theo trục hiện tại y rồi góc ψ
theo trục hiện tại z quay trước, đặt bên trái = phép nhân sau./

RZYZmobile  R( z , ) R( y, ) R( z, ) 
 ccc  ss ccs  sc cs 
 scc  cs  scs  cc ss  (2.21)
 
  sc ss c 

2.1.7 Quay quanh một trục tùy ý

Đôi khi ta muốn tạo chuyển động quay quanh một trục, ví dụ khi dùng robot vặn
vít hay vặn ốc. Xét Hình 2.7 ta muốn quay khối rắn hệ trục Oxyz góc θ quanh trục r đi
qua gốc. Phân tích vector r thành ba thành phần [rx ry rz]T và các góc α, β

Ta sẽ đưa trục r về trục z, quay quanh trục z góc θ sau đó quay trở lại trục r về vị
trí cũ, quá trình gồm năm bước:
- quay quanh trục z góc –α rồi quay quanh trục y góc –β để đưa r thẳng hàng với
z
- quay quanh trục z góc θ
- quay quanh trục y góc β rồi quay quanh trục z góc α để đưa r trở về vị trí cũ.
Vậy phép quay quanh r góc θ thực hiện bởi ma trận quay

R  r ,   R  z,   R  y,  R  z,  R  y, – R  z,   (2.22)

Ta tìm các góc α, β từ các thành phần của r để tính R(r, θ).
ry rx
sin   ,cos   ,
rx2  ry2 rx2  ry2

sin   rx2  ry2 ,cos   rz

Suy ra ma trận R(r, θ)

34
ĐỘNG HỌC THUẬN VÀ NGƯỢC ROBOT NỐI TIẾP

 rx2 a  c rx ry a  rz s rx rz a  ry s 
 
R(r , )   rx ry a  rz s ry2 a  c ry rz a  rx s 
(2.23)
 rx rz a  ry s ry rz a  rx s rz2 a  c 
 
a  1  c

Hình 2.7 Quay góc θ quanh trục r

Sau đây xét bài toán ngược, cho ma trận quay (2.16) ta tìm r và θ. Kết quả là
r  r  r 1
  cos 1 ( 11 22 33 ),
2
 r32  r23  (2.24)
1  
r r13  r31 
2sin  
 r21  r12 

2.1.8 Ma trận biến đổi thuần nhất

Phương trình (2.5) cho tọa độ của điểm A tịnh tiến và quay đối với hệ tọa độ gốc,
để thuận tiện cho việc tính toán ta đặt

a  1  R01 p0 
a    , H 0    (2.25)
1  0 1

với a là tọa độ ba thành phần, như vậy (2.5) trở thành:

a0  H 01a1 H là ma trận biến đổi thuần nhất

(2.26)

35
 CHƯƠNG 2

0 là vector hàng [0 0 0], H 01 là ma trận 4x4 gồm ma trận quay R01 chuyển hệ trục
0 sang hệ trục 1 và p 0 vectơ tịnh tiến từ gốc O0 đến gốc O1, gọi là ma trận biến đổi
thuần nhất, phương trình (2.26) chuyển tọa độ điểm A trong hệ tọa độ 1 sang hệ tọa
độ 0 bằng phép nhân ma trận.

Ngược lại cho tọa độ điểm A trong hệ tọa độ 0 ta có thể tìm tọa độ trong hệ tọa
độ 1 bằng biểu thức
vecto a~ là vecto cột có 4 thành phần ....

a1  ( H 01 )1 a0  H10a0

 R01T  R01T p0   R10  R10 p0  (2.27)
H 
0
1  
 0 1  0 1 

R10 là ma trận quay từ hệ 1 sang hệ 0 biểu thị trong hệ tọa độ 1

Các ma trận đồng nhất đặc biệt quay Rot và tịnh tiến Trans:

1 0 0 0  c 0 s 0
0 c  s 0  0 1 0 0 
Rot = phép quay

 , Rot y ,  
Trans = phép tịnh tiến
Rot x , ,
0 s c 0    s 0 c 0
   
0 0 0 1  0 0 0 1
(2.28)
c  s 0 0 1 0 0 dx 
 s c 0 0 0 1 0 d y 
Rot z ,  , Transd  
0 0 1 0  0 0 1 dz 
   
0 0 0 1 0 0 0 1
Ma trận H có dạng tổng quát
 nx sx ax dx 
n sy ay d y 
H 01   n s a d    y (2.29)
 nz sz az dz 
 
0 0 0 1

n: vector đơn vị hướng của trục x1 trong hệ trục 0

s: vector đơn vị hướng của trục y1 trong hệ trục 0
t: vector đơn vị hướng của trục z1 trong hệ trục 0
d : vector khoảng cách hai gốc tọa độ từ O0 đến O1, trong hệ trục 0 O0 O1 x0 z0 x1 z1 l
θ β

Ví dụ 2.9: Tìm ma trận biến đổi hai hệ trục theo hình sau

36
ĐỘNG HỌC THUẬN VÀ NGƯỢC ROBOT NỐI TIẾP

O0 rời qua O1 1 khoảng cách l, quay góc B

Hình 2.8 Phép quay quanh trục y và tịnh tiến

 c 0 s ls 
 0 1 0 0
H 0  Rot y ,Transl  
1  (2.30)
  s 0 c lc
 
 0 0 0 1 

Nếu có một loạt phép biến đổi 0→1→2…→n , trong hệ tọa độ hiện tại, điểm A
trong hệ tọa độ n có tọa độ an thì tọa độ trong hệ tọa độ 0 là: (pp nhân sau)

a0  H01 H12 ..H nn1an  H0nan (2.31)

Chú ý là nếu biến đổi trong hệ tọa độ cô định thì ta phải dùng qui tắc nhân trước.

2.2 ĐỘNG HỌC THUẬN ROBOT

Cho robot nối tiếp n bậc tự do (Hình2.9) , các khớp nối một bâc tự do quay hay
trượt, các biến khớp là góc quay hay khoảng dịch chuyển, ta ký hiệu là qi. Bài toán
động học thuận là tìm vị trí và hướng đầu công tác (end effector, gripper) theo vector
biến khớp q=[q1 q2 …qn]T.
Vị trí đầu công tác xác định bởi vector p0e nối từ gốc tọa độ tham chiếu O0xyz
đến gốc hệ tọa độ liên kết với đần công tác, trong hệ tọa độ tham chiếu.
Hướng đầu công tác là ma trận R0e vuông 3x3 gồm ba vector  n0e s0e a0e  là
các vector đơn vị chỉ phương của hệ tọa độ liên kết với đầu công tác, trong hệ tọa độ
tham chiếu. Các vector chỉ phương được chọn tùy theo cấu tạo đầu công tác, nếu là tay
kẹp thì a thẳng góc mặt phẳng tay kẹp, n trong mặt phẳng tay kẹp và theo chiều tay
kẹp, s chọn theo qui tắc bàn tay phải.
Theo qui tắc biến đổi đồng nhất, động học thuận xác định bởi ma trận
 Re p0e 
T0e (q)   0  (2.32)
0 1
e = ma trận hướng của đầu cuối
q - biến khớp ......

37
 CHƯƠNG 2

nằm trong ma trận Roe

cho biết hướng của đầu công tác

a thẳng góc với tay kẹp

n trong mặt phẳng tay kẹp
s = quy tắc bàn tay phải
vị trí của đầu cuối

biến khớp

Hình 2.9 Vị trí và hướng đầu công tác

2.2.1 Tính động học thuận trực tiếp

Đối với các cơ cấu đơn giản ta dùng phương pháp hình học để tính động học
thuận, ví dụ xét cánh tay phẳng hai bậc tự do quay H2.10

góc hướng

Ye tọa độ đầu cuối

robot 2 bậc tự do

Xe

Hình 2.10 Cánh tay phẳng 2DOF quay

Tọa độ tâm tay kẹp:

xe  a1 cos 1  a2 cos(1  2 )
(2.33)
ye  a1 sin 1  a2 sin(1  2 )
Thành phần của vector chỉ phương:
a0e   cos(1  2 ) sin(1  2 ) 
T

T
s0e  cos(1  2  900 ) sin(1  2  900 )  (2.34)

 sin(1  2 )  cos(1  2 ) 
T

38
ĐỘNG HỌC THUẬN VÀ NGƯỢC ROBOT NỐI TIẾP
Ma trận động học thuận:
n_oe s_oe a_oe poe

0 s(1  2 ) c(1  2 ) a1c1  a2c(1  2 )  xe

0 c(   ) s(   ) a s  a s(   )  ye
T  1 2 1 2 1 1 2 1 2 
(2.35)
1 0 0 0  ze
 
0 0 0 1 

2.2.2 Qui tắc Denavit-Hartenberg tính toán phức tạp cho ROBOT nối tiếp !
Xét robot nối tiếp n bậc tự do gồm n khớp một bậc tự do, n+1 khâu, khâu 0 là đế
robot, cố định, khâu n là đầu công tác, mổi khâu được gắn hệ trục tọa độ, hệ tọa độ
tham chiếu O0x0y0z0 cố định và liên kết với đế robot, hệ tọa độ Oixiyizi liên kết với
khâu thứ i và chuyển động khi khâu i chuyển động, hệ tọa độ Onxnynzn liên kết với đầu
công tác. Qui tắc Denavit-Hartenberg (gọi tắt là D-H) ra đời năm 1955, qui định cách
vẽ các hệ tọa độ để thống nhất cách tính động học thuận.

Hình 2.11 Các hệ tọa độ liên kết

Vị trí và hướng hệ tọa độ i+1 so với hệ i được xác định bởi ma trận đồng nhất

 Ri 1 pii 1 
H ii 1 (qi )   i 
 0 1 

Rii 1 là ma trận chỉ phương của hệ tọa độ i+1 trong hệ tọa độ i, pii 1 là vị trí của
gốc hệ tọa độ i+1 trong hệ tọa độ i.
Vị trí và hướng hệ tọa độ n (đầu công tác) là tích các ma trận

39
 CHƯƠNG 2

T0n (q)  H01 (q1 ) H12 (q2 )...H nn1 (qn ) (2.36)

Nếu ta chọn hệ tọa độ tham chiếu ở vị trí khác thì ma trận từ hệ tham chiếu này
đến hệ tọa độ 0 là H ref
0
. Nếu đầu công tác gắn một dụng cụ và ma trận từ đầu công tác
đến hệ tọa độ dụng cụ là H ntool . Vị trí và hướng hệ tọa độ của dụng cụ so với hệ tọa độ
tham chiếu xác định bởi:
tool
Tref (q)  H ref
0
T0n (q) H ntool (2.37)

Sau đây trình bày cách vẽ hệ trục và cách tính ma trận H

- Robot n bậc tự do có n+1 hệ trục đánh số từ 0 đến n.

- Hệ trục i gồm ba trục xi yi zi thẳng góc nhau từng đôi theo qui tắc bàn tay phải,
miêu tả chuyển động của khớp i+1, khớp i nối khâu i và khâu i+1, hệ trục i gắn liền
với khâu i, hệ trục 0 cố định gắn liền đế robot.
- Trục z0 hướng theo trục quay hay trục tịnh tiến (gọi chung là trục chuyển động
của khâu 1, gốc O0 và trục x0 tùy chọn,
- Trục zi hướng theo trục chuyển động của khâu i+1 (khớp i+1),
- Gốc Oi là giao điểm trục zi với pháp tuyến chung của zi-1 và zi, nếu hai trục song
song nhau gốc Oi chọn tùy ý, nếu hai trục cắt nhau Oi là giao điểm,
- Trục xi trên đường pháp tuyến chung của zi-1 và zi, hướng từ khớp i đến khớp
i+1, xi thẳng góc zi-1 và zi

Hình 2.12 Qui tắc Denavit-Hartenberg

- Trục yi kết hợp với zi , xi theo qui tắc bàn tay phải,
- Hệ trục cuối cùng có xn thẳng góc zn-1, zn thẳng góc xn và thường chọn là song
song zn-1.
- Hệ trục cuối cùng liên kết với tay kẹp, gốc Oe thường chọn ở giữa tay kẹp, trục
ze thường gọi là trục a (approach) theo hướng ngón tay, xe lả trục n (normal) thẳng góc
mặt phẳng bàn tay, còn ye là s (sliding).
40
ĐỘNG HỌC THUẬN VÀ NGƯỢC ROBOT NỐI TIẾP
Kế tiếp xác định bốn thông số:
ai: khoảng cách giữa hai trục zi-1 và zi,
di: khoảng cách giữa hai trục xi-1 và xi,
αi: góc giữa hai trục zi-1 và zi với trục quay là xi, chiều dương là chiều lượng giác,
θi: góc giữa hai trục xi-1 và xi với trục quay là zi-1, chiều dương là chiều lượng
giác.
ai, αi là hằng số, nếu khớp i-1 là khớp quay thì di là hằng số, θi là biến khớp, nếu
khớp i-1 là khớp tịnh tiến thì di là biến khớp, θi là hằng số.
Dựa vào bốn thông số ta tính ma trận đồng nhất H ii 1 . Chọn một hệ tọa độ trùng
với hệ tọa độ i-1, quay góc θi quanh trục zi-1, tịnh tiến di dọc trục zi-1, tịnh tiến khoảng
cách ai dọc trục xi rồi quay góc αi quanh trục xi, ta được hệ tọa độ i. Từ lý luận trên ta
được (2.38) (tham khảo 2.28) Rot = quay, Trans o ko quay

H ii1  Rot z ,Transz ,d Transx ,a Rot x ,

ci  si ci si si ai ci 
 s ci ci ci si ai si  (2.38)
 i
0 si ci di 
 
0 0 0 1 

Ví dụ 2.10: Cánh tay phẳng ba bậc tự do quay Hình 2.13

Các trục z0, z1, z2, z3 thẳng góc mặt phẳng robot (mặt phẳng xy), do đó α=0, các
trục x thẳng hàng các khâu, góc giữa hai trục x là góc θ, thông số a là khoảng cách hai
trục z, tức là chiều dài các khâu, các gốc nằm cùng mặt phẳng nên d=0. Ta có bảng
thông số: θ1 θ2 θ3 là các biến khớp.

Khâu a α θ d
1 a1 0 θ1 0
2 a2 0 θ2 0
3 a3 0 θ3 0

ci  si 0 ai ci 

 s ci 0 ai si 
H ii1 (i )   i
 0 0 1 0 
 
 0 0 0 1 

Động học thuận cho bởi biểu thức (2.39), c123 có nghĩa cos(θ1+ θ2 +θ3)

41
 CHƯƠNG 2

T03 (q)  H 01 (1 ) H12 (2 ) H 23 (3 )

c123  s123 0 a1c1  a2c12  a3c123 
s c123 0 a1s1  a2 s12  a3 s123  (2.39)
  123 
 0 0 1 0 
 
 0 0 0 1 

Hình 2.13 Cánh tay phẳng ba bậc tự do quay

Đối với tay kẹp, ta nhận thấy hệ trục O3xyz không phù hợp với hệ trục n s a,vậy
bổ sung thêm ma trận quay x3 →a, y3 → s, z3 → n. Trong hệ trục O3xyz n=[0 0 1]T ,
s=[0 -1 0]T, a=[1 0 0]T, ma trận chuyển đổi là T3e (2.40).

0 0 1 0
0 1 0 0 
T3  
e
(2.40)
1 0 0 0
 
0 0 0 1

Ví dụ 2.11: Cánh tay khuỷu ba bậc tự do (elbow, anthropomorphic (phỏng sinh))

Bảng DH
Khâu a α d θ
1 0 90o d1 θ1
2 a2 0 0 θ2
3 a3 0 0 θ3

42
ĐỘNG HỌC THUẬN VÀ NGƯỢC ROBOT NỐI TIẾP

T03  H 01 H12 H 23
c1 0 s1 0  c2  s2 0 a2c2  c3  s3 0 a3c3 
s 0 c1 0   s2 c2 0 a2 s2   s3 c3 0 a3 s3 
 1   
0 1 0 d1   0 0 1 0 0 1 1 0 
   
0 0 0 1 0 0 0 1 0 0 0 1 

Hình 2.14 Cánh tay khuỷu

c1c23 c1s23 s1 c1 (a2c2  a3c23 ) 

s c  s1s23 c1 s1 (a2c2  a3c23 ) 
  1 23 (2.41)
 s23 c23 0 a2c2  a3 s23  d1 
 
 0 0 0 1 

Ví dụ 2.12: Cánh tay khuỷu lệch trục H2.15

Bảng DH
Khâu a α d θ
o
1 0 -90 0 θ1
2 0 90o d2 θ2
3 0 0 d3 θ3

43
 CHƯƠNG 2

Hình 2.15 Cánh tay khuỷu lệch trục

Cánh tay này sử dụng trong một số robot như PUMA, Scorbot
Bảng DH:
Khâu a α d θ
1 a1 90o d1 θ1
2 a2 0 d2 (âm) θ2
3 a3 0 0 θ3

c1 0 s1 a1c1  c2  s2 0 a2c2  c3  s3 0 a3c3 

s 0 c1 a1s1   s2 c2 0 a2 s2   s3 c3 0 a3 s3 
T0   1
3
0 1 0 d1   0 0 1 d2   0 0 1 0 
   
0 0 0 1 0 0 0 1 0 0 0 1 
c1c23 c1s23 s1 c1 (a2c2  a3c23 )  s1d 2  a1c1 
s c  s1s23 c1 s1 (a2c2  a3c23 )  c1d 2  a1s1 
T0   1 23
3
 s23 c23 0 d1  a2 s2  a3 s23 
 
 0 0 0 1 

Ví dụ 2.13: Robot RRP

44
ĐỘNG HỌC THUẬN VÀ NGƯỢC ROBOT NỐI TIẾP

Hình 2.16 Cánh tay RRP

c1c2  s1 c1s2 c1s2 d3  s1d 2 

s c c1 s1s2 s1s2 d3  c1d 2 
T0   1 2
3
(2.42)
  s2 0 c2 c2 d3 
 
 0 0 0 1 

Ví dụ 2.14: Việc tính toán bằng tay thường tốn thời gian và dễ nhầm lẫn, ví dụ
này trình bày một chương trình tính động học thuận robot phẳng RPR (Hình 2.17)
dùng Symbolic Math Toolbox, độc giả có thể tham khảo để áp dụng cho các bài toán
khác và có thể mở rộng dùng GUI.
Chọn hệ tọa độ như hình vẽ, bảng DH là

Khâu a α d θ
1 0 90o 0 θ1
2 0 -90o d2 0
3 a3 0 0 θ3

Hình 2.17 Robot RPR

%calculate forward kinematics of 3 dof RPR robot

45
 CHƯƠNG 2
clc
syms a alp d th d1 d2 th1 th3 pi a3; khai báo symbol
hi=[cos(th) -cos(alp)*sin(th) sin(alp)*sin(th) a*cos(th);
sin(th) cos(alp)*cos(th) -sin(alp)*cos(th) a*sin(th);
tính Hi theo 2.38
0 sin(alp) cos(alp) d ;
0 0 0 1 ];
a=0; alp=pi/2; d=0; th=th1; thế số
h1=subs(hi); thay vào hi
h1=expand(h1) mở rộng h1
a=0; alp=-pi/2;th=0 ; d=d2; thay số mới
h2=subs(hi); thế vào hi
h2=expand(h2) mở rộng h2
a=a3; alp=0;d=0;th=th3; thế số lần 3
h3=subs(hi); sub thay thế giá trị vào ...

h3=expand(h3) expand - hàm mở roojg, triển khai

%forward kinematics
h03=h1*h2*h3; nhân 3 ma trận
%Orientation
R=h03(1:3,1:3) trích ma trận con R từ ma trận h03 (lấy từ hàng 1 đến 3 và cột 1 đến 3)
%position of end effector
p=h03(1:3,4) tương tự Poe từ 1 đến 3 của cột 4
%verify
th1=0;th3=0; thế vào tính gì đây ? vị trí ban đầu ??
h=subs(h03)

Answer
R =
[cos(th1)*cos(th3)-sin(th1)*sin(th3),-cos(th1)*sin(th3)-
cos(th3)*sin(th1), 0]
[cos(th1)*sin(th3)+cos(th3)*sin(th1),cos(th1)*cos(th3)-
sin(th1)*sin(th3), 0]
[0, 0, 1]
p =
d2*sin(th1)+a3*cos(th1)*cos(th3)-a3*sin(th1)*sin(th3)
a3*cos(th1)*sin(th3)-d2*cos(th1)+a3*cos(th3)*sin(th1)
0
h =
[ 1, 0, 0, a3]
[ 0, 1, 0, -d2]
[ 0, 0, 1, 0]
[ 0, 0, 0, 1]

Kết quả tính toán cho thấy

c13  s13 0  a3c13  d 2 s1 

R   s13 c13 0 , p   a3s13  d 2c1 

 0 0 1   0 

Ví dụ 2.15 Cánh tay khuỷu bốn bậc tự do

Với cánh tay khuỷu bốn bậc tự do ta có thể điều khiển vị trí [pex, pey pez] và một
hướng góc α của bàn tay, các trục z1..z4 song song nhau và thẳng góc z0, các trục x1..x4
46
ĐỘNG HỌC THUẬN VÀ NGƯỢC ROBOT NỐI TIẾP
xuôi theo chiều các thanh, biến khớp θi là góc giữa xi-1 và x1, khi cánh tay duỗi thẳng ra
ta cho θi=0 tương ứng vị trí Home, góc nghiêng của bàn tay so với mặt phẳng ngang
là α= θ2 +θ3 +θ4.

Hình 2.18 Cánh tay khuỷu bốn bậc tự do

Bảng DH

Khâu α a d θ
1 90o 0 d1 θ1
2 0 a2 0 θ2
3 0 a3 0 θ3
4 0 a4 0 θ4

Sử dụng (2.41) ta được

c1c23 c1s23 s1 c1 (a2c2  a3c23 )  c4  s4 0 a4c4 

s c  s1s23 c1 s1 (a2c2  a3c23 )   s4 c4 0 a4 s4 
T0   1 23
4
 s23 c23 0 a2c2  a3s23  d1   0 0 1 0 
  
 0 0 0 1 0 0 0 1 
c1c234 c1s234 s1 c1[a2c2  a3c23  a4c234 ] 
s c  s1s234 c1 s1[a2c2  a3c23  a4c234 ] 
  1 234 (2.43)
 s234 c234 0 a2 s2  a3 s23  a4 s234  d1 
 
 0 0 0 1 

Ví dụ 2.16: Cổ tay ba bậc tự do quay

Đầu công tác (tay kẹp, bàn tay) robot thường có từ hai đến ba bậc tự do quay
tương tự bàn tay người, các trục quay z3 z4 z5 gặp nhau ở tâm cổ tay, trục z3 nằm
47
 CHƯƠNG 2
ngang, trục z4 trong mặt phẳng đứng, trục z5 dọc theo bàn tay, khoảng cách từ tâm cổ
tay đến tâm bàn tay là d6 là khoảng cách giữa x5 và x6. Các góc quay là θ4 θ5 θ6 tương
ứng góc Pitch, Yaw, Roll ( θ ψ φ). Hệ tọa độ liên kết với bàn tay là x6 y6 z6 (n s a).
Bảng DH
Khâu α a d θ
o
4 -90 a4 0 θ4
5 90o 0 0 θ5
6 0 0 d6 θ6

Hình 2.19 Cổ tay ba bậc tự do

T36  H 34 H 45 H 56 
c4 0  s4 a4c4  c5 0 s5 0  c6  s6 0 0
s 0 c4 a4 s4   s5 0 c5 0   s6 c6 0 0
 4   
0 1 0 0   0 1 0 0  0 0 1 d6 
   
0 0 0 1   0 0 0 1  0 0 0 1

 c4c5c6  s4 s6 c4c5 s6  s4c6 c4 s5 d6c4 s5  a4c4 

s c c  c s  s4c5 s6  c4c6 s4 s5 d 6 s4 s5  a4 s4 
 4 5 6 4 6 (2.44)
  s5c6 s5 s6 c5 d 6c5 
 
 0 0 0 1 

Nếu a4=0 các trục quay cắt nhau, so sánh (2.21) và (2.44) ta nhận thấy hai ma
trận quay giống nhau, vậy có sự tương đồng giữa các góc θ4 θ5 θ6 và các góc Euler φ θ
ψ
Một cách đặt hệ trục kiểu khác (khớp cầu) trình bày trên Hình 2.20

48
ĐỘNG HỌC THUẬN VÀ NGƯỢC ROBOT NỐI TIẾP

Hình 2.20 Cổ tay khớp cầu với cách đặt hệ trục kiểu khác

Bảng DH
Khâu α a d θ
4 -90o 0 0 θ4
5 90o 0 0 θ5
6 0 0 d6 θ6
 c4c5c6  s4 s6 c4c5 s6  s4c6 c4 s5 d 6c4 s5 
s c c  c s  s4c5 s6  c4c6 s4 s5 d 6 s4 s5 
T3   4 5 6 4 6
6
(2.45)
  s5c6 s5 s6 c5 d6c5 
 
 0 0 0 1 

Khi bàn tay ở vị trí như trên Hình 2.15, θ4=0, θ5=-90o, θ6=90o

0 0 1 d 6 
1 0 0 0 
T36  
0 1 0 0 
 
0 0 0 1 

Ví dụ 2.17: Robot khuỷu 6 bậc tự do gồm cánh tay khuỷu 3DOF và cổ tay 3DOF

Hình 2.21 Robot khuỷu 6 bậc tự do

Khâu a α d θ
o
1 0 90 d1 θ1
2 a2 0 0 θ2
3 a3 0 0 θ3
o
4 a4 -90 0 θ4
5 0 90o 0 θ5
6 0 0 d6 θ6

49
 CHƯƠNG 2
Ta ghép hai ma trận (2.41) và (2.44)

c1c23 c1s23 s1 c1 (a2c2  a3c23 ) 

s c  s1s23 c1 s1 (a2c2  a3c23 ) 
T0   1 23
6
 s23 c23 0 a2c2  a3 s23  d1 
 
 0 0 0 1 

c4c5c6  s4 s6 c4c5 s6  s4c6 c4 s5 d 6c4 s5  a4c4   nx sx ax pex 

s c c  c s  s4c5 s6  c4c6 s4 s5 d 6 s4 s5  a4 s4  n y sy ay pey 
* 4 5 6 4 6   (2.46)
  s5c6 s5 s6 c5 d6c5   nz sz az pez 
   
 0 0 0 1  0 0 0 1 
c1c23[c4c5c6  s4 s6 ]  c1s23[ s4c5c6  c4 s6 ]  s1s5c6  c1[c234c5c6  s234 s6 ]  s1s5c6 
n   s1c23[c4c5c6  s4 s6 ]  s1s23[ s4c5c6  c4 s6 ]  c1s5c6    s1[c234c5c6  s234 s6 ]  c1s5c6 
 s23[c4c5c6  s4 s6 ]  c23[ s4c5c6  c4 s6 ]   c234 s6  s234c5c6 

c1c23[c4c5 s6  s4c6 ]  c1s23[s4c5c6  c4c6 ]  s1s5 s6   c1[c234c5 s6  s234c6 ]  s1s5 s6 

s   s1c23[c4c5 s6  s4c6 ]  s1s23[ s4c5c6  c4c6 ]  c1s5c6     s1[c234c5c6  s234c6 ]  c1s5 s6 
 s23[c4c5 s6  s4c6 ]  c23[ s4c5c6  c4c6 ]   c234c6  s234c5c6 

c1c23c4 s5  c1s23 s4 s5  s1c5  c1c234 s5  s1c5 

a =  s1c23c4 s5  s1s23s4 s5  c1c5    s1c234 s5  c1c5 
 s23c4 s5  c23 s4 s5   s234 s5 

c1c23[d6c4 s5  a4c4 ]  c1s23[d6 s4 s5  a4 s4 ]  d6 s1c5  c1[a2c2  a3c23 ]

pe   s1c23[d6c4 s5  a4c4 ]  s1s23[d6 s4 s5  a4 s4 ]  d6c1c5  s1[a2c2  a3c23 ]
 s23[d6c4 s5  a4c4 ]  c23[d6 s4 s5  a4 s4 ]  a2 s2  d1  a3c23 

c1c234 [a4  s5d6 ]  s1c5d6  c1[a2c2  a3c23 ]

pe   s1c234 [a4  s5d6 ]  c1c5d6  s1[a2c2  a3c23 ] (2.47)
 s234 [a4  s5d6 ]  d1  a2 s2  a3 s23 

Nếu a4=0 ba trục quay cổ tay giao nhau, tọa độ tâm cổ tay là pw= pe khi cho d6=0

c1c234  c1[a2c2  a3c23 ]

pw   s1c234  s1[a2c2  a3c23 ] (2.48)
 s234  d1  a2 s2  a3 s23 

50
ĐỘNG HỌC THUẬN VÀ NGƯỢC ROBOT NỐI TIẾP
Ví dụ 2.18 : Robot Scorbot 5 bậc tự do
Robot nằm trong mặt phẳng z0O0R, bàn tay có hai bậc tự do quay góc θ4 và θ5 ,
các trục quay z1.. z3 song song nhau và thẳng góc mặt phẳng z0O0R, trục quay z4 của
góc Roll θ5 thẳng góc z3, chú ý là x4 thẳng góc khâu d5, x5 thẳng góc mặt phẳng bàn tay.
Góc α= θ2+ θ3+ θ4 là góc hợp bởi trục đứng –z0 và z4
Bảng DH
Khâu a (mm) α d(mm) θ
1 101,25mm 90 334,25 θ1 (-155 ..+155o)
o o

2 220 0 0 θ2 (-35o..+130o)
3 220 0 0 θ3 (-130o..+130o)
4 0 90o 0 θ4 (-130o..+130o)
5 0 0 137,35 θ5 (-570o..+570o)

Hình 2.22 Robot Scorbot

c1c234c5  s1s5 c1c234 s5  s1c5 c1s234 c1 (d5 s234  a3c23  a2c2  a1 ) 

s c c  c s  s1c234 s5  c1c5 s1s234 s1 (d5 s234  a3c23  a2c2  a1 ) 
T0   1 234 5 1 5
5
(2.49)
 s234c5  s234 s5 c234 d5c234  a3 s23  a2 s2  d1 
 
 0 0 0 1 
Giả sử tay kẹp của robot kẹp một dụng cụ chiều dài l trong mặt phẳng O5xz và
nghiêng góc β so với trục z5 (Hình 2.8), nhân (2.48) với (2.30) ta được ma trận hướng
và vị trí mũi dụng cụ Todc  To5 H5dc .

Ví dụ 2.19: Robot Scara 4 bậc tự do RRPR

Bảng DH
Khâu a α d θ
1 a1 0 0 θ1
2 a2 180o 0 θ2
3 0 0 d3* 0
4 0 0 d4 θ4

cos(1  2  4 ) sin(1  2  4 ) 0 a1c1  a2c12 

 sin(     )  cos(     ) 0 a s  a s 
T0  
4 1 2 4 1 2 4 1 1 2 12 
(2.50)
 0 0 1 d3  d 4 
 
 0 0 0 1 
51
 CHƯƠNG 2
Ví dụ 2.20: Robot Stanford RRPRRR (Hình 2,23)
Kết hợp (2.41) và (2.43) ta được:
c1c2  s1 c1s2 c1s2 d3  s1d 2  c4c5c6  s4 s6 c4c5 s6  s4c6 c4 s5 d 6c4 s5 
s c c1 s1s2 s1s2 d3  c1d 2   s4c5c6  c4 s6  s4c5 s6  c4c6 s4 s5 d 6 s4 s5 
T0 
6  1 2  
  s2 0 c2 c2 d3    s5c6 s5 s6 c5 d 6c5 
  
 0 0 0 1  0 0 0 1 

Hình 2.23 Robot SCARA

c1[c2 (c4c5c6  s4 s6 )  s2 s5c6 ]  s1[ s4c5c6  c4 s6 ]

n   s1[c2 (c4c5c6  s4 s6 )  s2 s5c6 ]  c1[ s4c5c6  c4 s6 ]
  s2 (c4c5c6  s4 s6 )  c2 s5c6 

c1[c2 (c4c5 s6  s4c6 )  s2 s5 s6 ]  s1[ s4c5 s6  c4c6 ]

s   s1[c2 (c4c5 s6  s4c6 )  s2 s5 s6 ]  c1[s4c5 s6  c4c6 ]
 s2 (c4c5c6  s4c6 )  c2 s5 s6 

 c1[c2c4 s5  s2c5 ]  s1s4 s5 

a   s1[c2c4 s5  s2c5 ]  c1s4 s5 
  s2c4 s5  c2c5 

 c1s2 d3  s1d 2  d6{c1[c2c4 s5  s2c5 ]  s1s4 s5 }

pe   s1s2 d3  c1d 2  d 6{s1[c2c4 s5  s2c5 ]  c1s4 s5 } (2.51)
 c2 d3  d6 [ s2c4 s5  c2c5 ] 

52
ĐỘNG HỌC THUẬN VÀ NGƯỢC ROBOT NỐI TIẾP

Hình 2.24 Robot Stanford

2.2.3 Hệ trục toạ độ

Robot dùng nhiều hệ trục toạ độ descartes để miêu tả vị trí đầu công tác, mỗi hệ
trục gồm ba trục xyz và gốc, vuông góc nhau theo qui tắc bàn tay phải.

- Hệ toạ độ cơ sở: liên kết với đế cố định của robot, toạ độ các khâu của robot
tham chiếu đến hê toạ độ này. Robot có thể đặt ở nhiều vị trí và hướng mà không ảnh
hưởng đến chương trình
- Hệ toạ độ dụng cụ : miêu tả toạ độ và hướng của đầu mút dụng cụ gắn vào tay
kẹp, cho phép thay dụng cụ mà không cần lập trình lại.
- Hệ toạ độ thế giới: cố định, thường sử dụng khi có nhiều robot hoạt động
chung
- Hệ toạ độ người dùng: dùng cho robot phải thực hiện nhiều công việc khác
nhau, liên kết với một vật cố định, ví dụ bàn máy
- Hệ toạ độ vật: liên kết với vật gia công

Hình 2.25 Các hệ trục toạ độ

53
 CHƯƠNG 2
Thông qua các ma trận hướng và vị trí giữa các hệ toạ độ ta có thể tính vị trí và
hướng đầu công tác so với hệ toạ độ bất kỳ, ví dụ so với hệ toạ độ vật Toe
TwuTuo  TwbTbtTteTeo
Toe  (TwuTuo )1TwbTbtTte
TbwTwuTuo (TteTeo )1  Tbt
Tbt  Tbo (TtoTeo )1
Các ma trận Twu , Tuo , Twb , Tte được tính trước và không đổi, ma trận Toe phụ thuộc
quỹ đạo gia công, từ Tbt ta điều khiển robot để đạt được Toe mong muốn.

2.3 ĐỘNG HỌC NGƯỢC ROBOT

Khi cho trước ma trận quay và vị trí đầu cuối, ta phải tìm các biến khớp, đây là
bài toán động học ngược. Vậy ta phải giải n phương trình trong số 12 phương trình
lượng giác (2.50)

 R0n (q) pe (q)   R0n pe 

   (2.52)
 0 1  0 1

Ví dụ 2.21: Cho cánh tay robot Hình 2.10 với a1=0.3m, a2=0.2m, tìm θ1 θ2 để
xe=0.4m, ye=0.
Ta phải giải hai phương trình
0.3cos(θ1) +0.2cos(θ1+ θ2)= 0.4
0.3sin(θ1) +0.2sin(θ1+ θ2)= 0
Sử dụng Matlab
syms theta1 theta2
[st1,st2]=solve(0.3*cos(theta1) +0.2*cos(theta1+ theta2)- 0.4,
0.3*sin(theta1) +0.2*sin(theta1+ theta2))
y=single([st1,st2]) convert sang single # kiểu float # sd double cũng đc
y=180*y/pi đổi tự rad -> độ

st1 =
2*atan(15^(1/2)/15)
-2*atan(15^(1/2)/15)
st2 =
-2*atan(15^(1/2)/5)
2*atan(15^(1/2)/5)
y =
0.5054 -1.3181
-0.5054 1.3181
y =
28.9550 -75.5225
-28.9550 75.5225

Ta nhận thấy có hai cặp nghiệm (28.9550o -75.5225o) và (-28.9550o 75.5225o)

54
ĐỘNG HỌC THUẬN VÀ NGƯỢC ROBOT NỐI TIẾP
Ví dụ 2.22 : Cho robot Standford ví dụ 2.15 với d2=-0.154, d6=0.263, hướng và
vị trí đầu cuối cho bởi ma trận

0 1 0 0.154 
0 0 1 0.763 
T0  
6 
1 0 0 0 
 
0 0 0 1 

Tìm θ1 θ2 θ4 θ5 θ6 d3. Có thể viết chương trình như Ví dụ 2.21 để giải hệ 12 phương
trình với 6 ẩn số, kết quả là θ1= θ2= θ4= θ6=90o, θ5=0, d3=0.5.
Giải bài toán ngược rất cần thiết để điều khiển robot vì các biến khớp tìm được là
giá trị đặt cho hệ thống điều khiển vị trí các khâu, tuy nhiên giải hệ phương trình động
học ngược bằng phương pháp số không cho nghiệm dạng kín và không thuận tiện khi
lập trình, do đó các hệ thống robot đều có bộ teach pendant điều khiển bằng tay robot
đến vị trí đã định rồi ghi nhớ các biến khớp, như vậy không cần giải bài toán ngược,
một phương pháp khác dùng phương pháp hình học để tìm nghiệm dạng kín có thể lập
trình. Bài toán động học ngược có thể vô định ở một số vị trí của robot gọi là vị trí bất
thường, hoặc có thể có nhiều nghiệm, trong trường hợp này ta loại bỏ những nghiệm
không phù hợp kết cấu cơ khí của robot.
Robot thường có kết cấu cánh tay 3 DOF và bàn tay có từ 2 đến 3DOF, Khớp cổ
tay là khớp quay với các trục quay cắt nhau ở một điểm. Bài toán động học ngược
thường chia làm hai bài toán:
- Tim vị trí cổ tay sau đó giải bài toán động học ngược vị trí tìm ba biến khớp đầu
tiên
- Tìm ma trận quay của ba biến khớp cuối sau đó tìm ba góc quay của khớp cổ
tay (động học ngược hướng).
Lấy ví dụ robot 6 bậc tự do với ba bậc tự do cuối là ba khâu quay mà trục giao
nhau ở tâm cổ tay, khoảng cách từ tâm cổ tay đến tâm bàn tay là d6 ( Ví dụ 2.17,Ví
dụ 2.20) , liên kết với bàn tay là hệ trục n s a , tọa độ tâm cổ tay là pw=pe-d6a, từ biểu
thức của T06 ta lấy ra vector vị trí, cho giá trị d6 trong đó bằng 0 ta được biểu thức của
pw phụ thuộc ba biến khớp q1 q2 q3, vậy ta có ba phương trình để tìm ba biến khớp q1
q2 q3, từ đó suy ra ma trận quay R03 , biết R06 = R03 R36 suy ra R36 = ( R03 )T R06 , biết biểu
thức của R36 ( q4, q5,q6) ta giải phương trình để tìm ba góc Euler φ θ ψ (Ví dụ 2.7) .
Phương pháp trình bày ở trên gọi là phương pháp giải kết động học (decoupling
kinematic).

55
 CHƯƠNG 2

Hình 2.26 Giải kết động học

Ta dùng phương pháp hình học lượng giác để tìm động học ngược một số cánh
tay điển hình.

Ví dụ 2.23: Cánh tay quay hai bậc tự do

Hình 2.27 Cánh tay hai bậc tự do quay

Dùng định lý cosine cho tam giác OAB

r 2  s 2  a12  a22
cos 2   D,
2a1a2 (2.53)
2  acosd( D)

Xét tam giác OBC

OC  a1  a2 cos 2  a1  a2 D,
BC  a2 sin 2   a2 1  D2 ,
  atan2d(BC,OC) = atan2d(a2 1  D2 , a1  a2 D) (2.54)

Xét tam giác OBD

  atan2d(s, r ),

Suy ra

56
ĐỘNG HỌC THUẬN VÀ NGƯỢC ROBOT NỐI TIẾP

1      atan2d(s, r )  atan2d(a2 1  D2 , a1  a2 D) (2.55)

Ta có hai cấu hình cho cánh tay.

Với a1=150mm, a2=150mm, r=150mm, s=200mm suy ra
θ1=19.57o , θ2=67.11o, θ1=86.7o, θ2=-67.11o
r=150;s=200;a1=150;a2=150;
D=(r*r+s*s-a1*a1-a2*a2)/(2*a1*a2)
D1=sqrt(1-D*D)
theta21=acosd(D)
theta22=-acosd(D)
alpha=atan2d(a2*D1,a1+a2*D)
phi=atan2d(s,r)
theta11=phi-alpha
theta12=phi+alpha
%recheck
x1=a1*cosd(theta11)+a2*cosd(theta11+theta21)
y1=a1*sind(theta11)+a2*sind(theta11+theta21)
x2=a1*cosd(theta12)+a2*cosd(theta12+theta22)
y2=a1*sind(theta12)+a2*sind(theta12+theta22)

Ví dụ 2.24: Cánh tay quay ba bậc tự do

Hình 2.28 Cánh tay ba bậc tự do quay

Ta tìm tọa độ điểm B (pxe-a3sinβ, pye-a3cosβ) sau đó tính tương tự Ví dụ 2.1 tìm
θ1, θ2, sau cùng θ3= β-( θ1+θ2).

Ví dụ 2.25: Động học ngược cánh tay khuỷu ba bậc tự do (Hình 2.29)
Cho trước tọa độ tâm cổ tay là (xc, yc, zc) ta phải tìm ba góc θ. Từ Hình 2.29 ta
tính được:

1  atan2d( yc , xc ),  180o  1  180o (2.56)

với xc, yc không đồng thời là 0. Nếu xc, yc đồng thời là 0 thì θ1 không xác định (vị trí
bất thường).

57
 CHƯƠNG 2

Hình 2.29 Động học ngược cánh tay khuỷu

Trường hợp robot có vai bề rộng d (Hình 2. 30) ta có hai cấu hình cánh tay bên
trái và cánh tay bên phải.

Hình 2. 30 Cấu hình Robot có vai rộng

Với cấu hình cánh tay trái ta có tập các phương trình sau đây để tính θ1

  atan2d( yc , xc ),
  atan2d(d , r '2  d 2 )  atan2d(d , xc2  yc2  d 2 ), (2.57)
1    

Với cấu hình cánh tay phải ta có tập các phương trình sau:

58
ĐỘNG HỌC THUẬN VÀ NGƯỢC ROBOT NỐI TIẾP

  atan2d( yc , xc ),
  atan2d(d , r '2  d 2 )  atan2d(d , xc2  yc2  d 2 ),
(2.58)
    180o ,
1    

Chú ý là tùy theo kết cấu cơ khí, robot thường chỉ có một cấu hình.
Bây giờ ta tính θ2 và θ3, có hai cấu hình là khuỷu trên và khuỷu dưới (tuy nhiên
kết cấu cơ khí thường là khuỷu trên).
r 2  s 2  a22  a32 ( xc  yc  d 2  a1 )  ( zc  d1 )  a2  a3
2 2 2 2 2 2 2
cos 3    D,
2a2 a3 2a2 a3 (2.59)
3  acosd( D)  atan2d( 1  D 2 , D)

2  atan2d(s, r )  atan2d(a3 1  D2 , a2  a3 D) (2.60)

Như vậy robot khuỷu có bốn cấu hình, tùy theo kết cấu cơ khí ta chọn một cấu
hình làm lời giải (Hình 2.32).

Ví dụ 2.26: Tìm các góc θ cho robot Scorbot (Ví dụ 2.18) gắn một cây viết, tọa
độ đầu viết, hướng cây viết như sau
pt nt st at l β
[250 250 10] [cos(45 ) sin(45 ) 0] [-sin(45 ) cos(45 ) 0] [0 0 -1] 100mm 90o
o o o o

Giải: Đầu tiên ta tìm ma trận T05  T0v (T5v )1

 2/2  2/2 0 250  0 0 1 100 
   0 0 
T0  
2/2 2/2 0 250  v 0 1
v
, T5  ,
 0 0 1 10  1 0 0 0 
  
 0 0 0 1  0 0 0 1 
 0 0.7071 0.7071 250 
 0 0.7071 0.7071 250 
Suy ra T05   
 1 0 0 110 
 
0 0 0 1 
Từ (2.48)
c1c234c5  s1s5 c1c234 s5  s1c5 c1s234 c1 (d5 s234  a3c23  a2c2  a1 ) 
s c c  c s  s1c234 s5  c1c5 s1s234 s1 (d5 s234  a3c23  a2c2  a1 ) 
T0   1 234 5 1 5
5
 s234c5  s234 s5 c234 d5c234  a3 s23  a2 s2  d1 
 
 0 0 0 1 

59
 CHƯƠNG 2

Hình 2.31 Cấu hình khuỷu tay

Hình 2.32 Bốn cấu hình của cánh tay khuỷu

Hình 2.33 Tính động học ngược robot Scorbot

Biết pex, pey ta suy ra θ1 dùng (2.52) , α = θ2+θ3+θ4=atan2d(-az, ax/cos θ1), tọa độ
tâm cổ tay w trong mặt phẳng z0O0R là wz= pez +d5 cos α=, wR= pex2  pey2  d5 sin  ,
60
ĐỘNG HỌC THUẬN VÀ NGƯỢC ROBOT NỐI TIẾP
ta tính tiếp θ3 θ2 dùng (2.54) (2.55), sau cùng θ4= α- θ2-θ3 . θ5 suy từ phần tử (3,2) và
(3,1) của T05 Tổng hợp các phương trình như sau:

1  atan2d( pey , pex )  450 ,

α = θ2+θ3+θ4=atan2d(-az, ax/cos θ1)=900,

wz  pez  d5 cos   110,

wR  pex2  pey2  d5 sin   250 2  137.35  216.25,
r  wR  a1  216.25  101.25  115,
s  wz  d1  110  334.25  224.25
r 2  s 2  a22  a32
cos 3   D  0.344,
2a2 a3

3  atan2d( 1  D2 , D)  110012

Trường hợp θ3=110o12

2  atan2d( s, r )  atan2d(a3 1  D 2 , a2  a3 D)
 atan2d(224.25,115)  atan2d(220* 1  0.3442 , 220  220*0.344)
 117.9o

Trường hợp θ3=-110o12, θ2=-7.8o

4    2  3  90o  2  3
Trường hợp 1: θ4= 90o+117.9o-110.12o=97.98o
Trường hợp 2: θ4= 90o+7.8o+110.12o=207.92o
Cuối cùng θ5=0o

Ví dụ 2.27: Cánh tay khuỷu 6 bậc tự do quay (

Hình 2.21)
Từ các phương trình (2.41) và (2.44) ta suy ra các ma trận quay

c1c23 c1s23 s1  c4c5c6  s4 s6 c4c5 s6  s4c6 c4 s5 

R   s1c23
3
0  s1s23 c1  , R3   s4c5c6  c4 s6
 6
 s4c5 s6  c4c6 s4 s5 
 s23 c23 0    s5c6 s5 s6 c5 
n s a pe   R06 pe 
Cho T06     ta tính pw=pe-d6a, từ đó tính ra θ1 θ2 θ3
0 0 0 1   0 1
1  atan2d( pwy , pwx ), (2.61)

61
 CHƯƠNG 2

r 2  s 2  a22  a32 pwx  pwy  ( pwz  d1 )  a2  a3

2 2 2 2 2
cos 3    D,
2a2 a3 2a2 a3 (2.62)
3  acosd( D)  atan2d( 1  D 2 , D)
2
pwx  pwy
2
 ( pwz  d1 )2  (a2  a3 )2 (2.63)

2  atan2d(s, r )  atan2d(a3 1  D2 , a2  a3 D) (2.64)

Biết θ1 θ2 θ3 ta tính ra R03 và ( R03 )1  ( R03 )T ta được phương trình R36  ( R03 )T R06
, vế trái chứa các ẩn số θ4 θ5 θ6. Gọi rij phần tử hàng i cột j của R06 , sau khi nhân ma
trận, ta được phương trình tương ứng cột 3 của R36 là:
c4 s5  c1c23r13  s1c23r23  c23r33 ,
s4 s5  c1s23r13  s1s23r23  c23r33 ,
c5  s1r13  c1r23

Phương trình tương ứng hàng 3 của R36 là:

s5c6  s1r11  c1r21 ,

s5 s6  s1r12  c1r22

Suy ra

6  atan2d( s1r12  c1r22 , c1r21  s1r11 ) (2.65)

5  acosd( s1r13  c1r23 )
(2.66)
 atan2d( 1  ( s1r13  c1r23 )2 , s1r13  c1r23 ),
4  atan2d(c1s23r13  s1s23r23  c23r33 , c1c23r13  s1c23r23  s23r33 ) (2.67)

Chương trình sau đây cho một lời giải của động học ngược
clc
% Calculates the Inverse Kinematic of an Anthropomorphic arm with 6
DOF.
% 'q' is the solutions in degree and K is the Forward Kinematic
matrix.
% Position and orientation of end-effector
K=[0 0 1 0.200;0 1 0 0.200; 1 0 0 0.100]
% Denavit-Hartenberg's Parameters
a2=0.3; % [m]
a3=0.3;
d1=0.5;
d6=0.1;
R=K(1:3,1:3);
% Inverse Kinematic
pe=K(1:3,4); % End-effector's position
pw=pe-d6*a; % Wrist's position
% Vector's components that representes the wrist's position
pwx=pw(1);
62
ĐỘNG HỌC THUẬN VÀ NGƯỢC ROBOT NỐI TIẾP
pwy=pw(2);
pwz=pw(3);
%Calculate first three joint variables
s=pwz-d1
r=sqrt(pwx^2+pwy^2)
c3=(r^2+s^2-a2^2-a3^2)/(2*a2*a3) % cos(theta3)
s3=-sqrt(1-c3^2) % sin(theta3)
t1=atan2d(pwy,pwx);
t3=atan2d(s3,c3);
t2=atan2d(s,r)-atan2d(a3*s3,a2+a3*c3);
% Orientation Matrix of the spherical Wrist
R03=[cosd(t1)*cosd(t2+t3) -cosd(t1)*sind(t2+t3) sind(t1);
sind(t1)*cosd(t2+t3) -sind(t1)*sind(t2+t3) -cosd(t1);
sind(t2+t3) cosd(t2+t3) 0];

R36=R03'*R;% Matrix for the Euler's angle of spherical wrist

% Inverse kinematic for the spherical wrist

t4=atan2d(R36(2,3),R36(1,3));
t5=acosd(R36(3,3));
t6=atan2d(R36(3,2),R36(3,1));
% Solutions in degree
q=[t1 t2 t3 t4 t5 t6]'

2.4 HIỆU CHỈNH (CALIBRATION)

Ma trận động học thuận cho hướng và vị trí đầu cuối (tư thế, pose), tuy nhiên do
sai số đo các thông số a, d, α, θ nên kết quả sẽ không chính xác, do đó cần hiệu chỉnh
các thông số để giảm sai số. Qúa trình hiệu chỉnh được thực hiện bởi nhà sản xuất
trước khi xuất xưởng và giá trị đã hiệu chỉnh được cài vào chương trình hoạt động của
robot. Muốn hiệu chỉnh ta cần máy đo laser 3D chính xác để đo tư thế. Đặt các vector
thông số DH sau a=[a1 a2 … an]T, d=[d1 d2 … dn]T, α=[α1 α 2 … α n]T, θ=[θ1 θ2 … θn]T,
tư thế của robot có thể viết dưới dạng

xe  f6 x1 (a, , d , ) (2.68)

Cho các thông số giá trị định mức, tư thế của robot là xen Thực tế ta đo được xem ,
sai số x = xem  xen cần được cực tiểu bằng cách chỉnh lại các thông số. Tuyến tính
hóa quanh giá trị định mức

f f f f
x6 x1  a    d   (2.69)
a  a 
T
Đặt   aT T dT T  là vector cột 4nx1,    m   n là sai lệch của
thông số ,   f / a f /  f / d f /  , ta có phương trình

x  6x4n ( n ) 4nx1 (2.70)

63
 CHƯƠNG 2
Ta sẽ hiệu chỉnh với l tư thế và được kết quả sau

 x1   1 
 x   
x   2    2     (2.71)
 :   : 
   
 xl    l 

Giải phương trình dùng phương pháp cực tiểu bình phương ta tính được

  (T  1 T x (2.72)

Cập nhật thông số  u   n   , sau đó ta có thể hiệu chỉnh lại với bộ thông số
mới cho đến khi  hội tụ về một giá trị ngưỡng.
Khi robot hoạt động ta cần phải đưa robot về tư thế Home, các biến khớp được
tính bắt đầu từ tư thế này (bộ đếm góc quay của encoder đưa về 0), nếu không đưa về
tư thế Home chính xác sẽ gây ra sai số khi robot làm việc do offset của các biến khớp.

KẾT LUẬN

Chương hai đã khảo sát các phương pháp tính động học thuận và ngược của
robot nối tiếp, đây là bài toán cơ bản khi khảo sát robot, chúng ta cũng đã lập trình
Matlab để giải một số bài toán cụ thể, kiến thức thu được từ chương này sẽ được sử
dụng trong các chương sau. Người đọc nên nghiên cứu cách viết chương trình C để
giải các bài toán trong chương này song song với sử dụng Matlab.

BÀI TẬP

BT1 Cho hai hệ tọa độ 1(Oxyz) và 2 (Ouvw) hệ tọa độ 2 suy từ hệ tọa độ 1 bởi phép
quay quanh trục z1 60o. Tính tọa độ điểm A trong hệ 2 biết tọa độ trong hệ 1 là (1, 2,3)
BT2 Cho vector P1=[ 0 2 3]T, quay vector này quanh trục z góc 30o và tịnh tiến theo
trục x 2 đơn vị , tìm thành phần vector mới P2.
BT3 Cho hệ trục B suy từ hệ trục A bởi phép quay quanh trục zA 45o, tịnh tiến theo
truc xA 3 đơn vị và theo trục zA 1 đơn vị, tính ma trận TAB , TBA
BT4 Giải bài toán động học thuận và ngược cho robot trụ Hình BT4

64
ĐỘNG HỌC THUẬN VÀ NGƯỢC ROBOT NỐI TIẾP

Hình BT4
BT5 Giải bài toán động học thuận và ngược cho robot cartesian Hình BT5

Hình BT5
BT6. Giải bài toán động học thuận và ngược cho robot cartesian với bàn tay khớp cầu
Hình BT6

Hình BT6
BT7 Giải bài toán động học thuận và ngược cho robot RP Hình BT7

65
 CHƯƠNG 2

Hình BT7

BT8. Giải bài toán động học thuận và ngược cho robot RPR Hình BT8

Hình BT8
BT9 Giải bài toán động học thuận và ngược cho robot PPP Hình BT9

Hình BT9

66