Phương pháp định vị và tìm đường đi tối ưu cho robot tự hành với bản đồ 2D

Nội dung text: Phương pháp định vị và tìm đường đi tối ưu cho robot tự hành với bản đồ 2D

1. Hội nghị toàn quốc lần thứ 7 về Cơ Điện tử - VCM-2014 Phương pháp định vị và tìm đường đi tối ưu cho robot tự hành với bản đồ 2D Path-planing and localization method for a mobile robot in a 2D enviroment Trần Văn Luân, Ngô Văn Thuyên Trường Đại Học SPKT TP.HCM e-Mail: tranvanluan07118@gmail.com Tóm tắt Định vị và tìm đường đi tối ưu cho robot là hai yêu Chữ viết tắt cầu cần thiết cho robot tự hành. Robot cần bản đồ để SURF Speeded up robust features thực hiện tìm đường đi tối ưu. Khi vị trí robot không IMU Inertial measurement unit thể xác định, robot sẽ di chuyển để tìm vật mốc định vị. Bài báo đề cập đến các vấn đề như lập bản đồ 2D, 1. Đặt vấn đề giải thuật tìm đường đi tối ưu D*, phương pháp định Khi một robot di chuyển trên bản đồ 2D được thiết lập vị cho robot kết hợp phương pháp nhận dạng vật mốc trước [1], trong trường hợp robot không biết điểm SURF và cảm biến IMU. Ngoài ra, bộ lộc Kalman xuất phát thì vấn đề định vị cho robot là cần thiết để giúp ước tính góc lệch q cho robot. Kết quả thực robot có thể biết được điểm xuất phát. Khi robot đã có nghiệm cho thấy rằng robot có thể điều khiển di được điểm xuất phát và đích đến thì robot cần tiến chuyển đến mục tiêu. hành tìm đường đi tối ưu trên bản đồ để có thể tránh các vật cản bằng giải thuật D*. Tuy nhiên trong môi Abstract: trường di chuyển thì robot có thể gặp các vật cản Localization and path-planning are two fundamental động. Lúc này robot cần thu thập dữ liệu từ camera problems of mobile robots. Robot needs a map to Kinect có kế hoạch di chuyển đến tọa độ mới [7] [8]. perform actions like path-planing. When positioning Để tránh được vật cản động thì thuật toán D* sẽ tiến robot is not available, robot will move to search for hành cập nhật lại trạng thái hiện tai của robot và tìm the landmarks for localization. This paper represents a đường đi tối ưu mới để về đến điểm đích. Trong thực method to build a map 2D, the path-planing algorithm tế, robot di chuyển rất dễ bị lạc đường nên bài toán D*, localization method for robot combinedly SURF định vị luôn là yêu cầu cần thiết trong quá trình robot algorithm and an IMU sensor. In addition, the Kalman di chuyển. Trong bài báo này phương pháp định vị sẽ filter helps estimate the bias angle for robot. The được đề cập đến là xác định tọa độ robot dựa trên experimental results showed that the robot can move nhận dạng vật mốc qua phương pháp SURF đồng thời to the target. kết hợp Kinect và cảm biến IMU. Ký hiệu 2. Phương pháp điều khiển robot trên Kí hiệu Đơn vị Ý nghĩa bản đồ 2D K mm m Hệ số tỉ lệ bản đồ 2.1 Xây dựng bản đồ 2D cho robot θ Rad Góc lệch trục robot so với Dựa trên môi trường thực tế robot di chuyển ta có thể trục y thiết lập bản đồ 2D cho robot [1]. Với mảng 2 chiều D mm Quãng đường robot di có m hang và n cột, ta có thể xây dựng bản đồ 2D như chuyển giữa 2 tọa độ. sau: β Rad Góc lệch giữa phương di Trục x,y là tương ứng thứ tự hàng, cột của mảng chuyển robot và trục x 2 chiều. xk mm Giá trị ước tính của Góc tọa độ là vị trí m = 0 và n = 0. Kalman Như vậy, mỗi phần tử trong mảng hai chiều điều được u Ngõ vào điều khiển xác định tọa độ theo hệ trục tọa độ Oxy. Tọa độ của A,B Ma trận trạng thái của mỗi phần tử cũng chính là chỉ số hàng và chỉ số cột. Kalman Tùy theo môi trường thực tế robot di chuyển mà khi Q,R Giá trị nhiễu của hệ thống lập bản đồ ta cần nhân hệ số tỉ lệ Km để lập nên một P Ma trận sai số hiệp phương bản đồ thích hợp cho robot di chuyển. Bản đồ 2D sai được xậy dựng cho robot như hình (H.1). Trong bản K Độ lợi của Kalman đồ này, ký tự ‘#’ đươc tượng trưng cho tường nhà v Vec tơ mô tả vùng con của hoặc vật cản tĩnh có trong môi trường robot di bộ mô tả trong SURF chuyển. Các chữ cái ‘A’,’B’’C’ được đặt trong ℋ(x, 휎) Ma trâṇ đạo hàm Hessian bảng đồ là điểm xuất phát hoặc điểm đích của robot. VCM-2014
2. Hội nghị toàn quốc lần thứ 7 về Cơ Điện tử - VCM-2014 Trong hình (H.1), robot di chuyển từ điểm A đến được cung cấp điểm xuất phát thì robot sẽ di chuyển điểm C và các ký tự ‘.’(dấu chấm) là đường đi tối ưu và tiến hành định vị để xác định vị trí hiện tại của được tìm thấy cho robot di chuyển. robot. Việc di chuyển và tính toán này được lặp lại cho đến khi robot xác định được vị trí đúng của nó trong môi trường. Sau khi vị trí đúng của robot đã được xác định thì một đường đi tối ưu từ vị trí này đến mục tiên được tính toán nhờ thuật toán D*. Lưu đồ giải thuật được xây dựng như hình (H.3). Bắt đầu Sai Đã có điểm đích ? Đúng H. 1 Bản đồ 2D cung cấp cho robot Sai ĐãĐã có có đi điểmểm xu xuấtấ t Giả sử robot đi di chuyển từ điểm P(,,) x y q tới điểm phát ? phát ? P'( x ', y ',q ') như hình (H.2). Đúng θ’ Gọi giải thuật định vị cho robot trong môi trường θ - Gọi thuật toán tìm đường D* D P’(x’,y’,θ’) - Robot di chuyển và liên tục cập nhật dữ liệu từ Kinect β Sai Đã đến điểm y đích ? P(x,y,θ) x ) Đúng H. 2 Robot di chuyển giữa 2 điểm Quảng đường robot phải di chuyển giữa 2 điểm Kết thúc được tính theo công thức (1). Với Km là hệ số tỉ lệ của bản đồ 2D. H. 3 Lưu đồ giải thuật điều khiển cho robot D = K * (x '- x )22 + ( y ' - y ) (1) 2.3 Phương pháp định vị cho robot m Với phương pháp định vị cho robot dùng phương pháp nhận dạng vật mốc đơn thuần [4]. Robot cần Góc lệch β giữa phương di chuyển robot và trục x nhận dạng cùng lúc hai vật mốc mới xác định được được tính theo công thức (2). tọa độ (,,)xyq cho robot và quá trình tính toán định vị cho robot phức tạp. Do đó, phương pháp định vị kết xx'- b = arctag (2) hợp phương pháp nhận dạng vật mốc và cảm biến yy'- IMU sẽ khắc phục được nhược điểm đó. Trong quá trình định vị robot chỉ cần nhận nhận dạng một vật 2.2 Giải thuật điều khiển robot mốc là đủ để xác định tọa độ (x,y) và góc lệch q sẽ Trong quá trình di chuyển trên bản đồ 2D đã được được xác định bằng cảm biến IMU. Robot sẽ di cung cấp cho robot như hình (H.1), nếu robot không chuyển bất kỳ trong bản đồ 2D và tiến hành định vị VCM-2014
3. Hội nghị toàn quốc lần thứ 7 về Cơ Điện tử - VCM-2014 thông qua các vật mốc có trong môi trường đã được thức (3). định tọa độ trước. Quá trình nhận dạng vật mốc này sẽ được thực hiện thông qua phương pháp nhận dạng x- =+ Ax Bu (3) SURF [4] và cảm biến IMU. Robot nhận dạng được k k- 1 k vật mốc thì từ vật mốc, robot có thể tính toán được tọa Hiệu chỉnh và cập nhật thông số bộ lọc theo công thức độ của robot trên bản đồ thông qua trục Z trong hệ tọa (4). độ Camera Kinect. Đồng thời, để xác định góc lệch θ của robot trong bản đồ 2D thông qua cảm biến IMU - T [3]. Tọa độ vật mốc và tọa độ robot được thiết lập như Pkk=+ AP- 1 A Q (4) hình (H.4). Ước tính trạng thái mong muốn. Vật mốc P (x ,y ) 1 1 1 Tính độ lợi Kaman theo công thức (5). TT 1 K=+ PKK H() HP H R (5) Cập nhật giá trị đo theo công thức (6). Z θ xk= x k + K k() z k - Hx k (6) Cập nhật ma trận sai số hiệp phương sai theo công thức (7). P(x,y,θ) - ) PIKHPk=-() k k (7) y 2.4 Phương pháp nhận dạng vật mốc SURF Phương pháp nhâṇ d ạng SURF [4] là phương pháp x dựa trên những đặc trưng cục bộ. Từ ảnh vào, phát H. 4 Thiết lập tọa độ vật mốc và Robot hiện điểm nổi bật của ảnh, xây dây dựng bộ mô tả các đặc điểm và sau đó so sánh các bộ mô tả để xác định Quá trình định vi cho robot sẽ có sai số, do trong quá kết quả ảnh đưa ra. trình cảm biến độc về thường bị nhiễu của môi Phát hiện điểm nổi bật trường, do đó để giảm sai số bộ lọc Kalman được sử Các điểm nổi bật trong ảnh thường là những điểm dụng để ước tính trạng thái của robot. Quy trình tổng mà tại đó cường độ sáng bị gián đoạn như góc, quát của bộ lọc Kalman được cho thấy như hình đốm màu, giao điểm chữ T Cách thức được sử (H.5). dụng để xác định được những điểm nổi bật là đạo hàm kết hợp. Xét tại một điểm A có tọa độ (x,y) trên một ảnh I, Ma trân đạo hàm Hessian ℋ(x, 휎) Bắt đầu ̣ tại điểm A với tỷ lệ thay đổi kích thước 휎 được tính theo công thức (8). Nhận góc θ từ æö Lxx(,)(,) xss L xy x ÷ cảm biến IMU H x,s = ç ÷ (8) ( ) ç ÷ Kalman Filter èøçLxy(,)(,) xss L yy x ÷ Ước tính sai số Cập nhật dữ liệu Trong đó Lxxx (,)s , Lxxy (,)s , Lxyy (,)s lần lượt là từ bản đồ và robot tích châp̣ c ủa đạo hàm bâc̣ hai hàm Gausse ¶ 2 ¶ 2 ¶ 2 g()s , g()s , g()s với ảnh I tại điểm Hiệu chỉnh và cập ¶ x2 ¶ xy ¶ y2 nhật thông số A. Xây dựng bộ mô tả các đặc trưng Ước tính trạng Bộ mô tả đặc điểm thể hiện mối quan hệ về mặt thái không gian giữa điểm nổi bật và các điểm lân cận xung quanh nhằm xác định vị trí của các điểm nổi H. 5 Quy trình tổng quát của bộ lọc Kalman bật trong ảnh. Việc xây dựng bộ mô tả bao gồm hai bước là xác định hướng lặp lại dựa trên thông Ước tính sai số ma trận hiệp phương sai theo công tin từ vòng tròn xung quanh điểm nổi bật, bước VCM-2014
4. Hội nghị toàn quốc lần thứ 7 về Cơ Điện tử - VCM-2014 tiếp theo là dựng cửa sổ hình vuông đồng hướng SURF là so khớp các bộ mô tả của ảnh cần nhận với hướng được xác định từ quá trình trên và tạo dạng với các bộ mô tả trong tập dữ liệu để nhận bộ mô tả SURF từ vùng hình vuông này. dạng vật thể. Bước so khớp này dựa vào khoảng Trong bước đầu tiên sau khi phát hiện được các cách giữa các vec tơ trong bộ mô tả. điểm nổi bâṭ thì quá trình ti ếp theo là xem xét vùng các điểm lân câṇ xung quanh đi ểm nổi bâṭ . 2.5 Thuật toán tìm đường đi D* Vùng điểm lân câṇ là vùng tròn có tâm là các Giải thuật D* là một giải thuật được phát triển lên từ điểm nổi bâṭ có bán kính là 6 s, với s là tỷ lệ thay giải thuật A* [5]. Giải thuật D* được sử dụng để tránh đổi kích thước như hình (H.6). các vật cản động hoặc vật cản tĩnh chưa được thiết lập trong bản đồ 2D. Về nguyên tắc thuật toán D* giống với thuật toán A*, tuy nhiên khi gặp vật cản động thì D* cập nhật lại tọa độ hiện tại và tính toán lại đường đi tối ưu tai điểm hiện tại [6]. Trong khi đó, thuật toán A* lại tính toán lại toàn bộ đường đi. Khi robot di chuyển theo đường đi mà thuật toán D* tìm được, thì dữ liệu Camera Kinect luôn được cập nhật và khi có vật cản mới thì robot cần tính toán một góc quay tối ưu để tránh vật cản, đồng thời di chuyển H. 6 Vùng điểm lân cận tròn có bán kính 6 s đến một điểm mới không có vật cản. sau đó, robot mới cập nhật hệ tọa độ mới cho robot tính toán lại Việc mô tả bắt đầu bằng việc xác định hướng của đường đi tối ưu tai điểm hiện tại bằng giải thuật D*. vùng điểm lân câṇ hay còn gọi là hướng vec tơ đặc điểm, quá trình này sư d ụng thuâṭ toán đáp ứng ̉ 2.6 Kết quả thực nghiệm Haar Wavelet để xác định hướng. Hướng được xác Robot di động được thiết kế có thể tìm đường đi tối định bằng tổng các đáp ứng theo hai hướng trục ưu trên bản đồ 2D cung cấp. Trong hình (H.8), robot tung độ và trục hoành độ, các đáp ứng được tính sẽ tiến hành di chuyển từ vị trí điểm A đến điểm B kết bên trong của một cưa sổ trượt kích thước 4 s giới ̉ hợp trên môi trường thực tế robot. hạn trong góc /3 của vùng điểm lân câṇ . Tổng các đáp ứng sẽ cho ra một vec tơ cho mỗi góc /3, và vec tơ nào lớn nhất sẽ là vec tơ của điểm nổi bâṭ. Bước tiếp theo là trích xuất các bộ mô tả, để thực hiện điều này đầu tiên dựng một vùng hình vuông xung quanh điểm nổi bâṭ. Kích thước cử a sổ là 20 s, vùng này sẽ được chia thành 4x4 vùng con như hình (H.7). Vec tơ v mô tả tổng của một vùng con được xây dựng từ 4 phần tử là å dx , å dy , H. 8 Bản đồ 2D cung cấp cho robot và môi trường robot di å dx , å dy được tính như công thức (9) chuyển v=(,,,) å dx å dy å dx å dy (9) Nhận dạng vật mốc với Kinect dùng phương pháp SURF và IMU để định vị đáp ứng sẽ mất một thời gian nhất định để robot di chuyển tìm vật mốc. Nếu Với là tổng đáp ứng theo trục ngang, khoảng cách từ robot đến vật mốc từ 500 mm đến tổng đáp ứng theo trục đứng, tổng giá trị 2000 mm thì độ chính xác của robot nhận dạng vật tuyệt đối của đáp ứng theo trục ngang và mốc khoảng 90 % và mất khoảng 500 ms - 1000 ms để nhận dạng thành công. Do quá trình xử lý ảnh nhận tổng giá trị tuyệt đối của đáp ứng theo trục dạng vật mốc và quá trình xác định vật mốc trong cơ đứng. sở dữ liệu các vật mốc được tạo ra hao tốn thời gian nhất định. Nếu robot không tìm thấy vật mốc thì robot phải di chuyển để tìm kiếm vật mốc, quá trình này mất một phần thời gian. Kết quả nhận dạng ba vật mốc trong quá trình di chuyển robot như (H.9). H. 7 Mô tả đáp ứng trong ảnh So sánh các bộ mô tả H. 9 Nhận dạng ba vật mốc Bước cuối cùng trong phương pháp nhận dạng VCM-2014
5. Hội nghị toàn quốc lần thứ 7 về Cơ Điện tử - VCM-2014 Robot đã có thể định vị điểm xuất phát và tìm đường Enabled Monte Carlo Localisation for a Robotic đi tối ưu để di chuyển từ điểm A đến điểm B như Wheelchair. University of Essex, Wivenhoe (H.8). Trong quá trình di chuyển thì robot đi qua các Park, Colchester, CO4 3SQ, UK. vị trí cần định vị lần lượt như hình (H.10). [8] Ayrton Oliver, Steven Kang, Using the Kinect as a Navigation Sensor for Mobile Robotics. University of Auckland Auckland, New Zealand. Trần Văn Luân, sinh năm 1988, nhận bằng Kỹ sư Điện tự động tại Trường Đại Học Sư Phạm Kỹ Thuật TP. HCM năm 2012. Anh tham gia nghiên cứu tại Trường Đại Học Sư Phạm Kỹ Thuật TP. H. 10 Robot tai thời điểm nhận dạng các vật mốc HCM từ năm 2012 đến nay. Hiện anh đang là Giảng Viên thuộc Bộ 3. Kết luận môn Điện Công nghiệp, Khoa Điện – Điện Lạnh, Trong bài báo này, các thuật toán điều khiển cho Trường Cao Đẳng Kỹ Thuật Cao Thắng TP.HCM. robot di động đã được thiết kế có thể đạt được các yêu Hướng nghiên cứu chính là các giải thuật điều khiển cầu đặt ra. Phương pháp nhận dạng vật mốc có thể tự động, giải thuật tránh vật cản, tìm đường đi tối ưu xác định chính xác vật mốc để xác định tọa độ (x,y) và xử lý ảnh cho robot. và việc kết hợp thêm cảm biến IMU giúp quá trình xác định góc lệch q được thuận lợi hơn. Việc sử Ngô Văn Thuyên, sinh năm 1976 , dụng bộ lọc Kalman có thể giảm được sai số cho quá nhận bằng Tiến sỹ Kỹ thuật tại trình xác định góc lệch cho robot. Tuy nhiên, Trường Đại Học Công Nghệ phương pháp định vị cho robot chưa được tối ưu, Sydney, Úc. Tiến sỹ Ngô Văn robot mất nhiều thời gian để di chuyển xác định vật Thuyên là Phó Hiệu trưởng của mốc định vị. Để robot có thể định vị tốt nhất trong Trường Đại Học Sư Phạm Kỹ môi trường di chuyển, ta cần xây dựng môi trường 3D Thuật TP. HCM từ năm 2013 đến cho robot. nay. Hướng nghiên cứu chính lập kế hoạch, tránh vật cản, định vị cho robot di động, Tài liệu tham khảo Điều khiển dùng mạng neural. [1] Stefan Carlsson. 2D Mapping Solutions for Low Cost Mobile Robot. Royal Institute of Technology year 2013. [2] John Folkesson, Danica Kragic. Robot Obstacle Avoidance using the Kinect. Control and Robotics Master’s Program Royal Institute of Technology year 2011. [3] A. Benini, A. Mancini, A. Marinelli, S. Longhi. A Biased Extended Kalman Filter for Indoor Localization of a Mobile Agent using Low-Cost IMU and UWB Wireless Sensor Network. Polytechnic University Of Marche, 2012. [4] Ba Viet Ngo, Hai Thanh Nguyen and Nguyen Hung. Tracking Landmarks for Control of an Electric Wheelchair Using a Stereoscopic Camera System. Biomedical Engineering Department, International University, VNU- HCMC Vietnam, IEEE, 2013. [5] Stentz, A. Optimal and efficient path planning for unknown and dynamic environments. International Journal of Robotics & Automation 10 (3), 89-100, 1993. [6] Daniel Opoku, Abdollah Homaifar, Edward * Tunstel. The A-r-Star ( Ar ) Pathfinder. International Journal of Computer Applications (0975 – 8887) Volume 67– No.8, April 2013. [7] Theodoros Theodoridis, Huosheng Hu, Klaus McDonald-Maier, and Dongbing Gu. Kinect VCM-2014
6. BÀI BÁO KHOA HỌC THỰC HIỆN CÔNG BỐ THEO QUY CHẾ ĐÀO TẠO THẠC SỸ Bài báo khoa học của học viên có xác nhận và đề xuất cho đăng của Giảng viên hướng dẫn Bản tiếng Việt ©, TRƯỜNG ĐẠI HỌC SƯ PHẠM KỸ THUẬT TP. HỒ CHÍ MINH và TÁC GIẢ Bản quyền tác phẩm đã được bảo hộ bởi Luật xuất bản và Luật Sở hữu trí tuệ Việt Nam. Nghiêm cấm mọi hình thức xuất bản, sao chụp, phát tán nội dung khi chưa có sự đồng ý của tác giả và Trường Đại học Sư phạm Kỹ thuật TP. Hồ Chí Minh. ĐỂ CÓ BÀI BÁO KHOA HỌC TỐT, CẦN CHUNG TAY BẢO VỆ TÁC QUYỀN! Thực hiện theo MTCL & KHTHMTCL Năm học 2016-2017 của Thư viện Trường Đại học Sư phạm Kỹ thuật Tp. Hồ Chí Minh.