Báo cáo dự án thiết kế mô phỏng robot tự hành với thuật toán slam, amcl, rrt trong matlab và ros

TRƯỜNG ĐẠI HỌC BÁCH KHOA HÀ NỘI VIỆN CƠ KHÍ BỘ MÔN CƠ ĐIỆN TỬ BÁO CÁO DỰ ÁN THIẾT KẾ HỌC PHẦN: ROBOT TỰ HÀNH MÃ HỌC PHẦN: ME4512 Sinh viên thực hiện: Bùi Đức Huy 20184479 Phùng Minh Quang 20184592 Giảng viên hướng dẫn: TS Mạc Thị Thoa TS Phạm Đức An TS Nguyễn Xuân Thuận Đánh giá giảng viên hướng dẫn …………………………………………………………………………………… …………………………………………………………………………………… …………………………………………………………………………………… …………………………………………………………………………………… …………………………………………………………………………………… …………………………………………………………………………………… …………………………………………………………………………………… …………………………………………………………………………………… …………………………………………………………………………………… …………………………………………………………………………………… …………………………………………………………………………………… …………………………………………………………………………………… …………………………………………………………………………………… …………………………………………………………………………………… …………………………………………………………………………………… …………………………………………………………………………………… …………………………………………………………………………………… …………………………………………………………………………………… …………………………………………………………………………………… …………………………………………………………………………………… …………………………………………………………………………………… …………………………………………………………………………………… …………………………………………………………………………………… …………………………………………………………………………………… Kết đánh giá Họ tên Điểm Hà Nội, ngày 25 tháng năm 2021 Giảng viên hướng dẫn Bùi Đức Huy Phùng Minh Quang Lời cảm ơn Được hướng dẫn giúp đỡ nhiệt tình thầy cô TS Mạc Thị Thoa,TS Nguyễn Xuân Thuần TS Phạm Đức An, chúng em hoàn thành tập lớn giao “Mô Robot tự hành với thuật toán SLAM, AMCL, RRT Matlab ROS” Bởi thời gian kiến thức có giới hạn, khơng tránh khỏi sai sót ngồi ý muốn Do chúng em mong bảo đóng góp ý kiến thầy để đề tài hoàn thành Chúng em xin chân thành cảm ơn hướng dẫn TS.Mạc Thị Thoa,TS.Nguyễn Xuân Thuần TS.Phạm Đức An giúp chúng em hoàn thành đề tài MỤC LỤC CHƯƠNG GIỚI THIỆU CHUNG 1.1 Lý chọn đề tài 1.2 Giới thiệu sản phẩm liên quan Kích thước khối lượng Thành phần cấu tạo TurtleBot3 1.3 ROS Mô hình ROS Tầng ROS Filesystem Tầng ROS Computation Graph Tầng ROS Community CHƯƠNG CƠ SỞ LÝ THUYẾT VỀ ĐỘNG HỌC, SLAM, NAVIGATION 2.1 Động học Thuận 2.2 Động học nghịch 2.3 SLAM 10 2.4 Costmap 11 2.5 DWA 13 2.6 AMCL 14 CHƯƠNG KẾT QUẢ MÔ PHỎNG 15 3.1 ĐỘNG HỌC 15 3.2 SLAM 16 3.3 NAVIGATION 18 CHƯƠNG TÀI LIỆU THAM KHẢO 19 DANH MỤC HÌNH VẼ Hình - TurtleBot3 Burger TurtleBot3 Waffle Pi Hình - Kích thước TurtleBot3-Burger Hình - TurtleBot3 Waffle Pi Hình - Cấu tạo Turtlebot3 Burger Hình - Mơ hình ROS Filesystem Hình - Tầng ROS Computation Graph Hình - Giao tiếp nodes Hình - Hệ toạ độ Robot Hình – Quan hệ khoảng cách đến chướng ngại vật giá trị Costmap 12 Hình 10 - Biểu diễn Costmap 12 Hình 11 – Hệ toạ độ v - w 13 Hình 12 – Vận tốc dài vận tốc góc 14 Hình 13 – Quá trình sử dungj AMCL 15 Hình 14 – Mơ Matlab 15 Hình 15 – Mơ Simulink 16 Hình 16 - Robot mơi trường gazebo 17 Hình 17 - Cảm biến lazer quét bắt đầu 17 Hình 18 - Bản đồ lưu 17 Hình 19 - Dùng 2D pose đặt robot vị trí ngẫu nhiên 18 Hình 20 - Robot di chuyển theo đường mà thuật toán chọn 18 Hình 21 - Kết cuối cùng…………………………………………………….19 CHƯƠNG GIỚI THIỆU CHUNG 1.1 Lý chọn đề tài 1.2 Giới thiệu sản phẩm liên quan Khơng tìm thấy mục nhập bảng hình minh họa Hình 1- TurtleBot3 Burger TurtleBot3 Waffle Pi TurtleBot robot tảng tiêu chuẩn ROS Turtle có nguồn gốc từ robot Turtle, điều khiển ngôn ngữ lập trình máy tính giáo dục Logo vào năm 1967 Ngồi ra, xuất lần đầu hướng dẫn ROS, chương trình bắt chước hệ thống lệnh Logo rùa chương trình Nó sử dụng để tạo biểu tượng Rùa biểu tượng ROS Chín chấm sử dụng logo ROS có nguồn gốc từ mai sau rùa TurtleBot, có nguồn gốc từ Logo rùa, thiết kế để dễ dàng dạy người làm quen với ROS thông qua TurtleBot dạy ngôn ngữ lập trình máy tính sử dụng Logo Kể từ TurtleBot trở thành tảng tiêu chuẩn ROS, tảng phổ biến nhà phát triển sinh viên TurtleBot3 robot di động nhỏ, giá phải chăng, lập trình, dựa ROS để sử dụng giáo dục, nghiên cứu, sở thích tạo mẫu sản phẩm Mục tiêu TurtleBot3 giảm đáng kể kích thước tảng hạ giá thành mà hy sinh chức chất lượng nó, đồng thời cung cấp khả mở rộng TurtleBot3 tùy chỉnh theo nhiều cách khác tùy thuộc vào cách bạn tái tạo phận khí sử dụng phận tùy chọn máy tính cảm biến Ngoài ra, TurtleBot3 phát triển với SBC hiệu chi phí kích thước nhỏ, phù hợp với hệ thống nhúng mạnh mẽ, cảm biến khoảng cách 360 độ công nghệ in 3D Công nghệ cốt lõi TurtleBot3 SLAM , Điều hướng Thao tác , làm cho phù hợp với robot dịch vụ gia đình TurtleBot chạy thuật tốn SLAM (bản địa hóa lập đồ đồng thời) để xây dựng đồ lái xe xung quanh phịng bạn Ngồi ra, điều khiển từ xa từ máy tính xách tay, bàn di chuột điện thoại thông minh chạy Android TurtleBot theo dõi chân người họ phịng Ngồi ra, TurtleBot3 sử dụng trình điều khiển di động có khả điều khiển đối tượng cách gắn trình điều khiển OpenMANIPULATOR OpenMANIPULATOR _có ưu điểm tương thích với Waffle TurtleBot3 Waffle Pi Thơng qua khả tương thích bù đắp cho thiếu tự có hồn thiện robot dịch vụ với SLAM khả điều hướng mà TurtleBot3 có Kích thước khối lượng Hình 2: Kích thước TurtleBot3-Burger Hình 3-TurtleBot3 Waffle Pi Thành phần cấu tạo TurtleBot3 Hình Cấu tạo Turtlebot3 Waffle 1.3 ROS ROS hệ thống framework dùng cho ứng dụng robot Về bản, ROS có đặc tính thiết yếu hệ điều hành khả thực tác vụ (task) song song, giao tiếp, trao đổi liệu với tác vụ, quản lý liệu… ROS phát triển riêng biệt thư viện, công cụ hỗ trợ việc vẽ đồ thị, lưu liệu, hiển thị model 3D robot… ROS phát triển dạng module Từ nhóm nghiên cứu chuyên sâu vào lĩnh vực vẽ đồ hay định vị robot xây dựng hệ thống robot hoàn chỉnh tái sử dụng module phát triển trước Nhóm chọn hệ thống ROS để phát triển robot với khả vẽ đồ định vị dựa ưu điểm sau: • ROS cung cấp nhiều module giúp người dùng tuỳ biến cho phù hợp với yêu cầu phát triển loại robot • Những module hoạt động độc lập với trao đổi thơng qua gói liệu Vì dễ phát sửa lỗi cho module • ROS có mã nguồn mở cộng động nghiên cứu lĩnh vực robot phát triển Lượng người dùng lớn giúp cho hệ thống ROS ngày đa dạng phong phú tính liệu Những lỗi phát sinh trình sử dụng cộng đồng người dùng hỗ trợ xử lý nhanh chóng Về mặt trao đổi liệu giao tiếp ROS: ROS tích hợp vài chuẩn giao tiếp khác nhau, bao gồm giao tiếp đồng theo chuẩn RPC qua services, truyền liệu bất đồng qua topics, lưu trữ liệu Parameter Server Hiện ROS chạy tảng Linix Phần mềm cho ROS chủ yếu thử nghiệm Ubuntu Mac OS X Cho đến cộng đồng ROS bắt đầu xây dựng cho Fedora, Gentoo, Arch Linux tảng Linux khác ROS chưa hỗ trợ Microsoft Windows Hệ điều hành ROS với công cụ thư viện hỗ trợ thường phát hành dạng ROS Distribution, tương tự Linux distribution, cung cấp phần mềm để người dùng sử dụng, xây dựng phát triển Mơ hình ROS Mơ hình ROS chia làm ba tầng: Filesystem, Computation Graph, Community Tầng ROS Filesystem Filesystem nguồn tài nguyên ROS lưu trữ nhớ hệ thống, bao gồm thành phần như: • Packages: chứa lệnh thực thi ROS (các nodes), thư viện phụ thuộc ROS, tập liệu, file cấu hình, liệu cần thiết khác hệ thống • Manifests (manifests.xml): cung cấp sở liệu package, bao gồm license, cờ biên dịch • Stacks: tập hợp packages phối hợp với để thực chức (vd: Navigation stack) • Stack Manifests (stack.xml): cung cấp sở liệu stack, bao gồm license thông số phụ thuộc vào stack khác • Message(msg): cấu trúc liệu cho việc truyền nhận ROS • Service (srv): định nghĩa cấu trúc liệu cho lệnh truy cập (request) phản hồi (response) services ROS Hình -Mơ hình ROS Filesystem Tầng ROS Computation Graph Hình 6-Tầng ROS Computation Graph “Computation Graph”, gọi lược đồ tính tốn, mạng peer-topeer ROS liệu xử lý với Computation Graph gồm thành phần: Nodes, Master, Parameter Server, Messages, Services, Topics, Bags, tất thành phần cung cấp liệu cho Graph phương thức khác • Nodes: đơn vị thực lệnh tính tốn, xử lý liệu Hệ điều hành ROS thiết kế mơ-đun hố, mơ-đun xây dựng chi tiết chuyên biệt, hệ thống điều khiển robot thường bao gồm nhiều nodes Ví dụ, node điều khiển hệ thống cảm biến, node điều khiển động bánh xe, node thực việc tác vụ định vị, node hoạch định đường đi, node vẽ quỹ đạo hệ thống… Mỗi node ROS xây dựng nhờ sử dụng ROS client library, chẳng hạn roscpp rospy • Master: cung cấp tên đăng ký tra cứu đến node cịn lại Nếu khơng có Master, node khơng thể tìm thấy nhau, trao đổi thơng tin, hay gọi services khơng thực • Parameter Server: phần Master • Messages: Các node giao tiếp với thông qua message Một message đơn giản cấu trúc liệu, bao gồm trường định kiểu (integer, floating point, array ) • Topics: Messages định tuyến thông qua hệ thống trao đổi (transport system), phân loại thơng điệp thành dạng: publish (đưa tin) subcribe (đăng ký nhận thông tin) Một node gửi message việc đưa thông tin tới topic (chủ đề) Tên topic dùng để quy định nội dung message • Service: thực yêu cầu tương tác theo kiểu request/reply • Bags: định dạng để lưu phát lại liệu từ ROS messages Bags chế quan trọng để lưu trữ liệu, chẳng hạn liệu cảm biến Hình Giao tiếp nodes Tầng ROS Community Tầng định nghĩa tài nguyên phần mềm kiến thức chia cộng đồng phát triển ROS bao gồm: Distributions, Respositories, The ROS Wiki, Mailing lists CHƯƠNG CƠ SỞ LÝ THUYẾT VỀ ĐỘNG HỌC, SLAM, NAVIGATION 2.1 Động học Thuận Hình Hệ toạ độ robot Xét hệ qui chiếu cố định I hệ qui chiếu gắn liền với robot R Vị trí robot hệ qui chiếu cố định x  I   y    Ma trận cosin hướng cos( )  sin( )  R( )   sin( ) cos( )   0  Ma trận vận tốc robot hệ qui chiếu R  r1 r2       R     r r   1  2l   2l Ma trận vận tốc robot hệ qui chiếu cố định  r1 r2      cos( )         r  r    I  R( )R      sin( )       r1  r2    2l 2l   Phương pháp Euler  y '(t )  f (t , y(t ))  Cho  y(t0 )  y0 => t  t  h  n 1 n yn 1  yn  hf (tn , yn ) Áp dụng phương pháp Euler, ta xác định vị trí robot hệ qui chiếu cố định thời điểm ti+1 thơng qua vị trí thời điểm ti    r1 r2   xi  (ti 1  ti )    cos(i )      xi 1       r  r     I   yi 1    yi  (ti 1  ti )    sin(i )     i 1       r   r      i  (ti 1  ti )    2l    2l  2.2 Động học nghịch q  Xét điểm Q thân robot có tọa độ theo hệ quy chiếu R  x  q  y  Tọa độ điểm Q theo hệ qui chiếu cố định I  xQI   xPI  cos( )  sin( )   qx   I  I    với  yQ   yP   sin( ) cos( )   q y   xQI   I  tọa độ điểm Q theo hệ qui chiếu I  yQ   xPI   I  tọa độ điểm P theo hệ qui chiếu I  yP  Đạo hàm vế ta có:  xQI   xPI    sin( )  cos( )   qx   I I      yQ   y P    cos( )  sin( )   q y   xPI  v cos( )   xQI  cos( ) qx sin( )  qy cos( )  v   I    I       với  y P    v sin( )  sin(  ) q cos(  )  q sin(  )  y x y  Q              v   cos( )  qx sin( )  q y cos( )  qx cos( )  q y sin( )       sin( )    1  xQI   I  yQ     qy   qy   cos( )    sin( ) sin( )    cos( )  I v   qx   qx    xQ         y I  1 1      Q     sin( )   cos( )    qx   qx  2.3 SLAM Simultaneous localization and mapping (SLAM) tốn tính tốn xây dựng cập nhật đồ môi trường không xác định đồng thời theo dõi vị trí tác nhân bên Mặc dù ban đầu tốn gà trứng có số thuật tốn biết đến để giải nó, khoảng thời gian giải cho số môi trường định Các phương pháp giải gần phổ biến bao gồm lọc hạt, lọc Kalman mở rộng , giao điểm hiệp phương sai GraphSLAM Các thuật toán SLAM dựa khái niệm hình học tính tốnvà thị giác máy tính , sử dụng điều hướng robot , lập đồ robot đo đường cho thực tế ảo thực tế tăng cường Một số thuật toán SLAM: Gmapping, Kalman filter,…  Gmapping: Particle filter thuật toán phổ biến việc object tracking Ví dụ điển hình thuật tốn Monte Carlo localization sử dụng particle filter Bộ lọc Kalman trước đảm bảo độ xác cho hệ thống tuyến tính hệ thống mà nhiễu gassian áp dụng Hầu hết vấn đề giới thực hêj thống phi tuyến Bởi robot cảm biến phi tuyến, Particle filter thường sử dụng để pose estimation Nếu lọc Kalman phương pháp phân tích giả định hệ thống tuyến tính có tham số tuyến tính, Particle filter kỹ thuật để dự đốn thơng qua mô phương pháp thử sai Particle filter đặt tên giá trị ước tính tạo phân bố xác suất hệ thống biểu diễn particle Đây gọi phương pháp Sequential Monte Carlo Particle filter giống thuật tốn pose estimation khác, ước tính pose vât thể với giả định sai số đưuocj bao gốm input Khi sử dụng SLAM, giá trị đo lường robot giá trị đo cảm biến khoảng cách sử dụng để ước tính pose robot Trong phương pháp Particle filter, vị trí không chắn mô tả loạt particle gọi mẫu Chúng di chuyển hạt đến vị trí hướng ước tính dựa mơ hình chuyển động xác suất 10 robot, đồng thời đo trọng lượng particle theo giá trị đo thực tế giảm dần nhiễu để ước tính tư xác Trong trường hợp rơ bốt di động, hạt biểu diễn dạng hạt = pose (x, y, i), trọng lượng hạt particle tùy ý đại diện cho vị trí ước tính hướng rơ bốt biểu thị x, y, i robot độ ảnh hưởng particle Bộ lọc trải qua quy trình sau Ngoại trừ lần khởi chạy bước 1, bước ~ thực lặp lặp lại để ước tính tư rơ bốt Nói cách khác, phương pháp ước tính tư robot cách cập nhật phân bố particle thể xác suất robot mặt phẳng tọa độ X, Y dựa giá trị cảm biến đo Initialization Vì tư ban đầu robot (vị trí, hướng) khơng xác định, particle xếp ngẫu nhiên phạm vi mà tư thu với N particle Mỗi hạt ban đầu nặng / N, tổng trọng lượng hạt N xác định theo kinh nghiệm, thường hàng trăm Nếu vị trí ban đầu biết, hạt đặt gần robot Prediction Update Pose estimation Resampling 2.4 Costmap Tư robot ước tính dựa odemetry thu từ encoder IMU Và khoảng cách robot chướng ngại vật có nhờ cảm biến khoảng cách gắn robot Tư rô bốt cảm biến, thông tin chướng ngại vật đồ lưới thu từ SLAM sử dụng để tải đồ tối ưu khu vực có chướng ngại vật, khu vực trống khu vực không xác định để điều hướng Trong Navigation, costmap tính tốn khu vực chướng ngại vật, khu vực va chạm khu vực di chuyển rô bốt dựa bốn yếu tố nói Tùy thuộc vào loại điều hướng,costmap chia thành hai loại Một ‘global_costmap’, thiết lập kế hoạch đường để điều hướng khu vực tồn cầu đồ Loại cịn lại ‘local_costmap’ sử dụng để lập kế hoạch đường tránh chướng ngại vật khu vực giới hạn xung quanh rơ bốt Mặc dù mục đích chúng khác nhau, hai costmap biểu diễn theo cách Costmap biểu diễn dạng giá tị từu đến 255 Các giá trị tóm tắt sau:      000: Khu vưcj robot di chuyển hồn tồn tự 001 – 127: Khu vực có khả va chạm thấp 128 – 252: Khu vwcj có khả va chạm cao 253 – 254: Khu vực chắn xảy va chạm 255: Khu vực có chướng ngại vật, robot knoong thể di chuyển 11 Hình 9: Quan hệ khoảng cách đến chướng ngại vật giá trị costmap Hình 10: Biểu diễn Costmap 12 2.5 DWA Dynamic Window Approach (DWA) phương pháp phổ biến để lập kế hoạch tránh chướng ngại vật tránh chướng ngại vật Đây phương pháp lựa chọn tốc độ nhanh chóng đến điểm mục tiêu tránh chướng ngại vật va chạm với robot khơng gian tìm kiếm vận tốc Trong ROS, Trajectory planner sử dụng để lập kế hoạch đường cục bộ, DWA thay hiệu suất vượt trội Đầu tiên, Robot không hệ trục toạ đọ x-y, thay vào trọng hệ toạ độ với trục vận tốc dài vận tốc góc Trong hệ toạ độ robot có giá trị tối đa vận tốcbởi có hạn chế phần cứng, gọi Dynamic Window Trong Dynamic Window, Hàm G(v,w) sử dụng để tính v,w Giá trị tối ưu hố để robot tránh chướng ngại vật đến với địch nhanh Hình 11: Hệ toạ độ v-w 13 Hình 12: Vận tốc dài vận tốc góc 2.6 AMCL Mục đích thuật tốn để xác định xem robot đâu đồ cho trước Tức phải tìm pose robot Vì mục đích đó, thuật tốn tính tốn xác suất nơi robot có thẻ Đầu tiên, vị trí hướng (x, y, θ) rơ bốt thời điểm t ký hiệu xt thông tin khoảng cách thu từ cảm biến khoảng cách thời điểm t ký hiệu z0t = {z0, z1 , , zt}, thông tin chuyển động thu từ encoder thời điểm t u0t = {u0, u1, , ut} Sau đó, tính tốn niềm tin (xác suất hậu kỳ sử dụng cơng thức Bayes) với phương trình sau: Trong bước dự đốn, vị trí bel '(xt) robot khung thời gian tính mơ hình chuyển động p (xt | xt-1, ut-1) robot xác suất bel (xt-1) vị trí trước thơng tin chuyển động mà u nhận từ encoder: Sau bước cập nhật Lần này, mơ hình cảm biến p (zt | xt), xác suất bel (xt) số chuẩn hóa eta (ht) sử dụng để thu xác suất xác bel '(xt) dựa thông tin cảm biến: Tiếp theo trình SIR(Sampling Importance weighting Resampling): 14 Hình 13: Q trình sử dụng AMCL CHƯƠNG KẾT QUẢ MƠ PHỎNG 3.1 ĐỘNG HỌC Hình 14: Mơ Matlab 15 Hình 15: Mơ Simulink 3.2 SLAM Khởi tạo môi trường mô export TURTLEBOT3_MODEL=burger roslaunch turtlebot3_gazebo turtlebot3_world.launch Chạy Slam node Roslaunch turtlebot3_slam turtlebot3_slam.launch slam_methods:= gmapping Khởi tạo Node để điều khiển roslaunch turtlebot3_teleop turtlebot3_teleop_key.launch Lưu đồ rosrun map_server map_saver -f ~/map 16 Hình 16 - Robot mơi trường gazebo Hình 17 - Cảm biến lazer quét bắt đầu Hình 18- Bản đồ lưu 17 3.3 NAVIGATION Khởi chạy robot môi trường export TURTLEBOT3_MODEL=Burger roslaunch turtlebot3_gazebo turtlebot3_world.launch Chạy Navigation node roslaunch turtlebot3_navigation turtlebot3_navigation.launch map_file:=$HOME/map.yaml Khởi tạo node điều khiển roslaunch turtlebot3_teleop turtlebot3_teleop_key.launch Hình 19 - Dùng 2D pose đặt robot vị trí ngẫu nhiên Hình 20-Robot di chuyển theo đường mà thuật toán chọn 18 Hình 21: Kết cuối CHƯƠNG TÀI LIỆU THAM KHẢO Slide Robot tự hành https://emanual.robotis.com/ http://wiki.ros.org/ Download the 'ROS Robot Programming' Book for Free! - Learn Robotics - Robotsource 19 ... Thuần TS Phạm Đức An, chúng em hồn thành tập lớn giao “Mơ Robot tự hành với thuật toán SLAM, AMCL, RRT Matlab ROS? ?? Bởi thời gian kiến thức có giới hạn, khơng tránh khỏi sai sót ngồi ý muốn Do chúng... điều hành ROS với công cụ thư viện hỗ trợ thường phát hành dạng ROS Distribution, tương tự Linux distribution, cung cấp phần mềm để người dùng sử dụng, xây dựng phát triển Mô hình ROS Mơ hình ROS. .. GraphSLAM Các thuật toán SLAM dựa khái niệm hình học tính tốnvà thị giác máy tính , sử dụng điều hướng robot , lập đồ robot đo đường cho thực tế ảo thực tế tăng cường Một số thuật toán SLAM: Gmapping,