Ứng dụng kỹ thuật NeuralFuzzy trong vấn đề robot tự hành

ĐẠI HỌC QUỐC GIA THÀNH PHỐ HỒ CHÍ MINH
TRƯỜNG ĐẠI HỌC KHOA HỌC TỰ NHIÊN


NGUYỄN HỮU HIẾU






ỨNG DỤNG KỸ THUẬT NEURAL-FUZZY
TRONG VẤN ĐỀ ROBOT TỰ HÀNH





Chuyên ngành: VẬT LÝ ĐIỆN TỬ HƯỚNG KỸ THUẬT
Mã số: 60.44.03



LUẬN VĂN THẠC SỸ VẬT LÝ ĐIỆN TỬ HƯỚNG KỸ THUẬT


HƯỚNG DẪN KHOA HỌC
PGS.TS. DƯƠNG HOÀI NGHĨA

TP. HỒ CHÍ MINH - 2009
1


Lời cảm ơn!
ఇ♥ఇ

Trong suốt thời gian học tập và nghiên cứu khoa học tại trường Đại
học Khoa học Tự nhiên, tôi luôn nhận được sự động viên, giúp đỡ từ quý thầy
cô, bạn bè và gia đình. Với sự giúp đỡ đó, hôm nay, tôi đã hoàn thành luận
văn cao học.
Trước tiên, em xin gỡi lời cảm ơn đến thầy PGS.TS. Nguyễn Hữu
Phương và thầy PGS.TS. Dương Hoài Nghĩa đã tận tình hướ
ng dẫn, giúp đỡ
em trong thời gian qua.
Xin cảm ơn tất cả các thầy cô đã giảng dạy, truyền thụ cho em kiến thức
quý báu trong suốt thời gian học tập và nghiên cứu. Nhờ đó, em đã đủ kiến
thức để hoàn thành luận văn này.
Xin cảm ơn các bạn học viên cao học cùng khóa đã cùng bàn bạc, hỗ
trợ cho tôi hoàn thành luận văn tốt nghiệp.
Xin cảm ơn ba mẹ, anh chị trong gia đ
ình đã luôn sát cánh, động viên
để con hoàn thành chương trình học cao học.



Xin trân trọng và cảm ơn tất cả!

Nguyễn Hữu Hiếu.

2


Danh mục các từ viết tắt



EKF Extension Kalman Filter
FLC fuzzy logic
KF Kalman Filter
MISO Multi Input Single Output
MIMO Multi Input Multi Output
PCA Principal component analysis network
SISO Single Input Single Output













3

Danh mục các hình ảnh trong đề tài.

Hình1.1.Tổng thể hình dạng robot
Hình1.2.Sơ đồ khối của robot

Hình 2.1 Sơ đồ khối của hệ thống điều khiển Fuzzy logic.
Hình 2.2 Ảnh ba chiều được tạo thành từ hai bức ảnh nổi.
Hình 2.3 Robot tránh vật cản và tạo ra sai số định hướng
Hình 2.4 Cấu tạo khối của robot tự hành.
Hình 2.5. Cấu trúc bộ phận điều khiển chuyển động của robot.
Hình 2.6 Sơ đồ kh
ối giải thuật chuyển động của robot.
Hình 2.7 Mô hình Kinematic.
Hình 2.8 Sơ đồ khối hệ thống điều khiển chuyển động
Hình 2.9 Các hàm thành viên của sai số góc.
Hình 2.10 Các hàm thành viên của vận tốc bánh xe.
Hình 2.11 Các hàm thành viên của sai số khoảng cách.
Hình 2.12 Sơ đồ khối của kỹ thuật dùng ảnh nổi để đánh dấu điểm tương ứng.
Hình 2.13 Tránh vật cản của robot
Hình 2.14. Mô hình robot.
Hình 2.15. Robot di chuyển đến điểm goal và tránh vật c
ản trên đường đi.

Hình 3.1 Mạng neural PCA
Hình 3.2 Chi tiết trong mạng PCA.
Hình 3.3 Các hướng đi trong không gian.
Hình 3.4 Dạng của mạng neural MLP.
4

Hình 3.5 Môi trường dùng cho robot học hỏi.
Hình 3.6. Dãy 29 sóng siêu âm dò tìm.
Hình 3.7 Robot chuyển động gần của ra vào.
Hình 3.8 Mô hình robot dùng trong thí nghiệm.
Hình 3.9. Đường đi của robot trong môi trường bất kỳ.

Hình 4.1 Sơ đồ bố trí các sensor trong robot.
Hình 4.2 Hành động của robot dựa vào các sensor.
Hình 4.3 Sơ đồ điều khiển hành động của robot.
Hình 4.4 Trường hợp chỉ sensor O
2
dò được vật cản.
Hình 4.5 Trường hợp hai sensor dò được vật cản.
Hình 4.6 Hàm thành viên dạng tam giác cho Go-Tangent xéo góc
và Go-Tangent-Front.
Hình 4.7 Mạng Neural với các trọng số {a
ij
,b
ij
} và {p
k
,q
k
,r
k
}.
Hình 4.8 Các phương sai trong quá trình huấn luyện cho Go-Tangent xéo góc.
Hình 4.9 Hàm thành viên với hai tín hiệu input (khoảng cách và vận tốc).

Hình 4.10. Cấu trúc bốn môi trường thử nghiệm.

Hình 5.1 Mạng neural với các neural fuzzy.
Hình 5.2. Mạng neural 2 layer.
Hình 5.3 Hệ thống neural fuzzy với 2 luật fuzzy.

Hình.6.1. Hệ thống neural – fuzzy với một luật fuzzy.
Hình.6.2. Sơ đồ khối giải thuật tìm đường về đích của robot tự hành.
Hình.6.3. Robot tự tìm về đích trong môi trường thứ 1.
5


Hình.6.4. Robot tự tìm về đích trong môi trường thứ 2.
Hình.6.5. Robot tự tìm về đích trong môi trường thứ 3.
Hình.6.6. Robot tự tìm về đích trong môi trường thứ 4.

Hình PL.1. Giải thích các linh kiện trên robot.
Hình PL.2.Động cơ bước và đế robot
Hình PL.3 Sơ đồ lắp ráp các linh kiện robot.
Hình PL.4. Các bánh xe lắp vào động cơ bước.
Hình PL.5. Mạch điện tử điều khiển robot.
Hình PL.6. Lắp mạch điện tử cho robot.
Hình PL.7. Bo mạch điện tử tạ
o âm thanh cho robot.
Hình PL.8. Lắp ắc quy cho robot.
Hình PL.9. Robot được lắp ráp hoàn chỉnh.
Hình PL.10. Mạch điện tử của robot nhìn từ mặt dưới.
Hình PL.11. Mạch điện tử của robot nhìn từ mặt trên.

6


CHƯƠNG 1. TỔNG QUAN.

1.1 ROBOT TỰ HÀNH.
Ngày nay, robot tự hành được ứng dụng trong nhiều lĩnh vực khác nhau. Ví dụ:
robot chọn màu; cánh tay robot chọn lựa hàng hóa; robot dò tìm đường đi….
Trong đó, robot tự hành trong việc dò đường là vấn đề đang được quan tâm.
Cấu tạo dạng chung của robot dạng này gồm các hệ thống sau: hệ thống xử lý
thông tin; hệ thống dò tìm vật cản; hệ thống chuyển động.
Hệ thống dò tìm vật cản: dùng các sensor hồng ngoại hay sensor siêu âm
để
nhận biết vật cản xung quanh.
Hệ thống chuyển động: bao gồm các bánh xe và hệ thống truyền động. Hệ
thống này giúp robot chuyển động theo yêu cầu của hệ thống xử lý thông tin.
Hệ thống xử lý thông tin: có nhiệm vụ xử lý các thông tin từ hệ thống dò tìm
vật cản và đưa ra hành động cụ thể cho hệ thống chuyển động.
*Các kỹ thuật đã được ứng dụ
ng trong vấn đề robot tự hành:
Điều khiển fuzzy logic (FLC) đã được ứng dụng cho việc điều khiển robot.
Việc điều khiển này dựa vào hệ thống luật.

Bên cạnh đó, mạng neural cũng được ứng dụng vào khoa học robot tự hành.
Với mạng neural, robot có thể được học hỏi, được huấn luyện. Từ đó, mạng neural
sẽ hiệu chỉnh các thông số truyền gi
ữa các lớp neural nhằm tạo ra hệ thống tối ưu,
sát với yêu cầu thực tế.
Kết hợp fuzzy và neural trong khoa học robot tự hành là một hướng đi mới.
Việc kết hợp này có nhiều ưu điểm hơn so với chỉ dùng fuzzy hoặc neural. Hệ
thống này thân thiện hơn với người dùng thông qua các hệ thống luật fuzzy và có
thể huấn luyện, điều chỉnh các thông số truyền gi
ữa các lớp mạng neural. Do đó, hệ
thống tốn ít thời gian để xử lý thông tin.





7


1.2. CÁC VẤN ĐỀ CỦA ROBOT TỰ HÀNH.
*Vấn đề tìm hướng đi: xác định điểm đích cần đến. Trong quá trình chuyển
động, robot cần xác định vị trí vật cản và thực hiện việc tránh vật cản. Robot luôn luôn
cập nhật các vật cản xung quanh và tính toán lộ trình để đi về điểm đích.
*Việc làm được: robot có thể đi đến điểm đích một cách an toàn không va cham
với vậ
t cản.
*Việc chưa làm được: trong quá trình chuyển động thực tế, robot sẽ gặp khó
khăn do có sai số trong việc đo đạc khoảng cách, sai số vòng quay bánh xe, sai số
trong việc quay chuyển góc… Thêm vào đó, thời gian xử lý của hệ thống robot cũng
cần phải tối ưu hơn

1.3. CẤU TRÚC ROBOT TỰ HÀNH THƯỜNG GẶP.
Đây là dạng robot thường gặp.




Hình1.1.Tổng thể hình dạng robot



8


Tổng thể:
*Robot dạng hình trụ.
*2 động cơ bước.
*Các đèn led.
*Các sensor âm thanh, hình ảnh.
*Cổng kết nối giao tiếp Bluetooth.
*Mạch điện tử điều khiển




Hình1.2.Sơ đồ khối của robot
9


CHƯƠNG 2.
FUZZY LOGIC VÀ LỌC KALMAN TRONG

VIỆC ĐIỀU KHIỂN ROBOT TỰ HÀNH


2.1. GIỚI THIỆU.
Việc nghiên cứu robot tự hành đã xuất hiện từ lâu nhưng vẫn còn nhiều vấn đề
cần nghiên cứu tiếp. Đã có nhiều nghiên cứu về sự định hướng của robot ở môi trường
đã biết và chưa biết.
Các tác giả (Toshido Fukuda, Fellow và Naoyuki Kubota (1999)) đã chú ý đến hệ
thống robot tự hành bằng kỹ thuật Fuzzy và đưa ra mạng các sensor mà cho phép các
robot có thể nhận được môi trường. Trong việ
c đánh giá các vật cản, các tác giả
(Alexander Suppes, Frank Suhling và Michael hotter (2001)) đã đưa ra giải pháp dùng
Video để dò tìm vật cản dựa vào các ước lượng bức ảnh dữ liệu. Việc xác định các vật
cản dựa vào các ngưỡng xác định trong quá trình xử lý ảnh. Các tác giả cũng xác định
các lỗi thuộc hệ thống và các lỗi không thuộc hệ thống trong quá trình chuyển động
của robot như là sai số đo đạc, sai số giữa đường kính các bánh xe, sai s
ố vận tốc góc
giữa các bánh xe.
2.2. KỸ THUẬT ĐỊNH HƯỚNG CHO ROBOT.
Ở đây, tác giả dùng nguyên tắc ước lượng vị trí robot. Ý tưởng là dựa vào thuật
toán để xác định vị trí hiện tại của robot bằng cách từ vị trí trước đó, tính được vị trí
hiện tại khi biết được hành trình mà nó đã đi. Điều này được thực hiện nhờ vào việc
tính toán các vòng quay bánh xe. Nhờ đó, độ chính xác và tốc
độ robot cao hơn.
2.2.1 Lọc Kalman
Lọc Kalman (KF) được dùng rộng rãi trong nghiên cứu các hệ thống động, phân tích,
tiên đoán, ước lượng. Lọc Kalman là giải thuật tối ưu cho vấn đề lọc tuyến tính dữ liệu
rời rạc KF rất hữu dụng trong vấn đề như là: nó giúp ước lượng, dự báo và nhận dạng
mô hình.
Tuy nhiên, hệ thống thực tế thì không tuyến tính. Ở đây, sự tuyến tính được tạ

o
ra bằng giải thuật xấp xỉ. Việc này kết hợp với KF tạo ta KF mở rộng (extension
Kalman Filter-(EKF)). Hệ thống rời rạc và dạng đo đạc được chỉ ra phương trình (2.1)
và (2.2) như sau:
10


111
(,;,)
KKkKK
XfXuW
γ
−−−
= (2.1)
(;)
K
KK
Z
hX V= (2.2)
Trong đó, X
K
là vector trạng thái ở thời điểm k
Z
K
là vector tín hiệu đo đạc ở thời điểm K.
f(.) là hệ thống không tuyến tính.
Z(.) là hàm đo đạc ở thời điểm k.
u
K
là vector tín hiệu vào điều khiển ở thời điểm k.

W
K
, γ
K
và V
K
tương ứng là vector nhiễu ở tín hiệu trạng thái, ở đầu vào và ở
đầu ra.
Giả thiết W
k
, γ
K
, V
k
có phân bố Gauss với trung bình zero. Ma trận
Covariance lần lượt là Q, Г, R.
W
K
~ N(0,Q)
γ
K
~ N(0,Г)
V
K
~ N(0,R)
Tuyến tính hóa hệ thống quanh điểm gốc, ta được mô hình sau:
^
k
1
= A x + Bu

Hx
k
k
k
k
x
Z
∧
+
∧
⎧
⎪
⎨
⎪
=
⎩

Với:
0, u=0, = 0, = 0
f

x
x
A
ωγ
=
∂
=
∂

=0, u=0, =0, =0
f
=
u
x
B
ωγ
∂
∂

0, u=0, v=0
h
=
x
x
H
=
∂
∂

Giải thuật chính của EKF chia làm hai phần: phần tiên đoán và phần kiểm lỗi.
Phần tiên đoán: EKF tiên đoán vị trí tương lai của robot và dự đoán ma trận sai số
thống kê
−
K
P sử dụng phương trình (2.3), (2.4) như sau:
1
(,;0,0)
kkK

xfxu
∧
−
−
= (2.3)
TT
PAPABBQ
−
=+Γ+
(2.4)
Phần kiểm lỗi: phần này cập nhật các phép đo đạc dùng phương trình (2.5), (2.6), (2.7)
như sau:
1
()
TT
KPHHPHR
−−−
=+ (2.5)
11


((,0))
kk
kk
xx Kz hx
∧∧
∧
−−
=+ − (2.6)
()PIKHP

−
=− (2.7)
Trong đó, I là ma trận đơn vị.
2.2.2 Điều khiển Fuzzy logic.
Điều khiển Fuzzy logic có thể làm hệ thống hoạt động giống như cơ thể con
người. Kỹ thuật logic mờ dựa trên 4 khái niệm cơ bản sau đây: tập mờ, biến ngoi6n
ngữ và các luật hợp thành. Fuzzy logic có thể áp dụng cho nhiều hệ thống khác nhau.
Sơ đồ khối của Fuzzy logic được trình bày như hình 2.1 gồm bốn yế
u tố sau:
* Tập luật (If-then rules) chứa số lượng các luật Fuzzy logic, làm cho hệ thống hoạt
động tốt.
* “Bộ máy Fuzzy” làm cho việc quyết định đúng đắn nhất có thể.
* Bộ phận làm mờ hóa chuyển các giá trị đầu vào thành các thông tin mà “bộ máy
Fuzzy” có thể dùng để áp dụng luật Fuzzy.
* Bộ phận giải mờ sẽ chuyển kết luận cuối cùng thành yếu tố điều khiển duy nhất để
điều khiển hệ thống.












Hình 2.1 Sơ đồ khối của hệ thống điều khiển Fuzzy logic

2.2.3 Hệ thống nhìn vật nổi ba chiều.
Ý tưởng cơ bản của hệ thống nhìn vật nổi ba chiều là tìm các điểm ảnh và nội
suy vật thể ba chiều. Các điểm ảnh trong các bức ảnh nối thành phần là thành tố để tạo
thành vật th
ể ba chiều trong không gian ảnh khác. Những điểm ảnh thuộc về các bức
ảnh tương ứng của chúng sẽ có vị trí khác biệt nhau và được gọi là sự không tương
ứng (Marti Gaetan, 1997). Trạng thái lý tưởng được trình bày trong hình 2.2. Mỗi


Process
Fuzz
y
Lo
g
ic Controlle
r
Reference in
p
u
t

r(t)
Inferencen
mechanis
m
Rule-base
Fuzzification
Defuzzification
Input

u(t)
Outputs
y(t)
12


điểm ảnh trong không gian ba chiều sẽ được phân tích thành các vùng khác nhau trong
các bức ảnh làm nổi thành phần.


















Hình 2.2 Ảnh ba chiều được tạo thành từ hai bức ảnh nổi

Giả sử là điểm P trong mặt phẳng được chụp bởi hai camera khác nhau. Đó là P
L

và
P
R
. Sự không tương thích về vị trí của hai điểm ảnh giúp cho sự ước lượng về vị trí của
điểm ảnh tổng hợp trong không gian ba chiều. Cụ thể, vị trí điểm P có thể tính toán
thông qua các thông số của P
L
và P
R
và các thông số bên trong và bên ngoài của hai
camera. Như vậy hình vẽ 2.2, đây là cấu hình song song. Trong đó, điểm P(x, y, z)
được chụp bởi hai camera bên trái và bên phải tạo thành hai bức ảnh P
L
(x, y, z) và
P
R
(x, y, z). Từ đó, điểm P tương ứng có thể tính toán thông qua phương trình (2.8) sau:
⎟
⎟
⎠
⎞
⎜
⎜
⎝
⎛
−−
+
−
+
=

RLRL
RL
RL
RL
zyx
xx
bf
xx
yyb
xx
xxb
P ,
)(2
)(
,
)(2
)(
),,(
(2.8)
Trong đó, (x
L
-x
R
) là sự không tương thích, căn cứ vào đường b là khoảng cách giữa
camera bên trái và bên phải và f là chiều dài tiêu cự của camera.

2.3. HỆ THỐNG ĐIỀU KHIỂN CHUYỂN ĐỘNG VÀ CẤU HÌNH CỦA
ROBOT.
2.3.1 Hệ thống điều khiển chuyển động.
b

P(x,y,z)
y
R
Z
R
Z
L
P
L
P
R
y
L
x
L
x
R
O
L
O
R
13


Trong hầu hết chuyển động của robot, luôn có những vấn đề xảy ra khi robot đi
từ điểm xuất phát đến điểm đích. Sai sót có thể là sự định hướng không chính xác như
lúc đầu trong quá trình chuyển động.
Giả sử robot dịch chuyển từ vị trí P
0
đến vị trí P

g
trong môi trường không xác định.
Vật cản trong môi trường là O, trong khi robot đi từ điểm hiện hành P
K
đến điểm P
K+1

sự định hướng và vị trí của nó thay đổi. Đặc biệt, khi robot tránh vật cản và sau đó, nó
trả lại lộ trình ban đầu, sai số vị trí sẽ gia tăng như hình 2.3.
















Hình 2.3 Robot tránh vật cản và tạo ra sai số định hướng
2.3.2 Thiết kế robot tự hành.
Robot tự hành được trình bày như hình 2.4 và được dùng trong nghiên cứu, được xây
dựng trong thể tích của một hình trụ tròn và có ba bánh xe để di chuyển. Robot có
đường kính là 340mm, chiều cao là 170mm, có hai bánh xe lái phía tr

ước và môth
bánh xe phụ phía sau. Bộ cảm quang được bố trí trên trục của từng bánh xe dùng để đo
đạc.

P
o,
θ
o
Obstacle
P
1,
θ
1
P
2,
θ
2
P
s
,θ
s

P
2,
θ
2
P
k,
θ
k

14

Hình 2.4 Cấu tạo khối của robot tự hành.

Trong cấu tạo trên có ba module sử dụng sensor. Module thứ nhất đặt ở trung tâm
khung robot dùng để xác định hướng đi robot. Module thứ hai đặt dưới khung robot để
xác định vị trí robot. Module thứ ba đặt ở đỉnh robot dùng để chụp ảnh nổi cho robot
2.3.3 Điều khiển chuyển động.
Bộ phận điều khiển chuyể
n động gồm phần xử lý trung tâm PC104. Bộ xử lý
trung tâm thu thập thông tin từ 2 vi xử lý thành phần: đó là mã quang từ 2 bánh xe lái,
dữ liệu định hướng robot từ module sensor thứ 1, dữ liệu vị trí từ sensor thứ 2.






Motion Controllor board
Battery
Motor
Motor
240mm
208m
m
92.5mm
10mm
Position sensor
Encode
r

Encode
r

Driving wheel
Passive wheel
120
0
15



.

Hình 2.5. Cấu trúc bộ phận điều khiển chuyển động của robot.

Theo đó, thời gian tối đa để cập nhật lệnh điều khiển vị trí là khoảng 8ms. Thời gian
đáp ứng của động cơ được thiết lập trong khoảng 20ms.
Đặc tính của các sensor này như sau:
* Sensor định hướng: đây là bộ phận định hướng điện tử
với độ phân giải là 0.1 độ. Kỹ
thuật chia độ của bộ phận này cho phép loại bỏ sai sót của trường điện từ và ảnh
hưởng của địa cực bắc.
* Bộ mã hóa quang học:mỗi bánh xe robot được trang bị sensor mã quang với 200 lần
mã hóa trên một vòng quay. Điều này cho phép mỗi trục bánh xe có thể cung cấp
thông tin độc lập nhau và chính xác. Động cơ được thiết lập là 50 vòng/ 1 phút. Như
vậy thời gian gi
ữa hai xung mã hóa là khoảng 100ms.
* Sensor vị trí: đây là sensor điện tử dựa trên mạch điện mã hóa, sensor này cung cấp
hướng và tọa độ trên hai trục x và y. Độ phân giải thông thường là 400 tọa độ trên một
inch, điều này cho phép giảm thiểu việc tính toán trong quá trình chuyển động của
robot.
*Sensor chụp ảnh: sensor này gồm hai camera thành phần, hai camera có thể dùng để
xác định khoảng cách giữa các điểm trong khung từ 0.5 đến 6.0m.

Master Controller
X-Axis
LEAD/LAG
PULSE
Microcontroller II
COM 1
Stereo Vision
Sensor

M2
En1
M1
Microcontroller I
Vector
Compass
En2
COM 2
IEEE 1394
Velocity
PWM
si
g
nal
Position sensor
8-BIT
data
LEAD/LAG
PULSE
Y-Axis
LEAD/LAG
PULSE
16


2.3.4 Thuật giải điều khiển chuyển động
Hình 2.6 là sơ đồ khối của chương trình điều khiển chuyển động, trong đó, các
bức ảnh nổi thành phần được tính toán để xác định vị trí của vật cản ba chiều trên
đường đi của robot. Kết hợp lọc Kalman và kỹ thuật Fuzzy logic, việc tránh các vật
cản cũng như quá trình xác định vị trí, hướng của robot sẽ đượ

c cải tiến.
Giải thuật có thể được tóm tắt như sau:
1. Thiết lập vị trí bắt đầu và vị trí kết thúc.
2. Kiểm tra vật cản phía trước robot bằng sensor chụp ảnh. Khi robot phát hiện vật
cản, nó sẽ tính toán góc tránh vật cản mà vẫn duy trì khoảng cách với điểm đến.
3. Sau khi robot đã tính xong sai số góc và sai số khoảng cách ở bước hai, kế đó robot
sẽ tính toán vận tốc của hai bánh xe dựa vào k
ỹ thuật Fuzzy logic.
4. Trong khi robot di chuyển, nó sẽ tạo ra thông tin chuyển động từ sensor bên trong
và bên ngoài. Lọc Kalman hoạt động trong lúc robot hoạt động.
5. Nếu robot vẫn chưa đạt được đích đến thì bước hai sẽ lập lại.

Hình 2.6 Sơ đồ khối giải thuật chuyển động của robot.
Real
world

Stereo
Algorithms
Stereo vision
camera
[x’,y’, z’]
Trajectory
Planner
Avoidance
Control
Fuzzy
Logic
Control
[x
y
, y
y
,z
y
]
[x

r
, y
r
, θ
r
]
Robot
Dynamics
Kinematics
model
∆d,

Robot
[x, y, θ]
[x
f
y
f
, θ
f
]
C
Prediction
Wheel
encoder
R(θ),t
V
L,
V
R

θ
d

measurement
Vector
compass
Optical
position
senso
r
17


2.4. THỰC HIỆN HỆ THỐNG
Công việc điều khiển của chuyển động robot được chia thành những phần khác
nhau, mỗi phần có một chức năng riêng. Phần Kinematic gồm hai bánh xe robot với bộ
mã hóa quang học, kỹ thuật Fuzzy logic, lọc Kalman và chương trình tránh vật cản. Kỹ
thuật Fuzzy logic dùng điều khiển vị trí vật, để cải tiến việc xác định vị trí và việc định
hướng cho robot, ta dùng lọc Kalman. Kỹ
thuật chụp ảnh nổi dùng để xác định vị trí
vật cản và cho phép robot tránh vật cản. Tất cả các phần trên nhận thông tin từ các
sensor.

2.4.1 Mô hình Kinematic
Mô hình Kinematic nhận thông tin đầu vào từ bộ mã hóa quang học hai bánh xe
robot như hình 2.7. Mô hình sẽ tính toán chuyển động của robot với hệ tọa độ (x, y) và
góc định hướng (θ). Bộ mã hóa sẽ chuyển dữ liệu cho hệ thống, việc kiểm tra sự đo
đạc được tiế
n hành từ thông tin sensor định hướng và sensor định vị trí.
Các phương trình Kinematic như sau:

⎟
⎠
⎞
⎜
⎝
⎛
∆
+∆+==
+
2
cos
1
k
kkkkx
Dxxf
θ
θ
(2.9)
⎟
⎠
⎞
⎜
⎝
⎛
∆
+∆+==
+
2
sin
1

k
kkkky
Dyyf
θ
θ
(2.10)
kkk
f
θ
θ
θ
θ
∆+==
+1
(2.11)
Trong đó, ∆D
k
là sự gia tăng khoảng cách của trục bánh xe giữa hai thời điểm. Với
2
R
K
L
K
k
dd
D
∆+∆
=∆
, và ∆θ
K

là sự thay đổi hướng của robot với
b
dd
R
K
L
K
K
∆+∆
=∆
θ















18

Hình 2.7 Mô hình Kinematic.

2.4.2 Lọc Kalman
Từ phương trình Kinematic, vector trạng thái hệ thống được xác định là
[]
T
KKKK
yxx
θ
= . Vector đầu vào
[
]

T
R
K
L
KK
ddu ∆∆= và hàm hệ thống f(.) là
[]
T
yx
fff
θ
. Lọc
Kalman như sau: phương trình cập nhật thời gian (2.15) và (2.16) là
()
0,0;,
ˆ
1 kkk
uxfx
−
−
=
(2.12)
1
TT
kk
PAPABBQ
−
−
=+Γ+ (2.13)
Trong đó A , B là ma trận Jacobi của hệ thống với

10 sin
2
01 cos
2
00 1
xxx
k
kk
kk k
yyy
k
kk
kk k
kk k
fff
D
xy
fff
AD
xy
fff
xy
θθθ
θ
θ
θ
θ
θ
θ

θ
∂∂∂
⎡⎤
⎡∆⎤
⎛⎞
−∆ +
⎢⎥
⎜⎟
⎢⎥
∂∂∂
⎝⎠
⎢⎥
⎢⎥
⎢⎥
∂∂∂
⎢⎥
∆
⎛⎞
==∆+
⎢⎥
⎢⎥
⎜⎟
∂∂∂
⎝⎠
⎢⎥
⎢⎥
⎢⎥
⎢⎥
∂∂∂
⎢⎥

⎢⎥
∂∂∂
⎣⎦
⎣⎦
(2.14)
xx
LR
kk
yy
LR
kk
LR
kk
ff
DD
ff
B
DD
ff
DD
θθ
∂∂
⎡⎤
⎢⎥
∂∆ ∂∆
⎢⎥
⎢⎥
∂∂
=
⎢⎥

∂∆ ∂∆
⎢⎥
⎢⎥
∂∂
⎢⎥
∂∆ ∂∆
⎣⎦

(x
r
,
y
r
)
y
y-
r
b
x
m
x-axis
∆d
L
θ
∆d
∆DD
19

11
cos sin cos sin
2222 2222
11
sin cos sin cos
22222222
11
kk k kk k
kk kk
kk k kk k
kk kk
DD
bb
DD
bb
bb
θθ θθ
θθ θθ
θθ θθ
θθ θθ
⎡∆∆ ∆ ∆∆ ∆⎤
⎛⎞ ⎛⎞ ⎛⎞ ⎛⎞
++ + +− +
⎜⎟ ⎜⎟ ⎜⎟ ⎜⎟
⎢⎥
⎝⎠ ⎝⎠ ⎝⎠ ⎝⎠
⎢⎥
⎢⎥

∆∆ ∆ ∆∆ ∆
⎛⎞ ⎛⎞ ⎛⎞ ⎛⎞
=+− + ++ +
⎢⎥
⎜⎟ ⎜⎟ ⎜⎟ ⎜⎟
⎝⎠ ⎝⎠ ⎝⎠ ⎝⎠
⎢⎥
⎢⎥
−
⎢⎥
⎣⎦

(2.15)

Việc cập nhật đo đạc, toán tử Jacobi của hàm
[
]
T
KKK
yxh
θ
=(.) là ma trận đo đạc H
k

được trình bày như phương trình (2.19) như sau:
⎥
⎥
⎥
⎦
⎤

⎢
⎢
⎢
⎣
⎡
=
⎥
⎥
⎥
⎥
⎥
⎥
⎥
⎦
⎤
⎢
⎢
⎢
⎢
⎢
⎢
⎢
⎣
⎡
∂
∂
∂
∂
∂
∂

∂
∂
∂
∂
∂
∂
∂
∂
∂
∂
∂
∂
=
100
010
001
kkk
k
y
k
y
k
y
k
x
k
x
k
x
k

h
y
h
x
h
h
y
h
x
h
h
y
h
x
h
H
θ
θ
θ
θθθ
(2.16)
Phương trình cập nhật việc đo đạc như sau:
()
1−
−
+=
k
T
kkk
T

kkk
RHPHHPK (2.17)
(
)
(
)
0,
ˆˆˆ
−−
−+=
kkkkk
xhzKxx
(2.18)
()
−
−=
kkkk
PHKIP (2.19)
Với những phương trình này, các yếu tố cần thiết cho lọc Kalman đã được xác định.
2.4.3 FUZZY LOGIC ĐIỀU KHIỂN CHUYỂN ĐỘNG
Dùng Fuzzy logic để thiết lập tốc độ cho bánh xe trái và bánh xe phải của robot.
Tín hiệu vào và tín hiệu ra sẽ được kỹ thuật này lựa chọn. Tín hiệu vào: sai số góc
θ
error
, chính là sự khác biệt giữa góc tới điểm đích θ
g
và góc tới hiện hành θ
r
. Sai số
khoảng cách d

error
là sự khác biệt giữa vị trí hiện hành và vị trí điểm đích. Cách tính là
tìm khoảng cách giữa hai điểm. Tín hiệu ra của kỹ thuật Fuzzy logic là tốc độ bánh xe
trái và bánh xe phải của robot. Hình 2.8 là sơ đồ khối của kỹ thuật Fuzzy logic.
20




















Hình 2.8 Sơ đồ khối hệ thống điều khiển chuyển động

Kỹ thuật Fuzzy logic được dùng để xác định vận tốc bánh xe robot có giải thuật như
sau:
Bước 1: Xác định biến vào và ra cho hệ thống, trong đó, tính toán hai biến vào là: sai

số góc (chính là sự khác biệt giữa góc tới điểm đích và góc định hướng robot hiện tại)
và sai số khoảng cách (chính là sự khác biệt giữ
a tọa độ điểm đích và tọa độ hiện thời).
Đồng thời tính toán hai biến ra, đó là: vận tốc của bánh xe trái và bánh xe phải của
robot.
Bước 2: Xác định tập mờ. Giá trị tập mờ của các biến được thể hiện như bảng 2.1, giá
trị tập mờ có phần gối lên nhau và được thể hiện bằng dạng tam giác. Các dạng tam
giác đó gọi là hàm thành viên, dạng điển hình c
ủa hàm thành viên được thể hiện như
hình 2.9, hình 2.10, hình 2.11 như dưới đây.

Angle: θ
error
Distance: d
error

NL: Negative Z: Zero
NM: Negative Middle N: Near
NS: Negative Small M: Middle
ZE: Zero F: Far
PS: Positive Small
PM: Positive Middle
PL: Positive Large

Bảng 2.1 Các giá trị ngôn ngữ.
Fuzzy Logic Controller
Fuzzy of
motor lef
t

Fuzzy of
motor right
V
L
encode
r
drive
control
Motor
right
Motor
left
drive
control
θ
error
d
error
encode
r
V
R
v
v
v
f
v
f
21

Hình 2.9 Các hàm thành viên của sai số góc.

Hình 2.10 Các hàm thành viên của vận tốc bánh xe.














Hình 2.11 Các hàm thành viên của sai số khoảng cách.

Z
N M F
derror(m)

0
1
2 3 4
5
PL PM
PS
ZE

NS
NM
NL
0
0
20
0
40
0
60
0
90
0
-20
0
-40
0
-60
0
-90
0
θ
error
FSFWL FW
SLFW
S
SLBW BW
FSBW
0
2

0
4
0
6
0
1
0
0

-2
0
-4
0

-
6
0

-
8
0

8
0

1
0
0

v

22


Bước 3: Xác định luật. Hệ thống làm việc dựa vào luật, những chú thích về luật Fuzzy
là: tiến nhanh về trước (FSFW), tiến về phía trước (FW), đi chậm về phía trước
(SLFW), stop (S), lùi chậm về phía sau (SLBW), lùi về phía sau (BW), lùi nhanh về
phía sau (FSBW). Các luật mờ được trình bày như bảng 2.3 và bảng 2.4 dưới đây:

d
error

Z N M F
NL S FSFW FSFW FSFW
NM S FW FW FW
NS S SLFW FW FW
ZE S SLFW FW FSFW
PS S S SLFW SLFW
PM S S SLFW SLFW


θ
error

PL S S FW FW

Bảng 2.3. Luật mờ của động cơ bên trái.

d
error

Z N M F
NL S S BW BW
NM S S SLBW SLBW
NS S S SLBW SLBW
ZE S SLBW BW FSBW
PS S SLBW BW BW
PM S BW BW BW


θ
error

PL S FSBW FSBW FSBW

Bảng 2.4 Luật mờ của động cơ bên phải.

Ví dụ, các luật mờ được ứng dụng trong việc định hướng đường đi của robot như sau:
R
1
: Nếu θ
error
là góc 0 độ (ZE) và d
error
là khoảng cách gần (N) thì động cơ trái lùi chậm
về phía sau (SLBW).
R
2
: Nếu θ
error
là góc 0 độ (ZE) và d

error
là khoảng cách gần (N) thì động cơ phải tiến
chậm về phía trước (SLFW).
Bước 4: Giải mờ. Hành động ngỏ ra được quyết định vào điều kiện ngỏ vào. Việc giải
mờ sẽ tính toán vận tốc thực hiện của hai bánh xe robot. Cốt lõi của việc giải mờ được
thực hiện bởi phương trình (2.23) như sau:
()
()
∑
∑
=
=
=
n
i
d
ii
n
i
d
iii
errorerror
errorerror
xv
v
1
1
,min
,min
µµ

µµ
θ
θ
(2.23)
23


Trong đó v
i
là ngỏ ra Fuzzy của thuật thứ i,
error
i
θ
µ
và
error
d
i
µ
là giá trị hàm thành viên của
tập Fuzzy vào.
2.5 VIỆC TRÁNH VẬT CẢN.
2.5.1 Dò tìm vật cản
Ở đây, kỹ thuật ảnh nổi được dùng để dò tìm vật cản. Sơ đồ giải thuật dùng ảnh
nổi được trình bày như hình 2.12. Camera bên trái và bên phải chụp ảnh cùng một lúc.
Để giảm nhiễu trong ảnh, ta dùng lọc thấp qua (lowpass filter). Kế đó, kỹ thuật dò tìm
cạnh, đường biên được dùng. Bước tiếp theo, ta phải tìm đi
ểm tương ứng giữa ảnh bên
trái và ảnh bên phải. Kế đó, phương pháp tam giác được ứng dụng để tính toán độ sâu
và đánh dấu những điểm tương ứng. Qua đó, ta thấy rằng kết quả đạt được tốt nhất

trong khoảng 0.5 đến 6m.
Thuật giải cho việc dò tìm vật cản như sau:
If d
obs
≥ 0.5 và d
obs
≤ 6.0 then
found obstacle(s)
return(x
obs,
y
obs
, z
obs
)
else
not found obstacle(s)
return(0,0,0)
end
Trong đó, d
obs
là khoảng cách giữa robot và vật cản.

Hình 2.12 Sơ đồ khối của kỹ thuật dùng ảnh nổi để đánh dấu điểm tương ứng.
Pre-processing

Stereo processing

Depth Images
Raw input
Low-
p
ass
Edge detecting
Stereo matching
Depth calculation
Pre-processed
i
24


2.5.2 Tránh vật cản.

Để tránh vật cản, nhiều lệnh di chuyển phải được thiết lập để robot tránh được
vật cản. Do đó, robot phải có chương trình có thể phân tích được dữ liệu vào như trình
bày ở phần trên. Thêm vào đó, góc so với vật cản θ
obs
phải được tính toán để góc tránh
vật cản θ
avoid
được thiết lập để robot có thể tránh vật cản.

Hình 2.13 Tránh vật cản của robot

Góc θ
d
là sự khác biệt giữa góc định hướng θ
r
, và góc so với vật cản θ
obs
. Góc θ
d
được
tính toán bằng phương trình (2.24) như sau:
obsrd
θ
θ
θ
−= (2.24)
Góc tránh θ
avoid
được tính toán như sau:
If θ
d
<0 then
θ
avoid
= θ
g
- θ

obs

else
θ
avoid
= θ
g
+ θ
obs
end
Các phương trình cho đầu vào của điều khiển Fuzzy logic là như sau:
θ
error
= θ
r
- θ
avoid
(2.25)
(x
obs,
y
obs )
y-axis
y’-axis
Obstacle
Goal
x’-axis
x-axis
d
obs

d
s
(x
r
, y
r)
(x
0
, y
0)
θ
r

θ
obs

θ
s

θ
d