Thiết kế hệ thống định vị và dẫn đường

3.3.1. Xây dựng thuật toán quét bản đồ

Hình 3.17: Lưu đồ thuật toán SLAM

Robot có thể thực hiện các gói phần mềm SLAM sử dụng các loại thuật toán khác nhau dựa trên nền tảng ROS như Gmapping, Cartographer, hector, karto, tuy nhiên sử dụng phổ biến nhất vẫn là Hector mapping. Với dữ liệu cảm biến laser quét 360 độ, robot vừa có thể tạo bản đồ, vừa có khả năng tự định vị mình trong bản đồ đó. Đối với thuật toán Hector mapping, robot sử dụng bộ lọc để tìm ra tọa độ gần với vị trí thực tế của robot trên bản đồ. Hình 3.6 biểu diễn lưu đồ thuật toán SLAM cho quy trình vừa định vị vừa cập nhật bản đồ vào cơ sở dữ liệu. Quá trình này diễn ra liên tục mỗi khi có dữ liệu từ cảm biến trả về, robot vừa định vị bản thân, vừa cập nhật bản đồ vào cơ sở dữ liệu tại cuối quá trình.

3.3.1.1. Thuật toán xây dựng bản đồ (mapping):

a) Cơ sở lý thuyết:

Thuật toán sử dụng để xây dựng bản đồ từ dữ liệu laser phổ biến là Gmapping. Bản đồ được tạo ra là bản đồ dạng lưới, là dạng bản đồ tiêu chuẩn trong các ứng dụng robot. Thuật toán này được đề xuất đầu tiên trong cuốn Rao- Blackwellized Particle Filtering for Dynamic Bayesian Networks của Kevin Murphy và Stuart Russell. Ý tưởng chính là sử dụng bộ lọc hạt Rao- Blackwelliezed. Sau đó chúng được cải tiến bằng cách tối ưu hóa các bản phân phối đề xuất và giới thiệu tính năng lấy mẫu lại thích ứng, làm cho thuật toán phù hợp hơn cho các ứng dụng thực tế. Sau đó nó được gọi là Gmapping (G trong grid có nghĩa là lưới). Trong bài tập lần này chúng em không đề cập sâu tới thuật toán này mà chỉ đề cập tới khái niệm và cách sử dụng các package để tái tạo bản đồ trên nền ROS.

Hình 3.18: Tập hợp điểm cảm biến laser quét được

b) Gmapping và Laser Scan Matcher trong ROS:

Đối với gmapping thì nó yêu cầu một dữ liệu odometry và một cảm biến laser. Trong Ros thì để ứng dụng Gmapping ta sử dụng node slam_gmapping. Để sử dụng slam_gmapping ta cần cung cấp dữ liệu Odometry và dữ liệu mà

cảm biến laser publish lên topic /scan. Đầu ra của package này là một bản đồ 2D (nav_msgs/OccupancyGrid), có thể nói như là mặt sàn của môi trường đang dựng. Tương tự như các package khác, chúng ta cần thiết lập launch file cho gmapping. Trong đó, chúng ta có các thông số quan trọng sau:

map_udpate_interval là thời gian giữa hai lần cập nhật map tính bằng giây. delta là độ phân giải của map (đơn vị là mét trên một khối occupancy grid). xmin, xmax, ymin, ymax là quy định kích thước của bản đồ (đơn vị là mét).

 Xác định tọa độ của robot so với frame gốc ban đầu

Để tính được dữ liệu Odometry hay xác định vị trí của robot so với frame gốc ban đầu phục vụ cho build map nhóm sử dụng package có tên laser scan matcher để xác định tọa độ dựa vào các tín hiệu laser scan liên tiếp nhau. Hình dưới là map được tạo khi sử dụng gmapping và tf(chuyển đổi tọa độ) tĩnh không sử dụng đến laser scan matcher. Ta có thể thấy rằng nếu chỉ sử dụng gmapping, khi xoay lidar đi 1 góc thì map cũng bị di chuyển theo hướng của lidar. Laser scan matcher sẽ giải quyết vấn đề đó, đồng thời chúng có thể kết hợp với 1 số loại cảm biến khác như encoder, IMU để định vị vị trí tốt hơn cho robot trong môi trường. Tuy nhiên trong bài tập này, chúng em chỉ sử dụng 1 loại cảm biến duy nhất để lập bản đồ đó là lidar. Một số tham số cấu hình khi sử dụng laser scan matcher:

65 <param name="use_odom" value="false"/> <param name="use_imu" value="false"/> <param name="publish_pose" value="true"/> <param name="publish_tf" value="true"/>

Các thông số cần cấu hình cho laser_scan_matcher:

 Biến fixed_frame dùng để xác định đâu là frame cố định của hệ thống để package có thể tính được vị trí của base frame so với fixed_frame.

 Biến use_odom để xác định xem chúng ta có cung cấp nguồn dữ liệu khác là cảm biến đo lường odometry hay không.

 Biến use_imu giống như biến use_odom là xác định là chúng ta có đưa thêm cảm biến IMU vào hay không.

 Biến publish_pose, publish_tf dùng khai báo xem package này có publish các thông số tf và pose vào môi trường ROS hay không.

Để xây dựng được bản đồ ta cần di chuyển robot xung quanh map 1 lần để robot có thể quét được tất cả các vị trí. Chi tiết tham khảo thêm ở video đi kèm với báo cáo

3.3.1.2. Thuật toán định vị (localization):

a) Cơ sở lý thuyết:

Để thực hiện việc định vị cho robot ta sử dụng phương pháp định vị Monte Carlo thích nghi – AMCL (adaptive Monte Carlo localization).

Monte Carlo Localization (MCL) là một thuật toán định vị một robot bằng cách sử dụng một bộ lọc hạt. Thuật toán yêu cầu một bản đồ đã biết và nhiệm vụ là ước tính tư thế (vị trí và hướng) của robot trong bản đồ dựa trên chuyển động và cảm nhận của robot. Thuật toán bắt đầu với tin tưởng ban đầu về phân bố xác suất của tư thế robot, được biểu thị bằng các hạt được phân phối theo độ tin tưởng đó. Các hạt này được lan truyền theo mô hình chuyển động của robot mỗi khi tư thế của robot thay đổi. Khi nhận được các chỉ số cảm biến mới, mỗi hạt sẽ đánh giá độ chính xác của nó bằng cách kiểm tra khả năng nó sẽ nhận được các chỉ số cảm biến như vậy ở vị trí hiện tại của nó. Tiếp theo, thuật toán sẽ phân phối lại (lấy mẫu lại) các hạt thành các hạt có độ chính xác hơn. Tiếp tục lặp lại các bước chuyển động, cảm biến và lấy mẫu lại này và tất cả các hạt sẽ hội tụ về một cụm duy nhất gần tư thế thực của robot nếu quá trình định vị thành công.

AMCL là một biến thể của MCL. Trong đó AMCL tự động điều chỉnh số hạt dựa trên khoảng cách để đảm bảo rằng phân bố hạt hội tụ về phân bố thực của trạng thái robot dựa trên tất cả các phép đo chuyển động và cảm biến trong quá khứ với xác suất cao.

Hình 3.20: Ví dụ bản đồ đã có

AMCL xác định giới hạn dưới và giới hạn trên về số lượng các hạt sẽ được tạo ra trong quá trình lấy mẫu lại. Việc cho phép tạo ra nhiều hạt hơn có thể cải thiện cơ hội hội tụ về tư thế robot thực sự, nhưng có tác động đến tốc độ tính toán và các hạt có thể mất nhiều thời gian hơn hoặc thậm chí không thể hội tụ. Nếu robot biết vị trí ban đầu của nó, thông tin đó có thể giúp AMCL định vị robot nhanh hơn với số lượng hạt ít hơn, có thể sử dụng giá trị nhỏ hơn của giới hạn trên.

Trong trường hợp không có ước tính tư thế robot ban đầu, AMCL sẽ cố gắng định vị robot mà không cần biết vị trí ban đầu của robot. Thuật toán ban đầu giả định rằng robot có xác suất ngang nhau khi ở bất kỳ đâu trong không gian trống của khu vực và tạo ra các hạt phân bố đồng đều bên trong không gian đó. Lúc này sẽ AMCL đòi hỏi nhiều hạt hơn đáng kể so với định vị với ước tính đặt ra ban đầu.

Hình 3.21: Lần cập nhật đầu tiên của AMCL

Sau khoảng 8 lần cập nhật, các hạt bắt đầu hội tụ đến các khu vực có khả năng xảy ra cao hơn:

Hình 3.22: Lần cập nhật thứ 8 của AMCL

Sau 60 lần cập nhật, tất cả các hạt sẽ hội tụ đến đúng tư thế của robot và các lần quét laze phải căn chỉnh chặt chẽ với đường viền bản đồ:

Hình 3.23: Lần cập nhật thứ 60 của AMCl

b) Amcl trong ROS:

Amcl đưa vào một bản đồ dựa trên laser có sẵn đã được thực hiện ở phần mapping, quét laser và chuyển đổi thông báo, và kết quả đầu ra đưa ra các ước tính. Khi khởi động, amcl khởi tạo bộ lọc hạt của nó theo các thông số được cung cấp. Lưu ý rằng, do mặc định, nếu không có tham số nào được đặt, trạng thái bộ lọc ban đầu sẽ là một đám mây hạt (particlecloud) có kích thước vừa phải tập trung vào khoảng (0,0,0).

Các hạt sau khi được tọa từ dữ liệu cảm biến, sẽ được gán cho một trọng số nhất định, sau quá trình lấy mẫu lại, các hạt có trọng số cao hơn sẽ được giữ lại, các hạt có trọng số thấp hơn sẽ bị loại trừ, từ đó hạt có trọng số lớn nhất sẽ được chỉ định là vị trí hiện tại của robot. Quá trình này diễn ra liên tục mỗi khi có dữ liệu từ cảm biến trả về, robot vừa định vị bản thân, vừa cập nhật bản đồ vào cơ sở dữ liệu tại cuối quá trình.

Hình dưới biểu diễn quá trình lấy mẫu hạt khi robot đang trong quy trình thiết lập bản đồ, như trong hình, các hạt là các mũi tên đỏ sau quá trình lấy mẫu lại thì có xu hướng hội tụ về vị trí của robot trong thực tế, trong trường hợp

này, nếu robot xoay quanh tại chỗ càng lâu, thì các hạt sẽ càng hội tụ, từ đó cho ra một tọa độ chính xác nhất.

3.3.2. Lựa chọn thiết bị

3.3.2.1. Máy tính nhúng Raspberry PI

Raspberry Pi là một máy tính rất nhỏ gọn, kích thước hai cạnh như bằng khoảng một cái thẻ ATM và chạy hệ điều hành Linux. Có thể sử dụng Raspberry Pi như một máy vi tính bởi người ta đã tích hợp mọi thứ cần thiết trong đó. Bộ xử lí SoC Broadcom BCM2835 của nó bao gồm CPU, GPU, RAM, khe cắm thẻ microSD, Wi-Fi, Bluetooth và 4 cổng USB 2.0. Trong quá trình nghiên cứu làm đồ án đã chọn với Raspberry Pi 3 Model B những ưu điểm như công suất cao, tiêu thụ năng lượng thấp, bộ nhớ lớn, khả năng lập trình dễ dàng, tốc độ xử lý cao.

Hình 3.24: Raspberry Pi 3 Model B

Thông số kĩ thuật:

 CPU Quad Core 1.2GHz Broadcom BCM2837 64bit  RAM 1GB

 Mạng LAN không dây BCM43438 và Bluetooth năng lượng thấp (BLE) trên bo mạch

71  GPIO mở rộng 40 chân

 4 cổng USB 2

Đầu ra âm thanh nổi 4 cực và cổng video tổng hợp  HDMI kích thước đầy đủ

 Cổng hiển thị DSI để kết nối màn hình cảm ứng Raspberry Pi  Cổng Micro SD để tải hệ điều hành của bạn và lưu trữ dữ liệu  Được nâng cấp nguồn điện Micro USB chuyển mạch lên đến 2,5A Chức năng: Điều khiển toàn bộ hoạt động của mạch. Nhận tín hiệu từ Lidar và Arduino sau đó xử lí tín hiệu và đưa ra tín hiệu ra điều khiền động cơ, hiện thị trạng thái màn hình máy tính…

3.3.2.2. Cảm biến Radar RPLIDAR Model B

Máy quét laser 360 RPLIDAR là một giải pháp cho máy quét 2D 360 độ (LIDAR) chi phí thấp. Dữ liệu đám mây điểm 2D được tạo ra có thể được sử dụng trong lập bản đồ, định vị (SLAM) và mô hình hóa đối tượng / môi trường. RPLIDAR phát ra tín hiệu laser hồng ngoại được điều chế và tín hiệu laser sau đó được phản xạ bởi đối tượng sẽ được phát hiện. Tín hiệu trả về được lấy mẫu bằng cách thu nhận tầm nhìn trong RPLIDAR và giá trị khoảng cách đầu ra và giá trị góc giữa đối tượng và RPLIDAR.

Hình 3.25: Cảm biến radar RPLIDAR A1

Thông số kĩ thuật:

- Bộ phát triển máy quét laser 360 với chức năng quét laser đa hướng - Hệ thống quan sát tam giác laser tốc độ cao

- Cảm biến lý tưởng cho robot để định vị và lập bản đồ

- Tốc độ quét có thể định cấu hình của người dùng (tốc độ quay) qua tín hiệu PWM.

Các tính năng: Quét laser đa hướng, tốc độ quét có thể định cấu hình của người dùng thông qua tín hiệu PWM của động cơ, Plug Play sử dụng cáp USB đi kèm, Không yêu cầu mã hóa, sẵn sàng SLAM, 5,5hz (2000 mẫu / giây), phạm vi đo 6 mét, tránh chướng ngại vật, định vị và lập bản đồ, cảm biến điều hướng.

3.3.3. Thiết kế hệ thống định vị và lập bản đồ

3.3.3.1. Cài đặt hệ điều hành Robot (ROS)

Hiện nay có khá nhiều hệ điều hành dùng cho raspberry. Một số hệ điều hành thông dụng là: Raspbian, Ubuntu Mate, Snappy Core Ubuntu, Window 10 IoT Core, OSMC, OpenELEC, PiNet, RiscOS. Nhóm đã chọn sử dụng hệ điều hành Ubuntu với các ưu điểm: tương tự như Raspbian, Ubuntu cũng hướng đến người dùng sử dụng Raspberry Pi với chức năng không quá cao. Tuy nhiên Ubuntu có giao diện đẹp hơn rất nhiều so với Raspbian.

Hệ điều hành Robot (Robot Operating System - ROS) là một tập hợp các khung phần mềm để phát triển phần mềm robot, (cũng xem như phần trung gian của ngành robot) cung cấp chức năng giống như hệ điều hành trên cụm máy tính không đồng nhất. ROS cung cấp các dịch vụ hệ điều hành tiêu chuẩn thực hiện chức năng thường dùng, truyền thông giữa các quy trình và quản lý gói. Các gói này thực hiện các chức năng và ứng dụng thường được sử dụng như trình bộ truyền động phần cứng, mô hình robot, kiểu dữ liệu, lập kế hoạch, nhận thức, định vị và lập bản đồ đồng thời, các công cụ mô phỏng và các thuật toán khác.

Trước khi cài ROS, ta cần cấu hình vùng lưu trữ cho Ubuntu: lựu chọn cấu hình vùng lưu trữ Ubuntu để cho phép “restricted”, “universe” và

“multiverse”. Đối với cài đặt mặc đinh thông thường thì các phần này đã được cho phép. Có thể kiểm tra lại bằng các vào Software & Update.

Hình 3.26: Lựa chọn cấu hình cho Ubuntu

Sau đó ta mở cửa số Terminal trên Ubuntu và thực hiện các lệnh sau - Cài đặt sources.list

sudo sh -c 'echo "deb http://packages.ros.org/ros/ubuntu $(lsb_release -sc) mai n" > /etc/apt/sources.list.d/ros-latest.list'

sudo apt install curl

curl -s https://raw.githubusercontent.com/ros/rosdistro/master/ros. asc | sudo apt-key add -

- Cài đặt ROS (với nhiệm vụ lập bản đồ và điều khiển robot, nhóm chọn cài đạt gói ros melodic desktop)

sudo apt update

sudo apt install ros-melodic-desktop

- Thiết lập môi trường

echo "source /opt/ros/melodic/setup.bash" >> ~/.bashrc source ~/.bashrc

- Xây dựng packages

sudo apt install python-rosdep python-rosinstall python-rosinstall- generator python-wstool build-essential

- Kích hoạt rosdep

sudo apt install python-rosdep sudo rosdep init

rosdep update

Phương pháp cài đặt trên dựa theo hướng dẫn của trang chủ: http://wiki.ros.org/melodic/Installation/Ubuntu

3.3.3.2. Kết nối Terminal Raspberry với Máy chủ

Raspberry là một máy tính nhúng nên xử lý lệnh chậm, đồng thời raspberry sẽ được gắn lên xe tự hành di chuyển không, nên ta cần kết nối Raspberry với máy chủ để điều khiển, lập trình qua terminal. Thao tác này giúp việc điều khiển thuận tiện và dễ dàng hơn. Kế nối này được thực hiện qua giao thức SSH

Secure Shell (SSH) là giao thức và giao thức lệnh dựa trên UNIX để truy cập một cách an toàn vào máy tính từ xa. SSH thực sự là một bộ ba tiện ích - slogin, ssh và scp - là các phiên bản an toàn của các tiện ích UNIX trước đó,

rlogin, rsh và rcp. Các lệnh SSH được mã hóa và bảo mật theo một số cách. Cả hai đầu của kết nối máy khách / máy chủ đều được xác thực bằng chứng chỉ kỹ thuật số và mật khẩu được bảo vệ bằng cách mã hóa.

Nói tóm lại, SSH cho phép bạn kết nối với máy chủ của mình một cách an toàn để thực hiện các hoạt động dòng lệnh Linux. Và mặc dù bạn sẽ chạy các lệnh Linux, SSH vẫn có thể được thực hiện từ máy tính Windows, Mac hoặc Linux.

Để kết nối SSH, máy tính nhúng và máy chủ phải sử dụng chung một mạng nội bộ. Mạng nội bộ này cần kiểm soát được địa chỉ IP của các máy tính

Phương pháp tiếp cận SLAM Hector

Cấu trúc của bản đồ ảo