20/10/2021

MOBILE ROBOT MOBILE ROBOT

Ph.D. NGUYỄN VĂN TRƯỜNG
CHAPTER 4. Động lực học robot di động
INTellIGeNT RoboTIcs laboRaToRY,
DePaRTmeNT of mechaTRoNIcs eNGINeeRING,
facUlTY of mechaNIcal eNGINeeRING,
haNoI UNIVeRsITY of INDUsTRY, haNoI, VIeTNam
coNTacT PhoNe: 0918.156.929
WoRkING scheDUle: 8.30-11.00 am WeD.
offIce Room: 202-a10, haUI
emaIl: qVTRUoNGcDT@GmaIl.com
NGUYeNVaNTRUoNG@haUI.eDU.VN

PH.D. Nguyen Van Truong Intelligent Robotics Laboratory PH.D. Nguyen Van Truong 2 Intelligent Robotics Laboratory

1 2

4.1. Cơ sở lý thuyết về động 4.1.1. Mô hình động lực học Newton-Euler

lực học robot di động
 Mô hình này được suy ra bằng cách áp dụng trực tiếp các phương trình Newton Euler
cho chuyển động tịnh tiến và quay.
 Mô hình động lực học thuận là mô tả phản ứng động lực robot khi cho dE i
trước các lực /mô men.
 Chuyển động tịnh tiến được mô tả bởi:  fi
dt
 Động lực học nghịch xác định các lực / mô men cần thiết để có được quỹ  Động lượng: Ei  mi s i Gi

đạo chuyển động mong muốn cho robot. fi

 Giả định khối lượng là hằng số, mô hình Bi

Ei
dV
 1 (t ), 2 (t ),..., m (t ) q1 (t ), q 2 (t ),..., q n (t ) động lực học tổng quát cho bởi:
Mô hình động lực r
học thuận misi  Fi z
Q

 Mô men động lượng si

 1 (t )
 
q1 (t )
 G i  I iω i    r T rI 3  rrT  i dV ω i
  Vi 
 Biến thiên mô men động lượng x τi
 m (t ) qn (t ) O
dG i
 τi
dt y
i tổng mô men ngoại lực tạo ra chuyển động quay của Bi, Ii là tensor quán tính, i là khối lượng riêng của Bi, dV là thể tích phần tử
vô cùng nhỏ của Bi nằm ở vị trí r so với trọng tâm.
 1 (t ),  2 (t ),..., m (t ) q1 (t ), q 2 (t ),..., q n (t )
Mô hình động lực
học nghịch
PH.D. Nguyen Van Truong 3 Intelligent Robotics Laboratory PH.D. Nguyen Van Truong 4 Intelligent Robotics Laboratory

3 4
 20/10/2021

4.1.2. Mô hình động lực học 4.1.3. Mô hình động lực học Lagrange
Lagrange robot di động nonholonomic

 Mô hình động lực học Lagrange dạng tổng quát:

 Mô hình động lực học Lagrange của robot nonholonomic:
d  L  L
 τ, q  q1 , q2 ,..., qn 
T
 
dt  q  q d  L  L
   M T (q) λ  Eτ
dt  q  q
 qi là các tọa độ suy rộng,  là véc tơ ngoại lực tổng hợp (lực đối với chuyển
M (q) là ma trận mxn của m ràng buộc nonholonomic M (q)q  0 (*)
động tịnh tiến, mô men đối với chuyển động quay), L là hàm Lagrangian:
L  K  P  nhân tử Lagrange. Viết lại mô hình dưới dạng ma trận
   C(q, q )q  g (q)  M T (q)λ  Eτ
D(q)q
 K  K1  K 2  ...  K n
 P  P  P  ...  P
 1 2 n  E là ma trận không kỳ dị (định thức khác 0). Để loại ràng buộc M T (q )λ và
nhận được mô hình không có ràng buộc sử dụng ma trận nx(m-n) B(q ) :
 Ki là động năng của liên kết i, Pi là thế năng liên kết i.
1 T 1 T BT (q)MT (q)  0 (**)
K ms s  ω Iω
2 2  Từ (*) và (**) có thể thấy có véc tơ (n-m) chiều thỏa mãn:

 s là vận tốc thẳng của trọng tâm,  là vận tốc góc của chuyển động quay, q (t )  B(q ) v (t ) D  BT DB
m là khối lượng và I là tensor quán tính của vật    BT CB
C  B DB
T

 Mô hình rút gọn không có ràng buộc D(q) v  C(q, q ) v  g (q )  Eτ 

g  B g
T

E  BT E


PH.D. Nguyen Van Truong 5 Intelligent Robotics Laboratory PH.D. Nguyen Van Truong 6 Intelligent Robotics Laboratory

5 6

4.2. Động lực học robot di động 4.2. Động lực học robot di động
kiểu vi sai kiểu vi sai

 Mô hình động lực học Newton-Euler:  Giả thiết rằng trọng tâm trùng với điểm giữa của hai bánh dẫn động
(b=0), ma trận rang buộc nonholonomic:
mv  F M (q)   sin  cos  0

 N
Iω
 Giả thiết robot di chuyển trên địa hình phẳng nằm ngang (C(q, q )  0 ,g (q)  0)
 F là tổng các lực tác dụng lên robot, N là tổng mô men đối với trọng tâm
tác dụng lên robot, I tensor quán tính của robot. Giả thiết rằng trọng tâm  Mô hình động lực học Lagrange của robot:
trùng với điểm giữa của hai bánh dẫn động (b=0):   M T (q)λ  Eτ
D(q)q
 F  Fr  Fl  Trong đó:  xQ   m 0 0 cos  cos  
  rF  r  1
 r q   yQ  τ  D(q)   0 m 0 E   sin  sin  
r
    r 
 l  rFl  l 
    0 0 I   2a  2a 
 2a
 N  ( Fr  Fl ) 2a  ( r   l )  Trong đó:
 r cos  0
m 0  1 1 1 
 Mô hình động lực học của robot: BT (q)M T (q)  0  B(q)   sin  0 D  BT DB   E 
r 2a  2 a 

0 I
 1  0 1 
v  mr ( r   l )
 1
m 0  v1  1  1 1   r  v  mr ( r   l )
  0 I  v   r  2a  2a     
 
2a  Phương trình động lực học robot
( r   l )   2     l    a ( r   l )
2
 Ir  Ir
PH.D. Nguyen Van Truong 7 Intelligent Robotics Laboratory PH.D. Nguyen Van Truong 8 Intelligent Robotics Laboratory

7 8
 20/10/2021

4.3. Động lực học robot di động kiểu 4.3. Động lực học robot di động kiểu Ackerman
Ackerman
 Vận tốc của G trong hệ tọa độ Gxgyg:
 xG  cos   sin    x g 
 Mô hình tương đương Robot chịu tác dụng của lực phát động Fd và hai lực trượt ngang Fr và Ff  y    sin  cos    y  (##)
tác dụng vuông góc với các bánh xe tương ứng.  G   g 
 Các ràng buộc nonholonomic áp dụng cho trọng tâm G nằm ở khoảng cách b so với Q và
 Thay (##) vào (#):
khoảng cách d từ P: xg
 yg  b  b
 xQ sin   y Q cos   0   y g  b  y g  D (tan ) x g
 (*)  
  tan    tan  (###) 1  
 x P sin(   )  y P cos(   )  0 yg
P
Ff
  D x g
  
  D xg    D cos 2  x g
b
 yg  (tan ) xg 
b
x g
xg
  D D cos 2  
 Ký hiệu xG và yG là tọa độ của điểm G trong hệ tọa độ cố định G
y  Phương trình động lực học Newton-Euler: yg
ta được: Q d P
Ff
 xQ  xG  b cos  ; xP  xG  d cos  m( x g  y g)  Fd  F f sin
 y  y  b sin  ; y  x  d sin  b D G

m( yg  x g)  Fr  F f cos
Fd
 Q (**) y
 d
G P G
  Fr Q
   J  dF f cos  bFr
 xQ  xG  b sin  ; x P  xG  d sin  O x  b D
 y Q  y G  b cos  ; y P  y G  d cos 
Fd
   1   K u s  Fr
 Thay (**) vào (*):  T T
O x
 xG sin   y G cos   b  0 m: khối lượng robot, J: mô men quán tính của robot về trọng tâm G.
T: hằng số thời gian của hệ thống lái, us: thông số đầu vào điều
 
 xG sin(   )  y G cos(   )  d cos  0
khiển lái, K: hệ số mô men.

PH.D. Nguyen Van Truong (#)

Intelligent Robotics Laboratory PH.D. Nguyen Van Truong Intelligent Robotics Laboratory
9 10

9 10

4.3. Động lực học robot di động kiểu 4.4. Động lực học robot di động kiểu đa
Ackerman hướng ba bánh xe vạn năng

 Chuyển những phương trình động học vào không gian trạng thái. Xét  Xét robot ba ba bánh xe được đặt ở các góc 30; 150; và 270. Ma trận xoay
(###): của hệ tọa độ cục bộ Qxryr của rô bốt đối với hệ tọa độ Oxy cố định là:
d J DF f cos  J cos   sin   yr
m(b  x g)  F f cos  F f cos    R ( )   y Fd 1
b b b  sin  cos   30 o
xr

 mb 2  J    mbx g    Xét vị trí trọng tâm Q: 

 F f       xg 
 D cos   D cos 


s Q  xQ yQ T  120o 

 xg 
xg (mb 2  J ) tan
 
D 2 cos 2 
Fd yg Ff  T

FQ  FQx FQy  MsQ  FQ Q Fd 3
cos 2  ( mD 2  ( mb 2  J ) tan 2  ) cos  ( mD 2  (mb 2  J ) tan 2  )
2
P M  diag (m, m) 120o D
G 
y
 xG  (cos   (b / D) tan sin  ) x g Q d Fd 2 x
 y  (sin   (b / D) tan cos  ) x  Mối liên hệ giữa các hệ tọa độ O
b D

G g Fd
   ((1 / D) tan ) x g  Fr s Q  R ( )s r
x  (1 / a )((mb 2  J )(tan )x )  (1 / a )( D 2 cos 2  ) F s r  xr yr T 
 FQ  R ( )Fr
 g
  (1 / T )  ( K / T )u s
g d O x 

Fr  Fxr Fyr 
T
M (s  R T ( )R ( )s )  F
 Q r r

PH.D. Nguyen Van Truong 11 Intelligent Robotics Laboratory PH.D. Nguyen Van Truong 12 Intelligent Robotics Laboratory

11 12
 20/10/2021

4.4. Động lực học robot di động kiểu đa hướng ba bánh xe vạn năng 4.4. Động lực học robot di động kiểu đa
hướng ba bánh xe vạn năng
 Gọi IQ là mô men quán tính của robot về Q và Q là mô men lực tác dụng tại Q:
I Q   Q
 Từ các phương trình động học ngược:
 1 3
 D là khoảng cách giữa các bánh xe từ trọng tâm Q, Fdi là các lực dẫn động của r1   xr  y r  D y yr Fd 1
2 2 xr
các bánh xe. Các lực và mô men:  30o
 1 3
y r  D xr  a1 xr  a2* y r  b1 ( 1   2  2 3 )
yr r2   xr   
 1 1 y Fd 1
 2 2   yr  a1 yr  a2* xr  3b1 ( 1   2 )
 Fxr   2 Fd 1  2 Fd 2  Fd 3 xr  
30 o r3  xr  D   a   b (     ) 120o 
   

3 2 1 2 3
3 3  Q
 Fyr  Fd 1  Fd 2 
 Trong đó:
Fd 3
 2 2 120o 
120 o
D
  Q  ( Fd 1  F d 2  Fd 3 )D a1  3 (3I o  2mr 2 ) a2*  2mr 2 /(3I o  2mr 2 )
Q
 120o
Fd 3
a3  3D 2 /(3I o D 2  I Q r 2 ) Fd 2 x
D
O
 Sự quay của bánh xe được mô tả bởi phương trình: b1  Kr /(3I o  2mr ) b2  Kr /(3I o D  I Q r )
2 2 2

I oi   i  K i  rFdi

Fd 2 x
 Mô hình động lực học:
O
x  A(x)x  B(x)u
Io là mô men quán tính chung của các bánh xe, i là vị trí góc của bánh xe i,  là hệ
số ma sát tuyến tính, r là bán kính các bánh xe, I là mô men đầu vào dẫn động
 x  xQ yQ  xQ y Q 
T
 
y  xQ y Q  T u   1  2  3 
T

y  Cx
của bánh xe I, K là hệ số mô men

PH.D. Nguyen Van Truong 13 Intelligent Robotics Laboratory PH.D. Nguyen Van Truong 14 Intelligent Robotics Laboratory

13 14

4.4. Động lực học robot di động kiểu đa 4.5. Động lực học robot di động kiểu đa hướng bốn bánh xe mecanum
hướng ba bánh xe vạn năng
0 0 0 1 0
0  Tổng các lực thành phần Fx và Fy hoạt động trên robot theo phương x và y được cho bởi:
0 0 
 0 0 0 1 0 0 0 1 0 0   Fx  ( Fx1  Fx 2  Fx 3  Fx 4 )
C  0 0 0 0 1 0 
Fx2
0 0 0 0 0 1  Fx1
A ( x)      Fy  ( Fy1  Fy 2  Fy 3  Fy 4 )
-45o
 45o
0 0 0 a1  a2 0  0 0 1 0 0 0
Fy2 Fy1
0 0 0 a2 a1 0  Trong trường hợp không có chuyển động quay riêng biệt,
  hướng của chuyển động được xác định bởi một góc δ yQ
0 0 0 0 0 a3  trong đó: d2
y yr Fd 1 Q
 0 0 0  o xr   tan 1 ( Fy / Fx ) Fx3 Fx4
 
30 -45o xQ 45o
d1
   Tổng mô men:
 Fy4
 
B(x)   
  b1 ( x) b 2 ( x) b 3 ( x)  120o


  ( Fx1  Fx 2  Fx 3  Fx 4 )d1  ( Fy 3  Fy 4  Fy1  Fy 2 )d 2 Fy3
 b1 1 b1 2 2b1 cos  
 Phương trình động lực học Newton-Euler
b1  3 b1 4 2b1 sin   Q Fd 3

 b2 b2 b2 
 120o D mx  Fx   x x

a2  1  a2*  3I o /(3I o  2mr 2 ) Fd 2 x my  Fy   y y  i là các hệ số ma sát tuyến tính, m là khối
 I       lượng robot, IQ là mô men quán tính của robot
 O
 1   3 sin   cos  ,  2  3 sin   cos   Q z
   3 cos   sin  ,    3 cos   sin 
 3 4

PH.D. Nguyen Van Truong 15 Intelligent Robotics Laboratory PH.D. Nguyen Van Truong 16 Intelligent Robotics Laboratory

15 16