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 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

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 tồ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. Tồ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 toán SLAM dựa trên EKF trong mơi trường thực. Các q 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

Đú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

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

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 qt 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 tốn EKF-SLAM cần tn theo một số phép tính gần đúng và giới hạn các giả định:

Đặc điểm của bản đồ: Trong bản đồ cần bao gồm các điểm mốc. Các điểm

mốc cần biết trước và được quan sát rất rõ trên đường di chuyển của robot.

Nhiễu Gaussian: Như bất kỳ thuật toán EKF nào, EKF-SLAM đưa ra giả định

nhiễu Gaussian cho chuyển động của robot và nhận thức. Lượng khơng chắc chắn ở phía sau phải tương đối nhỏ, vì nếu khơng thì việc tuyến tính hóa trong EKF có xu hướng đưa ra các lỗi không thể đáp ứng được.

Phép quan sát: Các điểm mốc cần được quan sát khá rõ ràng, bởi thuật toán

khơng thể xử lí các mốc bị khuyết một phần (Ví dụ các điểm mốc được làm bằng chất liệu trong suốt như thủy tinh).

Định nghĩa véc tơ trạng thái yt bao gồm trạng thái của robot và toạ độ các điểm mốc trong bản đồ. Với

q = ( x y θ

)T

là véc tơ trạng thái của robot và

m = (m1,x m1, y m2, x m2,y ... mN,x T

N,y là véc tơ chứa toạ độ của N điểm

mốc là toạ độ của các vật cản tĩnh trong môi trường. Từ đó ta có

y = (q m)T

với

x, y và θ biểu diễn tọa độ và hướng của robot tại thời điểm t ;

m

mi,x ,

mi, y là tọa độ điểm mốc thứ i , với i = 1,...,N . Số chiều của véc tơ trạng thái là 2N + 3 , với N biểu

diễn số điểm mốc trên bản đồ. Từ đó, thuật tốn EKF-SLAM sử dụng để tính tốn phân bố xác suất:

p(yt | z1:t ,u1:t ) (2.25) Thuật toán EKF-SLAM cũng bao gồm hai bước đệ quy theo dạng chuẩn của thuật toán SLAM: bước dự đoán và bước chỉnh sửa.

2.3.1. Bước dự đốn (Prediction)

Từ biểu thức phương trình động học (1.3), biểu thức cập nhật độ dời vị trí theo thời gian được xây dựng bởi:

 vx (t) cosθt − vy (t)sinθt 

q = q +  v (t)sinθ + v (t) cosθ ∆t

t t −1   x ω(t)t y t  (2.26) Từ (2.26), biểu thức yt được viết lại thành

 (vx (t) cosθt − vy (t)sinθt )∆t    (vx (t)sinθt + vy (t) cosθt )∆t  y = y +  ω(t)∆t  t t −1    0       0    (2.27) với

∆t là thời gian trích mẫu

và  1 0 0 0 ... 0



F =  0 1 0 0 ... 0   0 0 1 0 ... 0 

là ma trận có kích thước

3×(2N + 3) . Chuyển động của robot chỉ ảnh hưởng tới các giá trị của trạng thái

robot cịn các điểm mốc vẫn giữ khơng đổi nên các giá trị cập nhật cho các điểm mốc đều bằng 0 .

Từ phương trình (2.27) được viết lại thành:

 vx (t) cosθt − vy (t)sinθt  y = y + FT  v (t)sinθ + v (t) cosθ∆t t t −1 x   x ωt (t)y t    (2.28)   x  

Từ đó giá trị kỳ vọng mong muốn cho phân bố xác suất ở bước dự đoán được tính bởi:

 vx,t cos µt −1,θ − vy,t sin µt −1,θ 

µ = µ + FT  v sin µ + v cos µ ∆t t t −1 x   x,t t −1,θ y,t t −1,θ 

ω 

 t 

(2.29)

Mơ hình chuyển động đầy đủ khi có nhiễu được mơ tả bởi:  vx (t) cosθt − vy (t)sinθt 

y = y + FT  v (t)sinθ + v (t) cosθ ∆t + N (0, FT R F )

t t −1 x   x ω(t)t y t  x t x (2.30)

Với thành phần nhiễu N (0, FT R

F ) tuân theo phân phối chuẩn đa biến có kỳ

vọng bằng 0

và Rt là ma trận hiệp phương sai. FT R F là dạng mở rộng ma trận hiệp phương sai đến các chiều của ma trận trạng thái. Từ đó, phương trình (2.30) được viết lại dưới dạng:

y = g(u , yt t t −1) + N (0, FxT R F t x ) (2.31)  vx (t) cosθt − vy (t)sinθt 

Với: g(u , y ) = y + FT  v (t)sinθ + v (t) cosθ ∆t là thành phần phi tuyến

t t −1 t −1 x  x ω(t)t y t 

 

trong mơ hình trạng thái. Sử dụng chuỗi Taylor bậc nhất mở rộng để tuyến tính hố xấp xỉ hàm

g(ut , yt−1) :

g(ut , yt−1) ≈ g(ut , µt−1) + Gt (xt −1 − µt−1) (2.32) Với µt

−1

là giá trị kỳ vọng của trạng thái yt-1 ngay trước đó. Ma trận Jacobi

Gt = g(ut ,μt−1) đạo hàm theo yt-1 được tính tốn bởi

G = I + Ft Tx g Ft x (2.33)

 0 0 −vx,t sin µt −1,θ − vy,t cos µt −1,θ  với g =  0 0 vx,t cos µt −1,θ − vy,t sin µt −1,θ 

0 0 0 

 

Từ đó, tính được biểu thức cập nhật cho ma trận hiệp phương sai:

x t x

x t x

 .

(m x)2  (m y)2i,x i, y

∑ = G ∑ GT + FT R F

t t t −1 t x t x (2.34) Từ phương trình (2.29) và (2.34), biểu thức cập nhật vị trí điểm mốc trong bản đồ và vị trí robot ở bước dự đốn:

  vx,t cos µt −1,θ − vy,t sin µt −1,θ  µ = µ + FT  v sin µ + v cos µ ∆t  t t −1 x  x,t t −1,θ y,t t −1,θ    ω    t   ∑ = G ∑ − GT + FT R F t t t 1 t x t (2.35)

2.3.2. Bước chỉnh sửa (Correction)

Mơ hình đo lường được biểu diễn bởi véc tơ

zi = (ri φi )T với ri là khoảng cách từ cảm biến và φi t t t t

là góc quay của robot so với điểm mốc thứ i .  ri   (m − x)2 + (m − y)2  σ r 0  zi =  t  =  i,x i, y  + N (0, ) φt  a tan 2(mi, y − y, mi,x − x) − θ  0 σφ  t i   (2.36)

Ở đó (m , m )T là toạ độ của điểm mốc thứ i đo được tại thời điểm t ,

i,x i, y

σ r 0 

N (0,  0

) là nhiễu đo lường bất định. Đặt φ    σ r 0  h(y ,i) =   và Q =   . t  a tan 2(m i, y − y, mi,x − x) − θ t  0 σφ 

Ma trận hiệp phương sai

Qt của các giá trị đo lường thu được gồm giá trị khoảng cách từ robot đến các điểm mốc và giá trị góc tương đối giữa điểm mốc đó với hướng của robot. Trong trường hợp này, trước tiên, khởi tạo giá trị ban đầu cho ma trận này bằng cách dựa vào thông số độ phân giải cũng như độ chính xác của cảm biến sử dụng. Các giá trị đo lường thu được sử dụng cảm biến RPLidarA2 với mức độ chính xác khi đo khoảng cách là 1% trong bán kính nhỏ hơn hoặc bằng 3 mét và 2% (m)

khi đo trong khoảng cách từ 3-5 mét. Do đó giá trị σ r

được lựa chọn = 0.022 = 0.004.

Ngồi ra độ phân giải góc của Lidar là 0.45o ~ 0.00785 rad, nên σφ

được lựa chọn =

t

σ

khác như môi trường, ánh sáng, chuyển động của robot, việc thiết lập xây dựng hệ trục tọa độ robot, ... Do đó 2 giá trị này có thể được hiệu chỉnh dựa vào chất lượng đầu ra của thuật toán SLAM. Các giá trị này có thể tăng thêm hoặc giảm đi tương đương với việc yếu tố bất định tồn tại trong mơ hình lường này lớn hoặc nhỏ. Hơn thế nữa, giá trị này cũng có thể thiết lập hiệu chỉnh trực tuyến trong quá trình robot hoạt động dựa trên ROS để từ đó đánh giá và xác định được giá trị tối ưu. Trong phạm vi của luận văn, bằng phương pháp thực nghiệm đã lựa chọn được tham số phù hợp

cho hệ thống đã xây dựng và kịch bản thử nghiệm: σ r = 0.00625 và

σφ = 0.0005.

Hàm h(y

t ,i) được xấp xỉ bằng chuỗi Taylor bậc nhất mở rộng:

h(y ,i) ≈ h(µ ,i) + Hi (y − µ )

t t t t t (2.37) Ở đó i là đạo hàm của h(µt , i) theo µt . Hàm h(µt , i) chỉ phụ thuộc vào 2

trạng thái của véc tơ trạng thái, là vị trí robot qt và vị trí của điểm mốc thứ i là mi . Tiếp đến vị trí mong muốn được tính tốn từ tư thế và các biến đo lường tại điểm mốc:  µ i,x   µt ,x  cos(φi + µ )   =  + rti  ti t ,θ 