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 cha 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 cha đợ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 ss T R D 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 s 1 , s 2 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 R D và góc dịch chuyển . Khi s 1 = - s 2 thì robot quay quanh điểm giữa hai bánh xe D. Khi s 1 = s 2 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 nhng 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 nhng đả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 and dl.dr > 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ế nhng 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. . 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. 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