đề tài mobile robot tránh vật cản

Điểm mới của mobile robot là sử dụng cảm biến Kinect có khả năng khôi phục môi trường phía trước robot dưới dạng 3D từ đó xác định được các thông số cần thiết cho giải thuật điều khiển r

Mục đích

Xây dựng một mobile robot hoàn chỉnh có khả năng tìm đến vị trí đặt trước và có khả năng tránh vật cản trong quá trình di chuyển Điểm mới của mobile robot là sử dụng cảm biến Kinect có khả năng khôi phục môi trường phía trước robot dưới dạng 3D từ đó xác định được các thông số cần thiết cho giải thuật điều khiển robot Đề tài cũng đi vào nghiên cứu xây dựng mô hình toán học cho robot di động sử dụng bánh đa hướng Omni Thiết kế bộ điều khiển bám quỹ đạo và đồng thời nghiên cứu công nghệ xử lý ảnh 3D thông qua cảm biến Kinect và bộ thư viện PCL nhằm phục vụ giải quyết bài toán tránh vật cản

CHƯƠNG 2 TỔNG QUAN HỆ THỐNG

Giới thiệu về mobile robot

Mobile robots (hay còn gọi là mobile robot), thường được gọi tắt là mobots được định nghĩa là một loại xe robot có khả năng tự dịch chuyển, tự vận động (có thể lập trình lại được) dưới sự điền khiển tự động để thực hiện thành công công việc được giao

Theo lý thuyết, môi trường hoạt động của robot tự hành có thể là đất, nước, không khí, không gian vũ trụ hay sự tổ hợp giữa chúng Địa hình bề mặt mà robot di chuyển trên đó có thể bằng phẳng hoặc thay đổi, lồi lõm

Theo bộ phận thực hiện chuyển động, ta có thểchia robot tựhành làm 2 lớp: chuyển động bằng chân (legged) và bằng bánh (wheeled)

Hình 2-1 Asterisk - Robot nhện di chuyển bằng chân

Hình 2-2 Curiosity- Robot thăm dò sao Hỏa di chuyển bằng bánh

Loại chuyển động bằng vòng xích (khi cần mô men phát động lớn hay khi cần di chuyển trên vùng đầm lầy, cát và băng tuyết)

Hình 2-3 Robot cứu hộ di chuyển bằng xích

Bài toán tránh vật cản

Tìm đường (navigation) là một khoa học dẫn hướng robot tự hành di chuyển trong không gian làm việc của nó Trong vấn đề tìm đường, bài toán được quan tâm nhiều nhất là bài toán tìm đường về đích mà không chạm vào vật cản trên đường đi

Có 2 loại bài toán tìm đường cho robot:

- Bài toán cục bộ (local): Môi trường làm việc của robot hoàn toàn không biết trước hoặc chỉ biết một phần Robot hoàn toàn nhờ vào sự cảm nhận môi trường trong quá trình di chuyển thông qua cảm biến gắn trên nó để dò đường

- Bài toán toàn cục (Global): Bản đồ môi trường làm việc của robot hoàn toàn được biết trước, vấn đề chính phải giải quyết là tìm đường đi cho robot trước khi nó xuất phát

Bài toán toàn cục có lợi thế là biết trước đường đi có tới đích hay không, có thể chọn đường tối ưu trước khi robot khởi hành Tuy nhiên hạn chế là đòi hỏi nhiều tài nguyên để tính toán và yêu cầu cao về độ chính xác của bản đồ môi trường làm việc Việc thay đổi môi trường ví dụ như có vật cản thay phía trước robot khiến robot không hoạt động đúng yêu cầu

Trong trường hợp này, bài toán cục bộ tỏ ra có ưu thế, nó có tính linh hoạt cao như có thể tránh vẩn cản khi vật đó di chuyển Đồng thời yêu cầu tài nguyên phục vụ tính toán thấp hơn Nhược điểm của bài toán này là robot chỉ cảm nhận môi trường qua cảm biến gắn cùng vì thế có thể không hoàn thành việc tới đích mặc dù có đường đi hơn nữa không thể chọn ra được đường đi tối ưu

Trong đồ án này, nhóm sinh viên tập trung vào giải quyết bài toán cục bộ với cảm biến sử dụng là Kinect Môi trường hoạt động của robot là mặt phẳng giới hạn bởi các bức tường Các vật cản xuất hiện ngẫu nhiên trong phạm vi hoạt động của robot Yêu cầu đặt ra là robot tự động tìm đường về được tới đích mà không chạm vào vật cản

Hệ thống mobile robot tránh vật cản

Hệ thống tổng quan bao gồm 4 bộ phận chính (Hình 2-4) (1) Bộ cảm biến Kinect, (2) bộ phận chấp hành là các bánh xe có gắn động cơ điện 1 chiều, (3) Laptop,(4) vi điều khiển và driver điều khiển động cơ

Các thành phần phối hợp hoạt động như sau: (1) Cảm biến Kinect liên tục thu thập dữ liệu về hình ảnh, khoảng cách sau đó gửi về (2) Laptop để có thể khôi phục lại không gian 3D trước robot đồng thời sử dụng các thuật toán thích hợp để phát hiện vật cản cũng như tìm được các thông số của vật cản để cung cấp cho thuật toán tránh vật cản.Sau khi các tín hiệu được xử lý thì sẽ tìm ra được quỹ đạo đặt tiếp theo cho robot Quỹ đạo là đầu vào cho bộ điều khiển cài đặt trong vi điều khiển và sau đó chuyển thành các tín hiệu điều khiển động cơ DC Khi di chuyển Kinect lại thu thập các dữ liệu mới và quá trình này được lặp lại

Hình 2-4 Sơ đồ tổng quan hệ thống

CHƯƠNG 3: MOBILE ROBOT SỬ DỤNG BÁNH OMNI

Bánh Omni

Bánh xe Omni còn có tên gọi là bánh xe Mecanum, kiểu bánh xe này được phát triển và đăng kí bản quyền bởi công ty Mecanum của Thụy Sỹ vào năm 1973

Bánh xe omni nó cho phép robot di chuyển ngay đến một vị trí trên mặt phẳng mà không phải quay trước Hơn nữa, chuyển động tịnh tiến dọc theo một quỹ đạo thẳng có thể kết hợp với chuyển động quay làm cho robot di chuyển tới vị trí mong muốn với góc định hướng chính xác Hình 3.1 Bánh Omni

Hầu hết các bánh xe omni đều dựa trên cùng một nguyên lý: trong khi bánh xe cung cấp một lực kéo theo hướng bình thường trên trục của động cơ, nó có thể trượt không ma sát theo 1 hướng khác so trục của động cơ (chuyển động phụ) Để đạt được điều này, bánh xe được chế tạo bằng cách thêm các bánh xe nhỏ dọc theo chu vi của bánh chính

Có hai lọai bánh Omni chính, phụ thuộc vào hướng chuyển động khả thi của bánh :

Hình 3.2 Bánh Omni với hướng chuyển động phụ 45 0 (a) và 90 0 (b) so với hướng chính

11 Như vậy, robot sử dụng bánh xe omni có thể chuyển động theo bất cứ hướng nào ngay lập tức, điều này làm robot có tính linh hoạt rất cao, thích hợp để tránh vật cản.

Mô hình toán học robot tự hành sử dụng bánh Omni

Động học

Hình 3.4 Phân tích lực tác dụng lên Mobile Robot sử dụng 4 bánh Omni

Gọi hệ trục tọa độ O x y w w w là cố định và O x y m m m là hệ trục gắn với trọng tâm robot Đặt:

F F F = F F là lực kéo do động cơ sinh ra tác dụng các bánh robot

F = F F T  là tổng hợp lực tác dụng lên robot trong hệ O x y m m m w w w w

F =   F F T   là tổng hợp lực tác dụng lên robot trong hệ O x y w w w

T z là momen của tác dụng lên robot quanh trục z đi qua O m vuông góc với mặt phẳng m m m

12 Tọa độ của robot trong 2 hệ trục tọa độ O x y w w w ,O x y m m m và quãng đường đi được của các bánh lần lượt là:

Từ sơ đồ phân tích lực như trong hình vẽ ta có:

Kí hiệu B T  − 1 là phép toán cho kết quả là ma trận thỏa mãn khi nhân với B T sẽ được ma trận đơn vị

Với B ở trên, ta có ma trận 1

Áp dụng nguyên lý công ảo:

13 Gọi v m =( ,v v v v 1 2 , , 3 4 ) T là vận tốc dài của từng bánh ta có:

(4) Với R m w là ma trận chuyển từ hệ O x y m m m sang hệ O x y w w w w os sin 0 sin os 0

Như vậy ta thu được phương trình động học thuận w w 1

Từ (5) và áp dụng tính chất (R m w ) − 1 =(R m w ) T ta thu được phương trình động học ngược

Động lực học

Quan hệ giữa vận tốc và lực tác dụng trên các hệ trục tọa độ: w w

= (7) Áp dụng định luật 2 Newton

F = X (8) Với M là ma trận quán tính

M R R X +X =F (9) Đơn giản hóa phương trình (9) ta có:

+ (10) Đặt  m =    1 , 2 , 3 , 4  T là vận tốc góc của các bánh xe Kết hợp với (3) ta có:

  Với r là bán kính bánh xe

Gọi c là hệ số ma sát động của bánh xe Ta có động học của mỗi bánh là:

I  +c =K −r Từ các phương trình () – () ta thu được mô hình động lực học:

Mô phỏng

Mô hình động học thuận:

Hình 3.5 Khối Simulink động học thuận

15 Hoàn toàn tương tự ta, ta khai triển biểu thức w w 1

X =R X =R   B − v m và xây dựng được mô hình động học ngược như hình bên

Hình 3.6 Khối Simulink động học ngược

CHƯƠNG 4 CẢM BIẾN KINECT VÀ ỨNG DỤNG TRONG BÀI TOÁN MOBILE ROBOT TRÁNH VẬT CẢN.

Mục đích

Mục đích của việc sử dụng cảm biến Kinect trong đề tài là dựa vào khả năng khôi phục môi trường trước robot tự hành dưới dạng 3D để phát hiện đồng thời tính toán các tham số của vật cản như khoảng cách từ robot đến vật cản, kích thước vật cản…

Việc phát hiện vật cản sử dụng Kinect được xem là sự kết hợp giữa xử lý ảnh và và phương pháp đo đạc khoảng cách bằng sóng hồng ngoại Kết quả thu về chính xác, ổn định hơn so với các phương pháp phổ biến như sử dụng cảm biến siêu âm hay sử dụng xử lý ảnh thông thường.

Giới thiệu cảm biến Kinect

Cấu trúc phần cứng

Hình 4-2 Cấu trúc phần cứng của cảm biến kinect

Bên trong Kinect bao gồm 1 camera RGB, cảm biến độ sâu , một dãy các microphone và 1 động cơ điều khiển góc nâng

Là một camera có 3 kênh dữ liệu có độ phân giải 1280x960 Camera này có khả năng chụp lại ảnh ảnh mầu

Cảm biến độ sâu Độ sâu thu về nhờ sự kết hợp của 2 bộ phận là bộ phát hồng ngoại IR và camera hồng ngoại đọc các tín hiệu phản hồi về từ đó tính toán ra bản đồ độ sâu

Dãy Micro bao gồm 4 micro được bố trí dọc theo thân Kinect có khả năng thu lại âm thanh đồng thời xác định hướng của âm thanh Dãy Microphone này được dùng trong các ứng dụng điều khiển bằng giọng nói

Ngoài ra Kinect còn có 1 cảm biến đo gia tốc để xác định hướng và 1 động cơ dùng để điều khiển góc nghiêngcamera

Trong số những cảm biến kể trên của Kinect, cảm biến độ sâu có khả năng ứng dụng cao trong đề tài robot tránh vật cản.

Nguyên lý cảm biến độ sâu

Cặp cảm biến IR camera và IR projector sẽ phối hợp với nhau để tạo ra giá trị độ sâu bằng công nghệ Light Coding của PrimeSense

Kĩ thuật Light Coding dùng nguồn sáng hồng ngoại chiếu liên tục kết hợp với một camera hồng ngoại để tính khoảng cách Việc tính toán này được thực hiện bằng chip PS1080 Soc của PrimeSen

Projector sẽ chiếu một chùm sáng hồng ngoại, tạo nên những đốm sáng ở không gian phía trước Kinect, tập hợp đốm sáng được phát ra này là cố định Những đốm sáng này được tạo ra nhờ một nguồn sáng truyền qua lưới nhiễu xạ (diffraction gratings) Tập hợp các đốm sáng này được IR camera chụp lại, thông qua giải thuật đặc biệt được tích hợp trong PS1080 SoC cho ra bản đồ độ sâu Bản chất của giải thuật này là các phép toán hình học dựa trên quan hệ giữa hai cảm biến IR camera và Projector

Hình 4-3 Nguyên lý cảm biến độ sâu

Các dữ liệu đo từ cảm biến

Hình 4-4 Dữ liệu thu được từ cảm biến Kinect

21 Các cảm biến của Kinect được điều khiển đồng thời thu thập và xử lý dữ liệu thông qua chip PS1080 có tần số 12MHz, sau đó được lưu trữ vào bộ nhớ Flash Các dữ liệu này có thể truyền vào máy tính thông qua cổng USB 2.0

Các tín hiệu thu thập bao gồm dữ liệu về độ sâu, màu sắc và âm thanh trong đó tín hiệu về độ sâu là dữ liệu quan trọng có nhiều công dụng

Sở dĩ dữ liệu về chiều độ sâu có tầm quan trọng như vậy bởi nó giúp việc nhận dạng các vật thể đơn giản hơn nhiều so với xử lý ảnh thông thường Các thuật toán xử lý ảnh thông thường dựa vào sự tương đồng về mầu sắc, tuy nhiên, có thể những vật có mầu sắc tương tự nhau nhưng không cùng một vật thể hoặc các phần của cùng một đối tượng nhưng có mầu khác nhau,do vậy gây khó khăn trong quá trình nhận dạng Trong khi đó, với thông tin về độ sâu, các vật thể được phân biệt với nhau thông qua vị trí Những điểm có khoảng cách gần nhau có xu hướng cùng một đối tượng mà không phân biệt mầu sắc Chỉ khi độ sâu giảm đột ngột như ở cạnh và ở một số phần nhỏ của đối tượng thì khi đó, hình ảnh trên bản đồ độ sâu mới có sự thay đổi

Một ưu điểm nữa của bản đồ độ sâu đó là dữ liệu có thể được nén cao hơn so với ảnh mầu thông thường do đó thích hợp trong việc truyền dẫn nhanh tín hiệu Các thuật toán nhận dạng đối với ảnh độ sâu đơn giản hơn và thậm chí có thể tái tạo lại vật thể 3D (Bộ thư viện mã nguồn mở hỗ trợ thuật toán trên là PCL)

Từ những phân tích trên ta có thể thấy được những ưu điểm của bản đồ độ sâu và nó rất thích hợp để ứng dụng trong các đề tài về robot tự hành.

Các thư viện xử lý ảnh cho Kinect

Hiện nay có nhiều bộ thư viện được viết cho Kinect Nổi bật trong số đó là 2 bộ thư viện mã nguồn mở OpenNI và bộ thư viện Kinect SDK của Microsoft

Trong đồ án này, nhóm sinh viên đã sử dụng thư viện OpenNI Đây là bộ thư viện mã nguồn mở, dùng được trên nhiều hệ điều hành khác nhau Thư viện được xây dựng hỗ trợ đầy đủ các nhu cầu cơ bản khi sử dụng Kinect trong đồ án mobile robot tránh vật cản

22 Mục đính chính khi sử dụng bộ thư viện OpenNI là thu được bản đồ độ sâu (Depth Map) mang thông tin về khoảng cách từ các vật thể trong không gian phía trước cảm biến Kinect.OpenNI cho phép thu bản đồ độ sâu trong giới hạn từ 0.5-5m phía trước Kinect

Hình 4-5 Bản đồ độ sâu thu được từ Kinect sử dụng OpenNI Để phát huy tối đa khả năng của Kinect, nhóm sinh viên kết hợp thêm bộ thư viện xử lý ảnh 3D là PCL

PCL là thư viện hỗ trợ xử lý ảnh 3D, được xây dựng với 14 module thực hiện các thuật toán khác nhau

- Module PCL_Common: Chứa cấu trúc dữ liệu và phương thức được sử dụng bởi phần lớn các thư viện trong PCL

- Module Features : Chứa các cấu trúc dữ liệu và cơ chế tính toán, ước lượng 3D từ các dữ liệu điểm PCD - Module PCL_filters : Chứa các kỹ thuật loại bỏ nhiễu

- Module Geometry: Chứa tất cả các cấu trúc dữ liệu và giải thuật để tính toán hình học

- Module PCL_IO : Chứa các hàm và các lớp để đọc và ghi dữ liệu dạng PCD, có thể thu thập dữ liệu từ nhiều nguồn khác nhau (Trong đồ án này dùng Kinect)

- Module PCL_Kdtree: Thư viện cung cấp cấu trúc dữ liệu Kd_tree, sử dụng

FLANN giúp nhanh chóng tìm kiếm vùng gần nhất (nearest neighbors searches)

23 - Module PCL_Keypoint : Là thư viện chứa thực thi của 2 thuật toán nhận dạng

“Point clound keypoint” Key Point (hay interest point) là các điểm trong ảnh hoặc trong point cloud mà có tính chất ổn định, riêng biệt và có thể dễ dàng phát hiện ra Thông thường số lượng Key Point nhỏ hơn tổng số điểm trong cloud

- PCL_Octree: Chứa các thuật toán hiệu quả để tạo nên một cấu trúc dữ liệu phân cấp từ dữ liệu point cloud Nó cho phép phân vùng không gian, downsampling (giảm số lượng các mẫu do đó tăng tốc độ tính toán) và thực hiện các phép toán tìm kiếm trong tập dữ liệu PointCloud

- Module registrantion (PCL_registration): Kết hợp các bộ dữ liệu vào một mô hình chung, thống nhất thường được thực hiện bằng một kỹ thuật gọi là registration.Ý tưởng chính là xác định các điểm tương ứng trong bộ dữ liệu và tìm một chuyển đổi khoảng cách tối thiểu các điểm tương ứng

- Module PCL_sample_consensus :Thư viện pcl_sample_consensus có khả năng tách các nhóm điểm có cùng tính chất (Sample Consensus hay SAC) giống như thuật toán RANSAC (Tìm kiếm đường thẳng trong tập hợp các điểm) Các nhóm điểm có thể là các mặt phẳng, mặt cầu, trụ Thư viện này rất thích hợp trong các ứng dụng dò tìm các đối tượng như tường, cửa, các vật trên bàn…

- PCL_Search: Cung cấp các phương pháp tìm kiếm lân cận (nearest neighbors) bằng cách sử dụng các cấu trúc dữ liệu khác nhau

- Module PCL_Segmentation : Chứa các thuật toán để phân chia Point Cloud thành các nhóm riêng biệt Các thuật toán này thích hợp nhất khi xử lý các point Cloud bao gồm các vùng không gian bị cô lập Trong trường hợp như vậy, các clustering thường chia nhỏ để sau đó có thể xử lý độc lập

- Module PCL_surface : Là thư viện thích hợp cho việc xây dựng lại các bề mặt từ dữ liệu quét 3D Các đối tượng chính gồm vỏ, bề mặt lưới, bề mặt nhẵn hay bình thường Khi có nhiễu có thể làm mịn và lấy mẫu lại

- Module PCL_visualization : Thư viện được tạo ra có thể nhanh chóng hiển thị các kết quả thuật toán trên dữ liệu 3D

Hình 4.6 Ví dụ ứng dụng thư viện PCL trong việc tách các đối tượng trên mặt bàn

Kiểu dữ liệu cơ bản trong PCL là PointCloud

PointCloud là 1 lớp C++ bao gồm:

• Width (int): Xác định chiều dài tập dữ liệu bằng số lượng điểm “Width” có 2 nghĩa là o Có thể xác định tổng số các điểm trong cloud (bằng số lượng các phần tử trong cloud) cho bộ dữ liệu có tổ chức o Có thể xác định chiều rộng (tổng số điểm liên tiếp) của một tập dữ liệu có tổ chức

Chú ý: Tập dữ liệu điểm có tổ chức là tập dữ liệu được chia thành các hàng và cột giống như ma trận

Vd: cloud.widthd0;// Tao ra 640 diem tren mot dong

• Height (int): Tương tự width nhưng đối với cột trong ma trận điểm

Nếu hieght=1 thì dữ liệu không được tổ chức (có thể dùng tính chất này để kiểm tra một tập dữ liệu có được tổ chức hay không)

25 Vd: cloud.width = 640; // Khai bao mot anh co cau truc, gom 640 dong va 480 cot cloud.height = 480; // tong so diem anh la 640*48007200

• Points (std::vector) Chứa các mảng dữ liệu lưu trữ tất cả các điểm có kiểu pointT

Kiểu PointT có thể là - pcl::PointXYZ Một cấu trúc chứa thông tin về 3 trục tọa độ X,Y,Z của điểm cần biểu diễn - pcl::PointXYZRGB Chứa thông tin vệ tọa độ và mầu sắc dạng RGB của điểm cần biểu diễn

- pcl::PointXYZRGBA Chứa thông tin vệ tọa độ và mầu sắc dạng RGB và mật độ của điểm cần biểu diễn

Ví dụ: pcl::PointCloud cloud; std::vector data = cloud.points;

• Is_dense(bool) Trả về giá trị logic, nếu tất cả giá trị trong points hữu hạn => True ngược lại là False

Ngoài ra lớp pointCloud còn chứa các thành phần chứa các tùy chọn của sensor như Sensor_origin, sensor_orientation Các thành phần này thường ít dùng trong các thuật toán của PCL

Phát hiện vật cản sử dụng Kinect

Thu thập dữ liệu từ Kinect

Để đọc dữ liệu thu được từ Kinect, chúng ta có thể sử dụng OpenNIGrabber, đây là một lớp có sử dụng thư viện OpenNI có sẵn trong PCL

Các bước thực hiện như sau:

Bước 1: Khai báo #include

Bước 2: Tạo một Grabber mới: pcl::Grabber* interface =new pcl::OpenNIGrabber();

Bước 3: Tạo callback function: boost::function f boost::bind (&SimpleOpenNIProcessor::cloud_cb_,this, _1);

// cloud_cb_ là hàm dùng để xử lý dữ liệu nhận về, ở ví dụ này cloub_cb_ để hiển thị //ra màn hình. Ở các lệnh trên, dữ liệu ta thu về từ Kinect có định dạng PointXYZRGBA, có thể sửa lại lệnh để thu về các loại dữ liệu khác Cụ thể như sau:

Dữ liệu thu về làđiểm ảnh mầu 3D pointXYZRGB: void (const boost::shared_ptr&)

- Dữ liệu thu về làđiểm ảnh không mầu 3D pointXYZ: void (const boost::shared_ptr&)

- Dữ liệu thu về làảnh RGB từ camera : void (const boost::shared_ptr< openNI_wrapper::Image>&)

- Dữ liệu thu về là bản đồđộ sâu: void (const boost::shared_ptr< openNI_wrapper::DepthImage>&)

Bước 4: Kết nối hàm callback với dữ liệu cần quan tâm và nhận dữ liệu, dừng dữ liệu: interface->registerCallback(f); interface->start();

Voxel grid

Nhằm tăng tốc độ xử lý, ta có thể đưa dữ liệu qua bộ lọc voxel Grid để giảm số lượng điểm ảnh và giới hạn vùng lấy dữ liệu phía trước cảm biến

Tập hợp các điểm gần nhau sẽ chỉ lấy một điểm đại diện

Các hàm PCL cung cấp cho bộ lọc bao gồm:

- pcl::VoxelGrid grid_;Khai báo một bộ lọc VoxtelGrid có tên là grid_, PointType là các kiểu biểu diễn điểm như PointXYZ, PointXYZRGB…

- grid_.setLeafSize (leaf_size_x, leaf_size_y, leaf_size_z);Chọn mật độ điểm theo các chiều lần lượt là X,Y,Z Đơn vị là Met.

- grid_.setFilterFieldName (field_name); : Chiều cần giới hạn ví dụ giới hạn theo chiều Z

- grid_.setFilterLimits (min_v, max_v);Thiết lập khoảng giới hạn hoảng giới hạn

Thử nghiệm với các thông số double min_v = 0.5, max_v = 1.5; std::string field_name ("z"); double leaf_x = 0.02, leaf_y = 0.02, leaf_z = 0.02;

28 Ta thu được kết quả sau:

Hình 4-8 Kết quả thu được khi sử dụng qua bộ lọc voxel Grid

Plannar segmentation

Plannar segmentation sẽ tách các point cloud có cấu trúc phẳng, sau đó tách ra point cloud có tổng số điểm lớn nhất, bằng giải thuật RANSAC (RANdom SAmple Consensus).

Euclidean cluster extraction

Euclidean cluster extraction làm công việc tách các point cloud có mặt trên nền nhà, tập hợp các điểm gần nhau sẽ được nhóm lại thành một point cloud hay cluster, mỗi cluster đại diện một vật thể

Như vậy dữ liệu sau khi nhận về từ Kinect sẽ được lọc qua bộ lọc Voxel Grid để giảm mật độ điểm ảnh nhằm tăng tốc độ xử lý.Sau khi lọc dữ liệu, ta sẽ tiến hành phân tích các dữ liệu về vật thể có thể tách ra thành các nhóm điểm đại diện cho vật thể (Object Clusters) Quá trình này sử dụng các công cụ Plannar Segmentation, Euclidean Cluster

Extraction Mỗi nhóm điểm đại diện cho một vật cản và cung cấp thông tin giúp nhận dạng vật cản, tính toán kích thước, khoảng cách đến vật cản để có thể tính toán tránh vật cản

29 Kết hợp các module lại với nhau ta thu được kết quả:

Hình 4-9 Kết quả phát hiện vật cản sử dụng thư viện PCL

Trong đó, các điểm ảnh được phân chia thành 3 phần Nhóm mầu xanh dương đại diện cho mặt sàn Nhóm mầu đỏ là nhóm ảnh đại diện cho vật thể gần Kinect nhất Nhóm xanh lá cây đại diện cho các vật thể còn lại

Riêng đối với nhóm gần nhất, ta xấp xỉ vật thể thành một hình hộp chữ nhật bao quanh với các thông số hoàn toàn xác định Từ đó có thể coi như nhận dạng vật thể đủ yêu cầu cung cấp cho thuật toán tránh vật cản.

Thuật toán tránh vật cản

Quay về đích

Giả sử robot đang ở vị trí có tọa độ O’( ,x y t t ) và góc so với trục ox là  Tọa độ đích là T( ,x y t t ) Khi đó cần quay robot một góc  để hướng robot về đích

Ta thấy rằng, góc  chính là góc giữa vector chỉ hướng của xe u và vector v=TO' Trong đó

Hình 1-11 Quay robot về đích

−Chiều quay xác định bằng vị trí tương đối của điểm đích T và đường thẳng d chứa vector u Đường thẳng d chia mặt phẳng thành 2 phần là phần trên và phần dưới

31 Xét trường hợp T nằm ở nửa mặt phẳng dưới khi đó

    : Quay robot sang phải (hình 2)

Tính toán tương tự khi T nằm ở mặt phẳng trên

Xử lý khi gặp vật cản

Vùng không gian thu được phía trước Robot (trong hệ tọa độ O’x’y’ gắn với robot) sau khi xử lý bằng thư viện PointCloud được biểu diễn như hình 3

Vùng không gian trên được chia thành 3 phần: Trái (T), Giữa (G) và Phải (P) Ranh giới giữa các vùng là các đường thẳng tiếp tuyến với đường tròn bao quanh robot và vuông góc với trục o’x’ như 3

Hình 4-11 Vùng không gian trước Robot

Vị trí tương đối của vật cản so với các vùng T, P, G bao gồm:

- Vật cản nằm hoàn toàn ở vùng T hoặc P

- Vật cản nằm giữa vùng T và G

- Vật cản nằm hoàn toàn ở G

- Vật cản nằm giữa G và P

• Trường hợp vật cản nằm hoàn toàn ở vùng T hoặc P:

Robot đi thẳng do đảm bảo điều kiện không va chạm với vật cản

• Trường hợp vật cản nằm giữa vùng T và G

Khi đó x min  −R và − R x max R

Robot sẽ quay sang phải một góc  để tránh vật cản Để đảm bảo  là nhỏ nhất, từ điểm M thuộc vật cản

Dựng tiếp tuyến MN với đường tròn an toàn Khi đó góc  0 − với arctan( m n ) (4.2) m n y x y

= − Do N( ,x y n n ) thuộc đường tròn tâm O’ bán kính R nên

Hình 4-12 Vật cản nằm giữa vùng T và G

Mặt khác ON ⊥ MN suy ra

Giải hệ (4.3) và (4.4) ta được:

Do quay về bên trái nên ta chọn nghiệm có giá trị x n nhỏ hơn

• Trường hợp vật cản nằm giữa vùng P và G

34 Khi đó ta có − R x min R và x max R Tính toán hoàn toàn tương tự trường hợp nằm ở vùng T và G ta thu được:

Hình 4-13 Vật cản nằm giữa vùng T và G

• Trường hợp vật cản nằm hoàn toàn trong vùng G Khi đó−Rx min x ax m R

Dưạ vào điểm giữa vật cản nằm có x>0 hay x