documents tips thiet ke quy dao va dieu khien xe tu hanh van chuyen trong kho

Hội nghị toàn quốc về Điều khiển và Tự động hoá - VCCA-2011

Thiết kế quỹ đạo và điều khiển xe tự hành vận chuyển trong kho
Trajectory planning and control of mobile robot for transportation
in warehouse
Nguyễn Văn Tính, Phạm Thượng Cát, Phạm Minh Tuấn, Bùi Thị Thanh Quyên
Viện Công nghệ thông tin, Viện Khoa học và Công nghệ Việt Nam
Email: ; ; ;
Hiện nay, có nhiều nhóm nghiên cứu trên thế giới
đã đạt được những thành tựu đáng kể về điều khiển rô
bốt tự hành nâng hạ (forklift) xắp xếp kệ hàng (pallet)
trong kho. Nhóm tác giả của trường đại học Girona,
Tây Ban Nha đã nghiên cứu hệ thống thị giác tìm vị
trí 3D của pallet dựa trên màu sắc và đặc tính hình
học của nó. Từ đó, quỹ đạo tuyến tính được tạo ra để
giúp xe nâng hạ có thể di chuyển và tiếp cận pallet
[3]. Nhóm tác giả của Viện kỹ thuật hệ thống của
trường đại học Hannover đã nghiên cứu thành công
bài toán nhận dạng/định vị pallet dựa trên hai máy
quét laser để forklift có thể tiếp cận và nâng hạ pallet
[4]. Bellomo và nhóm tác giả đã nghiên cứu phương
pháp ước lượng vị trí của pallet dựa vào dữ liệu máy
quét laser LIDAR và hệ thống thị giác [5]. Ngoài ra
bài báo này cũng trình bày thuật toán điều khiển
forklift chuyển động tiếp cận để nâng/hạ pallet dựa
trên thuật toán “tái lập quỹ đạo dạng polinom”.
Widyotriatmo và nhóm tác giả đã đề xuất thuật toán
“tiên đoán hướng” (predictive navigation) kết hợp
với quỹ đạo ngắn nhất, tránh chướng ngại vật
(obstacle avoidance), và sự bất định của các cảm biến,
cơ cấu chấp hành [6]. Thuật toán tạo quỹ đạo

h 3 spline được sử dụng cho rô bốt di động loại
bánh xe (wheeled mobile robot) được giới thiệu trong
[7], [8] và thuật toán mở rộng của nó trong việc tránh
các chướng ngại vật khi di chuyển trong [9].

Tóm tắt:
Báo cáo này đề xuất giải pháp thiết kế quỹ đạo và
điều khiển tối ưu xe tự hành cho bài toán vận chuyển
kệ hàng (pallets) trong kho. Giải pháp bao gồm thuật
toán thiết kế quỹ đạo h 3 spline với đa thức bậc 7 kết
hợp với thuật điều khiển tối ưu theo chuẩn toàn
phương để xác định tốc độ góc cần thiết cho rô bốt
bám quỹ đạo. Ngoài ra việc định vị xe tự hành từ các
tín hiệu đo can nhiễu được xác định bằng phương
pháp lọc Kalman mở rộng. Các thử nghiệm trên rô bốt
tự hành Pioneer P3-DX trong phòng thí nghiệm cho
kết quả khả quan.

Abstract:
This paper presents methods for trajectory planning
and optimal control of mobile robot for the problem
of pallet’s pick and place in warehouse. The
h 3 spline with 7th order polinom is used for
trajectory planning. Based on linearized error
dynamic equations and by using optimal control law a
sequence of angular velocities is generated such that
mobile robot always tracks the desired trajectory.
Furthermore, we propose an approach of mobile robot
localization using Extended Kalman filter. The
implementation of the proposed methods on robot

Pioneer P3-DX gives acceptable results.

Báo cáo này đề xuất giải pháp thiết kế quỹ đạo và
điều khiển tối ưu xe tự hành cho bài toán vận chuyển
kệ hàng (pallets) trong kho. Báo cáo gồm 5 phần.
Phần 2 mô tả thuật toán thiết kế quỹ đạo h 3 spline
với đa thức bậc 7 và thuật điều khiển tối ưu theo
chuẩn toàn phương để xác định tốc độ cần thiết cho
rô bốt bám quỹ đạo. Phần 3 trình bày phương pháp
định vị xe tự hành từ các tín hiệu đo can nhiễu được
xác định bằng phương pháp lọc Kalman mở rộng. Các
thử nghiệm trên rô bốt tự hành Pioneer P3-DX trong
phòng thí nghiệm được mô tả trong phần 4. Phần 5 là
một số kết luận và hướng nhiên cứu tiếp tục triển
khai trong thời gian tới.

1. Giới thiệu
Rô bốt di động là một hệ rô bốt có khả năng thực hiện
các nhiệm vụ ở nhiều vị trí khác nhau với khả năng
dịch chuyển bằng bánh xe, xích hay bằng chân phụ
thuộc vào địa hình. Khả năng di động làm rô bốt có
nhiều ứng dụng và đòi hỏi phải giải quyết nhiều vấn
đề mới. Những vấn đề nghiên cứu đang được nhiều
tác giả trên thế giới quan tâm cho lĩnh vực rô bốt di
động, xe tự hành là khả năng xác định phương hướng
(navigation) của rô bốt, bài toán thiết kế qũy đạo
chuyển động tránh vật cản và điều khiển xe tự hành
tiếp cận quỹ đạo mong muốn.
Do mô hình động học và động lực học của rô bốt
di động, xe tự hành thường là mô hình phi tuyến dạng

hệ nonholonomic (có các ràng buộc về tốc độ chuyển
động không khả tích), và có nhiều tham số bất định
nên bài toán thiết kế quỹ đạo và điều khiển đòi hỏi
nhiều nghiên cứu và áp dụng các phương pháp tính
toán phức tạp để bảo đảm xe chạy ổn định trơn tru và
bám quỹ đạo chính xác

VCCA-2011

2. Bài toán điều khiển rô bốt di động xắp
xếp hàng trong kho
2.1 Xây dựng mô hình động học cho rô bốt di dộng
Rô bốt di động sử dụng trong báo cáo này là Pioneer
P3-DX thuộc loại xe chuyển động trên mặt phẳng có
hai bánh đẩy và một bánh lái có thể thấy ở Hình 1

269


Hội nghị toàn quốc về Điều khiển và Tự động hoá - VCCA-2011
(trên) và mô hình của nó có hình dáng hình chữ nhật
(dưới).

trong đó v là vận tốc tuyến tính (translational velocity)
của rô bốt dọc theo trục x-axis và ω là vận tốc góc của
rô bốt. Giả định

v  v max và    max , trong đó

v max và  max lần lượt là các giá trị lớn nhất của

chúng. Hệ (2) là hệ động học khá phổ biến cho nhiều
loại xe tự hành dùng trong công nhiệp cũng như trong
các phòng thí nghiệm.
2.2. Bài toán xắp xếp kệ hàng trong kho và thuật
toán điều khiển tối ưu
Trong tọa độ toàn cục OXY , rô bốt có cấu
hình là ( X , Y ,  ) , kệ hàng có cấu hình là
x

y
v
Y

θ

o
l2

O

( X P , YP , P ) , và cấu hình cần chuyển kệ hàng đến là
( X G , YG , G ) (Hình 2).

l1

lw

X

H1. Từ trên xuống dưới: rô bốt Pioneer P3-DX và

cấu hình của nó trong hệ tọa độ OXY.

H 2. Bài toán xắp xếp pallet trong kho.
Cấu hình của kệ hàng trong hệ tọa độ toàn cục có thể
được xác định nhờ các cảm biến như máy quét laser,
camera gắn trên rô bốt. Cấu hình của rô bốt được bộ
định vị (localization) ước lượng dựa trên tín hiệu thu
về từ các cảm biến trong của rô bốt. Bài toán đặt ra là
cần tìm thuật toán để có thể điều khiển rô bốt di
chuyển từ vị trí hiện tại tiếp cận kệ hàng và sau đó
mang kệ hàng tới vị trí đích đã cho trước. Các tác vụ
rô bốt cần thực hiện trong nhiệm vụ xắp xếp này là:
I- Di chuyển từ cấu ban đầu tới cấu hình của kệ hàng
II- Nhấc kệ hàng lên
III- Di chuyển từ cấu hình đã nhấc kệ hàng tới cấu
hình đích
IV- Đặt kệ hàng xuống và đứng nguyên ở vị trí đích
Sơ đồ khối của bộ điều khiển cho rô bốt như trong
Hình 3.

Ở đây

l1 là chiều dài, l 2 là chiều rộng, và l w là
khoảng cách giữa 2 bánh xe sau. Tọa độ OXY và
oxy lần lượt là hệ tọa độ nhà kho và hệ tọa độ cục
bộ gắn trên rô bốt, và (X, Y) diễn tả tọa độ của trọng
tâm rô bốt trong hệ tọa độ nhà kho. Góc  biểu diễn
góc quay của trục x-axis so với trục X. Tọa độ cục bộ
của rô bốt oxy được thiết lập như sau: vị trí gốc của
tọa độ ở điểm giữa của trục sau và trục x-axis vuông

góc với trục chính của rô bốt. Như vậy cặp 3 (X, Y, θ)
mô tả đầy đủ vị trí (X,Y) và hướng  của rô bốt
thường được gọi là cấu hình (configuration) của rô
bốt di động. Với giả thiết xe không bị trượt (noslippage), rô bốt phải chuyển động với rằng buộc về
tốc độ như sau:

X sin   Y cos .
(1)
Điều kiện rằng buộc này thuộc dạng không khả tích
(non-holonomic) có nghĩa là ta không thể tích phân để
xác định được quỹ tích những điểm (X,Y) rằng buộc mà
chuyển động của xe phải thỏa mãn. Trong thực tế xe
cấu hình của xe (vị trí và hướng) có thể là bất kỳ ở mọi
điểm trong mặt phẳng nhưng khi chuyển động thì tốc độ
của xe phải thỏa mãn điều kiện rằng buộc nonholonomic (1).
Với điều kiện rằng buộc này, chuyển động của rô bốt có
thể mô tả bới hai biến điều khiển chuyển động (v, ω)
như sau:
 X  v cos 

(2)
 Y  v sin 
 


VCCA-2011

H 3. Sơ đồ khối của hệ điều khiển.
Bộ điều khiển rô bốt bao gồm nhiều khối chức năng,
như sau:

Khối định vị có chức năng thu nhận tín hiệu từ các
cảm biết trong như gyro, encoder và tín hiệu từ các

270


Hội nghị toàn quốc về Điều khiển và Tự động hoá - VCCA-2011
cảm biến ngoài như camera, GPS, .. để ước lượng vị
trí và hướng hiện tại của rô bốt. Đây là bộ phận rất
quan trọng trong quá trình rô bốt di chuyển. Rô bốt
cần cập nhật vị trí của nó thường xuyên để có hướng
điều chỉnh thích hợp cho quá trình tiếp cận pallet hoặc
di chuyển về đích.
Khối nhận dạng môi trường có nghĩa vụ phân tích và
nhận dạng môi trường hoạt động xung quanh rô bốt
dựa trên tín hiệu từ các cảm biến thị giác, và siêu âm.
Khối nhận dạng pallet có nhiệm vụ nhận dạng và ước
lượng vị trí và hướng của pallet dựa trên tín hiệu thu
được từ hệ thống thị giác.
Khối điều khiển chính là bộ phận có nhiệm vụ tạo và
điều khiển rô bốt bám theo quỹ đạo để rô bốt có thể
tiếp cận và nhấc pallet lên, sau đó điều khiển rô bốt đi
về đích đã định trước. Ở đây, chuyển động của rô bốt
sẽ bị hạn chế bởi điều kiện rằng buộc không khả tích
về tốc độ (1).
Hai tác vụ (II) và (IV) sẽ không được đề cập chi tiết
về thuật toán cũng như cách thức thực hiện ở báo cáo
này. Báo cáo sẽ đi sâu chi tiết về thuật toán và kết quả
thực nghiệm cho việc thực hiện tác vụ (I) và (III). Để
thực hiện hai tác vụ (I) và (III), chúng ta cần giải

quyết bài toán tìm tín hiệu điều khiển đầu vào v(.) và

 (.) để

3

đầu và điểm đích (như trong Hình 4). Quỹ đạo G paths, đường cong nội suy của hàm đa thức bậc 7 đi
qua 2 điểm A và B được tính như sau [7]:
T

P(s; η)   px (s), p y ( s)  ,

(3)

px ( s)   0  1s   2 s  3 s
2

3

 4 s 4   5 s5   6 s 6   7 s 7

,

(4)

p y ( s)  0  1s   2 s 2  3 s 3

,
(5)
  4 s 4  5 s 5   6 s 6   7 s 7

Trong đó s  [0, 1] là tham số quãng đường đã được
chuẩn hóa. Tại điểm khởi đầu s=0, px ( s) =XA,
p y ( s ) =YA, và tại điểm kết thúc thì s=1, px ( s ) =XB,
p y ( s ) =YB .Các hệ số của đa thức px ( s ) được tính

như sau:
  || X A  X B , YA  YB ||
a0 X A ,
a1

h cos qA , g1

h cos qB ,

a2 0 ,
a3 0 ,
a4 35( X B X A ) 20a1 10a2 4a3 15g1 5g2
a5
84( X B X A ) 45a1 20a2 6a3 39g1 ,
a6 70( X B X A ) 36a1 15a2 4a3 34g1 ,
a7
20( X B X A ) 10a1 4a2 a3 10g1 .
Các hệ số của đa thức p y ( s ) :

rô bốt có thể di chuyển từ vị trí và hướng

ban đầu PA ( X (0), Y (0),  (0)) tại thời điểm t  0 với

vA  v(0) , vA  v(0) , A   (0) , A   (0) tới vị
trí và hướng đích PB ( X (t f ), Y (t f ),  (t f )) tại thời

điểm t  t f với vB  v(t f ) , vB  v(t f ) , B   (t f ) ,

B   (t f ) .

3. Thiết kế quỹ đạo và điều khiển tối ưu rô
bốt di động.
Để rô bốt thực hiện được các dịch chuyển cần thiết
nêu trên ta cần thiết kế được quỹ đạo chuyển động rồi
tìm thuật toán điều khiển xe bám theo quỹ đạo.
3.1 Thiết kế quỹ đạo với đường cong  3  spline
Bài toán thiết kế quỹ đạo ở đây nhằm tìm ra một quỹ
đạo trơn đảm bảo điều kiện rằng buộc về tốc độ (1)
cho rô bốt (2) khởi động từ cấu hình ban đầu
PA ( X (0), Y (0),  (0)) di chuyển đến cấu hình đích

b0

YA ,

b1

h sin qA , g1B

b2
b3
b4
b5
b6

0,,

0, ,
35(YB YA ) 20b1 10b2 4b3 15g1B ,
84(YB YA ) 45b1 20b2 6b3 39g1B ,
70(YB YA ) 36b1 15b2 4b3 34g1B ,

b7

20(YB

h sin qB ,

YA ) 10b1

4b2

b3

10g1B .

PB ( X (t f ), Y (t f ),  (t f )) trong thời gian t f  0 .
Ta biết rằng với
( X (t f ), Y (t f ), q(t f )); t f

bất kỳ cấu hình đích
0 , cũng có thể tạo được một

quỹ đạo  3  spline ( G ) cho hệ (2) tiếp cận được
3

bằng

các

tín

hiệu

điều

khiển

v(t ),  (t ) ;

v(t )  0 t  [0, t f ] . Ngược lại, với quỹ đạo

G 3 đã

G 3 đi qua điểm A và B.
3
Các điểm trên quỹ đạo G còn được gọi là way point
H. 4. Quỹ đạo

cho trước thì luôn tồn tại cặp tín hiệu điều khiển
v(t ),  (t ) với v(t )  0 t  [0, t f ] và các điều kiện
ban đầu để hệ (2) bám sát quỹ đạo

(WP) bởi rô bốt có thể di chuyển qua các điểm này để
tới điểm đích định trước. Giả thiết có N điểm WP

3

G đã cho [7].

Giả thiết vA  0 và vB  0 , góc lêch  A và  B giữa
trục X và các véc tơ đơn vị tiếp tuyến (tangent) phải
trùng với góc lệch của mobile rô bốt tại các điểm ban

VCCA-2011

3

trên quỹ đạo G ( số điểm WP N có thể tùy chọn và
nằm trong dải giá trị tới hạn), ta có N  1 đoạn thẳng
được nối giữa các điểm bao gồm cả điểm đầu và điểm

271


Hội nghị toàn quốc về Điều khiển và Tự động hoá - VCCA-2011
đích. Như vậy để thực hiện tác vụ (I) và (III) trong
bài toán nâng hạ kệ hàng đã đặt ra, bộ tạo quỹ đạo cho

quá trình chuyển động sử dụng h 3
toàn phù hợp với mô hình rô bốt (2).

H 5. Sơ đồ khối của vòng điều khiển rô bốt di động bám quỹ đạo

 X ed   cos 
d
 d 

Y


sin

 e  
d
 d  0
 e  

3

Chúng ta sẽ sử dụng quỹ đạo G với các thông số
được tính theo phương trình (3), (4) và (5) trong việc
tính toán vận tốc tuyến tính và vận tốc góc cho rô bốt
ở phần tiếp theo.
3.2 Xác định vận tốc tuyến tính và vận tốc góc
điều khiển rô bốt di động
Sau khi có được tập các điểm WP, ta cần điều khiển
rô bốt di chuyển tuần tự qua các điểm WP để tới đích.
Sơ đồ khối của vòng điều khiển kín rô bốt được mô tả
trong Hình 5. Dựa vào sai số giữa quỹ đạo thực tế và
quỹ đạo G 3 đã thiết kế ta có thể sử dụng phương
pháp điều khiển tối ưu để tính vận tốc góc  trong
khi vận tốc tuyến tính của rô bốt được tính toán phụ
thuộc vào vị trí của các chướng ngại vật trong khu
vực môi trường rô bốt hoạt động. Vận tốc tuyến tính
và vận tốc góc của rô bốt được đảm bảo luôn nằm
trong vùng giới hạn đã định trước khi được gửi xuống
cơ cấu chấp hành của rô bốt.

Gọi

sin  d
cos  d
0

spline là hoàn

G3 .
0  X  X d 
0   Y  Yd 
1      d 

(6)

(vd ,  d ) là vận tốc tuyến tính và vận tốc góc

mong muốn. Mục tiêu điều khiển là xác định tốc độ
dài ν và tốc độ góc w của rô bốt sao cho các sai lệnh
về vị trí và góc trong (6) tiến về 0. Chiến lược điều
khiển ở đây là đặt ν ν d trước và tìm w sao cho rô
bốt bám sát quỹ đạo G 3 . Đặt e

[Ye , qe ]T là véc tơ

sai lệch, bao gồm sai lệch ngang Ye và sai lệch góc

qe . Từ hệ phương trình động học (2) và với giả thiết
sai lệch góc qed nhỏ ta có thể tính được đạo hàm của

véc tơ sai lệch nhìn trông hệ tọa độ (Od X d Y d ) như
sau:
Yed  vd ed
,
(7)
ed    d
Hệ phương trình trạng thái của (7) có dạng :
(8)
e = Ace + bcue ,

điểm WP trên G 3 . Định nghĩa hệ tọa độ quỹ
đạo (Od X d Y d ) như mô tả trong hình 6. Hệ tọa độ quỹ

0 vd 
0
trong đó Ac  
; bc    ; ue    d là tín

0 0 
1 
hiệu vô hướng.
Do rô bốt Pioneer P3-DX có cung cấp các số liệu đo
dưới dạng số nên ta chuyển hệ phương trình sai số (8)
sang dạng rời rạc để có thể tính được tín hiệu điều
khiển tốc độ quay của rô bốt trong từng thời điểm cắt
mẫu:
(9)
e(k  1)  Ae(k )  bue (k ) ,

đạo thực chất là hệ tọa độ gốc (OXY ) bị tịnh tiến một

trong đó A = I + Ac Ts , B = BcTs , và

H.6: Hệ tọa độ quỹ đạo (Od X d Y d )
Giả thiết ( X d , Yd , d ) là vị trí và hướng tức thời của

Ts là thời gian

đoạn X d ; Yd theo hướng X, Y và quay xung quanh

lấy mẫu. Tín hiệu điều khiển tối ưu ue  k  được xác

trục Z một góc qd . Sai số vị trí và góc giữa quỹ đạo

định sao cho tối thiểu hóa hàm mục tiêu sau:
N

thực của rô bốt và quỹ đạo G tính trong hệ tọa độ
(OXY ) là X e X X d ; X e Y Yd ; qe q qd .
Các sai số này nếu nhìn tính trong hệ tọa độ quỹ đạo
(Od X d Y d ) sẽ được tính như sau:

VCCA-2011



J   eT  k  Q  k  e  k   ueT  k  R  k  ue  k 

3

k 0



(10)

trong đó Q và R là các ma trận trọng số, Q  0 ,
R  0 . Ma trận Q được chọn phụ thuộc vào sai lệch

272


Hội nghị toàn quốc về Điều khiển và Tự động hoá - VCCA-2011
c1

ngang Q(1,1) 

trong đó

phương trình động học (2) của rô bốt ta có thể xây
dựng mô hình rời rạc bị can nhiễu như sau :
 xk 
 
(20)
x k   yk   f  x k 1 , u k 1   ξ k 1
 k 

c1 , c2 là các

c1

 c2 | Ye |
50
hằng số dương. Với cách lựa chọn này nếu rô bốt cách
3

xa quỹ đạo G , trọng số của độ lệch ngang nhỏ hơn,
và nếu rô bốt gần với quỹ đạo thì Q(1, 1) trở nên lớn
hơn. Tín hiệu điều khiển ue được tính toán theo luật
điều khiển tối ưu toàn phương:
(11)
ue*  k   kTo  k  e  k 

trong đó

sRk 1  sLk 1
cos k 1  k 1 / 2    xk 1
2
(21)
sRk 1  sLk 1
yk  f y  yk 1 
sin k 1  k 1 / 2    yk 1
2
(22)
sRk 1  sLk 1
(21)
k  f   k 1 
  k 1
lw
xk  f x  xk 1 

Trong đó véc tơ phản hồi trạng thái tối ưu được tính
ntheo biểu thức



k o = b T Mo b + R



-1

b T Mo A

(12)

Ở đây M o là ma trận đối xứng, xác định dương là
nghiệm của của phương trình Ricatti:



Mo - A T M o A - Q + A T M o b R + b T M o b



-1

Trong đó

b T Mo A  0

thống, có kỳ vọng là 0 và ma trận hiệp biến là M k .

(13)
Từ đó suy ra

 k 
*

 ue*

 k   d  k 

T

uk  sLk , sRk  là tín hiệu đo được từ các encorder
bánh xe trái và phải của rô bốt. Tín hiệu đầu ra của bộ
lọc Kalman là:
(22)
z k  h(xk )  ζk
(23)
z1k hx xk z1k
(24)
z2k hy yk z2k

(14)

3. Sử dụng bộ lọc Kalman mở rộng cho
ước lượng vị trí và hướng của rô bốt
Do các số liệu đo từ rô bốt bị can nhiễu nên cần phải
có phương pháp ước lượng phù hợp để có thể xác

định được vị trí và hướng tức thời của rô bốt chính
xác hơn Trong báo cáo này, bộ lọc Kalman mở rộng
được sử dụng do mô hình động học của rô bốt di động
là mô hình phi tuyến. Đầu ra của bộ lọc Kalman sẽ
được sử dụng như các giá trị phản hồi (X,Y,) trong
vòng điều khiển ở H.5.
Gọi

z3k

(25)
T

z1k , z2k , z3k

là tín hiệu đầu ra xác
T

được xác định bằng thực nghiệm: Mk

10 4.I3 3 ;

Nk 10 2.I3 3 .
Bộ lọc Kalman mở rộng ước lượng trạng thái của hệ
thống

xˆ k và ma trận hiệp biến sai lệch trạng thái Pk

dựa trên mô hình hệ thống f(.) trong pha dự đoán:
xˆ k  f  xˆ k 1 , uk 1 

(26)

(15)

sRk  sLk
(16)
lw
Từ các số liệu này ta có thể tiếp tục tính ra được sự
dịch chuyển của rô bột trong hệ tọa độ cố định là:
xk  sk cos k  k / 2
(17)


sin ˆ


/ 2

xˆk  f x  xˆk 1  sˆk 1 cos ˆk 1  ˆk 1 / 2
yˆ k  f y  yˆ k 1  sˆk 1

k 1

 ˆk 1

ˆk  f  ˆk 1  ˆk 1

k

P  Ak 1Pk 1A

(18)

T
k 1

sai lệch trạng thái



(27)
(28)
(29)

 Mk 1

(30)

Véc tơ trạng thái ước lượng

sRk  sLk
(19)
lw
Do các số liệu đo từ các encoder của hai bánh xe bị
cân nhiễu nên các dữ liệu hiệu này là các dữ liệu bị
can nhiễu nên cần có phương pháp lọc phù hợp để có
thế xác định được vị trí và hướng của rô bốt tốt hơn.
Ở đây chúng tôi chọn phương pháp lọc Kalman mở
rộng dạng rời rạc với phương trình trạng thái là hệ
động học rời rạc bị can nhiễu của rô bốt. Từ hệ

 k 

VCCA-2011

z3 k

z1k , z 2k , z3k là
nhiễu đầu ra có kỳ vọng bằng 0 và ma trận hiệp biến
là N k . Ở đây, Các ma trận hiệp biến của các nhiễu

 k 

yk  sk sin k  k / 2

qk

định tọa độ và hướng của rô bốt; ζ k

bánh xe bên trái và bên phải của rô bốt trong khoảng
thời gian trích mẫu Ts. Khoảng cách mà rô bốt đi
được và sự thay đổi hướng của rô bốt trong khoảng
thời gian Ts được tính như sau:

sRk  sLk
2

hq

Trong đó z k

sLk , sRk lần lượt là khoảng cách đi được của

sk 

ξ k  [ x (k ),  y (k ),  (k )] là nhiễu hệ

xˆ k và ma trận hiệp biến

Pk trong pha cập nhật như sau:

K k  Pk HTk Hk Pk HTk  Nk



 



1

(31)

xˆ k  xˆ k  K k z k  h xˆ -k

(32)

Pk   I  K k Hk  Pk

(33)

Trong đó I là ma trận đơn vị. Các ma trận A k , H k là
các ma trận Jacobi được tính như sau :

273


Hội nghị toàn quốc về Điều khiển và Tự động hoá - VCCA-2011

Ak

1

Hk

f
xT

xˆ k 1 ,u k

h
xT

xˆ k

(34)
1

(35)

Tính chi tiết các ma trận Jacobi A k và H k ta được :

 f x f x f x 
 x y
 k 
k
 k

 f y f y f y 
Ak  

 xk yk  k 
 f f f 


 xk yk  k 

 k  

1 0 sk .sin   k 

2  



  

 0 1 sk .cos   k  k  
2 


0 0


1




Hk  I33 .
Như vậy bằng phương pháp sử dụng bộ lọc Kalman
mở rộng như trên, ta sẽ xác định được vị trí và
hướng của rô bốt tại thời điểm k chính là véc tơ
T

H.7:Quỹ đạo mong muốn và quỹ đạo thực của rô bốt.

T

xˆ k   xˆk , yˆ k ,ˆk  . Tín hiệu xˆ k   xˆk , yˆ k ,ˆk  sẽ




được sử dụng trong vòng điều khiển ở Hình 5.
4. Kết quả thực nghiệm
Phần này trình bày kết quả thử nghiệm khi áp dụng
thuật toán thiết kế quỹ đạo và điều khiển rô bốt thực
hiện các tác vụ (I) và (III) ở phần trên. Thử nghiệm
được thực hiện trên rô bốt Pioneer P3-DX có kích
thước W×H=0,44×38m. Tốc độ lớn nhất của vận tốc
tuyến tính và vận tốc góc lần lượt là vmax  0.5 m/s và
max  0.523 rad/s. Tất cả các quá trình thiết kế quỹ

đạo/điều khiển rô bốt,... được thực hiện bởi ngôn ngữ
lập trình C++, thời gian trích mẫu là 100 ms.
Giả thiết vị trí và hướng ban đầu của rô bốt tại A, vị
trí và hướng của kệ hàng tại B và vị trí và hướng của
đích cuối cùng là C. Rô bốt đi từ cấu hình A(0m; 0m;
0o) B(1,4m; 1,4m; 90o) C(0,2m; 2,8m; 90o). Kết
quả tính toán thu được như sau: B(1,3960m; 1,3947m;
90,0460o); C(0,2030m; 2,7951m; 89,8418o).
Trong thực nghiệm, trước mắt chúng tôi chỉ dung
thiết bị đo duy nhất là encoder (đo tương đối) để tính
vận tốc tức thời và dự đoán tọa độ, hướng rô bốt mà
chưa sử dụng cảm biến ngoài như camera để đo vị trí
tuyệt đối của rô bốt trong không gian hoạt động nên
bộ lọc Kalman mở rộng chưa thể phản ảnh chính xác
vị trí thực của rô bốt. Do đó, trên thực tế khi gắp thả
kệ hàng, tọa độ của rô bốt thực bị sai lệch từ 1 đến 3
cm so với mong muốn; hướng thực bị sai lệch vài độ
so với mong muốn.

VCCA-2011

H.8: Rô bốt Pioneer P3-DX tiếp cận mục tiêu.
Trong tương lai, chúng tôi sẽ lắp thêm camera để đo
vị trí tuyệt đối của rô bốt và sẽ áp dụng phương pháp
phối hợp cảm biến (sensor fusion) giữa encoder và
camera trong bộ lọc Kalman mở rộng. Khi đó, đầu ra
của bộ lọc Kalman mở rộng sẽ phản ánh chính xác vị
trí và hướng của rô bốt hơn.
5. Kết luận
Báo cáo đã trình bày phương pháp điều khiển tự động

xe nâng hạ xắp xếp kệ hàng trong kho. Phần thiết ké
quỹ đạo sử dụng phương pháp thiết kế quỹ đạo
 3  spline . Hai tác vụ (I) và (III) được quy về bài
toán thiết kế thuật điều khiển để rô bốt có thể di
chuyển từ vị trí ban đầu tới điểm đích cho trước.
Phương trình động học của rô bốt P3-DX được mô tả
ngắn gọn. Thuật toán điều khiển được thiết kế theo
phương pháp điều khiển tối ưu theo chuẩn toàn
phương cho mô hình động học rô bốt rời rạc. Các tín
hiệu đo được đưa vào bộ lọc Kalman mở rộng dạng
rời rạc đẻ xác định vị trí và hướng tức thời của rô bốt.
Kết qủa thực nghiệm thu được cho thấy thuật toán
điều khiển hoàn toàn thực hiện được các nhiệm vụ đã
đặt ra như tiếp cận kệ hàng hay trở về một vị trí biết
trước.
Tài liệu tham khảo
[1]

274

J. Pagès, X. Armangué, J. Salvi, J. Freixenet
and J. Martis: A Computer Vision System for
Autonomous Forklift Vehicles in Industrial


Hội nghị toàn quốc về Điều khiển và Tự động hoá - VCCA-2011

[2]

[3]

[4]

[5]

Environments, Proc. of the 9th Mediterranean
Conf. on Control and Automation, 2001, pp.
379-384.
D. Lecking, O. Wulf, and B. Wagner: Variable
Pallet Pick-Up for Automatic Guided Vehicles
in Industrial Environments, Proc. of the IEEE
conference on Emerging Technologies and
Factory Automation ETFA’06, Prague 2006,
pp. 1169 - 1174.
N. Bellomo, E. Marcuzzi, L. Baglivo, M.
Pertile, E. Bertolazzi, and M. De Cecco: Pallet
Pose Estimation with LIDAR and Vision for
Autonomous Forklifts, Proceedings of the 13th
IFAC Symposium on Information Control
Problems in Manufacturing, Moscow, Russia,
June 3-5, 2009, pp. 612-617.
A. Widyotriatmo, B. Hong, and K.-S. Hong:
Predictive navigation of an autonomous
vehicle with nonholonomic and minimum
turning radius constraints, Journal of
Mechanical Science and Technology, 23
(2009) 381-388.
A. Piazzi, C. Guarino Lo Bianco, and M.

Pham Thuong Cat is a

Honorary Research Professor in
Computational
Sciences
of
Computer
and
Automation
Research Institute of the
Hungarian
Academy
of
Sciences. He is the Editor-inChief of the Journal of
Computer
Science
and
Cybernetics of the Vietnamese Academy of Science
and Technology (VAST) and a Senior Researcher of
the Institute of Information Technology of VAST. He
is the representative of the Vietnam NMO at the
International Federation of Automatic Control
(IFAC) and the Vice President of the Vietnamese
Association of Mechatronics. His research interests
include robotics, control theory, cellular neural
networks and embedded control systems. He coauthored 3 books and published over 140 papers on
national and international journals and conference
proceedings.
Nguyễn Văn Tính nhận bằng kỹ
sư tại Đại Học Bách Khoa Hà
Nội năm 2008. Anh đang làm
nghiên cứu viên tại Viện Công

nghệ Thông tin thuộc Viện Khoa
học Công nghệ Việt Nam, số 18
Hoàng Quốc Việt, Cầu Giấy, Hà
Nội.
T. T. Quyen Bui received her B.S.
and M.S. degrees in Hanoi
University
of
Technology,
Vietnam, in 2001 and 2006,
respectively. She is currently a
Ph.D. program student in the
School
of
Mechanical
Engineering, Pusan National
University, Korea. Her research interests include
robotics, vision systems, and navigation of autonomous
vehicles.
TS. Phạm Minh Tuấn tốt nghiệp
Đại học Bách khoa Hà Nội,
chuyên ngành Công nghệ Thông
tin năm 1997, sau đó nhận bằng
Thạc sĩ và Tiến sĩ về Kỹ thuật
Điều khiển ở trường Đại học
Công nghệ Nanyang, Singapore,
vào các năm 2002 và 2006. Từ
2006 đến 2011, ông làm việc tại
Phòng Công nghệ Tự động hóa,
Viện Công nghệ Thông tin, Viện KHCNVN. Hiện tại,

ông đang là Phó Giám đốc Trung tâm Điều khiển và
Khai thác Vệ tinh nhỏ thuộc Viện Công nghệ Vũ trụ,
Viện KHCNVN.
Lĩnh vực nghiên cứu chính của ông bao gồm điều
khiển xe tự hành, điều khiển tư thế vệ tinh, các hệ
nhúng, và hệ thống năng lượng tái tạo. Ông đã cho
đăng trên 30 bài báo tại các hội nghị, tạp chí trong
nước và quốc tế. Ông còn là thành viên Hội Cơ điện
tử Việt Nam.

Romano:  -Splines for the Smooth Path
Generation of Wheeled Mobile Robots, IEEE
Trans. Robotics, 23 (5) (2007) 1089-1095.
J. Connors, G. Elkaim: Experimental Results
for Spline Based Obstacle Avoidance of an
Off-Road Ground Vehicle, ION Global
Navigation Satellite Systems Conference ION
GNSS 2007, Fort Worth, TX, Sept. 25-28,
2007, pp. 1484-1490.
J. H. Wei and J. S. Liu: Collision-free
3

[6]

[7]

Composite  –Splines Generation for
Nonholonomic Mobile Robots by Parallel
Variable-Length Genetic Algorithm, Proc. of
the Int. Conf. on Computational Intelligence

for Modelling Control and Automation,
Vienna 2008, pp. 545 - 550.
M. Hentschel, O. Wulf and B. Wagner: A
Hybrid Feedback Controller for Car-LikeRobots, Proc. of the Int. Conf. on Informatics
in Control, Automation and Robotics
(ICINCO), 2006.
K. Kanjanawanishkul, M. Hofmeister: and A.
Zell, Path following with an optimal forward
velocity for a mobile robot, Proc. of the 7th
IFAC Symposium on Intelligent Autonomous
Vehicles (IAV), pp. 1-6, Lecce, Italy, Sept.
2010.
W. H. Kwon and S. Han: Receding horizon
control: Model predictive control for state
models, Springer-Verlag, London, 2005.
3

[8]

[9]

[10]

VCCA-2011

275