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.
xˆk = Axˆk-1 + Buk −1 (2.6)
Pk− = APAT + 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 :
K = P−HT (HP−HT + R)−1 k k k (2.8) xˆ = xˆ−+ K (z − H .xˆ−) k k k k k (2.9) P = (I − K H )P− k k k (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ự đố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 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 phi tuyến tính:
xk = f (xk −1,uk −1, wk −1) (2.11)
zk = h(xk , vk ) (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.
Bước dự đốn Bước hiệu chỉnh
Hình 2.3. Sơ đồ thuật tốn bộ lọc Kalman mở rộng Bước dự đốn với các phương trình cập nhật:
xˆ− = f (xˆ,u ,0)
k k −1 k −1 (2.13)
P−= AP AT + W Q WT
k k −1 k k −1 k (2.14)
Bước hiệu chỉnh với các phương trình cập nhật:
K = P−HT (H P−HT + V R VT )−1 k k k k k k k k k (2.15) xˆ = xˆ− + K (z − h(xˆ−,0)) k k k k k (2.16) P = (I − K H )P− k k k (2.17) Trong đó:
x− ∈ℜn
k
xk ∈ℜ n k
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 .
là ước lượng trạng thái hậu nghiệm ở bước k - 1 nhận được sau phép đo zk
P− là ma trận hiệp biến của sai số ước lượng trạng thái tiên nghiệm.
Pk là ma trận hiệp biến của sai số ước lượng trạng thái hậu nghiệm.
Qk −1 là 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.
Kk là hệ số lọc Kalman.
I là ma trận đơn vị.
Các biến ngẫu nhiên wk như (2.13) và (2.14).
và vk được giả định là nhiễu quá trình và nhiễu đo
A là ma trận Jacobian của các đạo hàm riêng của f theo x : Aij=∂f i ∂x j ( x ˆ k −1,uk −1 , 0)
W là ma trận Jacobian của các đạo hàm riêng của f theo w :
Wij =∂f i ∂w j ( x ˆ k −1 ,uk −1, 0)
H là ma trận Jacobian của các đạo hàm riêng của h theo x : Hij = ∂hi
(xˆ ∂xj
, 0)
V là ma trận Jacobian của các đạo hàm riêng của h theo v : Vij = ∂hi
(xˆ ∂vj
, 0)
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
k
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 q 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:
P(xk ,m | z0:k ,u0:k ) (2.18)
Với u0:k = {u0 , u1,..., uk }= {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 = {m1, m2,..., mn}: tập các điểm mốc thể hiện vị trí vật cản. z0:k = {z0, z1,..., zk} = {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
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 trí robot và vị trí điểm mốc trên bản đồ được xác định có dạng:
khi vị
P(zk | xk ,m) (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:
P(xk | xk-1,uk ) (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ố:
P(xk ,m | z0:k-1,u0:k ) (2.21)
Do trạng thái của robot x
k , xk- 1 và bản đồ m là độc lập, có thể viết lại: P(xk ,m | z0:k −1,u0:k , x0 ) = P(xk | z0:k −1,u0:k )P(m | z0:k −1,u0:k ) = ∫ P(xk | z0:k −1,u0:k , xk −1) P(xk −1 | z0:k −1,u0:k ) P(m | z0:k −1,u0:k )dxk −1 = ∫ P(xk | z0:k −1,u0:k , xk −1) P(xk −1,m | z0:k −1,u0:k ) dxk −1 (2.22)
Vì xk
có tính chất Markov nên (2.22) được viết gọn thành:
P(xk ,m | z0:k −1,u0:k , x0 ) = ∫ P(xk | xk-1,uk ) P(xk-1,m | z0:k-1,u0:k-1 )
dxk-1
(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ố:
P(x ,m | z ,u , x ) = P(zk | xk ,m) P(xk ,m | z0:k-1 ,u0:k )
k 0:k −1 0:k 0
P(z k | z0:k-1,u0:k)
(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ự đố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 − m j
)
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 P(mi , m j
)
là gần
như đạt tới giá trị cực đại ngay cả khi mật độ xác
suất P(mi ) hay P(m 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 ln 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
xk và đang quan sát được hai điểm mốc là mi và m j . 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 m j . 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 m j , cập nhật thông qua điểm mốc m j 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 nó tăng mỗi khi có thêm giá trị quan sát thu được.
Bộ lọc Kalman mở rộng (EKF) là một trong những kỹ thuật phổ biến nhất được sử dụng trong q trình bản đồ hóa robot di động. Trong một số trường hợp, robot di động không thể nhận biết trước về mơi trường, do đó nó khơng thể thực hiện được bản đồ hóa dựa trên bản đồ tiêu chuẩn. Vì vậy, thuật tốn bản đồ hóa và định vị đồng thời đã được xây dựng và phát triển để giải quyết vấn đề này. Cho đến nay, nhiều nghiên cứu đã được thực hiện để cải thiện hiệu suất của SLAM bằng cách phát triển các giải pháp lý thuyết và kết hợp các bộ ước lượng cho việc cải thiện độ chính xác của thuật tốn SLAM. Tuy nhiên, trong thực tế vẫn còn nhiều vấn đề phát sinh do ảnh hưởng của môi trường hoạt động và phương pháp thực hiện khác nhau [75].
Mục tiêu chính của nghiên cứu này là kết hợp thuật toán SLAM dựa trên EKF cơ bản có thể được thực hiện đủ để ước tính trạng thái của robot được vận hành trong môi trường thực liên quan đến các đối tượng động. Một số vấn đề trong thực tế triển khai thuật toán SLAM như xử lý dữ liệu đo lường, loại bỏ triển khai 2D EKF-SLAM cho phép đo độ lệch cho robot di động có bánh xe đa hướng, trích xuất các điểm mốc từ dữ liệu đo, lọc trước các điểm mốc đã trích xuất và liên kết dữ liệu trong các điểm mốc được quan sát trong quá trình vận hành hệ thống SLAM dựa trên EKF[25], [95]. Ngoài ra, việc so sánh hoạt động SLAM dựa trên EKF với hoạt động tính tốn offline cũng được thực hiện để xác định tính hiệu quả và hiệu suất của thuật tốn SLAM dựa trên EKF trong mơi trường thực. Các quá trình thực thi của hệ thống đã được thực hiện để đạt được những mục tiêu này. Đầu tiên, việc mơ phỏng thuật tốn SLAM dựa trên EKF trong các kịch bản khác nhau tương tự như kịch bản thực đã được thực hiện, được đánh giá trước khi triển khai trong thực tế. Việc xác thực SLAM đã được thực
hiện thành cơng trong q trình mơ phỏng, sau đó nó được triển khai trên nền tảng với dữ liệu thực để xác nhận hoạt động đó. Trong trường hợp này, FWOMR được sử dụng làm nền tảng, phương tiện di chuyển trong các tình huống khác nhau quanh khu vực khn viên tịa nhà. Một cảm biến lidar và các bộ mã hóa trục quay được trang bị trên robot cung cấp dữ liệu đo lường thực cho quá trình này. Việc triển khai đồng thời và có hệ thống hoạt động SLAM bằng cách sử dụng robot tự hành làm dữ liệu nền tảng, được thực hiện trong các tình huống khác nhau xung quanh môi trường khảo sát cho thấy rằng SLAM dựa trên EKF tạo ra đáng kể ma trận hiệp phương sai nhỏ hơn, đường đi trơn tru hơn và độ chính xác cao hơn trong ước tính vị trí của robot so với tính tốn trước đó. Từ đó, dữ liệu này chỉ ra rằng hoạt động SLAM dựa trên EKF đã chứng minh đạt kết quả tốt [38], [40],[100] .
Hình 2.5. Quy trình thuật tốn EKF-SLAM
Thuật toán EKF-SLAM là kỹ thuật giải quyết vấn đề SLAM dựa trên thuật toán của bộ lọc Kalman mở rộng với mục tiêu của q trình là sử dụng mơi trường để cập
Đúng Kết thúc Sai lệch vị trí EKF Quan sát mới Cập nhật vị trí EKF Cập nhật vị trí Thay đổi vị trí (độ dời) Bắt đầu
m
t
nhật vị trí của robot. Vì phép đo di chuyển của robot (cung cấp vị trí robot) thường gặp sai số, nên khơng thể dựa trực tiếp vào nó. Ta có thể sử dụng cảm biến lidar quét mơi trường để tăng độ chính xác về vị trí của robot. Điều này được thực hiện bằng cách trích xuất các tính năng từ mơi trường và tái tạo dữ liệu khi robot di chuyển.
Bộ lọc Kalman mở rộng thực hiện việc cập nhật vị trí của robot dựa trên các tính năng này, thường được gọi là điểm mốc. EKF theo dõi ước tính về sự khơng chắc chắn trong vị trí của robot và cả sự khơng chắc chắn ở những điểm mốc mà nó đã thấy trong mơi trường, từ đó nó so sánh với giá trị quan sát được. Nếu có sự sai lệch về vị trí sẽ gửi giá trị để cập nhật vị trí mới và các điểm mốc mới liên tục được cập nhật và so sánh ở bước tiếp theo như Hình 2.5.
Do vậy, thuật toán EKF-SLAM cần tuân theo một số phép tính gần đúng và