Tuyển tập công trình Hội nghị Cơ điện tử toàn quốc lần thứ 6 679 Mã bài: 149 Định vị cho Robot di động sử dụng camera và vật mốc Localization for mobile robot using camera and landmarks Lê Hoàng Anh Trường Đại học Lạc Hồng, lehoanganh.lhu@gmail.com Ngô Văn Thuyên Trường Đại học Sư Phạm Kỹ Thuật TP.HCM, nvthuyen@gmail.com Tóm tắt Để có thể di chuyển trong môi trường làm việc theo quỹ đạo mong muốn, robot di động phải có khả năng xác định vị trí của nó. Bài báo này trình bày thuật toán định vị cho robot di động dựa trên nhận dạng vật mốc nhân tạo sử dụng camera. Thuật toán tìm đường đi tối ưu A* được áp dụng để robot có thể di chuyển từ điểm xuất phát về điểm đích mà không va chạm với vật cản.Vị trí của robot được xác định từ tọa độ của các vật mốc trong môi trường làm việc sử dụng phương pháp hình học. Bộ lọc Kalman được sử dụng để kết hợp thông tin từ mô hình động học của robot và thông tin đo đạc từ bên ngoài để ước lượng vị trí của robot chính xác hơn. Thuật toán được thực hiện trên mô hình robot thực di chuyển theo kiểu lái vi sai, sử dụng camera Kinect và các hàm thị giác máy tính trong thư viện OpenCV. Kết quả thực nghiệm cho thấy robot có thể bám theo quỹ đạo mong muốn nhờ xác định được vị trí của nó. Thuật toán định vị dùng bộ lọc Kalman ước lượng vị trí của robot tốt hơn. Abstract To follow a desired trajectory in a working environment, a mobile robot must be able to localize itself. This paper presented a localization algorithm for mobile robots based on recognition of artificial landmarks with a camera. The path finding algorithm A* was applied to find an optimal path for the robot to move from its initial position to its goal without colliding with obstacles. A geometric method was used to determine the position of the robot from the coordinate of artificial landmarks in the environment. The Kalman filter combines the estimation of robot internal states based on its kinematic model and external sensory information to better estimate the robot positions. The algorithm has been implemented on a real mobile robot platform with two differential drive wheels, a Kinect camera and computer vision functions in OpenCV library. The experimental results showed that the robot can use its estimated position to follow its desired trajectory closely. The Kalman filter provided a better estimation of robot positions. Key words: Mobile robot localization, Kalman localization, landmark. 1. Đặt vấn đề Định vị đóng vai trò then chốt trong sự thành công của một hệ robot độc lập. Nếu robot không biết được vị trí hiện tại của nó trong môi trường thì robot sẽ rất khó khăn để quyết định phải làm việc gì tiếp theo. Các robot hầu như phải có một vài nhận biết về vị trí của nó trong môi trường để có thể hành động một cách chính xác [1]. Những vấn đề về định vị mà robot di động cần thực hiện đó là theo dấu vị trí (position tracking) tức là phải đảm bảo robot bám theo được vị trí của nó khi đang di chuyển, tự xác định vị trí ban đầu (wake-up) tức là robot phải xác định vị trí ban đầu của nó trong môi trường làm việc và cuối cùng là nhảy cóc vị trí (kidnapped) tức là robot đang định vị thì vị trí của nó bất ngờ bị thay đổi [2]. Sự khác nhau chủ yếu giữa các phương pháp định vị đã thực hiện đó chính là độ tin cậy của robot về vị trí hiện tại mà vấn đề này phụ thuộc vào loại thông tin thu được từ cảm biến. So với phương pháp định vị sử dụng các loại cảm biến khác như siêu âm, laser thì camera có khả năng cung cấp nhiều thông tin hơn, ngoài dữ liệu ảnh RGB thu được từ môi trường thì camera còn có khả năng cung cấp thông tin về giá trị độ sâu của từng điểm trong khung ảnh, hình dáng của vật thể. Thông tin về độ sâu của từng điểm ảnh là một trong những thông tin dữ liệu đầu vào quan trọng của thuật toán định vị. Bên cạnh đó phương pháp sử dụng camera còn có thể tận dụng được khả năng xử lý tính toán mạnh của máy tính. Trong các công trình đã thực hiện [3], [4], [5] và [6] thì camera là loại cảm biến được 680 Lê Hoàng Anh, Ngô Văn Thuyên VCM2012 lựa chọn để xây dựng thuật toán định vị. Tuy nhiên, các loại camera này chỉ cung cấp ảnh RGB, không có thông tin về dữ liệu độ sâu của từng điểm ảnh. Giới hạn này làm cho các thuật toán định vị không đạt hiệu quả cao. Bài báo này trình bày thuật toán định vị bằng phương pháp hình học dựa trên việc nhận dạng các vật mốc nhân tạo có màu sắc khác nhau được bố trí sẵn trong môi trường làm việc sử dụng kỹ thuật thị giác máy tính. Phương pháp xác định tọa độ và hướng của robot được mô tả chi tiết trong phần II. Phần III thảo luận về việc sử dụng bộ lọc Kalman trong thuật toán định vị và xây dựng bộ lọc Kalman cho mô hình robot di động thực. Phần IV và V của bài báo trình bày kết quả thực nghiệm và kết luận. 2. Thuật toán định vị bằng phương pháp hình học Khi đề cập đến vị trí có nghĩa là đề cập đến tọa độ , x y và hướng (góc ) của robot trong hệ tọa độ của môi trường. Đối với trường hợp sử dụng phương pháp hình học trong thuật toán định vị thì chỉ cần nhận dạng được hai vật mốc trong khung ảnh [3]. Sau đó thông tin về tọa độ tâm của hai vật mốc này được sử dụng để xác định khoảng cách từ vật mốc đến camera, đó cũng chính là khoảng cách từ vật mốc đến robot. Giá trị khoảng cách này được xác định dựa vào dữ liệu độ sâu các điểm ảnh thu được từ camera Kinect một thiết bị của hãng Microsoft dùng cho các máy trò chơi. Giả sử hai vật mốc nhận dạng có vị trí trong hệ tọa độ môi trường lần lượt là ( , ) A A A x y và ( , ) B B B x y .Tọa độ của robot được xác định chính là giao điểm của hai đường tròn ( ) A C và ( ) B C như trong Hình 1. Các vật mốc được bố trí tại những đường thẳng giới hạn không gian di chuyển của robot 0 x , 0 y , x n , y m nên tọa độ của robot được xác định chính là giao điểm thứ nhất của hai đường tròn 1 1 ( , ) P x y . Hướng của robot được xác định bởi góc như trong Hình 2, đó là góc lệch giữa trục ( ) r y của hệ tọa độ đặt trên robot và trục x của hệ tọa độ môi trường. Góc được xác định bằng cách tính toán góc lệch của vật mốc so với hệ tọa độ môi trường và góc lệch của vật mốc so với hệ tọa độ đặt trên robot. (0,0) x y ),( AA yxA ),( BB yxB ),( 11 yxP ),( 22 yx A r B r m y )( A C )( B C Hình 1. Xác định tọa độ robot bằng phương pháp hình học ) ( r x ) ( r y ),( 11 yxP Hình 2. Các hệ tọa độ tham chiếu Góc lệch của vật mốc so với hệ tọa độ môi trường được xác định dựa vào vị trí hiện tại của robot 1 1 ( , ) P x y và tọa độ của một trong hai vật mốc theo hệ tọa độ môi trường đã nhận dạng như Hình 3. Giả sử vật mốc B được chọn để thực hiện việc tính toán. Tùy thuộc vào vị trí của robot so vật mốc mà góc có thể được xác định như trình bày trong Bảng 1. Bảng 1. Góc lệch của vật mốc so với hệ tọa độ môi trường Vị trí của robot so với vật mốc Góc lệch 1 B y y và 1 B x x hoặc 1 B y y và 1 B x x 1 1 tan( ) B B y y a x x 1 B y y và 1 B x x hoặc 1 B y y và 1 B x x 0 1 1 180 tan( ) B B y y a x x 1 B y y và 1 B x x 0 90 1 B y y và 1 B x x 0 180 1 B y y và 1 B x x 0 0 Tuyển tập công trình Hội nghị Cơ điện tử toàn quốc lần thứ 6 681 Mã bài: 149 ),( AA yxA ),( BB yxB ),( 11 yxP ),( 22 yx C )( A C )( B C m y Hình 3. Góc lệch của vật mốc so với hệ tọa độ môi trường Góc lệch của vật mốc so với hệ tọa độ đặt trên robot ( ) r được xác định dựa vào tọa độ tâm của vật mốc trên ảnh thu được từ camera như trong Hình 4. Tùy thuộc vào tọa độ tâm của vật mốc theo trục x mà góc lệch này được xác định như sau: Nếu tọa độ tâm vật mốc > 320 thì: ( ) 0 ( ) 90 r r (1) Nếu tọa độ tâm vật mốc ≤ 320 thì: ( ) 0 ( ) 90 r r (2) với 0 ( ) ( 320 )57 640 vm r x là góc lệch giữa tâm của vật mốc đang xét so với tâm khung ảnh theo trục x , vm x là tọa độ tâm vật mốc đang xét theo trục x trên khung ảnh (pixel). Hằng số 320 là tọa độ tâm khung ảnh theo trục x và 640 là giá trị giới hạn độ lớn của ảnh thu được từ camera (pixel), 0 57 là giới hạn góc nhìn của camera Kinect theo chiều ngang. 640x480 pixel x y (0,0) 0 57 ),( vmAvmA yxA ),( vmBvmB yxB Camera ) ( r y ) ( r x )(r Ảnh thu được qua camera Tọa độ tâm khung ảnh (320,240) 0 57 a) b) )(r )(r Hình 4. Góc lệch của vật mốc so với hệ tọa độ đặt trên robot Hướng của robot trong môi trường làm việc (Hình 5) được xác định: 0 90 (3) Với là góc lệch giữa trục ( ) r x của hệ tọa độ đặt trên robot và trục x của hệ tọa độ môi trường được xác định bởi công thức: ( ) r (4) ) ( r x ) ( r y ),( 11 yxP )(r ),( BB yxB ),( AA yxA Hình 5. Các góc biểu diễn hướng của robot trong hệ tọa độ môi trường 3. Thuật toán định vị sử dụng bộ lọc Kalman 3.1 Định vị sử dụng bộ lọc Kalman Thuật toán định vị sử dụng phương pháp hình học dựa vào việc nhận dạng hai vật mốc cho kết quả sai lệch khi giá trị khoảng cách từ robot đến hai vật mốc thu được bởi camera không chính xác do ảnh hưởng bởi các yếu tố như cường độ chiếu sáng, kích thước vật mốc và góc nhìn…Do đó bộ lọc Kalman được sử dụng để kết hợp thông tin ước lượng nội và thông tin đo đạc trực tiếp từ cảm biến để tăng độ chính xác cho thuật toán định vị. Hình 6 mô tả quy trình tổng quát của bộ lọc Kalman [7]. Hình 6. Quy trình tổng quát của bộ lọc Kalman Cập nhật thời gian (Dự đoán) 1. Ước lượng giá trị kế tiếp 1 1 k A Bu x x k k 2. Ước lượng ma trận covariance kế tiếp 1 T k k P AP A Q Cập nhật giá trị đo (Hiệu chỉnh) 1.Tính độ lợi Kalman 1 ( ) k T T k k P H HP H R K 2.Cập nhật lại giá trị đo k z ( ) kk k k x x Hx K z 3.Cập nhật lại ma trận covariance ( ) k k k P I K H P Khởitạo 1 k x và 1 k P 682 Lê Hoàng Anh, Ngô Văn Thuyên VCM2012 3.2 Xây dựng bộ lọc Kalman cho mô hình robot di động thực Tọa độ vị trí của robot trong môi trường x , y và hướng góc được sử dụng như là các biến trạng thái của bộ lọc Kalman [2]. Giá trị quãng đường di chuyển của hai bánh xe và tọa độ của robot xác định bởi thuật toán định vị được xem như là ngõ vào của hệ thống. Phương trình động học mô tả vị trí và hướng của robot trong môi trường được xác định dựa vào mô hình di chuyển theo kiểu lái vi sai: 1 cos( ) 2 x k k f x x s (5) 1 sin( ) 2 y k k f y y s (6) 1k f (7) Trong đó 2 r l s s s , r s , l s là quãng đường đi được của bánh xe bên phải và bánh xe bên trái, r l s s b , b là khoảng cách giữa hai bánh xe. Do đó vectơ trạng thái của hệ thống có thể được viết lại như sau: T k k k k X x y với vectơ ngõ vào T k r l u s s và hàm hệ thống ( ) T x y f x f f f trong đó các hàm thành viên của ( ) f x là các phương trình (6), (7) và (8). Ma trận Jacobian A hệ thống và B ngõ vào của mô hình robot di động này được xác định như sau: 1 0 sin( ) 2 0 1 cos( ) 2 0 0 1 k k x x x k k k y y y k k k k x k k k x f f f s x y f f f A s x y f f f x y (8) k x x l r y y k l r l r x f f s s f f B s s f f s s = 1 1 cos( ) sin( ) cos( ) sin( ) 2 2 2 2 2 2 2 2 1 1 sin( ) cos( ) sin( ) cos( ) 2 2 2 2 2 2 2 2 1 1 k x s s b b s s b b b b (9) Vectơ đo đạc của hệ thống T k z x y gồm ba thành phần và trực tiếp có được từ thuật toán định vị dựa vào các vật mốc nên ma trận Jacobian của vectơ đo H : 1 0 0 0 1 0 0 0 1 H (10) Độ lệch tiêu chuẩn nhiễu vị trí của hệ thống đối với tọa độ x , y là 0.01 x y m (phương sai 2 2 4 2 10 x y m ) và độ lệch tiêu chuẩn nhiễu về hướng 0 0,5 (phương sai 2 5 7,62.10 2 rad ) do đó ma trận hiệp phương sai số nhiễu hệ thống và phép đo được chọn: 4 4 5 10 0 0 0 10 0 0 0 7,62.10 Q (11) 4 4 5 10 0 0 0 10 0 0 0 7,62.10 R (12) 4. Kết quả thực nghiệm Thuật toán định vị dựa trên việc nhận dạng các vật mốc trong môi trường phụ thuộc vào nhiều yếu tố như cường độ chiếu sáng, kích thước vật mốc, màu sắc, vị trí đặt vật mốc và góc nhìn… Để có thể đạt được hiệu quả tốt nhất thì thuật toán phải được thực hiện trong môi trường có ánh sáng đều, kích thước vật mốc phải đủ lớn để robot có thể phát hiện ra vật mốc ở khoảng cách cho phép của camera, màu sắc của vật mốc phải tương phản với màu nền và không trùng với các màu có trong môi trường. Kết quả thực nghiệm thuật toán định vị được thực hiện bằng cách điều khiển robot di chuyển với quỹ đạo được trình bày trong Hình 7. Kết quả này được so sánh với đường di chuyển thực của robot trong môi trường và thuật toán định vị có sử dụng bộ lọc Kalman như được trình bày trong Hình 8, Hình 9, Hình 10 và Hình 11. Tuyển tập công trình Hội nghị Cơ điện tử toàn quốc lần thứ 6 683 Mã bài: 149 Hình 7. Không gian môi trường thực nghiệm thuật toán định vị 0 60 120 180 240 300 360 0 60 120 180 240 300 360 420 X [cm] Y [cm] Goals Desired trajectory Localization Hình 8. Thuật toán đinh vị với quỹ đạo gồm 10 điểm đích 0 60 120 180 240 300 360 0 60 120 180 240 300 360 420 X [cm] Y [cm] Goals Desired trajectory Ground truth Localization Hình 9. So sánh thuật toán định vị với đường di chuyển thực của robot 0 60 120 180 240 300 360 0 60 120 180 240 300 360 420 X [cm] Y [cm] Goals Desired trajectory Ground truth Kalman Localization Hình 10. So sánh thuật toán định vị sử dụng bộ lọc Kalman với đường di chuyển thực 0 60 120 180 240 300 360 0 60 120 180 240 300 360 420 X [cm] Y [cm] Goals Desired trajectory Ground truth Localization Kalman Localization Hình 11. Kết quả so sánh tổng hợp thuật toán định vị Kết quả thực nghiệm cho thấy robot có khả năng định vị dựa trên nhận dạng các vật mốc bằng camera. Nhờ xác định được vị trí mà robot có thể bám theo quỹ đạo mong muốn tạo ra từ thuật toán A*. Khi thuật toán lọc Kalman được áp Các thông số kỹ thuật của mô hình robot di động 1 Cơ cấu di chuyển Lái vi sai 2 Khối lượng 5kg 3 Khoảng cách hai bánh xe 45 cm 4 Đường kính bánh xe 8 cm 5 Tốc độ di chuyển tối đa 0.2 m/s 6 Công suất động cơ 20w 7 Khả năng tải nặng 10kg 8 Chiều cao đặt camera so với mặt sàn 43 cm 684 Lê Hoàng Anh, Ngô Văn Thuyên VCM2012 dụng, robot có thể xác định vị trí của nó chính xác hơn. 5. Kết luận Camera Kinect có giá thành hợp lý nhưng lại có khả năng cung cấp thông tin về độ sâu của từng điểm trong khung ảnh tương đối chính xác vì vậy ngày càng được dùng nhiều trong các ứng dụng định vị cho robot di động. Thuật toán hình học có thể được sử dụng để xác định vị trí của robot nhờ việc nhận dạng và xác định vị trí các vật mốc nhân tạo trong môi trường làm việc. Việc sử dụng thuật toán lọc Kalman để kết hợp ước lượng dựa trên mô hình và giá trị đo đạc bên ngoài cho kết quả định vị chính xác hơn. Hình 12. Mô hình robot thực tế Bảng 2. Thông số kỹ thuật mô hình robot di động Tài liệu tham khảo 1. Roland, S. and N. Illah R, Introduction to Autonomous Mobile Robots, 1st ed, 2004, London: The MIT Press. 2. Negenborn, R., Robot Localization and Kalman Filters, in Institute of Information and Computing Sciences2003, UTRECHT UNIVERSITY. 3. Bais, A. and R. Sablatnig, Landmark Based Global Self-localization of Mobile Soccer Robots, ACCV 2006: p. 842-851. 4. Tsukiyama, T., Mobile Robot Localization from Landmaek Bearings, XIX IMEKO World Congress, 2009: p. 2109-2112. 5. Stroupe, A.W., K. Sikorski, and a.T. Balch, Constraint-Based Landmark Localization, Submitted to Proc IROS 2002. 6. Jang, G., et al., Robust Self-localization of Mobile Robots using Artificial and Natural Landmarks, IEEE 2003, CIRA 2003: p. 412-417. 7. Welch, G. and G. Bishop, An Introduction to the Kalman Filter, 2006, ACADEMIC PRESS, INC: New York. p. 42. Lê Hoàng Anh nhận bằng tốt nghiệp kỹ sư ngành Điện Tử Viễn Thông tại trường Đại học Lạc Hồng năm 2006 và nhận bằng thạc sĩ ngành Kỹ Thuật Điện Tử tại trường Đại học Sư Phạm Kỹ Thuật TP.HCM năm 2012. Hướng nghiên cứu chính là robot di động. Hiện nay anh đang công tác tại khoa Điện- Điện Tử trường Đại học Lạc Hồng. Ngô Văn Thuyên tốt nghiệp kỹ sư ngành Điện năm 1999 tại Đại học Sư Phạm Kỹ Thuật TP.HCM, nhận bằng thạc sĩ ngành Kỹ Thuật Điện tại Đại học Bách Khoa TP.HCM năm 2003 và nhận bằng tiến sĩ tại Đại học Công nghệ Sydney (Australia) năm 2008. Hướng nghiên cứu chính là quy hoạch quỹ đạo, định vị cho robot di động và điều khiển thông minh. Hiện nay tiến sĩ Ngô Văn Thuyên đang công tác tại khoa Điện-Điện Tử trường Đại học Sư Phạm Kỹ Thuật TP.HCM. . nhiều trong các ứng dụng định vị cho robot di động. Thuật toán hình học có thể được sử dụng để xác định vị trí của robot nhờ việc nhận dạng và xác định vị trí các vật mốc nhân tạo trong môi. biểu di n hướng của robot trong hệ tọa độ môi trường 3. Thuật toán định vị sử dụng bộ lọc Kalman 3.1 Định vị sử dụng bộ lọc Kalman Thuật toán định vị sử dụng phương pháp hình học dựa vào. nghị Cơ điện tử toàn quốc lần thứ 6 679 Mã bài: 149 Định vị cho Robot di động sử dụng camera và vật mốc Localization for mobile robot using camera and landmarks Lê Hoàng Anh Trường Đại học