Chúng ta đã phải xác định phương trình cập nhật điều khiển đầu vào, robot của có tất cả các công cụ cần thiết để thực hiện một di chuyển, nó thích hợp có thể cập nhật vị trí dự kiến của nó và sử dụng phương sai phương trình (2.7), (2.8) và tiếp tục tăng cường ước lượng vị trí của nó như bình thường bằng cách đọc feature mới. Phương trình (2.8) là bước cuối cùng trong việc thực hiện một bộ lọc Kalman trong môi trường 1 chiều. Ở đây không có sự khác nhau của bài toán SLAM có thể xảy ra. Trong môi trường 1 chiều, giả sử chúng ta đọc cảm biến sau mỗi bước di chuyển, chúng ta có thể thấy Ki * vari-1 lớn hơn hoặc bằng varu * T, thời gian ước lượng luôn luôn nhận được giá trị tốt hơn hoặc giữ nguyên.
2.1.6. Phát triển lọc Kalman với môi trường thực tế cho SLAM
Phát triển dựa trên bộ lọc đơn giản, bây giờ chúng ta sẽ điều chỉnh bộ lọc Kalman trong môi trường đa chiều với nhiều feature. Chúng ta cần chú ý đến sự thay đổi giữa quan sát và di chuyển. Quan sát liên quan đến định vị feature được sử dụng để cập nhật và tăng sự tự tin và di chuyển sẽ cung cấp các điểm thuận lợi mới cho các robot trong khi giảm niềm tin vào trạng thái thế giới.
2. Những kí hiệu sử dụng trong môi trường đa chiều
Chúng ta cần một số cách thể hiện ước lượng vị trí feature. Trong môi trường một chiều, ta sử dụng biến z để quan sát. Chúng ta có thể đơn giản bằng cách thay đổi ý nghĩa của z để chỉ ước lượng thực tếvị trí feature và chỉ cần bỏ ký hiệu cho các quan sát. Ngoài ra, chúng ta cần quen với các ký hiệu để phân biệt giữa các feature ở một thời gian cụ thể i. Khi đó, zijthể hiện ước lượng vị trí feature thứ j vào thời điểm ở
Kalman Filter cho 1 chiều và 1 feature: Cập nhật trạng thái cho đầu vào sensor:
xi = xi-1 + Ki * [xs - xi-1] (2.4) Cập nhật không chắc chắn cho đầu vào sensor:
vari= vari-1- Ki* vari-1 (2.6)
Cập nhật trạng thái cho điều khiển đầu vào:
xi = xi-1 + ui * T (2.7) Cập nhật không chắc chắn cho điều khiển đầu vào:
vari= vari-1+ varu* T (2.8)
Trong đó giá trị trọng số của Kalman là:
bước i, zilà véc tơ của tất cả các quan sát vào thời điểm ở bước i: zi = [zi0, zi1... zin]. Lưu ý rằng zi0 là một véc tơ xác định vị trí cho feature 0.
Trong môi trường một chiều, ta chỉ cần một phương sai đơn biến var, nhưng điều này không còn đủ để mô hình hóa trong hệ thống. Sự không chắc chắn được lưu trong một ma trận phương sai P cần liên tục cập nhật.
Vector chuyển động ui là véc tơ chuyển động dự báo cho điều khiển đầu vào tại thời điểm bước i, nhưng điều này phải bao gồm cả vận tốc và hướng đa chiều.
Trong ví dụ một chiều, Ki là một trọng số dựa trên sự chênh lệch. Chức năng của nó sẽ được bảo tồn. Tương tự như vậy, một giá trị duy nhất thể hiện sự không chắc chắn vari sẽ không còn được đầy đủ, chúng ta sẽ cần ma trận Pi thể hiện sự không chắc chắn. Ki và Pi sẽ được xem xét trong thời gian ngắn hơn.
3. Phương trình chuyển động
Trong dạng tổng quát của các bộ lọc Kalman được sử dụng trong môi trường SLAM thực tế, các bản cập nhật cần thiết bởi việc điều khiển đầu vào được xử lý bởi “phương trình dự đoán”.Khi chúng ta di chuyển và cần phải “dự đoán” tháithay đổi như thế nào. Các dữ liệu cảm biến được xử lý bởi “phương trình cập nhật”, cập nhật vector trạng thái và chắc chắn dựa trên các dữ liệu. Ta xem xét phương trình dự đoán Phương trình dự đoán có đầu vào thiết bị chuyển động như sau:
xi = Fi * xi-1 + Gi * ui (2.9)
Trong đó:
Fi: là ma trận chuyển dịch trạng thái
Gi: là ma trận có thể dịch đầu vào vào sự thay đổi dự đoán được trong trạng thái. Phương trình 2.9phức tạp hơn của phương trình 2.7.Fimô tảtrạng thái sẽ thay đổi do các nhân tố không liên quan đến điều khiển đầu vào. Một quy ước rất có ích trong SLAM là cho rằng feature này là tĩnh.
Ngoại trừ hàng đầu tiên tương ứng với những thay đổi do robot gây ra, Fi như ma trận đường chéo với mỗi phần tử có chứa ma trận xác định (nếu Fi thay đổi vị trí của các featuremà chúng ta biết là tĩnh). Nếu robot của chúng ta có thể có một vận tốc khác 0 tại bước i, sau đó trạng thái có thể thay đổi ngay cả khi không có đầu vào của thiết bị truyền động, cột đầu tiên của Fi có thể giải thích cho việc này. Để đơn giản hơn một chút, giả sử rằng các robot dừng sau mỗi bước thời gian. Trong trường hợp này, các đầu vào thiết bị truyền động xác định đầy đủ các vị trí có nhiều feature mới của robot. Điều này có nghĩa là Fi là ma trận đơn vịhay là một ma trận đường chéo có đường chéo các phần tử là ma trận định danh và có thể được bỏ qua hoàn toàn. Như vậy, mục đích của chúng ta, đơn giản hóa công thức(2.9):
xi = xi-1 + Gi * ui (2.10)
4. Phương trình Sensor
Đặc điểm kỹ thuật của phương trình "dự đoán" trong các phần trước chỉ còn lại bộ cảm biến, hay "cập nhật", phương trình được sử dụng để kết hợp ước lượng feature dựa trên việc đọc cảm biến. Những phương trình này tương tự như phương trình 2.4 và 2.6 về hình thức, nhưng nó có giá trị để kiểm tra lần lượt để hiểu chính xác tình hình đã thay đổi như thế nào so với môi trường một chiều. Như thường lệ, chúng ta cần cập nhật trạng thái dự đoán và độ tin cậytrong trạng thái hiện tại bằng cách sử dụng dữ liệu mới. Chúng ta bắt đầu với phương trình cập nhật trạng thái:
xi = xi-1 + Ki * vs (2.13)
Trong đó vs là một vector điều chỉnh được đề xuất bởi việc đọc cảm biến mới được định nghĩa như sau:
vs = xs – Hi * xi – 1 (2.14)
Mục đích của Hi là chuyển đổi ước lượng trạng thái trước đó thành một thể hiện được sử dụng bởi các cảm biến. Nói cách khác, nếu các cảm biến đã nhận được trạng thái thế giới đúng như dự đoán của xi-1, chúngthể hiện thông tin dưới dạng Hi * xi - 1. Mục đích của Hi tương tự như của Gi. Thay cho việc sử dụng [phương trình 2.14] ta có:
xi = xi-1 + Ki * [xs – Hi * xi – 1] (2.15)
Phương trình này rất giống với ví dụ môi trường một chiều. Tuy nhiên, Ki được xây dựng trong môi trường đa chiều và chứa nhiều feature. Ki xây dựng trên độ tin cậytương đối của cảm biến so với ước lượng trước như trong phần 2.4 Phát triển một bộ lọc đơn giản,nhưng Ki bây giờ đề cập đến một ma trận và được xác định như sau:
Ki = Pi-1 * HiT * Si-1 (2.16)
Si là hiệp phương sai được xác định như sau:
Si = Hi * Pi-1 * HiT + Rs (2.17)
Trong đó RS là ma trận phương sai cho nhiễu cảm biến. Ri và Si là ma trận, với kích thước cộng với một số feature. Trong phần 2.4 Phát triển một bộ lọc đơn giản, nhiễu cảm biến có thể được biểu diễn chỉ với một biến duy nhất, VAR. Để hiểu tại sao Si và Ri là ma trận, chúng ta sử dụng khi định nghĩa Pi trong phần trước. Kết hợp [phương trình 2.16] và [phương trình 2.17] chúng ta thấy ma trận Kalman xây dựng theo cách này:
Ki = Pi-1 * HiT * [Hi * Pi-1 * HiT + Rs]-1 (2.18)
Sau khi chúng ta đã cập nhật trạng thái, chúng ta sử dụng [phương trình 2.13] để cập nhật Pi như sau:
2.1.7. Bộ lọc Kalman mở rộng
Bộ lọc Kalman mở rộng (EKF) rất giống với bộ lọc Kalman. Phương trình 2.13 và 2.19 được sử dụng trong EKF, phương trình 2.10 và 2.12 được sửa đổi để sử dụng trong EKF. Tuy nhiên, EKF thường được sử dụng hơn so với KF cơ bản trong bài toán thực tế SLAM. EKF phổ biến làdo nó có thể xử lý các mô hình quá trình phi tuyến.
Trong EKF, giả định rằng một feature, e, mô tả cách thay đổi trạng thái từ một bước thời gian đến các bước tiếp theo. f cũng được giả định là có thể được tính toán để tạo ra một ước lượng trạng thái mới. Trong khi SLAM chỉ quan tâm đến việc robot tạo ra thay đổi, không phải là toàn bộ trạng thái, bởi vì chúng ta vẫn đang giả định feature. Như vậy, ta có thể viết lại phương trình (2.10) là:
xi = f[xi-1, ui, i-1] (2.10b) Để giải quyết vấn đề này, EKF sử dụng Jacobi của f (một xấp xỉ của trạng thái).Jacobian của f, kí hiệu Jf, là một ma trận mô tả cách trạng thái sẽ thay đổi sau
Kalman Filter trong môi trường SLAM thực tế:
Cập nhật trạng thái cho thiết bị truyền động: xi = xi-1 + Gi * ui (2.10) Cập nhật không chắc chắn cho đầu vào thiết bị truyền động: Pi = Pi-1 + Qi (2.12)
Cập nhật trạng thái cho việc đọc sensor: xi = xi-1 + Ki * vs (2.13) Cập nhật không chắc chắn cho việc đọc sensor: Pi = Pi-1 – Ki * Si * KiT(2.19)
Trong đó:
vs = xs – Hi * xi – 1 (2.14)
Ki = Pi-1 * HiT * Si-1 (2.16)
thời gian bằngi,trạng thái bắt đầu thay đổi một cách tuyến tính tại thời gian bằng i. Ví dụ, giả sử tốc độ của robot được thay đổi theo một số chức năng phức tạp f, tại thời điểm i robot được làm chậm lại với tốc độ 1 mét trên giây hoặc m / s. Chúng ta có thể mong đợi rằng tại thời điểm i + 1 tăng tốc của robot sẽ khác hơn 1 m / s, nhưng EKF sẽ giả định một sự giảm tốc liên tục cho các mục đích của việc tính toán ma trậnphương sai. Các EKF cập nhật phương trình là:
Pi = Jf * Pi-1* Jf T + Qi (2.12b)
2.1.8. Hạn chế của kĩ thuật lọc Kalman
Bộ lọc Kalman, như đã lập luận ở trên, là công cụ SLAM rất tốt bởi nó là phương pháp tối ưu để giảm thiểu lỗi bình phương. Cho đến nay, việc các biến thể của KF chiếm số lượng gần như tuyệt đối trong các nghiên cứu là bằng chứng cho thấy rằng: KF phương pháp cơ sở để tiếp cận SLAM [1]. Mặt khác, gần đây đã có một số giải pháp tiếp cân SLAM từ những hướng khác. Đã đến lúc chúng ta nên xem xét liệu KF có phải là giải pháp tốt nhất trong mọi trường hợp hay không.
Xét về độ phức tạp, KF luôn duy trì và cập nhật một ma trận phương sai Pi. Ma trận này có kích thướckhá lớn vì nó cần liên hệ giữa feature này đến những feature khác. Do chúng ta phải cập nhật từng giá trị của ma trận ở mỗi lần lặp của bộ lọc. Do vậy, độ phức tạp của mỗi bước lặp trong thuật toán KF là O (m2),với m là số lượng các feature. Trường hợp có thể được thực hiện mà KF đang làm quá nhiều công việc. Ví dụ, chưa đảm bảo rằng trong thực tế có giá trị bất định duy trì bởi KF sẽ phản ánh chính xác các lỗi thực tế trong các cảm biến hoặc trong dự đoán trạng thái. Duy trì phương sai của ước lượng không phải lúc nào cũng tốt trong thực tế.
Vấn đề theo dõi feature là phức tạp và chúng ta không thể đảm bảo rằng một robot sẽ có thể tìm thấy một feature nữa sau khi nó di chuyển. Hơn nữa, các feature sai có thể được di dời hoặc một feature có thể bị nhầm lẫn với một feature khác. Giả sử robot của chúng ta được đặt trong môi trường như vậy, robot được trang bị một loạt
cảm biến. Việc KF sử dụng tất cả các thông tin có sẵn là không hoàn toàn chính xác. bằng cách sử dụng nhiều công cụ tìm tia laser một mình có thể hữu ích hơn là cố gắng để tìm kiếm một vài feature xung quanh.
Hạn chế cuối cùng của KF là khi các robot ghi nhận lại một khu vực trong môi trường và trở về lần nữa. Ngay cả khi các feature cũ có thể được di dời, các robot có thể gặp khó khăn trong việc nhìn thấy chúng và đếm chúng như feature mới. Khả năng của robot để xử lý các vị trí trước đó được biết đến như khả năng kết thúc vòng lặp. Đóng vòng lặp là một trong những vấn đề khó khăn nhất đối với các nhà nghiên cứu SLAM. Để xem lý do tại sao đóng vòng lặp là một vấn đề, hãy tưởng tượng một robot cố gắng để di chuyển trở lại vị trí trước đó sau khi chuyển động xung quanh một số chướng ngại vật. Các lỗi tích lũy do hậu quả của tắc kéo dài các feature quen thuộc có thể đủ để ngăn chặn các robot công nhận featuređược sử dụng trước đó. Đóng vòng lặp đó được mô tả ở hình: