SCIENCE - TECHNOLOGY Số 13.2023 ● Tập san SINH VIÊN NGHIÊN CỨU KHOA HỌC 171
NGHIÊN CỨU PHƯƠNG PHÁP THIẾT LẬP BẢN ĐỒ 3D TRONG MÔI TRƯỜNG ĐA CẤU TRÚC CHO MOBILE ROBOT ỨNG DỤNG CÔNG NGHỆ POINTCLOUD
RESEARCH ON THE METHOD OF ESTABLISHING 3D MAPS IN A MULTI-STRUCTURED ENVIRONMENT FOR MOBILE ROBOTS APPLYING POINTCLOUD TECHNOLOGY Đặng Đức Tú1,*, Nguyễn Tiến Phương1, Nguyễn Anh Tú2 TÓM TẮT Xây dựng bản đồ 3D bằng công nghệ Point Cloud trong robot di động là mộ
t
công nghệ phổ biến trong lĩnh vực robot di động. Mục đích của việc xây dựng bả
n
đồ 3D nhằm đưa ra một môi trường giúp robot định vị trong không gian 3 chiề
u
chính xác hơn so với bản đồ 2D thông thường. Bài báo trình bày các bước xây dự
ng
bản đồ 3D sử dụng phương pháp LeGO-LOAM trong môi trường đa cấu trúc, đồ
ng
thời đưa ra thử nghiệm trên các môi trường mô phỏng và môi trường thực dự
a trên
robot dạng visai kết hợp với cảm biến Lidar 3D. Các kết quả ở cuố
i bài báo đưa ra
đánh giá về độ chính xác của bản đồ 3D và tính ứng dụng thực tế củ
a phương pháp
LeGO-LOAM. Từ khóa: Xây dựng bản đồ 3D, Công nghệ PointCloud, Robot di độ
LeGO-LOAM. ABSTRACT Building 3D maps
using Point Cloud technology in mobile robots is a popular
technology in the field of mobile robots. The purpose of building 3D maps is to
provide an environment that helps the robot accurately navigate in 3D space
compared to conventional 2D maps. This pa
per presents the steps to build 3D
maps using the LeGO-LOAM method in a multi-
structure environment, and it also
conducts tests in both simulated and real environments using a visai robot
combined with a 3D Lidar sensor. The results at the end of the paper
provide an
assessment of the accuracy of the 3D map and the practical applicability of the
LeGO-LOAM method. Keywords:
Building 3D maps, PointCloud technology, Mobile robot, 3D Lidar,
LeGO-LOAM. 1Lớp Cơ điện tử 02 - K14, Trường Cơ khí - Ô tô, Trường Đại học Công nghiệp Hà Nội2Trường Cơ khí - Ô tô, Trường Đại học Công nghiệp Hà Nội *Email: ductu842001@gmail.com 1. GIỚI THIỆU Ngày nay, trong cuộc cách mạng 4.0, robot di động là một trong các lĩnh vực phát triển. Trong đó, định vị và xây dựng bản đồ cho robot di động là một công nghệ được sử dụng rất nhiều trong các ứng dụng điều hướng robot. Trong khi bản đồ 2D được được phát triển từ năm 1991 [1], trong khi đó đến năm 2004 bản đồ 3D lần đầu tiên được phát triển bởi [2] và đã đánh dấu một bước khởi đầu cho công nghệ bản đồ 3D trong robot di động. Lập bản đồ là một trong những công nghệ then chốt cho tự hành robot, vì nó là nền tảng của việc lập kế hoạch đường đi và điều khiển robot. Robot có xu hướng di chuyển an toàn và hiệu quả nếu bản đồ chính xác hơn và có thể đối phó với môi trường năng động. Bản đồ 2D được tạo bởi các nguồn mở như gmapping, hectorslam được sử dụng rộng rãi cho nhiều loại robot dịch vụ, vì phương pháp lập bản đồ 3D thường cần nhiều thời gian xử lý hơn để lưu và cập nhật thông tin không gian. Lập kế hoạch chuyển động trong bản đồ 3D cũng cần nhiều thời gian xử lý hơn và các thuật toán thường phức tạp nên chúng không được các nhà phát triển robot dịch vụ nói chung sử dụng rộng rãi. Tuy nhiên, các phương pháp lập bản đồ 2D thường không thể phản ánh đầy đủ môi trường. Hình dạng của các đối tượng và các khu vực bị che khuất có thể bị bỏ qua cùng với biên dạng 3D của vật thể trong môi trường không được làm rõ, điều này có thể gây nguy hiểm hoặc bỏ qua quỹ đạo di chuyển tốt nhất để lập kế hoạch đường đi cho robot [3]. Từ đó, bản đồ 3D là một giải pháp để hạn chế sự thiếu hụt thông tin môi trường xung quanh robot. Khi các hệ thống phần cứng ngày càng được nâng cấp và hiện đại, vấn đề bộ nhớ và thời gian xử lý được nâng cao đáng kể, vấn đề về thời gian thực của lập bản đồ số 3D không còn là một rào cản quá lớn. Do đó các phương pháp lập bản đồ 3D đang được cộng đồng nghiên cứu quan tâm và phát triển cùng với các phương pháp lập bản đồ 2D truyền thống. Trong [4, 5], các giải pháp về 3D SLAM và bản đồ 3D đã được nghiên cứu và ứng dụng trong robot di động. Trên cơ sở về những ưu điểm mà bản đồ 3D đem lại trong lĩnh vực robot di động, trong phạm vi bài báo này, nhóm nghiên cứu tiến hành làm rõ lý thuyết cơ bản và trình bày các kết quả xây dựng bản đồ 3D sử dụng phương pháp LeGO-LOAM [6].
CÔNG NGHỆ Tập san SINH VIÊN NGHIÊN CỨU KHOA HỌC ● Số 13.2023
172
KHOA H
ỌC
2. CƠ SỞ LÝ THUYẾT PHƯƠNG PHÁP XÂY DỰNG BẢN ĐỒ 3D Phương pháp xây dựng bản đồ 3D có quy trình như hình 1. Hình 1. Sơ đồ quy trình xây dựng bản đồ 3D Để làm rõ phương pháp, nhóm nghiên cứu tiếp tục trình bày làm rõ từng bước trong sơ đồ trên. 2.1. Thu thập dữ liệu đám mây điểm Để có thể thu được dữ liệu đám mây điểm, phương pháp đo khoảng cách đến mục tiêu bằng cách chiếu sáng mục tiêu đó bằng một tia laze xung quanh và đo các xung phản xạ thông qua cảm biến được sử dụng (Light detection and range). Thông thường các loại cảm biến như Sick, Lidar sẽ cung cấp dữ liệu đám mây điểm được sử dụng phương pháp trên. Để thuận tiện trong việc xây dựng bản đồ 3D, Lidar 3D là cảm biến được nhóm lựa chọn. Cùng với đó hệ điều hành ROS cho phép hỗ trợ người dùng có thể khai thác dữ liệu từ cảm biến thông qua gói hỗ trợ velodyne_driver. Dạng dữ liệu được trả về của cảm biến Lidar được mã hóa dưới dạng 8 bit. Thông thường để khai thác được dạng dữ liệu này, thư viện PCL được sử dụng và biến đổi chúng về dạng thông tin tọa độ x, y, z của một điểm. 2.2. Phân đoạn đám mây điểm (Segmentation) Từ bước phân đoạn đám mây điểm đến xây dựng bản đồ 3D là một loạt các bước thuộc phương pháp LeGO-LOAM. Đây là một phương pháp sử dụng để xác định vị trí và tạo bản đồ trong thời gian thực bằng cách sử dụng dữ liệu đám mây điểm từ cảm biến Lidar. Trong bước phân đoạn đám mây điểm, thư viện PCL được sử dụng nhằm đưa thông tin dạng mã hóa 8 bit thu được về dạng thông tin tọa độ các điểm. Các thông tin tọa độ được biến đổi sang dạng ảnh 3 chiều với kích thước 1800x16, thông tin tọa độ z là độ sâu của mỗi điểm ảnh. Mục đích của việc đưa về dạng ảnh 3 chiều làm cho các dữ liệu được sắp xếp theo thứ tự trong khung tọa độ. Hình 2. Quá trình phân đoạn đám mây điểm Tiếp theo đối với 2 điểm liền kế nhau được xét bởi góc tạo bởi giữa 2 điểm đó với phương ngang, nếu góc tạo bởi 2 điểm nhỏ hơn
10
thì được xét là điểm thuộc mặt đất (hình 2b). Với x, y, z là tọa độ của điểm và i là thứ tự của điểm, công thức được biểu diễn:
i1i22i1ii1i
zzαarctanxxyy
(1) Các điểm mặt đất được bỏ qua ở phần tính toán tiếp theo. Đây là mục đích chính của phân đoạn mặt đất giúp tối ưu hóa thời gian tính toán của phương pháp. Sau khi loại bỏ các điểm mặt đất, các điểm còn lại không thuộc mặt đất tiếp tục được phân cụm. Hai điểm liên tiếp được xét, nếu góc β ≤ 600 thì 2 điểm đó cùng một đối tượng (hình 2c). Mỗi một đối tượng bao gồm ít nhất 30 điểm, nếu ít hơn 30 điểm sẽ bị loại bỏ. 2.3. Trích xuất tính năng đám mây điểm (Feature Extraction) Sau khi được phân cụm, đám mây điểm tiếp tục được phân tích và trích xuất các điểm thuộc mặt phẳng và cạnh của mặt phẳng hay còn gọi là điểm phẳng và điểm cạnh, nhằm phân loại và trích xuất các điểm đặc biệt từ đó đưa ra trạng thái của robot trong từng thời điểm quét. Cơ sở để phân loại các điểm phẳng và điểm cạnh là thực hiện phân tích xem độ nhám của chúng so với các điểm xung quanh thông qua công thức tính sau:
ji
jS,jii1
crr
S.r
(2) Trong đó, S là tập hợp 10 điểm được lấy lần lượt trong tập điểm quét được tại một lần quét của lidar, ri là điểm có vị trí ở giữa trong tập điểm S, rj lần lượt là các điểm trong tập S. Hình 3. Trích xuất điểm phẳng và điểm cạnh Mục đích của việc trích xuất các đặc trưng của từng điểm trong một cụm nhằm phục vụ cho việc tính toán và ước tính vị trí và hướng trong các lần quét khác nhau của cảm biến.
SCIENCE - TECHNOLOGY Số 13.2023 ● Tập san SINH VIÊN NGHIÊN CỨU KHOA HỌC 173
Phân đoạn và trích xuất tính năng đám mây điểm là hai bước tiền xử lý dữ liệu của LeGO-LOAM. 2.4. Ước tính vị trí và hướng (Lidar Odometry) Ước tính vị trí và hướng của robot và đám mây điểm là bước xử lý dữ liệu chính trong LeGO-LOAM bao gồm so khớp nhãn 2 đám mây điểm (Label Matching) và tối ưu hóa so khớp nhãn (Label Matching Optimization). So khớp nhãn 2 đám mây điểm (Label Matching) Bước đầu tiên của Label Matching là phân cụm các điểm đặc trưng cạnh và phẳng từ mỗi khung LiDAR sử dụng một thuật toán phân cụm dựa trên dịch chuyển Euclid (Euclidean Cluster Extraction). Việc so khớp Label Matching sử dụng một phương pháp phổ biến là điểm gần nhất lặp lại ICP, cụ thể phương pháp này tính toán để tìm các điểm có khoảng cách gần nhau từ đó tìm ra đặc trưng giữa hai khớp quét k, k + 1 liên tiếp. Phương pháp ICP sử dụng thuật toán KD-tree sử dụng khoảng cách tìm điểm gần nhất, thuật toán này được thư viện PCL hỗ trợ. Đồng thời, thuật toán sử dụng khoảng cách giữa các điểm để đưa ra quyết định các điểm có tọa độ thỏa mãn khoảng cách nhỏ dần theo các bước, cuối cùng thu được điểm hàng xóm gần nhất. Đối với 2 điểm bất kì A, B trong không gian, khoảng cách của 2 điểm được tính bằng công thức:
222
ABABABAB
dxxyyzz
(3) Mục đích của sử dụng KD-tree trong so khớp nhãn để so sánh và phân loại đối tượng dựa trên đặc trưng của các điểm. Các điểm gần nhau nhất trong 2 lần quét liên tiếp được quy định là có cùng đặc trưng. Ngoài ra, sử dụng KD-Tree còn giúp giảm thiểu độ phức tạp tính toán và cải thiện độ chính xác của phương pháp. Tối ưu hóa so khớp nhãn đám mây điểm Để xác định chính xác khoảng cách cần biến đổi của một điểm khớp với vị trí của nó trong lần quét trước. Giả sử P, P0, P1, P2 là các điểm thứ i, j, l, m thuộc tập điểm
LLLL
k1,ik,jk,lk,m
X,X,X,X
với k, k + 1 là 2 lần quét laser liên tiếp. Công thức khoảng cách từ điểm phẳng đến mặt phẳng và điểm cạnh đến đường thẳng cạnh chứa điểm đó tại lần quét trước được sử dụng:
LLLLk1,ik,jk1,ik,lεLLk,jk,lXXXXdXX
(4)
LLk1,ik,jLLLLk,jk,lk1,ik,mHLLLLk,jk,lk,jk,m XXXXXXdXXXX
(5) Khi đó tổng tích lũy trạng thái biến đổi khoảng cách được tính:
εHNNL
k1,i
εiHii1i1lossddDX
(6) Mặt khác, phương trình biến đổi quỹ đạo của một điểm trong không gian 3 chiều được biểu diễn như sau:
LLLk1,i
k1,ik1,i
XRXT
(7) Từ công thức (6), (7) tổng tích lũy trạng thái biến đổi khoảng cách được tính:
LLLk1,ik1,ik1lossDXDGX,T
(8) Trong đó
T
Lk1xxxxyzTt, t, t,
θ, θ, θ
, với
L
k1
T
là thông tin thay đổi tọa độ một điểm trong lần quét k + 1. Mặt khác, biểu diễn tổng tích lũy trạng thái biến đổi bằng hàm phi tuyến, khi đó ta có:
Lk1
fTd
(9) Việc chuyển đổi hệ tọa độ tại lần quét k + 1 khớp với hệ tọa độ tại lần quét k được thực hiện thông qua phương pháp LM bằng cách tối ưu hàm phi tuyến:
1
LLTTT
k1k1
TTJJ
λdiagJJJd
(10) Với J là ma trận Jacobian được tính bằng cách đạo hàm riêng hàm phi tuyến khoảng cách theo thông tin của từng phần tử trong nó và được tính như sau:
L
k1
Jf/T
. Để tối ưu hóa và đưa ra trị hàm lỗi (9) về với giá trị nhỏ nhất, phương pháp LM (Levenberg-Marquardt) được sử dụng, mục tiêu của phương pháp được biểu diễn qua công thức:
m
2
ββiii1
βargminSβargminyf(x,β)
(11) Quá trình khớp khung tọa độ trong một lần xử lý kết thúc khi đạt số lần lặp tối đa hoặc d = 0 trong công thức tính số 10. Mục tiêu của so khớp nhằm đưa khung tọa độ của 2 lần quét liên tiếp khớp nhau, từ đó đưa ra vị trí và hướng của robot trong không gian. 2.5. Xây dựng bản đồ (Lidar Mapping) Lidar Mapping sử dụng dữ liệu mà Lidar Odometry đã biến đổi để đưa ra bản đồ toàn cục. Sự khác biệt giữa LeGO-LOAM và LOAM đó là LeGO LOAM lưu tập dữ liệu đám mây điểm đặc trưng của từng khung hình, trong khi LOAM lưu toàn bộ bản đồ đám mây điểm. Tập dữ liệu trong Lidar Mapping của LeGO LOAM được biểu diễn thông qua công thức:
t11122t1t1epepepMF,F,F,F,...,F,F
(12) Trong đó Fe, Fp là tập đám mây điểm cạnh và điểm phẳng đã được trích xuất và thực hiện so khớp ở các lần quét. Ngoài ra, trong Lidar Mapping còn sử dụng một kỹ thuật khác nhằm tăng độ chính xác giữa các khung hình đám mây điểm là đóng vòng lặp (Loop Closure). Đóng vòng lặp là việc khép lại một chu trình so khớp đám mây điểm hiện tại với các đám mây điểm quét ở lần quét tiếp theo, qua đó cố định ràng
CÔNG NGHỆ Tập san SINH VIÊN NGHIÊN CỨU KHOA HỌC ● Số 13.2023
174
KHOA H
ỌC
buộc được đám mây điểm đã qua xử lý trong hệ tọa độ toàn cục. Bản đồ được cập nhật trong tần số 2Hz. 3. KẾT QUẢ MÔ PHỎNG, THỰC NGHIỆM VÀ ĐÁNH GIÁ Để đưa ra đánh giá chính xác và trực quan hơn, nhóm nghiên cứu đã đưa ra phương án so sánh kết quả phương pháp xây dựng bản đồ 3D sử dụng LeGO-LOAM với một phương pháp khác là LOAM cùng với dữ liệu từ IMU, Encoder và Map2D. Các thông số robot và cảm biến Lidar được nhóm sử dụng trong cả hai môi trường mô phỏng và thực nghiệm: Kích thước robot: 550x400x250mm Vận tốc dài: 0,3m/s Vận tốc góc: 0,4rad/s Số kênh laser của cảm biến: 16 Phạm vi quét của cảm biến: 3600. 3.1. Kết quả mô phỏng Nhóm nghiên cứu thực hiện xây dựng môi trường mô phỏng và điều khiển chuyển động robot theo quỹ đạo được đánh dấu bởi các điểm màu đỏ có tọa độ lần lượt (0,0); (20,0); (20,10); (0,10) (hình 4). Hình 4. Môi trường mô phỏng Kết quả bản đồ 3D quét được trong môi trường mô phỏng như hình 5, 6. Hình 5. Bản đồ 3D được xây dựng bằng LeGO-LOAM trên mô phỏng Hình 6. Bản đồ 3D được xây dựng bằng LOAM trên mô phỏng Kết quả cho thấy bản đồ được xây dựng bằng phương pháp LeGO-LOAM tốt hơn so với phương pháp LOAM. Để phân tích rõ hơn, nhóm trình bày trong phần đánh giá các kết quả thu được. 3.2. Kết quả thực nghiệm Với mục đích ban đầu so sánh kết quả giữa các phương pháp và giữa môi trường mô phỏng và thực nghiệm, nhóm nghiên cứu tiếp tục tiến hành thử nghiệm trên môi trường thực. Cụ thể, tầng 2 tòa A10 trường Đại học Công nghiệp Hà Nội được sử dụng làm môi trường thực nghiệm (hình 7). Hình 7. Môi trường thực nghiệm tại tầng 2 tòa A10 Trong trường hợp này, robot di chuyển thông qua phương pháp điều khiển bám quỹ đạo Pure Pursuit với các tọa độ đặt lần lượt là (0,0); (25,0); (25,12), (0,12). Kết quả bản đồ 3D được xây dựng trong môi trường thực nghiệm như hình 8. Hình 8. Một số khu vực được xây dựng trong bản đồ 3D
SCIENCE - TECHNOLOGY Số 13.2023 ● Tập san SINH VIÊN NGHIÊN CỨU KHOA HỌC 175
Trong phần tiếp theo, dựa vào các kết quả trả về, nhóm nghiên cứu đưa ra đánh giá về từng phương pháp xây dựng bản đồ. 3.3. Đánh giá Để đánh giá các kết quả thu được, nhóm nghiên cứu đưa ra hai tiêu chí để đánh giá độ chính xác và mức độ hiệu quả của LeGO-LOAM là: quỹ đạo của robot và thời gian xử lý. Đánh giá sai lệch quỹ đạo Trong quá trình robot di chuyển, thông tin vị trí của robot được sử dụng đưa ra quỹ đạo của robot, tần số lấy mẫu vị trí của robot là 20Hz. Đối với quỹ đạo ở môi trường thực, quỹ đạo từ phương pháp LeGO-LOAM được so sánh với dữ liệu ước tính quỹ đạo từ IMU, Encoder và Map2D được. Nhóm nghiên cứu tách thành 2 biểu đồ so sánh với quỹ đạo đặt do 2 quỹ đạo này tương đối trùng khớp nhau. Hình 9. Quỹ đạo của robot trong môi trường thực sử dụng phương pháp LeGO-LOAM Hình 10. Quỹ đạo của robot trong môi trường thực từ dữ liệu IMU, Encoder và Map 2D Từ các kết quả thông tin vị trí thu được trong quá trình thực nghiệm và mô phỏng, sai số trung bình được tính thông qua công thức Root Mean Square (RMS): n
2
xrefmethodk1
1
error(xx)
n
(13) n
2
yrefmethodk1
1
error(yy)
n
(14) Từ công thức (13), (14) các sai số trung bình được tính như bảng 1. Bảng 1. Sai số trung bình quỹ đạo theo phương x, y Sai số (m) Mô phỏng Thực nghiệm LeGO-LOAM LOAM LeGO-LOAM LOAM IMU+ Encoder+ Map 2D Trục x 0,021 5,292 0,092 - 0,072 Trục y 0,319 1,014 0,081 - 0,085 Sai số khi sử dụng phương pháp LeGO-LOAM là tương đối nhỏ và ổn định ở khoảng 0,09m, tại một số vị trí sai số tăng lên khoảng 0,11m. Điều này do tại các vị trí robot thực hiện xoay tại mỗi điểm mốc gây ra sai số. Ngoài ra, tại vị trí khoảng 5m cuối của quãng đường đi, ước tính sai lệch tăng lên khoảng 0,16m. Đánh giá hiệu suất thời gian Để đánh giá hiệu suất phương pháp LeGO-LOAM, nhóm nghiên cứu tiến hành so sánh phương pháp này với phương pháp LOAM. Đối với cả hai phương pháp đều có các giai đoạn thời gian trung như: trích xuất tính năng đám mây điểm, ước tính vị trí, xây dựng bản đồ. Hình 11, 12 là biểu đồ thời gian xử lý của từng phương pháp ở cả môi trường thực và mô phỏng được nhóm trích xuất: Hình 11. Thời gian xử lý của các module trong môi trường thực Hình 12. Thời gian xử lý của các module trong môi trường mô phỏng Bảng thời gian trung bình xử lý của từng phương pháp như thể hiện trong bảng 2.