Ngõ ra của bộ điều khiển PID chính là tổng của ngõ ra ba khâu này:
out = out + out + out
Khâu tỉ lệ là tích giữa sai số hiện tại với hằng số độ lợi Kp:
out = × ( )
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:
= ∫ ( ) 0
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: out = × ( )
Hàm truyền của bộ điều khiển PID liên tục: ( ) = + + (3.2) (3.3) (3.4) (3.5) (3.6) Sự phát triển của các 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 tố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 về miền thời gian rời rạc cho từng khâu như sau:
Với T là chu kỳ lấy mẫu (giây). Từ đó cho ta ngõ ra tác động u(kT) là:
( ) = ( ) + ∑ ( ) +
0
(3.9)
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 đồ án chúng em sử dụng phương pháp cân chỉnh thủ công theo các bước sau:
Bước 1: Khởi tạo các giá trị KP, KI, 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 1/2 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.
Q trình cân chỉnh cho thấy với hệ số KP= 0.7, KI = 3.1, KD = 1 cho đáp ứng nhanh và ít dao động nhất.
3.1.2. Robot Operation System (ROS)
3.1.2.1. Giới thiệu chung
Robot Operating System (ROS) là hệ điều hành robot cung cấp các thư viện và các công cụ để giúp các nhà phát triển phần mềm tạo ra các ứng dụng cho robot. Nó khơng chỉ cung cấp các dịch vụ hệ điều hành tiêu chuẩn (trừu tượng hóa phần cứng, quản lý tranh chấp, quản lý quy trình) mà cịn cung cấp các chức năng cấp cao (cơ sở dữ liệu tập trung, hệ thống cấu hình robot, …). ROS là gói cơng cụ được cài đặt trên Ubuntu.
3.1.2.2. Tính năng của ROS
ROS cung cấp các gói cơng cụ được tiêu chuẩn hóa giúp:
•Quản lý phần cứng bằng cách viết trình điều khiển. •Quản lý bộ nhớ và các quy trình.
•Quản lý đồng thời, song song và hợp nhất dữ liệu.
•Cung cấp các thuật tốn lý luận trừu tượng, sử dụng trí tuệ nhân tạo.
3.1.2.3. Phương pháp hoạt động của ROS
Hình 3.6 bên dưới mơ tả hoạt động của ROS gồm các phần chính là các tệp tin được bố trí từ trên xuống theo thứ tự và hoạt động như sau: Metapackages, Packages, Packages manifest, Misc, Messages, Services và Codes.
Hình 3.6: Hệ thống ROS –[2]
Trong đó gói tổng (Metapackages) là một nhóm các gói (packages) có liên quan tới nhau. Ví dụ, trong ROS có một gói tổng tên là Navigation, gói này có chứa tất cả các gói liên quan tới việc di chuyển robot, bao gồm di chuyển thân, bánh, các thuật toán liên quan như Kalman, Particle filter, … Khi cài đặt gói tổng, nghĩa là tất cả các gói con trong nó cũng được cài đặt.
Gói (Packages): khái niệm gói rất quan trọng, chúng ra có thể nói rằng các gói chính là các ngun tử cơ bản nhất tạo nên ROS. Trong một gói gồm có: ROS node, datasets, configuration files, source files, tất cả được gói lại trong một “gói”. Tuy nhiên, mặc dù có nhiều thành phần trong một gói, nhưng để làm việc, chúng ta chỉ cần quan tâm đến 2 thành phần, đó chính là src folder, chứa source code của chúng ta, và file Cmake.txt, đây là nơi ta khai báo những thư viện cần thiết để thực thi (compile) code.
Hình 3.7: Sự tương tác các node trong ROS –[2]
Trong hình 3.7 trên ta có thể thấy sự tương tác giữa các node ở trong ROS: Master chính là nodes kết nối tất cả các node còn lại, các nodes cịn lại muốn giao tiếp với nhau phải thơng qua node Master.
Nodes: ROS nodes chỉ đơn giản là quá trình sử dụng ROS API để giao tiếp với nhau.
Một robot có thể có rất nhiều nodes để thực hiện q trình giao tiếp của nó.
Master: ROS Master đóng vai trị như một node trung gian kết nối giữa các node khác
nhau. Master bao quát thông tin về tất cả các node chạy trong mơi trường ROS. Nó sẽ trao đổi thơng tin, giao tiếp sẽ bắt đầu giữa hai node ROS.
Message: Các node ROS có thể giao tiếp với nhau bằng cách gửi và nhận dữ liệu dưới
dạng ROS message. ROS message là một cấu trúc dữ liệu được sử dụng bởi các node ROS để trao đổi dữ liệu. Nó giống như một protocol, định dạng thông tin gửi đi giữa các node, ví dụ như string, float, int, …
Topic: Một trong những phương pháp để giao tiếp và trao đổi tin nhắn giữa hai node
được gọi là ROS Topic. ROS Topic giống như một kênh tin nhắn, trong kênh đó dữ liệu được trao đổi bằng ROS message.
Service: Service là một loại phương pháp giao tiếp khác với Topic. Service sử dụng tương
tác publish hoặc subcrible nhưng trong q trình hoạt động, nó tương tác theo kiểu request
– response.
3.1.2.4. Các node chính trong ROS 3.1.2.4.1. Twist to motor
Node Twist to motor thực hiện chuyển vận tốc tuyến tính và vận tốc góc sang giá trị vận tốc cho từng động cơ. Mối quan hệ giữa vận tốc tuyến tính v, vận tốc góc ω và vận tốc động cơ bên trái vl và động cơ bên phải vr như hình 3.8.
Hình 3.8: Các giá trị vận tốc của robot
Trong đó:
+ l là khoảng cách giữa hai bánh xe (m).
+ I là tâm ảo của đường tròn khi robot được chạy theo quỹ đạo cong. + R là khoảng cách từ tâm ảo đến trọng tâm của robot.
Khi đó vận tốc của từng động cơ được tính theo cơng thức sau:
= + ×
{
= − ×
Vi điều khiển nhận và xử lý tốc độ nhận được rồi xuất xung PWM để điều khiển hai động cơ thông qua cầu H, kết hợp với thuật toán PID chạy theo vận tốc đã đặt.
3.1.2.4.2. IMU
Node Imu thực hiện nhận giá trị từ module IMU MPU6050 thông qua Jetson Nano. Những giá trị này sẽ được tách và xử lí thành những giá trị Roll Pitch Yaw tương ứng. Sau đó sẽ được truyền sang Node Odometry để ước lượng hướng quay của robot.
3.1.2.4.3. Odometry
Node Odometry thực hiện ước lượng vị trí của robot (x, y, θ) từ thơng tin của encoder và IMU. Vị trí của robot được biểu diễn qua bộ ba thông số k = {x, y, θ}, trong đó x, y là toạ độ trong hệ toạ độ Decade và θ là hướng của robot so với trục x. Quỹ đạo của robot sau thời gian t là k0:t = x0, x1, x2,… xt. Giả sử robot có vị trí đầu (x0, y0, θ0). Vị trí mới của robot sau một khoảng thời gian t di chuyển được một đoạn d và xoay góc θ như trên hình 3.9 được tính theo cơng thức:
′
′
Trong đó, x, y và θ là khoảng cách di chuyển và góc xoay của robot trong khoảng Δt, được tính theo cơng thức:
= ⋅ cos( ) { = − ⋅ sin( )
= ⋅Δ
Tốc độ góc có thể được tính từ encoder rồi quy ra góc quay của robot. Tuy nhiên trong q trình thử nghiệm, việc tính tốc độ góc từ encoder có sai số lớn, gây ảnh hưởng bài tốn xác định vị trí. Vì vậy, IMU được dùng để đọc trực tiếp giá trị góc quay của robot. Khoảng cách di chuyển d của robot được tính từ khoảng cách di chuyển độc lập của hai bánh xe trái và phải thông qua giá trị đọc về của encoder theo công thức:
=+ ℎ
right
{
Trong đó:
Δencoder_left, Δencoder_right là số xung encoder đọc được trong khoảng thời gian Δt (xung). dleft, dright là khoảng cách hai bánh xe trái và phải đi được (m). tick_meter là số xung encoder khi bánh xe đi được một mét (ppm)
Hình 3.9: Cập nhật vị trí mới robot
3.1.2.4.4. AMCL/SLAM
Node SLAM thực hiện vẽ bản đồ mơi trường khi robot nhận được ước lượng vị trí và dữ liệu từ cảm biên Lidar. Để có thể định vị và tránh vật cản, robot cần biết bản đồ môi trường.
Ởlần đầu tiên, người dùng sẽ điều khiển robot di chuyển bằng tay để vẽ được bản đồ môi trường. Bản đồ sẽ được vẽ bằng phương pháp Hector SLAM. Node AMCL thực hiện nhiệm vụ xác định vị trí của robot trong bản đồ đã vẽ. Do đó node AMCL hoạt động khi robot tự
định vị và di chuyển trong bản đồ. Hai node SLAM và AMCL không hoạt động cùng lúc.
3.1.2.4.5. RPLidar
Node Rplidar thực hiện đọc dữ liệu từ cảm biến laser Lidar trả về khoảng cách từ robot đến vật cản trong tầm đo lên đến 12m và góc quét 360°. Dữ liệu này được dùng để vẽ bản đồ, định vị vị trí của robot và tránh vật cản.
3.1.2.4.6. Robot State Publisher
Node Robot State Publisher hỗ trợ hiển thị vị trí và hướng xoay robot trong phần mềm mô phỏng 3D. Mỗi bộ phận của robot sẽ được biểu diễn bằng một Vector vị trí trong khơng gian. Ví dụ trạng thái vị trí của robot trong khơng gian tại thời điểm t có thể được biểu diễn thơng qua Vector như hình 3.10. Nếu robot có nhiều khớp nối hoạt động độc lập như cánh tay robot 6 trục thì mỗi trục sẽ được biểu diễn bằng một Vector. Khi hoạt động phần mềm mô phỏng sẽ dễ dàng hiển thị sự di chuyển của từng khớp nối thông qua sự thay đổi của từng Vector và mối quan hệ giữa những Vector.
Hình 3.10: Vector vị trí của robot
3.1.2.4.7. Map Server
Node Map Server dùng để quản lý bản đồ được robot sử dụng. Map Server cung cấp công cụ để lưu bản đồ vẽ từ SLAM và tải bản đồ để sử dụng cho AMCL. Bản đồ được lưu thành hai file khác nhau. Một file YAML dùng để lưu thông số của bản đồ như: tên bản đồ, độ phân giải, gốc toạ độ ban đầu (thường là (0,0,0)), mức ngưỡng xác định vật cản. File còn lại là file ảnh của bản đồ, thường là ảnh xám. Pixel có màu đen là vật cản, màu trắng là khơng có vật cản, màu xám chỉ vùng bản đồ chưa có thơng tin do chưa được quét.
Hình 3.11: Bản đồ mơi trường được quản lí bằng Map Server
3.1.2.4.8. Move Base
Node Move Base sử dụng vị trí robot từ AMCL và bản đồ từ Map Server để điều khiển robot di chuyển đến đích. Đường đi vạch sẵn sẽ được tìm bằng thuật tốn Dijkstra. Trong q 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 bao gồm vận tốc tuyến tính và vận tốc góc được truyền đến Node Twist To Motor để điều khiển robot.
3.1.2.5. Các phép biến đổi tọa độ trong ROS
Gọi hệ tọa độ gắn liền với robot và nằm ở tâm quay của robot là "base_footprint". Mỗi cảm biến sử dụng trong robot phải được khai báo là nằm ở hệ tọa độ nào và phép biến đổi tọa độ từ hệ tọa độ "base_footprint" tới hệ tọa độ đó. Các gói Hector SLAM và Navigation cũng yêu cầu các phép biến đổi hệ tọa độ trên robot được khai báo đầy đủ. Hình 3.12 trực quan hóa các hệ tọa độ trên mơ hình 3D của robot bằng cơng cụ RVIZ. Hình 3.13 mơ tả các phép biến đổi hệ tọa độ trên robot trong đồ án.
3.1.3. Giao tiếp giữa Jetson Nano và STM32
3.1.3.1. Giao tiếp UART
Có nhiều phương thức để trao đổi thơng tin trong robot, tuy nhiên do giao thức rosserial chưa hoàn thiện và hay xảy ra lỗi đối với Kit STM cho nên nhóm chúng em đã sử dụng chuẩn UART để thay thế. Mục đích của việc giao tiếp này kit STM có thể nhận được tín hiệu vận tốc gửi về từ Jeston nano đồng thời gửi số liệu của encoder lên Jeston nano để từ đó đưa vào ROS để xử lí.
3.1.3.1.1. Giới thiệu về chuẩn giao tiếp UART
UART (viết tắt của Universal Asynchronous Receiver Transmitter) là một vi mạch sẵn có trong vi điều khiển, với chức năng chính là truyền dẫn dữ liệu nối tiếp. Các thiết bị giao tiếp UART với nhau có thể được kết nối như hình 3.14.
Hình 3.13: Chuẩn giao tiếp UART
Trong UART, giao tiếp giữa hai thiết bị có thể được thực hiện theo hai phương thức là giao tiếp dữ liệu nối tiếp và giao tiếp dữ liệu song song. Giao tiếp dữ liệu nối tiếp có nghĩa dữ liệu được truyền qua lại giữa hai thiết bị ở dạng bit - bit và chỉ cần hai đường kết nối. Nó địi hỏi ít mạch và cáp/dây, ít tốn kém hơn giao tiếp dữ liệu song song. Bởi trong giao tiếp dữ liệu song song, dữ liệu được truyền qua nhiều cáp/dây cùng một lúc. Và cũng nhờ đó mà giao tiếp dữ liệu song song lại có ưu điểm hơn giao tiếp dữ liệu nối tiếp về mặt tốc độ truyền dữ liệu.
Giao tiếp UART có các thơng số chính:
-Baud rate (tốc độ baud): Khoảng thời gian để 1 bit được truyền đi, nó phải được cài đặt
giống nhau ở cả thiết bị gửi và thiết bị nhận.
- Frame (khung truyền): Khung truyền sẽ quy định xem mỗi lần truyền là bao nhiêu
bit
được truyền đi.
-Start bit: Trong một khung truyền, đây sẽ là bit được truyền đi đầu tiên, nó báo hiệu cho
thiết bị nhận có một gói dữ liệu sắp được truyền đến. Đây là bit bắt buộc phải có trong một khung truyền.
- Data: Dữ liệu nhận được trong q trình truyền và nhận, bit có trọng số thấp nhất LSB
(viết tắt của từ Least Significant Bit) được truyền đi trước, sau đó truyền dần các bit cho đến bit cuối cùng là bit có trọng số cao nhất MSB (viết tắt của từ Most Significant Bit).
- Parity bit (bit kiểm tra chẵn-lẻ): bit này nhằm mục đích kiểm tra dữ liệu truyền.
- Stop bit: Khi kết thúc một khung truyền, stop bit sẽ được truyền. Nó bao gồm một hoặc
các bit nhằm báo cho thiết bị rằng các bit đã được gửi xong, thiết bị nhận sẽ kiểm tra khung truyền để đảm báo tính đúng đắn của dữ liệu. Đây là bit bắt buộc phải có trong một khung truyền.
3.1.3.1.2. UART trên kit STM32F407VE
Kit STM32F407VE có hỗ trợ 6 bộ UART như sau:
Hình 3.14: Bộ UART trên kit STM32F407VE
Thiết lập trên CubeMX:
Hình 3.15: Thiết lập trên CubeMX
byte cho mỗi lần gửi từ Jeston nano xuống cũng như 8byte cho mỗi lần gửi từ STM lên. Sau khi nhận được 8 byte hàm ngắt sẽ được gọi và tiếp tục nhận 8 byte tiếp.
3.1.3.1.3. UART trên Jetson Nano
Trên board của Jeston Nano cũng hỗ trợ 2 bộ UART:
Hình 3.16: Các chân cắm trên Jetson Nano
Ởđây nhóm em sử dụng bộ UART1 tương ứng với chân 8 và 10. Chân Rx trên STM sẽ nối với chân Tx trên Jeston nano tương ứng là chân PA2(STM) sẽ nối với chân 10 trên Jeston nano. Còn lại chân PA3(STM) sẽ nối với chân 8 trên Jeston nano, chân GND sẽ nối thông giữa 2 mạch.
3.2. Các thuật toán áp dung
3.2.1. Thuật toán quét và tạo dựng bản đồ (SLAM)
3.2.1.1. Giới thiệu chung
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. Bên cạnh đó việc cài đặt công nghệ này cũng không tốn nhiều thời gian.
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ờ đó xe tự hành có thể di chuyển trong vùng an tồn và hiệu quả nhất.
Hình 3.17: Xe AGV sử dụng cơng nghệ SLAM
Các cảm biến cung cấp những dữ liệu để xây dựng hình ảnh xung quanh chúng và vị trí của chúng trong mơi trường đó.
Từ đó, thiết bị (robot, xe) có thể định vị đang ở đâu, trạng thái, tư thế của nó trong mơi