Phương pháp phối hợp đa cảm biến sử dụng kỹ thuật Bayesian

2.1.2. Phương pháp sử dụng bộ lọc Kalman cho bài toán định vị robot

Bộ lọc Kalman (Kalman Filter) rời rạc sử dụng mơ hình tố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 được phát triển bởi Kalman và Bucy vào năm 1960 [47]. 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 véc tơ 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 [91]. 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 hợp, ví dụ ước tính các giá trị làm trơn của vị trí, vận tốc, và gia tốc tại một dãy các điểm trong quỹ đạo. Mơ hình bộ lọc Kalman chuẩn được suy ra từ 2 phương trình tuyến tính. Phương trình thứ nhất mơ tả động học của hệ thống:

1 1 1

k  A k B k  k

x x u w (2.4)

Trong đó:

k

x là véc tơ trạng thái của hệ thống tại thời điểm k.

A là ma trận trạng thái không suy biến.

Véc tơ uk1miêu tả đầu vào hệ thống tại thời điểm k - 1.

Mối quan hệ véc tơ đầu vào uk1 và véc tơ trạng thái xk được xác định bằng ma

trận B, wk1 là một biến ngẫu nhiên biểu diễn cho nhiễu hệ thống. Phương trình tuyến tính thứ 2 mơ tả quan sát nhiễu của hệ thống, với phép đo zm:

k H k  k

z x v (2.5)

Trong mỗi phần tử của vec-tơ zk 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, vk biểu diễn nhiễu đo. Các biến ngẫu nhiên wk và vk đượ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, Qk; 0, Rk; ( T) 0

k N k N E i j 

w v w v

k

Q là các ma trận hiệp phương sai nhiễu đầu vào; Rk là ma trận hiệp phương sai của nhiễu đo. Bài tố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ó mà ta đo được một đại lượng z phụ thuộc tuyến tính vào x. Nói cách khác, dựa trên các số liệu đo z, có thể tìm được một hệ số Kalman K trong mỗi chu kỳ lấy mẫu tín hiệu đo thuộc một vòng lặp đệ quy gọi là bộ lọc Kalman, sao cho giá trị ước lượng trạng thái của hệ gần với giá trị thực nhất. Như vậy, quá trình thực hiện của bộ lọc Kalman là một vòng lặp đệ quy gồm 2 giai đoạn là pha dự đốn (với các phương trình cập nhật thời gian) và pha hiệu chỉnh (với các phương trình cập nhật số liệu). Hệ số Kalman Kk sẽ được tính tốn, sau đó véc tơ trạng thái và ma trận lỗi tương quan Pk sẽ được cập nhật từ các đánh giá bước trước ˆxk và Pk. Để khởi động bộ lọc Kalman, người ta phải cung cấp một ước tính tiên nghiệm ˆ0

x và một ước tính hiệp phương sai lỗi của nó P0. P0 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 Pk tiếp cận gần với giá trị chính xác của nó.

Hình 2.2. Sơ đồ thuật tốn bộ lọc Kalman chuẩn rời rạc

Hình 2.2 trình bày sơ đồ thuật tốn lọc Kalman. Trong pha dự đoán, các cập nhật thời gian cho ước lượng trạng thái tiên nghiệm và hiệp phương sai sai số tiên nghiệm được tiến hành.

1 ˆk Aˆk-1 B k

x = x + u (2.6)

Bước hiệu chỉnh Bước dự đoán

T k

P APA Q (2.7)

Trong pha hiệu chỉnh, hệ số Kalman sẽ được tính và ước lượng trạng thái hậu nghiệm được cập nhật trên cơ sở phép đo z:

1 ( ) T T k k k K P H HP H R  (2.8) ˆk  ˆkKk( k H ˆk) x x z .x (2.9) ( ) k k k P  IK H P (2.10)

Nếu giá trị của Q được chọn là lớn hơn chính giá trị thật của nó, bộ lọc Kalman sẽ đặt độ tin cậy vào các giá trị đo lường hơn là hệ thống. Khi đó, các kết quả đánh giá sẽ phụ thuộc sai số của phép đo. Nếu giá trị Q chọn nhỏ hơn chính nó, độ trễ thời gian sẽ được tăng lên. Khi Q 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 tốn, như vậy việc chọn giá trị Q thông thường dựa trên thực nghiệm.

Đối với robot di động, việc sử dụng bộ lọc Kalman để hiệu chỉnh định vị dựa trên hai hoặc nhiều dữ liệu đầu vào khác nhau [18], [34] và [91]. Bộ lọc Kalman giải quyết các vấn đề phổ biến của ước tính trạng thái xm 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 khi ước tính hoặc mối quan hệ giữa các phép đo quá trình điều khiển là phi tuyến thì việc giải quyết bài toán này là một bộ lọc Kalman được tuyến tính hóa hoặc bộ lọc Kalman mở rộng EKF (Extended Kalman Filter). Bằng việc khai triển chuỗi Taylor các hàm hệ thống và hàm đo phi tuyến, có thể tuyến tính hóa q trình dự đố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 tốn dự đố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.4) của ước tính trạng thái zm, nhưng q 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 phi tuyến tính:

1 1 1

( , , )

k  f k k k

x x u w (2.11)

( , )

k h k k

z x v (2.12)

Các biến ngẫu nhiên wk và vk cũng được giả định là nhiễu quá trình và nhiễu đo như (2.4) và (2.5), trong trường hợp này hàm phi tuyến f ở phương trình (2.11) 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 trước đó k. Nó cịn bao gồm các tham số đầu vào uk và nhiễu quá trình wk. Hàm phi tuyến h của phép đo trong phương trình (2.12) biểu diễn mối liên quan giữa trạng thái xk với phép đo zk. Tương tự như bộ lọc Kalman chuẩn rời rạc, quá trình thực hiện của bộ lọc Kalman mở rộng cũng là một vòng lặp đệ quy với hai giai đoạn là bước dự đốn (với các phương trình cập nhật thời gian) và bước hiệu chỉnh (với các phương trình cập nhật số liệu) như trên Hình 2.3.

Hình 2.3. Sơ đồ thuật toán bộ lọc Kalman mở rộng Bước dự đốn với các phương trình cập nhật:

1 1 ˆk  f(ˆk , k,0) x x u (2.13) 1 T W 1WT k k k k k P  AP A  Q (2.14)

Bước hiệu chỉnh với các phương trình cập nhật:

1 ( ) T T T k k k k k k k k k K P H H P H V R V  (2.15) ˆk  ˆk Kk( k h(ˆk,0)) x x z x (2.16) ( ) k k k P  IK H P (2.17) Trong đó:

n k



x là ước lượng trạng thái tiên nghiệm ở bước k nhận được từ tiên nghiệm quá trình ở bước k.

n

k 

x là ước lượng trạng thái hậu nghiệm ở bước k - 1 nhận được sau phép đo zk

k

P là ma trận hiệp biến của sai số ước lượng trạng thái tiên nghiệm.

k

P là ma trận hiệp biến của sai số ước lượng trạng thái hậu nghiệm. 1

k

Q  là ma trận hiệp phương sai nhiễu đầu vào.

k

R là ma trận hiệp phương sai của nhiễu đo.

k

K là hệ số lọc Kalman.

I là ma trận đơn vị.

Các biến ngẫu nhiên wk và vk được giả định là nhiễu quá trình và nhiễu đo như (2.13) và (2.14).

A là ma trận Jacobian của các đạo hàm riêng của f theo x: ij i (ˆ 1, 1,0) k k j f A x      x u

W là ma trận Jacobian của các đạo hàm riêng của f theo w: ij ˆ 1 1 W i ( k , k ,0) j f w      x u

H là ma trận Jacobian của các đạo hàm riêng của h theo x: ij i (ˆ ,0) k j h H x    x

V là ma trận Jacobian của các đạo hàm riêng của h theo v: ij i (ˆ ,0) k j h V v    x

Qua những phân tích ở trên chúng ta thấy rằng khi sử dụng bộ lọc Kalman rời rạc cho phép đạt được nhiều kết quả mong muốn [90]. 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ẫu nhiên phi tuyến cho mơ hình hệ thống [42]. Việc ứng

dụng EKF cho robot di động rất đa dạng, ví dụ như định vị cho robot di động có các bánh xe được điều khiển vi sai [38], hay ứng dụng EKF cho tổng hợp đa cảm biến để nâng cao khả năng ước lượng trạng thái của robot di động [44], [92]. Hơn nữa, trong một môi trường đa cảm biến với các ma trận đo lường giống hệt nhau, các phép đo có thể được xử lý một cách riêng biệt để có được độ phức tạp tính tốn thấp hơn. Trong trường hợp các ma trận đo khơng giống nhau là thích hợp sử dụng hợp nhất véc tơ đầu vào chứa các thông tin của tất cả các cảm biến. Thêm một lý do quan trọng khi tổng hợp đa cảm biến có nhiều loại khác nhau, do thời gian xử lý, thời gian cập nhật trên mỗi cảm biến khác nhau, nhưng vẫn có thể mơ hình hóa được với các dạng thơng tin của EKF và do đó có thể tổng hợp được nhiều cảm biến khác nhau, tăng số chiều dữ liệu [40].

SLAM được định nghĩa là một quá trình mà ở đó robot có thể xây dựng bản đồ của mơi trường, đồng thời sử dụng bản đồ đó để ước tính vị trí của robot trong mơi trường đó. Thuật tốn SLAM là cơng trình được nghiên cứu bởi các tác giả Smith, Self và Cheeseman [86], sau đó nó được phát triển bởi Hugh Durrant-Whyte và cộng sự [25], [53]. Thuật toán SLAM đề xuất vấn đề xây dựng bản đồ của môi trường không xác định cho robot di động đồng thời điều hướng môi trường bằng bản đồ [95].

Như trên Hình 2.4, thuật tốn SLAM được định nghĩa là việc ước tính vị trí vật cản mơi trường và vị trí của robot trong mơi trường đó.

Ở dạng xác suất, SLAM được biểu diễn dưới dạng phân bố xác suất sau:

( k, | 0:k, 0:k)

P x m z u (2.18)

Với u0:k = u , u ,..., u 0 1 k = u0:k -1, uk: tập chứa các giá trị đầu vào điều khiển từ thời điểm ban đầu đến thời điểm k, m= m , m ,..., m 1 2 n: tập các điểm mốc thể hiện vị trí vật cản.z0:k z , z ,..., z0 1 k  z0:k1, zk: tập các điểm mốc quan sát được từ thời điểm ban đầu đến thời điểm k [68]. Phân bố xác suất này mô tả phân bố hậu nghiệm liên kết giữa vị trí điểm mốc trong bản đồ m với trạng thái của robot xk tại thời điểm k khi biết giá trị quan sát được z0:k và tín hiệu đầu vào điều khiển u0:k.

Hình 2.4. Kỹ thuật SLAM

Việc tính tốn giá trị phân bố xác xuất này u cầu mơ hình chuyển trạng thái và mơ hình quan sát để mơ tả việc tác động của tín hiệu điều khiển và các giá trị quan sát được. Mơ hình quan sát mơ tả xác suất thực hiện được giá trị quan sát zk khi vị trí robot và vị trí điểm mốc trên bản đồ được xác định có dạng:

( k | , )

P z x mk (2.19)

Với giả thiết rằng khi vị trí của robot và bản đồ được xác định thì các giá trị quan sát được là độc lập có điều kiện, khi đó mơ hình chuyển động của robot được mô tả bởi phân bố xác suất chuyển trạng thái sau:

-1 ( k| k , k)

P x x u (2.20)

Phân bố này được giả sử thoả mãn tiêu chuẩn Markov, ở đó trạng thái xk chỉ phụ thuộc vào trạng thái ngay trước nó xk-1 và tín hiệu điều khiển uk, và xk độc lập với các giá trị quan sát được và bản đồ m.

Thuật toán SLAM được thực thi với hai bước là bước dự đoán (prediction) và bước chỉnh sửa (correction).

• Bước dự đốn được mơ tả bởi phân bố:

( k, | 0:k-1, 0:k)

P x m z u (2.21)

Do trạng thái của robot xk, xk-1 và bản đồ m là độc lập, có thể viết lại: 0: 1 0: 0 0: 1 0: 0: 1 0: 0: 1 0: 1 1 0: 1 0: 0: 1 0: 1 0: 1 0: 1 1 0: 1 0: 1 ( , | , , ) ( | , ) ( | , ) ( | , , ) ( | , ) ( | , ) ( | , , ) ( , | , ) k k k k k k k k k k k k k k k k k k k k k k k k k k P P P P P P dx P P dx                    x m z u x x z u m z u x z u x x z u m z u x z u x x m z u (2.22)

Vì xk có tính chất Markov nên (2.22) được viết gọn thành:

0: 1 0: 0

( k, | k , k, ) ( k | k-1, k) ( k-1, | 0:k-1, 0:k-1) k-1

P x m z  u x P x x u P x m z u dx (2.23) Sau khi tính tốn bước dự đốn, bước chỉnh sửa được thực hiện theo phân bố:

0: 1 0: 0 ( | , ) (x , | , ) ( , | , , ) ( | , ) k k k 0:k -1 0:k k k k k 0:k -1 0:k P P P P   z x m m z u x m z u x z z u (2.24)

Biểu thức (2.23) và (2.24) mơ tả q trình tính tốn đệ quy cho việc tính tốn xác suất hậu nghiệm cho trạng thái robot xk và bản đồ m tại thời điểm k dựa vào tập quan sát z0:k và tập tín hiệu điều khiển đầu vào u0:k.

Có thể thấy trong Hình 2.4, hầu hết những sai lệch xảy ra giữa vị trí ước tính và vị trí thật của các điểm mốc (vật cản) là bởi sai lệch về vị trí của robot khi thu được các tín hiệu quan sát vật cản. Điều này có nghĩa là các giá trị sai lệch trong việc dự đoán vị trí của các điểm mốc có tính tương quan cao. Thực tế, điều này có nghĩa là khoảng cách giữa các điểm mốc mimj có khả năng biết được chính xác, ngay cả khi vị trí thực tế của điểm mốc là một giá trị chưa biết trước. Trong dạng xác suất, điều này có nghĩa là mật độ xác suất chung cho mỗi cập điểm mốc (m ,m )P i j là gần như đạt tới giá trị cực đại ngay cả khi mật độ xác suất P(m )i hay (m )P j là khá nhỏ. Vấn đề quan trọng nhất bên trong SLAM là việc nhận ra rằng mức độ tương quan giữa các dự đốn về vị trí các điểm mốc đơn điệu tăng khi càng có nhiều giá trị khoảng cách quan sát thu được. Thực tế, điều này có nghĩa là thơng tin về vị trí tương đối của các điểm mốc luôn gia tăng. Trong dạng xác suất, điều này có nghĩa là mật độ xác suất chung của tất cả điểm mốc sẽ đơn điệu tăng đến cực đại khi mà có càng nhiều tín hiệu quan sát được thực hiện. Sự hội tụ này xảy ra bởi các quan sát được thực hiện có thể coi gần đúng là độc lập với nhau. Trong Hình 2.4, xét vị trí robot tại

k

x và đang quan sát được hai điểm mốc là mi và mj. Vị trí tương quan giữa các điểm mốc quan sát được là độc lập trong hệ trục toạ độ gắn với robot và các quan sát liên tiếp từ vị trí này sẽ mang lại các phép đo lường độc lập hơn nữa về mối tương quan giữa các điểm mốc. Tiếp đến khi robot di chuyển đến vị trí xk+1, robot quan sát

được hai điểm mốc mới có tương quan với điểm mốc mj. Những điểm mốc mới này ngay lập tức có liên kết và tương quan với toàn bộ bản đồ thu được. Những lần cập nhật sau của những điểm mốc này cũng sẽ cập nhật điểm mốc mj, cập nhật thông qua điểm mốc mj và tương tự như thế với các điểm mốc khác. Toàn bộ điểm mốc sẽ hình thành nên một mạng liên kết với nhau bởi vị trí tương quan mà độ chính xác của