CƠ SỞ LÝ THUYẾT
Bộ lọc complementary
Những phương pháp định hướng trong không gian thường được sử dụng là Euler, Quaternion và ma trận Cos (DCM) Trong đó Quaternion là cách biểu diễn công thức tuyến tính của động lực định hướng Bất kỳ 1 hướng ngẫu nhiên trong không gian 3D của khung tọa độ A so với khung tọa độ B có thể được biểu diễn bằng 1 quaternion B A q [10]
Trong đó là góc quay, e là vector đơn vị biểu diễn các trục quay
Quaternion liên hợp của A B q hoặc quaternion nghịch đảo sẽ biểu diễn phép quay nghịch đảo và được định nghĩa như sau:
Quaternion sau một loạt các phép quay có thể dễ dàng tính toán được bằng phép nhân quaternion Ví dụ chúng ta có 3 khung tọa độ A,B và C với quaternion A B q là hướng của khung A so với khung B và B C q là hướng của khung B so với khung C, khi đó hướng của khung A so với khung C sẽ được tính toán như sau:
Trong đó là phép nhân quaternion và được cho bởi công thức như sau:
Khi đó các quaternion đơn vị có thể được áp dụng để thực hiện các phép quay của các vector trong không gian 3D
Ví dụ: cho vector v A nằm trong hệ trục tọa độ A có thể được biểu diễn trong hệ trục tọa độ B như sau:
Công thức trên có thể được biểu diễn dưới dạng ma trận như sau:
Trong đó R ( ) A B q là direct cosine matrix 2 (DCM) của quaternion định hướng A B q
Phép quay nghịch đảo được định nghĩa như sau
Các góc Euler sẽ được tính từ quaternion như sau:
2.1.2 Bộ lọc complementary Ước lượng vị trí và hướng của robot trong không gian là 1 nhiệm vụ khó khăn do tín hiệu trả về từ cảm biến IMU thường bị ảnh hưởng nhiều bởi nhiễu và sai số cộng dồn theo thời gian Do đó cần phải xử lý những tín hiệu thô này để có thể đạt
2direct cosine matrix là một ma trận chuyển đổi biến đổi một hệ quy chiếu tọa độ này sang một hệ quy chiếu khác
7 được hiệu quả ước lượng hướng ổn định và không có sai lệch với các tính toán không quá phức tạp
Bộ lọc complementary phân tích các tín hiệu trong miền tần số và kết hợp chúng nhằm thu được tín hiệu ước lượng tốt hơn về một trạng thái cụ thể Nếu tín hiệu bị ảnh hưởng bởi nhiễu có tần số khác nhau, có thể áp dụng nhiều bộ lọc với băng thông thích hợp sao cho tổng của hai tín hiệu sau khi được lọc có thể bao phủ toàn bộ dải tần số quan tâm
Do tín hiệu con quay hồi chuyển bị ảnh hưởng bởi nhiễu tần số thấp và tín hiệu gia tốc kế bị ảnh hưởng bởi các nhiễu tần số cao, để ước lượng trạng thái, hướng di chuyển đọc được từ IMU, bộ lọc complementary kết hợp thực hiện lọc thông cao để lọc nhiễu tần số thấp và bộ lọc thông thấp để lọc nhiễu tần số cao Nó ước lượng trạng thái ở dạng quaternion từ dữ liệu con quay hồi chuyển đồng thời sử dụng dữ liệu gia tốc kế ở dạng delta quaternion để hiệu chỉnh cho các thành phần roll và pitch của trạng thái trong khi góc yaw được ước lượng từ dữ liệu của con quay hồi chuyển
Hình 2 1 Sơ đồ khối của bộ lọc complementary cho IMU [10]
• L là khung tọa độ cục bộ (local) thường là cảm biến và G là khung tọa độ toàn cục (global) chính là trái đất
• L t là vận tốc góc quay quanh 3 trục tọa độ của cảm biến đo từ cảm biến con quay hồi chuyển tại thời điểm t
• L a t là gia tốc của 3 trục tọa độ tại thời điểm t
Trong bước dự đoán, dữ liệu từ vector vận tốc góc được đo bằng con quay hồi chuyển được sử dụng để tính toán ước lượng đầu tiên của hướng ở dạng quaternion Giả sử các điều kiện ban đầu đã biết, quaternion mô tả tốc độ thay đổi hướng dưới dạng đạo hàm quaternion được tính bằng cách nhân trạng thái trước đó với vector vận tốc góc Đạo hàm quaternion thuận tại thời điểm t k được định nghĩa như sau:
q t là vector vận tốc góc được sắp xếp dưới dạng một quaternion thuần tại thời điểm t k và
L q t − là ước lượng trước đó của hướng Có thể viết lại theo cách khác:
Trong đó, thành phần L đại diện cho cross-product matrix, ma trận này được liên kết với L và được tính như sau:
Hướng của khung toàn cục so với khung cục bộ tại thời điểm t có thể tính thông qua tích phân đạo hàm của quaternion với chu kỳ lấy mẫu = −t t k t k − 1
9 Ở bước hiệu chỉnh, các thành phần góc roll và góc pitch trong quaternion dự đoán được hiệu chỉnh bằng cách áp dụng quaternion delta ˆq acc từ dữ liệu của gia tốc kế Nghịch đảo của quaternion dự đoán G L q để xoay khung tọa độ của vector trọng trường đã được chuẩn hóa L (được đo bằng gia tốc kế) sang khung tạo độ toàn cục như dưới đây
Trong đó, G g p là vector “trọng trường dự đoán” Vector đoán thường sẽ có 1 sai số nhỏ so với vector trọng trường thực tế G g Delta quaternion q acc được sử dụng để xoay G g thành G g p
Do tín hiệu từ gia tốc kế bị ảnh hưởng bởi nhiễu tần số cao nên trước khi đưa dữ liệu của delta quaternion vào quaternion dự đoán, cần phải loại bỏ nhiễu bằng cách sử dụng phép nội suy với quaternion đơn vịq I
Giả sử có 2 quaternions p và q, Cos góc tạo giữa điểm đầu và điểm cuối của cung tròn giữa chúng bằng tích vô hướng giữa chúng:
Có 2 phép nội suy dựa vào góc lệch của q I vàq acc là LERP và SLERP
Hình 2 2 Biểu diễn 2-D của hai phương pháp nội suy khác nhau giữa quaternion
Với q 0acc là giá trị tích vô hướng của các thành phần củaq acc là 1 ngưỡng của giá trị q 0acc
Nếu q 0acc thì phương pháp nội suy tuyến tính đường thẳng (LERP) được sử dụng:
Nếu q 0acc thì phương pháp nội suy tuyến tính cầu (SLERP) được sử dụng:
sin (1 ) sin( ) ˆ acc sin I sin( ) acc q − q q
Cuối cùng, quaternion ước tính từ dữ liệu của con quay hồi chuyển được nhân với quaternion delta đã được lọc Quaternion dự đoán G L q được hiệu chỉnh bởi delta quaternions như sau: ˆ ˆ
Bộ lọc Kalman mở rộng (EKF)
Bộ lọc Kalman mở rộng được sử dụng rộng rãi trong nhiều ứng dụng khác nhau, đặc biệt là trong lĩnh vực kết hợp dữ liệu cho robot di động
Vector trạng thái của hệ thống có dạng x n [11]:
• x z k , k là vector trạng thái và vector đo lường của hệ thống
• w v k , k là các biến ngẫu nhiên, lần lượt đại diện cho nhiễu hệ thống và nhiễu đo lường Chúng được giả định là nhiễu trắng, độc lập với nhau và có phân
• Q là ma trận hiệp phương sai nhiễu quá trình và R là ma trận hiệp phương sai nhiễu đo lường
• f là hàm phi tuyến thể hiện mối liên hệ giữa trạng thái hiện tại với trạng thái trước đó
• h là hàm phi tuyến thể hiện mối liên hệ giữa trạng thái x k với phép đo z k
Trong thực tế, giá trị riêng của nhiễu tại mỗi bước là không thể xác định Do đó, vector trạng thái và vector đo lường được ước lượng mà không có thành phần này:
Các phương trình phi tuyến trên có thể được biểu diễn bằng một phương trình tuyến tính có dạng:
• x z k , k là vector trạng thái và vector đo lường
• x z k , k là xấp xỉ vector trạng thái và vector đo lường
• xˆ k là trạng thái ước lượng từ bước trước đó
• A là ma trận Jacobian của các đạo hàm riêng đối với trạng thái x
• W là ma trận Jacobian của các đạo hàm riêng đối với w
• H là ma trận Jacobian của các đạo hàm riêng đối với x
• V là ma trận Jacobian của các đạo hàm riêng đối với v,
Trong đó , là các thành phần nhiễu ngẫu nhiên có giá trị trung bình bằng 0 và ma trận hiệp phương sai lần lượt được thể hiện: WQW T và VRV T
Khi đó trạng thái ước lượng hậu nghiệm của phương trình phi tuyến ban đầu được ước lượng: ˆ k k ˆ k x = x +e (2.28)
Thay ngược vào trên, ta có: trạng thái ước lượng được cập nhật như sau:
Quá trình thực hiện của bộ lọc Kalman mở rộng thường bao gồm giai đoạn:
Giai đoạn dự đoán: tại mỗi thời điểm k, bộ lọc nhận vector trạng thái xˆ k − − 1 và ma trận hiệp phương sai của hệ thống P k − 1 ở bước trước đó để ước tính ma trận hiệp phương sai và vector trạng thái tiên nghiệm ở bước hiện tại
Giai đoạn hiệu chỉnh: dữ liệu từ phép đoz k được bộ lọc dùng để hiệu chỉnh ước lượng trạng thái và ma trận hiệp phương sai tại bước hiện tại
Hình 2 3 Sơ đồ mô phỏng bộ lọc EKF [11]
GNSS-RTK
2.3.1 Hệ thống định vị vệ tinh toàn cầu -Global Navigation Satellite System GNSS
Hệ thống định vị vệ tinh toàn cầu -Global Navigation Satellite System thường được viết tắt GNSS là tên gọi chung những hệ thống định vị sử dụng tín hiệu từ các vệ tinh khác nhau truyền về 1 thiết bị nhận tín hiệu, tính toán khoảng cách với các vệ tinh từ đó suy ra vị trí trên trái đất của thiết bị đó Ban đầu của những hệ thống này được sử dụng cho mục đích quân sự của 1 số nước lớn như Mỹ,
Nga , tuy nhiên ngày nay, cùng với sự phát triển khoa học kỹ thuật thì ngày càng nhiều lĩnh vực như nông nghiệp, công nghiệp, giao thông… sử dụng công nghệ này để xác định vị trí hiện tại của thiết bị
Các phép đo tín hiệu GNSS bị ảnh hưởng bởi một số loại sai số (ngẫu nhiên) Các sai số này có thể được chia thành ba loại dựa trên vị trí mà chúng xảy ra: sai số do máy thu, sai số dựa trên vệ tinh và sai số do môi trường truyền dẫn [12]
Các nguyên nhân gây ra sai số từ máy thu GNSS là:
• Nhiễu đo của máy thu bao gồm các nguồn sau: Nhiễu nhiệt từ bộ khuếch đại, ăng-ten và các phần cứng khác của máy thu gây ra, tất cả đều góp phần vào nhiễu ngẫu nhiên Ngoài ra, việc kết hợp tín hiệu của nhiều hệ thống vệ tinh khác nhau như GPS/GLONASS hoặc GPS/BDS có thể cải thiện độ chính xác hơn là chỉ sử dụng tín hiệu từ GPS [13]
• Sai lệch đồng hồ máy thu là một tham số bổ sung trong thuật toán ước tính vị trí Thông thường, yêu cầu ít nhất bốn vệ tinh kết nối để xác định vị trí máy thu GNSS trong không gian ba chiều cũng như sai lệch đồng hồ máy thu, tuy nhiên vẫn không thể triệt tiêu hoàn toàn sai số dư
• Sai số do nhiễu xạ của tín hiệu vệ tinh: Sai số này do phản xạ từ các đối tượng trong vùng lân cận của (các) ăng ten thu khiến tín hiệu đến máy thu từ nhiều đường Điều này có thể xảy ra do sự hiện diện của các tòa nhà hoặc các vật cản khác
Hình 2 4 Hiện tượng Multipath signals [14]
Các nguyên nhân gây ra sai số từ vệ tinh bao gồm:
• Sai số do đồng hồ vệ tinh : Đồng hồ vệ tinh GNSS chạy với tốc độ khác với đồng hồ máy thu do có sự khác biệt về thế năng hấp dẫn Để bù đắp cho các sai số tương đối quy định, tần số đồng hồ vệ tinh được điều chỉnh thành
• Sai số do hiện tượng Ephemeric là những sai số bắt nguồn từ sự sai lệch vị trí của các vệ tinh GNSS đang được máy thu theo dõi Vị trí không chính xác là do chuyển động của vệ tinh trên quỹ đạo bị thay đổi do có nhiều tác động nhiễu như: Tính không đồng nhất của trọng trường trái đất, ảnh hưởng của sức hút mặt trăng, mặt trời và các thiên thể khác Các thông số mới để lập mô hình vị trí chính xác được tải lên vệ tinh GNSS vài giờ một lần
Các nguyên nhân gây ra sai số từ môi trường truyền dẫn
• Sai số gây ra bởi độ trễ của tín hiệu GPS khi nó truyền qua các lớp của khí quyển bao gồm độ trễ tầng điện ly và tầng đối lưu Tốc độ lan truyền tín hiệu tăng tỉ lệ thuận với mật độ điện tử tự do trong tầng điện ly và tỉ lệ nghịch với bình phương tần số của tín hiệu Ảnh hưởng của tầng đối lưu có thể được mô hình hoá theo các yếu tố khí tượng là nhiệt độ, áp suất và độ ẩm Để làm giảm ảnh hưởng của tầng điện ly và tầng đối lưu người ta quy định chỉ quan sát vệ tinh ở độ cao từ 150km trở lên so với đường chân trời [15]
• Trong [16], các nhà nghiên cứu đã chỉ ra rằng nhiễu điện từ chỉ là một vấn đề khi cả ba điều kiện sau được thỏa mãn:
1 Đường dây tải điện có điện áp trên 230kV
2 Tần số xem xét nhỏ hơn 30MHz
3 Khoảng cách giữa đường truyền và máy thu nhỏ (dưới 100m)
Vì máy thu GPS hoạt động ở tần số vi ba cao hơn đáng kể 30MHz (thường từ 1,228GHz đến 1,575GHz), điều kiện thứ hai không được đáp ứng và do đó không có khả năng gây nhiễu cho máy thu GPS do các đường truyền gần đó gây ra
2.3.2 Phương pháp định vị đo động thời gian thực Real-Time Kinematic (RTK)
• Các hệ thống GPS sử dụng trong các ứng dụng đơn giản thường có độ chính xác ở hàng “m” Để cải thiện độ chính xác xuống dưới hàng “m”, hệ thống GPS sử dụng hiệu chỉnh vi sai DGPS là 1 phương pháp được sử dụng phổ biến Ngoài ra, với sự phát triển hiện nay hệ thống RTK-GPS có thể đạt được độ chính xác đến hàng “cm” [17]
Real-Time Kinematic là phương pháp được sử dụng để tăng độ chính xác định vị trí GNSS sử dụng trạm base cố định, truyền không dây dữ liệu hiệu chỉnh vị trí đến bộ thu di động – hay còn gọi là Rover
Hình 2 5 RTK với tình huống đường cơ sở ngắn (bên trái) và đường cơ sở dài (bên phải) Mũi tên màu xanh thể hiện chiều của dữ liệu tham chiếu truyền từ trạm Base sang Rover [14]
Trạm Base được đặt ở vị trí đã biết trước, thu nhận tín hiệu từ vệ tinh Các tín hiệu này vẫn sẽ bao gồm sai số do máy thu, vệ tinh và môi trường Để loại bỏ các sai số này, các dữ liệu sau khi được tính toán Base sẽ gửi các chỉ dẫn hiệu chỉnh tín hiệu đến Rover theo thời gian thực Rover sẽ sử dụng các chỉ dẫn hiệu chỉnh của Base để căn chỉnh kết quả vị trí Từ đó có thể đạt đến độ chính xác ở hàng
Các máy thu tín hiệu có thể có 1 hay nhiều băng tần thu Số lượng băng tần thu được sẽ quyết định số lượng dữ liệu mà chúng nhận được từ vệ tinh Những máy thu đa băng tần thường có khoảng cách tối đa giữa trạm base và rover (baseline) lớn hơn so với loại đơn băng tầng Bộ thu đa băng tần cũng ổn định hơn với các điều kiện khác nhau của không gian truyền tín hiệu Nó có thể duy
17 trì độ chính xác đến hàng “cm” ngay cả khi hoạt động trong các khu vực phức tạp: rừng, thành phố, ven biển, v.v…
2.4 Mô hình toán của robot
Mô hình hóa robot bằng phương trình chuyển đổi trạng thái
Mô hình xác suất của robot
Các biến trạng thái của robot được xem như là các biến ngẫu nhiên và tuân theo các quy luật xác suất Theo định lý Bayes, ta có thể tính xác suất sự kiện ngẫu nhiên A khi biến sự kiện liên quan B đã xảy ra được ký hiệu là P(A|B) Gọi A, B là hai biến trạng thái với at, bt là các giá trị của A, B tại thời điểm t:
• Xác suất xảy ra giá trị A = a t mà không quan tâm đến B là : p(A = a t )
• Xác suất xảy ra giá trị B = b t mà không quan tâm đến A là : p(B = b t )
• Xác suất xảy ra B khi biết A xảy ra là : p( B = b t | A = a t )
• Khi biết ba đại lượng này xác suất của A khi biết B sẽ là:
Các quy luật xác suất được áp dụng cho mô hình robot với trạng thái của robot x t là biến trạng thái cần được xác định dựa trên các phép đo từ cảm biến z t và tín hiệu điều khiển u t Để có thể ước lượng các biến trạng thái trên, robot sẽ cần có hai mô hình chính là process model (bao gồm các thông tin của robot hay còn được gọi là odometry) và measurement model
Process Model hay gọi là mô hình chuyển động biểu diễn phân bố xác suất vị trí hiện tại của robot p(x t |x t−1 , u t ) dựa vào thông tin chuyển động bao gồm trạng thái tại thời điểm trước đó và tín hiệu điều khiển hiện tại Đây có thể gọi là bước dự đoán trạng thái đầu tiên trong bộ lọc Particle sẽ được trình bày ngay sau Trạng thái (pose) của robot trong mặt phẳng chỉ bao gồm tọa độ (x,y) trong mặt phẳng và góc cho biết hướng của robot tại vị trí đó
Do đó có thể biểu diễn trạng thái robot bằng 1 vector:
Measurement Model hay là mô hình quan sát sử dụng thông tin từ các cảm biến để đánh giá lại ước lượng vị trí đã thực hiện ở Process Model Mô hình được biểu diễn bởi một phân bố xác suất p(z t |x t , m) Trong đó z t là phép đo từ cảm biến, x t là trạng thái của robot ở thời điểm t trong bản đồ m của môi trường Tại thời điểm t, robot thực hiện phép đo z t là một phân bố xác suất nên giả sử thu được một tập các giá trị z t (i) với 0 ≤ i ≤ k Ta biểu diễn phép đo là một mảng có dạng:
Các điểm đo không có tính ràng buộc, ta có thể xấp xỉ phân bố của z t :
Tương tự, bản đồ được sử dụng có dạng chia lưới thành nhiều ô vuông nhỏ có tọa độ tâm (x,y) và hai mức giá trị thể hiện có hay không có chướng ngại vật Gọi mi là ô vuông thứ i, vì vậy 1 bản đồ m sẽ là tập hợp hữu hạn các ô vuông mi.
Định vị là lập bản đồ đồng thời - Simultaneous localization and mapping (SLAM)
SLAM là quá trình robot xây dựng lại bản đồ của môi trường quanh nó trong khi đồng thời xác định vị trí của chính nó trong bản đồ đó Cảm biến như laser- Lidar SLAM, camera-visual SLAM được dùng để quét, thu thập hình ảnh những vùng không gian xung quanh sau đó xây dựng và định hình một bản đồ ảo Nhờ đó robot có khả năng định vị đáng tin cậy để hệ thống hoạt động hiệu quả và chính xác
Hệ thống Lidar SLAM sử dụng cảm biến Lidar (Light Detection and Ranging) và sự xếp chồng dữ liệu về khoảng cách ở các lần quét liên tiếp nhau của cảm biến để tạo bản đồ 2D hay 3D về môi trường So với vSLAM, Lidar
SLAM có khối lượng tính toán ít hơn, phù hợp với nhiều phần cứng, giá thành vừa phải
Bài toán được phát biểu dưới dạng xác suất như sau:
Từ phát biểu trên, ta có thể chia bài toán thành 2 giai đoạn khác nhau:
• Định vị: Xác định vị trí hiện tại của robot bằng bản đồ xây dựng được đến hiện tại sử dụng Particle filter
• Vẽ bản đồ từ vị trí đã dự đoán được: Khi đã biết được vị trí của robot so với gốc tọa độ, cần xác định các đối tượng trong môi trường làm việc để xây dựng nên bản đồ Để cải thiện độ chính xác, cảm biến có thể sử dụng dữ liệu từ camera hoặc nguồn dữ liệu vị trí từ cảm biến IMU Sử dụng dữ liệu này sẽ hỗ trợ cho robot cải thiện ước tính về vị trí của nó Khi thông tin vị trí mới được thu thập cứ sau vài giây, các đặc trưng sẽ căn chỉnh và cải thiện ước tính
2.6.2 Bộ lọc Particle Ước lượng vị trí của robot di động trong bản đồ là vấn đề xác định trạng thái của robot trong môi trường bằng cách sử dụng dữ liệu từ các cảm biến, khi vị trí ban đầu không xác định và thuật toán xác suất dựa trên bộ lọc particle filter hay còn được gọi là Monte Carlo localization, là một trong những cách tiếp cận phổ biến giúp giải quyết bài toán trên
Trong bộ lọc Particle [19] có các mẫu trạng thái được gọi là particle và được ký hiệu là:
Mỗi particle [ ] (1 ) t x m mM tại thời điểm t là 1 trạng thái của robot trên bản đồ M đại diện cho số lượng particles trong tập particlesX t Số lượng particle
M trong tập hợp thường rất lớn (thường là 500-5000), với số lượng càng lớn, vị trí của robot sẽ được cập nhật chính xác hơn nhưng đòi hỏi tài nguyên tính toán lớn hơn
Bộ lọc Particle xây dựng hàm xác suất bel x( ) t từ hàm xác suất bel x( t − 1 ) bằng phương pháp đệ quy, xác suất sẽ được biểu diễn bằng tập hợp các particles, do đó tập x t sẽ được xây dựng từx t − 1 Đầu vào của thuật toán này sẽ là tập particle tại thời điểm ( t − 1 ) là x t − 1 cùng với tín hiệu điều khiển và giá trị đo tại thời điểm t là u t vàz t Đầu tiên thuật toán này xây dựng tạm một tập hợp particle x
Khởi tạo trạng thái [ ] t x m cho thời điểm t dựa trên trạng thái trước đó và tín hiệu điều khiển hiện tại
Tính trọng số cho từng particle tương ứng Dựa trên độ đúng của phép đo z t của trạng thái x [ ] t m
Cập nhật lại particle tạm thời
Thực hiện lấy mẫu lại biến một tập hợp gồm M particle từ tập particle tạm thời vừa được cập nhật ta được một tập hợp particle khác có cùng kích thước Xác suất chọn lọc các particle dựa vào trọng số của nó Bước lấy mẫu lại có chức năng quan trọng để buộc các particle trở lại phân phối posterior bel x( ) t
Như đã trình bày trong (2.37), Mô hình robot hoạt động trên mặt phẳng có vector trạng thái dạng:
Trong đó (x,y) là vị trí của robot và là hướng của robot tại vị trí đó Mô hình chuyển động dự đoán trạng thái của bộ lọc particle sẽ diễn tả xác suất biến đổi trạng thái p x x ( t | t − 1,u t )
Hình 2 7 Ví dụ về particles Khi có tín hiệu điều khiển u t vị trí và hướng của robot sẽ thay đổi từ x t − 1 đến điểm x t i là 1 trong những điểm bên phải Từ giá trị đo được của các cảm biến robot xác định được vị trí có xác suất đúng cao nhất là điểm màu đỏ.
Mapping với bộ lọc Rao-Blackweilized
Bộ lọc Rao-Blackwilized Particle Filter (RBPF) là một dạng của bộ lọc Particle dùng để biểu diễn một phân phối xác suất trong không gian mapping Ưu điểm của RBPF là nó có thể giới hạn không gian lấy mẫu, nghĩa là sẽ lấy mẫu trong một không gian con của bản đồ Việc chia nhỏ không gian thành các phần như vậy sẽ giúp cho việc xử lí bài toán trở nên đơn giản hiệu quả hơn Trong bài toán SLAM, Rao-Blackwilized particle filter [20] ước tính xác suất posterior p x m z u ( 1: t , | 1: t , 1: 1 t − ) cho quỹ đạox 1: t = x 1 , ,x t của robot và bản đồ m của môi trường từ dữ liệu quan sát z 1: t = z 1 , ,z t và tín hiệu đo từ odometry
Thành phần thứ nhất p x ( 1: t |z u 1: t , 1: 1 t − ) đại diện cho những quỹ đạo x 1:t của robot được ước lượng từ bộ lọc Particle với đầu vào là dữ liệu quan sát z 1:t và tín hiệu đo từ odometryu 1: 1 t −
Thành phần p m x ( | 1: t ,z 1: t ) có thể tính khi biết trước trạng thái x 1:t và dữ liệu quan sát z 1:t Để ước lượng thành phần thứ nhất trên các quỹ đạo có tiềm năng, Rao- Blackwellized mapping sử dụng bộ lọc Particle trong đó từng bản đồ được xây dựng dựa trên những dữ liệu quan sát z 1:t và quỹ đạo x 1:t và được đại diện bởi các particle Trong quá trình lấy mẫu lại, Trọng số của từng particle sẽ tỉ lệ thuận với xác suất p z m x ( t | , t ) của quan sát gần đây nhất và bản đồ M liên quan đến particle đó và trạng thái của nó
Từ việc phân chia 2 thành phần trạng thái và bản đồ, Rao-Blackwilized particle filter cho phép SLAM xây dựng và cập nhật bản đồ dựa trên những ước tính quỹ đạo của các mẫu trong quá trình lập bản đồ với trạng thái đã biết trước.
Robot localization với AMCL
Thuật toán Monte Carlo Localization (MCL) được sử dụng để ước tính vị trí và hướng của robot (sau đây được gọi tắt là trạng thái) a) Biểu diễn trạng thái:
Trạng thái của robot trong không gian 2D được thể hiện thông qua vector trạng thái x = ( x y , , )(trong đó x, y là vị trí của robot, đại diện cho hướng của robot) Các thông số này được ước tính dựa trên dữ liệu về môi trường đọc được từ cảm biến và motion model của robot
Trạng thái ước tính tại thời điểm hiện tại của robot là một hàm mật độ xác suất phân phối trên không gian trạng thái Phương pháp MCL sử dụng tập hợp
M các particle X t = x t [1] , x t [2] , , x t [M ] để thể hiện ước tính trạng thái của robot tại thời điểm t Mỗi particle là một giả thuyết về trạng thái của robot, vùng không gian trạng thái tập trung càng nhiều các particles thì xác suất đó là vị trí thực tế của robot càng cao và ngược lại
Trạng thái hiện tại của robot phụ thuộc vào trạng thái ngay trước đó của nó Tại thời điểm ban đầu, do không có thông tin về trạng thái trước đó của robot nên dự đoán trạng thái hiện tại của robot có thể được phân bố đồng đều trên không gian b) Khởi tạo các particles
Số lượng các particles khởi tạo ban đầu càng lớn khả năng hội tụ về đúng vị trí thực tế càng cao tuy nhiên nó cũng đòi hỏi khối lượng tính toán lớn hơn Số lượng các particles được điều chỉnh dựa trên trọng lượng các cụm particles, điều này giúp giảm số lượng particles theo thời gian, tăng hiệu quả cho quá trình localization
Trạng thái ban đầu hoặc vị trí chính xác trong hệ tọa độ toàn cục có thể được sử dụng để khởi tạo tập particles ban đầu Với trường hợp không có vị trí ban đầu, các particles được trải điều trên không gian trạng thái do đó trong những thời gian đầu đòi hỏi lượng tính toán lớn Để tối ưu hóa hiệu suất của thuật toán, vị trí ban đầu có thể được chỉ định thủ công, điều này giúp tăng tốc và độ chính xác của thuật toán từ những chu kỳ đầu tiên c) Quá trình cập nhật trạng thái và lấy mẫu lại:
Trọng số các particles được tính toán dựa trên sự thay đổi của trạng thái và quá trình motion update, sensor update của robot
Motion_update: Motion Model cung cấp thông tin cho quá trình dự đoán xu hướng phân bố của các particles trong quá trình lấy mẫu lại Thông thường, mô hình chuyển động được bao gồm trong thuật toán MCL là odometry (odometryMotionModel)
Dựa trên chuyển động của robot thực tế, các sai số có thể được ước tính dưới dạng vector bốn phần tử
• Sai số quay do chuyển động quay
• Sai số quay do chuyển động tịnh tiến
• Sai số tịnh tiến do chuyển động tịnh tiến
• Sai số tịnh tiến do chuyển động quay
Bằng cách xác định chính xác các thông số này, các particles được phân bố một cách chính xác hơn, từ đó cung cấp cho thuật toán MCL đủ giả thuyết để tìm ra ước tính tốt nhất cho vị trí của robot
Sensor_update: Sensor model sử dụng dữ liệu từ cảm biến để tính toán xác suất vị trí của robot cho từng particles và cập nhật trọng số t [ ] i cho các particles tỷ lệ với xác suất được tính trước đó
Những dữ liệu từ cảm biến gồm các dạng:
• Vị trí và hướng của cảm biến so với vị trí của robot
• Giới hạn phạm vi tối thiểu và tối đa Kết quả ngoài các phạm vi này bị bỏ qua trong quá trình tính toán xác suất
• Số lượng chùm tia được sử dụng để tính toán xác xuất vị trí của robot
• Thành phần nhiễu từ cảm biến được xác định
• Độ lệch tiêu chuẩn do nhiễu đo lường
• Trọng số cho xác suất của phép đo ngẫu nhiên
• Trọng số cho xác suất đo lường dự kiến
Sensor model cũng lưu trữ bản đồ của môi trường robot Sử dụng bản đồ dạng nhị phân để chỉ định bản đồ của bạn với các không gian trống hoặc vật cản
Ngưỡng cập nhật trạng thái là một vectơ ba phần tử xác định sự thay đổi tối thiểu trong trạng thái robot, ( , , x y ), để kích hoạt quá trình cập nhật Sự thay đổi trong tư thế robot này dựa trên dữ liệu odometry Các ngưỡng này được điều chỉnh dựa trên mô hình thực tế của robot có xem xét đến ảnh hưởng của nhiễu ngẫu nhiên nhằm tránh việc cập nhật không cần thiết và ảnh hưởng đến hiệu suất của thuật toán
Việc lấy mẫu lại và phân phối các particles sẽ dựa trên trọng lượng của chúng Trong quá trình này, các particles có trọng số thấp sẽ bị loại bỏ dần giúp các particles có thể hội tụ về trạng thái đúng thực tế
Bản địa hóa Monte Carlo có thể được cải thiện bằng cách lấy mẫu các hạt theo cách thích ứng dựa trên ước tính sai số sử dụng phân kỳ Kullback-Leibler (KLD) Ban đầu, cần sử dụng M lớn do nhu cầu bao phủ toàn bộ bản đồ với sự phân bố ngẫu nhiên đồng đều của các hạt Tuy nhiên, khi các hạt đã hội tụ xung quanh cùng một vị trí, việc duy trì kích thước mẫu lớn như vậy là lãng phí về mặt tính toán
Phương pháp lấy mẫu lại KLD là một biến thể của Bản địa hóa Monte Carlo trong đó tại mỗi lần lặp lại, kích thước mẫu Mx được tính toán lại Kích thước mẫu M x được tính toán sao cho với xác suất (1 −), sai số giữa giá trị sau đúng và ước lượng dựa trên mẫu nhỏ hơn Các biến , là các tham số cố định
QUÁ TRÌNH THỰC HIỆN
Các thành phần của hệ thống
Hình 3 1.Sơ đồ phần cứng robot
Robot Operating System Framework (ROS)
ROS là một thư viện và công cụ trung gian mã nguồn mở, được sử dụng để phát triển các thuật toán cho robot ROS có một môi trường hoạt động cho phép các thiết bị có thể kết nối với nhau thông qua Topic và Node Node sẽ public các dữ liệu lên các Topic, các Node khác nhau có thể Subscribe vào các Topic bất kì để lấy dữ liệu
Hình 3 2 Cấu trúc cơ bản của ROS Luận văn này sử dụng ROS phiên bản Noetic cho cả ROS master và ROS client
Hình 3 3 Cấu trúc liên kết thông tin của hệ thống
Bảng 3 1 Tần số lấy mẫu các cảm biến Cảm biến Tần số lấy mẫu
Hình 3 4 Quá trình xử lý dữ liệu từ IMU trước khi vào bộ lọc EKF
Hình 3 5 Quá trình xử lý dữ liệu từ encoder trước khi vào bộ lọc EKF
Hình 3 6 Quá trình xử lý dữ liệu từ GNSS-RTK rover trước khi vào bộ lọc EKF
Hình 3 7 Quá trình xử lý dữ liệu từ Lidar trước khi vào bộ lọc EKF
Localization với thuật toán AMCL
a) Sử dụng thuật toán AMCL truyền thống
Package AMCL trong metapackage Navigation triển khai thuật toán Adaptive Monte Carlo Localization Đầu ra là trạng thái của robot từ bản đồ cho trước (Occupancy grid map được quét bằng SLAM), sử dụng các thông tin có được từ Lidar, Encoder, IMU
Package sẽ biến đổi dữ liệu từ frame /scan về frame /odom, sau đó hệ thống sẽ ước tính vị trí của robot trong static map bằng cách so sánh và ước lượng giá trị odometry của robot từ ~odom_frame với static map từ ~map_frame
Các thông số quan trọng của package AMCL
Thông số (parameter) Ý nghĩa odom_alpha1 Thông số ước tính nhiễu tác động lên chuyển động quay của odometry từ thành phần chuyển động quay của robot
Odom_alpha2 Thông số ước tính nhiễu tác động lên chuyển động quay của ước lượng odometry từ thành phần tịnh tiến của chuyển động của robot
Odom_alpha3 Thống số ước tính nhiễu tác động lên chuyển động tịnh tiến ước lượng của odometry từ thành phần tịnh tiến của chuyển động của robot
Odom_alpha4 Thông số ước tính nhiễu tác động lên chuyển động tịnh tiến ước lượng của odometry từ thành phần quay của chuyển động của robot
Odom_model_type Chọn loại model robot:
- Value = “diff” : Robot chuyển động dạng vi sai 2 bánh
- Value = “omni”: Robot chuyển động dạng omni (theo cả phương x và y)
Localization với bộ lọc Kalman mở rộng
Trong luận văn này, bộ lọc Kalman mở rộng được dùng để kết hợp dữ liệu từ nhiều loại cảm biến khác nhau như IMU, Encoder, GNSS-RTK, Lidar…
Mô hình trạng thái tại thời điểm k của Robot được biểu diễn dưới dạng rời rạc:
Giả sử nhiễu quá trình và nhiễu đo là Gaussian, không tương quan và giá trị trung bình bằng 0, không có tương quan chéo:
Robot được khởi tạo từ vị trí ban đầu tại bước k = 0, với hiệp phương sai P0 và ước lượng trạng thái xˆ 0 mô tả vị trí, góc quay, vận tốc tịnh tiến, vận tốc góc và gia tốc tịnh tiến theo 3 trục
Các ma trận được tính như sau:
❖ Trong pha hiệu chỉnh (Correct):
Trạng thái của hệ thống được quan sát bằng một số phép đo tạo nên vector z k =h x( k )+v k trong đó v k là nhiễu đo lường Dữ liệu từ các cảm biến như Encoder, IMU và GNSS-RTK là đầu vào cho bộ lọc
Với hệ thống kết hợp dữ liệu từ nhiều cảm biến, với từng ma trận thông tin z nhận được từ cảm biến, các quá trình cập nhật trạng thái và quá trình hiệu chỉnh sẽ được thực hiện khi có thông tin từ cảm biến
Mô hình đo lường sẽ thay đổi tuỳ theo loại cảm biến đang cung cấp dữ liệu
• Dữ liệu kiểu nav_msgs/Odometry (từ Encoder odometry và amcl_pose): z ( ) 0 0 0 0 0 0 0 0 0 0 0 0
• Dữ liệu kiểu sensor_msgs/IMU (từ IMU): z ( ) 0 0 0 0 0 0 0 0 0 0 0 0
• Dữ liệu kiểu sensor_msgs/NavSatFix (từ navsat_transform_node):
Hình 3 8 Sơ đồ kết hợp dữ liệu các cảm biến sử dụng bộ lọc Kalman mở rộng
Quá trình tổng hợp dữ liệu sử dụng 2 bộ lọc EKF,
• EKF_local: nhận dữ liệu từ odometry được tính từ tính hiệu encoder, dữ liệu từ IMU và cho ra kết quả ước lượng vị trí cục bộ Kết quả ước lượng này thường bị ảnh hưởng bởi hiện tượng trôi và sai số tích luỹ theo thời gian do tính chất tương đối của 2 cảm biến
• EKF_global: Nhận dữ liệu từ GNSS-RTK, kết quả dự đoán của AMCL và kết quả ước lượng từ bộ EKF_local để ước lượng ra kết quả định vị cuối cùng
Hình 3 9 Sơ đồ liên kết thông tin giữa các node
CHƯƠNG 4: KẾT QUẢ THỰC HIỆN VÀ ĐÁNH GIÁ.
Đánh giá sai số từng thành phần
Thực hiện đánh giá độ chính xác của IMU thông qua quá trình thay đổi góc của IMU theo trục z và thu thập giá trị IMU trả về
Ban đầu, IMU được đặt trên mặt phẳng nằm ngang, song song với mặt đất Sau đó xoay IMU theo trục z, mỗi lần thay đổi 45 độ Góc ước tính từ IMU được public trên topic /imu/rpy/filtered
Kết quả đánh giá sai số được trình bày trong bảng dưới đây
• r (deg) là góc xoay thực tế của IMU
• ˆ (deg) f là góc xoay theo chiều thuận chiều kim đồng hồ của IMU
• ˆ (deg) r là góc xoay theo chiều ngược chiều kim đồng hồ của IMU
• f (deg)là sai số góc xoay theo chiều thuận chiều kim đồng hồ của
• r (deg)là sai số góc xoay theo chiều ngược chiều kim đồng hồ của IMU
Hình 4 1 Sai số góc Yaw trước và sau khi qua bộ lọc complementary
IMU có sai số nhỏ trong khoảng thời gian đầu, tuy nhiên càng về sau hiện tượng trôi xuất hiện càng rõ gây ảnh hưởng đến kết quả ước lượng góc xoay
Thực hiện đánh giá độ chính xác của Lidar trong môi trường ngoài trời
Robot được đặt cố định tại điểm mốc, di chuyển vật cản ra xa dần theo bước 1m
Hình 4 2 sai số cảm biến Lidar khi thực hiện đánh giá tại vị trí tĩnh
Tuy thuộc loại cảm biến trong nhà, tuy nhiên khi hoạt động trong môi trường ngoài trời, Lidar sick LMS100 vẫn đạt được sai số khá nhỏ (khoảng 0.7m) ở ngưỡng giới hạn của lidar là 25m
4.1.3 GPS a Sân trước thư viện
Hình 4 3 Hình ảnh thực tế vị trí đặt base và khu vực khảo sát, điều kiện bầu trời quang đãng, vị trí đặt base trống, không bị che khuất phía trên
Mục đích: đánh giá khả năng hoạt động của GPS-RTK trong điều kiện ngoài trời, có các tòa nhà xung quanh thông qua việc đánh giá: GPS ở trạng
41 thái di chuyển (Độ ổn định của tín hiệu, sai số theo phương di chuyển, sai lệch theo phương ngang)
Hình 4 4 Deviation Map quỹ đạo di chuyển
Hình 4 5 Chart vs Histogram Carrier Range Status
Hình 4 6 Chart vs Histogram Histogram Position accuracy
Quỹ đạo di chuyển thu được từ tín hiệu của rover hoàn toàn tương đồng với quỹ đạo thực tế
Trong toàn bộ quá trình di chuyển, mặc dù đôi khi bị che khuất nhưng GNSS_RTK đều ở trạng thái fix, chứng tỏ GNSS_RTK có thể hoạt động ổn định trong môi trường này b Trạm biến áp 220kV Thủ Đức
Mục đích: Đánh giá khả năng hoạt động của GPS-RTK trong điều kiện ngoài trời, có nhiều thiết bị xung quanh, môi trường có điện trường mạnh thông qua việc đánh giá: GPS ở trạng thái tĩnh, và trạng thái đang di chuyển (Độ ổn định của tín hiệu, sai số theo phương di chuyển, sai lệch theo phương ngang)
❖ Thử nghiệm đánh giá độ ổn định trong một số khu vực trong trạm biến áp
Hình 4 7 Lộ trình khảo sát được đánh dấu, màu vàng - fix, màu đỏ - float, màu đen
Hình 4 8 Deviation Map thể hiện quỹ đạo di chuyển hoàn toàn tương đông với quỹ đạo thực tế
Hình 4 9 Chart vs Histogram Carrier Range Status
Hình 4 10 Chart vs Histogram Position accuracy
Trong quá trình di chuyển, từ khu vực 110kV sang 220kV Khi giữa base và rover xuất hiện vật cản, thông số 3D accuracy bắt đầu tăng lên
Khi di chuyển từ sân ngắt 110kV ra đường nội bộ, ngoài các tủ hợp bộ thì giữa base và rover có thêm vật cản là bể dầu sự cố và nhà để dụng cụ chữa cháy Thông số 3D accuracy bắt đầu tăng lên tuy nhiên rover vẫn hoạt động ở chế độ fix
Khi bắt đầu di chuyển từ đường nội bộ vào khu vực sân ngắt 220kV Chế độ hoạt động của rover thay đổi từ fix xuống float trước khi về chế độ No RTK khi đi vào sâu trong khu vực sân ngắt 220kV
Rover vẫn ở chế độ No RTK ngay cả khi đã đi ra khỏi khu vực sân ngắt 220kV Di chuyển lên đến vị trí không còn vật cản nào giữa base và rover thì tín hiệu mới khôi phục về trạng thái fix
Tiếp tục di chuyển trong đường nội bộ khu vực sân 220kv rover vẫn hoạt động ở trạng thái fix Đến vị trí cuối đường nội bộ Rover lại rơi vào trạng thái
No RTK và phải di chuyển lên đến vị trí không còn vật cản nào giữa base và rover thì tín hiệu mới khôi phục về trạng thái fix
Khi giữa base và rover có nhiều vật cản, các gói thông tin truyền qua sóng
RF bị ảnh hưởng, có thể không đến được rover Nếu khoản thời gian này kéo dài lâu có thể làm thay đổi chế độ hoạt động của rover
Khi đi qua các máy biến áp hoặc đến gần những trụ điện, chế độ hoạt động của rover cũng bị ảnh hưởng Nguyên nhân có thể do từ trường quanh máy biến áp và các trụ đã ảnh hưởng đến sóng RF liên lạc giữa base và rover
Cần phải chọn vị trí đặt Base tối ưu để robot có tầm hoạt động hiệu quả nhất, ít bị ảnh hưởng bởi các thiết bị trong sân ngắt
❖ Thử nghiệm đánh giá độ ổn định của tín hiệu, sai số theo phương di chuyển, sai lệch theo phương ngang trong khu vực đường nội bộ khu vực 110kV
Hình 4 11 Vị trí khảo sát được đánh dấu là hình chữ nhật màu vàng
Hình 4 12 Hình ảnh thực tế vị trí đặt base và khu vực khảo sát, điều kiện bầu trời quang đãng, vị trí đặt base trống, không bị che khuất phía trên
Hình 4 13 Kết quả Survey in sau hơn 90 phút, giá trị Mean 3D đạt 0.35m, khá phù hợp với thông số nhà sản xuất đưa ra
❖ Di chuyển và thu thập dữ liệu:
Hình 4 14 Vị trí các điểm dừng và lộ trình di chuyển
Hình 4 15 Deviation Map thể hiện quỹ đạo di chuyển hoàn toàn tương đông với quỹ đạo thực tế
Hình 4 16 Chart và Histogram Carrier Range Status
Trong toàn bộ quá trình di chuyển, mặc dù đôi khi bị che khuất nhưng GNSS_RTK đều ở trạng thái fix, chứng tỏ GNSS_RTK có thể hoạt động ổn định trong môi trường này
Hình 4 17 Chart vs Histogram Position accuracy
− Sai số khoảng 10cm khi Rover cách base nhỏ hơn 60m (Chi tiết đánh giá trong phần phụ lục.)
− Sai số tăng dần khi đi ra xa base, sai số lớn nhất khoảng 1.48%
− GPS-RTK có thể hoạt động ổn định trong môi trường này.
Đánh giá bài toán Localization sử dụng bộ lọc EKF
Quá trình đánh giá bài toán localization sẽ được thực hiện trong khuôn viên trường Đại học Bách khoa Tp HCM
Sân trước thư viện được chọn làm vị trí đánh giá kết quả Localization sử dụng bộ lọc EKF hợp nhất dữ liệu từ Encoder + IMU + GPS
Hình 4 18 Vị trí thực hiện khảo sát.
Ground truth cho các đánh giá này được thu thập bằng cách dừng xe tại nhiều vị trí trên quỹ đạo di chuyển để xác định vị trí và hướng của xe tại từng điểm khảo sát
Hình 4 19 Xây dựng hệ trục tọa độ 30x30m tại sân trước thư viện
50 a) Đánh giá sai số vị trí
Hình 4 20 Qũy đạo thực tế và quỹ đạo ước lượng
Hình 4 21 Phân bố của sai số vị trí đầu ra EKF _local
Hình 4 22 Phân bố của sai số vị trí bộ lọc EKF_global Sai số theo vị trí được tính theo công thức: e = √∆x 2 + ∆y 2
Hình 4 23 Đồ thị khoảng cách sai lệch giữa vị trí thực tế với vị trí ước lượng từ bộ
Hình 4 24 Đồ thị khoảng cách sai lệch giữa vị trí thực tế với vị trí ước lượng từ bộ
Vị trí ước lượng từ bộ EKF_local không còn đúng sau 1 quãng đường ngắn Sai số lớn (~40m theo trục x và 18m theo trục y) Nguyên nhân lệch sai lệch lớn đến từ sai lệch quá lớn từ dữ liệu của Encoder và IMU khi quay:
• Giá trị IMU trả về là giá trị vận tốc góc, khi quay vận tốc góc của robot nhỏ nên dẫn tới tính toán góc quay từ giá trị vận tốc không chính xác
• Địa hình gập ghềnh, không phẳng, độ ma sát không đều cũng là một nguyên nhân làm cho số đo góc được tính toán từ Encoder khi quay không chính xác
Nhờ có dữ liệu vị trí chính xác từ GPS mà khi kết hợp dữ liệu này với dữ liệu từ Encoder và IMU, kết quả đã tốt hơn rất nhiều (nhỏ hơn 0.81m theo trục x và nhỏ hơn 0.63m theo trục y)
53 b) Đánh giá sai số góc yaw
Hình 4 25.Góc Yaw của robot trong quá trình di chuyển
Hình 4 26 Sai số góc yaw theo khoảng cách di chuyển
Hình 4 27 Sai số góc yaw theo khoảng cách di chuyển của bộ EKF_local và
Sai số góc yaw khi chỉ sử dụng dữ liệu từ Encoder tăng dần theo thời gian, do sai số tích lũy
Sai số góc yaw khi hợp nhất dữ liệu từ bộ EKF_local với EKF_global tương đồng nhau vì sự khác biệt căn bản từ 2 phương pháp này đến từ GPS mà GPS chỉ có thông tin về vị trí, không có thông tin về hướng.
Đánh giá kết quả bài toán Localization sử dụng AMCL
Bài toán Localization sử dụng thuật toán AMCL yêu cầu phải có sẵn map Quá trình mapping được thực hiện ngoài trời, khu vực sân trước thư viện
Qua thực nghiệm cho thấy cảm biến LidarA1 không thể sử dụng ngoài trời vào ban ngày, nguyên nhân có thể do ánh sáng mặt trời Do đó thời gian thử nghiệm được chọn là vào buổi chiều tối
Do sử dụng lidar A1, tầm quét khi ra ngoài trời chỉ còn tối đa 7m và khu vực sân trước thư viện quá trống trải nên một số vật cản là các thùng carton được sắp xếp để tạo môi trường chạy thử
Hình 4 28 Bố trí môi trường giả định cho hoạt động của robot
Thư viện Hector_mapping trên nên tảng Ros được sử dụng để xây dựng map dựa trên dữ liệu từ cảm biến Lidar
Hình 4.42 Bản đồ xây dựng được từ gói Hector_mapping
Bảng 4.1 Sai số giữa bản đồ quét được và thực tế với các cạnh lấy mẫu
Bản đồ xây dựng khá chính xác, hình dạng và kích thước tương đương với bản đồ thực tế
Sai số khoảng cách trung bình tới 4 điểm của robot dao động từ 6.47→ 11.45 % so với khoảng cách thực tế
Với cùng bản đồ đã xây dựng ở mục trước, thuật toán AMCL được sử dụng để định vị robot cho môi trường đã xây dựng
Hình 4 29 Khởi chạy thuật toán AMCL
Hình 4 30 Quá trình hội tụ các particle khi robot di chuyển
Hình 4 31 Dừng xe và đo khoản cách cách từ xe đến 4 mốc trên bản đồ
Hình 4 32 Kết quả định vị trên Rviz dừng đo lần 1
Bảng 4 1 Đánh giá kết quả định vị dựa trên khoảng cách của robot đến 4 mốc lần 1 trên bản đồ
Khoảng cách thực tế (m) Khoảng cách trên Rviz
Tiếp tục dừng robot ở điểm cuối của map, thu được kết quả:
Hình 4 33 Kết quả định vị trên Rviz dừng đo lần 2 Bảng 4 2 Đánh giá kết quả định vị dựa trên khoảng cách của robot đến 4 mốc lần 2
59 trên bản đồ Khoảng cách thực tế (m) Khoảng cách trên Rviz
Sai số định vị so với thực tế khá tốt, chỉ khoảng 0.11m (1.32%)
Trong môi trường có nhiều điểm tương đồng và ít vật thể, khi robot di chuyển qua đoạn rẽ, nhờ có dữ liệu từ GPS mà độ ổn định của kết quả định vị cao hơn so với thuật toán AMCL truyền thống
Thuật toán AMCL có thể định vị tốt cho robot trong môi trường ít object và môi trường không có quá nhiều điểm khác biệt.
Đánh giá kết quả bài toán Localization sử dụng kết hợp AMCL và EKF
Vị trí được lựa chọn thực hiện thí nghiệm là khu vực sân trước thư viện Dưới đây là một thử nghiệm khi định vị sử dụng bộ Dual_EKF trong trường hợp tín hiệu GPS ổn định
Hình 4 34 Kết quả định vị khi bộ Dual_EKF có đầy đủ dữ liệu từ GNSS-RTK Thử nghiệm xóa toàn bộ dữ liệu của GNSS-RTK
Hình 4 35 Kết quả định vị khi bộ Dual_EKF không có dữ liệu từ GNSS-RTK Thực hiện thêm dữ liệu từ topic amcl_pose vào bộ EKF_global của bộ Dual_EKF
Hình 4 36 Kết quả định vị khi bộ Dual_EKF kết hợp với AMCL không có dữ liệu
61 Hình 4 37 Đồ thị liên kết các node dựa vào topic
Khi bộ Dual_EKF không nhận được dữ liệu từ GNSS-RTK kết quả định vị phụ thuộc hoàn toàn vào dữ liệu từ Encoder và IMU Các loại cảm biến này bị ảnh hưởng bởi sai số tích lũy dẫn đến kết quả định vị cuối cùng bị lệch
Thuật toán AMCL sử dụng dữ liệu từ Lidar và map có sẵn để trả về trạng thái của robot Dữ liệu này sẽ giúp loại bỏ hiện tượng trôi do sai số tích lũy từ Encoder và IMU, cải thiện độ chính xác cho bài toán định vị
Vị trí được lựa chọn thực hiện thí nghiệm là khu vực đường đi giữa Hội trường A5, toà nhà B4 và sân bóng Trong khu vực này khi di chuyển qua các góc khuất gữa hội trường A5, B4 do bị che khuất bởi toà nhà và nhiều tán cây, tín hiệu GPS trong khi vực này không ổn định
Hình 4 38 Quỹ đạo di chuyển trong thử nghiệm 2 được đánh dấu màu vàng
Tổng quãng đường di chuyển là 212m Sai lệch vị trí giữa vị trí đầu và vị trí cuối là khoảng 1.7m, sai lệch về hướng là khoảng 15 độ
Sai lệch về hướng và vị trí là do trong quá trình di chuyển robot bị nghiêng theo mặt đường với những góc khác nhau làm thay đổi góc giữa lidar với mặt đất làm cho dữ liệu từ lidar bị sai lệch, không thể khớp với bản đồ để đưa ra ước
63 tính tốt nhất để bù cho ước tính sai lệch khi tín hiệu GNSS-RTK không ổn định
Do đó kết quả Định vị bị ảnh hưởng nhiều bởi sai số tích luỹ
Hình 4 39 Kết quả định vị
Vị trí được lựa chọn thực hiện thí nghiệm là khu vực đường nội bộ 110kv Trong khu vực này, tại thời điểm thực hiện thử nghiẹmw, GNSS-RTK thường xuyên gặp vấn đề gián đoạn liên lạc giữa base và rover dẫn đến dữ liệu GNSS- RTK bị gián đoạn
Tổng quãng đường di chuyển là 153m Sai lệch vị trí giữa vị trí đầu và vị trí cuối là khoảng 0.5m, sai lệch về hướng là khoảng 3 độ
Trong quá trình thực hiện, tính hiệu GNSS-RTK thường xuyên bị gián đoạn dẫn đến việc có một số, thời điểm kết quả định vị bị thay đổi đột ngột Tuy nhiên
64 nhờ có cập nhật chỉnh sửa từ AMCL_Pose mà kết quả định vị sau đó đã được sửa lại thành công
Hình 4 40 Kết quả định vị thử nghiệm 3
Ứng dụng kết quả Localization vào Navigation robot tại sân trước thư viện Đại học Bách khoa
Trong thử nghiệm này, Robot sẽ sử dụng kết quả Định vị từ Dual_EKF để thực hiện Navigation cho môi trường ngoài trời
Thực hiện navigation theo nhiều quỹ đạo, khoảng cách khác nhau
Hình 4 41 Kết quả 4 lần Navigation Sau 4 lần chạy thử và thu thập dữ liệu đánh giá:
Hình 4 42 Đánh giá sai số vị trí của robot sau 4 lần chạy thử
Hình 4 43 Đánh giá sai số góc của robot sau 4 lần chạy thử
• Robot đã được định vị thành công, có nhiều điểm tương đồng với kết quả khảo sát
• Sai số theo trục x khoảng 1.93% và trục y 3.6%
• Quỹ đạo robot di chuyển vẫn chưa thực sự tối ưu, sai số về góc, vị trí tuy chưa hoàn toàn chính xác với điểm chỉ định tuy nhiên kết quả localization vẫn chính xác với thực tế
• Ảnh hưởng từ bản chất trễ của hệ thống cũng là một phần nguyên ngân khiến robot không di chuyển theo đúng quỹ đạo vạch sẵn