Mô hình Bayesian với một số “biến ngẫu nhiên”, biến mà đưa về giá trị khác nhau với xác suất khác nhau tùy thuộc vào các giá trị được trả về bởi các biến khác. Kết quả là một đồ thị dạng cây của các biến ngẫu nhiên phụ thuộc vào nhau và xác định xác suất của các trạng thái khác nhau trên thế giới. Mạng này được gọi là một mạng Bayes. Ví dụ, chúng ta có thể sử dụng sơ đồ sau đây để minh họa cho quá trình SLAM:
Hình 2.5 Mạng Bayesian đơn giản dùng cho SLAM
v thể hiện vị trí thực tế của chiếc xe, trái với xvi, xvi tương ứng với một ước lượng vị trí xe. Mũi tên chỉ sự phụ thuộc - giá trị của bất kỳ biến ngẫu nhiên nào cũng ảnh
hưởng đến xác suất liên quan đến mỗi nút con của nó. V1 trong ví dụ phụ thuộc vào cả hai vị trí xe trước và điều khiển đầu vào cuối cùng. Biến trong biểu đồ có thể phụ thuộc vào các biến khác thậm chí nếu không có mũi tên trực tiếp kết nối chúng. Ví dụ, giả sử chúng ta không biết giá trị của v1 nhưng chúng ta muốn biết giá trị của v2, v2 là phụ thuộc vào v0. Mặt khác, nếu chúng ta biết giá trị của v1, v0 sẽ hoàn toàn không liên quan đến giá trị của v2, chúng ta nói v2 là độc lập có điều kiện của v0 từ v1.
Độc lập có điều kiện cho phép các bộ lọc Kalman làm tối ưu ước lượng trong khi chỉ cập nhật ước lượng trạng thái và các giá trị không chắc chắn, tất cả việc đọc cảm biến trước, ước lượng trạng thái và điều khiển đầu vào là không thích hợp cho các dự báo gần nhất của trạng thái và các giá trị không chắc chắn. Các quy tắc độc lập có điều kiện trong một mạng Bayes được xác định bởi tập hợp các quy tắc d-khả ly (d-separation). Hai biến được cho là độc lập với nhau nếu tất cả các con đường giữa chúng là d-khả ly. Chỉ có một quy tắc d-khả ly là thích hợp và được minh họa dưới đây.
Hình 2.6 Minh họa d-khả ly
Chúng ta hãy xem xét một mạng Bayes liên quan đến vị trí robot, quan sát được thực hiện bởi các cảm biến. Chúng ta mong đợi rằng không có quan sát nào độc lập có điều kiện với bất kỳ feature nào. Ta cũng thấy rằng lỗi kết quả ước lượng pose trong mối quan hệ rắc rối giữa các cảm biến dựa trên ước lượng feature. Sơ đồ ở hình 3.3giúp chúng ta hiểu được phụ thuộc có điều kiện thích hợp trong SLAM, mạng Bayes bên trái minh họa cách quan sát thực hiện bởi các robot bị ảnh hưởng bởi kết quả ước lượng pose và vị trí xe, lỗi của biến ngẫu nhiên. Các mạng Bayesian bên
phải đại diện cho một mạng lưới tương tự được đưa xe v, được xác định bởi xvi và E(xvi):
Hình 2.7 Mạng Bayesian liên quan đến vị trí robot
Trong đó:
- XV là pose robot được ước lượng - V : Pose robot thực sự
- E(xv): Lỗi trong pose của robot được ước lượng - Oi: quan sát dựa trên việc đọc cảm biến thứ i
Chúng ta có thể tìm thấy không có quan hệ độc lập có điều kiện trong mạng Bayesian mà không kết hợp v, ngay cả khi chúng ta chọn một ước lượng cho xvi để tạo ra d- khả ly, quan sát vẫn còn gián tiếp phụ thuộc vào nhau qua E(xvi). Tuy nhiên, trong mạng bên phải ước lượng là độc lập có điều kiện với nhau cho vị trí robot thực tế của d-khả ly. Điều này sẽ có ý nghĩa trực quan tốt. Khi chúng ta thấy cần một ma trận của các giá trị không chắc chắn Pi, nếu định hướng con robot dự đoán là hơi tắt, một trong số các ước lượng feature sẽ luân chuyển và do đó có liên quan đến nhau. Nếu chúng ta biết vị trí chính xác của robot thì không nên có mối quan hệ giữa các feature quan sát.
Tất nhiên, một điểm rất quan trọng trong SLAM là chúng ta không biết các pose của robot một cách chính xác, nhưng sự độc lập có điều kiện feature cho pose để FastSLAM xử lý từng feature riêng biệt. Có lẽ mối quan hệ giữa các feature không có giá trị theo dõi lưu giữ nếu chúng ta có thể làm cho một ước lượng đủ của biến ngẫu nhiên, khi biết chính xác thì d-khả ly sẽ tách tất cả các ước lượng feature. 2.2.2. Thuật toán FastSLAM
Ta xét xác suất p. Với bất kỳ đầu vào nào, p trả về xác suất của đầu vào. Ví dụ, robot của chúng ta trong môi trường một chiều được xác định bởi các biến x. Sau đó p (x) xác định sẽ là một đường cong Gauss. Nếu x là một pose của chiếc xe đa chiều ước lượng feature, p (x) xác định khả năng của tất cả các trạng thái có thể. p ( x | { u0, u1,... ui }, { z0, z1,... zi } ) mô tả khả năng của bất kỳ trạng thái trên thế giới cho tất cả các cảm biến và chuyển dữ liệu được thực hiện bởi các robot ở bất cứ thời điểm i nào. Trong đó, xi thể hiện một trạng thái thế giới cụ thể, trong khi x đề cập đến tất cả các trạng thái trên thế giới. x chia thành pose của xe, v và địa điểm p0, p1,... pm. Giống như feature x, các biến này đại diện cho tất cả các vị trí có thể của pose và chúng ta có thể phá vỡ p (x | Ui, Zi ) như sau:
p(x | Ui,Zi) = p(v,p0,p1,…pm| Ui,Zi) (2.20) Bây giờ chúng ta sẽ sử dụng xác suất số học để đơn giản hóa bài toán SLAM. Giả sử chúng ta có hai biến ngẫu nhiên không liên quan A và B. Chúng ta có thể nói, p (A, B) = p (A) * p (B). Nếu giá trị của A phụ thuộc vào giá trị của B. Nếu A không phụ thuộc vào B, thừa số thích hợp là p (A, B) = p (A) * p (B | A) chúng ta biết dự đoán feature phụ thuộc vào dự đoán trạng thái có nghĩa là chúng ta có thể viết
p(v,p0,p1,…pm| Ui,Zi)như sau:
p(v,p0,p1,…pm| Ui,Zi) = p(v | Ui,Zi) * p(p0,p1,…pm| Ui,Zi,v) (2.21) Nếu chúng ta đủ ước lượng chính xác, chúng ta có thể sử dụng thừa số sau:
p(v | Ui,Zi) * p(p0,p1,…pm| Ui,Zi,v) = p(v | Ui,Zi) * p(p0 | Ui,Zi,v) * p(p1 | Ui,Zi,v) * …
Tóm lại, ta có:
p(x | Ui,Zi) = p(v | Ui,Zi) *∏m p(pm| Ui,Zi,v) (2.23) 2.2.3. Fast SLAM và particle
KF không theo một con đường duy nhất mà cập nhật pose duy nhất của robot, bước đi mới nhất trên con đường Robot FastSLAM để khi thuật toán kết thúc sẽ cập nhật lại kết quả mới cho những nơi các robot đã đi qua và nơi nó dừng lại trên đường đi. FastSLAM không sử dụng pose cũ trong các đường dẫn cho bất kỳ ước lượng nào. Vì vậy, pose cũ là ước lượng cơ bản trong thời gian. Khi đường dẫn Robot đầy đủ không được sử dụng và không quan trọng cho bài toán SLAM, chúng ta sẽ trình bày một thuật toán FastSLAM thích hợp và cập nhật ước lượng pose chứ không phải là con đường phát triển robot. Điều này sẽ đơn giản hóa các ký hiệu và thường là các công thức trông quen thuộc hơn với những gì có trong luận văn này.
Duy trì nhiều ước lượng pose, mỗi chỉ số thời gian đòi hỏi một biến thay đổi của ký hiệu mà chúng ta đặt cho pose của robot. Trước đây, xvi gọi pose chiếc xe tại thời điểm i. Bây giờ xvi thiết lập pose của robot ở thời gian i. xvij là ước lượng pose thứ j vào thời điểm i. Do đó chúng ta có xvi = { xi0, xi1,... xim)và mỗi xij gọi là một particle. 2.2.4. Ước lượng Pose
Giả sử chúng ta có một tập ước lượng pose xvi -1 và có một điều khiển đầu vào ui. Tập Particle không được cập nhật cho đến khi chúng ta thực hiện ước lượng vị trí feature từ những quan sát trong vị trí mới. Thay vào đó, ta tạo ra hàng hoạt Xvt, bộ Particle tạm thời ui và các hạt M trong xvi -1. Cả hai số M của các Particle, chúng ta duy trì trong các bộ lọc và số lượng chúng ta tạo ra cho hoàng loạt Xvt phụ thuộc vào FastSLAM trong thực tế. Khi tăng số lượng các featuer thì các Particle có thể giảm. Xvt được lựa chọn xác suất bằng cách lấy mẫu từ xvi -1. Ví dụ, để tạo ra một hạt cho hàng loạt Xvt chúng ta bắt đầu với một hạt từ xvi -1, xv (i -1 ), j. Sau đó chúng ta sử dụng mật độ xác suất theo quy định của xv (i -1 ), j + ui để lựa chọn tạm thời particle mới cho hàng loạt xvt và quá trình này được minh họa ở hình 2.23.
Hình 2.8 Lựa chọn particle tạm thời theo mật độ xác suất
Quá trình này được lặp lại, lựa chọn Particle tốt hơn từ xvi-1 thường xuyên hơn cho đến khi co N particle trong xvt với N>=M. Khi chúng ta có xvt, chúng ta cần một phương pháp để chọn các Particle tốt nhất từ xvt để mang vào xvi. Mỗi Particle trong xvt, xvtk mang một trọng lượng đặc biệt wtk. Căn cứ vào trong lượng đó để tăng kì vọng cho các Particle. Điều mấu chốt là ta muốn giữ lại các Particle mà những quan sát mới hỗ trợ. Ta có công thức sau:
(2.24) 2.2.5. Ước lượng feature
Chúng ta không cần phải dừng lại ở lọc Kalman trong thuật toán FastSLam. Nhưng chúng ta cần một bộ lọc Kalman cho mỗi feature, mỗi particle.
2.2.6. Vòng lặp đóng với FastSLAM
Vòng lặp đóng đến một lúc nào đó được xử lý một cách tự nhiên bởi FastSLAM. Chúng ta đã biết, mỗi particle trong tập tạm thời có một trọng số dựa trên quan sát. Khi robot khám phá một khu vực mới, robot không tưởng tượng được khu vực trông như thế nào và một trong hai điều có thể xảy ra. Một là những particle tạo ra ước lượng tốt trong khi những particle khác thì không, khi đó trọng số được chọn là của các particle đa dạng và xấu thì đều được loại bỏ. Hai là, tất cả các particle tạo ra ước lượng tồi và vòng lặp sẽ đóng ngay cả khi các particle tốt nhất. Trường hợp hai này thường xảy ra khi robot di chuyển trong thời gian dài.
Sự phức tạp tính toán của là O ( m * M), hay là được tính bằng số feature nhân với số các particle. O ( m * M ) là kết quả của việc sao chép các bản đồ cũ cho các particle mới được thiết lập. Nếu thay cho việc sao chép mỗi bản bằng việc thay đổi feature phù hợp với từng particle có thể làm giảm độ phức tạp đáng kể. Chúng ta sử dụng cây để lưu trữ bản đồ. Feature lưu trữ kết quả trong O (M * log ( m ) ). Điều này trái ngược với KF phương pháp tiếp cận mà chạy trong thời gian nhất định O (m2) thậm chí cho một quan sát duy nhất.
Kiểm tra thử nghiệm đã chỉ ra rằng, một vài particle là đủ với bất kể số lượng các feature, do đó sự phức tạp của FastSLAM có xu hướng tốt hơn so với sự phức tạp của KF. Nếu chúng ta sử dụng một bộ lọc Kalman để theo dõi cùng một số feature, nó cần thực hiện hơn 2,5 tỷ cập nhật phương sai cho một quan sát duy nhất của một mốc duy nhất. Rõ ràng FastSLAM đại diện cho một cải tiến lớn so với KF đưa ra một môi trường nơi mà hàng chục ngàn các feature cần phải được ánh xạ, đặc biệt là khi chúng ta xem xét khả năng của vòng lặp đóng cải tiến trong FastSLAM.
Tóm lại, FastSLAM được dựa trên sự độc lập có điều kiện của feature. Vấn đề SLAM có thể được chia thành m + 1 vấn đề ước lượng riêng biệt nếu chúng ta có thể giải thích cho lỗi trong pose của robot. Chúng ta ước lượng trạng thái tối ưu bằng cách theo dõi nhiều ước lượng pose và trọng lượng bản đồ dựa trên từng particle một cách
thích hợp. Các particle được duy trì bởi xác suất lấy mẫu từ trước đó và xác suất loại bỏ pose mà không phản ánh dữ liệu cảm biến mới. Kết quả là một xấp xỉ của các thuật toán lập bản đồ tối ưu được thực hiện trong thời gian thực với hàng chục ngàn các feature.
2.3. DP-SLAM
DP-SLAM [1] với mục đích là đồng thời định vị và lập bản đồ mà không cần mốc. DP- SLAM thích hợp với kĩ thuật mà bản đồ chính xác khi một vòng lặp được đóng lại. Điều đáng chú ý của DP-SLAM là nó cũng đủ chính xác để kết thúc vòng lặp. DP-SLAM hoạt động bằng việc duy trì phân bố xác suất chung trên bản đồ và pose của robot sử dụng sử dụng một bộ lọc particle. Điều này cho phép DP-SLAM duy trì sự không chắc chắn về bản đồ qua nhiều bước thời gian cho đến khi việc nhập nhằng này được giải quyết. Điều này ngăn cản các lỗi trong bản đồ được tạo ra theo thời gian.
2.3.1. Mô tảthuật toán DP-SLAM
DP-SLAM sử dụng một bộ particle để duy trì một phân bố xác suất chung trên bản đồ và vị trí robot. Điều này sẽ rất tốn kém mà không có cấu trúc dữ liệu tốt cho đến khi nó yêu cầu một bản sao hoàn chỉnh của toàn bộ các ô lưới cho mọi particle và yêu cầu tạo bản sao của bản đồ trong suốt quá trình tạo lại mẫu trong bộ lọc particle. Do đó, trong DP-SLAM, chúng ta duy trì một cây Particle để tránh số lượng lớn sao chép của ô lưới. Điều này là trái với FastSLAM. Trong FastSLAM ta chỉ duy trì một kích thước cố định của tập các particle và không quan tâm đến các particle cũ. Cây Particle có chiều cao tuyến tính với lượng thời gian chạy thuật toán. Nghĩa là không có ràng buộc về lượng thời gian cần thiết để kiểm tra một ô trong ô lưới. Vấn đề này được giải quyết bằng cách xem xét cẩn thận một phương pháp collapsing nhánh của cây. Chú ý rằng bất kỳ Particle nào mà không tạo ra con nào nghĩa là ước lượng pose kém vàơ sẽ được gỡ bỏ, các nút tổ tiên cũng sẽ được gỡ bỏ. Ngoài ra, nếu một Particle chỉ có một con duy nhất, particle đó có thể được kết hợp với nút cha và được coi là particle duy nhất. Kĩ thuật “cắt tỉa” này được mô tả như trong hình dưới đây.
Hình 2.9 Cây particle
Trong hình vẽ 2.9, Pose 3 sẽ được cắt ra vì nó chỉ có một con duy nhất, nhưng các thông tin của pose 3 không bị xóa mà nó sẽ được chuyển đến pose 6. Nếu chúng ta duy trì cây theo cách này thì cây sẽ được phân nhánh ít nhất thành hai nhánh và độ sâu của cây không lớn hơn số các Particle trong mỗi thế hệ.
2.3.2. Bản đồ phân bố Particle
Qua cây Particle, chúng ta có thể tiết kiệm thời gian bằng cách chỉ nhớ bản đồ của một particle con từ bản đồ cha của nó. Để thực hiện điều này, ta có thể lưu trữ danh sách các thay đổi của mỗi particle mà các ô lưới khác với bản đồ particle cha. Lưu trữ các thông tin của bản đồ theo cách này có một chút vấn đề khi chúng ta muốn sử dụng bản đồ. Giả sử như robot muốn kiểm tra khu vực ngay phía trước nó để xem có trở ngại hay không. Nếu chúng ta muốn kiểm tra một particle thì có nhiều việc liên quan tới nó như là tất cả các cây tổ tiên phải được cập nhật và danh sách thay đổi cho mỗi particle phải được tìm kiếm cho mỗi điểm. DP-SLAM giải quyết vấn đề này bằng cách phát triển một bản đồ phân bố particle. Bản đồ này là một lưới với một cây cân bằng được lưu trữ trong mỗi nút. Thông tin ô lưới từ tất cả các particle được lưu trữ trong cây. Bản đồ này có thể được hình dung như trong hình 2.10.
Hình 2.10 Bản đồ phân bố particle
Một particle chứa một quan sát của một ô vuông, nó xác định một id duy nhất vào trong cây cùng với dự đoán cho ô lứoi hay không ô lưới. Nếu robot muốn kiểm tra một điểm cụ thể, robot chỉ cần tìm kiếm qua các cây của particle đã cập nhật cho các ô. Đồng thời, ta có thể giữ cho các cây tổ tiên xung quanh cho biết khi nào ta có thể cắt tỉa hoặc giữ lại các nhánh của bản đồ phân phối particle. Một con không có nút anh em sẽ được sát nhập vào nút cha trong cây tổ tiên, trước tiên ta có thể xem xét