- Tìm hiểu nghiên cứu giao tiếp giữa Raspberry và mạch vi xử lý Arduino - Thiết kế phần cứng cho mobile robot và lập trình để điều khiển tốc độ động cơ cũng như là góc quay của robot.. 6
TỔNG QUAN
Giới thiệu chung
Robot tự hành, còn được gọi là AMR (Autonomous mobile robot) hoặc AGV (Autonomous guided vehicle) là những robot có khả năng di chuyển tự động Không giống những robot công nghiệp ít di chuyển hơn, robot tự hành có khả năng di chuyển trong những môi trường không được kiểm soát và có khả năng sử dụng cảm biến Và chúng di chuyển theo các con đường đã được định trước
Robot di động ngày nay đã trở nên phổ biến trong công nghiệp và đời sống Tất cả các lĩnh vực từ y tế, quân sự, giáo dục hay đời sống hay ngày đều có sự xuất hiện của robot di động Ví dụ như các bệnh viện cũng như nhà máy đã và đang sử dụgn robot di động trong nhiều năm để vận chuyển công cụ và vật liệu Trong nhà, robot hút bụi tự động đã làm sạch hơn và ngày càng phổ biến hơn với giá thành rẻ hơn Bên cạnh đó, robot di động cũng được sử dụng trong các nhiệm vụ như thám hiểm hoặc các nhiệm vụ liên quan đến nguy hiểm hoặc những môi trường nguy hiểm cho con người Đây là mục đích chính nhóm quyết định chọn ý tưởng “Robot di động trong nhà sử dụng lidar” làm đồ án cho khoá luận tốt nghiệp này của mình
Trong mô hình này, robot tự hành được thiết kế dựa trên hệ điều hành dành cho robot (Robot Operating System) là nền tảng cho việc thiết kế phần mềm, ứng dụng dành cho robot Bằng việc sử dụng ROS cho robot, để thực hiện điều khiển một cách dễ dàng và trực quan hơn so với việc xây dựng thuật toán điều khiển robot Giúp robot có thể tự định vị và xử lý được các tình huống trong môi trường thực tế.
Vấn đề đặt ra
Trong bài toán nghiên cứu liên quan đến robot di động, bài toán định vị và tìm đường là những vấn đề rất quan trọng và vẫn đang được nghiên cứu và phát triển các giải pháp hiệu quả Đối với một bài toán tìm đường, mục tiêu của bài toán này là tìm một đường đi thích hợp cho robot Các con đường phù hợp ở đây là các con đường đáp ứng tiêu chí như là để dẫn robot đến đích và con đường ngắn nhất đến đích Vấn đề này có thể giải quyết thủ công bằng các lập trình hoặc cũng có thể giải quyết bằng cách điều hướng cho robot Tuy nhiên, nhược điểm trong số các cách
2 trên là đường dẫn có thể không đúng hoặc có thể không phải là đường ngắn nhất và bản đồ phức tạp thì việc tìm đường càng khó
Lựa chọn thứ hai là sử dụng các thuật toán được tạo tự động Kết quả là, đường dẫn đến vị trí thích hợp sẽ tự động tìm thấy, tốc độ tìm đường sẽ được xác định bởi thuật toán được sử dụng Phương pháp này có ưu điểm là tìm đường dẫn tự động đã được chứng minh để xác định con đường ngắn nhất Tuy nhiên, để tìm đường tự động, thông tin về bản đồ là bắt buộc Ngoài việc tìm đường, robot di động cũng phải xử lý vấn đề bản đồ hoá để đạt được mục tiêu của nó.
Phương pháp nghiên cứu
Dựa vào môi trường hoạt động của robot là môi trường trong nhà, nhóm đã tìm hiểu và nghiên cứu thiết kế phần cứng của robot có thể chịu được tải trọng của một số linh kiện được đặt phía trên và dạng tròn để có thể dễ dàng di chuyển trong nhà Để di chuyển chính xác, mượt mà Quan trọng hơn hết là động cơ của robot phải chịu được tải trọng không tải cũng như có tải để robot di chuyển chính xác Nhóm tiến hành nghiên cứu đầu tiên là bộ điều khiển cho robot, có rất nhiều bộ điều khiển cho động cơ như là bộ điều khiển fuzzy, điều khiển trượt, điều khiển PID, … Nhóm lựa chọn bộ điều khiển PID cho đề tài vì tính kinh điển của bộ điều khiển PID được áp dụng rộng rãi trong các hệ thống điều khiển công nghiệp
Tiếp theo, nhóm tiến hành nghiên cứu mô hình ROS và ứng dụng của ROS cho mô hình của nhóm Nghiên cứu và thiết đặt ROS trên hệ điều hành Ubuntu được cài đặt trên máy tính nhúng Raspberry
Nghiên cứu cách sử dụng cảm biến lidar, thực hiện quét bản đồ và thiết đặt trục của robot trên ứng dụng của ROS Tìm kiếm thuật toán tìm đường đi ngắn nhất, tránh vật cản tối ưu cho robot và kiểm chứng.
Nội dung thực hiện đề tài
Các phần tiếp theo của đề tài có nội dung như sau:
Chương 2: Tổng quan hệ thống robot và cơ sở lý thuyết
Chương 2: Trình bày tổng quan về robot, yêu cầu cho robot, động học của robot và tổng quan kiến thức về hệ điều hành cho robot cùng các gói dữ liệu cần có để thực hiện để vận hành robot
Chương 3: Thiết kế và xây dựng phần cứng cho robot
Chương 3: Thiết kế phần cứng robot trên solidworks, tìm hiểu và lựa chọn các module, vi điều khiển cho robot Đồng thời thể hiện sơ đồ phần điều khiển của robot
Chương 4: Thiết kế chương trình điều khiển và lập trình tích hợp điều khiển robot
Chương 4: Thiết kế chương trình điều khiển động cơ và nghiên cứu thiết đặt các thông số để tối ưu việc tảo bản đồ và di chuyển tránh vật cản của robot ngoài thực tế
Chương 5: Kết quả thực nghiệm
Chương 5: Trình bày kết quả từ thực nghiệm của robot của phần cứng và phần mềm
Chương 6: Kết luận và hướng phát triển
Chương 6: Trình bày những vấn đề mà đề tài giải quyết được dựa trên các yêu cầu đề ra, cùng với khó khăn chưa giải quyết và đưa ra một số hướng phát triển để giúp đề tài hoàn thiện hơn
CƠ SỞ LÝ THUYẾT
Động lực học của Mobile robot
Hình 2.1: Hệ trục toạ độ của mobile robot Để có thể tìm ra bảng DH cho robot ta cần dựa theo 6 quy tắc bàn tay phải về cách đặt hệ trục tọa độ cho các khớp như sau i 𝑎 𝑖−1 𝛼 𝑖−1 𝑑 𝑖 𝜃 𝑖−1
Trong đó: ai-1 là khoảng cách từ trục zi-1 đến trục zi đo dọc theo trục xi-1, αi-1 là góc lệch từ trục zi-1 đến trục zi đo dọc theo trục xi-1, di là khoảng cách từ trục xi-1 đến trục xi đo dọc theo trục zi, θi là góc quay từ trục xi-1 đến trục xi đo dọc theo trục zi
2.1.1 Động học thuận của robot:
Từ bảng DH, với cách đặt hệ trục tọa độ như trên, ta có thể tính các ma trận chuyển đổi đồng nhất giữa các hệ trục tọa độ theo công thức sau:
Trong đó: 𝑐 𝜃 là 𝑐𝑜𝑠𝜃; 𝑠 𝜃 là 𝑠𝑖𝑛𝜃; 𝑐 𝛼 là 𝑐𝑜𝑠𝛼; 𝑠 𝛼 là 𝑠𝑖𝑛𝛼 Áp dụng công thức trên, ta tính được các ma trận chuyển động đồng nhất
Ma trận chuyển đổi đồng nhất giữa hệ trục 0 và hệ trục 1:
Ma trận chuyển đổi đồng nhất giữa hệ trục 1 và hệ trục 2:
Và ta có thêm ma trận chuyển đổi đồng nhất giữa hệ trục tại gốc toạ độ và tại ví trí bắt đầu của robot
Nhân các ma trận với nhau ta thu được ma trận chuyển đổi đồng nhất giữa hệ trục thứ 2 và hệ trục thức 0
Từ ma trận trên ta có được ma trận chuyển đổi đồng nhất giữa hệ trục thứ 2 và hệ trục tại toạ độ gốc (map) là:
Từ tọa độ điểm đầu cuối và ma trận chuyển đổi đồng nhất, ta có thể tính được tọa độ điểm đầu cuối so với hệ trục tọa độ 0, ta có phương trình động học thuận của robot là:
2.1.2 Tính toán động lực học của robot
Ta có toạ độ của robot sau khi di chuyển trên hệ trục là như sau:
Hình 2.2: Hệ trục toạ độ của robot
(𝑥 𝑟, 𝑦 𝑟 ) là toạ độ điểm cuối của robot
(𝑥 𝑐, 𝑦 𝑐 ) là toạ độ điểm đầu của robot
Chuyển vế phương trình trên, ta có:
(2.9) Đạo hàm cả hai vế theo thời gian, ta có:
Với phương trình 𝑥 𝑟 − 𝑥 𝑐 ta thu được quãng đường robot đi được, và khi đạo hàm theo thời gian ta thu được vận tốc của robot di chuyển trên quãng đường trên:
Từ phương trình trên, ta đặt biến 𝜔 là đạo hàm của góc 𝜃 theo thời gian ta có được:
Với 𝜔 là gia tốc góc (rad/s 2 )
Phương trình động lực học của robot tổng quát:
Hàm vi phân của robot
Tính động năng của robot:
𝑚 𝑖 là khối lượng ở khâu thứ i
𝑣 𝑐𝑖 𝑇 vận tốc tại khâu thứ i
𝜔 𝑖 là gia tốc góc của khâu thứ i
𝐼 𝑖 là moment quán tính của khâu thứ i
𝑔 𝑇 là gia tốc trọng trường
𝑃 𝑐𝑖 0 là vị trí trọng tâm khâu thứ i
𝑈 𝑟𝑒𝑓𝑖 là năng lượng thế năng tại gốc
Ta có vận tốc thẳng của robot được tính theo vận tốc bánh trái và vận tốc bánh phải:
Với 𝑣 𝑙 là vận tốc bánh trái; 𝑣 𝑟 là vận tốc bánh phải Động năng của robot là:
U = 0 vì phương của robot là phương ngang vuông góc với gia tốc trọng trường nên thế năng của robot bằng 0
Ta có hàm vi phân của robot:
Từ phương trình (2.21), (2.22) và (2.20), ta tính toán như sau:
Từ phương trình trên ta tìm được phương trình Euler Lagrange của từng động cơ:
Giới thiệu về hệ thống ROS
2.2.1 Giới thiệu về ROS (Hệ điều hành Robot):
ROS là một hệ thống framework dùng cho các ứng dụng robot ROS có những đặc tính thiết yếu của một hệ điều hành như khả năng thực hiện các tác vụ song song, trao đổi dữ liệu, quản lý dữ liệu Đồng thời, nó cũng đi kèm với các công cụ và thư viện để xây dựng và viết các chương trình cho nhiều máy tính hỗ trợ việc vẽ đồ thị, lưu dữ liệu, hiện thị model 3D của robot
Gói ROS của nó bao gồm SLAM, lập kế hoạch chuyển động, kiểm soát thao tác Về mặt trao đổi dữ liệu và giao tiếp trong ROS: ROS tích hợp một vài chuẩn giao tiếp khác nhau
Hình 2.3: Mô hình robot với nền tảng ROS của mobile robot
ROS gồm ba cấp khái niệm: Filesystem, Computation Graph và Community [1] Cấp thứ nhất – Filesystem: giải thích về các cấu trúc thư mục và các tập tin tối thiểu để ROS hoạt động
Cấp thứ hai – Computation Graph: là giao tiếp giữa các quá trình xử lý và hệ thống Quản lý các quá trình giao tiếp giữa nhiều máy tính với nhau, …
Cấp thứ ba – Community: Giải thích/ hướng dẫn các công cụ và các khái niệm để chia sẻ kiến thức, thuật toán chương trình xử lý bất kỳ nhà phát triển nào
Filesystem chủ yếu là các nguồn tài nguyên của ROS được thực thi trên bộ nhớ lưu trữ hệ thống, bao gồm:
Packages: là đơn vị chính để tổ chức phần mềm trên ROS Một package có thể chứa các nodes, các thư viện, các file hoặc bất cứ file nào cho việc tổ chức
Mục đích của package là để tạo ra tập hợp chương trình có kích thước nhỏ nhất để có thể dễ dàng sử dụng lại
Hình 2.4: Cấu trúc package trong ROS
Metapackages: là một pakage chuyên biệt, dùng để phục vụ cho việc thể hiện mối quan hệ một nhóm các pakage khác với nhau
Stacks: Khi chúng ta kết hợp các pakage với nhau với một vài chức năng thì được gọi là Stack Trong ROS, có rất nhiều stack với công dụng khác nhau Tương tự Pakage, nơi chứa thông tin về stack gọi là Stack Manifests
Hình 2.5: Cấu trúc Stack trong ROS
Message types: là mô tả của một thông điệp được gửi qua lại giữa các quá trình được lưu trữ dưới dạng my_pakage/msg/MyMessageType.msg Message định nghĩa cấu trúc dữ liệu cho các thông điệp được gửi đi
Computation Graph là một mạng nơi các quy trình trong ROS được kết nối với nhau Bất kỳ một node nào trong hệ thống cũng có thể truy cập vào mạng này, tương tác với các node khác, trao đổi các dữ liệu nằm trong mạng Các khái niệm cơ bản của Computation Graph là nodes, Master, Parameter Server, message, services, topics và bags
Nodes: là một quy trình đúng để tính toán, điều khiển Một node có thể được tạo ra khi biên dịch một package thành công và trong cùng một pakage có thể tạo nhiều node Khi một node muốn giao tiếp, tương tác các node khác thì bản thân node đó phải được kết nối với mạng ROS Trong một hệ thống, mỗi node sẽ có một chức năng khác nhau
Master: ROS master cung cấp một tên đăng ký và tra cứu phần còn lại của
Computation Graph Nếu không có ROS Master thì các node không thể tìm thấy nhau, trao đổi các message hay gọi các service
Parameter Server: cho phép dữ liệu được lưu trữ bởi các từ khoá trong một vị trí trung tâm Với các biến này, nó có thể được cấu hình các node trong khi nó đang hoạt động hoặc để thay đổi hoạt động của các node
Messages: các node giao tiếp với nhau thông qua message Một message đơn giản là một cấu trúc dữ liệu, bao gồm các trường được định nghĩa như integer, floating, point, boolean, … Messages có thể bao gồm các kiểu cấu trúc và mảng lồng vào nhau (giống kiểu struct trong C) Chúng ta có thể tự phát triển kiểu message dựa trên các message chuẩn
Topic: messages được định tuyến thông qua hệ thống vận chuyển, trong đó được phân loại thành publish và subcribe Một node sẽ gửi một messages bằng việc publishing message đó lên một topic định trước Topic chỉ là một cái tên để nhận nội dung của message Một node thì có thể subcribe một topic có tên và kiểu dữ liệu được khai báo Trong cùng một thời điểm, có thể nhiều publishers và subcribers cùng truy cập vào một topic, và một node có thể publish và subcribe nhiều topic
Services: mô hình publish/subcribe thì rất linh hoạt trong việc giao tiếp nhưng đặc điểm là truyền được đa đối tượng và một chiều Nhưng đôi khi lại không thích hợp cho việc truyền theo dạng request/reply, thường được dùng trong kiểu hệ thống phân bố Do đó, việc truyền nhận theo dạng request/reply được dùng thông qua services Sevices được định nghĩa một cặp cấu trúc dữ liệu: một cho request và một cho reply Một node cung cấp một service thông qua một thuộc tính name, và một client sử dụng service bằng việc gửi một request message và đợi phản hồi.
SLAM
2.3.1 Giới thiệu về SLAM (Simultaneous localization and mapping):
Simultaneous localization and mapping (SLAM) là công nghệ định vị và xây dựng bản đồ đồng thời Bằng cách sử dụng SLAM, các kết cấu hạ tầng có thể thay đổi dễ dàng
SLAM sử dụng cảm biến để quét tất cả những vùng xung quanh sau đó sẽ xây dựng và định hình một bản đồ ảo Nhờ đó mà mobile robot có thể di chuyển trong vùng an toàn và hiệu quả nhất
Là phương pháp sử dụng cảm biến laser hoặc cảm biến khoảng cách Cảm biến laser thường tạo ra điểm 2D hoặc 3D Đám mây điểm cảm biến laser cho phép khoảng cách chính xác cao và lý tưởng cho việc xây dựng bản đồ
Laser SLAM được trang bị cảm biến là Lidar trong môi trường trong nhà có thể giúp robot xây dựng bản đồ và giúp robot tự chủ đi lại Hệ thống SLAM laser tính toán khoảng cách di chuyển và sự thay đổi tư thế của lidar bằng các khớp và so sánh hai đám mây điểm tại thời điểm khác nhau, do đó hoàn thành định vị của chính robot trong bản đồ Bản đồ được xây dựng bằng laser SLAM có độ chính xác cao, không có sai số và có thể được sử dụng trực tiếp để định vị và dẫn đường Laser SLAM có thể tự động điều hướng mà không cần bất kỳ thiết bị định vị phụ trợ nào, có thể điều hướng tự do, tự động lập kế hoạch tuyến đường và di chuyển an toàn
2D Navigation stack dùng để lấy các thông tin, dữ liệu từ odometry, cảm biến, điểm mục tiêu và xuất ra tín hiệu vận tốc gửi xuống cho robot hoạt động
Một yêu cầu khi sử dụng Navigation tack về phần cứng như sau:
Nó chỉ gửi trực tiếp giá trị vận tốc xuống cho bộ phận điều khiển di chuyển của robot để đạt được các giá trị robot mong muốn như vận tốc theo trục x, vận tốc theo trục y và vận tốc xoay
Navigation stack cần được lắp một cảm biến laser scan được gắn lên robot Laser scan này dùng để xây dựng bản đồ (mapping), định vị (localization) và phát hiện vật cản
Hình 2.6: Mô hình Navigation stack trong ROS
Costmap 2D là một pakage để triển khai bản đồ chi phí từ việc lấy dữ liệu của cảm biến từ môi trường Costmap 2D dùng để chứa thông tin các vật cản cũng như những nơi mà robot không thể tới được Costmap sử dụng dữ liệu của các cảm biến từ bản đồ đã được xây dựng trước đó và cập nhật thông tin của vật cản vào costmap
Costmap được cấu hình đầy đủ, mỗi bit chức năng nằm trên một lớp khác nhau Bản đồ tĩnh được dựng trước đó được nằm trên một lớp, các vật cản mà lidar quét được nằm trên một lớp khác nhằm cho vấn đề xử lý các vật cản trong bản đồ thông minh hơn
Robot sẽ di chuyển trong map với hai định hướng đường đi là toàn cục (global) và cục bộ (local): định hướng đường đi toàn cục (global) navigation được dùng để tạo đường đi cho mục tiêu trong bản đồ hoặc đường đi có khoảng cách xa; định hướng đường đi cục bộ (local navigation) được dùng để tạo đường đi trong khoảng cách gần và để tránh vật cản khi robot di chuyển
16 Định hướng đường đi toàn cục thì dùng costmap toàn cục và costmap cục bộ dùng cho định hướng cục bộ Mỗi costmap sẽ có những thông số riêng của nó và cũng có những thông số dùng chung
Thuộc tính obstacle_range được dùng để chỉ khoảng cách tối đa mà cảm biến có thể đọc và nhận thông tin mới về môi trường costmap Nếu robot phát hiện ra vật cản có khoảng cách nhỏ hơn obstacle_range thì nó sẽ đặt một vật cản vào costmap Nếu robot di chuyển, nó có thể xoá costmap và cập nhật freespace Chúng ta chỉ có thể cập nhật các giá trị được trả về từ thông tin laser trên bề mặt vật cản mà không thể nhận biết đươc toàn bộ vật cản Với những thông tin này, ta đã có thể xây dựng được bản đồ
Thuộc tính inflation_radius là thông số để giữ khoảng cách nhỏ nhất giữa tâm của robot và vật cản
Thuộc tính laser_scan_sensor dùng để khai báo các thông tin về loại cản biến và được dụng để thêm/xoá vật cản trong costmap
Thông tin về khai báo biến cho file global_costmap_params Yaml
Thuộc tính global_frame và robot_base_frame định nghĩ các frame toàn cục và frame gắn với mobile base
Thuộc tính update_frequency dùng để khai báo tần số cập nhật cho costmap
Thuộc tính static_map để dùng khai báo có hay không dùng bản đồ đã dựng trước đó
Thông tin về khai báo biến cho file local_costmap_params Yaml
Thuộc tính Publish_frequency để xác định tần số cập nhật thông tin
Thuộc tính rolling_window dùng để giữ costmap nằm ở tâm robot khi di chuyển.
Thuật toán cho global planner và local planner
2.5.1 Thuật toán A* cho global planner
A* là giải thuật tìm kiếm trong đồ thị [1], tìm đường đi từ một đỉnh hiện tại đến đỉnh đích có sử dụng hàm để ước lượng khoảng cách hay còn gọi là hàm Heuristic
Từ trạng thái hiện tại A* xây dựng tất cả các đường đi có thể đi dùng hàm ước lượng khoảng cách (hàm Heuristic) để đánh giá đường đi tốt nhất có thể đi Tuỳ theo mỗi dạng khác nhau mà hàm Heuristic sẽ được đánh giá khác nhau A* luôn tìm được đường đi ngắn nhất nếu tồn tại đường đi như thế
A* lưu giữ một tập các đường đi qua đồ thị, từ đỉnh bắt đầu đến đỉnh kết thúc
Thứ tự ưu tiên cho một đường đi được quyết định bởi hàm Heuristic được đánh giá là:
Trong đó: g(x) là chi phí của đường đi từ điểm xuất phát cho đến thời điểm hiện tại h(x) là hàm ước lượng chi phí từ đỉnh hiện tại đến đỉnh đích f(x) thường có giá trị càng thấp thì độ ưu tiên càng cao
Hàm f(x) có giá trị càng thấp thì độ ưu tiên của x càng cao Để hiểu rõ hơn về các giá trị trong thuật toán, ta cần làm rõ hai định nghĩa sau: Khoảng cách giữa hai điểm A, B là độ dài đoạn thẳng ngắn nhất nối hai điểm đó lại với nhau (có thể cắt ngang qua vật cản) Chi phí giữa hai điểm A, B là độ dài đường đi cần thực hiện để đi từ điểm A đến điểm B (không được cắt qua vật cản)
Giả thiết môi trường hoạt động robot được chia thành mạng lưới các ô vuông với ma trận 4x4 như hình
Hình 2.7: Áp dụng thuật toán A* trên ma trận 4x4
Vị trí xuất phát có toạ độ là A(1,1), vị trí đích là B(1,4), các chấm tròn đen là vật cản Trong trường hợp này bước tiếp theo phải đi từ A chỉ có thể là node (2,1) Khi đi tới node (4,2) sẽ có hai node liền kề có thể được lựa chọn làm node kế tiếp Quá trình lựa chọn dựa vào hàm f(x) được tính toán như phương trình (2.1) trong đó: g(4,3) là chi phí từ node A(1,1) → (2,1) → (3,1) → (4,2) → (4,3), h(4,3) là khoảng cách giữa node (4,3) và node B g(3,3) là chi phí từ node A(1,1) → (2,1) → (3,1) → (4,2) → (3,3), h(3,3) là khoảng cách giữa node (3,3) và node B
Theo kết quả tính toàn được ở node (3,3) có chi phí f(x) nhỏ hơn, vì vậy sẽ chọn node (3,3) là node đi tiếp theo Thuật toán sẽ tiếp tục xét các node liền kề với node (3,3) cho đến khi đạt được node mong muốn
2.5.2 Thuật toán Dynamic Window Approach để tránh vật cản cho local planner
Thuật toán Dynamic Window Approach (DWA) là dùng để tìm ra một tín hiệu điều khiển hợp lý gửi xuống robot nhằm mục đích điều khiển nó đến đích an toàn, nhanh chóng dựa trên global planner đã hoạch định từ trước Thuật toán này gồm hai bước chính là cắt giảm không gian tìm kiếm của vận tốc và tìm được vận tốc tối ưu trong không gian tìm kiếm đó [2]
Các vận tốc có thể điều khiển được trong không gian tìm kiếm được cắt giảm theo ba bước sau:
- Quỹ đạo tròn: thuật toán chỉ xét đến quỹ đạo là hình tròn (đường cong) được xác định duy nhất bởi một cặp vận tốc thẳng và vận tốc xoay (𝑣, 𝜔)
- Vận tốc cho phép: nhằm tạo ra một quỹ đạo an toàn cho robot để tránh vật cản Một cặp vận tốc (𝑣, 𝜔) được cho phép là khi robot có thể dừng trước vật cản
19 gần nhất mà không có sự va chạm trên đường cong tương ứng với vận tốc đó Vận tốc cho phép được định nghĩa như sau:
• 𝑉 𝑎 là chuỗi các giá trị vận tốc (𝑣, 𝜔) cho phép robot dừng trước vật cản mà không có sự va chạm
• 𝑑𝑖𝑠𝑡(𝑣, 𝜔) là khoảng cách nhỏ nhất mà robot dừng trước vật cản để không có sự va chạm
• 𝑣 𝑏 ̇ và 𝜔 𝑏 ̇ là gia tốc của vận tốc thẳng và vận tốc xoay tối đa nếu robot di chuyển sẽ gây va chạm với vật cản
Hình 2.8: Vận tốc cho phép 𝑉 𝑎 trong DWA
- Dynamic window: nhằm hạn chế vận tốc cho phép đối với những vận tốc có thể đạt được trong khoảng chu kỳ cho trước với gia tốc tối đa của robot Để
∆𝑡 là khoảng thời gian mà trong đó gia tốc 𝑣 𝑏 ̇ , 𝜔 𝑏 ̇ là vận tốc thực được gửi xuống robot Từ đó, vận tốc 𝑉 𝑑 sẽ được định nghĩa như sau:
Hình 2.9: Vận tốc trong cửa sổ động 𝑉 𝑑 trong DWA Kết thúc ba bước trên ta tìm được không gian tìm kiếm:
Ta có hàm mục tiêu định nghĩa như sau:
𝐺(𝑣, 𝜔) = 𝛼 ℎ𝑒𝑎𝑑𝑖𝑛𝑔(𝑣, 𝜔) + 𝛽 𝑑𝑖𝑠𝑡(𝑣, 𝜔) + 𝛾 𝑣𝑒𝑙(𝑣, 𝜔) (2.29) Để có thể tối ưu hoá vận tốc ngõ ra thì hàm mục tiêu phải có giá trị tối đa Để thực hiện được điều này, ta thực hiện các bước sau:
Target heading: heading là giá trị đo tiến độ hướng đến đích của robot Giá trị sẽ mang giá trị tối đa khi robot di chuyển trực tiếp về phía đích Giá trị của ℎ𝑒𝑎𝑑𝑖𝑛𝑔(𝑣, 𝜔) được tính bởi công thức 180 − 𝜃, với 𝜃 là góc giữa hướng của robot và điểm đích
Hình 2.10: Heading của robot trong DWA
Không gian trống (clearance): hàm 𝑑𝑖𝑠𝑡(𝑣, 𝜔) thể hiện khoảng cách tính từ robot đến vật cản gần nhất nằm trên quỹ đạo cong của nó Giá trị này sẽ rất lớn nếu không có vật cản nằm trên quỹ đạo cong di chuyển của nó Giá trị này càng nhỏ thì việc nó đối mặt với vật cản càng cao khi đó nó sẽ di chuyển xung quanh vật cản ấy
Vận tốc: hàm 𝑣𝑒𝑙(𝑣, 𝜔) là vận tốc di chuyển thẳng của robot và hỗ trợ di chuyển nhanh hơn
Các hệ số 𝛼, 𝛽, 𝛾 được chọn sao cho phù hợp với đặc tính của robot và môi trường hoạt động Khi hàm mục tiêu có giá trị lớn nhất thì quỹ đạo tối ưu sẽ đươc chọn với vận tốc (𝑣, 𝜔) tốt nhất và là kết quả của thuật toán
Hình 2.11: Lưu đồ xác định mục tiêu của robot
Giao thức
UART hay bộ thu – phát không đồng bộ đa năng là một trong những hình thức giao tiếp kỹ thuật số giữa thiết bị với thiết bị đơn giản Các giao tiếp UART giữa hai nút riêng biệt bằng cách sử dụng một cặp dẫn và một nối đất chung
Về cơ bản, trong giao tiếp UART, có hai dây kết nối là RX và TX tương ứng với chức năng nhận dữ liệu và truyền dữ liệu Vì chỉ có 1 dây nên dữ liệu sẽ lần lượt truyền từng bit, từng bit 1, nên nó có kiểu truyền dữ liệu nối tiếp
Hình 2.12: Giao tiếp UART với data bus
Tốc độ Từ 9600 bps -> 115200 bps
Phương thức truyền dữ liệu Không đồng bộ
Kiểu truyền dữ liệu Nối tiếp
Số lượng Master (thiết bị chủ) 1
Số lượng Slave (Thiết bị tớ) 1
Bảng 2.2: Thông tin giao thức UART
Cả hai UART cũng phải được cấu hình để truyền và nhận cùng một cấu trúc gói dữ liệu Nghĩa là khi thiết lập ở vi điều khiển, các cấu hình tốc độ baud phải giống nhau
Chân Tx (truyền) của một chíp kết nối trực tiếp với chân Rx (nhận) của chip kia và ngược lại Thông thường, quá trình truyền nhận sẽ diễn ra ở 3.3V hoặc 5V UART là một giao thức một master, một slave, trong đó một thiết bị được thiết lập để giao tiếp với duy nhất một thiết bị khác
Dữ liệu truyền đến và đi từ UART song song với thiết bị điều khiển
Khi gửi trên chân Tx, UART đầu tiên sẽ dịch thông tin song song này thành nối tiếp và truyền đến thiết bị nhận
UART thứ hai nhận dữ liệu này trên chân Rx của nó và biến đổi nó trở lại thành song song để giao tiếp với thiết bị điều khiển của nó
ROS – Serial là một giao thức để gửi dữ liệu thông qua một giao diện nối tiếp Trong một máy tớ - máy chủ ROS thực hiện nối tiếp, một máy chủ ROS là một máy tính chạy chương trình ROS và một máy khách ROS là vi điều khiển nhận dữ liệu từ cảm biến và chuyển dữ liệu đó đến máy chủ dưới dạng tin nhắn ROS ROS máy chủ này là một node xuất bản (publishing) trong khi ROS khách là một máy đăng ký (subcriber)
Các gói máy khách ROS Serial có sẵn cho một số bộ vi xử lý như: Arduino, STM32, Linux, …Đồng thời, các gói máy chủ ROS serial có sẵn trong các phiên bản của Python hoặc C++
Hình 2.13: Tích hợp ROS cho robot
Điều khiển động cơ sử dụng thuật toán PID
2.7.1 Giới thiệu bộ điều khiển PID
Hình 2.14: Sơ đồ điều khiển PID cho robot
PID là thuật toán điều khiển tỉ lệ - tích phân – vi phân được sử dụng để điều khiển những đối tượng như nhiệt độ lò nhiệt, tốc độ, và vị trí động cơ, áp suất … PID là thuật toán điều khiển được sử dụng phổ biến trong các hệ thống điều khiển phản hồi công nghiệp hiện nay Để có thể điều khiển được đối tượng, PID phải lấy được giá trị đầu ra của y của hệ thống như hình 2.16 Sau đó so sánh với với giá trị đặt y0 để tìm được sai số e PID xử lý sai số e để tìm ra giá trị điện áp u tác động vào đối tượng giúp ngõ ra bám theo giá trị đặt
Bộ điều khiển PID gồm 3 khâu điều khiển là khâu tỉ lệ (P), tích phâ (I), và vi phân (D)
Ngõ ra của bộ điều khiển PID chính là tổng của ngõ ra ba khâu này:
Khâu tỉ lệ là tích giữa sai số hiện tại với hằng số độ lợi KP:
Khâu tích phân là tích giữa tổng sai số tức thời theo thời gian với độ lợi KI:
Khâu vi phân là tích giữa độ dốc sai số theo thời gian với độ lợi vi phân KD:
Vậy ngõ ra tác động:
Hàm truyển của bộ điều khiển liên tục:
Sự phát triển của hệ thống số yêu cầu cần có những bộ điều khiển rời rạc Do đó thuật toán điều khiển PID rời rạc cũng được sử dụng rộng rãi trên máy tính cũng như hệ thống nhúng Chuyển đổi tín về miền thời gian rời rạc cho từng khâu như sau:
(3.9) Với T là chu kỳ lấy mẫu (giây) Từ đó cho ta ngõ ra tác động u(kT) là:
2.7.2 Thiết kế bộ tham số PID
Có nhiều phương pháp để cân chỉnh PID cho hệ thống điều khiển như: cân chỉnh bằng phương pháp thủ công, dùng phương pháp Ziegler – Nichols, dùng phần mềm hỗ trợ cân chỉnh PID Tuỳ theo mức độ phức tạp và yêu cầu đặt ra mà mỗi phương pháp sẽ phù hợp với từng hệ thống khác nhau Nhóm nghiên cứu sử dụng cân chỉnh theo phương pháp sau:
Bước 1: Khởi tạp các giá trị cho KP, KI và KD ban đầu bằng 0
Bước 2: Tăng dần KP cho đến khi đáp ứng động cơ bắt đầu dao động Khi đó KP sẽ được chọn bằng ẵ giỏ trị bắt đầu dao động
Bước 3: Cân chỉnh KD để bộ điều khiển nhanh chóng trở về giá trị đặt khi bị vọt lố Trong hệ thống điều khiển động cơ, ở ngõ ra đã có một khâu tích phân lý tưởng nên giá trị KI sẽ được cho bằng 0
Quá trình cân chỉnh cho thấy với hệ số Kp = 12.82, Ki = 70, Kd = 0.09 cho đáp ứng nhanh và hệ thống hoạt động ổn định
THIẾT KẾ PHẦN CỨNG
Tổng quan phần cứng robot
3.1.1 Sơ đồ tổng quát hệ thống
Robot tự hành trong nhà là một hệ thống được thiết với khung xe nhỏ gọn, các ngoại vi và mạch điều khiển đáp ứng các yêu cầu xử lý cảm biến, tự hành tránh vật cản và điều khiển từ xa
Từ những yêu cầu trên phần cứng robot bao gồm: khung xe micar, cảm biến Lidar, vi điều khiển Arduino giao tiếp ngoại vi và máy tính, động cơ DC encoder, mạch cầu H để điều khiển động cơ, cảm biến IMU, nguồn cấp cho mạch cầu H và máy tính được cài đặt nền tảng ROS
Hình 3.1: Sơ đồ tổng quát mô hình
Hình 3.2: Sơ đồ khối chi tiết các module Robot Phần Power Supply là nguồn cung cấp cho robot hoạt động được cấp từ khay pin 3 pin 11.1V
30 Hình 3.3: Sơ đồ nối dây phần cứng của mạch điều khiển
Hình 3.4: Sơ đồ nối dây cho phần máy tính nhúng và vi điều khiển
Thiết kế khung xe robot
Với chức năng chính của robot là tự hành tránh vật cản trong nhà, robot được thiết kế hình tròn, kích thường 30x30 cm Với hình dạng đối xứng xe có thể dễ tính toán khoảng cách đến vật cản ở mọi hướng và dễ dàng tránh vật cản cũng như có thể đi qua không gian hẹp Robot được được thiết gồm 3 tầng chính
Tầng 1 được dùng làm khung chính của robot Với kích thường 30x30 cm được gắn các mạch điều khiển chính như: Nguồn, gá động cơ, động cơ, vi điều khiển Arduino, mạch cầu H L298 Vì là tầng điều khiển chính nên thiết kế nhiều lỗ để cố định các mạch
Hình 3.5: Thiết kế tầng 1 của robot
Tầng 2 của robot được thiết kế theo dạng tròn, tầng này được thiết kế để chứa
PC (Raspberry) và nguồn cho PC hoạt động để lấy dữ liệu và điều khiển hành vi cho robot
Hình 3.6: Thiết kế tầng 2 của robot
Tầng trên cùng của xe cũng được thiết kế theo dạng tròn có kích thường giống như tầng dưới Vì cảm biến Lidar yêu cầu có không gian rộng, tránh để các linh kiện khác che tầm quét của cảm biến Nên Lidar sẽ được gắn ở vị trí trên cùng
Hình 3.7: Thiết kế tầng trên cùng của robot
Tổng tải trọng đặt lên khung xe lên đến hơn 4kg Từ đó, vật liệu được sử dụng làm khung robot được lựa chọn là mica có độ dày 5mm Đặc trưng của vật liệu này là cú tỉ trọng bằng ẵ so với thuỷ tinh, chịu được lực và chịu nhiệt tốt Đồng thời, vật liệu cũng dễ dàng thiết kế, thao tác và rẻ hơn so với các vật liệu như nhôm, inox…
Sử dụng phần mềm Solidworks để thiết kế kh ung cho robot Bộ khung cơ bản bao gồm 3 tầng như đã trình bày
3.2.4 Gá động cơ và trụ đồng
Gá động cơ được thiết kế riêng dành cho động cơ GA-370, được cố định với đế của robot và gắn chặt với động cơ
Hình 3.8: Gá động cơ của robot Trụ đồng được sử dụng để làm trụ đỡ chia các tầng với nhau
Hình 3.9: Trụ động đỡ các tầng của robot
3.2.5 Tổng hợp các chi tiết cho phần cứng
Số thứ tự Tên Số lượng
Hình 3.10: Bảng tổng hợp các chi tiết của robot
Hình 3.11: Khung robot hoàn chỉnh trên Solidworks
Hình 3.12: Khung robot hoàn chỉnh trên Solidworks
Lựa chọn các thiết bị cho robot
3.3.1 Raspberry Pi 4 Model B Để có thể đáp ứng khả cũng như tốc độ xử lý, chúng ta cần board có khả năng xử lý mạnh mẽ để khởi chạy các thuật toán điều khiển có độ phức tạp cao Bên cạnh đó, nhằm giảm kích thước và tăng tính linh động cho sản phẩm thì việc chọn một bộ vi xử lý tích hợp có kích thước nhỏ là một sự lựa chọn hợp lý
Raspberry Pi 4B là sản phẩm mới nhất trong họ máy tính nhúng Raspberry Pi Cấu hình của Raspberry Pi 4B nổi bật với vi mạch Broadcom BCM2711, Cortex- A72 (ARM v8) 4 nhân 64bit với tốc độ xử lý là 1,5 Ghz, đây là dòng vi mạch có tốc độ xử lý nhanh nhất từ trước đến nay của họ máy tính nhúng này Phiên bản máy tính nhưng Raspberry Pi này hỗ trợ hai cổn micro-HDMI, hỗ trợ 2 cổng USB 2.0 và
2 cổng 3.0, hỗ trợ Wifi chuẩn IEEE 802.11ac với Dual-Band 2.4GHz và 5GHz Chi tiết thông tin cấu hình cơ bản của Raspberry Pi 4B:
Bộ xử lý: Broadcom BCM271, Cortex-A72 (ARM v8) 64-bit SoC với tốc độ xử lý 1,5 GHz
Bộ nhớ: 1GB/2GB/4GB/8GB/16GB
Kết nối mạng truyền thông: 2,4GHz và 5GHz IEEE 802.11ac Wireless LAN, Bluetooth 5.0, BLE, Ethernet tốc độ cao (Gigabit Ethernet)
Hình 3.13: Các thành phần tích hợp trên Raspberry Pi 4B (Model B)
3.3.2 Lựa chọn động cơ Để đáp ứng được tải trọng của robot và vận tốc mong muốn của robot, ta cần tính toán để lựa chọn động cơ phù hợp cho robot Trong nghiên cứu này, nhóm lựa chọn động cơ encoder để trả về các xung của động, từ đó điều khiển chính xác vị trí và vận tốc của robot
Lực tác động lên dẫn động của bánh xe được tính toán như sau:
Hình 3.14: Lực tác động lên bánh xe Trong đó:
⃗⃗⃗⃗⃗⃗ : Lực ma sát của bề mặt bánh xe với mặt sàn
𝑃⃗ : Trọng lực mà bánh xe đang chịu
𝑚 1 : Khối lượng mà mỗi bánh xe phải chịu với (𝑚 1 = 𝑚/3) m là khối lượng của robot
𝑣: vận tốc mong muốn của robot
𝑎: gia tốc mong muốn của robot
Phương trình lực tác động lên bánh xe:
Chọn phương án chuyển động của robot là chuyển động thẳng Hai bánh xe chuyển động tịnh tiếng cùng vận tốc mong muốn là 0.15(m/s), hệ số ma sát an toàn đối với bề mặt sàn 𝜇 = 0.7
Khi robot chuyển động với vận tốc không đổi nên gia tốc a không đổi 𝑎 0 (𝑚/𝑠 2 ), ta có:
Với 𝐹⃗⃗⃗⃗⃗⃗ 𝑚𝑠 được tính như sau:
𝐹 𝑚𝑠 = 𝜇 𝑁 = 𝜇 𝑚 1 𝑔 = 0.7 × 1.33 × 9.8 = 9.12 (𝑁) (3.14) Công suất cần thiết cho động cơ:
Vì vậy, công suất tối thiểu để xe có thể chuyển động là 1.82 (W)
Ta có đường kính của bánh xe: d = 65 mm = 0.065 m
Khoảng cách mà bánh xe quay được một vòng là:
Số vòng mà động cơ cần quay trong vòng một phút để đạt vận tốc quay mong muốn là:
Khi xe tăng tốc, vận tốc lớn nhất mong muốn đạt được là: 𝑉 𝑚𝑎𝑥 = 0.15 (m/s) sau 0.5 giây robot sẽ đạt vận tốc lớn nhất
𝑣: là vận tốc lớn nhất
𝑣 0 : là vận tốc ban đầu
𝑡: là thời gian tăng tốc
Ta tính lại công suất cần thiết của robot khi có gia tốc 𝑎 = 0.3 (𝑚/𝑠 2 )
Vì vậy công suất tối thiểu cần thiết để tăng tốc là:
1000 = 1.4 × 10 −3 (𝐾𝑊) = 1.4 (𝑊) (3.20) Để lựa chọn động cơ từ lực đã tính từ công thức trên, moment xoắn của động cơ cần thiết là:
Với R là bán kính của bánh xe Để động cơ phù hợp và hoạt động ổn định ta cần nhân moment xoắn của động cơ với hệ số an toàn là 1.5
Với tốc độ không yêu cầu di chuyển nhanh, động cơ DC được yêu cầu có khả năng chịu tải tốt cũng như đáp ứng chi phí Động cơ Encoder GA25-370 là lựa chọn phù hợp của nhóm với moment
Encoder được sử dụng là encoder tương đối có ngõ ra là hai kênh tín hiệu A và
B lệch pha nhau Số xung trên mỗi vòng quay được chỉ định bởi nhà sản xuất, pha
A sớm hay trễ pha hơn pha B cho ta biết chiều quay của động cơ Nhóm nghiên cứu chọn động cơ DC giảm tốc GA25-370
Hình 3.15: Động cơ giảm tốc GA25-370 kèm encoder
Thông số kỹ thuật động cơ: Đại lượng Giá trị
Tỉ số truyển 1:45 Điện áp hoạt động 6-12V DC
Tốc độ không tải r/min 130
Dòng điện không tải 50 mA
Bảng 3.1: Thống số của động cơ Đại lượng Giá trị
Số xung/vòng 11 Điện áp hoạt động 5V DC
Loại encoder Encoder tương đối
3.3.3 Lựa chọn mạch cầu điều khiển
Với khả năng di chuyển linh hoạt của robot trong nhà đòi hỏi phải điều khiển chiều quay, tốc độ động cơ và vị trí yêu cầu mạch điện có khả năng đảo chiều dòng điện cũng như có thể điều khiển cường độ dòng điện chạy qua động cơ
Hình 3.16: Sơ đồ nguyên lý mạch cầu H
Với việc điều khiển các khoá S1, S2, S3 và S4 ta có thể đổi chiều dòng điện qua động cơ từ đó có thể đổi chiều quay động cơ Lựa chọn cầu H thích hợp là một trong những yếu tố quyết định chất lượng điều khiển động cơ Do đó nhóm lựa chọn trên thị trường module cầu H là L298
Board L298 là mạch điều khiển động cơ một chiều DC cùng lúc Là IC điều khiển cầu kép toàn kỳ có dải điện áp hoạt động rộng, xử lý dòng tải có mức tối đa 3A
Với các thông số kỹ thuật như sau:
- Điện áp tín hiệu điều khiển: 5 – 7V
- Điện áp logic: Low từ -0.3 – 1.5V; High từ 2.3V-Vss
- Dòng điện tối đa cho từng động cơ: 2A
- Dòng yêu cầu của tín hiệu điều khiển: 0 – 36 mA
- Công suất: 1 cầu tương đương 25W
Với khả năng chịu dòng tốt, module được lựa chọn để điều khiển cho hệ thống robot này Qua quá trình sử dụng cho thấy module hoạt động ổn định, không bị quá nhiệt Đồng thời, chi phí sử dụng thấp, dễ dàng thay mới để robot hoạt động tốt hơn
Với yêu cầu điều khiển chính xác góc quay, cũng như vận tốc góc quay chính xác cho robot thì IMU là linh kiện không thể thiếu
IMU là thiết bị điện tử kết hợp hai loại cảm biến là cảm biến gia tốc (accelerometer) và cảm biến góc quay (gyroscope) Cho phép xác định góc quay, vận tốc, vị trí của các hệ thống chuyển động trong không gian ba chiều một cách hiệu quả
Hình 3.18: Cảm biến gia tốc GY-521 6DOF MPU 6050
- Độ phân giải góc: ±250, 500, 1000, 2000 degree/sec
- Độ phân giải gia tốc: ±2, ±4, ±6, ±8, ±16
3.3.5 Vi điều khiển Arduino Uno
Với giá thành trên thị trường rẻ và thư viện mã nguồn mở thích hợp cho các dự án lớn nhỏ nên nhóm tìm hiểu để tiếp cận với dự án một cách dễ dàng hơn
Arduino là một bo mạch vi điều khiển được phát triển bởi Arduino.cc, phát triển dựa trên vi điều khiển AVR Atmega328P
Các thông số kỹ thuật của Arduino:
Lidar dùng để đo khoảng đến các đối tượng bằng cách sử dụng xung lazer (pulsed laser) phát ra và sau đó đo các xung phản xạ ánh sáng các đối tượng bằng cảm biến Sự khác biệt về thơi gian truyền về và bước sóng được dùng để tính toán và xây dựng mô hình vật thể
Lidar được sử dụng rất nhiều trong thực tế, một số ứng dụng như: xây dựng bản đồ số trong ngành địa chất, khảo cổ học, hệ thống dẫn đường bằng lazer, … Một trong những ứng dụng nổi bật của Lidar hiện nay là được dùng trong xe tự hành và robot tự hành
- Điện áp sử dụng: 5VDC
- Phương pháp phát hiện vật cản: Laser
- Khoảng cách phát hiện vật tối đa: 12m
- Tốc độ lấy mẫu tối đa: 8000 Samples per time
- Tần số quét tối đa: 10Hz
Mô hình robot thực tế
Hình 3.21: Mô hình robot thực tế
Hình 3.22: Mô hình robot thực tế
THIẾT KẾ CHƯƠNG TRÌNH ĐIỀU KHIỂN 47 4.1 Chương trình điều khiển robot
Chương trình điều khiển robot trên ROS
Hình 4.1: Lưu đồ điều khiển của robot trên ROS
Robot được thiết kế nhận tín hiệu điều khiển từ hệ điều hành ROS gửi xuống Tín hiệu mà vi xử lý nhận được gồm vận tốc thẳng 𝑣 và vận tốc xoay 𝜔 Nhiệm vụ của chương trình nhúng trên vi xử lý Arduino là có thể điều khiển động cơ thực thi đúng với các vận tốc đó
Từ vận tốc nhận được, ta phải chuyển về vận tốc riêng cần tthiết cho bánh trái và bánh phải thực hiện đúng với lệnh mà phần mềm gửi xuống để vận tốc tâm xe đúng với vận tốc yêu cầu
Vận tốc riêng của từng bánh xe là:
- L là khoảng cách giữa hai bánh xe (đơn vị mét)
- 𝑣 𝑙 , 𝑣 𝑟 lần lượt là vận tốc tịnh tiến của bánh trái và bánh phải của robot (m/s)
Hình 4.2: Lưu đồ giao tiếp giữa vi điều khiển và máy tính
Sau khi khởi động hệ thống và khởi tạo các thiết bị ngoại vi như Timer, UART,
… sau mỗi chu kỳ ngắt timer vi điều khiển Arduino sẽ lấy các giá trị vận tốc thẳng và vận tốc góc từ các giá trị toàn cục trên ROS Sau đó tính toán số xung cần thiết cấp cho động cơ trái và động cơ phải hoạt động Vì robot sử dụng động cơ có encoder, nên tín hiệu encoder sẽ được tính toán qua bộ điều khiển PID Từ đó xuất ra số xung phù hợp cho động cơ hoạt động đúng với các giá trị toàn cục của ROS.
Xử lý tín hiệu từ cảm biến IMU và Encoder
Hình 4.3: Sơ đồ xử lý tín hiệu cảm biến
Hình 4.4: Lưu đồ xử lý tín hiệu cảm biến trên vi điều khiển
Encoder của từng động cơ xuất ra tín hiệu, sau khi được xử lý từ Arduino ta thu được vận tốc của bánh trái (𝑣 𝑙 ) và vận tốc của bánh phải (𝑣 𝑟 )
Từ hai tín hiệu vận tốc trên qua hàm caculate tính toán với công thức (), ta thu được 𝜔 𝑒𝑛𝑐𝑜𝑑𝑒𝑟 là vận tốc góc quay của xe
Với L là khoảng cách giữa hai bánh xe
Cảm biến IMU ta thu được tín hiệu có độ lớn 16 bit, qua hàm scale cho ra vận tốc góc quay của xe 𝜔 𝑔𝑟𝑦
Từ hai tín hiệu vận tốc là 𝜔 𝑒𝑛𝑐𝑜𝑑𝑒𝑟 và 𝜔 𝑔𝑟𝑦 , nhóm tính toán qua bộ filter được tính theo công thức:
Chương trình điều khiển PID
Hình 4.5: Sơ đồ điều khiển PID
Hình 4.6: Sơ đồ điều khiển
Sau khi xử lý tín hiệu cảm biến từ IMU và Encoder trong khối Sensor của sơ đồ, nhóm thu được vận tốc bánh trái (𝑣 𝑙 ), vận tốc bánh phải (𝑣 𝑟 ), và vận tốc góc quay
Sử dụng bộ điều PID cho vận tốc góc quay với đầu vào là sai số giữa vận tốc góc đặt (𝜔 𝑆𝑃 ) và 𝜔 thu được từ cảm biến cho ra tín hiệu y Từ đó, nhóm tính được vận tốc cần có cho từng bánh được đặt là 𝑣 𝑙 𝑆𝑃 và 𝑣 𝑟 𝑆𝑃 được tính dựa theo công thức
(4.1), (4.2) được thể hiện trong khối Caculate
Với mục tiêu để điều khiển vận tốc động cơ sao cho vận tốc xe bám theo vận tốc xe mong muốn và vận tốc góc quay xe bám theo vận tốc góc quay mong muốn.
Tích hợp hệ thống trên ROS
4.2.1 Lập bản đồ môi trường
Trong lĩnh vực mobile robotics, lập bản đồ môi trường là một trong những công việc cực kỳ quan trọng Robot cần phải học một bản đồ của môi trường chưa biết trước khi có thể tự vận hành một cách tốt nhất Việc lập bản đồ của môi trường cho robot làm việc có thể thực hiện một cách hiệu quả nhất nhiệm vụ mà ta đặt ra cho chúng chẳng hạn như: xác định vị trí của chính robot trong môi trường, hoạch định đường đi, điểm đến và thực hiện nhiệm vụ dựa trên đường đi đã được tính toán Để xây dựng được bản đồ 2D nhằm cung cấp bản đồ tĩnh để Navigation stack hoạt động thì có một package gmapping được hỗ trợ trên nền tảng ROS Nó yêu cầu dữ liệu từ odometry và một cảm biến laser Từ đó xây dựng được bản đồ thực tế từ các dữ liệu trên
Hình 4.7: Lập bản đồ môi trường từ laser
4.2.1.1 Thuật toán vẽ bản đồ Hector SLAM
Hector SLAM là được sử dụng để xây dựng bản đồ lưới 2D cho môi trường xung quanh dùng cảm biết Laser Scan (Lidar) Do đó, Hector SLAM rất phù hợp cho viêc lập bản đồ trong điều kiện thông tin về khung toạ độ (odometry) của robot bị sai lệch, không thể đo được
Trong đề tài này nhóm sử dụng cảm biến laser scan RPLidar A1 có tầm quét lên đến 12m để quét môi trường cần xây dựng bản đồ Để thực hiện vẽ bản đồ môi trường trong ROS gồm 4 bước:
Bước 1: Cảm biến Lidar sẽ tiến hành quét môi trường xung quanh Giá trị trả về từ cảm biến gồm 360 điểm tưng ứng với 360 độ khi cảm biến quét môi trường xung quanh Mỗi điểm sẽ có 1 giá trị khoảng cách tính từ Lidar đến vị trí có vật cản
Hình 4.8:Khởi động lidar trên Rviz
Bước 2: Sau khi Rviz sử dụng Hector SLAM để nội suy tất cả các điểm ảnh trên thành các giá trị tương ứng trên bản đồ và vẽ chúng lên bản đồ dưới được lập sẵn
Hình 4.9: Bản đồ được vẽ trên Rviz lấy dữ liệu từ Lidar
Bước 3: Quá trình Scan Matching sẽ tối ưu hoá các liên kết của các điểm đã vẽ và các điểm mới được quét sao cho giá trị chiếm dụng được giảm tối thiểu
Hình 4.10: Dùng Scan matching để mở rộng bản đồ
Bước 4: Thực hiện di chuyển Lidar dựa vào sự tính toán của các quá trình trên ta sẽ thu được một bản đồ của môi trường cần quét
Hình 4.11: Bản đồ sau khi vẽ
4.2.1.2 Lưu bản đồ (map server):
Sau khi di chuyển trong môi trường và thu được kết quả map trả về là tốt nhât thì chúng ta lưu lại Việc lưu lại nên được thực hiện bằng command trên terminal Bất cứ khi nào mình muốn lưu lại bản đồ thì gõ lệnh:
Rosrun map_server map_saver [-f mapname]
Trong đó mapname là tên bản đồ do chúng ta đặt tên Sau khi lưu, chúng ta sẽ nhận được hai file để mô tả cho bản đồ mà chúng ta đã lưu là file ảnh pgm và file mô tả ảnh yaml
File ảnh sẽ mô tả các không gian bị chiếm dụng trong môi trường Theo tiêu chuẩn, các pixel màu trắng sẽ là khoảng không gian trống; các pixel màu đen là nơi đã có vật thể ở đó (như tường, vật cản, …); các pixel màu xám là những vùng mà robot chưa đi qua
File yaml sẽ chứa các thông tin: image: mymap.pgm resolution: 0.050000 origin: [-51.224998, -51.2244998, 0.000000] occupied_thresh: 0.65 free_thresh: 0.196 image là đường dẫn đến ảnh chứa thông tin resolution là độ phân giải của bản đồ (m/pixel) origin là toạ độ của pixel bên trái dưới cùng và việc xoay của bản đồ negate nếu bằng 0 thì màu trắng là không gian trống, màu đen là không gian chiếm dụng Nếu bằng 1 thì ngược lại occupied_thresh là ngưỡng để xem xét một pixel có phải đang bị chiếm dụng hay không free_thresh là ngưỡng để xem xét một pixel có phải đang ở trong không gian trống hay không
4.2.2 Điều hướng trên bản đồ (Navigation): Để thực hiện điều hương cho xe đến đích chính xác và an toàn (tránh vật cản) thì ta cũng phải cần dùng cảm biến để xem xét dữ liệu từ môi trường Lidar là cảm biến được sử dụng trực tiếp khi xe thực hiện điều hướng trên đường đi để tránh vật cản
Sử dụng ví trị robot từ odometry source và bản đồ từ Map Server để điều khiển robot di chuyển đến điểm đặt trước Đường đi sẽ được vạch sẵn tìm được bằng thuật toán A* Trong quá trình di chuyển, nếu có vật cản xuất hiện, robot sẽ tiến hành tìm lại đường đi mới đến đích Thông tin điều khiển gồm vận tốc và vận tốc góc sẽ được truyền qua UART xử lý để điều khiển động cơ cho robot
4.2.2.1 Thiết lập định vị robot trên bản đồ ảo
Tạo lập các thông tin trên bản đồ ảo nhằm mục đích định vị vị trí của robot như là: map, odom, base footprint, laser map là vị trí khởi tạo ban đầu của bản đồ ảo odom là vị trí khơi tạo ban đầu của robot trên bản đồ ảo base footprint là vị trí robot đang di chuyển hiện tại so với odom laser là vị trí của cảm biến lidar so với base footprint Để khởi tạo các vị trí trên, nhóm sử dụng bộ chuyển đổi tf_static (chuyển đổi tĩnh) và tf (chuyển đổi động) bằng ngôn ngữ python map => odom, base footprint => laser: sử dụng tf_static
4.2.2.2 Thiết lập cảm biến trên bản đồ ảo
Bản đồ ảo có các hai loại chứa thông tin của cảm biến: sensor_msgs Laser can và sensor_msgs PointCloud Để có thể đưa tín hiệu các cảm biến như: Lidar, IMU, …vào bản đồ ảo cho robot hoạt động nhóm thực hiện đưa tín hiệu cảm biến lidar vào loại chứa thông tin sensor_msgs Laser scan
*Thiết lập đường đi tối ưu:
Từ việc định vị robot trên bản đồ ảo, thiết lập cảm biến trên bản đồ ảo Bản đồ sẽ cung cấp các thông tin vật cản và những nơi robot không thể tới được Từ đó sử dụng thuật toán A* để triển khai bản đồ chi phí từ đó định hướng đường đi toàn cục (global) và đường đi cục bộ (local)
Chi phí cho global gồm các param: global costmap params và costmap common params
Chi phí cho local gồm các param: local costmap params, costmap common params
*Thiết lập hành vi tránh vật cản:
Dựa trên quy hoạch đường đi tối ưu trên bản đồ ảo, từ đó tìm ra tín hiệu điều khiển hợp lý để robot đến đích an toàn
Các param để thiết lập cho hành vi tránh vật cản: dwa_local_planner_params
4.2.2.4 Thiết lập các thống số cho move_base hoạt động:
Thiết lập costmap_common_params.yaml obstacle_range: 3.0 ; raytrace_range: 3.0 frootprint: [[-0.105, -0.105], [-0.105, 0.105], [0.041, 0.105], [0.041, -0.105]] inflation_radius: 0.5 observation_sources: scan scan:{sensor_frame: laser, date_type: LaserScan, topic: scan, marking: true, clearing: true}
Bảng 4.1: Bảng thông số kỹ thuật của costmap
Thiết kế giao diện điều khiển qua mạng wifi
Để dễ dàng điều khiển và quan sát các thông số của robot như: vận tốc thẳng, ví trị toạ độ (x, y), vận tốc góc quay Nhóm thực hiện viết phần mềm điều khiển và giám sát robot qua mạng wifi trong nhà
Phần mềm được nghiên cứu và thiết kế trên ngôn ngữ Python được chạy trên hệ điều hành Window, Linux Phần mềm bao gồm giao diện đăng nhập địa chỉ IP wifi của máy chủ điều khiển và máy tính nhúng của robot, giao diện hiển thị vận tốc thẳng, vận tốc góc quay, biểu đồ toạ độ và các nút điều khiển cho robot hoạt động
Hình 4.14: Ảnh giao diện điều khiển trên window Để đăng nhập vào phần mềm điều khiển robot ta cần nhập địa chỉ IP wifi kết nối chung của máy tính điều khiển và máy tính nhúng trên robot
Hình 4.15: Giao diện điều khiển của phần mềm
Verticel: là vận tốc của robot, dữ liệu được lấy từ node verticel của robot trên hệ thống ROS
X: là toạ độ di chuyển theo phương ngang của robot
Y: là toạ độ di chuyển theo phương dọc của robot
Distance: là khoảng cách của robot so với toạ độ gốc ban đầu
Hình 4.16: Hình ảnh robot khi được kết nối và điều khiển
KẾT QUẢ THỰC NGHIỆM
Khảo sát bộ điều khiển PID
Bộ điều khiển PID cho động cơ bánh trái với vận tốc góc mong muốn là 3 rad/s
Hình 5.1: Đáp ứng của động cơ bánh trái với thời gian t = 12s
Trong hình (5.2) đường line màu cam là vận tốc góc mong muốn và đường line màu xanh là vận tốc góc thực của động cơ bánh trái Từ đồ thị này, ta thấy rằng trung bình vận tốc góc thực tế trong khoảng 12s là 2.8 rad/s
Hình 5.2: Đáp ứng của động cơ bánh phải với thời gian t = 12s
Trong hình (5.3) đường line màu cam là vận tốc góc mong muốn và đường line màu xanh là vận tốc góc thực của động cơ bánh phải Từ đồ thị này, ta thấy rằng trung bình vận tốc góc thực tế trong khoảng 12s là 3.3rad/s
Hình 5.3: Đồ thị đáp ứng vận tốc của robot với thời gian t = 12s
Trong hình (5.4) đường line màu cam là vận tốc robot mong muốn và đường line màu xanh là vận tốc thực của robot Từ đồ thị này, ta thấy rằng trung bình vận tốc thực tế trong khoảng 12s là 0.078 m/s
Hình 5.4: Đồ thị đáp ứng vận tốc góc quay của robot trong khoảng thời gian t = 12s
Trong hình (5.5) đường line màu cam là vận tốc robot mong muốn và đường line màu xanh là vận tốc góc thực của robot Từ đồ thị này, ta thấy rằng trung bình vận tốc thực tế trong khoảng 12s là 0.38 rad/s.
Tạo lập bản đồ môi trường
Sau khi khởi tạo robot, nhóm thực hiện vẽ bản đồ 2D của môi trường được thể hiện trong phần 4.2.1 bằng phương pháp Hector SLAM Sau khi đã lưu được bản đồ, nhóm thực hiện so sánh hình dạng và kích thước với môi trường thực tế Để thực hiện vẽ bản đồ môi trường thực nghiệm cần tiến hành ba bước như sau:
Bước 1: Khởi động robot – khởi tạo môi trường truy xuất cho robot hoạt động Đồng thời kiểm tra các port USB kết nối giữa Lidar và Rapberry
Bước 2: Khởi động app Rviz và tiến hành điều khiển để robot chạy xung quanh môi trường
Hình 5.5: Khởi tạo Rviz và điều khiển robot thực hiện quét bản đồ
Bước 3: Sau khi bản đồ dẫn hình thành, ta tiến hành lưu bản đồ
Hình 5.6: Bản đồ thu được (trái) và bản đồ thực tế đo được (phải)
Bản đồ sử dùng lidar
Bản đồ thực tế Tỉ lệ chính xác
Bảng 5.1: Bảng so sánh thông bản đồ thu được và thực tế
Qua thực nghiệm tạo dựng bản đồ môi trường bằng Hector Slam, ta thấy độ chính xác của bảng độ được tạo bởi lidar có độ chính xác cao Một số chiều dài còn sai lệch do nhiễu từ các vật dụng nhỏ trong nhà như quạt, bàn, ghế, tủ… Tuy nhiên, bản đồ thu được vẫn mang lại độ chính xác cao.
Tự động di chuyển đến điểm đích và tránh vật cản
Sau khi thu được bản đồ từ Hector SLAM, nhóm tiến hành thử nghiệm dùng thuật toán tìm đường đi A* bằng cách đặt điểm đích cùng với góc quay của robot trên bản đồ đã lưu và cho robot di chuyển đến điểm đó Khi robot đến đích, đánh giá vị trí đích đến và góc quay của robot trên bản đồ so với vị trí, góc quay thực tế
5.3.1 Di chuyển thắng đến toạ độ (1.5,0) và giữ nguyên góc 0 o
Hình 5.7: Bản đồ mô phỏng di chuyển của robot
Với điểm chấm đen là điểm bắt đầu cua robot và điểm chấm đỏ là điểm kết thúc của robot Khảo sát hình trên cho ta thấy robot di chuyển thẳng và góc lệch giữa góc đặt và góc thức tế của robot khá lớn (~30 0 ), vị trí robot di chuyển đến khá chính xác
5.3.2 Di chuyển thẳng đến toạ độ (1,0) và xoay góc -90 0
Hình 5.8: Bản đồ mô phỏng di chuyển robot Ở khảo sát trên, ta thấy robot di chuyển thẳng đến toạ độ chính xác và xoay góc -90 0 , sai số giữa góc quay thực tế và góc đặt là 15 0 Qua thực nghiệm trên cho thấy robot hoạt động ước lượng chính xác toạ độ điểm đến, nhưng góc quay của robot vẫn còn sai lệch lớn
5.3.3 Di chuyển thẳng đến toạ độ (1.5,0) và tránh vật cản
Hình 5.9: Bản đồ mô phỏng di chuyển robot và tránh vật cản
Hình (5.6) là mô phỏng vị trí ban đầu của robot và vật cản cần tránh trước robot
Hình 5.10: Bản đồ mô phỏng di chuyển robot
Sau khi điều hướng góc quay và toạ độ robot cần tới, thuật toán A* tìm đường ngắn nhất đến điểm đích đã vạch đường đi cho robot, lách qua vật cản mà tránh cho robot đụng chạm vật cản
Hình 5.11: Bản đồ mô phỏng di chuyển robot Sau khi đã vạch ra đường đi theo thuật toán tìm đường ngắn nhất, robot tiến hành di chuyển đến đích và hướng góc quay trùng với góc quay đã đặt cho robot
5.3.4 Di chuyển thẳng đến toạ độ (2.5,3) và tránh vật cản
Hình 5.12: Bản đồ mô phỏng robot
Với thử nghiệm này, nhóm cho điểm đích xa hơn, phức tạp hơn là ra khỏi căn phòng Với môi trường nhiều tín hiệu nhiễu, thuật toán tìm đường đi ngắn nhất A* vẫn hoạch định được đường đi của robot đến điểm đích
Hình 5.13: Bản đồ mô phỏng robot Cảm biến lidar liên tục cập nhật bản đồ môi trường tương ứng với bản đồ đã lưu để robot thực sự định vị được toạ độ của chính robot
Hình 5.14: Bản đồ mô phỏng robot đến đích
Kết quả di chuyển robot đạt yêu cầu và hướng robot hoàn toàn trùng khớp với góc quay đã đặt.
Nhận xét và kết luận
Bộ điều khiển PID được thiết kế cho robot có độ ổn định cao Bên cạnh đó các thông số như thời gian xác lập, độ vọt lố và sai số xác lập được điều chỉnh phù hợp với khả năng đáp ứng của robot
Qua quá trình thử nghiệm cho robot di chuyển đến đích đặt trước dựa vào thuật toán tìm đường đi ngắn nhất trong nhiều trường hợp khác nhau Nhóm nghiên cứu nhận thấy robot hoạt động ổn định và khá tốt Với nhiều điểm và hướng di chuyển khác nhau, robot vẫn đáp ứng đúng yêu cầu đặt ra, mặc dù môi trường có nhiều tín hiệu nhiễu nhưng robot vẫn xử lý tối ưu, đúng như các thông số đã cân chỉnh Tuy nhiên, sau mỗi lần di chuyển thì góc quay của robot bị sai lệch không đáng kể do tích luỹ sai số của góc từ tín hiệu cảm biến Encode và IMU dẫn đến sai lệch ở góc quay