Đ
ẠI
HỌC QUỐC GIA HÀ
NỘI
TRƯỜNG ĐẠI HỌC CÔNG
NGHỆ
NGUY
ỄN XUÂN HẢI
NÂNG CAO Đ
Ộ CHÍNH XÁC ĐỊNH VỊ ROBOT
B
ẰNG PHƯƠNG PHÁP TỔNG HỢP DỮ LIỆU
C
ẢM BIẾN LẬP MÃ QUANG VỚI BỘ LỌC
KALMAN M
Ở RỘNG
LUẬN VĂN TH
Ạ
C
SĨ CÔNG NGH
Ệ ĐIỆN TỬ
- VI
ỄN THÔNG
Hà Nội -
2014
Đ
ẠI
HỌC QUỐC GIA HÀ
NỘI
TRƯỜNG ĐẠI HỌC CÔNG
NGHỆ
ợc đánh dấu cụ thể trong luận văn. Các phần còn lại, tôi xin cam đoan là
do tôi tự tìm hiểu để viết ra. Nếu có bất kỳ sự sai xót nào, tôi xin chịu hoàn toàn trách
nhiệm.
Hà Nội, ngày 02 tháng 06 năm 2014
Nguyễn Xuân Hải
2
LỜI CẢM ƠN
Để hoàn thành Luận văn Thạc s
ĩ c
ủa mình, em xin gửi lời cảm ơn chân thành
tới các thầy cô trường Đại học Công Nghệ - Đại học Quốc Gia Hà Nội đ
ã truy
ền đạt
những kiến thức quý báu cho em trong suốt quá trình học tập và hoàn thành Luận văn
Thạc s
ĩ.
Em xin gửi lời cảm ơn tới PGS.TS Trần Quang Vinh, người trực tiếp chỉ bảo,
hướng dẫn em trong suốt quá trình nghiên cứu và hoàn thành Luận văn Thạc s
ĩ.
Cuối cùng, tôi xin chân thành cảm ơn gia đ
ình, b
ạn bè và đồng nghiệp đ
ã
đ
ộng
viên, khuyến khích tôi trong suốt quá trình học tập và nghiên cứu.
Xin chân thành cảm ơn.
Hà Nội, ngày 02 tháng 06 năm 2014
Nguyễn Xuân Hải
3
4.2. Tính toán các ma trận trong vòng lặp đệ quy Kalman 34
CHƯƠNG 5: THỰC NGHIỆM VÀ THẢO LUẬN 37
4
5.1. Thiết lập thực nghiệm 37
5.2. Kết quả thực nghiệm áp dụng bộ lọc Kalman cho tổng hợp dữ liệu của một
cảm biến lập mã quang 38
5.3. Tổng hợp cảm biến từ phép đo odometry với cảm biến chỉ hướng 40
5.3.1. Cập nhật các tham số cho các ma trận. 40
5.3.2. Hiệu chỉnh chính xác góc quay qua bộ lọc Kalman 41
KẾT LUẬN 43
DANH MỤC TÀI LIỆU THAM KHẢO 44
5
DANH MỤC CÁC HÌNH VẼ, ĐỒ THỊ
Hình 1.1. Các bộ phận của một robot 8
Hình 1.2. Các khối cơ bản trong robot di động 10
Hình 2.1. S
ơ đ
ồ thuật toán bộ lọc Kalman tuyến tính rời rạc 15
Hình 2.2. Sơ đồ thuật toán bộ lọc Kalman mở rộng 18
Hình 3.1.
Đĩa có khe ch
ắn sáng và tín hiệu ra bộ lập mã quang 21
Hình 3.2a. Mô-đun cảm biến từ-địa bàn CMPS03 22
Hình 3.2b. Nguyên lý hoạt động của cảm biến từ-địa bàn. 22
Hình 3.3a. Các cảm biến siêu âm 23
Hình 3.3b. Nguyên lý hoạt động của cảm biến siêu âm 23
Hình 3.4. Một số loại máy quét laser của công ty SICK 23
Hình 3.5. Thời gian bay của tia laser 24
Hình 3.6. Phép
đo d
Nhiều ứng dụng của các ngành nghề, l
ĩnh v
ực khác nhau đ
òi h
ỏi cần phải trang
bị thiết bị hoàn toàn tự động làm việc theo chương tr
ình hay do con ngư
ời điều khiển
như robot.
Robot công nghiệp, thường là các tay máy (arm-robot) trong các hệ thống sản
xuất tự động. Robot trong các ứng dụng này được đặt tại một vị trí cố định, làm việc
trong môi trường nhất định biết trước.
Thời gian gần đây, bắt đầu bằng các xe tự hành trong v
ũ tr
ụ, xuất hiện một loại
robot khác là robot thông minh. Một loại trong số đó là các robot di động hoạt động tự
trị (autonomous). Các robot di dộng với khả năng giám sát môi trường xung quanh để
đưa ra các quyết định cần thiết trong quá trình hoạt động thực hiện công việc được
giao, cụ thể là tự động đi tới đích chính xác và an toàn. Trong hệ thống này, robot thu
nhận thông tin qua các cảm biến, xử lý dữ liệu thu được và tự đưa ra các quyết định.
Quá trình dẫn đường của các robot di động thông minh thường được phân tích
thành 4 giai đoạn: cảm nhận, định vị, vạch đường đi và điều khiển chuyển động .
Giai đoạn định vị là rất quan trọng, nó cho phép trả lời câu hỏi: robot đang ở
đâu? để từ đó có thể vạch ra các quyết định dẫn đường. Để có được câu trả lời này, bộ
phận điều khiển robot cần phải có được các dữ liệu môi trường (qua các cảm biến
ngoại) hay dữ liệu về bản thân robot (qua các cảm biến nội) trong giai đoạn cảm nhận.
Vì hệ thống robot chứa đựng nhiều yếu tố ngẫu nhiên có thể gây nên các sai số
c
ũng như không th
ể có loại cảm biến nào đủ hoàn hảo để có thể chỉ dùng nó là đủ để
Các thiết bị này đang thay thế dần các máy tự động cứng chỉ đáp ứng một việc nhất
định. Điều này dẫn đến nhu cầu ứng dụng robot để tạo ra các hệ thống tự động linh
hoạt.
Thuật ngữ robot được sinh ra từ trên sân khấu, không phải trong phân xưởng
sản xuất. Những robot xuất hiện lần đầu tiên trên ở NewYork vào ngày 09/10/1922
trong vở “Rossum’s Universal Robot” của nhà soạn kịch người Tiệp Karen Kapek viết
năm 1921, c
òn t
ừ “robot” là cách gọi tắt của từ “robota” - theo tiếng Tiệp có ngh
ĩa là
công việc lao dịch.
Những robot thực sự có ích được nghiên cứu để đưa vào những ứng dụng trong
công nghiệp, y học, đời sống, quân sự, không gian…
Từ những năm 80, nhất là vào những năm 90, do áp dụng rộng rãi các tiến bộ
kỹ thuật về vi xử lý và công nghệ thông tin, số lượng robot đ
ã gia tăng v
ới nhiều tính
năng vượt bậc. Chính vì vậy mà robot đ
ã có m
ột vị trí quan trọng trong những ứng
dụng công nghiệp, y học, đời sống, quân sự, không gian…
Các bộ phận cơ bản của một robot có thể được mô tả như trên h
ình 1.1
gồm: bộ
phận cảm nhận, bộ điều khiển và bộ chấp hành.
Hình 1.1. Các bộ phận củ a một robot.
Bộ phận cảm nhận có nhiệm vụ thu thập thông tin từ môi trường dựa trên số
liệu các cảm biến đặt trên robot. Môi trường có thể là biết trước hoặc không biết trước.
9
Bộ điều khiển có nhiệm vụ xử lý các thông tin thu được từ bộ cảm nhận. Bộ
Robot di động phải có một số bộ phận chuyển động. Chuyển động có thể dưới
dạng bánh xe, chân, cánh hoặc một số cơ cấu khác. Việc lựa chọn cơ cấu chuyển động
là dựa vào chức năng của robot và các công việc của robot cần phải thực hiện. Trong
nhiều môi trường làm việc công nghiệp, bánh xe là dạng chuyển động thích hợp nhất.
Đối với các hệ thống nghiên cứu ứng dụng khác, chân hoặc cánh có thể giúp cho robot
di động chuyển động được trên địa hình mà robot không có khả năng đi qua.
Một nhân tố quan trọng trong thiết kế của các hệ thống robot di động là khả
năng mang theo các thiết bị. Thiết bị bao gồm nguồn điện c
ũng như t
ất cả các phần
cứng là các bộ cảm biến và các bộ xử lý mà robot
đòi h
ỏi. Các thiết bị lắp đặt của
10
robot cố định có thể được kết nối trực tiếp với các nguồn điện và bộ xử lý, không gian
làm việc thường được xác định trước và có thể được kiểm soát. Hầu hết các robot di
động đều có một trọng tải nhất định, điều này có thể dẫn tới những sự giới hạn trong
thiết kế của hệ thống, đặc biệt trong các ứng dụng hay bị giới hạn về kích cỡ và cân
nặng.
Các thành phần cơ bản, thông dụng trong các hệ thống robot di động sử dụng
trong thực tế được chỉ ra như trên hình 1.2.
Hình 1.2. Các kh
ối cơ bản trong robot di động
.
Các khối cơ bản cho các hệ thống robot di động bao gồm các bộ phận cảm biến,
bộ phận dẫn đường, các mô-đun lập kế hoạch, điều khiển và các bộ chấp hành dùng để
di chuyển robot và tác động tới môi trường của nó. Trong khi một số các phần cụ thể
của robot di động sẽ phụ thuộc vào mục đích ứng dụng, một hệ thống robot di dộng
điển hình
đòi h
phần mềm một cách hợp lý cho một robot di động thông minh. Vì vậy việc tập trung
vào xây dựng phát triển các giải thuật tổng hợp cảm biến (sensor fusion) trên cơ sở
thiết kế lắp ráp phần cứng của các robot di động thông minh tự quản trị là hết sức cần
thiết. Chương tiếp theo trình bày về tổng hợp cảm biến.
12
CHƯƠNG 2: PHƯƠNG PHÁP TỔNG HỢP CẢM BIẾN VỚI
BỘ LỌC KALMAN
2.1. Phương pháp tổng hợp cảm biến dùng cho robot di động
Robot dùng số liệu thu được từ các cảm biến để xác định trạng thái của nó trong
môi trường, trên cơ sở đó mà ra các quyết định cho các nhiệm vụ dẫn đường như định
vị và lập bản đồ. Tuy nhiên, chưa thể có loại cảm biến nào hoàn hảo để thực hiện
nhiệm vụ này một cách thật chính xác được. Vì vậy kỹ thuật tổng hợp cảm biến
(sensor fusion) đ
ã
được sử dụng, đó là việc sử dụng đồng thời các dữ liệu từ một cảm
biến hay từ nhiều cảm biến rồi lấy thông tin hợp nhất ra để tạo nên bức tranh về trạng
thái môi trường chính xác hơn. Một phương pháp tổng hợp cảm biến hiệu quả nhất để
ước tính đồng thời các giá trị đo của các cảm biến đó là việc dùng bộ lọc Kalman mở
rộng (EKF) với các phép tính ma trận. Tổng hợp cảm biến có thể được định ngh
ĩa như
sau: đó là quá tr
ình k
ết hợp các dữ liệu nhận được từ nguồn các cảm biến để có được
kết quả thông tin với một ý ngh
ĩa t
ốt hơn so với khi sử dụng các nguồn này riêng lẻ.
Vì dữ liệu cảm biến có nhiều loại nên có tác giả đ
ã dùng thu
ật ngữ “tổng hợp
thông tin” như một thuật ngữ tổng thể cho tổng hợp các loại dữ liệu. Có thể định ngh
ĩa hơn là t
ừ một nguồn dữ liệu đơn.
Xét một ví dụ đơn giản là cần định vị chính xác vị trí và hướng của robot khi nó
di chuyển trong một không gian chưa biết trước. Nếu chỉ sử dụng một camera ảnh thì
có thể xác định khá chính xác góc hướng của robot, nhưng không thể xác định chính
xác khoảng cách từ robot đến một vật cản nào đó. Nhưng nếu kết hợp dữ liệu từ
camera này với dữ liệu nhận được qua các cảm biến siêu âm hay máy đo xa laser th
ì có
thể xác định được chính xác không những góc hướng của robot mà còn cả khoảng
cách từ robot đến đối tượng. Một ví dụ tiếp theo là bằng cách sử dụng các bộ lập mã
trục xoay trong phương pháp odometry cho phép định vị robot khá đơn giản. Tuy
nhiên, với mô hình hệ thống khá đơn giản như vậy, các giá trị định vị gặp phải các sai
số tích l
ũy
bị gây nên do các kết cấu cơ khí được thiết kế không thể thật sự hoàn hảo
trong các robot. Nếu áp dụng phương pháp tổng hợp cảm biến bằng việc xử lý các số
liệu đo được qua bộ lọc Kalman mở rộng sẽ cho phép cải thiện đáng kể độ chính xác
định vị kiểu này.
2.2. Phương pháp tổng hợp cảm biến với bộ lọc Kalman
Bộ lọc Kalman sử dụng mô hình toán học cho lọc các tín hiệu bằng cách sử
dụng các phép đo với một số lượng đáng kể các sai số thống kê và có hệ thống.
Phương pháp được phát triển bởi Kalman và Bucy vào năm 1960 [3,8].
Thông thường, một bộ lọc Kalman tổng hợp dữ liệu đo được trong một khoảng
thời gian liên tiếp, cung cấp một ước tính khả năng xảy ra lớn nhất của một tham số.
Nó c
ũng có th
ể liên kết với các đầu vào của đa cảm biến thành một vector của các
trạng thái bên trong có chứa các tham số quan tâm, miễn là chúng chỉ phụ thuộc tuyến
tính giữa các đầu vào và các trạng thái của hệ thống [2]. Bộ lọc sử dụng thuật toán thời
gian rời rạc để loại bỏ nhiễu từ các tín hiệu của cảm biến để tạo ra được dữ liệu tổng
:
.
k k k
z H x v= +
(2.2)
Trong mỗi phần tử của vec tơ
k
z
chứa một quan sát cảm biến tại thời điểm k.
Ma trận H liên quan tới các phép đo trạng thái bên trong, v
k
biểu diễn nhiễu đo.
Các biến ngẫu nhiên w
k
và v
k
được giả định là nhiễu quá trình và nhiễu đo; độc
lập với nhau, ồn trắng và có phân bố xác suất chuẩn:
~ (0, ) ~ (0, ) ( ) 0
T
k k k k i j
w N Q v N R E w v =
Q
k
là các ma trận hiệp phương sai nhiễu đầu vào; R
k
là ma trận hiệp phương sai
của nhiễu đo.
Bài toán bộ lọc Kalman chính là tìm giá trị ước lượng của trạng thái x khi biết
được sự biến thiên của nó qua một đại lượng z đo được phụ thuộc tuyến tính vào x.
0
P
−
.
0
P
−
có thể được khởi tạo với một ước
tính giá trị ban đầu không chính xác, khi đó ứng dụng tiếp theo của bộ lọc Kalman sẽ
cho
−
k
P
tiếp cận gần với giá trị chính xác của nó.
1
)(
−−−
+=
k
T
kkk
T
kkk
RHPHHPK
(2.3)
ˆ ˆ ˆ
( )
k k k k k k
x x K z H x
− −
k
chọn nhỏ hơn chính nó, độ trễ thời gian
sẽ được tăng lên. Khi Q
k
nhỏ hơn rất nhiều so với giá trị thật, bộ lọc sẽ phân kỳ dẫn
đến kết quả là không ổn định về mặt tính toán, như vậy việc chọn giá trị Q
k
thông
thường dựa trên thực nghiệm.
0 0
ˆ
,Px
− −
k
z
Hình 2.1. Sơ đồ thuật toán bộ lọc Kalman tuyến tính rời rạc.
Tính hệ số khuếch đại Kalman
1
)(
−−−
+=
k
T
kkk
T
kkk
RHPHHPK
Cập nhật các đánh giá
ˆ ˆ ˆ
( )
của ước tính trạng thái
m
x∈ℜ
mà quá trình
đi
ều khiển thời gian rời rạc phụ thuộc vào
phương tr
ình sai phân ng
ẫu nhiên tuyến tính. Nhưng điều gì sẽ xảy ra, nếu qúa trình
ước tính hoặc mối quan hệ giữa các phép đo với quá trình
đi
ều khiển là phi tuyến? Để
giải quyết bài toán này, bộ lọc Kalaman mở rộng viết tắt là EKF (extended Kalman
filter) đ
ã được phát triển. Bằng việc khai triển chuỗi Taylor tại lân cận m
ột điểm n
ào
đó trên qu
ỹ đạo, ta có thể tuyến tính hóa sự dự đoán xung quanh dự đoán hiện tại sử
d
ụng một phần dẫn xuất của quá tr
ình và chức năng đo lường để đưa vào tính toán dự
đoán. Đ
ể thực hiện, ta bắt đầu hiệu chỉnh từ bộ lọc Kalman rời rạc. Hãy giả đ
ịnh rằng
phương tr
ình tr
ạng thái của hệ thống (2.1) của
ước tính trạng thái
m
ợc giả định là nhiễu quá trình và nhiễu đo
như (2.1) và (2.2), trong trường hợp này hàm phi tuyến, f, ở (2.8) liên quan đến trạng
thái ở bước thời gian hiện tại k đến trạng thái ở bước thời gian tiếp theo k+1. Nó bao
gồm như các tham số, u
k
và nhiễu quá trình, w
k
. Hàm phi tuyến, h, của phép đo trong
phương tr
ình (2.9) bi
ểu diễn mối liên quan giữa trạng thái x
k
với phép đo z
k.
Thực tế, không biết được các giá trị riêng lẻ của nhiễu w
k
và v
k
tại mỗi bước
thời gian. Tuy nhiên, có thể tính gần đúng vec-tơ trạng thái và vec-tơ đo lường:
1
( , ,0)
k k k
x f x u
+
=
(2.10)
và:
17
-
k
x
và
k
z
là các vec-tơ trạng thái và phép đo thực.
-
k
x
và
k
z
là các vec-tơ trạng thái và phép đo xấp xỉ từ (2.10) và (2.11).
-
ˆ
k
x
ước tính trạng thái hậu nghiệm tại thời điểm k.
- Các biến ngẫu nhiên w
k
và v
k
c
ũng đư
ợc giả định là nhiễu quá trình và nhiễu
đo như (2.10) và (2.11).
- A là Jacobi của đạo hàm riêng của f theo x:
i
k
j
h
H x
x
∂
=
∂
- V là Jacobi của đạo hàm riêng của h theo v:
ij
ˆ
| ( ,0);
i
k
j
h
V v
v
∂
=
∂
18
z
k
Hình 2.2. Sơ đồ thuật toán bộ lọc Kalman mở rộng.
Tương tự như bộ lọc Kalman rời rạc, quá trình thực hiện của bộ lọc Kalman mở
rộng c
ũng đư
P
là ma trận hiệp phương sai của sai số dự báo trạng thái.
Q
k-1
là ma trận hiệp phương sai nhiễu đầu vào phụ thuộc vào độ lệch
chuẩn của nhiễu.
2. Bước hiệu chỉnh với các phương tr
ình c
ập nhật phép đo:
- - -1
( )
T T T
k k k k k k k k k
K P H H P H V R V= +
(2.16)
1 1
ˆ
,
k k
x P
− −
Tính hệ số khuếch đại
Kalman
- - -1
( )
T T T
k k k k k k k k k
K P H H P H V RV= +
Cập nhật các đánh giá
- -
(2.17)
-
( - )
k k k k
P I K H P=
(2.18)
Trong đó
ˆ
n
k
x ∈ℜ
là ước tính trạng thái hậu nghiệm tại thời điểm k, giá trị thu
được sau khi đo lường z
k
.
R
k
là ma trận hiệp phương sai của nhiễu được ước lượng từ nhiễu trong phép
đo.
Qua những phân tích ở trên chúng ta thấy rằng bộ lọc Kalman mở rộng (EKF)
rời rạc đã
đ
ạt được nhiều kết quả mong muốn [9]. Tuy nhiên, mô hình tuyến tính của
hệ thống không phải lúc nào c
ũng kh
ả thi. Vì vậy EKF đ
ã đư
ợc dẫn xuất từ các
phương tr
ình ng
ư
ợc.
Tùy theo các đặc điểm kỹ thuật, cảm biến có thể được chia thành nhiều loại
khác nhau.
Một trong những cách phân loại đó là cảm biến được chia làm hai loại: thụ
động và tích cực. Cảm biến thụ động thường dựa vào các thông số môi trường để cung
cấp phương tiện cho việc quan sát trong khi đó, cảm biến tích cực phải phát năng
lượng vào môi trường hoặc để thay đổi nó hoặc để tăng cường nó rồi thu thập dữ liệu
được phản xạ về.
Tùy vào tính chất mục đích sử dụng các cảm biến còn
đư
ợc phân thành: cảm
biến nội, cảm biến ngoại.
Cảm biến nội được đặt trong bản thân robot, đó là các cơ cấu điện, điện tử, cơ
khí, thủy lực… để nhận các thông tin phản hồi về vị trí của các bộ phận trên robot.
Cảm biến ngoại là các cảm biến giúp robot giao tiếp được với môi trường bên
ngoài, phản ánh các thông tin về môi trường xung quanh tới bộ điều khiển robot. Ví
dụ, cảm biến đo xa siêu âm cho phép robot ước lượng khoảng cách từ nó đến các đối
tượng.
3.1.1. Cảm biến lập mã trục xoay (cảm biến lập mã quang)
Các bộ lập mã quang
đư
ợc dùng cho phương pháp đo qu
ãng đư
ờng đi của robot
(phương pháp này thường gọi là odometry hay dead reckoning) [5], trong đó robot đo
một tín hiệu đang phát ra trong nó.
21
Bộ mã hóa quang đã trở thành thiết bị phổ biến để đo tốc độ góc và vị trí bên
trong một động cơ mô-tô hoặc trục của bánh xe hoặc một cơ cấu lái. Trong robot di
đ
ủ nhanh để đếm tốc độ trục
quay đ
ã đư
ợc dự kiến. Hình 3.1 là bộ lập mã quang từng bước loại vuông pha 2 kênh.
Hình 3.1.
Đĩa có khe chắn sáng và tín hiệu ra bộ lập mã quang
.
3.1.2. Cảm biến chỉ hướng (heading sensor).
Cảm biến cho phép đo góc hướng của robot so với phương từ trường của quả
đất. Cảm biến được tích hợp trong một mô-đun cảm biến từ-địa bàn CMPS03 của hãng
Philips (trên hình 3.2a) hoạt động dựa trên nguyên tắc hiệu ứng Hall với độ phân giải
góc là 0,1° [12]. Nguyên tắc hoạt động như biểu diễn trên hình 3.2b, mô-đun có 2 trục
hướng x và y.
22
Hình 3.2a. Mô-đun c
ảm biến từ
-đ
ịa bàn
CMPS03.
Hình 3.2b. Nguyên lý ho
ạt
đ
ộng
c
ủa cảm biến từ
-đ
ịa bàn
.
Cảm biến cung cấp số liệu đo cường độ từ trường theo các trục hướng này, từ đó
sóng phản xạ từ vật cản. Biết thời gian này và vận tốc sóng siêu âm truyền trong môi
trường thì có thể xác định được khoảng cách từ cảm biến đến vật.
Hình 3.3a. Các c
ảm biến si
êu âm
[13].
Hình 3.3b. Nguyên lý ho
ạt động
c
ủa cảm
bi
ến siêu âm
.
Cảm biến siêu âm có ưu điểm là có giá thành thấp, dễ sử dụng và có lợi thế tạo
ra kết quả nhanh hơn so với các thiết bị đo khác. Tuy nhiên, kết quả đo khoảng cách
của cảm biến siêu âm bị một số hạn chế cơ bản do chùm tia siêu âm bị tán xạ thành
hình nón chứ không phải là một tia hẹp. Độ chính xác của kết quả đo không ổn định do
bị ảnh hưởng bởi các hiện tượng như: tiếng dội (echo), búp sóng bên (side lobe), phản
xạ gương (specular reflection), xuyên âm (cross-talk) của chùm sóng siêu âm nên khó
xác định được kích thước của vật.
3.1.4. Cảm biến đo xa laser (Laser Range Finder - LRF)
Máy đo khoảng cách bằng tia laser là một thiết bị sử dụng tia lase để quét và
tìm khoảng cách đến vật thể. Có nhiều loại máy quét lase trên thị trường, một ví dụ 2
loại khác nhau của máy quét lase như h
ình 4.
Hình 3.4. M
ột số loại máy quét laser của công ty SICK
[14].
Copyright: Tài liệu đại học ©