Đề tài Giải bài toán cục bộ tránh vật cản cho robot tự hành

GiảI bàI toán cục bộ tránh vật cản cho robot tự hành TS. Nguyễn Văn Giáp KS. Từ Diệp Công Thành, KS. Đoàn Thế Thảo, KS. Nguyễn Văn Trung Bộ Môn Cơ Điện Tử, Khoa Cơ Khí, Tr−ờng Đại Học Bách Khoa Tp Hồ Chí Minh Email: nvgiap@dme.hcmut.edu.vn, tdcthanh@dme.hcmut.edu.vn Tóm tắt Một ph−ơng pháp tránh vật cản mới cho robot tự hành đã và đang đ−ợc phát triển và ngày càng hoàn thiện, gọi là kỹ thuật dẫn h−ớng thích nghi (Adaptive navigation). Với ph−ơng pháp này cho phép đơn giản hóa bài toán cục bộ. Bài báo này tập trung vào việc ứng dụng ph−ơng pháp dẫn h−ớng thích nghi để điều khiển mô hình robot tự hành tránh vật cản. Các cảm biến gắn trên robot sẽ phát hiện vật cản để điều khiển hệ thống dẫn h−ớng đ−a robot đi từ điểm xuất phát tới điểm đích mà không chạm vào các vật cản. Abstract A local navigation technique with obstacle avoidance, called adaptive navigation, had been developed and perfected. This technique allows to simplify the local problem. This paper focuses on the application of adaptive navigation technique to control a mobile robot. Sensors have to be used to detect obstacles and navigation system must be incorporated into the robot to navigate robot from start point reaching to the goal with obstacle avoidance. 1. GIớI THIệU Bài toán dẫn h−ớng cho robot có thể chia làm hai loại: bài toán toàn cục (global) và bài toán cục bộ (local), tùy theo môi tr−ờng làm việc của robot. ở bài toán toàn cục, môi tr−ờng làm việc của robot đã biết tr−ớc và từ đó đ−ờng đi tránh vật cản đ−ợc xác định [5]. Trong bài toán cục bộ, môi tr−ờng làm việc của robot hoàn toàn ch−a biết tr−ớc, hay chỉ biết một phần. Đối với bài toán này các loại cảm biến gắn trên robot sẽ phát hiện vật cản để điều khiển hệ thống dẫn h−ớng đ−a robot đi từ điểm xuất phát tới điểm đích mà không chạm vào các vật cản. Nội dung bài báo này chỉ tập trung giải quyết bài toán cục bộ. Một ph−ơng pháp dẫn h−ớng đ−ợc phát triển rất sớm đó là ph−ơng pháp đi theo đ−ờng biên vật cản (The wall-following method) [2]. ở ph−ơng pháp này, robot di chuyển dọc theo vật cản với một khoảng cách đ−ợc định tr−ớc. Kiểu dẫn h−ớng này th−ờng đ−ợc sử dụng cho những robot làm những công việc nh−: robot làm việc trên các đ−ờng ống, robot lau nhà, hút bụi. Với ph−ơng pháp phát hiện biên của vật cản (Edge detection method) [3,8], robot sử dụng thuật toán để tìm vị trí biên của vật cản và tạo ra các điểm đích phụ (subgoal) rồi thực hiện giải thuật tìm đ−ờng đi tối −u đến các điểm đích phụ đó. Để có thể xây dựng đ−ợc bản đồ vị trí vật cản, ng−ời ta sử dụng ph−ơng pháp chia l−ới (Certainty Grid method) [4], khi đó không gian làm việc của robot đ−ợc chia thành nhiều mảng trong không gian hai chiều, các phần tử (square elements) chứa một giá trị nhất định để chỉ có hay không vật cản ở vị trí đó. Đối với những robot có tốc độ di chuyển cao, ng−ời ta sử dụng ph−ơng pháp Potetial Field Methods [6,7]. Bài báo này tập trung vào ph−ơng pháp dẫn h−ớng thích nghi [1]. ở đây điểm đầu, điểm đích và kích th−ớc hình học của robot đã biết tr−ớc, còn hình dáng hình học và tọa độ của vật cản ch−a đ−ợc biết. H−ớng di chuyển của robot đ−ợc xác định dựa vào góc h−ớng tới điểm đích và tín hiệu từ cảm biến. −u điểm của ph−ơng pháp này là sử dụng ít cảm biến và luật dẫn h−ớng đơn giản hơn so với các ph−ơng pháp khác.     ∆−∆ ∆+∆= 12 12 2 ss ssTRD T ss 12 ∆−∆=∆Ψ (1) ( ) ( )   Ψ Ψ    ∆∆ ∆−∆=   ∆ ∆ sin cos xy yx Y X (2) ( ) ( ) ( ) ( ) ( ) ( ) ( ) ( ) ( )         ∆ ∆ ∆Ψ +        Ψ =        Ψ − − − iD iD iD iD iD iD iD iD iD Y X Y X Y X 1 1 1 (3) 2. Mô HìNH HóA Và ĐộNG HọC ROBOT Hai bánh xe phía tr−ớc của robot (xem hình 1 a) đ−ợc dẫn động độc lập bằng hai động cơ b−ớc thông qua bộ truyền đai, còn bánh sau để chạy không. Hình 1 : Mô hình và động học robot. Gọi ∆s1, ∆s2 lần l−ợt là đoạn dịch chuyển của hai bánh tr−ớc, T là khoảng cách giữa hai bánh xe (hình 1 b). Từ đó ta tính đ−ợc bán kính góc quay RD và góc dịch chuyển ∆ψ. Khi ∆s1 = - ∆s2 thì robot quay quanh điểm giữa hai bánh xe D. Khi ∆s1 = ∆s2 thì robot đi theo quĩ đạo là đ−ờng thẳng. Dịch chuyển theo hai ph−ơng trong hệ tọa độ tham chiếu viết d−ới dạng ma trận : Vị trí của robot tại thời điểm thứ i đ−ợc tính nh− sau : 3 PH−ơNG PHáP DẫN H−ớNG THíCH NGHI Ký hiệu sử dụng: x(t), y(t): vị trí của robot tại thời điểm t. (a) (b)    −−= (t)*θθ(t)η(t).θ (4)   >− ≤+= θ(t)φ(t)π;φ(t) θ(t)φ(t)π;φ(t) (t)θ *t (5) ( )πφφ 2(t)0; x(t) y(t)tan(t) 1 <≤= − (6) v(t): vận tốc của robot ở thời điểm t. θ(t): góc chỉ h−ớng của robot tại thời điểm t (0 ≤ θ(t) < 2π) so với trục x. θo: góc chỉ h−ớng của robot tại thời điểm ban đầu so với trục x. (xo,yo) , (0,0): vị trí đầu và vị trí cuối của robot. ϕ(t): góc h−ớng đích hợp bởi đoạn thẳng qua hai điểm (x(t), y(t)) và (0,0) so với trục x. θ*(t): góc dẫn h−ớng tính toán cho robot tới đích hay tránh vật cản. θ*t(t): góc dẫn h−ớng tính toán cho robot tới đích. θa(t): góc dẫn h−ớng tính toán cho robot tránh vật cản. Trên cơ sở lý thuyết và theo tài liệu [1], có luật dẫn h−ớng nh− sau : Không gian làm việc lý thuyết của robot nh− hình 2. Khi robot không phát hiện vật cản thì θ*(t) = θ*t(t). Với : Hình 2: Không gian làm việc của robot trong mặt phẳng 2-D Cách ứng xử của robot khi phát hiện vật cản. Robot sử dụng 3 cảm biến khoảng cách để xác định khoảng cách từ robot tới vật cản. Gọi dc, dl, dr là khoảng cách theo 3 ph−ơng, dl, dr đặt lệch so với dc một góc α. Khi cảm biến không phát hiện đ−ợc vật cản hay khoảng cách d > dmax, ngõ ra của sensor bằng -1. • Phát hiện ba vị trí : hình 3(a) Góc dẫn h−ớng tránh vật cản θa(t): Với : ε = tan-1((dcosα - dc)/dsinα) ( ε < π/2 ) d = max(dl,dr)   <− ≥+= 0:1 0:1 )sgn( x x x Hình 3: Các tr−ờng hợp tránh vật cản cho robot tự hành ( )    −−+= ε 2 π ddsgnθ(t)(t)θ rl*a (7) • Phát hiện hai vị trí : Ta có : (xem hình 3b) dc > drcosα và dl < 0. Kết luận : bên trái không có vật cản. Trên hình 3c thì dc < dlcosα và dr < 0, suy ra bên phải không có vật cản. Tuy nhiên, có thể có vật cản ở bên phải nh−ng tại thời điểm tức thời cảm biến không phát hiện đ−ợc. Để đảm bảo tránh đ−ợc vật cản, robot nên quay sang trái một góc ( π/2 - ε ). Khi dc <0 (xem hình 3d) robot không cần quay (giả thuyết khoảng cách giữa hai vật cản lớn hơn đ−ờng kính của robot). • Phát hiện một vị trí : Ta có : (xem hình 3e) dc < 0. T−ơng tự nh− tr−ờng hợp ở hình 3d, robot không cần quay. Khi dc > 0 (hình 3f) robot cần phải quay. Tuy nhiên, dl, dr < 0 tức là bên trái và bên phải đều không có vật cản, tr−ờng hợp này cho robot quay sang trái một góc π/2. 4. ĐIềU KHIểN Sơ đồ khối mạch điều khiển robot bằng máy tính nh− hình 4a. Vì là mô hình thí nghiệm nên ta chọn điều khiển bằng máy tính để thuận tiện cho việc nghiên cứu đáp ứng của hệ thống. Nh− đã biết cổng máy in của máy tính có thể giao tiếp với thiết bị ngoại vi qua 12 đ−ờng tín hiệu (8 đ−ờng xuất và 5 đ−ờng nhập). Để tăng số đ−ờng xuất nhập ta có thể sử dụng thêm một số loại IC chuyên dùng. Ngoài ra ta có thể sử dụng các kit vi xử lý, ch−ơng trình nạp trực tiếp lên ROM. Giải thuật điều khiển (hình 4b) t−ơng đối đơn giản nh−ng đảm bảo robot tránh đ−ợc vật cản và về đúng điểm đích. Ch−ơng trình điều khiển phải thỏa mãn những yêu cầu sau : - Luôn cập nhật thông tin về vị trí và góc của robot trong mặt phẳng. - Xử lý tín hiệu cảm biến và tính góc tránh vật cản cho robot θ*a(t). - So sánh các giá trị góc lệch, góc h−ớng đích và góc tránh vật cản để xác định góc dẫn h−ớng thích hợp θ*(t). ( )    −−+= ε 2 π ddsgnθ(t)(t)θ rc*a (8)    −+= ε 2 πθ(t)(t)θ *a (9) θ(t)(t)θ *a = (10) θ(t)(t)θ *a = (11) 2 πθ(t)(t)θ *a += (12) Tóm tắt thuật toán xác định góc dẫn h−ớng θ*(t) cho robot : Begin If dc 0 Then θ*(t) = θ*a(t) Else if dc > 0 Then if θ*t(t) ≥ θ*a(t) ≥ θ(t), or θ*t(t) ≤ θ*a(t) ≤ θ(t) Then θ*(t) = θ*t(t) Else θ*(t) = θ*a(t) End if Else if dl < 0 and θ*t(t) ≥ θ(t), or dr < 0 and θ*t(t) ≤ θ(t) Then θ*(t) = θ*t(t) Else θ*(t) = θ*a(t) End if End. Mô hình robot đ−ợc chế tạo tại Bộ môn Cơ Điện Tử, Khoa Cơ Khí tr−ờng Đại Học Bách Khoa TP. Hồ Chí Minh để kiểm chứng giải thuật điều khiển có hình dạng nh− trên hình 5. Qua nhiều lần thí nghiệm, robot đã đi đến điểm đích mà không chạm vào các vật cản. Hình 4: a) sơ đồ khối mạch điều khiển b) sơ đồ giải thuật điều khiển Không START đi thẳng STOP Quay Robot h−ớng đến đích Kiểm tra điểm đích Sai ii đúng Có vật cản Bẻ lái chuyển h−ớng robot đọc cảm biến (b) Máy tính Mạch xử lý tín hiệu Cảm biến Mạch công suất điều khiển động cơ b−ớc (a) 5. KếT LUậN Việc ứng dụng kỹ thuật dẫn h−ớng thích nghi này giúp ta đơn giản hóa bài toán dẫn h−ớng tránh vật cản cho robot tự hành trong bài toán điều khiển cục bộ. Ph−ơng pháp này không cần sử dụng nhiều cảm biến và luật dẫn h−ớng d−ới dạng ph−ơng trình vi phân bậc nhất. Tuy nhiên, với ph−ơng pháp này ta không thể xác định một cách đầy đủ biên dạng của vật cản, để làm đ−ợc điều này ta phải xử lý đồng thời nhiều cảm biến hơn hoặc sử dụng camera. Mặc dù còn một số hạn chế nh−ng trên thực tế những robot loại này đ−ợc sử dụng trong nhiều lĩnh vực nh− robot hàn, sơn, vận chuyển trong các nhà máy hay những robot phục vụ trong sinh hoạt. Hình 5 : Mô hình robot tự hành tránh vật cản. TàI LIệU THAM KHảO [1] ATSUSHI FUJIMORI, PETER N. NIKIFORUK, Adaptive Navigation of Mobile Robot with Obstacle Advoidance, IEEE Transactions on Robotics and Automation, Vol-13, August 1997. [2] BAUZIL, G., BRIOT, M. AND RIBES, P., "A Navigation Sub-System Using Ultrasonic Sensors for the Mobile Robot HILARE." 1st In.. Conf. on Robot Vision and Sensory [3] CROWLEY, J. L., "Dynamic World Modeling for an Intelligent Mobile Robot." IEEE Seventh International Conference on Pattern Recognition, Proceedings July 30-August 2, Montreal Canada, 1984, pp. 207-210. [4] ELFES, A., "A Sonar-Based Mapping and Navigation System." Carnegie-Mellon University The Robotics Institute, Technical Report, 1985, pp. 25-30. [5] J. C. LATOMBE, Robot Motion Planning, Kluwer Academic Press, Norwell, MA, 1991. [6] KHATIB, O., Real-Time Obstacle Avoidance for Manipulators and Mobile Robots, 1985 IEEE International Conference on Robotics and Automation, March 25-28, 1985. [7] KROGH, B. H., A Generalized Potential Field Approach to Obstacle Avoidance Control International Robotics Research Conference, Bethlehem, PA, August, 1984. [8] R.S. AHLUWALLA, E.Y. HSU, Sensor-Based Obstruction Avoidance Technique for a Mobile Robot, Journal of Robotic Systems, 1984.