CHƯƠNG 3

PHƯƠNG TRÌNH ĐỘNG LỰC HỌC ROBOT

3.1. Các khái niệm cơ bản
Bài toán động lực học Robot chính là phân tích, tổng hợp quá trình chuyển động của
các khớp và mô tả mối quan hệ giữa lực, mômen, năng lượng,… của các khớp với vị trí,
tốc độ, gia tốc được biểu diễn trong phương trình chuyển động. Trong phương trình động
lực học, lực và mômen là các tín hiệu vào. Dựa vào phương trình động lực học, sẽ tính
được lực, mômen cần thiết để khớp Robot có thể chuyển động được với tốc độ và gia tốc
mong muốn. Nghiên cứu động lực học Robot phục vụ cho các mục đích: Mô phỏng hoạt
động các khớp của Robot để khảo sát, thử nghiệm quá trình làm việc đa dạng mà không
cần sử dụng Robot thật, phân tích, tính toán kết cấu các khớp của Robot và thiết kế hệ
thống điều khiển tay máy.
Một số khái niệm cơ bản sau:
- Các biến khớp được mô tả bởi vector q   1 2  trong không gian khớp và được mô
T

tả bởi vector X   x y  trong không gian làm việc.

T

- Phương trình chuyển động của TM trong không gian khớp là hàm của các biến khớp có
dạng như sau: F / M  f (q, q , q )
- Có rất nhiều phương pháp để thành lập phương trình động lực học nhưng thường gặp
hơn cả là phương pháp Newton – Euler dựa trên sự cân bằng lực, mômen trên từng thanh
nối và phương pháp Euler – Lagrange dựa trên sự cân bằng về năng lượng. Cả hai phương
pháp đều cùng cho một kết quả như nhau, trong luận án trình bày phương pháp Euler –
Lagrange theo tài liệu để áp dụng vào đối tượng điều khiển nghiên cứu. Phương trình
chuyển động Lagrange của Robot nhiều bậc tự do đủ cơ cấu chấp hành ở hệ toạ độ khớp
là phương trình vi phân cấp hai phi tuyến biểu diễn quan hệ giữa mômen  và góc quay q,
tốc độ q , gia tốc , q .

- Đối với một TM có n khâu, ta có: n

K   Ki
 Động năng của thanh nối thứ i, được xác định: i 1
n
P   Pi
 Thế năng của thanh nối thứ i, được xác định: i 1

- Ki và Pi đều là hàm số phụ thuộc biến khớp i: K i =K(qi ,q i ) và Pi =P(q i ,q i )

3.2. Mô hình động lực học

34
 Mô hình động lực học là phương trình chuyển động của Robot trong không gian. Nó
thường bao gồm các thành phần như: thành phần quán tính, thành phần tương hỗ, thành
phần ly tâm và thành phần trọng trường. Ngoài ra khi Robot chuyển động luôn chịu tác
động bởi lực ma sát và có cả nhiễu tác động. Do đó mà phương trình động lực học của
Robot được viết dưới dạng tổng quát như sau:
τ = M  q  .q
 + H(q,q)
 + G(q) + Fd (q)
 + Fs (q)
 + τd (3.1)

Trong đó:
: vectơ nx1 mômen sinh ra bởi cơ cấu chấp hành đặt lên khớp
 (t) = [1(t), 2(t),..., n(t) ]T
q: vectơ nx1 các biến khớp,
q(t) = [q1(t), q2(t),..., qn(t) ]T
q (t) : vectơ nx1 tốc độ thay đổi các biến khớp (vận tốc góc, vận tốc thẳng),
 t ) =  q 1 (t), q 2 (t).... q n (t)  T ,
q(

 t ) :
q( vectơ nx1 gia tốc biến khớp,

 t ) =  q1 (t), q 2 (t).... q

 n (t) 
T
q( ,

M(q): ma trận quán tính (nxn) trong hệ toạ độ khớp có các thành phần (i,j),  :
H(q,q)

vectơ tương hỗ và ly tâm (nx1),

 =  h1,h 2 ....h n 
T
H(q,q)
G(q): vectơ trọng trường (nx1).
G = [G1,G2, .... Gn]T
Fd: vectơ lực ma sát động,
Fs : vectơ nx1 hằng số ma sát nhớt,
d : vectơ nx1 mô tả nhiễu ngoại chưa biết.
Phương trình (3.9) viết lại, được mô tả bởi:
τ = M(q)q
 + N(q,q)
 + τd (3.2)
trong đó:
N  q,q  = H(q,q)
 + G(q) + F(q)
 (3.3)
F(q)
 = Fd (q)
 + Fs (q)
 (3.4)

Phương trình động lực học được viết lại theo đặc điểm của phương pháp thích nghi
Craig (1985), được xác định:
35
 τ = M(q)q
 + H(q,q)
 + G(q) + Fs (q)
 + Fd (q)
 =
(3.5)
= M(q)q  = Y  q,q,q
 + N(q, q)    p
Y(q, q,q)
  - ma trận (nxr) biến đổi theo t đã biết, có thể xác định được từ các đại lượng
đo được,
ｐ - vector rx1 gồm các tham số hằng số hoặc thay đổi cần xác định.
Đặc điểm của phương pháp thích nghi Craig (1985), theo tài liệu [29] là công thức được
mô tả trong bảng phân tích thông số chưa biết và hàm truyền thời gian. Lý do của việc
phân tích động lực học Robot có thể phân tích trong dạng này là động lực học robot
tuyến tính trong bảng thông số mô tả bởi vectơ ｐ. Trong phần các thông số chưa biết và
các hàm truyền thời gian sẽ được sử dụng trong công thức của luật cập nhật thích nghi
cũng để phân tích sự ổn định sai số bám của hệ thống.
3.3. Thuộc tính của phương trình động lực học
- Ma trận quán tính M(q): là ma trận đối xứng và xác định dương, có các đặc tính
sau:
Động năng của Robot được xác định từ ma trận quán tính:
1 T
K= q M(q)q (3.6)
2
Ma trận quán tính là hàm của biến khớp q, nếu là khớp quay thì biến khớp q xuất
hiện trong ma trận quán tính sẽ là cos(q) và sin(q) mà 1  sin(q)  1 và 1  cos(q)  1 , do
đó M(q) cũng bị chặn bởi:
μ1I  M(q)  μ 2 I (3.7)
trong đó:
μ1 ,μ 2 - thành phần của ma trận bị chặn có thể tính được và được dùng để xác định
giá trị riêng lớn nhất và nhỏ nhất của M(q).
μ1I  M(q)  (M(q)  μ1I)  0
(3.8)
 xT (M(q)  μ1I)x  0 x  R n
Mặt khác nếu là khớp tịnh tiến thì do kết cấu cơ khí và do yêu cầu công nghệ thì
khoảng tịnh tiến d cũng bị chặn bởi:
m1  M(q)  m 2 (3.9)
trong đó: m1, m2 được xác định trong từng trường hợp cụ thể phụ thuộc vào kết cấu cơ khí
và yêu cầu công nghệ.
- Thành phần tương hỗ và thành phần ly tâm H(q, q ):

36
 H(q, q ) biểu thị các quan hệ giữa lực động với tốc độ thay đổi giá trị biến khớp. Thành
phần tương hỗ và ly tâm có thể được viết dưới dạng sau:
H(q,q)
 = C(q,q)q
  (3.10)

1 
H(q, q)
 = [ M(q) + S(q,q)]q
 
2
trong đó:
1
S(q, q ): ma trận đối xứng lệch, xác định bởi: S = (C  CT ),S = - ST
2
H(q, q ) là hàm bậc hai của đạo hàm các biến khớp. Nếu khớp của Robot là khớp
quay thì trong H(q, q ) có hàm sin(q) và cos(q) do đó H(q, q ) cũng bị chặn. Nếu khớp của
Robot là khớp tịnh tiến thì cũng do kết cấu cơ khí và yêu cầu công nghệ nên H(q, q )
cũng bị chặn được xác định bởi:
2
  v b (q) q
H(q,q) (3.11)
trong đó: vb(q) – một hàm vô hướng của q
2
hoặc: H(q,q)
  H (q) q

H(q) được xác định như sau:

q T H1 (q)q   H1 (q) 
 T   H (q) 
H(q,q)
q H 2 (q)q 
 

 
  q
T
 
 
 2  q = q T H(q)q (3.12)
   
q T H (q)q   H n (q) 
 n 

Để xác định được v b với giả thiết là hàm thỏa mãn với điều kiện: v b = sup
q
H(q,q)


- Thành phần trọng trường G(q):

Robot luôn chịu tác dụng của lực trọng trường và phụ thuộc vào biến khớp q. Và
G(q) cũng bị chặn được xác định bởi:
G(q)  g b (q) (3.13)
Trong đó: gb là hàm vô hướng được xác định cụ thể cho từng robot.
- Thành phần lực ma sát F( q ):
Thành phần ma sát xuất hiện khi Robot chuyển động và gồm hai thành phần là lực ma
sát nhớt và lực ma sát động. Và các hệ số ma sát này xác định rất khó khăn nên người ta
thường đưa ra dưới dạng gần đúng như sau:
F(q)
 = Fdq + Fs (q)
 (3.14)

37
 Ma sát nhớt thường được giả định: Fd q  vec  vi q i 
vi : - hệ số không đổi,
Fd  diag  vi  : - ma trận đường chéo
 = vec  k isgn(q i )
Ma sát động thường được giả định: Fs (q)
ki: - hệ số không đổi,
Hàm dấu sgn được xác định:
+1 x>0
sgn(x) = 
-1 x<0

 Fs (q)
 = K ssgn(q )

Hệ số ma sát động xác định: K d = diag  x i 

Hàm dấu xác định: sgn(x) = vec  sgn(x i )
Vậy các điều kiện ràng buộc của các ma sát:
  v q +k
F s q + F d (q) (3.15)
Thành phần ma sát tĩnh có thể có trong F(q)
 được xác định:

Fsisgn(q i )  k si k i  exp  - q i /ε   (3.16)

- Thành phần nhiễu τ d là véctơ nx1 mô tả nhiễu ngoại chưa biết được xác định: d  d ,
d: hàm vô hướng không đổi, được tính trong trường hợp Robot cụ thể.
- Các tham số động lực học: Các tham số hệ thống như khối lượng mỗi khớp m i, và
mômen quán tính In xuất hiện trong phương trình động lực học của Robot.
p =  m1 ,m 2 ,...,m n , I1 , I 2 ,...,I n 
T
(3.17)

Nhận xét : Từ phương trình động lực học tổng quát và thuộc tính của phương trình động

lực học của Robot cho thấy : mô hình động lực học của Robot công nghiệp là mô hình

phi tuyến có các tham số hằng bất định, có tác động thành phần tác độngxen kênh giữa các

khớp, tất cả các yếu tố ảnh hưởng đó làm cho chuyển động của TMCN mất ổn định và tồn

tại sai lệch lớn giữa vị trí khớp đặt và vị trí khớp thực.
3.4. Cách thành lập phương trình động lực học
Mômen đặt lên trục khớp i được xác định theo phương trình Largange bậc hai có dạng:
 Khớp i là khớp tịnh tiến ri , lực đặt theo chuyển động của khớp Fi

38
 d L L
F   (3.18)
i dt r r
i i
 Khớp i là khớp quay θi , momen đặt xung quanh khớp Mi

d  L  L
τ =   (3.19)
i dt  q  q
 i  i

Trong đó:
L = K – P: Phương trình Largange (3.20)
K: Tổng động năng của thanh nối,
P: Tổng thế năng của thanh nối,
i : Moment đặt lên khớp i khi thực hiện chuyển động quay, (nx1)
qi, : Biến khớp,
q i : Vận tốc quay của biến khớp.

Thiết lập phương trình động lực học thực hiện các bước sau:
Bước 1: Tính động năng của các thanh nối K i được xác định bởi:
1 1
K  vT
c m vc  w T I w (3.21)
i 2 i i i 2 i i i

Trong đó:
ci :Trọng tâm thanh nối
li : Chiều dài thanh nối
mi : Khối lượng thanh nối
si : Khoảng cách từ trục quay đến ci
vci :Tốc độ dài của ci
wi : Tốc độ góc thanh nối
Ii : Ma trận momen quán tính, được xác
định:
Hình 3.1. Biểu diễn động năng của một thanh nối
 I xx  I yx  I zx 
 
Ii   I yz I yy I zy 
  I xz I yz I yy  (3.22)

I xx   ( y 2  z 2 )pdv I xy  I xy   xypdv
V V

I yy   ( x  z )pdv
2 2
I xz  I zx   zxpdv 39
V V

I zz   ( x  y )pdv
2 2
I yz  I zy   zypdv
V V
Bước 2: Tính thế năng của các thanh nối Pi , được xác định bởi:
Pi   mi G iT 0 Pc i

T
0
Pc   x c
i i
yc i
z c 
i
(3.23)
G iT   0 0 1
T

Bước 3: Thiết lập hàm Lagrange (3.20)

Bước 4: Thiết lập phương trình Lagrange (3.18) hoặc (3.19).

3.5. Ví dụ minh họa
Xây dựng phương trình động học thuận và động lực học cho Robot Almega 16.

- Thiết lập bài toán động học thuận cho Robot Almega 16

a2 a3
Y1 lC2 Y2 lC3 Y3
2 3
C2
O1 X1 C1 O2
X2 X3
O3
Z1
C0 Z2 Z3
d1 Z0
lC1 Y0
1
O0 X0

Hình 3.2. Robot Almega16 có gắn các hệ trục tọa độ

Các bước thực hiện bài toán động học thuận cho robot Almega 16:

40
  Xác định số khớp và số thanh nối
3 khớp đầu cảu robot Almega 16 là 3 khớp quay với 4 thanh nối và đế
 Gắn lên các thanh nối các hệ trục tọa độ như hình vẽ trên.
Các thông số động học được xác định theo phương pháp Danevit-Hartenberg theo bảng
sau:

θi di ai αi
1 θ* 1 d1 0 900
2 θ* 2 0 a2 0
3 θ* 3 0 a3 0

Từ bảng thông số D – H trên ta tính được các Ai như sau:

 cos1 0 sin 1 0 
 sin 1 0 cos1 0 
A1   (3.24)
0 1 0 d1 
 
 0 0 0 1

 cos2  sin 2 0 a 2cos2 

 sin 2 cos2 0 a 2 sin 2 
A2  
0 
(3.25)
0 0 1
 
 0 0 0 1 

cos3  sin 3 0 a 3cos3 

sin 3 cos3 0 a 3 sin 3 
A3  
0 0 1 0 
 
 0 0 0 1 
(3.26)
Nhân các ma trận A1 , A 2 , A3 ta được ma trận biểu diễn vị trí và hướng của khâu tác
động cuối của Robot như sau:
0
T3  A1A 2 A3
cos 1 cos 23  cos 1 sin 23 sin 1 cos 1 (cos 23a 3  cos 2a 2 ) 
 sin  cos   sin 1 sin 23 cos1 sin 1 (cos 23a 3  cos 2a 2 ) 
1 23
T3    (3.27)
 sin  23 cos23 0 sin 23a 3  sin 2a 2  d1 
 0 0 0 1 

Trong đó: cosθ23 = cos(θ2 + θ3)

41
- Thiết lập mô hình động lực học cho Robot ALMEGA 16
Bước 1: Tính động năng của các thanh nối

 Tính động năng thanh nối thứ nhất

Xc =0 X  =0
c
 1  2 1
Pc1 Yc1 =0  Y  =0  V 2 0  K1 I112
c2 c1 2 (3.28)
 Z =l 
 c1 c1  Zc2 =0

 Tính động năng thanh nối thứ hai

Xc = lc cos 2 sin 1 X c = -lc sin 2 sin 1 2  lc cos 2 cos 1 1
 2 2
 2 2 2
Pc2 Y
 c2 c2
= l cos  2 cos 1   c2
Y = -l c2 sin  2 cos  

1 2  l c2 cos 2 sin 11

 Z = d  l sin   Z = l cos  
 c2 1 c2 2  c2 c2 2 2

X c2 = lc2 sin 22 sin 12 22  lc2 cos 22 cos 12 12  2lc2 sin 2 sin 1 cos 2 cos 1 1 2
 22 2 2 2 2 2
2
2 2 2 2
2
2
 Yc2 = lc2 sin 2 cos 1 2  lc2 cos 2 sin 1 1  2lc2 sin 2 cos 1 cos 2 sin 1 1 2

 2 2 2 2
 Zc2 = lc2 cos 2 v2

2  2 Y
 2  Z 2 = l2 ( 2  cos2  2 )
 Vc  X c c c c 2 2 1
2 2 2 2 2

1 2 2 2 2 1 2
 K2 = m 2lc2 ( 2  cos 2 1 )  I 2  2 (3.29)
2 2
 Tính động năng thanh nối thứ ba
X c  cos1 cos(2  3 )lc  cos 2a 2 
 3  3 
Pc3 Yc3  sin 1 cos(2  3 )lc3  cos 2 a 2 
 Z  sin(   )l  sin  a  d
 c3 2 3 c3 2 2 1

2  Y 2
  Z 2 2
 Vc3  X c3 c3 c3
2 2 2 2 2 2
  1  cos(2  3 )lc3  cos 2a 2   ( 2   3 ) lc3   2a 2  2 cos 3 ( 2   3 ) 2lc3 a 2
1

m3  12 cos(2  3 )lc3  cos 2a 2 
2 2 2 2 2
 K3 =  ( 2   3 ) lc3   2a 2 
2 
1 2
 2 cos 3 ( 2   3 ) 2lc3 a 2   I3 ( 2   3 ) (3.30)
2

42
Bước 2: Tính thế năng của các

 Tính thế năng thanh nối thứ nhất:

0
P1   m1  0 0 1 g  0   m1gl c1 (3.31)
 
 lc1 
 Tính thế năng thanh nối thứ hai:
 lc2 cos 2 sin 1 
 
P2  m 2  0 0 1 g l c2 cos 2 cos 1   m 2g  d1  l c2 sin  2  (3.32)
 d1  lc sin 2 
 2 
 Tính thế năng thanh nối thứ ba
 cos 1  cos(2  3 )lc  cos 2a 2  
  3 
P3   m3  0 0 1 g  sin 1  cos(2  3 )lc3  cos  2a 2  
 
 sin(  2   3 )l c 3
 sin  2 a 2  d1 


 m3g sin(2  3 )lc3  sin 2a 2  d1 
(3.33)
Bước 3: Thiết lập hàm Lagrange
Phương trình Lagrange như sau:
L  (K1  K 2  K 3 )  (P1  P2  P3 ) (3.34)

43
 1 2 1 1 2 1
I11  m 2lc2 ( 2  cos 2 1 )  I 2 2  m3  1 cos(2  3 )lc3  cos  2a 2  
2 2 2 2 2 2
L
2 2 2 2 
1
 ( 2   3 ) lc3   2a 2  2cos 3 ( 2   3 ) 2lc3 a 2   I3 ( 2   3 )  m1gl c1 
2 2 2 2
 2
  
 m 2g d1  lc2 sin 2  m3g sin(2  3 )l c3  sin 2a 2  d1 
1 2

  1  I1  m 2lc2 cos  2  m3 cos(2  3 )lc3  cos 2a 2  
2 2 2

2  
1 2
2 
2 2
  2
1 2
  2  m 2lc2  m3 lc3  a 2  2cos 3l c3 a 2  I 2  I3    3 m 3lc3  I 3 
2

2
 
2 3 3 
    m l  cos  l a  I   m gl  m g d  l sin 

2
c3 3 c3 2  3 1 c1 2  1 c2 2 

 m3g sin(2  3 )lc3  sin 2a 2  d1  (3.35)

Bước 4: Thiết lập phương trình Lagrange

Ta có phương trình:
d L L
  
dt d i i
Tính toán từng thành phần trong phương trình
d L
dt d 1

  1
 I  m l 2 cos 2  m cos(   )l  cos  a 2 
 1 2 c2 2 3 2 3 c3 
2 2 



  1 2  2 m 2lc2 cos 2 sin 2  2m3 cos(2  3 )lc3  cos 2a 2 sin(2  3 )l c3

2


 sin 2a 2     1 3 2m3 cos(2  3 )lc3  cos 2a 2 sin(2  3 )lc3 
d L
dt d2
 
2 2

 2  m 2lc2  m3 lc3  a 2  2 cos 3lc3 a 2  I2  I3  
2
 
  2

 3  m3 lc3  cos 3lc3 a 2  I3   2 2 3m3 sin 3l c3 a 2   3 m3lc3 sin 3a 2 (

2

d L
dt d 3
  
 2  m3  lc3  cos 3lc3 a 2  I3   3 m 3lc3  I3   2 3m3 sin 3lc3 a 2

2

2

L
 0
1
L
2

  1  m 2lc2 cos 2 sin 2  m3 cos(2  3 )l c3  cos  2a 2 sin(2  3 )lc3
2

 sin 2a 2    2m 2glc2 cos 2  m3g cos(2  3 )lc3  cos 2a 2  
44
L
3
2
 
  1 m3 sin(2  3 )lc3 cos(2  3 )lc3  cos 2a 2  sin 3 ( 2   3 ) 2 lc3 a 2

 m3g cos(2  3 )lc3

Như vậy xác định được:


1  1  I1  m 2lc22 cos 22  m 3 cos( 2  3 )l c3  cos  2a 2 
2
 


  1 2  2m 2lc22 cos 2 sin  2  2m 3 cos( 2  3 )l c3  cos  2a 2 sin( 2  3 )l c3
 

 sin 2a 2     1 3 2m3 cos(2  3 )lc3  cos 2a 2 sin( 2  3 )l c3  (3.36)

 
 2  2  m 2lc22  m3 lc23  a 22  2cos 3lc 3 a 2  I 2  I3   3  m3 lc23
   

 cos 3l c3 a 2  I3   2 2 3m 3 sin 3l c3 a 2   32m 3l c3 sin 3a 2



  12  m 2lc22 cos 2 sin 2 + m3 cos(2  3 )lc3  cos 2a 2 sin(2  3 )lc3 
 

 sin 2a 2    2m 2glc2 cos 2  m3g cos(2  3 )lc3  cos 2a 2  (3.37)

   
3  2  m3 lc3  cos 3lc3 a 2  I3   3 m3lc3  I3   2 3 m3 sin 3lc3 a 2

2

2

2
 2
  1 m 3 sin( 2  3 )lc3 cos(2  3 )lc3  cos 2a 2   2 m3 sin 3l c3 a 2  
  3 2 m3 sin 3lc3 a 2  m3g cos(2  3 )l c3 (3.38)

Phương trình động lực học dưới dạng ma trận trạng thái
  M(q,q,q)q
    H(q,q)
  G(q) (3.39)
 1   M11 M12 M13   q1   H11   G1 
   M M 22 M 23  q 2    H 21    G 2  (3.40)
 2   21      
 3   M31 M 32 M 33   q 3   H31   G 3 
Trong đó:

45
 M  I  m l2 c 2  m c l  c a 2
 11 1 2 c2 2 3 23 c3  2 2 
3 c3 
M  m l 2  m l2  a 2  2c l a  I  I
 22 2 c2 2 3 c3 2 2 3 

2
M 32  m3 lc3  c3lc3 a 2  I3 

2
M 23  m3 lc3  c3lc3 a 2  I3

 (3.41)
2
M 33  m3lc3  I3

M12  M 21  0
M  M  0
 13 31


 11 2 c2 2 2 3 23 c3 
H    2m l2 c s  m c l  c a s l  s a   
 2 2 23 c3 
2 2  1 2 

  
 2m3 c 23lc3  c 2a 2 s 23lc3  1 3
 2 (3.42)
H 21  2m3s3lc3 a 2 2 3  m 2lc2 c2s 2

   
 m3 c 23lc3  c 2a 2 s 23lc3  s 2a 2  12  m3lc3 s3a 2 32

 2 
H 31  m3s 23lc3 c 23lc3  c 2a 2 1  m3s3lc3 a 22
2

G11  0


G 21  2m 2lc 2 c 2g  m3 c23lc3  c 2a 2 g  (3.43)
G  m c l g
 31 3 23 c3

BÀI TẬP
Bài 1. Cho robot như sau, hãy thiết lập phương trình động lực học của Robot

4

r1

46
Bài 2. Cho một Robot có 5 khâu phẳng, cấu hình RRRRR. Hãy thiết lập phương trình
động lực học của Robot

Bài 3: Cho một Robot như hình vẽ. Hãy thiết lập phương trình động lực học của Robot

Bài 4: Cho một Robot như hình vẽ. Hãy thiết lập phương trình động lực học của Robot

Bài 5: Cho một Robot có 2 khâu phẳng, cấu hình TT. Hãy thiết lập phương trình động học
thuận của Robot

47
Bài 6: Cho một Robot có 2 khâu phẳng, cấu hình RT. Hãy thiết lập phương trình động lực
học của Robot

Bài 7: Cho một Robot có 3 khâu phẳng, cấu hình RTR. Hãy thiết lập phương trình động
lực học của Robot:

CHƯƠNG 4
THIẾT KẾ QUỸ ĐẠO CHUYỂN ĐỘNG ROBOT CÔNG NGHIỆP

4.1. Khái niệm chung

Đường di chuyển của TM biểu thị trình tự di chuyển của cơ cấu Robot từ điểm này
đến điểm khác trong không gian. Đường di chuyển đơn thuần là trình tự thay đổi cấu hình
Robot mà không bao hàm yếu tố thời gian. Còn quỹ đạo chuyển động biểu thị sự di
chuyển của cơ cấu Robot theo thời gian trong đoạn đường di chuyển. Như vậy quỹ đạo
chuyển động sẽ phụ thuộc vào tốc độ và gia tốc của cơ cấu TM.
Có 2 phương pháp mô tả chuyển động của TM là mô tả trong không gian khớp và
trong không gian tay (không gian làm việc hay còn gọi là không gian Đecac).

48
 Phương pháp mô tả trong không gian khớp, chuyển động của cơ cấu TM được xác
định thông qua các giá trị biến khớp. Đặc điểm của phương pháp mô tả trong không gian
khớp là có thể đảm bảo được tay TM đạt đến vị trí đặt trước một cách chính xác, nhưng
đường di chuyển giữa hai điểm không thể dự báo trước được.
Phương pháp mô tả trong không gian tay sẽ biểu diễn chuyển động của Robot giữa
các điểm bằng các đoạn di chuyển xác định trong không gian tay và được biến đổi về
không gian khớp tương ứng thông qua giải bài toán động học ngược. Giả sử để tay TM di
chuyển từ điểm A đến điểm B theo một đường thẳng, ta cần chia đoạn AB thành nhiều
đoạn nhỏ và điều khiển TM di chuyển bám theo các đoạn nhỏ đó. Phương pháp thực hiện
là tính toán ra tập hợp vị trí khớp tương ứng với các đoạn nhỏ và điều khiển khớp bám
theo các vị trí đặt tương ứng. Khi tất cả các đoạn di chuyển nhỏ được xác định, Robot sẽ
di chuyển từ A đến B theo đúng quỹ đạo thiết kế.
Cả 2 phương pháp thiết kế quỹ đạo trong không gian khớp và không gian tay đều
được ứng dụng điều khiển TM trong công nghiệp. Mỗi phương pháp đều có ưu điểm và
nhược điểm riêng. Phương pháp mô tả chuyển động trong không gian tay dễ dàng quan sát
được và đảm bảo tay Robot di chuyển đúng quỹ đạo, tuy nhiên nhược điểm của nó là khối
lượng tính toán lớn và yêu cầu thời gian xử lý tính toán nhanh.
4.2. Thiết kế quỹ đạo trong không gian khớp
Thông thường quỹ đạo ở dạng đa thức bậc cao sẽ đáp ứng được các yêu cầu về vị trí,
tốc độ và gia tốc ở mỗi điểm giữa hai đoạn di chuyển. Phần lý thuyết sau đây sẽ trình bày
một số phương pháp thiết kế quỹ đạo trong không gian khớp, xác định đường biểu diễn vị
trí khớp theo thời gian.
4.2.1. Quỹ đạo dạng đa thức bậc 3
Bài toán thiết kế quỹ đạo cho khớp ở đây là xácđịnh đường biểu diễn vị trí khớp (góc
quay hoặc độ dịch chuyển tịnh tiến) theo thời gian khi di chuyển từ vị trí ban đầu q 0 đến vị
trí cuối cùng qc trong thời gian tc, với q là biến khớp tổng quát. Quỹ đạo di chuyển sẽ thoả
mãn 4 điều kiện:
- Vị trí ban đầu.
- Vị trí cuối cùng.
- Tốc độ ban đầu.
- Tốc độ cuối cùng.
Sử dụng đa thức bậc 3 cho quỹ đạo của khớp:
q(t) = a0 + a1*t + a2*t2 + a3*t3 (4.1)
Các điều kiện ban đầu và cuối cùng với giả thiết tốc độ ban đầu và cuối bằng 0:

49
 q(t0) = q0
q(tc) = qc
q ( t 0 )  q 0  0 (4.2)
q (t C )  q C  0
Đạo hàm phương trình (4.1) ta nhận được phương trình tốc độ khớp theo thời gian:
q
 ( t ) = a1 + 2*a2*t + 3*a3*t2 (4.3)
Sử dụng các điều kiện đầu và cuối (4.2) kết hợp với (4.3) ta nhận được hệ phương
trình sau:
q(t0) = a0 = q0
q(tc) = a0 + a1*tc + a2*tc2 + a3*tc3 = qc (4.4)
q
 (t 0 ) = a1 = 0
q
 ( t C ) = a1 + 2*a2*tc + 3*a3*tc2 = 0
Giải hệ phương trình (4.4) thu được 4 hệ số a0, a1, a2, a3
Ví dụ: Thiết kế quỹ đạo chuyển động cho một khớp Robot quay từ góc ban đầu 30 0
đến vị trí cuối cùng 600 trong thời gian 5s.
Sử dụng hệ phương trình (4.4) ta có:
q(0) = a0 = q0 = 30
q(5) = a0 + a1*5 + a2*52 + a3*53 = 60
q
 ( t 0 ) = a1 = 0
q
 (t C ) = a1 + 2*a2*5 + 3*a3*52 = 0
Giải hệ trê thu được: a0 = 30; a1 = 0; a2 = 3.6; a3 = -0.48
Phương trình biểu diễn góc khớp, tốc độ góc và gia tốc góc lần lượt là:
q(t) = 30 + 3.6*t2 - 0.48*t3
q
 ( t ) = 7.2*t - 0.48*t2
q ( t ) = 7.2 - 2.88*t
Đoạn chương trình sau cho ta kết quả dưới dạng đồ thị trong Matlab:
>> t=0:0.01:5;
>> q=30+3.6*t.^2-0.48*t.^3;
>> dq=7.2*t-1.44*t.^2;
>> ddq=7.2-2.88*t;
>> figure(1);subplot(2,1,1);grid on; hold on;
>> xlabel('times-seconds');ylabel('rotation angle');
>> hold on;
>> plot(t,q);
50
 >> hold on;
>> subplot(2,1,2);grid on; hold on;
>> xlabel('times-seconds');
>> hold on;
>> plot(t,dq,t,ddq,'r');
60
rotation angle

50

40

30
0 0.5 1 1.5 2 2.5 3 3.5 4 4.5 5
times-seconds

10
dq
5 ddq

-5

-10
0 0.5 1 1.5 2 2.5 3 3.5 4 4.5 5
times-seconds

Hình 4.1. Đồ thị góc quay, vận tốc và gia tốc góc khớp theo thời gian

4.2.2. Thiết kế quỹ đạo theo đa thức bậc 5

Phương pháp thiết kế quỹ đạo sử dụng đa thức bậc 3 thoả mãn 4 điều kiện đầu và cuối về
vị trí và tốc độ. Để xét đến điều kiện đầu và cuối về gia tốc ta cần sử dụng đến quỹ đạo đa
thức bậc 5:
q(t) = a0 + a1*t + a2*t2 + a3*t3 + a4*t4 + a5*t5
q ( t ) = a1 + 2*a2*t + 3*a3*t2 + 4*a4*t3 + 5*a5*t4
q (t C ) = 2*a2 + 6*a3*t + 12*a4*t2 + 20*a5*t3
Hệ phương trình trên cho phép ta xác định được 6 hệ số của quỹ đạo dựa vào 6 điều kiện
biên.
4.2.3. Quỹ đạo dạng 2-1-2
Đây là dạng quỹ đạo chuyển động mà đồ thị tốc độ khớp có dạng hình thang với giả
thiết tốc độ điểm đầu và cuối bằng 0, gia tốc chuyển động là hằng số ở giai đoạn khởi
động và hãm. Để đảm bảo tính đối xứng của quỹ đạo, điểm trung bình được chọn:
qC  q0 t
qm = và tm = C
2 2
Quỹ đạo phải thoả mãn các điều kiện ràng buộc:
- Vị trí khớp biến đổi từ điểm đầu q0 tới điểm cuối qc trong thời gian tc
- Tốc độ cuối đoạn Parabol bằng tốc độ không đổi của đoạn tuyến tính
51
 q m  q1
q * t 1  (4.5)
t m  t1
Phương trình biểu diễn đoạn khởi động (0-t1) có dạng Parabol:
q(t) = q0 + 0.5* q 1 *t2
Do đó vị trí của khớp tại thời điểm t1 là:
q1(t1) = q0 + 0.5* q 1 *t12 (4.6)
Kết hợp (4.5) và (4.6) ta nhận được phương trình sau:
q 1 *t12 - q 1 *tc*t1 + qc - q0 = 0 (4.7)
Giải phương trình (4.7) tính được khoảng thời gian t1 như sau:
t c 1 t c * q 1  4 *  q c  q 0 
2

t1 =  * (4.8)
2 2 q 1
Từ (4.8) gia tốc khớp trong giai đoạn khởi động và hãm thoả mãn điều kiện:
t c * q 1  4 *  q c  q 0 
2
4 *  qc  q0 
 0  q 1  2 (4.9)
q 1 tc
Mặt khác gia tốc chuyển động phải nhỏ hơn gia tốc cho phép từ độ bền cơ khí nên
gia tốc được chọn phải thoả mãn bất đẳng thức:
4 * qc  q0 
2
 q 1  q CP (4.10)
tc
Như vậy từ (4.10) ta xác định được gia tốc khớp, từ (4.8) xác định được t1 và từ (4.6)
xác định được q1. Với các thông số đó, ta xây dựng được phương trình quỹ đạo chuyển
động của khớp Robot như sau:
 1
 q 0  * q 1 * t 2 (0  t  t 1 )
2
  t 
q(t )  q 0  q 1 * t 1 *  t  1   t1  t  t c  t1  (4.11)
  2
 q  1 * q *  t  t  2  t C  t1  t  t C 
 C 2 1 C

Đồ thị sau cho ta quỹ đạo dạng 2-1-2 với góc ban đầu là 30 0, góc cuối cùng là 600, thời
gian chuyển động là 5 (s), gia tốc giới hạn là 10 rad/s2. Chương trình được cho ở phụ lục.

52
 60

55

50

Goc quay
45

40

35

30
0 0.5 1 1.5 2 2.5 3 3.5 4 4.5 5
Thoi gian (s)

Hình 4.2. Đồ thị góc quay khớp theo thời gian

6
Van toc goc (rad/s)

0
0 0.5 1 1.5 2 2.5 3 3.5 4 4.5 5
Thoi gian (s)

Hình 4.3. Đồ thị vận tốc góc theo thời gian

53
 8

Gia toc goc (rad/s2)

2

-2

-4

-6

-8
0 0.5 1 1.5 2 2.5 3 3.5 4 4.5 5
Thoi gian (s)

Hình 4.4. Đồ thị gia tốc theo thời gian

4.2.4. Quỹ đạo dạng 4-1-4

Dạng quỹ đạo 2-1-2 đơn giản và dễ thực hiện, tuy nhiên tốc độ khớp ở thời điểm đầu
và cuối của giai đoạn khởi động và hãm thay đổi không được êm. Dạng quỹ đạo 4-1-4 sẽ
tạo ra sự thay đổi tốc độ êm hơn, trong đó quỹ đạo ở giai đoạn khởi động và hãm có dạng
đa thức bậc 4. Quỹ đạo chuyển động được xây dựng như sau :

Hình 4.5. Xây dựng quỹ đạo 4-1-4

54
 Thỏa mãn các điều kiện đầu và cuối về vị trí, tốc độ và gia tốc của khớp :

q (0)  q 0 q (t c )  q c
 
q (0)  q 0 q (t c )  q c
q (0)  q q (t )  q
 0  c c

Để đơn giản việc tính toán có thể chọn tham số t 0 bằng 1/2 khoảng thời gian gia tốc
hoặc giảm tốc và tốc độ, gia tốc ban đầu và cuối cùng bằng 0. Ta cũng xác định được các
điểm phụ của quỹ đạo chuyển động q01 và q02 tại t = t0 và t = tc - t0 là :
q01 =q0 và q02 = qc
Nối q01 và qc2 bằng 1 đường thẳng và xác định điểm q02, qc1 tại t = 2t0 và t = tc-2t0
Bằng cách chọn điểm phụ như trên, quỹ đạo đoạn cd là một đường thẳng với tốc độ
không đổi, quỹ đạo ac và df có thể chọn là đa thức bậc 4 có dạng :
q(t) = a0 + a1t + a2t2 + a3t3 + a4t4
* Xác định giá trị q02 và qc1 :
Do be là đường thẳng, giá trị q02 và qc1 có thể được xác định từ giá trị đầu q 0 và giá trị
cuối qc dựa trên phương trình bậc nhất của đường thẳng be :
qc  q0
q 02  q 0  t 0  q 0  At 0
t c  2t 0
qc  q0
q c1  q c  t 0  q c  At 0
t c  2t 0
qc  q0
Với A =
t c  2t 0
Hệ số góc của đường thẳng cd là tốc độ chuyển động khớp trong đoạn cd.
* Phương trình đoạn cd:
Quỹ đạo cd là đường thẳng biểu diện bởi phương trình :
qcd = A(t - 2t0) + q02
với  2t 0  t  t c  2t 0 
=> qcd = a0cd +a1cd(t - 2t0)
Cho các hệ số a0cd, a1cd của phương trình đoạn cd được xác đinh như sau :
a0cd = q0 +At0
a1cd = A
* Phương trình đoạn ac :
Quỹ đạo ac biểu diễn bởi phương trình dạng đa thức bậc 4 :
qac = a0ac + a1act + a2act2 + a3act3 + a4act4

55
 Các hệ số của phương trình đoạn ac là a 0ac, a1ac, a2ac, a3ac, a4ac được xác định từ điều
kiện đầu và điều kiện cuối :
Tại t = 0:
q ac (0)  q 0

 q ac (0)  0
 q (0)  0
 ac
Tại t = 2t0 :
q ac (2 t 0 )  q cd (2 t 0 )
q ac (2 t 0 )  q cd (2 t 0 )
Các hệ số được xác định như sau :
a0ac = q0
a1ac = 0
a2ac = 0
1
a 3 ac  A
4 t 20
1
a 4 ac   A
16 t 30

* Phương trình đoạn df :

Quỹ đạo đoạn df biểu diễn bởi phương trình bậc bốn như quỹ đạo đoạn ac :
q df  a 0 df  a 1df   a 2 df  2  a 3df  3  a 4 df  4

Với :   t   t c  2 t 0 
Các điều kiện đầu và cuối cùng cho tính toán các hệ số là :
q df (  0)  q dc (t c  2 t 0 )
q df (  0)  q dc (t c  2 t 0 )
q df (  0)  q dc (t c  2 t 0 )  0
q df (2 t 0 )  q c
q df (2 t 0 )  0
Sử dụng điều kiện đầu và cuối, tương tự như phương pháp tính phương trình đoạn
ac, các hệ số của phương trình qdf được xác định như sau :

56
 a 0 df  q c1
a 1df  A
a 2 df  0
a 1df  32a 4 df t 30
a 3df  
12t 02
3q c  3a 0 df  4a 1df t 0
a 4 df  
16t 04

* Đoạn chương trình khảo sát quỹ đạo 4-1-4 bằng Matlab với góc ban đầu là 30 0, góc
cuối cùng là 600, thời gian chuyển động là 5 (s) được cho ở phụ lục. Kết quả cho duới
dạng đồ thị dưới đây.
60

55

50
Goc quay

45

40

35

30
0 0.5 1 1.5 2 2.5 3 3.5 4 4.5 5
Thoi gian (s)

Hình 4.6. Đồ thị góc quay theo thời gian với quỹ đạo 4-1-4

10
Van toc goc (rad/s)

0
0 0.5 1 1.5 2 2.5 3 3.5 4 4.5 5
Thoi gian (s)

10
Gia toc goc (rad/s2)

-5

-10
0 0.5 1 1.5 2 2.5 3 3.5 4 4.5 5
Thoi gian (s)

Hình 4.7. Đồ thị vận tốc và gia tốc góc theo thời gian của quỹ đạo 4-1-4

57
 4.3. Thiết kế quỹ đạo cho tay robot trong hệ tọa độ Decac

Như mục trên đã trình bày, phương pháp xây dựng quỹ đạo chuyển động cho khớp
TM không thích hợp biểu diễn TM khi nó thực hiện nhiệm vụ trong môi trường làm việc
vì không cho phép tách rời hướng và vị trí tay máy. Trong những hệ thống robot phức tạp,
ngôn ngữ lập trình được sử dụng cho điều khiển TM thực hiện nhiệm vụ trong dây chuyền
sản xuất. Trong trường hợp như vậy, quá trình làm việc của tay TM là di chuyển giữa các
điểm đã được đặt trước trong môi trường làm việc với đường di chuyển giữa các điểm dọc
theo quỹ đạo được thiết kế cho điều khiển các khớp TM. Một chu kỳ điều khiển có thể
được mô tả như sau:
(1) Tăng thời gian bằng t = t+ t
(2) tính vị trí và hướng của tay dựa trên dạng quỹ đạo chuyển động của robot.
(3) tính vị trí tương ứng của các khớp bằng sử dụng phương trình động học ngược.
(4) Gửi thông tin tới các bộ điều khiển vị trí các khớp robot.
(5) Quay lại bước đầu tiên

Một cách tổng quát để tay robot thực hiện chuyển động giữa điểm ban đầu tới diểm
cuối cùng theo một đường thẳng, cần phải xác định phép biến đổi từ vị trí đầu tới vị trí
cuối và chia thành các đoạn nhỏ. Phép biến đổi tổng quát R giữa cấu hình tay ở vị trí ban
đầu (T0) đến vị trí cuối cùng (Tc) là : Tc = T0R hoặc R = T0-1Tc
Ba phương pháp khác nhau được sử dụng để biến đổi phép biến đổi tổng thành các
đoạn nhỏ :
 Chia chuyển động giữa các điểm thành một số lớn đoạn tương ứng với số các
chuyển độngvi sai. Sử dụng các phương trình dịch chuyển vi sai, ta xác định được vị trí và
hướng tay trong mỗi đoạn nhỏ thông qua ma trận Jacobien :
DT6 = J D  hoặc D  =J-1.DT6
dT = ΔT
Tmới = Tcũ +dT
 Phép biến đổi R có thể được thực hiện bằng một phép biến đổi tịnh tiến và hai
phép biến đổi quay. phép tịnh tiến thực hiện dịch chuyển khung tọa độ tay từ vị trí ban đầu
đến vị trí cuối cùng. Phép quay thứ nhất nhằm đặt khung tọa độ tay ở vị trí mong muốn và
phép quay thứ 2 xung quanh trục khung tọa độ để đạt được hướng cuối cùng của tay.
 Phép biến đổi R được thực hiện bằng hai phép biến đổi: phép tịnh tiến để đưa
khung tọa độ tay đến vị trí cuối cùng và phép biến đổi xoay xung quanh vector k nhằm
đạt được hướng mong muốn của tay.
58
 BÀI TẬP
1. Thiết kế quỹ đạo chuyển động dạng đa thức bậc 3 cho khớp 1 của Robot 6 bậc tự do từ
góc ban đầu 500 đến góc cuối cùng 800 trong thời gian 3s. Xác định góc quay, tốc độ và
gia tốc góc của khớp ở thời điểm 1,2,3 s. Biết rằng Robot chuyển động từ trạng thái đứng
yên và dừng ở vị trí cuối cùng.
2. Xây dựng quỹ đạo dạng 2-1-2 của khớp 1 robot quay từ góc ban đầu 40 0 đến góc cuối
cùng 1200 trong 4s với tốc độ lớn nhất là 10 0/s. Vẽ đường biểu diễn vị trí, tốc độ và gia tốc
chuyển động của khớp.
3. Thiết kế quỹ đạo chuyển động dạng 2-1-2 cho Robot 3 bậc tự do từ góc ban đầu 50 0 đến
góc cuối cùng 800 trong thời gian 3s. Xác định góc quay, tốc độ và gia tốc góc của khớp ở
thời điểm 1,2,3 s. Biết rằng Robot chuyển động từ trạng thái đứng yên và dừng ở vị trí
cuối cùng.
4. Thiết kế quỹ đạo dạng 2-1-2 cho một khớp quay của robot từ góc ban đầu q 1 = 600 tới
góc cuối q3 = 100 qua điểm trung gian q2 = 400 . Biết td1 = td3 = 3s, trị tuyệt đối gia tốc tại
các điếm cuối là 600/s.

5. Thiết kế quỹ đạo dạng bậc 3 cho một khớp quay của robot từ góc ban đầu q 1 = 100 tới
góc cuối q3 = 600 qua điểm trung gian q2 = 400 , biết td1 = td3 = 3s.

59