thiết kế quỹ đạo và điều khiển xe tự hành vận chuyển trong kho

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

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: [email protected]; [email protected] ; [email protected]; [email protected]

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 3 splineh 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 3 splineh with 7

th 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.

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

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 3 splineh đượ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].

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 3 splineh

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.

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

mailto:[email protected]




VCCA-2011

(trên) và mô hình của nó có hình dáng hình chữ nhật

(dưới).

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.

Ở đây 1l là chiều dài, 2l là chiều rộng, và wl 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 (no-

slippage), rô bốt phải chuyển động với rằng buộc về

tốc độ như sau:

sin cos .X Y (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 non-

holonomic (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:

cos

sin

X v

Y v

(2)

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 maxvv và max , trong đó

maxv 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à ),,( YX , kệ hàng có cấu hình là

),,( PPP YX , và cấu hình cần chuyển kệ hàng đến là

),,( GGG YX (Hình 2).

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.

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

X

Y

x y

θ v

1l

2l

wl

O

o

270


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à

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

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

(0)Av v , (0)Av v , (0)A , (0)A tới vị

trí và hướng đích ( ( ), ( ), ( ))B f f fP X t Y t t tại thời

điểm ft t với ( )B fv v t , ( )B fv v t , ( )B ft ,

( )B ft .

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 spline3

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

( (0), (0), (0))AP X Y di chuyển đến cấu hình đích

( ( ), ( ), ( ))B f f fP X t Y t t trong thời gian 0ft .

Ta biết rằng với bất kỳ cấu hình đích

( ( ), ( ), ( )); 0f f f fX t Y t t tq , cũng có thể tạo được một

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

bằng các tín hiệu điều khiển ( ), ( )v t t ;

( ) 0 [0, ]fv t t t . Ngược lại, với quỹ đạo 3G đã

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 ( ) 0 [0, ]fv t t t và các điều kiện

ban đầu để hệ (2) bám sát quỹ đạo 3G đã cho [7].

Giả thiết 0Av và 0Bv , 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

đầu và điểm đích (như trong Hình 4). Quỹ đạo 3G -

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

x ys p s p s P η , (3)

2 30 1 2 3

4 5 6 74 5 6 7

( )xp s s s s

s s s s

, (4)

2 30 1 2 3

4 5 6 74 5 6 7

( )yp s s s s

s s s s

, (5)

Trong đó [0,1]s là tham số quãng đường đã được

chuẩn hóa. Tại điểm khởi đầu s=0, ( )x

p s =XA,

( )y

p s =YA, và tại điểm kết thúc thì s=1, ( )x

p s =XB,

( )y

p s =YB .Các hệ số của đa thức ( )x

p s được tính

như sau:

|| , ||A B A BX X Y Y

0 AXa ,

1cos

Aa h q ,

1cos

Bg h q ,

2 0a ,

3 0a ,

4 1 2 3 1 235( ) 20 10 4 15 5B AX Xa a a a g g

5 1 2 3 184( ) 45 20 6 39B AX Xa a a a g ,

6 1 2 3 170( ) 36 15 4 34B AX Xa a a a g ,

7 1 2 3 120( ) 10 4 10B AX Xa a a a g . (6)

Các hệ số của đa thức ( )y

p s :

0 AYb ,

1sin

Ab h q , 1 sinB Bg h q ,

2 0b , ,

3 0b , ,

4 1 2 3 135( ) 20 10 4 15B A BY Yb b b b g ,

5 1 2 3 184( ) 45 20 6 39B A BY Yb b b b g ,

6 1 2 3 170( ) 36 15 4 34B A BY Yb b b b g ,

7 1 2 3 120( ) 10 4 10B A BY Yb b b b g .

H. 4. Quỹ đạo 3G đi qua điểm A và B.

Các điểm trên quỹ đạo 3G còn được gọi là way point

(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

trên quỹ đạo 3G ( 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ó 1N đoạn thẳng

được nối giữa các điểm bao gồm cả điểm đầu và điểm

271


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 3 splineh là hoàn

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 3G .

Chúng ta sẽ sử dụng quỹ đạo 3G 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 3G đã 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.

H.6: Hệ tọa độ quỹ đạo ( )d d dO X Y

Giả thiết ( , , )d d dX Y là vị trí và hướng tức thời của

điểm WP trên 3G . Định nghĩa hệ tọa độ quỹ

đạo ( )d d dO X Y như mô tả trong hình 6. Hệ tọa độ quỹ

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

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

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

thực của rô bốt và quỹ đạo 3G tính trong hệ tọa độ

( )OXY là ; ;e d e d e dX X X X Y Y q q q .

Các sai số này nếu nhìn tính trong hệ tọa độ quỹ đạo

( )d d dO X Y sẽ được tính như sau:

cos sin 0

sin cos 0

0 0 1

de d d d

de d d d

dde

X X X

Y Y Y

(6)

Gọi ),( ddv 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 3G . Đặt [ , ]T

e eY qe là véc tơ

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

eq . Từ hệ phương trình động học (2) và với giả thiết

sai lệch góc d

eq 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 độ ( )d d dO X Y như

sau: d d

e d e

de d

Y v

, (7)

Hệ phương trình trạng thái của (7) có dạng :

euc ce = A e +b , (8)

trong đó 0

0 0

dvcA

; 0

1cb

; e du là tín

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:

( 1) ( ) ( )ek k u ke Ae b , (9)

trong đó sTcA = I + A , sTcB = B , và sT là thời gian

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

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

0

NT T

e e

k

J k k k u k R k u ke Q e

(10)

trong đó Q và R là các ma trận trọng số, 0Q ,

0R . Ma trận Q được chọn phụ thuộc vào sai lệch

272


VCCA-2011

ngang

||50

)1,1(

21

1

eYcc

cQ

trong đó 21, cc là các

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

xa quỹ đạo 3G , trọng số của độ lệch ngang nhỏ hơn,

và nếu rô bốt gần với quỹ đạo thì (1, 1)Q 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:

* Te ou k k kk e (11)

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

ntheo biểu thức

R-1

T T

o o ok = b M b + b M A (12)

Ở đây oM là ma trận đối xứng, xác định dương là

nghiệm của của phương trình Ricatti:

R-1

T T T T

o o o o oM - A M A -Q + A M b +b M b b M A 0

(13)

Từ đó suy ra

* *e dk u k k (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 ,Lk Rks s lần lượt là khoảng cách đi được của

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:

2

Rk Lkk

s ss

(15)

w

Rk Lkk

s s

l

(16)

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à:

cos / 2k k k kx s (17)

sin / 2k k k ky s (18)

w

Rk Lkk

s s

l

(19)

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ệ

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 :

1 1 1,

k

k k k k k

k

x

yx f x u ξ

(20)

trong đó

1 11 1 1 1cos / 2

2

Rk Lkk x k k k xk

s sx f x

(21)

1 11 1 1 1sin / 2

2

Rk Lkk y k k k yk

s sy f y

(22) 1 1

1 1

w

Rk Lkk k k

s sf

l

(21)

Trong đó x y

[ ( ), ( ), ( )]k

k k kξ

là nhiễu hệ

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

M .

,T

k Lk Rks su 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à:

( )k k kz h x ζ (22)

1 1k x k kz h x z (23)

2 2k y k kz h y z (24)

3 3k k kz hq q z (25)

Trong đó 1 2 3, ,T

k k k kz z zz là tín hiệu đầu ra xác

định tọa độ và hướng của rô bốt; 1 2 3, ,

T

k k k kz z zζ là

nhiễu đầu ra có kỳ vọng bằng 0 và ma trận hiệp biến

là k

N . Ở đây, Các ma trận hiệp biến của các nhiễu

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

3 310 .kM I ;

2

3 310 .kN I .

Bộ lọc Kalman mở rộng ước lượng trạng thái của hệ

thống ˆkx

và ma trận hiệp biến sai lệch trạng thái kP

dựa trên mô hình hệ thống f(.) trong pha dự đoán:

1 1ˆ ˆ ,k k kx f x u

(26)

1 1 1 1ˆ ˆˆ ˆ ˆ cos / 2k x k k k kx f x s

(27)

1 1 1 1ˆ ˆˆ ˆ ˆ sin / 2k y k k k ky f y s

(28)

1 1ˆ ˆ ˆk k kf

(29)

k 1 1 1 1

T

k k k kP A P A M

(30)

Véc tơ trạng thái ước lượng ˆkx và ma trận hiệp biến

sai lệch trạng thái kP trong pha cập nhật như sau:

1

T T

k k k k k k kK P H H P H N

(31)

kˆ ˆ ˆ

k k k k-

x x K z h x (32)

k k k kP I K H P (33)

Trong đó I là ma trận đơn vị. Các ma trận ,k k

A H là

các ma trận Jacobi được tính như sau :

273


VCCA-2011

1 1

1

ˆ ,k k

k T

x u

fA

x (34)

ˆ k

k T

x

hH

x (35)

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

x x x

k k k

y y y

k

k k k

k k k

f f f

x y

f f f

x y

f f f

x y

A

1 0 .sin2

0 1 .cos2

0 0 1

kk k

kk k

s

s

3 3kH I .

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

k k k kx yx

. Tín hiệu ˆˆ ˆ ˆ, ,T

k k k kx yx

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à 5.0max v m/s và

523.0max 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; 90

o) C(0,2m; 2,8m; 90

o). Kết

quả tính toán thu được như sau: B(1,3960m; 1,3947m;

90,0460o); C(0,2030m; 2,7951m; 89,8418

o).

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.

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

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

spline3 . 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] J. Pagès, X. Armangué, J. Salvi, J. Freixenet

and J. Martis: A Computer Vision System for

Autonomous Forklift Vehicles in Industrial

274


VCCA-2011

Environments, Proc. of the 9th Mediterranean

Conf. on Control and Automation, 2001, pp.

379-384.

[2] 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.

[3] 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.

[4] 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.

[5] A. Piazzi, C. Guarino Lo Bianco, and M.

Romano: 3 -Splines for the Smooth Path

Generation of Wheeled Mobile Robots, IEEE

Trans. Robotics, 23 (5) (2007) 1089-1095.

[6] 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.

[7] J. H. Wei and J. S. Liu: Collision-free

Composite 3 –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.

[8] M. Hentschel, O. Wulf and B. Wagner: A

Hybrid Feedback Controller for Car-Like-

Robots, Proc. of the Int. Conf. on Informatics

in Control, Automation and Robotics

(ICINCO), 2006.

[9] 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.

[10] W. H. Kwon and S. Han: Receding horizon

control: Model predictive control for state

models, Springer-Verlag, London, 2005.

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-in-

Chief 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 co-

authored 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.

275

thiết kế quỹ đạo và điều khiển xe tự hành vận chuyển trong kho

Documents