- Mở rộng khảo sát chất lượng hệ thống SLAM trên các tập dữ liệu ở môi trường khác.. TÓM TẮT LUẬN VĂN Luận văn này tập trung xây dựng một hệ thống định vị và vẽ bản đồ đồng thời SLAM trê
CƠ SỞ LÝ THUYẾT
Lý thuyết Lie
(Tài liệu tham khảo chính của phần này là [50])
Trong toán học, đa tạp trơn (smooth manifold) là một không gian topo nhưng có thể được xấp xỉ tuyến tính bằng một không gian vector tại từng điểm Ví dụ điển hình về đa tạp trơn chính là những siêu mặt phẳng, siêu mặt cong nhúng trong không gian đa chiều ℝ 𝑛 Tại mỗi điểm 𝒳 của một đa tạp trơn ℳ luôn tồn tại duy nhất một không gian tiếp tuyến 𝑇 𝒳 ℳ, cũng chính là xấp xỉ tuyến tính của ℳ tại 𝒳
Hình 3.1 Luôn tồn tại một không gian vector 𝑇 𝒳 ℳ, tiếp tuyến với đa tạp trơn ℳ tại điểm 𝒳, xấp xỉ tuyến tính ℳ [50]
Trong toán học, nhóm (group) (𝒢,∘) là một tập hợp 𝒢 được trang bị một phép hợp (composition operation):
∘ ∶ 𝒢 × 𝒢 → 𝒢, (𝒳, 𝒴) ↦ 𝒳 ∘ 𝒴 (3.1.1) thoả mãn các tiên đề sau ∀𝒳, 𝒴, 𝒵 ∈ 𝒢:
Phép ∘ được gọi là phép hợp (composition operation) trong 𝒢 và không có tính giao hoán Phần tử đơn vị ℰ của một nhóm 𝒢 bất kì đều là duy nhất Tính duy nhất cũng áp dụng cho nghịch đảo của một phần tử bất kì trong nhóm
Nhóm Lie (Lie group) là một đa tạp trơn, trong đó các phần tử thoả các tiên đề của nhóm Nhóm Lie vừa thừa hưởng tính cục bộ của đa tạp trơn, qua đó cho phép ta thực hiện các phép toán giải tích, đại số tuyến tính truyền thống như cộng trừ vector xung quanh xấp xỉ tuyến tính, vừa tận dụng tính toàn cục của nhóm để thực hiện nhiều biến đổi phi tuyến trên một miền xác định rộng lớn Một phép toán bổ sung khá thông dụng trong nhóm Lie là phép tác động (action operation), cho phép ta chuyển đổi phần tử của các tập hợp khác Cụ thể, phép tác động của một nhóm Lie ℳ đối với tập 𝒱 là một ánh xạ:
∙ ∶ ℳ × 𝒱 → 𝒱, (𝒳, 𝓋) ↦ 𝒳 ∙ 𝓋 (3.1.2) thoả mãn các tiên đề sau ∀𝒳, 𝒴 ∈ 𝒢 và ∀𝓋 ∈ 𝒱:
3.1.2 Không gian tiếp tuyến của nhóm Lie
Vì cấu trúc của không gian tiếp tuyến 𝑇 𝒳 ℳ không phụ thuộc vị trí xấp xỉ tuyến tính 𝒳 nên 𝑇 𝒳 ℳ được gọi chung là đại số Lie (Lie algebra) 𝔪 của nhóm Lie ℳ:
𝔪 ≜ 𝑇 ℰ ℳ = 𝑇 𝒳 ℳ, ∀𝒳 ∈ ℳ (3.1.3) Nếu 𝑚 là số bậc tự do của ℳ thì 𝔪 có số chiều là 𝑚 Vì 𝔪 là không gian vector nên mọi phần tử thuộc 𝔪 đều có thể được biểu diễn duy nhất thông qua một vector thuộc ℝ 𝑚 và ngược lại Do đó, ta có thể chuyển đổi qua lại giữa các phần tử thuộc 𝔪 và ℝ 𝑚 thông qua 2 phép đẳng cấu:
(3.1.5) trong đó {𝐞 𝑖 } là cơ sở tiêu chuẩn của ℝ 𝑚 và 𝐸 𝑖 = 𝐞 𝑖 ∧ Nói cách khác, 𝛕 chính là toạ độ của 𝛕 ∧ trong cơ sở {𝐸 𝑖 } của 𝔪 toạ thành từ cơ sở tiêu chuẩn {𝐞 𝑖 } của ℝ 𝑚 Để chuyển đổi qua lại giữa các phần tử thuộc ℳ và 𝔪, ta sử dụng 2 ánh xạ: exp : 𝔪 → ℳ, 𝛕 ∧ ↦ 𝒳 = exp(𝛕 ∧ ) (3.1.6) log : ℳ → 𝔪, 𝒳 ↦ 𝛕 ∧ = log(𝒳) (3.1.7)
Trong nhiều trường hợp, ta cần quan tâm phần tử thuộc không gian tiếp tuyến cục bộ 𝑇 𝒳 ℳ thay vì không gian toàn cục 𝑇 ℰ ℳ Do đó, 𝛕 𝒳 ∧ ∈ 𝑇 𝒳 ℳ và ℰ 𝛕 ∧ ∈ 𝑇 ℰ ℳ được sử dụng để phân biệt loại không gian tiếp tuyến đang được nhắc tới Để thao tác trực tiếp trên không gian ℝ 𝑚 tuyến tính thay vì 𝔪 phi tuyến, ta định nghĩa thêm 2 ánh xạ:
Exp ∶ ℝ 𝑚 → ℳ, 𝛕 ↦ 𝒳 = Exp(𝛕) ≜ exp(𝛕 ∧ ) (3.1.8) Log ∶ ℳ → ℝ 𝑚 , 𝒳 ↦ 𝛕 = Log(𝒳) ≜ log(𝒳) ∨ (3.1.9) Trong nhiều trường hợp, ta cần chuyển đổi độ dời đã tính trong 𝑇 𝒳 ℳ sang 𝑇 ℰ ℳ và ngược lại Để thuận tiện, gọi 𝛕 𝒳 ∧ ∈ 𝑇 𝒳 ℳ là độ dời nguyên gốc trong 𝑇 𝒳 ℳ và
ℰ ∈ 𝑇 ℰ ℳ là biểu diễn độ dời 𝒳 𝛕 ∧ trong 𝑇 ℰ ℳ Cũng có thể hiểu ngược lại: ℰ 𝛕 ∧ ∈
𝑇 ℰ ℳ là độ dời nguyên gốc trong 𝑇 ℰ ℳ và 𝒳 𝛕 ∧ ∈ 𝑇 𝒳 ℳ là biểu diễn độ dời ℰ 𝛕 ∧ trong
𝑇 𝒳 ℳ Người ta đã chứng minh được rằng:
𝒳 ∘ Exp( 𝛕 𝒳 ) = Exp( 𝛕 ℰ ) ∘ 𝒳 (3.1.10) Để biểu diễn độ dời giữa các phần tử của ℳ thông qua khái niệm vector tuyến tính truyền thống, ta giới thiệu phép ⊕ và ⊖ Vì phép hợp trong ℳ không có tính giao hoán nên ta trình bày 2 phiên bản của ⊕ và ⊖: phiên bản cánh phải và cánh trái Phiên bản cánh phải như sau:
Vì 𝒳 𝛕 ∈ 𝑇 𝒳 ℳ nên ta nói 𝒳 𝛕 được biểu diễn cục bộ tại 𝒳 Phiên bản cánh trái như sau:
Vì ℰ 𝛕 ∈ 𝑇 ℰ ℳ nên ta nói ℰ 𝛕 được biểu diễn toàn cục Việc biểu diễn độ dời cục bộ tại 𝒳 hay toàn cục không ảnh hưởng đến kết quả cuối cùng:
𝒳 ⊕ 𝛕 𝒳 = 𝛕 ℰ ⊕ 𝒳 (3.1.15) Để thuận tiện cho việc chuyển đổi qua lại giữa 𝒳 𝛕 và ℰ 𝛕, ta sử dụng một ánh xạ tuyến tính trong 𝔪, gọi là phép liền kề (adjoint operation) của ℳ tại 𝒳:
16 Định nghĩa chi tiết về phép liền kề nằm ngoài giới hạn của luận văn Ở mục sau, ta sẽ cụ thể hoá phép liền kề cho trường hợp nhóm Lie ma trận Vì 𝒳 𝛕 là toạ độ của
𝒳 trong 𝑇 𝒳 ℳ và ℰ 𝛕 là toạ độ của ℰ 𝛕 ∧ trong 𝑇 ℰ ℳ, tồn tại một ma trận chuyển toạ độ (hay ma trận liền kề) 𝐀𝐝 𝒳 ∈ ℝ 𝑚×𝑚 từ 𝒳 𝛕 sang ℰ 𝛕:
Một số tính chất đáng chú ý của ma trận liền kề như sau:
3.1.3 Đạo hàm trong nhóm Lie
Xét một ánh xạ 𝑓: ℳ → 𝒩 giữa hai nhóm Lie ℳ và 𝒩 Tận dụng phép ⊕ và
⊖, ta định nghĩa đạo hàm cánh phải (hay ma trận jacobian cánh phải) của 𝑓(𝒳) tại
𝒳𝛕 ∈ ℝ 𝑛×𝑚 (3.1.20) trong đó 𝑛 và 𝑚 lần lượt là số bậc tự do của 𝒩 và ℳ Chú ý phép chia trong biểu thức trên cần được hiểu là phép lấy đạo hàm (ma trận jacobian) của vector trong ℝ 𝑛 tại vector trong ℝ 𝑚 :
Việc định nghĩa đạo hàm trong nhóm Lie như trên độc lập với cấu trúc phức tạp của các phần tử trong 𝒩 và ℳ Vi phân của 𝑓(𝒳) tại 𝒳 được định nghĩa là vi phân trong các không gian tiếp tuyến cục bộ tương ứng 𝑇 𝑓(𝒳) 𝒩 và 𝑇 𝒳 ℳ Ta cũng rút ra biểu thức xấp xỉ Taylor bậc nhất khi 𝒳 𝛕 nhỏ:
𝒳𝛕 (3.1.22) Đạo hàm cánh trái (ma trận jacobian cánh trái) được tính tương tự:
17 Đạo hàm cánh trái biểu diễn vi phân của 𝑓(𝒳) tại 𝒳 trong các không gian tiếp tuyến toàn cục tương ứng 𝑇 ℰ 𝒩 và 𝑇 ℰ ℳ Xấp xỉ Taylor bậc nhất khi ℰ 𝛕 nhỏ như sau:
Tận dụng tính chất của phép liền kề, dễ dàng chứng minh hai loại đạo hàm liên hệ với nhau thông qua biểu thức sau:
Từ đây trở đi, nếu không đề cập thêm, ta hiểu đạo hàm đang được nói đến là đạo hàm cánh phải, phép ⊕ và ⊖ được nhắc đến thuộc cánh phải Một quy tắc quan trọng khi thực hiện đạo hàm chính là quy tắc chuỗi Cụ thể, gọi thêm một ánh xạ khác 𝑔: 𝒩 → 𝒪 giữa hai nhóm Lie 𝒩 và 𝒪 Nếu 𝒴 = 𝑓(𝒳) và 𝒵 = 𝑔( 𝒴) thì 𝒵 𝑔( 𝑓(𝒳)) Từ tính xấp xỉ Taylor bậc nhất, dễ dàng chứng minh quy tắc chuỗi:
Sau đây, ta rút ra một số kết quả hữu ích khi thực hiện đạo hàm:
3.1.4 Đa tạp phức hợp các nhóm Lie
Một đa tạp phức hợp (của các nhóm Lie) ℳ ≜ 〈ℳ 1 … ℳ 𝑀 〉 là hợp của 𝑀 đa tạp thành phần không tương tác nhau Mỗi phần tử 𝒳 ∈ ℳ là tập hợp các phần tử từ các đa tạp thành phần
𝒳 ≜ 〈𝒳 1 … 𝒳 𝑀 〉 ∈ ℳ, 𝒳 1 ∈ ℳ 1 , … , 𝒳 𝑀 ∈ ℳ 𝑀 (3.1.29) Nhằm thoả các tiên đề của nhóm, phần tử đơn vị, phần tử nghịch đảo và phép hợp của đa tạp phức hợp được định nghĩa như sau:
Phép Exp(∙) và Log(∙) được định nghĩa tương tự để thống nhất kí hiệu:
Biểu diễn vị trí và góc hướng
(Tài liệu tham khảo chính của phần này là [50])
3.2.1 Biểu diễn góc hướng 3 chiều Để biểu diễn góc hướng của một hệ toạ độ trong không gian 3 chiều, ta sử dụng ma trận xoay 𝐑 ∈ 𝑆𝑂(3) Người ta đã chứng minh 𝑆𝑂(3) là một nhóm Lie ma trận
Ta kí hiệu đại số Lie của 𝑆𝑂(3) là 𝔰𝔬(3) Mỗi phần tử 𝛉 ∧ ∈ 𝔰𝔬(3) là một ma trận đối xứng xiên (skew-symmetric matrix):
Một liên hệ thú vị là 𝛉 chính là biểu diễn góc-trục xoay (axis-angle) của phép xoay thực hiện bởi ma trận xoay 𝐑 Do đó, khi phân tích 𝛉 = 𝜃𝐮, 𝜃 chính là lượng xoay quanh trục 𝐮 Người ta đã chứng minh phép Exp(∙) và Log(∙) trong 𝑆𝑂(3) có dạng tường minh thông qua công thức Rodrigues kinh điển Để tính ma trận xoay 𝐑 từ 𝛉, ta áp dụng phép Exp(∙):
𝐑 = Exp(𝛉) = 𝐈 3×3 + sin 𝜃 [𝐮] × + (1 − cos 𝜃)[𝐮] × 2 (3.2.2) Để tính 𝛉 từ 𝐑, ta áp dụng phép Log(∙):
Chú ý khi lập trình, góc 𝜃 có thể rất nhỏ, khiến kết quả trả dữ liệu rác hoặc nan Để xử lý vấn đề này, ta nên sử dụng xấp xỉ Taylor nếu 𝜃 nhỏ: sin 𝜃
3.2.2 Biểu diễn tư thế 3 chiều Để biểu diễn vị trí và góc hướng, hay còn gọi là tư thế (pose), trong không gian
3 chiều, ta sử dụng ma trận thuần nhất 𝐓 ∈ 𝑆𝐸(3):
𝟎 1×3 1] ∈ 𝑆𝐸(3) ⊂ ℝ 4×4 , 𝐑 ∈ 𝑆𝑂(3), 𝐭 ∈ ℝ 3 (3.2.5) Người ta đã chứng minh 𝑆𝐸(3) là một nhóm Lie ma trận Ta kí hiệu đại số Lie của 𝑆𝐸(3) là 𝔰𝔢(3) Mỗi phần tử 𝛕 ∧ ∈ 𝔰𝔢(3) đều có dạng:
𝛉] ∈ ℝ 6 (3.2.6) trong đó 𝛒 ∈ ℝ 3 và 𝛉 ∈ ℝ 3 lần lượt là thành phần toạ độ của 𝛕 ∧ liên quan tới vị trí và góc hướng Chú ý 𝛉 cũng chính là biểu diễn góc-trục xoay (axis-angle) của phép xoay thực hiện bởi ma trận xoay 𝐑 Vì cấu trúc đặc biệt của ma trận thuần nhất và ma trận xoay, ta có thể rút ra các công thức ngắn gọn sau khi tính nghịch đảo và nhân ma trận trên các ma trận thuần nhất:
Công thức tường minh cho phép Exp(∙) và Log(∙) trong 𝑆𝐸(3) như sau:
Log(𝐑) ] (3.2.8) trong đó các ma trận trung gian 𝐕(𝛉) và 𝐕 −1 (𝛉) được tính như sau:
1 − cos 𝜃 [𝛉] × 2 (3.2.10) Chú ý khi lập trình, góc 𝜃 có thể rất nhỏ, khiến kết quả trả dữ liệu rác hoặc nan Để xử lý vấn đề này, ta nên sử dụng xấp xỉ Taylor nếu 𝜃 nhỏ:
Ma trận liền kề của phép liền kề trong 𝑆𝐸(3) có dạng:
0 𝐑 T ] ∈ ℝ 6×6 (3.2.12) Phép tác động của 𝑆𝐸(3) đối với ℝ 3 được định nghĩa như sau:
𝐓 ∙ 𝐩 ≜ 𝐑𝐩 + 𝐭, 𝐩 ∈ ℝ 3 (3.2.13) Ánh xạ này rất hữu ích khi ta cần chuyển toạ độ các điểm trong không gian Thực hiện lấy đạo hàm, ta thu được:
Tối ưu bình phương cực tiểu phi tuyến
3.3.1 Dẫn giải bài toán IRLS
(Tài liệu tham khảo chính của phần này là [51])
Giả sử ta cần tối ưu biến 𝐱 thuộc một đa tạp bất kì ℳ Thực hiện phép đo cho ra các thặng dư (residual) 𝑟 𝑖 = 𝑟 𝑖 (𝐱) ∈ ℝ phụ thuộc giá trị 𝐱 Kết quả đo tổng hợp được biểu diễn dưới dạng vector:
Do nhiễu cảm biến và nhiều yếu tố bất định khác, các thặng dư 𝑟 𝑖 phân bố theo một mô hình xác suất, hay mô hình đo lường (measurement model) 𝑝(𝑟 𝑖 |𝐱) Giả sử việc đo đạc các thặng dư là độc lập từng đôi một, khả năng (likelihood) xảy ra kết quả đo 𝐫 khi biết 𝐱 trở thành:
(3.3.2) Áp dụng định lý Bayes, khả năng hậu nghiệm (posteriori) xảy ra 𝐱 khi biết kết quả đo 𝐫 trở thành:
𝑝(𝐫) ∝ 𝑝(𝐫|𝐱) 𝑝(𝐱) (3.3.3) Trong biểu thức trên, 𝑝(𝐱) thể hiện phân phối tiên nghiệm (prior distribution) của
𝐱 Ta cần tìm 𝐱 sao cho tối đa khả năng hậu nghiệm (Maximum a Posteriori – MAP):
Sử dụng logarit tự nhiên, bài toán trên trở thành:
Bỏ qua 𝑝(𝐱), thực hiện giải phương trình đạo hàm bằng 0:
𝒟𝐱 = 0 (3.3.6) trong đó 𝑤 𝑖 = 𝑤(𝑟 𝑖 ) ∈ ℝ được định nghĩa như sau:
Hàm 𝑤(𝑟 𝑖 ) được gọi là hàm trọng số, thể hiện mức độ ảnh hưởng của từng thặng dư trong quá trình tối ưu Giả sử 𝑤(𝑟 𝑖 ) = const thì việc giải phương trình đạo hàm trên tương đương việc giải bài toán bình phương cực tiểu truyền thống Trong thực tế, vì 𝑤(𝑟 𝑖 ) ≠ const nên để bài toán gốc được xấp xỉ thành bình phương cực tiểu có trọng số lặp lại (iteratively reweighted least squares – IRLS) [52]: việc tính trọng số và thặng dư được hoán đổi liên tục tại mỗi vòng lặp đến khi hội tụ:
3.3.2 Thiết kế hàm trọng số Để đảm bảo chất lượng hội tụ cho IRLS, ta cần chọn một mô hình phân phối 𝑝(𝑟 𝑖 ) phù hợp để biểu diễn tốt nhất tương quan độ lớn giữa các thặng dư 𝑟 𝑖 Trong trường hợp lý tưởng, 𝑝(𝑟 𝑖 ) tuân theo phân phối Gauss, dẫn đến 𝑤(𝑟 𝑖 ) = const, tức bài toán trở về dạng bình phương cực tiểu truyền thống Trong thực tế, dữ liệu ngoại lai (outlier) là nguyên nhân chính khiến 𝑝(𝑟 𝑖 ) lệch Gauss khá nhiều Để triệt tiêu ảnh hưởng của các outlier, phân phối t (t-distribution) thường được sử dụng Phân phối t có thể được coi là mở rộng của phân phối Gauss vì ngoài kì vọng 𝜇 và độ lệch chuẩn
𝜎, phân phối t sử dụng thêm bậc tự do 𝜈 ∈ [0; ∞) để tuỳ chỉnh độ dài của “đuôi” phân phối:
(3.3.9) trong đó Γ(∙) là hàm gamma Khi 𝜈 → ∞, phân phối t tiến tới phân phối Gauss truyền thống Giá trị 𝜈 càng nhỏ thì “đuôi” của phân phối càng dài, bao quát càng nhiều các
23 outlier, khiến phân phối trở nên bền vững hơn Mô hình hoá 𝑝(𝑟 𝑖 ) bằng phân phối t, ta thu được dạng hàm trọng số 𝑤(𝑟 𝑖 ) như sau:
Một phương pháp thông dụng để lựa chọn các thông số của phân phối t như sau:
𝜇 = median{𝑟 𝑖 } , 𝜎 = 1.4826 mad{𝑟 𝑖 − 𝜇} , 𝜈 =4 kurtosis{𝑟 𝑖 } − 6 kurtosis{𝑟 𝑖 } − 3 Tuy nhiên, trong nhiều trường hợp, phương pháp trên không cho kết quả tối ưu Để đảm bảo kết quả tốt nhất, ta có thể sử dụng thêm các thuật toán tìm kiếm nhanh như Nelder-Mead [53] để tìm kì vọng 𝜇, độ lệch chuẩn 𝜎 và bậc tự do 𝜈 phù hợp với tập thặng dư hiện tại {𝑟 𝑖 }
Hình 3.2 Khi 𝜈 giảm, “đuôi” của phân phối (trái), thể hiện rõ hơn trên thang log
(phải), trở nên dài hơn, khiến phân phối bền vững hơn với outlier [54]
Một cách khác, đơn giản hơn, để cải thiện chất lượng IRLS đối với tập thặng dư chứa outlier là thiết kế hàm trọng số thông qua các công cụ ước lượng M (M- estimator), hay còn gọi là nhân bền vững (robust kernel) Mục tiêu của nhân bền vững không phải triệt tiêu mà giảm ảnh hưởng của outlier trong quá trình tối ưu Trong một số trường hợp, nhân bền vững tỏ ra hữu hiệu hơn phân phối t trong việc thiết kế hàm trọng số, nhất là khi tập thặng dư nhỏ, được hình thành thông qua một số giải thuật tìm kiếm rời rạc chứ không phải lấy mẫu liên tục nên không tuân theo các mô hình xác suất truyền thống
Nguyên tắc hoạt động của nhân bền vững rất đơn giản: khi thặng dư nhỏ, nhân bền vững cho giá trị như L2; khi thặng dư tăng, giá trị của nhân tăng chậm, thậm chí
24 bão hoà, dẫn đến ảnh hưởng của các outlier tiềm tàng (có thặng dư lớn) bị giảm Tốc độ làm giảm ảnh hưởng của outlier khác nhau sẽ cho ra nhiều loại nhân khác nhau Trong giới hạn luận văn, để thuận tiện, ta sử dụng nhân bền vững tổng quát Barron [55] với hàm đặc trưng như sau:
(3.3.11) trong đó 𝛼 ∈ ℝ là hệ số định dạng để tuỳ chỉnh mức độ bền vững của nhân và 𝑐 > 0 là hệ số co dãn để tuỳ chỉnh hình dạng của nhân quanh điểm 𝑟 = 0 Sở dĩ gọi là nhân tổng quát vì bằng cách tuỳ chỉnh 𝛼, ta suy ra nhiều nhân bền vững quen thuộc:
Hình 3.3 Hàm đặc trưng của nhân tổng quát Barron (trái) và hàm trọng số tương ứng với các giá trị 𝛼 khác nhau [56]
Dựa vào hàm đặc trưng, trọng số tương ứng từng thặng dư 𝑟 𝑖 được tính như sau:
THIẾT KẾ GIẢI THUẬT
Sơ đồ khối toàn hệ thống
Hình 4.1 Sơ đồ khối của hệ thống SLAM
Hệ thống SLAM có nhiệm vụ tiếp nhận luồng dữ liệu thô từ các cảm biến LiDAR và camera, xử lý để ước lượng tư thế 3D của đối tượng theo thời gian và cập nhật bản đồ Để hoàn thành các nhiệm vụ trên, hệ thống thực thi các tác vụ sau: đồng bộ cảm biến, ước lượng tư thế và quản lý bản đồ Dữ liệu thô trả về từ LiDAR là đám mây tia quét Dữ liệu thô từ camera là ảnh bayer dạng GRBG, sau khi được debayer, chuyển sang grayscale và căn chỉnh để loại bỏ biến dạng thì được đưa vào hệ thống
Hệ thống bao gồm các module:
• Sensor Synchronizer: đồng bộ 2 luồng dữ liệu thô từ camera (khung ảnh) và
LiDAR (đám mây) sao cho tại mỗi lần ngắt, dữ liệu từ 2 cảm biến có timestamp sát nhau
• Candidate Selector: lựa chọn tập điểm ảnh chọn lọc có gradient cao
• Image Aligner: ước lượng độ dời tư thế 3D dựa vào xếp chồng giữa 2 khung ảnh liên tiếp
• Cloud Registrator: ước lượng tư thế 3D dựa vào xếp chồng đám mây hiện tại và bản đồ
• Windowed Optimizer: kết hợp tư thế ước lượng từ camera và LiDAR bằng đồ thị nhân tố
• Map Manager: cập nhật, truy xuất và lưu trữ bản đồ đám mây.
Giải bài toán IRLS trong không gian Lie
Nhắc lại, bài toán IRLS được phát biểu dưới dạng:
Sau khi đã xây dựng hàm trọng số, việc tiếp theo là áp dụng giải thuật lặp phù hợp để tìm giá trị tối ưu 𝐱 ∗ Trong giới hạn luận văn, ta áp dụng phương pháp Levenberg–Marquardt (LM) – một biến thể trong nhánh tối ưu bằng suy giảm độ dốc Ý tưởng chính của LM là giới thiệu hệ số tắt dần 𝜆 để kiểm soát tốc độ hội tụ Nếu 𝜆 lớn, hệ phương trình cho nghiệm gần với Gradient Descent, khiến bước nhảy nhỏ nhưng đảm bảo hội tụ Ngược lại, nghiệm gần với Gauss-Newton, bước nhảy dài nhưng dễ bị dao động Bằng cách điều chỉnh hệ số 𝜆, tại mỗi vòng lặp, bước nhảy được điều chỉnh liên tục sao cho giá trị hàm năng lượng luôn luôn giảm
Hàm năng lượng trong bài toán IRLS có dạng:
(4.2.2) Đặt 𝐖 = diag(𝑤 1 , … , 𝑤 𝑛 ) ∈ ℝ 𝑛×𝑛 là ma trận chéo chứa trọng số của các thặng dư, hàm năng lượng có thể viết gọn thành:
Giả sử ta có được một ước lượng ban đầu 𝐱 0 của 𝐱 Nguyên tắc của tối ưu suy giảm độ dốc là tại mỗi vòng lặp, thay vì tìm trực tiếp 𝐱 ∗ , ta tìm độ dời tối ưu 𝛅 ∈ ℝ 𝑚 (𝑚 là số bậc tự do của đa tạp ℳ ∋ 𝐱) trong không gian tiếp tuyến tại 𝐱 0 sao cho hàm năng lượng giảm nhiều nhất Vì 𝐸(𝐱) phụ thuộc phi tuyến vào 𝐱 nên ta phải tuyến tính hoá Taylor bậc nhất:
Tiếp tục khai triển, ta thu được một đa thức bậc hai theo 𝛅:
𝐸(𝐱 0 ⊕ 𝛅) = 𝐫 0 T 𝐖𝐫 0 + 2𝛅 T 𝐉 T 𝐖𝐫 0 + 𝛅 T 𝐉 T 𝐖𝐉𝛅 (4.2.6) Thực hiện giải phương trình đạo hàm bằng 0, ta thu được:
𝜕𝛅 = 2𝐛 T + 2𝛅 T 𝐇 = 𝟎 → 𝐇𝛅 = −𝐛 (4.2.7) trong đó 𝐇 ∈ ℝ 𝑛×𝑛 là ma trận hessian và 𝐛 ∈ ℝ 𝑛 là bias vector của bài toán:
𝐇 = 𝐉 T 𝐖𝐉, 𝐛 = 𝐉 T 𝐖𝐫 0 (4.2.8) Phương pháp LM trong không gian Lie được tóm tắt qua bảng sau:
Bảng 4.1 Các bước thực hiện giải thuật Levenberg–Marquardt. Đầu vào
Hệ số tắt dần 𝜆 Lượng tắt dần 𝜆 inc , 𝜆 dec Đầu ra
13 𝐱 ∗ = 𝐱 0 ; Chú ý tại bước 5, công thức chỉ mang tính tượng trưng: việc giải hệ phương trình trong thực tế phải dựa trên các giải thuật phân tích Cholesky, QR, SVD chứ không
28 phải dựa trên nghịch đảo ma trận Khi lập trình, để giảm khối lượng tính toán, ta có thể tính ma trận hessian và bias vector theo nguyên tắc cộng dồn:
Xây dựng module Candidate Selector
Đầu tiên, ta cần biểu diễn toạ độ các điểm đám mây hiện tại trong hệ quy chiếu gắn với camera Sau đó, ta lựa chọn các điểm ảnh theo nguyên tắc: phân đồ đều trong toạ độ cầu và gradient cao Để đảm bảo chất lượng định vị và giảm thời gian tính toán, ta ưu tiên chọn những điểm ảnh trong vùng gradient cao như góc, cạnh Ta cũng chọn một vài điểm ảnh có gradient yếu hơn bằng cách giảm độ phân giải của ô lưới và thực hiện các bước nêu trên Cuối cùng, những điểm ảnh nào có kèm độ sâu sẽ được lựa chọn làm điểm ảnh chọn lọc
Hình 4.2 Tập điểm ảnh trước (trái) và sau (phải) chọn lọc [29].
Xây dựng module Image Aligner
Một trong những mô hình camera phổ biến nhất là mô hình pinhole Theo mô hình, chùm tia sáng từ vật thể đi qua một lỗ nhỏ trước camera và đập vào mặt phẳng ảnh ở sau, tạo thành ảnh của vật Vì ảnh bị ngược nên ta có thể xem có tồn tại một mặt phẳng ảnh ảo ở trước camera Trong giới hạn luận văn, hình pinhole được lựa vì ta làm việc với camera có góc nhìn nhỏ (