Hoạch định và bám đường đi cho mobile robot ứng dụng điều khiển hồi tiếp ảnh

Bài viết này trình bày phương pháp tìm đường đi ngắn nhất cho mobile robot trong môi trường có vật cản, và điều khiển mobile robot bám theo đường đi (ảo) đã vạch ra bằng cách ứng dụng tiêu chuẩn ổn định Lyaponive kết hợp với camera hồi tiếp vị trí thực sự hiện tại của robot.

TẠP CHÍ KHOA HỌC CƠNG NGHỆ GIAO THƠNG VẬN TẢI SỐ 27+28 – 05/2018 39 HOẠCH ĐỊNH VÀ BÁM ĐƯỜNG ĐI CHO MOBILE ROBOT ỨNG DỤNG ĐIỀU KHIỂN HỒI TIẾP ẢNH Le Duc Hanh1, Nguyen Duy Anh1 Ho Chi Minh City University of Technology, Vietnam National University Ho Chi Minh City ldhanh@hcmut.edu.vn Tóm tắt: Bài báo trình bày phương pháp tìm đường ngắn cho mobile robot mơi trường có vật cản, điều khiển mobile robot bám theo đường (ảo) vạch cách ứng dụng tiêu chuẩn ổn định Lyaponive kết hợp với camera hồi tiếp vị trí thực robot Đầu tiên vị trí robot điểm đầu, cuối vật cản xác địng thông qua cameara đặt cao Sau giải thuật tìm đường ngắn A* ứng dụng để tính tốn đường tối ưu cho robot để đến điểm đích Vị trí thực đường ảo chuyển sang tọa độ thực thơng qua q trình calib camera Cuối Robot bám theo đường hoạch định Q trình thực nghiệm xem đạt kết tốt sai số đường thực robot đường hoạch định nhỏ robot đến đích Từ khóa: Điều khiển hồi tiếp, hoạch định đường đi, ổn định lyaponov, xử lý ảnh, hồi tiếp ảnh Chỉ số phân loại: 1.4 Abstract: This paper presents a visual feedback control algorithm to find a shortest path for a task and control robot to follow that calculated direction by using Lyapunov algorithm combining with fixed camera Firstly by using image processing algorithm, the starting, targets, obstacle positions are determined Secondly the shortest path A* algorithm applied to find the shortest path for mobile robot to reach the target The position of the virtual line according to the world coordinate will be planning based on calibration process Finaly, posisiton based visual control algorithm is applied to control robot to follow that path The succession is determnied when robot approachs the target and have small error between the calculated path and the real working path of robot Keywords: Visual tracking, Shortest path, Camera calibration, classical control, Image processing Classification number: 1.4 Giới Thiệu Ngày robot sử dụng nhiều lĩnh vực an ninh quốc phịng, tìm kiếm cứu nạn, giúp người làm việc nơi nguy hiểm hay nơi người tiếp cận Robot phục vụ nhà hướng nghiên cứu đầy hứa hẹn Tuy nhiên, hầu hết robot phục vụ nhà chưa thể tự động hoàn toàn, việc trang bị cho cảm biến đủ mạnh để né tránh vật cản hay nhận biết vị trí cần đến nhiều nghiên cứu giới quan tâm Các phương pháp dò line truyền thống sử dụng đường line cố định mang lại nhiều hạn chế việc ứng dụng, đường line sau thời gian sử dụng bị mờ không rõ, dễ bị tác động môi trường xung quanh, phức tạp muốn thay đổi hướng di chuyển Việc ứng dụng camera vào việc điều khiển robot để bám theo đường đặt nhu cầu thực tế cấp thiết, giải hạn chế việc dò theo line truyền thống linh động việc điều khiển hướng đi, có yêu cầu người sử dụng Đồng thời việc sử dụng camera để quy hoạch đường giúp cho việc di chuyển robot tối ưu Hiên nghiên cứu tìm đường robot nhiều nhà nghiên cứu quan tâm U Farooq et al [1], sử dụng phương pháp điều khiển thông minh fuzzy kết hợp với cảm biến hồng ngoại điều khiển robot bám theo đường line cho trước J.H Su [2] sử dụng phương pháp dò line cảm biến quang phương pháp điều khiển on-off để điều khiển robot dị theo line Ngày camera đóng vai trò quan trọng phát triển công nghiệp, dựa vào sức mạnh tốc độ ngày tăng máy tính, camera đóng vai trị cảm biến hiệu giúp cho robot nhận diện, phát vật dụng xung quanh 40 Journal of Transportation Science and Technology, Vol 27+28, May 2018 thực tác vụ theo yêu cầu Để nhận dạng vật, thông tin thu thập từ camera xử lý để nhận dạng đặc điểm cốt lõi vật truyền tọa độ vật cho camera P.T.L Hải et al [3] sử dụng phương pháp phân đoạn ảnh với phép tốn hình thái học (morphology) để phân biệt, xác định vị trí tính vận tốc robot Phương pháp sử dụng phần mềm Matlab nên khơng đảm bảo tính thời gian thực củng khó khăn việc giao tiếp với robot tay máy có thị trường không dùng Matlab Hay H.Đ Chiến et al [4] sử dụng phương pháp nhận dạng vật theo màu sắc sử dụng phần mềm Halcon Phương pháp sử dụng phần mềm thương mại sẵn có để thực gây khó khăn việc phát triển chức kết nối với thiết bị ngoại vi khác cánh tay robot N.V Khanh et al [5] sử dụng camera gắn trực tiếp robot để điều khiển robot bám theo đường hàn cho trước cách sử dụng phương pháp điều khiển sử dụng tọa độ điểm ảnh camera hay A Cherubini et al [6] sử dụng phương pháp điều khiển hồi tiếp ảnh để điều khiển mobile robot bám theo đường line màu trắng Các phương pháp dò theo line cho trước sử dụng camera cảm biến có hạn chế chạy theo đường line cho trước cố định khơng linh hoạt sử dụng Việc ứng dụng phương pháp điều khiển đường line bám theo thay đổi hướng ứng dụng thực tiễn sống đặc biệt nơi vật cản thay đổi, đường phải linh hoạt thay đổi theo Để tạo đường ảo nhiều nhà nghiên cứu tập trung nghiên cứu vào việc quy hoạch đường Z Miljkovic et al [7] ứng dụng giải thuật Triz Maz để quy hoạch đường cho robot qua trạm dừng đến điểm đích cần đến S Kloder et al [8] sử dụng giải thuật tìm đường cho nhiều robot để thực tác vụ yêu cầu, cách sử dụng định nghĩa hoán vị bất biến nghiên cứu giải toán va chạm đường nhiều robot từ hoạch định đường cho robot Tuy nhiên nghiên cứu có giới hạn tập trung vào việc nghiên cứu tìm đường mơ máy tính, việc ứng dụng để robot bám theo đường hoạch định vận chưa nghiên cứu sâu Từ vấn đề thảo luận trên, sử dụng camera xử lý ảnh phương pháp hữu dụng cho robot quy hoạch đường giúp robot bám theo đường vạch nhà hay kho xưởng Ý tưởng đề tài từ điểm đầu, điểm đích đến chướng ngại vật, cách sử dụng giải thuật thơng minh để tìm đường ngắn nhất, sau sử dụng chương trình lập trình thơng dụng kết hợp với camera mã nguồn mở xử lý ảnh để thực tác vụ điều khiển cho robot bám theo đường vạch Tinh Chỉnh Camera Camera calibration (hiệu chỉnh camera) phương pháp tính tốn thực nghiệm nhằm tìm thông số camera cho việc tái tạo không gian 3D cảnh thực tế ảnh mà camera chụp lại Để có thơng số đó, tính tốn sau chủ yếu dựa vào mơ hình camera thơng dụng mơ hình Pinhold [9] hình Hình Mơ hình Pinhold Trong mơ hình Pinhold điểm T M   X Y Z  không gian 3D chuyển đổi không gian điểm ảnh 2D, T m   x y  Ta có:  f  f x cot  cx   x  x  X       f x Z  y   cy   Y       sin  1   Z    0     Hay công thức (2) viết lại: Zm  KM (1) (2) TẠP CHÍ KHOA HỌC CƠNG NGHỆ GIAO THƠNG VẬN TẢI SỐ 27+28 – 05/2018 Với ma trận K gọi ma trận thông số nội camera Để đơn giản ta viết lại ma trận thơng số nội dạng:  fx  K   0  cx   c y   s fy với s tham số nghiêng Trong thực đối tượng thật biểu diễn hệ tọa độ 3D mà khơng phải tọa độ camera ta giả sử Để tính toán hệ tọa độ này, cách đơn giản ta chuyển hệ tọa độ camera H.2 Giả sử M w điểm hệ tọa độ w bất M c điểm hệ tọa độ camera kì, M w chuyển M c Khi đó, phép quay R phép tịnh tiến t Ta có: M c  RM w  t (3) với ma trận R vector t mô tả hướng, vị trí tương đối hệ tọa độ camera hệ tọa độ thực tế Ma trận R chứa phép quay (theo x, y, z) vector t chứa phép tịnh tiến Những thông số chứa R t gọi thông số ngoại camera Hình Chuyển đổi tọa độ pixel tọa độ thực Như từ công thức (2) (3), ta viết lại: Zm p  KM c  K  RM w  t  (4) Từ cơng thức (4), ta có cơng thức tổng qt cuối là: 41 Trong khoa học máy tính, A* thuật tốn tìm kiếm đồ thị Thuật tốn tìm đường từ nút khởi đầu đến nút đích cho trước [10] Thuật tốn sử dụng đánh giá “heuristic” để xếp loại nút theo ước lượng quãng đường tốt qua nút Thuật tốn duyệt nút theo thứ tự đánh giá heuristic Heuristic hiểu đơn giản giải thuật để giải thuật tốn, phương pháp tạo dựa kinh nghiệm, trực giác người Mục đích thuật giải Heuristic giải thuật tốn phức tạp nhanh chóng, đưa kết so với giải thuật thơng thường, chi phí thấp Thuật tốn A* ứng dụng hàm heuristic để tìm kiếm đường Từ trạng thái tại, A* xây dựng tất đường đi dùng hàm ước lượng khoảng cách ( hàm Heuristic) để đánh giá đường tốt Thứ tự ưu tiên gán cho đường x định hàm f(x) = g(x) + h(x) Trong g(x): chi phí đường tính từ điểm xuất phát tới điểm x tại, h(x) khoảng cách từ điểm x tới điểm đích Hàm f(x) có giá trị thấp độ ưu tiên x cao Để hiểu rõ giá trị thuật toán, ta cần làm rõ hai định nghĩa sau: Khoảng cách điểm A, B độ dài đoạn thẳng ngắn nối điểm lại với (có thể cắt ngang qua vật cản) Chi phí điểm A, B độ dài đường cần thực để từ điểm A đến điểm B (không cắt ngang qua vật cản) Giả thiết môi trường hoạt động robot chia thành mạng lưới ô vuông với ma trận 4x4 hình Zm p  K  RM w  t   R 1  K 1Zm p  t   M w (5) Với: - m p   x y 1 : tọa độ pixel - Mw X Y Z T T : tọa độ thực Giải Thuật A* Hình Áp dụng thuật toán A* ma trận 4x4 42 Journal of Transportation Science and Technology, Vol 27+28, May 2018 Vị trí xuất phát có tọa độ A(1,1), vị trí đích B(1,4), chấm tròn đen vật cản Trong trường hợp bước phải từ A node (2,1) Khi tới node (4,2) có node liền kề lựa chọn làm node Quá trình lựa chọn dựa vào hàm f(x) tính tốn phương trình (1) đó: g(4,3) chi phí từ node A(1,1) → (2,1) → (3,1) → (4,2) → (4,3), h(4,3) khoảng cách node (4,3) node B g(3,3) chi phí từ node A(1,1) → (2,1) → (3,1) → (4,2) → (3,3), h(3,3) khoảng cách node (3,3) node B f 4,3  g 4,3  h 4,3  4.414  3.162  7.576 f 3,3  g 3,3  h 3,3  4.828  2.236  7.064 Theo kết tính tốn node (3,3) có chi phí f(x) nhỏ hơn, chọn node (3,3) node Thuật toán tiếp tục xét node liền kề với node (3,3) đạt node mong muốn Thiết kế điều khiển Mơ hình động học robot giới thiệu nhiều nghiên cứu Mơ hình bao gồm điểm quan trọng Điểm R: điểm tham chiếu cho robot; Điểm M: trung điểm hai bánh chủ động; Điểm C: Điểm tracking robot Mơ hình thể H.4 Hình Mơ hình hóa Robot Phương trình động học robot M  x   cos       v   y   sin  0   (5)                Phương trình động học robot điểm tracking C  xc  x  d cos    xc  x  d sin         yc  y  d sin     y c  y  d cos  (6)       c    c     Với d khoảng cách từ tâm xe đến điểm bám line C Phương trình động học điểm tham chiếu R  x R  vR cos R     (7)  y R  vR sin R      R   R Trong v R vận tốc mong muốn robot điểm tham chiếu Bộ điều khiển thiết kế cho điểm tracking C đến vị trí điểm tham chiếu R với vận tốc mong muốn 𝑣𝑣𝑅𝑅 Để làm điều đó, cần phải xác định sai số bám đường sau:  e1   cos  sin    xR  xc        e2    sin  cos  0  yR  yc  (8)      e   0  R  c   3  Sau có sai số e1, e2, e3, điều khiển tracking [11] cho phép xác định giá trị vận tốc v ω cần thiết để điểm tracking bám theo điểm tham chiếu: v  vR cos e3  k1e1 (9)    k2 v2 e2   R  k3 sin e3 Thực Nghiệm Tiến hành thực nghiệm nhiều trường hợp khác để đưa đánh giá trình tìm đường đi, khả bám theo đường tìm robot Việc đánh giá khả bám đường robot đánh giá theo sai số e1, e2, e3 sai lệch tọa độ tracking robot so với tọa độ đường tìm Khơng gian dùng để thực nghiệm bố trí hình Camera gá đặt cố định phía mặt phẳng làm việc robot với phạm vi hoạt động 1200 x 900 mm vị trí điểm đích đến điểm bắt đầu xác định trước, vị trí vật cản Cấu trúc điều khiển robot tóm tắt H.6 gồm có vi điều khiển trung TẠP CHÍ KHOA HỌC CÔNG NGHỆ GIAO THÔNG VẬN TẢI SỐ 27+28 – 05/2018 tâm nhận lệnh từ máy tính điều khiển vi điều khiển khác điều khiển động Đầu tiên đường ngắn từ điểm đầu đến điểm đích tính tốn dựa giải thuất A* Kết đường ngắn thể hình 43 coi khoảng cách an tồn robot vật cản Vì vậy, vật cản mở rộng thêm từ biên chúng với bán kính bán kính Vật cản sử dụng đề tài khối tròn, bán kính 6cm, có màu đỏ đồng mơ hình robot kiểm nghiệm có bán kính từ trung điểm hai bánh dẫn động đến biên bánh xe 10.5 cm Hình Khơng gian hoạt động robot Laptop Xử lý ảnh Frame Camera Bluetooth Hình Kết tìm đường ngắn MCU Master I2C MCU slave MCU slave Driver Driver Left Motor Right Motor Encoder Encoder Sau có đường ngắn nhất, robot dựa đường mà tiến đến điểm đích, kết q trình điều khiển thể H.8 với đường màu đỏ đường hoạch định, đường màu xanh tọa độ robot Hình Cấu trúc điều khiển robot Để áp dụng thuật tốn A* tìm đường cho robot vị trí robot, vật cản vị trí điểm đích cần phải xác định Vị trí điểm xác định theo tâm đối tượng xử lý ảnh Để đưa kích thước vật cản vào tốn vật cản mở rộng từ tọa độ tâm theo hướng với bán kính 6cm Trên thực tế, robot có kích thước định, để đưa kích thước robot vào tốn khơng làm tốn trở nên phức tạp, robot đơn giản hóa kích thước cách lập đường biên bao xung quanh robot Đường biên đường trịn, có tâm trung điểm hai bánh chủ động, bán kính khoảng cách từ tâm đến biên ngồi bánh Bán kính Hình Kết bám theo đường line Hình Sai số e1 44 Journal of Transportation Science and Technology, Vol 27+28, May 2018 Education, (2010), pp 455-461 [3] P.T.L Hải, T.T Hùng, N.V Khanh L.T Hiếu, “Ứng Dụng Xử Lý Ảnh Kỹ Thuật Số Trong Việc Theo Dõi Sự Chuyển Động Của Các Robot Di Động “, Tạp chí Khoa học Trường Đại học Cần Thơ, 2015, pp 39-47 Hình 10 Sai số e2 Hình 11 Sai số e3 Dựa vào H.8-H.11, nhận kết hợp camera giải thuật điều khiển phát huy tác dụng, giup cho robot bám theo đường line mục tiêu đề với sai số nhỏ Kết Luận Nghiên cứu trình bày phương án hiệu giúp cho robot hoạt động vùng khơng gian hẹp xưởng hay nhà Bằng cách kết hợp xử lý ảnh điều khiển, đường robot trở nên linh hoạt khơng cịn bị cứng nhắc dị line truyền thống Trong q trình robot bám đường đi, nhờ camera robot nhận biết vật thể xung quanh tránh vật cản Ngồi người dùng tự định nghĩa đường line cho robot thông qua giao diện camera giúp cho việc thay đổi đường khơng cịn khó khăn nữa Tài liệu tham khảo [1] U Farooq, M Amar, M U Asad, G Abbas, A Hanif, “Fuzzy logic Reasoning System for Line Following Robot, “ IACSIT International Journal of Engineering and Technology, (2014), pp 244-248 [2] J H Su, C S Lee, H H Huang, S H Chuang C Y Lin, “An intelligent line following robot project for introductory robot courses, “ World Transactions on Engineering and Technoloy [4] H.Đ Chiến, N.P Cường, Đ.Đ Duy, “Ứng Dụng Xử Lý Ảnh Nhận Dạng Vật Thể Theo Màu Sắc Cho Robot Tay Máy”, Khoa Điện –Điện tử Đại học Bà Rịa Vũng Tàu, 2015 [5] N V Khanh, N T Tien, B T Hieu, H V Hung, “Study on Tracking Control of Welding Mobile Robot Using Camera - Straight Welding Path Application, “Proceeding of The International Symposium on Electrical and Electronics Engineering (ISEE 2005), HCMUT, 2005, pp 102-107 [6] A Cherubini, F Chaumetter, G Oriolo, “An image-based visual servoing scheme for following paths with nonholonomic mobile robots”, 10th International Conference on Control, Automation, Robotics and Vision, 2008, pp 220-225 [7] Z Miljkovic, P Milica, B Babic, Integration of Proces Planning, Scheduling, and Mobile Robot Navigation Based on TRIZ and Multi-Agent Methodology,” FME Transactions 41, (2013), pp 97-106 [8] S Kloder and S Hutchinson, “Path Planning for Permutation-Invariant MultiRobot Formations,” IEEE Trans on Robotics, 22 (2006), pp 650-665 [9] A Cherbuini, F chaumette and G.Oriolo, “A position-based visual servoing scheme for following paths with nonholonomic mobile robots”,IEEE International Conference on Intelligent Robots and Systems, 2008, pp.101-105 [10] C Zeng (2012) GA-based Global Path Planning for Mobile Robot Employing A* Algorithm Journal of Computer, Vol.7, No.2, 470-474 [11] L.H.Danh, L.T.D.Hieu, N.T.Tien.et al Nghiên cứu thiết kế điều khiển bám line cho AGV qua giao lộ Hội nghị toàn quốc lần thứ Cơ điện tử - VCM – 2014 , pp 597-601 Ngày nhận bài: 6/3/2018 Ngày chuyển phản biện: 9/3/2018 Ngày hoàn thành sửa bài: 29/3/2018 Ngày chấp nhận đăng: 5/4/2018 ... trực tiếp robot để đi? ??u khiển robot bám theo đường hàn cho trước cách sử dụng phương pháp đi? ??u khiển sử dụng tọa độ đi? ??m ảnh camera hay A Cherubini et al [6] sử dụng phương pháp đi? ??u khiển hồi tiếp. .. đến đi? ??m đích, kết q trình đi? ??u khiển thể H.8 với đường màu đỏ đường hoạch định, đường màu xanh tọa độ robot Hình Cấu trúc đi? ??u khiển robot Để áp dụng thuật toán A* tìm đường cho robot vị trí robot, ... từ máy tính đi? ??u khiển vi đi? ??u khiển khác đi? ??u khiển động Đầu tiên đường ngắn từ đi? ??m đầu đến đi? ??m đích tính tốn dựa giải thuất A* Kết đường ngắn thể hình 43 coi khoảng cách an toàn robot vật cản