thiết kế bộ điều khiển dự báo chuyển động cho robot di động vi sai tránh vật cản động

THÀNH PHỐ HỒ CHÍ MINH TRƯỜNG ĐẠI HỌC SƯ PHẠM KỸ THUẬT GVHD: SVTH: ĐỒ ÁN TỐT NGHIỆP NGÀNH CÔNG NGHỆ KỸ THUẬT ĐIỀU KHIỂN VÀ TỰ ĐỘNG HOÁ THIẾT KẾ BỘ ĐIỀU KHIỂN DỰ BÁO CHUYỂN ĐỘNG CHO R

TỔNG QUAN

Đặt vấn đề

1.1.1 Tình hình nghiên cứu ngoài nước

Model predictive control (MPC) một bài toán phổ biến trong lĩnh vực nghiên cứu về robot, và tự động được sự quan tâm nghiên cứu cho nhiều phát triển và ứng dụng khác nhau Trên thế giới MPC được ứng dụng nhiều trong ô tô: điều khiển hành trình; tự động hóa và robot: điều khiển chuyển động của robot, hệ thống máy bay không người lái (UAV), và nhiều hệ thống tự động khác Bài báo “Motion Control of a Wheeled Mobile Robot Using ModelPredictive Control: A Survey”[1] là một trong số những những bài báo đề cập đến MPC của tác giả Kiattisin Kanjanawaniskul là một tài liệu đề cập về lý thuyết động học của robot di động sử dụng bánh xe, nonlinear MPC, linear MPC, … trong tài liệu này đưa ra những lý thuyết và kiểm chứng trên mô phỏng MPC giải quyết các vấn đề như theo dõi quỹ đạo, bám đường đi, ổn định điểm Kết quả tài liệu đưa ra có số liệu đánh giá tốt về các vấn đề được đưa ra trong tài liệu Tiếp đó là bài báo “Online Motion Planning based on Nonlinear Model Predictive Control with Non-Euclidean Rotation Groups”[2] của Christoph Rosmann, Artemi Makarow, Torsten Bertram đề xuất phương pháp lập kế hoạch chuyển động mới dự trên MPC, bài báo đề xuất một phương pháp lập kế hoạch chuyển động trực tuyến mới để điều hướng robot dựa trên điều khiển MPC Kết quả mô phỏng đạt hiệu quả tốt có khả năng tùy cơ ứng biến cao cho các tình huống có thể chưa lường trước được

Tiếp đó một bài báo tên là “Trajectory Generation for Mobile Robots in a Dynamic Environment using Nonlinear Model Predictive Control”[3] của tác giả Jonas Berlin1 Georg Hess, Anton Karlsson, William Ljungbergh, Ze Zhang, Per- Lage Gotvall, Knut Akesson, trình bày một phương pháp tạo quỹ đạo tầm xa, không va chạm cho robot di động trong môi trường công nghiệp có các chướng ngại vật tĩnh và động Đối với việc lập kế hoạch tầm xa, biểu đồ tầm nhìn cùng với A* được sử dụng để tìm đường đi không va chạm đối với các chướng ngại vật tĩnh Đường dẫn này được sử dụng làm đường dẫn tham chiếu cho thuật toán lập kế hoạch quỹ đạo, ngoài ra còn xử lý các chướng ngại vật động đồng thời tuân thủ các ràng buộc và đạo không va chạm bằng cách ở gần đường đi ban đầu nhưng đồng thời tuân theo mọi ràng buộc Thuật toán được đánh giá bằng mô phỏng trong nhiều môi trường khác nhau và tạo thành công các quỹ đạo khả thi kéo dài hàng trăm mét trong khung thời gian có thể điều chỉnh được

1.1.2 Tình hình nghiên cứu trong nước Ở Việt Nam nghiên cứu trong lĩnh vực robot di động hiện tại khá là phổ biến được khai thác và nghiên cứu, về tình hình nghiên cứu ở trường ĐH Sư Phạm Kỹ Thuật TP HCM có một trong số những bài báo nghiên cứu về SLAM như bài báo“Xây dựng bản đồ 2D cho robot di động sử dụng point cloud Library”[4] của tác giả Đào Hồng Phong, Ngô Văn Thuyên Sử dụng phương pháp ghép các đám mây điểm ảnh của môi trường từ việc thu thập của robot khi di chuyển dựa vào tốc độ, hướng, góc mở của camera Kinect và thư viện Point Cloud (PCL) Kết quả xây dựng thành công từ dữ liệu 3D thu thập Bài báo “Development of Path Tracking and Obstacle Avoidance based on an Advanced Potential Field Controller”[5] của tác giả Nguyễn Quang Chiến, Phạm Đức Huy, Trần Đức Thiện thực hiện nghiên cứu về vấn đề bám đường đi và tránh né vật cản khi có vật cản chặn đường thì robot sẽ tiến hành tìm kiếm lộ trình mới đi đến điểm mục tiêu bằng phương pháp sử dụng Advanced Potential Field Controller (bộ điều khiển trường tiềm năng nâng cao) có kết quả thực nghiệm của bài báo khá tốt Tiếp nối đó cũng có một số bài báo trong nước nghiên cứu về robot di động như bài báo “Nghiên cứu tối ưu bài toán định vị bản đồ cho robot di động trong môi trường không xác định sử dụng phương pháp học tăng cường”[6] của tác giả Nguyễn Anh Tú, Nguyễn Hồng Sơn, Bùi Huy Anh, Trần Quốc Hoàn Trình bày giải pháp định vị và tránh vật cản cho robot di động trong môi trường không xác định sử dụng phương pháp học tăng cường Q-learning, kết quả bài báo cho thấy robot có thể tránh vật cản tốt được thực hiện trong môi trường mô phỏng Gazebo

Nghiên cứu về điều hướng cho robot là vấn đề quan trọng trong robot di động bài báo “Xây dựng hệ thống định vị và điều hướng trong nhà dựa trên monocular SLAM cho robot di động”[7] của tác giả Nguyễn Cảnh Thanh, Đỗ Đức Mạnh, Hoàng Văn Xiêm Trình bày bài báo đề xuất hệ thống định vị và điều hướng cho robot hai bánh vi sai trong nhà trong môi trường ROS dựa trên Monocular SLAM, phương pháp này cho phép thu thập, tiếp nhận và xử lí thông tin từ môi trường nhằm định được vị trí Việc lập nên kế hoạch chuyển động dựa vào thuật toán DWA và A* Bên cạnh đó việc quy hoạch quỹ đạo đóng vai trò tìm đường đi cho robot di chuyển đến điểm mục tiêu, cũng có một số bài báo nghiên cứu về vấn đề này như bài báo “Hoạch định và bám đường đi cho mobile robot ứng dụng điều khiển hồi tiếp ảnh”[8] của tác giả Le Duc Hanh, Nguyen Duy Anh Tìm đường đi ngắn nhất trong môi trường có vật cản, và điều khiển mobile robot bám theo đường đi đã vạch ra ứng dụng tiểu chuẩn Lyapunov kết hợp với camera hồi tiếp vị trí hiện tại của robot

1.1.3 Tính cấp thiết đề tài

Từ những phân tích trên, nhóm nhận thấy một số vấn đề cần cải tiến khi áp dụng giải thuật tối ưu đường đi và phương pháp bám quỹ đạo kết hợp tránh vật cản động Ngoài ra các đề tài liên quan đến sử dụng giải thuật tối ưu, bám đường đi và tránh vật cản tại Việt Nam vẫn còn hạn chế Do đó nhóm thực hiện chọn đề tài: “ Thiết kế bộ điều khiển dự báo chuyển động cho robot di động vi sai tránh vật cản động ” để nghiên cứu, phát triển.

Mục tiêu đề tài

Mục tiêu của đề tài được đặt ra như sau:

- Thiết kế và thi công mô hình robot di động vi sai hai bánh

- Thiết kế giải thuật điều hướng và tránh vật cản động

- Thiết kế giao diện điều khiển và thu thập dữ liệu cho robot

- Áp dụng giải thuật và thu thập dữ liệu thời gian thực vào mô hình robot.

Giới hạn đề tài

Mô hình robot thực tế do nhóm sinh viên tự thiết kế và thi công nên chưa có cơ sở nào đánh giá mức độ bền vững và ổn định trong quá trình hoạt động của mô hình

Giới hạn vận tốc tối đa của robot là 0.25 m/s

Chỉ xét vật cản có vận tốc không đổi và có chiều cao đến tầm quét của cảm biến lidar

Giới hạn đề tài không thực hiện khảo sát các bộ điều khiển Các thông số được chọn sao cho kết quả là tối ưu nhất

Chưa xét đến tốc độ truyền nhận giữa máy chủ và khách thông qua ROS Network.

Phương pháp nghiên cứu

Đề tài này kết hợp nghiên cứu phân tích dữ liệu, phân tích mô phỏng, phân

Phân tích dữ liệu: sử dụng các công cụ Matlab, QT Designer hỗ trợ quan sát và thu thập dữ liệu để vẽ đáp ứng đồ thị cũng như đưa ra một số đánh giá về kết quả của hệ thống dựa trên các bài bảo ở Google Scholar để kiểm tra đánh giá kết quả

Phân tích mô phỏng: dựa vào công cụ Gazebo trên ROS để mô phỏng Robot các giải thuật slam và điều hướng robot

Phân tích thực nghiệm: dựa trên những kết quả đã thực hiện trên mô phòng rồi đưa ra nhận xét trên thực nghiệm, đánh giá kết quả thực nghiệm so với lý thuyết đã được học từ các nghiên cứu trước đó Ngoài ra khi di chuyển ta có thể quan sát bằng mắt những điểm bất thường từ đó thực hiện xử lý các trường hợp di chuyển không đúng với mục tiêu ban đầu

Tiêu chí đánh giá hiệu quả của hệ thống: bao gồm kiểm tra sai số bám của robot so với quỹ đạo đặt, so sánh chất lượng điều khiển dựa trên các dữ liệu đã thu thập từ mô hình trước.

Nội dung đề tài

Nội dung đề tài bao gồm 6 chương Bố cục các chương được mô tả như sau:

Chương 1: Tổng quan đề tài

Chương 1 trình bày về tổng quan của đề tài thực hiện bao gồm phần đặt vấn đề, mục tiêu, giới hạn của đề tài và các phương pháp nghiên cứu.

CƠ SỞ LÝ THUYẾT

Tổng quan robot di động

2.1.1 Khái niệm robot di động

Robot di động để dễ hiểu một cách đơn giản theo đúng nghĩa đen thì có thể hiểu là một loại máy có thể di chuyển từ nơi này đến nơi khác có thể thực hiện các chức năng một cách tự động tùy vào từng loại robot Một số loại robot có cơ cấu đặc trưng riêng có thể thay thế con người làm việc trong một số môi trường độc hại hoặc khắc nghiệt Ví dụ như robot human trong hình Hình 2-1 a có hình dáng giống với con người có thể làm một số hành động cơ bản của con người, robot di động phục vụ trong nhà kho trong Hình 2-1 b thực hiện vận chuyển và phân phối hàng hóa một cách tự động tăng năng xuất hiệu quả làm việc của nhà kho a) b)

Hình 2-1 Mô hình Robot: a) Robot human b) Robot di động 2.1.2 Phân loại robot di động Để thúc đẩy quá trình sản xuất từ đó sự phát triển của các robot ra đời từ nhu cầu sản xuất tiêu dùng, công việc đời sống hàng ngày mà sự đa dạng của các loại robot phát triển một cách mạnh mẽ ngày càng có nhiều loại robot ra đời Robot có thể được phân loại theo yếu tố môi trường hoạt động, có thể chia làm các nhóm sau: robot di chuyển trên mặt đấy (terrestrial robot), robot di chuyển trên không (Unmanned aerial vehicles – UAVs), robot di chuyển dưới nước (Autonomous underwater vehicles – AUVs) Trong đó robot di chuyển trên mặt đất là loại robot phổ biến và đa dạng nhất Dựa vào những cấu trúc và khả năng di chuyển riêng biệt của robot cũng có thể chia robot chia thành các loại nhỏ hơn: robot di chuyển dùng bánh (Wheeled mobile robots – WMRs) [9] , robot di chuyển bàng bánh xích (tracked robot), robot di chuyển dùng chân (Legged mobile robots – LMRs) [10]

Với sự phát triển về mặt dịch vụ mua bán thương mại công việc điều phối hàng hóa trong các nhà kho hay vận chuyển hàng hóa có nhu cầu cao, các robot di động trên mặt đất được nghiên cứu và sản xuất phổ biến đáp ứng như cầu cho vấn đề này Loại robot di động điều phối hàng hóa trong nhà kho hay vận chuyển hàng hóa được sử dụng rộng rải tăng hiệu xuất, là làm việc một cách tự động thay thế con người, một những loại robot sử dụng trong nhà kho như robot điều phối vật liệu và quản lí Pallet Hình 2-2, với khả năng mang theo tải tới 2 tấn kg, khối lượng robot 500 kg, với vận tốc tối đa lên đến 1,2 m/s, kích thước robot 1,35x0,96x0,315 (m) thời gian hoạt động

8 tiếng cho mỗi lần sạc pin, đồng tời trang bị cho robot là các cảm biến Lidar tránh sự va chạm, với hai bánh dẫn động lái vi sai tăng sự linh hoạt đối với robot ở các góc hẹp Áp dụng robot này giúp tăng năng xuất giảm bớt sự phụ thuộc vào con người, tối ưu hóa quy trình quản lý hàng hóa

Hình 2-2 MobilePalletTM Pallet Management & Material Handling Robots

Robot hai bánh cân bằng có thể thay đổi về mặt chiều cao, có thể di chuyển ở bề mặt gồ ghề như Ascento 2 robot trong Hình 2-3 là loại robot hai bánh cân bằng muốn của người điều khiển, robot này có thể phát hiện và tránh né vật cản bằng cách đi vòng qua hoặc bật nhảy vượt qua vật cản và tiếp đất giữ cho tư thế robot ở vị trí cân bằng Đây là loại robot có thể dùng để nghiên cứu và phát triển đưa vào thực tế [11] Đối với robot sử dụng chân, còn được gọi là robot di động sử dụng chân (LMR), được thiết kế để di chuyển bằng các chi mô phỏng chuyển động của động vật hoặc con người Những robot này thường có nhiều chân, thường từ hai đến sáu chân, với thiết kế bốn chân phổ biến nhất do tính ổn định của chúng Một ví dụ nổi bật về robot sử dụng chân là robot Spot do Boston Dynamics phát triển (Hình 2-4) Robot này mô phỏng chuyển động và cấu trúc của một loài động vật bốn chân, cho phép nó điều hướng qua các địa hình phức tạp với sự linh hoạt và chính xác Robot sử dụng chân rất đa năng và có thể vượt qua các bề mặt không đồng đều, leo cầu thang và thích nghi với các môi trường khác nhau mà robot có bánh xe hoặc robot bánh xích có thể gặp khó khăn Thiết kế của chúng cho phép nhiều ứng dụng khác nhau, từ các nhiệm vụ tìm kiếm và cứu hộ trong các khu vực bị thảm họa đến các nhiệm vụ thám hiểm trong các cảnh quan đầy thách thức và khó đoán Sự phát triển và tiến bộ của robot sử dụng chân tiếp tục là một trọng tâm quan trọng trong lĩnh vực robot, nhằm nâng cao hiệu quả, khả năng thích ứng và hiệu suất tổng thể của chúng trong các tình huống thực tế

Robot di chuyển trên không trung hay còn được biết đến với tên gọi máy bay không người lái (UAV) UAV thường được sử dụng trong mục đích quân sự, giao hàng, quay phim, giám sát, cứu hộ, … Do các lợi ích mà UAV mang lại nên UAV ngày càng được nghiên cứu và phát triển mạnh mẽ

Hình 2-5 DJI Mini 2 Air Quadcopter robot

Hình 2-5 là Drone Quadcopter có bốn cánh nằm trên bốn trục lệch nhau 90, nguyên lí vận hành bay của Drone 4 cánh phụ vào sự chuyển động quay quanh trục của 4 động cơ Drone có thể ứng dụng trong việc chụp ảnh và quay video từ không gian trên cao, từ các góc quay trên cao và di chuyển mượn mà tạo nên những thước phim độc đáo và chất lượng Ngoài ra, ta có thể sử dụng để kiểm tra và giám sát các công trình nghiên cứu, quan sát các địa hình và cứu hộ trong tình trạng khẩn cấp

Robot rắn (Snake Robot) trong Hình 2-6 là robot di động trên mặt đất sử dụng thân để di chuyển Robot di chuyển như rắn có nhiều khớp để tăng độ linh hoạt của robot Ngoài ra robot có thể di chuyển trên các địa hình không bằng phẳng, phân bố lực đều trên bề mặt di chuyển Robot có thân hình nhỏ nên không có khả năng mang tải, để điều khiển loại robot này cần các thuận toán phức tạp [12]

Ngoài các loại robot trên, thì robot di chuyển dưới nước (AUV) cũng là một loại robot có tính ứng dụng cao Robot này có thể được thiết kế tự động, hoặc bán tự động hoặc có thể do con người điều khiển hoàn toàn trong môi trường dưới nước nhằm vào các công việc thu thập thông tin, dữ liệu, thí nghiệm ở dưới nước khai thác những thứ mà con người chưa được biết đến về thế giới dưới nước

Aquanaut ở Hình 2-7 được Houston Mechatronic tạo ra Aquanaut là một robot được thiết kế phục vụ cho những công việc dưới nước, có tính cơ động cao có thể biển đổi qua lại giữa một chiếc tàu ngầm được thiết kế di chuyển đường dài thành một robot có hình dạng nữa thân trên của con người, có 2 cánh tay máy để thao tác cầm, nắm các vật khi ở dưới nước Với thông tin về robot có kích thước có chiều rộng 1.52m, chiều dài 3.5m, chiều cao 0.97m ở hình dạng tàu ngầm và 1.6m ở dạng nữa người, có khối lượng nặng 1 tấn tốc độ tối đa có thể di chuyển là 13km/h Robot có thể di chuyển quãng đường 200km cho mỗi lần pin và lặn sâu 300m, và một số cảm biến được trang bị trên robot như cảm biến sonar, GPS, cảm biến lực, camera… ngoài ra có thệ hế dẫn đường quán tính để đảm bảo thông tin nhật về từ môi trường xung quanh một cách đầy đủ nhất để robot có thể hoạt động một cách an toàn Từ sản phẩm AUV này sẽ đóng vai trò thúc đẩy cho việc khám phá và tìm hiểu về thế giới đại dương.

Cấu trúc robot di động vi sai

Hình 2-8 Cấu hình của robot di động vi sai bốn bánh

Cấu trúc của mô hình robot di động vi sai hai bánh được trình bày trong Hình 2-1 Với ( x Q , y Q ) lần lượt là vị trí của robot theo trục x và trục y, 2a là khoảng cách giữa hai bánh xe,  là hướng của robot so với trục ngang song song với trục Ox, v Q là vận tốc tuyến tính của robot, v l và v r lần lượt là vận tốc tuyến tính của bánh trái và bánh phải,  l và  r lần lượt là vận tốc góc của bánh trái và bánh phải

Robot bao gồm hai bánh xe được cố định vào hai bên của thân robot, cả hai bánh đều đảm nhận việc truyền động độc lập cho robot Ngoài ra để giữ thăng bằng cho robot, nhóm sử dụng bốn bánh mắt trâu ở các góc ngoài của thân robot.

Động học robot di động

2.3.1 Lý thuyết động học robot di động Động học robot di động liên quan đến cấu hình robot trong không gian làm việc, mối liên hệ giữa không gian hình học, các ràng buộc trong quỹ đạo của nó Công thức động học dựa trên cấu trúc hình học của robot Nghiên cứu về động học của robot là cơ sở để nghiên cứu về phần động lực học, tính ổn định và điều khiển robot Bài toán động học thuận và động học nghịch tính toán sự biến đổi giữa các biến ở không gian khớp và các biến trong không gian làm việc và ngược lại [13]

Xét một hệ robot di động, trong không gian khớp có các biến tổng quát

1, 2, , n q q q và các biến tổng quát trong không gian làm việc x x 1 , 2 , ,x m Ta định nghĩa các vector theo công thức (2.1):

Bài toán động học thuận là bài toán tìm p khi đã biết trước các biến q Đối với công thức tổng quát p m và p n ( n có nghĩa là không gian Euclidean với n chiều) Ta có mối liên hệ bởi hàm phi tuyến theo công thức (2.2):

(2.2) Đối với robot di động, ta sẽ phải phân tích về chuyển động của robot Để tính toán được các chuyển động của robot ta sẽ xấp xỉ các biến khớp bằng cách tìm vận tốc của các khớp q= q q 1, 2, ,q n  T Vì vậy ta cần phải tìm mối liên hệ đạo hàm (vi phân) của q và p được gọi là động học thuận vi phân và được biểu diễn bởi (2.3) d p=J q d (2.3)

J (2.5) Ở công thức (2.5), ( ) i j , là phần tử ij i j

 được gọi là ma trận Jacobian của robot, ma trận này biểu thị mối liên hệ giữa vận tốc của khớp với vận tốc của robot trong không gian làm việc Gọi q= q 1, ,q n  và p= x x 1, 2, ,x m  T lần lượt là vận tốc của các khớp và không gian làm việc Chia công thức (2.3) cho dt ta được phương trình hay d d dt p =J dt q p = Jq (2.6)

2.3.2 Động học robot di động vi sai

Robot di động vi sai thường được biểu diễn bởi 3 đại lượng chính Các đại lượng được biểu diễn dưới dạng ma trận công thức (2.7)

Các đại lượng trên được chú thích như Giả sử bánh xe lăn không trượt, v l và v r là vận tốc tuyến tính của bánh trái và vận tốc bánh phải của xe, v Q là điểm chính giữa của xe Dựa vào Hình 2-8, ta có công thức (2.8) và (2.9) l Q v =v −a (2.8) r Q v =v +a (2.9)

Cộng và trừ v l và v r ta được công thức (2.10) và (2.11)

Ta tính được sự thay đổi vị trí theo trục tọa độ như sau:

Từ công thức (2.11) ta suy ra được:

Ta lại có vị trí góc và vận tốc góc của bánh trái, bánh phải được kí hiệu lần lượt là  l , l  ,  r , r  và mối liên hệ giữa vận tốc góc và vận tốc dài của bánh trái và phải được tính bằng công thức (2.15) và (2.16): l l v =r (2.15) r r v =r (2.16)

Dựa vào các biểu thức (2.12), (2.13), (2.15) và (2.16) ta có mô hình động học của WMR được mô tả như sau:

Từ công thức (2.17), (2.18) và (2.19) ta tìm được phương trình động học của phương pháp lái vi sai:

Và J là ma trận Jacobian

2.4 Lý thuyết MPC Điều khiển dự đoán mô hình (MPC) là một phương pháp điều khiển rời rạc tiên tiến hoạt động với thời gian thực Từ một tập hợp các giá trị trạng thái và dựa trên một mô hình, MPC tối ưu hóa một vấn đề xung quanh một mục tiêu và đưa ra một chuỗi các tín hiệu điều khiển như đầu ra Bộ giá trị điều khiển đầu tiên sau đó được sử dụng như đầu vào cho hệ thống và sau một khoảng thời gian ngắn, đặt là bước thời gian của hệ thống, các giá trị trạng thái mới được đo và quá trình được lặp lại

Sự khởi đầu của MPC bắt đầu tại Shell Oil Company vào năm 1979 khi một ý tưởng có tên là "Điều khiển ma trận động" (Dynamic Matrix Control) được Cutler và Ramaker trình bày trong bài [14] DMC là loại điều khiển dự đoán đầu tiên có thể được áp dụng trong công nghiệp Ý tưởng là xử lý các hệ thống điều khiển đa biến mà không có bất kỳ ràng buộc nào và dự đoán các giá trị tương lai cho các hệ thống tuyến tính Ý tưởng rằng thuật toán sẽ dự đoán hành vi tương lai của hệ thống dẫn đến đầu ra ít tấn công hơn và sự hội tụ mượt mà hơn đến điểm mục tiêu [15]

Reference Trajectory Predicted Output Measured Output Predicted Control Input Past Control Input

MPC dựa trên tối ưu hóa lặp đi lặp lại trong một khoảng thời gian hữu hạn của mô hình hệ thống Tại thời điểm t, trạng thái hiện tại của hệ thống được lấy mẫu và một chiến lược điều khiển giảm thiểu chi phí được tính toán (thông qua một thuật toán tối ưu hóa số) cho một khoảng thời gian tương lai tương đối ngắn H.

Tổng quan về nền tảng ROS

Robot Operating System (ROS) là bộ công cụ phát triển phần mềm, mã nguồn mở dành cho các ứng dụng robot ROS cung cấp một nền tảng phần mềm tiêu chuẩn cho các nhà phát triển trong các ngành sẽ hỗ trợ họ từ nghiên cứu và tạo nguyên mẫu cho đến triển khai và sản xuất

Hình 2-10 Biểu tượng của ROS [16]

ROS hỗ trợ việc phát triển phần mềm robot theo mô hình kiến trúc phân tán, trong đó các thành phần phần mềm độc lập có thể chạy trên các node riêng biệt và giao tiếp với nhau thông qua các topic, service và action ROS cung cấp cơ chế tạo ra, quản lý và chuyển đổi các message để các node có thể giao tiếp và trao đổi dữ liệu với nhau

ROS hỗ trợ nhiều ngôn ngữ lập trình như C++, Python và Java, giúp cho người phát triển có sự linh hoạt trong việc lựa chọn ngôn ngữ phù hợp với nhu cầu của dự án

Một điểm mạnh của ROS là sự phổ biến và sự hỗ trợ từ cộng đồng người dùng rộng lớn Cộng đồng ROS cung cấp nhiều tài liệu, ví dụ và gói phần mềm mở rộng, giúp người dùng tiết kiệm thời gian và công sức trong quá trình phát triển robot

ROS được nhiều công ty lớn sử dụng để nghiên cứu và phát triển như Bosch, Amazon, Boston Dynamics, Microsoft… vì các điểm nổi bật sau [17]:

• Tính năng cao cấp: ROS có sẵn các chức năng gói cao cấp như Simultaneous

Localization and Mapping (SLAM), Adaptive Monte Carlo Localization (AMCL)…

• Các công cụ: cung cấp các công cụ để tìm và sửa lỗi (debugging), trực quan hoá, mô phỏng…

• Hỗ trợ các cảm biến và cơ cấu chấp hành cao cấp: hỗ trợ nhiều cảm biến như 3D lidar, encoder, IMU, camera…

• Khả năng hoạt động đa nền tảng: hỗ trợ giao tiếp giữa các chương trình khác nhau trong robot, hoặc giao tiếp với thiết bị khác

• Tính mô đun hoá: Các quy trình xử lý được lập trình độc lập nên có chương trình bị sự cố thì robot không dừng hẳn mà có các biện pháp xử lý

• Xử lí tài nguyên đồng thời quản lý và sử lý tài nguyên đồng thời

Dựa trên các ưu điểm trên thì ROS được ứng dụng nhiều như xe tự hành, robot nhiều chân, robot hình người, thiết bị bay không người lái …

Bên cạnh các điểm mạnh nêu trên ROS có một số nhược điểm sau [17, 18] :

• Độ trễ và hiệu suất: Việc truyền thông giữa các node thông qua giao tiếp thông qua mạng có thể gây ra độ trễ và ảnh hưởng đến hiệu suất của hệ thống

• Quản lý các gói phụ thuộc và quản lý các phiên bản: ROS được cập nhập liên tục nên các gói phiên bản trước không tương thích và yêu cầu các gói phụ thuộc đi kèm

• Học phức tạp: ROS có nhiều khái niệm mới cần thời gian để học và làm quen

Yêu cầu các kiến thức chuyên sâu về Robot đặc biệt là các giải thuật để có thể ứng dụng

Trong ROS, có một số cấp độ khái niệm quan trọng để hiểu cách hệ thống hoạt động và tương tác với nhau Dưới đây là một số cấp độ khái niệm chính trong ROS :

• Package: Package là cấp độ tổ chức cao nhất trong ROS Nó là một thư mục chứa các tập tin và mã nguồn liên quan đến một chức năng cụ thể hoặc một phần của hệ thống robot Package bao gồm các tệp tin cấu hình, mã nguồn, thư viện, message, và các công cụ khác liên quan đến chức năng cụ thể đó Một package trong ROS có thể được sử dụng lại và chia sẻ giữa các dự án khác nhau

• ROS Master: là một quy trình quản lý trong ROS, hoạt động như trung tâm quản lý đăng ký và giao tiếp giữa các node, topic, service và action trong hệ thống Nó duy trì thông tin về các thành phần trong mạng robot và cung cấp các dịch vụ để các thành phần có thể tìm thấy và giao tiếp với nhau

• Node: là một quy trình độc lập trong ROS, chạy trên một máy tính riêng biệt

Nó là một đơn vị cơ bản để thực hiện các chức năng cụ thể trong hệ thống robot Mỗi node trong ROS có thể chứa mã nguồn, các thư viện, và các phần mềm phụ thuộc khác Node giao tiếp với nhau bằng cách gửi và nhận thông điệp (messages) thông qua các topic, service, hoặc action

Hình 2-11 Giao tiếp giữa các node [19]

• Topic: là một kênh truyền thông không đồng bộ trong ROS Nó cho phép các node gửi và nhận thông điệp (messages) theo một định dạng cụ thể Các node có thể xuất bản (publish) thông điệp lên một topic và đọc (subscribe) thông điệp từ topic đó

Hình 2-12 Ví dụ về topic trong ROS [20]

• Service: Service (dịch vụ) là một cơ chế giao tiếp đồng bộ trong ROS Nó cho phép các node yêu cầu (request) một dịch vụ và các node khác cung cấp (advertise) dịch vụ đó Dịch vụ được sử dụng để thực hiện các chức năng cụ thể khi được yêu cầu

Hình 2-13 Ví dụ về service trong ROS [20]

• Action: là một cơ chế giao tiếp phức tạp trong ROS, cho phép trao đổi dữ liệu trong các hoạt động kéo dài trong thời gian Action bao gồm ba phần chính: goal, result, và feedback Một node có thể gửi một goal đến một action server, và server sẽ xử lý và trả về kết quả Trong quá trình thực hiện, action server có thể gửi feedback để thông báo tiến trình cho node gửi goal

Hình 2-14 Ví dụ về action trong ROS [20]

Ngoài ra, Bảng 2-1 So sánh giữa Topic, Service và Actionnhóm đã trình bày tóm tắt các kiểu giao tiếp trong ROS

Bảng 2-1 So sánh giữa Topic, Service và Action

Kiểu giao tiếp Đặc điểm Chiều Mô tả

Sử dụng khi trao đổi dữ liệu liên tục

Service Đồng bộ Hai chiều

Sử dụng khi yêu cầu xử lý và phản hồi trạng thái hiện tại

Sử dụng khi yêu cầu phản hồi mất nhiều thời gian

Tổng quan về Navigation Stack

Navigation Stack trong ROS là một bộ công cụ mạnh mẽ và linh hoạt được biến từ đó đưa ra các lệnh vận tốc để gửi đến cho robot thực hiện Cấu trúc của gói Navigation Stack được mô tả trong Hình 2-15

AMCL sensor transforms odometry source global_planner local_planner global_costmap local_costmap recovery_behaviors map_server base_controller sensor_msgs/LaserScan move_base tf tf/tfMessage internal nav_msgs/Path odom nav_msge/Od ometry cmd_vel geometry_msge/Twist map nav_msgs/GetMap sensor topics sensor_msgs/PointCloud sensor_sources move_base_simple/goal geometry_msgsPoseStamped provided node optional provided node platform specific node

Hình 2-15 Cấu trúc gói Navigation Stack [21]

Cấu trúc mô tả trong Hình 2-15 gồm các thành phần:

• Move_base: Đây là package chính của Navigation Stack và chịu trách nhiệm chính cho việc lập kế hoạch đường đi và điều khiển chuyển động của robot Nó sử dụng một cấu trúc điều khiển điểm đến (goal-based control) để tạo ra một đường đi an toàn và hiệu quả từ vị trí hiện tại của robot đến mục tiêu

• Amcl: Package này thực hiện việc xác định vị trí (localization) của robot trong bản đồ đã biết bằng cách sử dụng một bộ lọc hạt (particle filter) AMCL sẽ cập nhật vị trí ước tính của robot dựa trên dữ liệu từ cảm biến và lấy mẫu lại các hạt để tăng độ chính xác của ước tính

• Global_planner: Đây là một local planner dùng để lập kế hoạch đường đi toàn cầu Nó sử dụng các thuật toán như Dijkstra hoặc A* để tạo ra một đường đi từ điểm bắt đầu đến điểm đích trên bản đồ

• Local_planner: Đây là một local planner dùng để lập kế hoạch đường đi cục bộ Nó xác định các tuyến đường ngắn nhất và an toàn từ vị trí hiện tại đến điểm tiếp theo trên đường đi toàn cầu costmap_2d: Package này tạo ra một bản đồ chi phí 2D dựa trên dữ liệu từ cảm biến, nhằm đại diện cho các vùng an toàn và vùng cấm của môi trường Costmap được sử dụng để lập kế hoạch đường đi an toàn cho robot.

Tổng quan về SLAM

Simultaneous Localization and Mapping (SLAM) là một trong vấn đề cơ bản của robot di động Quá trình SLAM gồm hai phần: xây dựng bản đồ môi trường xung quanh (Mapping) và định vị vị trí của robot trong bản đồ (Localization) SLAM là một thách thức vì robot phải xử lý dữ liệu cảm biến nhiễu và không đầy đủ, các chướng ngại vật động và các ràng buộc về tính toán

Hình 2-16 Ứng dụng của SLAM

SLAM có thể được phân thành hai loại: trực tuyến và ngoại tuyến SLAM trực tuyến thực hiện ánh xạ và bản địa hóa trong thời gian thực, trong khi SLAM ngoại tuyến xử lý dữ liệu cảm biến sau khi robot kết thúc quá trình khám phá SLAM cũng có thể sử dụng các loại cảm biến khác nhau, chẳng hạn như máy quét laze, máy ảnh, cảm biến IMU hoặc GPS

Như Hình 2-17 [22], thuật toán SLAM được định nghĩa là việc ước tính vị trí vật cản môi trường và vị trí của robot trong môi trường đó

Dưới dạng xác suất, thuật toán SLAM được biểu diễn dạng phân bố xác suất như sau :

Trong đó: u 0: k = u u 0, 1, ,u t  = u 0: 1 t − ,u t là tập chứa các giá trị đầu vào điều khiển từ thời điểm bắt đầu đến thời điểm t, m= m m 1, 2, ,m i là tập hợp các điểm mốc vị trí vận cản, z 0: t = z z 0, , ,1 z t  = z 0: 1 t − ,z t là tập các điểm mốc quan sát được từ thời điểm bắt đầu đến thời điểm t Phân bố xác suất này mô tả phân bố hậu nghiệm liên kết giữa vị trí mốc trong bản đồ m với trạng thái x t của robot tại thời điểm k khi có giá trị quan sát được z 0:t và tín hiệu đầu vào điều khiển u 0:t [22] Để tính toán giá trị phân bố xác suất này, ta cần sử dụng mô hình chuyển trạng thái và mô hình quan sát Mô hình chuyển trạng thái mô tả cách tín hiệu điều khiển tác động lên việc chuyển đổi giữa các trạng thái của robot, mô hình quan sát mô tả xác suất thực hiện được giá trị quan sát z t khi vị trí robot và vị trí điểm mốc trên bản đồ đã được xác định có dạng:

Với giả định rằng khi vị trí của robot và bản đồ được xác định, các giá trị quan sát được là độc lập có điều kiện Khi đó, mô hình chuyển động của robot được mô tả bởi phân bố xác suất chuyển trạng thái sau:

Nền tảng ROS cung cấp một số gói và công cụ để giúp triển khai và kiểm tra các thuật toán SLAM, chẳng hạn như Gmapping SLAM, Cartographer SLAM, Hector SLAM, Rtabmap SLAM và SLAM_Toolbox… Gmapping SLAM sử dụng cảm biến lidar và dữ liệu từ phép đo hình học để thực hiện SLAM trực tuyến dựa trên bộ lọc hạt Nó tạo ra một bản đồ lưới chiếm chỗ 2D hiển thị xác suất mỗi ô bị chiếm bởi chướng ngại vật Cartographer SLAM sử dụng cảm biến lidar và dữ liệu từ cảm biến IMU để thực hiện SLAM trực tuyến dựa trên kỹ thuật scan matching Nó tạo ra một bản đồ con 2D hoặc 3D thể hiện môi trường cục bộ của robot Hector Slam sử dụng cảm biến lidar để tạo ra bản đồ mà không dùng đến dữ liệu từ phép đo hình học Rtabmap SLAM sử dụng camera và dữ liệu từ phép đo hình học để thực hiện SLAM trực tuyến hoặc ngoại tuyến dựa trên kỹ thuật phát hiện đóng vòng lặp Nó tạo ra bản đồ đám mây điểm 3D thể hiện các đặc điểm hình học của môi trường Cuối cùng, SLAM_Toolbox cung cấp các công cụ cho SLAM trực tuyến hoặc ngoại tuyến, chẳng hạn như hợp nhất bản đồ, lưu, chỉnh sửa và trực quan hóa

Hector SLAM áp dụng giải thuật Scan Maching, sử dụng tần số lấy mẫu cao của lidar 2D và ước tính tư thế 2D ở tốc độ quét của cảm biến Scan Maching là một thuật toán ước tính chuyển động tịnh tiến và xoay của Robot dựa trên sự so sánh kết hợp dữ liệu được quét Phương pháp Gaussian Newton được sử dụng để giải quyết vấn đề Scan Maching và từ dữ liệu thu thập có thể ước tính vị trí robot (x, y, theta) trong bản đồ hiện có Để ước tính trạng thái trong điều hướng được thêm vào hệ thống đo lường quán tính (IMU) bằng cách sử dụng bộ lọc EKF Nếu sử dụng lidar 2D có tần số lấy mẫu cao, Hector SLAM có tác dụng lập bản đồ đáng kể trong môi trường trong nhà nhỏ và không cần phải ở trong một số môi trường địa lý phức tạp Do thuật toán có thể không cần thu thập dữ liệu nhiều nên rất phù hợp với một số robot có khả năng tính toán hạn chế [23]

Quá trình slam kết thúc sẽ được bản đổ lưới chiếm dụng (Occupancy grid map) 2D đại diện cho môi trường xung quanh của robot Bản đồ lưới chiếm dụng được chia thành các ô vuông nhỏ có kích thước cố định, và mỗi ô vuông trên bản đồ đại diện cho một vị trí trong môi trường [24]

Hình 2-18 Bản đồ lưới chiếm dụng 2.7.2.1 Scan Matching

Scan matching là quá trình so khớp, căn chỉnh các dữ liệu quét laser mới với dữ liệu quét laser đã có hoặc với bản đồ hiện tại Mục tiêu là tìm ra sự tương quan tốt nhất giữa các dữ liệu quét laser, từ đó xác định vị trí của robot trong môi trường hoặc cập nhật bản đồ

Hình 2-19 Quá trình Scan Maching

Bài toán Scan Matching thường được định nghĩa là bài toán tối ưu hóa phi tuyến, có thể được giải quyết bằng các kỹ thuật tối ưu hóa độ dốc (gradient optimization) Bởi vì từng bước giảm của độ dốc (gradient) khó xác định nên tốc độ hội tụ thường chậm Do đó, phương pháp Gauss-Newton thường được sử dụng để giải quyết các vấn đề đó, trong Hector SLAM cổ điển với ma trận Hessian bậc hai được xấp xỉ bằng tích của ma trận Jacobian Ưu điểm lớn nhất của phương pháp Gauss-Newton là việc tính toán ma trận Hessian bậc hai bị bỏ qua và việc tính toán có thể giảm đi rất nhiều [25]

Thuật toán ước tính tư thế robot bằng cách tìm ra sự căn chỉnh tối ưu giữa các điểm cuối (endpoint) của laser với bản đồ được xây dựng dựa trên tìm phép biến đổi ( rigrid transformation)  =[ ,P P x y , ] T là cực tiểu[26]:

Trong đó M S( ( )) i  là giá trị chiếm dụng tại ô S i ( ) , S i ( ) là vị trí của điểm cuối laser S i =[S i x ; ;S i y ; ] T ,  là ước tính tư thế ban đầu, [ ,p p x y , ] T là ma trận chuyển vị tương ứng với toạ độ x, y , góc quay giữa điểm cuối với toạ độ gốc

Hình 2-20 Mô tả thông số Robot

Vị trí ước tính tại điểm ban đầu S i ( ) bằng:

Robot di chuyển một khoản  , để ước tính  sao cho tối ưu hoá phép đo sai số sao cho:

Triển khai Talor bậc nhất cho M S ( i ( + ) ) ta được:

Giả sử robot chuyển động rất nhỏ nên cho  tiến đến gần bằng 0, để tìm cực tiểu phương trình (2.30) ta đạo hàm riêng từng phần được:

Rút  ta được phương trình Gauss Newton :

Occupancy Grid Map giúp giải quyết vấn đề tạo bản đồ nhất quán từ dữ liệu bị nhiễu và các yếu tố không chắc chắn, với giả định rằng tư thế của robot đã được biết trước Ý tưởng cơ bản của lưới chiếm chỗ là biểu diễn bản đồ dưới dạng một tập hợp gồm các biến ngẫu nhiên, được sắp xếp trong một lưới cách đều nhau Mỗi ô là biến ngẫu nhiên nhị phân để biểu diễn trạng thái ô đó Các thuật toán ánh xạ lưới chiếm chỗ thực hiện ước tính hậu nghiệm gần đúng cho các biến ngẫu nhiên đó

Hình 2-22 Xử lý dữ liệu tạo bản đồ lưới chiếm dụng

Mỗi ô có thể biểu diễn 3 trạng thái là bị chiếm (occupied) màu đen, không bị chiếm (free) màu trắng và không xác định màu xám (unknown) như Hình 2-23

Hình 2-23 Các trạng thái trong occupancy grid map

Mục tiêu ước tính xây dựng bản đồ từ dữ liệu quan sát và vị trí nên biểu diễn dưới dạng xác suất như sau [24] :

Trong đó m là bản đồ gồm nhiều ô độc lập với nhau và có chỉ số i hay i i m= m , z 1:t là tập hợp dữ liệu quan sát từ vị trí bắt đầu đến thời điểm t, x 1:t là tập hợp tư thế từ vị trí bắt đầu đến thời điểm t Áp dụng Bayes rule và định lý Makov để tính xác suất cho mỗi ô ta được

Sử dụng phép tỉ lệ thay cho tính xác suất từ các dữ liệu thu được để tránh được sự mất ổn định về số đối với xác suất gần bằng 0 hoặc bằng một khi thực hiện nhiều phép nhân Xác suất có thể dễ dàng được phục hồi từ tỷ lệ log-odd:

Công thức Log Odds được định nghĩa :

=  −  (2.37) Áp dụng công thức Log Odds vào phương trình (2.36) ta được:

Ta được giải thuật Occupancy grid mapping được mô tả như sau:

3: if m i in perceptual field of z t then

Gói Hector_SLAM hỗ trợ nhiều package, trong đó :

• Hector_mapping: Đây là node chính của Hector SLAM Nó sử dụng dữ liệu từ cảm biến LiDAR để xây dựng và cập nhật bản đồ 2D của môi trường xung quanh robot Điều đặc biệt là hector_mapping không yêu cầu thông tin odometry (đoạn đường đã đi) và yêu cầu tài nguyên tính toán thấp, giúp đơn giản hóa quá trình triển khai SLAM trên robot

Lý thuyết thuật toán EKF

Trước tiên cần biết đến bộ lọc Kalman (Kalman Filter - KF) là một tập hợp các phương trình toán học cung cấp một giải pháp tính toán hiệu quả (đệ quy) cho phương pháp bình phương tối thiểu Bộ lọc này rất mạnh mẽ ở một số mặt như: hỗ trợ ước lượng các trạng thái quá khứ, hiện tại và thậm chí là tương lai, và có thể làm điều đó ngay cả khi tính chính xác của hệ thống mô hình không được biết đến Bộ lọc Kalman dựa trên giả định về hệ thống tuyến tính và tín hiệu đo lường, áp dụng nhiều trong hệ thống tuyến tính ít bị nhiễu

Bộ lọc Kalman mở rộng (Extended Kalman Filter) là một biến thể của bộ lọc Kalman cơ bản (Kalman Filter - KF) được sử dụng để ước tính trạng thái của hệ thống phi tuyến Khác với KF, EKF cho phép ước lượng trong mô hình phi tuyến tính bằng cách tạo ra một giá trị ước lượng xấp xỉ tại mỗi thời điểm lấy mẫu EKF được áp dụng nhiều trong các công trình nghiên cứu và thực tế trong lĩnh vực robot, cụ thể phổ biến trong các hệ thống điều khiển, dẫn đường và xử lí tín hiệu ước lượng trạng thái của một hệ thống phi tuyến

Quá trình thực hiện của thuật toán của bộ lọc EKF bao gồm 2 giai đoạn: dự đoán và cập nhật được mô tả theo Hình 2-25 [28]

Hình 2-25 Quy trình diễn ra lọc Kalman rời rạc bao gồm 2 giai đoạn dự đoán (Predict) và cập nhật (Update)

Các phương trình trong giai đoạn dự đoán được trình bày như sau: ˆ k − + 1= ˆ k + k x Ax Bu (2.40)

Trong đó: xˆ k+ 1 là ma trận trạng thái được ước tính của robot tại thời điểm k+1, u k là ma trận tín hiệu điều khiển tại thời điểm k, Plà ma trận hiệp phương sai lỗi ước tính đại diện cho độ tin cậy của dư đoán trạng thái trước khi cập nhật với đo lường mới,

Q k là ma trận hiệp phương sai nhiễu quá trình tại thời điểm k, W k là ma trận Jacobian của nhiễu quá trình tại thời điểm k[28]

Các phương trình trong giai đoạn cập nhật được trình bày như sau:

Trong đó: K k là ma trận Kalman gain tại thời điểm k, là ma trận hiệp phương sai của nhiễu đo lường tại thời điểm k, H k là ma trận Jacobian của hàm đo lường tại thời điểm k mô tả cách đo lường liên đến trạng thái hệ thống, V k là ma trận Jacobian của nhiễu đo lường tại thời điểm k

(2) Project the error convariance ahead

Time update Predict Measurment Update Update z k

Hình 2-26 Mô tả hoàn chỉnh về quá trình của thuật toán EKF tổng hợp từ các công thức ở hai giai đoạn dự đoán (Predict) và cập nhật (Update)

Từ Hình 2-26 [28] có thể mô tả đơn giản quá trình hoạt động của bộ lọc

Bộ lọc áp dụng cho ước tính vị trí:

Có vector trạng thái x n , n là số lượng biến trạng thái của robot Trong nghiên cứu này sử dụng vector trạng thái của robot với n=3 biến sau tại thời điểm k:

Từ mô hình ta có vận tốc của robot theo 2 phương x và y như sau:

Phương trình mô tả cách trạng thái của hệ thống thay đổi từ thời điểm k sang thời điểm k+1:

Trong đó: x k là biến trạng thái của robot tại thời điểm k, u k là tín hiệu điều khiển của robot tại thời điểm k, W k là nhiễu quá trình có thể bằng 0

Vector đo lường z m có dạng:

Trong đó: w k và v k là các giá trị ngẫu nhiên đại diện cho nhiễu quá trình và nhiễu đo lường Hàm phi tuyến tính f ( )  ở (2.49) có trạng thái tại thời điểm k+1 được tính thông qua trạng thái tại thời điểm k Hàm phi tuyến phương trình đo (2.50) có trạng thái phụ thuộc vào phép đo của bộ quan sát

Phương trình mô tả trạng thái chuyển động được biểu diễn như sau:

Khai triển ma trận Jacobian của công thức (2.51) như sau:

Thông tin trạng thái từ cảm biến IMU đo lường được tại thời điểm k được viết như sau:

Triển khai ma trận Jacobian của công thức (2.53) như sau:

Quá trình định vị vị trí bằng extended Kalman filter bao gồm: quá trình dự đoán và cập nhật

Quá trình dự đoán (Predict):

Dự đoán trạng thái tiếp theo dựa trên trạng thái hiện tại theo công thức sau:

Trong đó: x ˆ k + 1| k là x ước tính tại thời điểm k+1 được tính dựa vào x tại thời điểm k, và Flà:

Dự đoán ma trận hiệp phương sai lỗi ước tính tiếp theo (sử dụng ma trận Jacobian của hàm dự đoán và ma trận nhiễu quá trình):

Trong đó: f là hàm động học của robot, Plà ma trận hiệp phương sai lỗi ước tính thực tế, 𝑸 là ma trận hiệp phương sai nhiễu quá trình và P là ma trận xác định

Dự đoán giá trị đo lường được tính theo công thức sau: ˆ k = ˆ k − z Hx (2.58)

Vector đo lường z k imu , lấy giá trị từ IMU đo góc  nên hàm đo lường h x ( ) k sẽ có dạng như sau:

Khai triển ma trận Jacobian từ công thức (2.59) sẽ có dạng như sau:

Dự đoán giá trị đo lường từ trạng thái hiện tại:

Giá trị sai số giữa thực tế và ước lượng: k ˆ z = − k k e z z (2.62) Đặt Snhư sau: ˆ T k k k

Trong đó:R là nhiễu quan sát từ cảm biến

Tính toán ma trận Kalman gain, để xác định mức độ tin cậy của đo lường và mô hình dự đoán:

Dùng ma trận Kalman gain để ước lượng trạng thái dựa trên đo lường thực tế: ˆ ˆ k k z k

Cập nhật ma trận hiệp phương sai sau khi có giá trị đo lường mới:

Trong đó: I là ma trận xác định

Kết thúc lại quay lại bước dự đoán.

Lý thuyết AMCL

AMCL là một phương pháp để ước tính vị trí trong một bản đồ được xác định trước về bản chất chủ là thuật toán MCL (Monte Carlo Localization) Monte carlo Localization hay còn gọi là bộ lọc hạt, được sử dụng để xác định vị trí của robot trên bản đồ xác định trước Bộ lọc nhận thông tin bản đồ, ước tính vị trí và hướng của robot từ thuật toán khi di chuyển, và cảm nhận môi trường xung quanh Sử dụng thuật toán bộ lọc hạt để biểu thị sự phân bố của các trạng thái có khả năng xảy ra, mỗi hạt biểu thị sự phân bố của các trạng có thể xảy ra, hay còn gọi là giả thuyết đưa ra cho mỗi vị trí của robot [29]

• MCL biểu diễn phân phối hậu nghiệm bằng một tập các mẫu có trọng số

• Trong việc xác định vị trí, các bộ lọc hạt được lan truyền theo quan sát

• Trong bước tái lấy mẫu, các hạt mới được vẽ với xác suất tỉ lệ với xác suất của bộ quan sát

Mô tả toán học của bộ lọc hạt

• Tập hợp mẫu có trong số được ký hiệu như sau:

Trong đó: s [ ] i trạng thái giả định của hạt, w [ ] i trọng số tương ứng của mỗi hạt

• Các mẫu đại diện cho hậu nghiệm:

Trong đó:  s [ ] i : gọi là delta function, nó không phải là một hàm thông thường mà là một phân phối Thường được sử dụng để biểu diễn một ước lượng rời rạc của phân phối xác suất

Trong bộ lọc hạt, các mẫu phân phối nghiệm được gọi là các hạt (particle), được ký hiệu như sau:

Trong đó X t là tập hợp các hạt tại thời điểm t, m là số lượng các hạt trong tập hợp X t , mỗi hạt x t   m với ( 1   m M ) thể hiện trạng thái cụ thể tại mỗi điểm

Thuật toán Particle filter được mô tả như sau:

Quá trình hoạt động của MCL diễn ra như sau:

1 Khởi tạo: ban đầu các hạt (Particle) được khởi tạo giá trị ngẫu nhiên trong không gian vị trí có thể của robot, tập hợp các particle được biểu diễn như (2.69) với mỗi phần tử x [ ] m là một ước lượng vị trí ban đầu của robot Phân phối xác xuất ban đầu p x ( ) 0 , số lượng Particle M Tập hợp các particle với trạng thái ban đầu, mỗi hạt (Particle) được biểu diễn vị trí và hướng của robot trong không gian được lấy mẫu từ p x ( ) 0

2 Dự đoán: các hạt (Particle) được di chuyển dự vào dữ liệu từ cảm biến của robot, dữ liệu về vận tốc tiến và vận tốc xoay được đưa vào mô hình chuyển động kinematic và Dynamic của robot sử dụng để tính toán dự đoán di chuyển

Vị trí dự đoán di chuyển có thể được tính như sau:

Trong đó: xˆ t m là vị trí dự đoán của particle thứ m tại thời điểm t, f x ( t m − 1,u t ) là hàm mô hình di chuyển hoặc mô hình dự đoán tốc độ hướng của robot

3 Cập nhật trọng số: các particles được cập nhật dựa trên dữ liệu cảm biến như lidar, camera, hoặc IMU Cụ thể, mỗi particle được đánh giá thông qua việc tính toán xác suất của dữ liệu cảm biến được quan sát so với dự đoán của particle đó Các particle có xác suất cao tương ứng với dữ liệu cảm biến thực tế sẽ được tăng trọng số, trong khi các particle có xác suất thấp sẽ bị loại bỏ hoặc có trọng số giảm đi

Công thức tính toán trọng số trong AMCL khi dữ liệu cảm biến được thu thập là cảm biến lidar:

Trong đó: wˆ t m là trọng số cập nhật của particle thứ i, f x ( ) t [ ] m thường được gọi là hàm mục tiêu, g x ( ) t [ ] m là hàm đề xuất

Hình 2-27 Biểu đồ phân phối mẫu

4 Lấy mẫu lại (Resampling): Sau khi cập nhật, một phần hoặc toàn bộ các particles được lấy mẫu lại dựa trên trọng số của chúng Các particle có trọng số cao sẽ được lựa chọn với xác suất cao hơn để tái tạo lại, trong khi các particle có trọng số thấp sẽ bị loại bỏ Quá trình này giúp tập hợp các particles tập trung vào các vị trí có xác suất cao nhất và tạo ra một tập hợp particle mới

Tính tổng trọng số của tất cả các particles:

Tính trọng số tích lũy của mỗi particle bằng cách tính tổng các trọng số từ particle đầu tiên đến particle thứ m:

Trong đó: W m là trọng số tích lũy của particle thứ m

Quá trình này tạo ra một mảng trọng số tích lũy   W ( ) m M m = 1 ; W M = W , với W là tổng trong số của tất cả các particles.Sau đó chọn ngẫu nhiên giá trị  từ phân phối đều trong khoảng   0;1

Chọn lọc qua tập hợp các particles cũ như sau:

• Với mỗi x t m thứ m nếu có w t m  thì chọn lại particle này và thêm vào tập hợp particle mới

• Lặp lại quá trình này cho đến khi có đủ số lượng particle mới

5 Lặp lại: Quá trình dự đoán, cập nhật và lấy mẫu được lặp lại theo chu kỳ, với mỗi lần lặp cải thiện ước lượng vị trí của robot dựa trên dữ liệu cảm biến mới

Về điều kiện dừng của vòng lặp AMCL được sử dụng trong hệ ROS là sử dụng:

• Số vòng lặp tối đa (Max iterations): Điều kiện dừng này xác định số lần lặp tối đa mà thuật toán sẽ thực hiện trước khi kết thúc Nếu số lần lặp đạt đến ngưỡng này mà không đạt được mức độ chính xác mong muốn, thuật toán sẽ kết thúc và trả về kết quả cuối cùng

• Độ chêch lệch trọng số giữa các lần lặp (Min weight change): Điều kiện dừng này xác định ngưỡng cho độ chênh lệch của trọng số tích lũy giữa các lần lặp liên tiếp Nếu độ chênh lệch này nhỏ hơn ngưỡng, có thể coi rằng thuật toán đã hội tụ và kết thúc

Ngoài ra còn các các điều kiện dừng các để đáp ứng đúng các mục đích khác nhau như: Ngưỡng trọng tối thiểu, độ chính xác của vị trí ước lượng.

Lý thuyết A*

Heuristic là phương pháp giải quyết vấn đề dựa trên phỏng đoán, ước chừng, kinh nghiệm, trực giác để tìm ra giải pháp gần như là tốt nhất và nhanh chóng Hàm Heuristic là hàm ứng với mỗi trạng thái hay mỗi sự lựa chọn một giá trị ý nghĩa đối với vấn đề, dựa vào giá trị hàm này ta lựa chọn hành động

A* là thuật toán tìm kiếm trong đồ thị Thuật toán này tìm một đường đi từ điểm hiện tại đến điểm đích sử dụng hàm Heuristic ước lượng khoảng cách để đánh giá đường đi tốt nhất có thể Tùy theo mỗi dạng khác nhau mà hàm Heuristic được

A* lưu giữ một tập hợp các lời giải chưa hoàn chỉnh là các đường đi qua đồ thị từ điểm bắt đầu đến điểm kết thúc Thứ tự ưu tiên gán cho một đường đi được quyết định bởi hàm Heuristic:

Trong đó n là điểm hiện tại đang xét, g n ( ) là khoảng cách từ điểm đang xét đến điểm ban đầu, h n ( ) là khoảng cách ước lượng Heuristic từ điểm đang xét đến điểm cuối, f n ( ) là hàm tổng dùng để so sánh đường đi từ điểm đầu đến điểm đích [31]

A* thực hiện tính toán hàm f n ( ) tại điểm bắt đầu sau đó mở rộng ra các điểm lân cận Xét từng điểm lân cận, điểm nào có giá trị f n ( ) nhỏ nhất sẽ được thêm vào đường đi và xét điểm đó là điểm hiện tại Thuật toán tiếp tục mở rộng cho đến khi tìm đến điểm đích, đường đi qua các điểm đã chọn chính là đường đi ngắn nhất Pseudocode của thuật toán A* được trình bày như Hình 2-28.

Lý thuyết cảm biến Lidar

Cảm biến Light Detection And Ranging (Lidar) là công nghệ sử dụng ánh sáng tia laser đo khoảng cách và xây dựng bản đồ 3D của vật thể Cảm biến phát ra chùm tia laser tới môi trường và nhận về rồi tiếp nhận ánh sáng phản chiếu ngược lại phần cứng để xử lý Từ dữ liệu đó, hệ thống sẽ xây dựng bản đồ 3D trong không gian

Nguyên lý hoạt động của cảm biến Lidar khá đơn giản, bộ phát ra một chùm laser, khi tia laser chạm vật thể sẽ phản hồi lại, từ đó bộ thu tiếp nhận được tín hiệu

Bộ xử lý sẽ thực hiện tính toán thời gian truyền và nhận, từ đó có thể tìm được khoảng cách từ lidar đến vị trí vật cản và hướng của nó Minh họa cho nguyên lý hoạt động của cảm biến dò line được thể hiện ở Hình 2-29

Hình 2-29 Nguyên lý hoạt động cảm biến Lidar

Lý thuyết cảm biến IMU

Cảm biến Inertial Measurement Unit (IMU) là một thiết bị cơ điện hoặc trạng thái rắn chứa một dãy cảm biến có thể đo chuyển động Tức là phát hiện gia tốc tuyến tính và tốc độ góc xung quanh các trục X, Y và Z và cung cấp dữ liệu về chuyển động đó

Cảm biến IMU có thể đo chuyển động bằng cách chuyển đổi quán tính được phát hiện, là các lực được tạo ra do khả năng chống lại sự thay đổi hướng của vật thể, thành dữ liệu đầu ra mô tả chuyển động của vật thể Dữ liệu này sẽ được sử dụng bởi một số hệ thống khác, chẳng hạn như để điều khiển phương tiện Đầu ra của IMU thường là dữ liệu cảm biến thô: gia tốc kế (đo gia tốc tuyến tính dọc theo mỗi trục) và con quay hồi chuyển (tốc độ quay/đo vận tốc góc quanh mỗi trục) Minh họa về gia tốc kế và con quay hồi chuyển trong ba trục chuyển động X,Y, Z được thể hiện ở Hình 2-30

Hình 2-30 Gia tốc kế và con quay hồi chuyển trong ba trục chuyển động

Lý thuyết cảm biến Encoder

Cảm biến Encoder là một bộ cảm biến chuyển động cơ học tạo ra tín hiệu analog hoặc tín hiệu kỹ thuật số (digital) đáp ứng với chuyển động Loại thiết bị cơ điện này có khả năng biến đổi chuyển động (chuyển động tịnh tiến, chuyển động quay của trục, ) thành tín hiệu đầu ra số hoặc xung

Cấu tạo của cảm biến encoder gồm có các bộ phận chính: Thân, trục, một đèn LED làm nguồn phát sáng, 1 đĩa mã hóa có rãnh nhỏ quay quanh trục, khi đĩa này quay và chiếu đèn LED lên trên mặt đĩa thì sẽ có sự ngắt quãng xảy ra Các rãnh trên đĩa chia vòng tròn 360 thành các góc bằng nhau Một đĩa có thể có nhiều dãy rãnh tính từ tâm tròn, một bộ cảm biến ánh sáng thu tín hiệu để nhận tín hiệu từ đĩa quay và một bo mạch điện tử giúp khuếch đại tín hiệu Cấu tạo của cảm biến encoder được thể hiện trong Hình 2-31

Hình 2-31 Cấu tạo cảm biến encoder

Nguyên lý hoạt động của cảm biến Encoder dựa trên nguyên lý đĩa quay quanh trục Trên đĩa mã hóa có các rãnh nhỏ để nguồn phát sáng chiếu tín hiệu quang qua đĩa Chỗ có rãnh thì ánh sáng xuyên qua được, chỗ không có rãnh ánh sáng không xuyên qua được Với các tín hiệu có, hoặc không có ánh sáng chiếu qua, người ta ghi nhận được đèn led có chiếu qua lỗ hay không Số xung đếm được và tăng lên được tính bằng số lần ánh sáng bị cắt Cảm biến thu ánh sáng sẽ bật tắt liên tục để tạo ra các xung vuông Từ đó ta sẽ biết được vị trí và tốc độ của động cơ

Hình 2-32 Nguyên lý hoạt động cảm biến Encoder

Lý thuyết bộ điều khiển PID

Bộ điều khiển vi tích phân tỉ lệ (Proportional Integral Derivative – PID) bao gồm 3 khâu: khâu tỉ lệ (Proportional), khâu tích phân (Integrated) và khâu vi phân (Derivative) PID là một bộ điều khiển đơn giản, được sử dụng nhiều trong thực tế để điều khiển các hệ thống Bộ điều khiển PID có khả năng điều chỉnh sai số thấp nhất có thể, tăng tốc độ đáp ứng, giảm độ vọt lố, hạn chế sự dao động Cấu trúc bộ điều khiển PID được trình bày trong Hình 2-33 Có 3 thông số để điều chỉnh tín hiệu điều khiển: K P ,K K I , D

Hình 2-33 Cấu trúc bộ điều khiển PID

Trong đó: r t ( ) là giá trị đặt, y t ( ) là giá trị ngõ ra, giá trị ngõ ra được hồi tiếp về để so sánh với giá trị đặt r t ( ) Sự sai lệch giữa tín hiệu đặt và ngõ ra gọi là sai số e t ( )

Sai số được đưa vào bộ điều khiển PID sau đó xuất ra tín hiệu điều khiển u t ( ) điều khiển hệ thống Plant

Các thông số K P ,K K I , D là các thông số của bộ điều khiển PID cần phải lựa chọn Nếu thông số này được lựa chọn phù hợp thì hệ thống sẽ hoạt động tốt và chính xác, có nghĩa là độ vọt lố thấp, thời gian xác lập nhanh và sai số xác lập thấp.

Lý thuyết giao thức UART

2.15.1 Khái niệm về giao thức UART

Universal Asynchronous Receiver – Transmit (UART) là một giao thức truyền nhận dữ liệu nối tiếp bất đồng bộ giữa các vi xử lý với nhau Giao tiếp này đã ra đời từ rất lâu và là một giao thức khá phổ biến để giao tiếp giữa các thiết bị kỹ thuật số Cụm từ “bất đồng bộ” có nghĩa là không có tín hiệu đồng bộ hoá đầu ra của các bit từ UART truyền đến việc lấy mẫu các bit bởi UART nhận

UART truyền nhận thông qua hai dây TX và RX, trong đó TX là truyền và RX là nhận Để giao tiếp UART giữa hai thiết bị kĩ thuật số, ta cần đấu nối TX của MCU này với RX của MCU kia, sơ đồ kết nối được thể hiện như Hình 2-34

Một số thông số cơ bản trong truyền nhận UART là tốc độ truyền nhận (baudrate), là số bit có thể gửi đi trong một giây, data (dữ liệu truyền hoặc nhận), start bit, stop bit (bit bắt đầu và bit kết thúc), một bit parity (bit kiểm tra chẳn lẻ) tuỳ chọn

UART truyền dữ liệu nối tiếp theo một trong ba chế độ sau:

• Full Duplex: giao tiếp đồng thời đến và đi từ mỗi MCU truyền/nhận

• Haft Duplex: dữ liệu đi theo một chiều tại một thời điểm

• Simplex: chỉ giao tiếp một chiều

Hình 2-35: Cách thức hoạt động của giao thức UART

Giao thức UART là giao thức truyền nhận dữ liệu nối tiếp được chuyển đổi từ giao tiếp song song Dữ liệu truyền từ CPU sang chip UART1 bằng giao tiếp song song, chip UART1 sẽ thực hiện việc chuyển đổi tín hiệu song song sang nối tiếp và truyền đi sang chip UART2 bằng chân TX lần lượt từ các bit LSB đến MSB Tiếp đó hệ thống sẽ hiện thêm một bit start, một bit stop và một bit kiểm tra – parity (nếu có) Chip UART2 sau khi nhận được tín hiệu từ chân RX, sẽ loại bỏ các bit thừa và giữ lại data truyền về cho CPU

Hình 2-36: Gói dữ liệu trong UART

Gói dữ liệu trong UART được trình bày như Hình 2-36 bao gồm:

• Start bit: bit bắt đầu dữ liệu, thường khi bắt đầu UART kéo đường dữ liệu xuống mức thấp

• Data bit: bao gồm 8 đến 9 bit dữ liệu

• Parity bit: bit kiểm tra dữ liệu truyền xem chính xác hay không

• Stop bit: thường là 1 hoặc 2 bit được đặt ở cuối gói dữ liệu, thường khi kết thúc UART sẽ giữ đường dữ liệu ở mức cao

ROS Serial là giao thức gửi dữ liệu qua giao diện nối tiếp Trong triển khai nối tiếp máy khách-máy chủ ROS, máy chủ ROS là một máy tính chạy chương trình ROS và máy khách ROS là một bộ vi điều khiển nhận dữ liệu từ cảm biến và gửi dữ liệu đó đến máy chủ dưới dạng tin nhắn ROS Máy chủ ROS là nút xuất bản, trong khi máy khách ROS là nút đăng ký

Các gói ROS Serial có sẵn cho một số bộ vi điều khiển như Arduino, STM32, Linux, v.v Trong khi đó, các gói máy chủ ROS Serial có sẵn trong các phiên bản

Other Node ROS serial node

Hình 2-37: Tích hợp ROS Serial cho hệ thống robot

ROS serial tận dụng giao thức UART để thiết lập kết nối nối tiếp giữa máy chủ và hệ thống nhúng Dữ liệu được truyền ở định dạng nối tiếp qua giao diện UART, cho phép giao tiếp hai chiều giữa các nút ROS và hệ thống nhúng

Bằng cách sử dụng giao tiếp UART, ROS serial có thể tạo điều kiện thuận lợi cho việc trao đổi tin nhắn, lệnh và dữ liệu ROS giữa máy chủ và hệ thống nhúng Giao thức UART cung cấp một phương thức đáng tin cậy và được hỗ trợ rộng rãi cho giao tiếp nối tiếp, khiến nó phù hợp để kết nối ROS với các nền tảng nhúng hỗ trợ giao diện UART

ROS serial cung cấp cầu nối giữa máy chủ chạy ROS và hệ thống nhúng, cho phép giao tiếp hai chiều Nó cho phép hệ thống nhúng xuất bản dữ liệu cảm biến hoặc thông tin khác lên các chủ đề ROS, đăng ký các chủ đề ROS để nhận lệnh hoặc hướng dẫn và trao đổi tin nhắn ROS với các nút khác trong mạng ROS

Thư viện nối tiếp ROS cung cấp cách triển khai nhẹ nhàng giao thức truyền thông ROS có thể chạy trên các nền tảng nhúng có giới hạn tài nguyên Nó cho phép tuần tự hóa và giải tuần tự hóa các thông điệp ROS tương thích với giao thức truyền thông được sử dụng trong ROS Ngoài ra, ROS serial bao gồm các thư viện máy khách dành cho các nền tảng vi điều khiển khác nhau, giúp giao tiếp với ROS dễ dàng hơn

Với ROS serial, các nhà phát triển có thể tận dụng sức mạnh của ROS để điều khiển và phối hợp ở cấp độ cao hơn đồng thời tận dụng khả năng của các hệ thống nhúng để tích hợp cảm biến và điều khiển bộ truyền động ở mức độ thấp Điều này cho phép phát triển các hệ thống robot phức tạp kết hợp sức mạnh tính toán của ROS với khả năng thời gian thực của các hệ thống nhúng

Nhìn chung, ROS serial là một công cụ có giá trị trong hệ sinh thái ROS hỗ trợ giao tiếp giữa các nút ROS và hệ thống nhúng, cho phép tích hợp và cộng tác liền mạch giữa điều khiển cấp cao và giao diện phần cứng cấp thấp trong các ứng dụng robot.

Lý thuyết giao thức I2C

2.16.1 Khái niệm về giao thức I2C

Inter – Integrated Circuit (I2C) là một giao thức truyền nhận dữ liệu nối tiếp bán song công giữa thiết bị chủ - tớ, có nghĩa là thiết bị chủ và thiết bị tớ đều có thể truyền, nhận dữ liệu luân phiên Giao thức này thường được dùng trong ứng dụng có khoảng cách ngắn

I2C truyền nhận thông qua hai dây SDA và SCL, trong đó SDA là chân truyền dữ liệu và SCL là chân xung đồng hồ Để giao tiếp I2C giữa các thiết bị chủ - tớ, ta cần đấu nối nối các dây SDA và SCL với nhau, sơ đồ kết nối được thể hiện như Hình 2-38

Hình 2-38 Giao thức I2C 2.16.2 Cách thức hoạt động

Với giao thức I2C, dữ liệu được truyền dưới dạng các khung truyền Mỗi một thiết bị tớ có một khung địa chỉ khác nhau để thiết bị chủ có thể phân biệt Trong một lần truyền dữ liệu cần một khung địa chỉ để thiết bị chủ xác định thiết bị tớ và một hoặc nhiều truyền chứa dữ liệu đang được truyền Ngoài ra còn có các điều kiện bắt đầu và dừng, các bit Read/Write và các bit ACK / NACK Khung truyền trong giao thức I2C được thể hiện trong Hình 2-39

• Start bit: bit bắt đầu dữ liệu

• Bit Read/Write: Bit này dùng để xác định quá trình là truyền hay nhận dữ liệu từ thiết bị chủ Nếu thiết bị chủ gửi dữ liệu đi thì ứng với bit này bằng ‘0’, và ngược lại, nhận dữ liệu khi bit này bằng ‘1’

• Bit ACK/NACK: là từ viết tắt của Acknowledged / Not

Acknowledged Dùng để so sánh bit địa chỉ vật lý của thiết bị so với địa chỉ được gửi tới Nếu trùng thì thiết bị tớ sẽ được đặt bằng ‘0’ và ngược lại, nếu không thì mặc định bằng ‘1’

• Data bit: Gồm 8 bit và được thiết lập bởi thiết bị gửi truyền đến thiết bị nhân Sau khi các bit này được gửi đi, lập tức 1 bit ACK/NACK được gửi ngay theo sau để xác nhận rằng thiết bị nhận đã nhận được dữ liệu thành công hay chưa Nếu nhận thành công thì bit ACK/NACK được set bằng ‘0’ và ngược lại

THIẾT KẾ VÀ THI CÔNG HỆ THỐNG

Tổng quan hệ thống

Hệ thống bao gồm một mô hình robot di động vi sai hoạt động trong bản đồ và một máy tính PC để điều khiển và giám sát hoạt động của robot Robot cấu trúc vi sai có thể điều khiển từ xa bằng Wifi đồng thời xây dựng bản đồ bằng công nghệ SLAM Từ bản đồ vừa xây dựng robot có thể tự tìm ra con đường ngắn nhất từ điểm đầu đến Trong quá trình di chuyển nếu nhận thấy có vật cản, robot có thể tự lên một lộ trình mới từ vị trí hiện tại đến điểm đích và di chuyển theo con đường mới này tuỳ theo tình huống Người giám sát có thể biết thông tin của robot cũng như môi trường theo thời gian thực thông qua giao diện người dùng trên máy tính PC Mô tả hệ thống như hình Hình 3-2

Vật cản động Đích Quỹ đạo robot

Hình 3-1 Tổng quan hệ thống

Như Hình 3-2 ta chọn điểm đích khi không có vật cản nào robot sẽ đi thẳng đến đích tuy nhiên khi có vật cản chặn đường đi robot sẽ tiến hành tìm đường mới né vật cản Khi phát hiện vật cản đầu tiên robot quyết định tăng tốc vượt qua nhưng khi phát hiện vật cản thứ hai robot sẽ giảm tốc chờ vật cản đi qua hoặc như vật cản thứ ba sẽ tăng tốc vượt qua vật cản về đích

Hình 3-2 Sơ đồ hệ thống

Sơ đồ khối hệ thống được mô tả trong Hình 3-2 Ở khối computer, người vận hành sử dụng máy tính điều khiển và thu thập dữ liệu xử lí tính toán các giải thuật xây dựng bản đồ và tìm đường đi tối ưu Ngoài ra còn giám sát robot thời gian thực thông qua giao diện điều khiển khi robot hoạt động Trên mô hình robot chia thành các khối chức năng mang nhiệm vụ cụ thể đảm bảo vận hành robot một cách chính xác nhất Đầu tiên là khối Master có nhiệm vụ nhận tín hiệu điều khiển từ máy tính xử lý các giải thuật và đưa ra quyết định điều khiển robot từ tín hiệu đặt ban đầu Master nhận thông tin về môi trường thông qua cảm biến gắn trực tiếp và điều khiển robot hoạt động thông qua Slave Khối Slave có nhiệm vụ ghi nhận và xử lý dữ liệu cảm biến gửi về khối Master và nhận tín hiệu và điều khiển các cơ cấu chấp hành.

Lựa chọn thiết bị cho mô hình robot

Dựa vào các hoạt động của hệ thống cũng như sơ đồ khối hệ thống, nhóm đặt ra các yêu cầu thiết kế, khả năng của mô hình robot Từ đó lựa chọn các thiết bị phù hợp cho mô hình

Từ những đặc điểm hệ thống trên thì rút ra các chức năng hệ thống như sau

• Robot có thể điều khiển từ xa và xây dựng bản đồ

• Robot có thể tự tìm đường đi từ điểm bắt đầu đến điểm kết thúc

• Robot có khả năng tránh vật cản

• Giao diện điều khiển robot từ xa, hiển thị vị trí robot trong bản đồ, hiển thị trạng thái robot

Dựa vào chức năng trên thì Robot có yêu cầu thiết kế trình bày trong bảng

Bảng 3-1 Thông số mô hình

Kiểu lái Vi sai hai bánh

Kích thước 600 350 265 mm (dài rộng  cao)

Tổng khối lượng M = 10 kg Đường kính bánh xe 125mm

3.2.2 Tính toán lựa chọn động cơ Để lựa chọn động cơ ta giả sử robot chuyển động trên bề mặt phẳng song song với mặt đất, các bánh xe đồng thời tiếp xúc với mặt đất và khối lượng robot phân bố đều

Hình 3-3 minh họa các lực tác động lên toàn bộ robot khi robot di chuyển Trong đó F k là lực kéo của bánh xe, F ms là lực ma sát của bánh xe với sàn nhà, P là trọng lực của toàn bộ robot, N là lực nâng của sàn, O là tâm xoay của bánh xe

Hình 3-3 Các lực tác động lên robot

Xét hợp lực tác động lên robot khi robot di chuyển đạt tốc độ định mức Bỏ qua lực cản không khí, theo định luật II Newton ta có: k ms r

F +F + + =P N m a (3.1) trong đó m r và a lần lượt là khối lượng và gia tốc của robot

Chiếu phương trình (3.1) theo phương y của robot ta có:

Ta có tổng lực tác dụng lên mặt đất:

Trong đó: g là gia tốc trọng trường bằng 9,81 m s 2

Robot được thiết kế gồm 2 bánh chủ động và 4 bánh trợ lực bố trí đối xứng nên theo giả thuyết ta có phản lực tác động lên mỗi bánh xe như sau:

N = = = = N (3.4) Để robot chuyển động thì lực kéo phải lớn hơn lực ma sát lơn nhất mà robot phải chịu là ma sát nghỉ với hệ số ma sát bằng 0,8 với công thức sau:

Momen tối thiểu cho mỗi động cơ:

Tốc độ cần thiết của động cơ để robot đạt được vận tốc mong muốn:

Công suất cần thiết của động cơ:

Từ các thông số đã tính, nhóm quyết định chọn động cơ chọn hai động cơ BLHM5100k kèm hộp số có moment lớn có tích hợp encoder

Bảng 3-2 Thông số kỹ thuật động cơ BHLM5100K-GFS

Thông số Giá trị Đơn vị Điện áp hoạt động 24 VDC

Tốc độ qua giảm tốc 83.3 rpm

Hình 3-4 Động cơ BHLM5100k-GFS 3.2.3 Driver điều khiển động cơ

Dựa vào thông số động cơ đã chọn, nhóm tiến hành lựa chọn driver BHLD100 đi kèm với động để điều khiển động cơ Mạch cầu BHLD100 được minh họa như trong Hình 3-5 có thông số kỹ thuật trình bày trong Bảng 3-3

Bảng 3-3 Thông số kỹ thuật driver BHLD100

Thông số Giá trị Đơn vị

Chế độ Xung nội hoặc xung ngoại

Hình 3-5 Driver BLHD100K 3.2.4 Cảm biến Lidar

Cảm biến Lidar được sử dụng trong việc phát hiện và thu thập thông tin của vật cản vì độ chính xác cũng như khoảng cách phát hiện xa Đối với mô hình robot, nhóm sử dụng cảm biến RPLidar A1M8 như trong Hình 3-6 với thông số kỹ thuật được trình bày trong Bảng 3-4

Bảng 3-4 Thông số kỹ thuật cảm biến RPLidar A1M8

Thông số Giá trị Đơn vị

Nguồn cấp cho động cơ 5 – 10 VDC

Nguồn cấp cho bộ Scanner 5 VDC

Khoảng cách tối đa có thể quét được 12 m

Hình 3-6 Cảm biến RPLidar A1M8 3.2.5 Máy tính nhúng Raspberry Pi4

Mô hình sử dụng hai vi điều khiển, mỗi khối chức năng ở Hình 3-7 có một vi điều khiển đảm nhận việc xử lý và truyền nhận dữ liệu của khối chức năng đó

Như đã trình bày ở phần tổng quan hệ thống, Master có nhiệm vụ xử lý các giải thuật và đưa ra quyết định điều khiển robot nên cần một bộ xử lý tốt Vì vậy nhóm lựa chọn sử dụng máy tính nhúng Raspberry Pi4 để thực hiện các nhiệm vụ này Máy tính nhúng Raspberry Pi4 được minh họa như trong Hình 3-7 có thông số kỹ thuật như sau :

- Broadcom BCM2711, Quad core Cortex-A72 (ARM v8) 64-bit SoC @ 1.5GHz

- 2.4 GHz và 5.0 GHz IEEE 802.11ac wireless

- Đầu cắm GPIO 40 chân tiêu chuẩn Raspberry Pi

- Khe cắm thẻ nhớ Micro-SD để tải hệ điều hành và lưu trữ dữ liệu

- 5V DC qua đầu nối USB-C (tối thiểu 3A)

- 5V DC qua đầu cắm GPIO (tối thiểu 3A)

- Nhiệt độ hoạt động: 0-50 độ C

- Mức tiêu thụ điện năng tối đa khoảng 7,6W khi tải và 3,4W khi không tải

- Bộ cấp nguồn USB Type-C, ở mức 5.1V/3A

- Pi có thể chạy trên các hệ điều hành khác nhau như Raspberry OS (tiền thân là Raspbian), Ubuntu, hoặc các hệ điều hành đa phương tiện khác như Kodi OSMC và Libre Elec

Hình 3-7 Máy tính nhúng Raspberry Pi4 Model B 3.2.6 Arduino Mega 2560

Về khối Slave, nhóm sử dụng board Arduino Mega để điều khiển động cơ Board được minh họa trong Hình 3-8 và có thông số kỹ thuật như sau :

Bảng 3-5 Thông số Arduino Mega Điện áp hoạt động 5VDC Điện áp vào 7-12VDC

Tần số xung clock 16 MHz

Hình 3-8 Board Arduino Mega 3.2.7 Pin Để cấp nguồn cho toàn bộ thiết bị trên mô hình, nhóm sử dụng nguồn điện một khối pin này giảm áp cấp nguồn cho các vi điều khiển, cảm biến, Các khối Pin được đóng khối từ những viên Pin loại 21700 (minh họa ở Hình 3-9) ghép nối tiếp kết hợp với mạch cân bằng điện áp cho ra điện áp ổn định Thông số của hai khối Pin được trình bày lần lượt ở và

Bảng 3-6 Thông số khối Pin 24VDC

Thông số Giá trị Đơn vị Điện áp ngõ ra 24 V

Hình 3-9 Pin sạc 21700 Lishen 4000mAh 3.2.7.1 Mạch giảm áp

Vì máy tính nhúng Raspberry sử dụng nguồn 5VDC/3A nên nhóm sử dụng một mạch giảm áp từ 12VDC xuống 5VDC Mạch giảm áp được nhóm sử dụng là mạch LM2596S được mình họa ở Hình 3-10 có một ngõ ra USB type A dễ dàng cấp nguồn cho Raspberry, thông số kỹ thuật của mạch được trình bày ở Bảng 3-7

Bảng 3-7 Thông số kỹ thuật mạch giảm áp LM2596S

Thông số Giá trị Đơn vị Điện áp đầu vào 9 – 36 VDC Điện áp đầu ra 5.2 VDC

Hình 3-10 Mạch giảm áp LM2596S

3.2.8 Nút nhấn, công tắc và đèn Để đóng ngắt nguồn điện một cách an toàn cho mô hình robot, nhóm sử dụng một công tắt xoay được minh họa như trong Hình 3-11 với thông số kỹ thuật như sau:

- Chức năng: bật tắt nguồn

Hình 3-11 Nút nhấn giữ nguồn 24VDC

Bên cạnh đó, một nút nhấn nhả được nhóm sử dụng có chức năng khởi động lại các vi điều khiển để hoạt động lại từ đầu Nút nhấn được mình họa trong Hình 3-12 với thông số kỹ thuật như sau:

- Đường kính khoét lỗ: 16mm

- Điện áp định mức: 250VAC/30VDC

Thiết kế mô hình robot

Sau khi chọn các thiết bị, nhóm tiến hành thiết kế mô hình robot di động vi sai bốn bánh trên phần mềm Solidworks phiên bản 2021 Tổng quan mô hình 3D của robot được mô tả trong a) b)

Hình 3-15 Mô hình 3D robot a) có thân vỏ b) không có thân vỏ 3.3.1 Mặt đế

Với số từ các chi tiết thiết bị đã có ta thiết kế chi tiết mặt khung đế của robot cho phù hợp với kích thước của động cơ, khối lượng của động cơ để robot trở nên chắc chắn sử dụng vật liệu sắt để làm kết cấu khung của robot di động với kích thước chi tiết của mặt đế trong bản vẽ ở Hình 3-16

Hình 3-16 Chi tiết mặt đế của robot

Thiết kế phần liên kết giữa động cơ và mặt kế của xe gọi là giá đỡ động cơ với chi tiết được thiết kế như Hình 3-17, sử dụng vật liệu sắt để nâng cao khả năng chắc chắn một phần cũng do khối lượng động cơ khá nặng và phần chịu tải của toàn khối lượng của xe nên vật liệu sắt được sử dụng để thi công đảm bảo sự chắc chắn cho khung xe a) b)

Hình 3-17 Giá đỡ động cơ a) chưa lắp động cơ, b) đã lắp động cơ

Hai động cơ được gắn vào chung giá và gắn vào phần đế như sau

Hình 3-18 Giá động cơ và phần đế 3.3.3 Chi tiết khung Để chắc chắn và thi công nhanh gọn sử dụng nhôm định hình có kích thước 20x20 là sự lựa chọn tối ưu về thời gian thi công cũng như đảm bảo độ chắc chắn cho robot mô tả như Hình 3-19

Hình 3-19 Chi tiết khung robot được gắn với đế robot 3.3.4 Giá đỡ Lidar và IMU

Thiết kế giá đỡ Lidar và IMU dựa vào các chi tiết gắn ốc có trước của Lidar và IMU sao cho trọng tâm quét của Lidar và trọng tâm xoay IMU trùng với trọng tâm xoay của robot, được thiết kế với vật liệu đề xuất mica để tăng sự chính xác về phần cứng như mô tả Hình 3-20

Hình 3-20 Giá đỡ IMU và Lidar

Bố trí cảm biến IMU mặt dưới và lidar mặt trên để đảm bảo cảm biến có cùng trục tăng độ chính xác a) b)

Hình 3-21 Giá đỡ Lidar và IMU, a) Gắn IMU, b) Gắn Lidar 3.3.5 Chi tiết hộp điều khiển Để hệ thống dây điện trong robot gọn gàng thì việc thiết kế một hộp điều khiển đưa những board xử lí vào trong tủ điện là cần thiết, hộp được thiết kế với vật liệu là mica đảm bảo sự thẩm mĩ, chắc chắn và thi công nhanh Thiết kế như mô tả

Hộp được cắt mica từng phần như Hình 3-23 và ghép với nhau thông qua ke góc vuông a) b) c) d) e) f)

Hình 3-23 Hộp đựng vi điều khiển bao gồm (a) mặt trước, (b) mặt sau, (c) mặt trái, (d) mặt phải, (e) mặt trên và (f) mặt dưới 3.3.6 Chi tiết bộ bánh phụ của robot

Với hai bánh chính và thêm bốn bánh phụ ở bốn góc giúp robot có thể di chuyển linh hoạt phân bố tải trọng đồng đều Do phần bánh kích thước không tương đồng với bánh chính nên cần thêm một miếng chêm để đảm báo bánh có thể cùng chạm đất

Hình 3-24 Bộ bánh xe phụ

3.3.7 Chi tiết phần vỏ đầu robot phía bên trái và phải

Các chi tiết phụ được thiết kế theo mô tả từ các hình sau như phần vỏ robot di động ở phần đầu góc trái và phải với chi tiết khá phức tạp gia công chi tiết bằng in 3D nhựa a) b)

Hình 3-25 Vỏ đầu robot, a) phần vỏ đầu góc trái, b) phần vỏ đầu góc phải

Phần giao diện của robot gọi là vỏ của robot được thiết kế bằng sắt tấm dày 1 mm sau để tối ưu chi phí cũng như độ cứng và bền của vỏ robot di động Thiết kế được mô tả như Hình 3-26

Hình 3-26 Phần vỏ bảo vệ

Thi công mô hình robot

3.4.1 Chi tiết giá đỡ động cơ

Giá động cơ được bóc tách thành các mặt phẳng để cắt laser và hàn gắn kết lại với nhau để cấu tạo nên giá đỡ hoàn chỉnh đảm bảo sự chính xác về số liệu kích thước

Hình 3- 1 Chi tiết giá đỡ động cơ sau khi gia công

3.4.2 Chi tiết giá đỡ Lidar và IMU

Bản vẽ kích thước chi tiết được gia công bằng cắt laser mica Thành phẩm sau khi gắn cảm biến như Hình 3-28 và Hình 3-28

Hình 3-27 Giá cảm biến gắn Lidar

Hình 3-28 Giá cảm biến gắn imu

3.4.3 Chi tiết hộp điều khiển

Chi tiết hộp điều khiển được cắt bằng laser và rắp lắp như Hình 2-30 a) b)

Hình 3-29 Hộp điều khiển, a) hộp điều khiển với bố trí các linh kiện bên trong, b) hộp điều kiển khi lắp ráp hoàn thiện

3.4.4 Chi tiết phần vỏ đầu robot phía bên trái và phải

Phần đầu robot được in 3D bằng nhựa PLC Bên trái gắn nút nhấn khẩn cấp và đèn báo hiệu của nguồn động cơ Bên phải gắn nút reset vi điều khiển và đèn báo hiệu cả hệ thống a) b)

Hình 3-30 Chi tiết phần vỏ đầu robot, a) vỏ đầu robot phía bên trái, b) vỏ đầu robot phía bên phải

Phần vỏ có kích thước như được gia công bằng sắt tấm cắt bằng laser sau đó được mang đi chấn các góc, sau đó hàn cố định khung để đảm bảo độ cứng của vỏ và gia công làm phẳng lại các bề mặt rồi phủ sơn a) b)

Hình 3-31 Chi tiết vỏ robot sau khi gia công, a) góc nhìn ngiêng, b) góc nhìn thẳng

3.4.6 Lắp ráp hoàn thiện robot

Sau khi hoàn tất các bộ phận , nhóm thực hiện lắp ráp hoàn thiện robot như gắn giá động cơ, khung xe, các cảm biến và các phần vỏ thì được mô hinh hoàn thiện như sau a) b) c) d)

Hình 3-32 Mô hỉnh robot hoàn thiện a) góc trên xuống không có vỏ b) góc trên xuống có vỏ c) góc nghiên không có vỏ b) góc nghiên có vỏ

Sơ đồ nối dây hệ thống

Sơ đồ cấu trúc giao tiếp như Hình 3- 2

Hình 3- 2 Sơ đồ kết nối hệ thống

Máy tính và Raspberry sẽ kết nối với chung router Wifi, từ máy tính điều khiển raspberry thông qua giao tiếp ssh để chạy các câu lệnh và chương trình bằng sóng wifi truyền nhận dữ liệu, lidar sẽ giao tiếp với raspberry bằng UART, truyền nhận dữ liệu khoảng cách và góc cho raspberry, arduino mega đọc các dữ liệu: góc từ IMU thông qua giao tiếp I2C, tín hiệu digital của động từ driver truyền đến arduino mega Sau đó arduino mega sẽ xử lí tín hiệu từ cảm biến của encoder và IMU rồi gửi lên raspberry thông qua giao thức UART

Sơ đồ cấp nguồn cho robot mô tả như Hệ thống sử dụng nguồn điện 24V cấp nguồn cho cả hệ thống,1 công tắc xoay (Power) đóng ngắt và 1 đèn (Đ1) báo hiệu Nút nhấn dùng khẩn cấp (EMG) sử dụng để ngắt nguồn điện cho động cơ và đèn (Đ2) báo hiệu Để cấp nguồn cho vi điều khiển thì nhóm sử dụng giảm áp xuống 5v cấp nguồn

Hình 3-33 Sơ đồ nối nguồn 24v

Tiếp theo là sơ đồ nối dây nguồn 5v gồm có raspberry nối với cảm biến lidar và vi điều khiển arduino mega 2560 Vi điều khiển đọc cảm biến với Imu và điều khiển động cơ đồng thời đọc tín hiệu encoder

Hình 3-34 Sơ đồ nối nguồn 5v

Thiết kế và thi công mạch PCB

Để đảm bảo yêu cầu điều khiển robot như sự ổn định, thay thế lắp đặt dễ dàng dễ vận hành, kiểm tra sửa chữa khi vận hành trên thực tế nên nhóm đã thiết kế mạch PCB đáp ứng nhu cầu trên Mạch PCB sẽ được cấp nguồn 24V trực tiếp, nguồn sẽ được cấp trực tiếp cho driver và giảm áp Sau bộ giảm áp sẽ cấp cho raspberry và arduino mega 2560 và imu Sơ đồ nguyên lý như

Hình 3-35 Sơ đồ đi dây PCB

Hình 3-36 Sơ đồ nguyên lý mạch PCB

Sau khi thiết kế PCB hoàn thiện nhóm tiến hành thi công Mạch PCB được nhóm thi công một lớp Đảm bảo tính linh loạt dễ tháp lắp nhóm sử dụng các jack để dễ dàng tháo lắp như Hình 3-37 Thứ tự các jack được đnáh dấu như hình và chức năng từng là jack một cấp nguồn cho hai quạt làm mát cho hộp điều khiển, jack hai cấp nguồn và tín hiệu cho cảm biến imu, jack ba và bốn là điều khiển hai driver, jack năm là nút reset arduino, jack sáu cấp nguồn động cơ, jack bảy là nút nhấn khẩn cấp và đèn, jack tám công tắt nguồn và đèn, cuối cùng jack chín là nguồn 24V

Hình 3-37 Mạch PCB hoàn thiện

Thiết kế vật cản động

Từ những đặc điểm của một vật cản động trong nghiên cứu này từ đó đưa ra các yêu cầu sau:

• Vật cản có thể chuyển động thẳng tốc độ không đổi

• Có thể chiều chỉnh vận tốc của vật cản

• Chiều cao của vật cản động phải nằm trong khoảng lidar quét được Dựa vào chức năng trên thì vật cản có yêu cầu thiết kế trình bày trong bảng

Bảng 3-8 Thông số mô hình

Kiểu lái Vi sai hai bánh

Tổng khối lượng 4 (kg) Đường kính bánh xe 65 (mm)

3.7.2 Lựa chọn thiết bị và thi công

Dựa vào yêu cầu đặt ra ở trên nhóm chọn thiết bị phù hợp

Bảng 3-9 Danh sách thiết bị vật cản động

STT Tên thiết bị Hình ảnh

6 Khớp nối trục động cơ

7 Hộp đế pin 18650 loại 3 cell

8 Công Tắc ON-OFF 2 Chân 6A

Kết cấu xe vật cản được thiết kế gồm hai phần chính bao gồm khung xe (Hình 3-38 a) và vỏ xe (Hình 3-38b) ,khung xe được bố trí board điều khiển arduino nano, board mở rộng arduino nano, module L298, và hai động cơ JGB37-520, pin Lipo

12V Vỏ xe được thiết kế với chiều cao nằm trong tầm có thể quét của Lidar để robot có thể phát hiện được vật cản a) b) c)

Hình 3-38 Mô hình xe vật cản, a) Khung xe vật cản, b) Vỏ xe vật cản, c) Tổng quan mô hình sau khi lắp rắp hoàn chỉnh.

Chương trình điều khiển

3.8.1 Lưu đồ chương trình robot Đầu tiên, Arduino sẽ khởi tạo các biến, thực hiện các nút (node) giao tiếp với ROS Ngắt ngoài được sử dụng để đọc tín hiệu xung encoder từ driver của động cơ Khi có tín hiệu đặt từ Raspberry, Arduino chuyển vận tốc đặt thành vận tốc đặt của hai bánh Timer của Arduino thực hiện lấy mẫu tính toán tín hiệu điều khiển động cơ

Cập nhật giá trị IMU và vận tốc robot T=0.1s

Cập nhật giá trị đặt

1 Điều khiển động cơ PID vận tốc

Hình 3-39 Lưu đồ chương trình chính

Chương trình ngắt ngoài được sử dụng cho từng động cơ Khi có sự kiện ngắt ngoài cạnh xuống, chương trình sẽ kiểm tra động cơ quay thuận hay quay ngược từ đó cộng hay trừ biến xung của encoder Lưu đồ chương trình ngắt ngoài được thề hiện như Hình 3-40

Hình 3-40 Lưu đồ chương trình con thực hiện ngắt ngoài

Chương trình bộ điều khiển được sau 0.01s

Chuyển vận tốc đặt thành vận tốc đặt của bánh xe

Tính sai số e và cộng dồn sai số errTotal

Tính vận tốc bánh xe hiện tại

Tính đạo hàm sai số và cập nhật sai số hiện tại vào preErr

Hình 3-41 Chương trình con bộ điều khiển PID

Chương trình con điều khiển động cơ

Start u>0 state = 1 state = 0 state != pre_state Chờ 10ms Đảo chiều động cơ pre_state = state

Hình 3-42 Chương trình con điều khiển động cơ

Chương trình con sẽ kiểm tra trạng thái của động cơ, nếu trạng thái động cơ

3.8.2 Lưu đồ chương trình robot vật cản Đối với robot vật cản, Arduino Mega thực hiện tính toán bộ điều khiển PID sau mỗi 5 ms Sau đó, tính toán quãng đường đi được nếu quãng đường lớn hơn giá trị đặt thì đổi vận tốc đặt cho robot Bộ điều khiển PID và ngắt ngoài đọc cảm biến

Encoder được thực hiện theo lưu đồ trình bày ở mục 3.8.1 Lưu đồ chương trình chính của robot vật cản được thể hiện trong

2 Đ PID vận tốc Điều khiển động cơ Flag_PID = 0

Tính vận tốc Tính quãng đường đi x > dis Đ Đảo vận tốc Flag_check_mdoe = 0 đặt

Hình 3-43 Lưu đồ chương trình chính của robot vận cản

Timer của Arduino kiểm tra sự kiện sau mỗi 5 ms Lưu đồ chương trình ngắt được thể hiện trong Hình 3-44

Hình 3-44 Lưu đồ chương trình ngắt

Sau khi tính toán tín hiệu điều khiển Arduino sẽ điều khiển động cơ thông qua hàm điều khiển động cơ Lưu đồ chương trình điều khiển được thể hiện trong Hình 3-45

Hình 3-45 Lưu đồ chương trình điều khiển động cơ

THIẾT KẾ VÀ MÔ PHỎNG GIẢI THUẬT

Tổng quan chương trình điều hướng cho robot

Dựa vào yêu cầu hoạt động của hệ thống được trình bày ở mục 3.1, nhóm thực hiện thiết kế giải thuật điều hướng cho robot Giải thuật điều hướng bao gồm lập kết hoạch toàn cục A* và lập kế hoạch cục bộ MPC được thực hiện trên ROS Lưu đồ tổng quan của giải thuật điều hướng được mô tả như Hình 4-1

Khởi tạo trạng thái ban đầu của robot

Lập kế hoạch toàn cục Đọc dữ liệu từ cảm biến

Lập kế hoạch cục bộ

Giải bài toán tối ưu Điều khiển robot xy