MOBILE ROBOT

Ph.D. NGUYỄN VĂN TRƯỜ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

 MOBILE ROBOT

CHAPTER 2. Chuyển động của robot di động

❑Mô tả vị trí và hướng của robot
❑Ma trận biến đổi tọa độ thuần nhất

PH.D. Nguyen Van Truong 2 Intelligent Robotics Laboratory

 2.1 Đặt vấn đề
Trạng thái (Vị
trí+Hướng) của robot Trạng thái (Vị
trong hệ quy chiếu cố trí+Hướng) của robot Trạng thái (Vị
định? trong hệ quy chiếu gắn trí+Hướng) của robot
với ngôi nhà? trong hệ quy chiếu gắn
với người đang chuyển
động?

Camera

PH.D. Nguyen Van Truong 3 Intelligent Robotics Laboratory

 2.1.1. Các hệ trục tọa độ trong
không gian chuyển động
z’
▪ Hệ quy chiếu gốc S (gắn cố định với
giá) và các véc tơ đơn vị

S : Oxyz {x, y, z} y’
z’
z
▪ Hệ quy chiếu động S’ gắn liền với khâu O’
và các véc tơ đơn vị y’
x’

S  : Oxyz {x, y, z} z O’

▪ O’ là điểm định vị (điểm cực)
của robot
▪ Robot được xác định hoàn toàn
trong không gian bởi vị trí và
hướng của nó đối với hệ quy
chiếu gốc cố định.
PH.D. Nguyen Van Truong
x’
y y
x
O
x
▪ Chuyển động tổng quát =
Chuyển động của điểm cực so
với hệ quy chiếu gốc +
Chuyển động quanh điểm cực
4 Intelligent Robotics Laboratory
 2.1.2. Véc tơ định vị
❖ Véc tơ định vị một điểm
▪ Một điểm O’ bất kỳ thuộc khâu được biểu diễn trong hệ S:

O = Ox x + Oy y + Oz z z’

Ox , Oy , Oz biểu thị các thành phần của véc tơ O trong hệ S:


O = Ox Oy Oz 
T

z’
y’

z
O’
O được gọi là véc tơ định vị điểm O’ trong hệ S y’
x’

z O’
x’
y y
x
O
x

PH.D. Nguyen Van Truong 5 Intelligent Robotics Laboratory

 z’
Ma trận cô-sin chỉ hướng

▪ Các véc tơ đơn vị của hệ S’ được biểu y’

diễn trong hệ S:
z’
z
O’
y’
x = xx x + xy y + xz z x’
y = yx x + yy y + yz z (*) z O’
x’
z = zx x + zy y + zz z y y
x
O
xx , xy , xz , yx , yy , yz , zx , zy , zz x
là các hình chiếu các véc tơ đơn vị của S’ lên các trục x, y, z trong hệ S. Hay còn
được gọi là các cô-sin chỉ hướng các trục của hệ S’ đối với hệ S (do giá trị của chúng
là cô-sin góc tạo bởi véc tơ đơn vị trong hệ S’ và véc tơ đơn vị trong hệ S. Ví dụ:
xx = xT x = x  x = cos(x, x)

PH.D. Nguyen Van Truong 6 Intelligent Robotics Laboratory

 Ma trận cô-sin chỉ hướng

▪ Ba véc tơ đơn vị trong phương trình (*) có thể kết hợp thành một ma trận mà có
vai trò xác định hướng của một hệ quy chiếu động trong một hệ quy chiếu cố
định. Ma trận này gọi là là ma trận cô-sin chỉ hướng của hệ quy chiếu S’ đối với
hệ quy chiếu S (do các phần tử là cô-sin chỉ hướng của các trục):

 xx yx zx   x'T x yT x zT x 

 
R = x' y ' z ' =  xy yy zy  = x'T y yT y zT y 
 xz yz zz   x'T z yT z zT z 

PH.D. Nguyen Van Truong 7 Intelligent Robotics Laboratory

2.1.3.Ma trận quay và các phép quay
z
z’
Xét trường hợp gốc OO’ (O = 0):
pz
▪ Một điểm tùy ý P trong không P
gian được biểu diễn trong hệ S: y’


p = px py pz 
T
pz’ py’
y
hoặc trong hệ S’: OO’ py

 
px
px’
p = p x
T
p y p z
x
p và p là các véc tơ cùng biểu x’
diễn điểm P:
p = p x x + p y y + p z z = x y z p = R p
➢ Như vậy ma trận cô-sin hướng (sau đây gọi là ma trận quay) biến đổi tọa
độ của điểm tùy ý P trong hệ quy chiếu động S’ sang tọa độ của nó trong
hệ quy chiếu gốc cố định S
PH.D. Nguyen Van Truong 8 Intelligent Robotics Laboratory
 Tính chất của ma trận quay
z
▪ Các véc tơ cột của ma trận quay có z’
tính chất:
pz
x y = 0; y z = 0; z x = 0;
T T T P
(**)
x x = 1; y y = 1; z z = 1.
T T T y’
pz’
▪ Ma trận quay có tính chất trực giao: py’
y
R R = I 33
T
OO’ py
px
RT = R −1 px’

det(R) = 1 x
x’
▪ Ma trận quay đặc trưng bởi 9 phần tử không độc lập do có đến 6 ràng buộc
do các điều kiện trực giao cho trong (**). Điều này ngụ ý rằng 3 tham số là
đủ để mô tả vị trí và hướng của một vật rắn trong một hệ quy chiếu xác định.

PH.D. Nguyen Van Truong 9 Intelligent Robotics Laboratory

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

z’  ▪ Phép quay quanh trục x,

y’ y, z.
z’ y’ z
z’ ▪ Các phép quay này là
 y y’
z dương nếu chúng được
xx’
y z’ thực hiện ngược chiều
z xx’ kim đồng hồ về trục của
y yy’
hệ quy chiếu động.
x
zz’ y’ x’
x y z
x’ 
y z’
x

y’ z’
x’
y’ x’ x
yy’ +

x 
x’ x’
zz’

PH.D. Nguyen Van Truong 10 Intelligent Robotics Laboratory

 Các ma trận quay cơ bản

▪ Ma trận quay quanh trục x

 xx  1  yx   0   z x   0  1 0 0 
       
x =  xy  = 0 y  =  yy  = cos   z =  z y  = − sin   R x ( ) = 0 cos  − sin  
 xz  0  yz   sin    z z   cos   0 sin  cos  

▪ Ma trận quay quanh trục y

 xx   cos    yx  0  z x   sin    cos  0 sin  
x =  xy  =  0  y  =  yy  = 1 z =  z y  =  0  R y (  ) =  0 1 0 
 xz  − sin    yz  0  z z  cos   − sin  0 cos  

▪ Ma trận quay quanh trục z

 xx  cos    yx  − sin    z x  0 cos  − sin  0
x =  xy  =  sin   y  =  yy  =  cos   z =  z y  = 0 R z ( ) =  sin  cos  0
       
 xz   0   yz   0   z z  1  0 0 1

✓ Chú ý: R k (− ) = RTk ( ) k = x, y, z

PH.D. Nguyen Van Truong 11 Intelligent Robotics Laboratory
 Phép quay tổng hợp

▪ Phép quay tổng hợp là tổ hợp của các phép quay quanh các trục.
i
▪ Xét mối liên hệ của p (biểu diễn p trong hệ quy chiếu Si:Oixiyizi)
và p i −1 (biểu diễn p trong hệ quy chiếu Si-1:Oi-1xi-1yi-1zi-1)
▪ Nếu R ii −1 là ma trận quay của hệ quy chiếu Si đối với hệ quy chiếu
Si-1, ta có:
pi −1 = R ii −1pi
▪ Thực hiện lần lượt n phép quay liên tiếp:
S0: O0x0y0z0 → Sn: Onxnynzn
p 0 = R10 R12 ...R nn −1p n

R 0n = R10 R12 ...R nn −1

PH.D. Nguyen Van Truong 12 Intelligent Robotics Laboratory

 Phép quay Euler

Ví dụ: Phép quay Euler: là phép quay được thực hiện theo thứ tự: Quay quanh trục z
một góc , sau đó quay quanh trục y mới một góc  , cuối cùng quay quanh trục z
mới một góc  (gọi tắt là góc ZYZ)
zz’ z’’ z’
z’’z’’’
 y’’’

x’ y’y’’ y’’


OO’
x O’O’’ O’’O’’’
y
x’’’
 y’
x’  x’’ x’’ 

c c c − s s −c c s − s c c c 
 
R ZYZ = R z ( )R y  (  )R z  ( ) =  s c c + s s − s c s + c c s s 
 − s c s s c 


PH.D. Nguyen Van Truong 13 Intelligent Robotics Laboratory

 Phép quay Euler

▪ Nếu cho trước ma trận quay của khâu tác động cuối theo phép
quay Euler:
 r11 r12 r13   = A tan 2(r23 , r13 )

= r21 r22 r23   2
( ) 
1

 = A tan 2 r13 + r23 , r33 

2 2
R ZYZ
  
r31 r32 r33   = A tan 2(r32 ,−r31 )
✓ Chú ý:
▪ Ký hiệu c và s lần lượt là viết tắt của cos  và sin 
▪ Hàm A tan 2( y, x) tính toán arctan của tỷ số y / x nhưng sử dụng
dấu của mỗi đối số để xác định góc kết quả thuộc về góc phần tư
nào, điều này cho phép xác định chính xác một góc trong phạm vi
2.
▪ Khi sin  = 0, ta chỉ xác định được tổng hoặc hiệu của  và  .
Trường hợp này được gọi điểm kỳ dị của các góc Euler.

PH.D. Nguyen Van Truong 14 Intelligent Robotics Laboratory

 2.2. Phép biến đổi thuần nhất các
hệ trục tọa độ

▪ p 0 là véc tơ xác định tọa độ của điểm P tùy ý đối với hệ

quy chiếu tham chiếu cố định S0
z1
▪ Hệ quy chiếu động: S1:O1x1y1z1.
▪
O10 là véc tơ xác định tọa độ O1 đối với hệ S0.
P
y1
▪
R 10 là ma trận quay của hệ S1 đối với hệ S0.
▪ p1 là véc tơ xác định tọa độ của P đối với hệ S1. P0
O1
z0
p 0 = O10 + R10 p1 P1
x1

▪ Tồn tại cả phép cộng và phép nhân ma trận, để

đơn giản trong tính toán ta sử dụng khái niệm O1 0 y0
phép biến đổi thuần nhất, chỉ dùng một phép O0
biến đổi duy nhất để mô tả được cả vị trí và
hướng của vật trong không gian. x0

PH.D. Nguyen Van Truong 15 Intelligent Robotics Laboratory

 2.2. Phép biến đổi thuần nhất các hệ trục tọa độ
▪ Cấu trúc của ma trận biến đổi thuần nhất từ hệ quy chiếu S1 về hệ
quy chiếu S0 (ma trận cỡ 4x4):
 R 0
O 0

T1 =  T
0 1 1

 13
0 1 
▪ Sự chuyển đổi tọa độ giữa hệ S1 và hệ S0 được mô tả bởi ma
~ ~
trận biến đổi thuần nhất thỏa mãn phương trình: p = T1 p
0 0 1

 1
  0

01T3 = 0 0 0
p ~ p
▪ Trong đó: p =   p =  
~ 1 0

 1  41  1  41
▪ Nếu chỉ phép tịnh tiến thuần túy ▪ Nếu chỉ phép quay thuần túy
1 0 0   0
0 1 0 O10   R0 0
T10 =  T10 =  1

0 0 1   0
   
0 0 0 1 0 0 0 1
PH.D. Nguyen Van Truong 16 Intelligent Robotics Laboratory