Báo cáo thực tập Xe tự hành tránh vật cản

uay của xe. Và có: 2 2 w 2 2 t p r r Vv Vr r D D                       (3.2) .t tV r (3.3) .p pV r (3.4) Trong đó, r là bán ính bánh xe, D là hoảng cách giữa 2 bánh xe. Để đơn giản,trong phạm vi đồ án ta chỉ quan tâm đến 2 chuyển động cơ bản của xe robot: - Chuyển động tịnh tiến (w=0, v≠0) :Vt =Vp xe robot chuyển động tiến hoặc lùi. - Chuyển động quay (w≠0, v =0 ):Vt = -Vp xe robot chuyển động quay quanh trục đi qua trung điểm 2 bánh xe A. Kết hợp (3.1) và (3.2), ta được: os( ) os( ) 2 2 r r sin( ) sin( ) 2 2 2r 2r D D t p r r c c x V y V                                   (3.5) 3.2 Mô hình động lực học xe robot: Xe Robot thuộc lớp đối tượng “mechanical nonholonomic systems”, theo Euler- Lagrange có phương trình dạng : Báo cáo thực tập tốt nghiệp 12 ( ). ( , ). ( ) ( ). ( ).TM q q C q q q G q B q J q     (3.6) Trong đó: - ( )M q là ma trận đối xứng, xác định dương - ( , )C q q là ma trận hướng tâm và lực coriolis - ( )G q là ma trận vector lực quán tính - ( )B q là vector biến đổi tín hiệu vào -  là ma trận tín hiệu vào - ( )J q là ma trận liên ết -  là ma trận của lực liên ết Thực tế với đối tượng Robot di động : ( , ) 0C q q  , ( ) 0G q  , ( ). 0J q q  (3.7) Từ đó, hệ phương trình động lực học của đối tượng Robot di động được viết lại như sau, với ( , , ) Tq x y  : 1 2 0 0 os( ) os( ) sin( ) 1 0 0 sin( ) sin( ) os( ) 0 0 D 0 m x c c m y c r I D                                                      (3.8) Đặt: 1 2 1 ( )l R     ; 1 2 1 ( )a R     Trong đó: m là hối lượng của xe, I là momen quán tính của xe Hệ phương trình (3.8) trở thành : os( )+ sin( ) m lx c m     os( )- sin( ) m ly c m     (3.9) a I    Báo cáo thực tập tốt nghiệp 13 Đạo hàm 2 vế của các phương trình hệ động học (3.1), ta được .sin( ) . os( )x v v c     . os( ) .sin ( )y v c v    (3.10) w  So sánh các vế của các phương trình (3.9), (3.10): lv m   (3.11) aw = I  Bộ điều khiển được thiết kế sẽ thực hiện 2 nhiệm vụ : - Bám vận tốc đặt trước vc : bộ điều khiển sẽ sử dụng mô hình động lực học của xe robot. - Bám quỹ đạo đặt trước qd : bộ điều khiển sẽ sử dụng mô hình động học của xe robot 3.3 Đánh giá : - Trong thời gian thực tập, chúng em đã thu được một số kết quả như sau:  Tìm hiểu được cơ bản về lý thuyết xe robot,tìm hiểu,xây dựng được mô hình động học,mô hình động lực học cho xe robot.  Tìm hiểu được về nguyên lý điều khiển xe robot, bước đầu xây dựng được cấu trúc hệ thống điều khiển xe robot. - Trong thời gian hoàn thành đồ án, chúng em sẽ hoàn thiện hệ thống điều khiển, cài đặt bộ điều khiển với thuật toán “bám mục tiêu + tránh vật cản” chạy thử nghiệm trên xe robot. Báo cáo thực tập tốt nghiệp 14 CHƢƠNG 4 CẢM BIẾN KINECT 4.1 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. 4.2 Giới thiệu cảm biến Kinect Hình 4-2 Cảm biến Kinect Kinect là sản phẩm của Microsoft dựa trên công nghệ camera được phát triển bởi PrimeSense, những sản phẩm đầu tiên được bán tại Bắc Mỹ vào ngày 4 tháng 11 năm 2010 . Báo cáo thực tập tốt nghiệp 15 Kinect được coi như là một thiết bị ngoại vi cho Xbox 360, cho phép giao tiếp với con người thông qua các cử chỉ, đem lại những cảm giác thú vị cho người chơi game trên Xbox. Khả năng hiểu được cử chỉ con người của Kinect dựa trên hai đặc tính chính sau: thông tin về độ sâu ảnh (depth map), khả năng phát hiện và bám theo đặc tính cơ thể người (body skeleton tracking). Bên cạnh phục vụ cho mục đích chơi game, sản phẩm Kinect còn được dùng vào mục đích nghiên cứu xử lý ảnh 3D, phát hiện cử chỉ (gesture recognition), bám theo người (body tracking) và nhiều mục đích khác. Lý do chính cho sự thành công của sản phẩm Kinect là giá cả khá rẻ (khoảng 140$ trên 1 sản phẩm) cho thiết bị có khả năng cung cấp các thông tin 3D với chất lượng chấp nhận được. 4.2.1 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. Camera RGB 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. Báo cáo thực tập tốt nghiệp 16 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ãyMicrophone 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 ngẩng camera. 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. 4.2.2 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. Báo cáo thực tập tốt nghiệp 17 Hình 4-3 Nguyên lý cảm biến độ sâu 4.1.3 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 Báo cáo thực tập tốt nghiệp 18 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 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. 4.2 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 xe tự hành tránh vật cản. Báo cáo thực tập tốt nghiệp 19 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). Báo cáo thực tập tốt nghiệp 20 - 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. Báo cáo thực tập tốt nghiệp 21 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.width=640;// 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) Báo cáo thực tập tốt nghiệp 22 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*480=307200.  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::PointXYZRGBAChứ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. Báo cáo thực tập tốt nghiệp 23 4.3 Phát hiện vật cản sử dụng Kinect Sơ đồ xử lý phát hiện vật cản sử dụng Kinect. Hình 4-7 Sơ đồ xử lý phát hiện vật cản sử dụng Kinect Khối đầu tiên có nhiệm vụ thu về bản đồ độ sâu từ Kinect. Việc này cần sự hỗ trợ của OpenNI. Các khối còn lại sử dụng thư viện PCL có nhiệm vụ xử lý dữ liệu thu được để tách ra thành từng nhóm điểm đại diện cho những đối tượng tương ứng. Sau đây là hoạt động cụ thể của từng khối. 4.3.1 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::ConstPtr&)> 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: Báo cáo thực tập tốt nghiệp