Hệ điều hành Robot Operating System (ROS) là hệ điều hành mã nguồn mở đƣợc rất nhiều chuyên gia, kỹ sƣ trên thế giới vận dụng và ngày càng phát triển mạnh hơn. Nó hỗ trợ rất nhiều thƣ viện, chƣơng trình cũng nhƣ những ví dụ cụ thể để ngƣời học có thể hiểu rõ hơn về những ứng dụng trên thực tế. Để hiểu rõ hơn về thực tế đề tài “Lập trình Robot 3 bánh di chuyển theo ngƣời sử dụng cảm biến Kinect” chạy nhƣ thế nào thì ở đây nhóm chúng em sử dụng một ví dụ có sẵn của mã nguồn mở ROS để mô phỏng và dựa vào thuật toán này để làm cơ sở cho thực hiện chƣơng trình trên Robot.
Cụ thể, chƣơng trình mô phỏng gồm có hai con rùa có sẵn chạy trên hệ điều hành ROS, con rùa thứ nhất sẽ đƣợc điều khiển bằng bàn phím máy tính để di chuyển còn con rùa thứ 2 sẽ di chuyển theo con rùa thứ nhất và giữ khoảng cách với
13 con rùa thứ nhất. Và trên thực tế nếu ứng dụng thuật toán tƣơng tự thì Robot sẽ di chuyển theo con ngƣời và giữ khoảng cách với con ngƣời.
Hình 2.4: Chương trình mô phỏng cho hệ thống
Một cơ sở lý thuyết nền tảng cho ứng dụng này là sử dụng thuật toán transformations (tf). /tf sẽ liên tục theo dõi sự dịch chuyển của các khung hình để tính toán đƣợc toạ độ thay đổi của vật thể thứ hai so với vật thể thứ nhất hay nói cách khác là tính đƣợc toạ độ vector trong hệ toạ độ khi con rùa thứ hai di chuyển theo con rùa thứ nhất từ đó xác định vị trí của con rùa thứ nhất để thay đổi góc quay cũng nhƣ vận tốc di chuyển để đảm bảo con rùa thứ hai liên tục di chuyển theo con rùa thứ nhất.
14
Hình 2.5: Con rùa thứ hai di chuyển theo con rùa thứ nhất.
Khi con rùa thứ nhất di chuyển thì con rùa thứ hai sẽ di chuyển theo bằng việc thay đổi góc quay và vận tốc nhƣng đảm bảo giữ khoảng cách nhất định với con rùa thứ nhất.
Theo hình 2.5 ta thấy nếu ta muốn con rùa thứ hai di chuyển theo con rùa thứ nhất thì ta phải sử dụng thuật toán /tf. Mà nếu ta chỉ tính /tf (O2O1) thì con rùa thứ hai sẽ di chuyển và nằm chồng lên con rùa thứ nhất do đó ta phải tính thêm /tf(O1O’2) để con rùa thứ hai di chuyển và luôn cách con rùa thứ nhất một khoảng D. Khi không còn nhấn các phím điều chỉnh thì con rùa thứ nhất dừng lại và con rùa thứ hai cũng dừng lại và nằm ở vị trí đúng với khoảng cách yêu cầu.
𝑂2 𝑋2 𝑌2 𝑌1 𝑋1 𝑋′2 𝑌′2 𝑂′2 𝑂1 /tf(𝑂1, 𝑂′2) /tf(𝑂2, 𝑂1) Ө
15 x y O (0,0) Hình 2.6: /tf trên ROS
Từ hình 2.6 ta có, có giả sử con rùa thứ nhất (turtle1) có tọa độ trên trục tọa độ Oxy là (𝑿𝒕𝒖𝒓𝒕𝒍𝒆𝟐,𝒀𝒕𝒖𝒓𝒕𝒍𝒆𝟐).Ta có thể suy ra đƣợc 𝒕𝒇(𝒕𝒖𝒓𝒕𝒍𝒆𝟐)là (xturtle2 – 0, yturtle2 - 0) và viết gọn là (xturtle2,yturtle2) và cũng chính là tọa độ của turtle2. Nhƣng khi vật di chuyển tới vị trí turtle1 ta sẽ có tfturtle2,turtle1 sẽ là (xturtle1 – xturtle2 , yturtle1– yturtle2) và ngƣợc lại nếu ta tính tfturle1,turtle2 sẽ là (xturtle2 – xturtle1, yturtle2 – yturtle1).
Camera Kinect đƣợc đặt trên Robot để theo dõi vị trí con ngƣời và hiển nhiên khi sử dụng /tf để tính toán toạ độ thay đổi và toạ độ này đƣợc lấy trên hệ trục toạ độ của Camera Kinect nhƣng thực tế hệ toạ độ này lại không giống với hệ toạ độ thực. Và để tính toán kết quả chính xác theo hệ toạ độ thực thì chúng ta phải quy đổi hệ trục toạ độ này. Ta giả sử hệ trục tọa độ của kinect là Oxkykzk và hệ trục tọa độ thực là Oxwywzw nhƣ hình bên dƣới: Turtle1 (𝑿𝒕𝒖𝒓𝒕𝒍𝒆𝟏,𝒀𝒕𝒖𝒓𝒕𝒍𝒆𝟏) 𝒕𝒇(𝒕𝒖𝒓𝒕𝒍𝒆𝟐,𝒕𝒖𝒓𝒕𝒍𝒆𝟏) Turtle2 (𝑿𝒕𝒖𝒓𝒕𝒍𝒆𝟐,𝒀𝒕𝒖𝒓𝒕𝒍𝒆𝟐) 𝒕𝒇(𝒕𝒖𝒓𝒕𝒍𝒆𝟐) (𝑿𝒕𝒖𝒓𝒕𝒍𝒆𝟐,𝒀𝒕𝒖𝒓𝒕𝒍𝒆𝟐)
16
Kinect
Hình 2.7: Chuyển đổi hệ trục tọa độ
Từ hình 2.7 ta thấy hệ trục tọa độ của Camera Kinect là (xk, yk, zk) nhƣng khi chuyển đổi qua hệ trục thực sẽ là: xw = -xk, yw = zk, zw = yk. Khi con ngƣời di chuyển Kinect đọc đƣợc toạ độ của đầu con ngƣời là (xk,yk,zk) thì khi chuyển qua hệ trục tọa độ thực sẽ là (-xk,zk,yk). Vậy để điều khiển Robot thì chúng ta sẽ sử dụng toạ để thực để tính toán. 2.3.2. Mô hình động lực học. Hình 2.8: Mô hình động lực học (𝑿𝒌,𝒀𝒌,𝒁𝒌) 𝑿𝒌 𝒁 𝒌 𝒀𝒌 𝒀𝒘 𝑿𝒘 𝒁𝒘
17 Giả sử robot chuyển động không trƣợt với vận tốc tịnh tiến là v và vận tốc góc ω không thay đổi trong hệ tọa độ Oxy trạng thái của xe đƣợc biểu diễn qua tọa độ tổng quát sau q = (x, y, θ) ở đây (x,y) là vị trí c và θ là góc chuyển động so với trục của bánh xe ta có phƣơng trình (2.1): 𝑥 = 𝑅2 ν𝑟 + ν𝑙 𝑐𝑜𝑠θ 𝑦 = 𝑅 2 ν𝑟 + ν𝑙 𝑠𝑖𝑛θ θ = 𝑅 𝐿 ν𝑟 − ν𝑙 (2.1)
Với: ν𝑟: vận tốc dài quanh trục bánh xe phải. ν𝑙: vận tốc dài quanh trục bánh xe trái.
L: khoảng cách giữa hai bánh xe. R: bán kính của bánh xe.
Nếu chỉ xét trƣờng hợp tính toán của bánh xe ngõ vào là v vàω ta có phƣơng trình: 𝑥 = 𝑣𝑐𝑜𝑠θ
𝑦 = 𝑣𝑠𝑖𝑛θ 𝜃 = 𝜔
(2.2) Với 𝜔 là vận tốc góc.
Kết hợp công thức 2.1 và 2.2 trên ta tính đƣợc ν𝑟 và ν𝑙 cụ thể nhƣ công thức 2.3: 𝑣𝑟 = 2𝑣 + 𝜔𝐿2𝑅
𝑣𝑙 = 2𝑣 − 𝜔𝐿2𝑅 (2.3)
Từ công thức 2.3 để tính đƣợc vr và vl ta cần tính đƣợc 𝜔 mà để tính 𝜔 chúng ta phải phân tích hình 2.9.
18
D
D
Hình 2.9: Tọa độ Robot xoay và chạy theo người
Từ hình 2.9 ta thấy: ϴD = arctan(𝑌21 − 𝑌11
𝑋21 − 𝑋11 ). (2.4) Mà từ công thức 2.2 ta có 𝜃 = 𝜔
Để Robot luôn quay theo ngƣời thì 𝜔 luôn là một số cố định. Vì vậy áp dụng thuật toán PID cho 𝜔 ta đƣợc công thức 2.5:
𝜔 = 𝐾𝑃𝑒 + 𝐾𝐼 𝑒𝑑𝜏 + 𝐾𝐷𝑒 (2.5) = PID(e)
Trong đó: e = ϴD - ϴ0 (ϴ0 là góc ban đầu)
Từ các công thức đƣợc trình bày ở trên ta có đƣợc một sơ đồ các node trong hệ thống nhƣ hình 2.10. 𝐎𝟎 𝐗𝟎 𝐘𝟎 𝐎𝟏𝟎 𝐎𝟏𝟏 𝐗𝟏𝟎 𝐗𝟏𝟏 𝐘𝟏𝟎 𝐘𝟏𝟏 (𝑿𝟐𝟎,𝒀𝟐𝟎) (𝑿𝟐𝟏,𝒀𝟐𝟏)
19 Node
kinect Node PID
Node Tracking Node rosserial Node PID Rosserial Motor right Motor left ROS ARDUINO /(x,y) /µ /V_l,V_r /v_l,v_r /PWM_r /PWM_l
Hình 2.10: Sơ đồ các node trong hệ thống
Trong mô hình trên ta sử dụng hai bộ PID. Bộ thứ nhất nằm trong board Arduino nhằm cân chỉnh tốc độ của M1 và M2 luôn giữ ở tốc độ cố định nhƣ mong muốn, giúp Robot chuyển động tịnh tiến đƣợc chính xác và đúng hƣớng mong muốn. Bộ thứ hai nằm trong ROS, bộ này giúp cân chỉnh gốc quay của Robot, luôn giữ góc ở một giá trị nhất định so với mục tiêu trong trƣờng hợp này góc đó bằng 0 vì vậy mà Robot luôn luôn hƣớng về mục tiêu.
2.4. Thuật toán PID
Để điều khiển ngõ ra của thiết bị theo mong muốn và ổn định ví dụ nhƣ điều khiển động cơ, nhiệt độ, áp suất, tốc độ hay các biến khác trong các ứng dụng khác nhau chúng ta cần một thuật toán để điều khiển. Thuật toán PID là một trong những thuật toánphổ biến đƣợc giới thiệu để giải quyết các bài toán về điều khiển. Bộ điều khiển PID có thể sử dụng để điều khiển bất kỳ ngõ ra nào miễn sao giá trị của chúng bị tác động thông qua thuật toán PID.
PID đƣợc viết tắt của các từ Propotional (tỉ lệ), Integral (tích phân), Derivative (đạo hàm) và là giải thuật điều khiển đƣợc dùng nhiều nhất trong các ứng dụng điều khiển tự động với yêu cầu chính xác (accurate), nhanh (fast response), ổn định (small overshot).
/(x,y) head
20 Bộ điều khiển PID sẽ giám sát đầu ra của hệ thống, sau đó so sánh với giá trị mong muốn. Nếu hệ thống chƣa đạt đƣợc giá trị mong muốn, nghĩa là sai số e khác không, bộ PID sẽ tính toán ra giá trị tác động u mới (tác động vào hệ thống) để hiệu chỉnh hệ thống. Quá trình này sẽ đƣợc thực hiện liên tục cho đến khi nào e = 0, lúc đó hệ thống sẽ đạt trạng thái ổn định. Hệ thống hoạt động theo nguyên lí này còn đƣợc gọi là hệ thống điều khiển vòng kín (Closed Loop Control) vì có hồi tiếp trạng thái của hệ thống về bộ điều khiển nhƣ hình 2.11.
Hình 2.11:Mô hình bộ điều khiển PID4.
Các thành phần của bộ điều khiển PID
- Thành phần tỉ lệ (Kp) có tác dụng làm tăng tốc độ đáp ứng của hệ và làm giảm (chứ không triệt tiêu) sai số xác lập của hệ (steady – state error).
- Thành phần tích phân (KI) có tác dụng triệt tiêu sai số xác lập nhƣng có thể làm giảm tốc độ đáp ứng của hệ.
- Thành phần vi phân (KD) làm tăng độ ổn định của hệ thống, giảm độ vọt lố và cải thiện tốc độ đáp ứng của hệ thống.
Hệ PID có thể sử dụng ở ba chế độ P, PI, PD tùy vào đặc tính của hệ thống.
- Hiệu chỉnh tỉ lệ P: 𝐊𝐩càng lớn thì sai số xác lập càng nhỏ, vọt lố cao, mặc dù vậy ta không thể tăng 𝐊𝐩đến vô cùng khi 𝐞𝐱𝐥không bằng không, 𝐊𝐩≤ 𝐊𝐠𝐡.
- Hiệu chỉnh tỉ lệ PI: Hiệu chỉnh tỉ lệ PI làm chậm đáp ứng của hệ thống, tăng độ vọt lố, giảm sai số xác lập.
- Hiệu chỉnh tỉ lệ PD: Hiệu chỉnh PD đẩy nhanh đáp ứng của hệ thống, giảm thời gian quá độ nhƣng lại nhạy với nhiễu tần số cao. - Hiệu chỉnh tỉ lệ PID: Cải thiện đáp ứng quá độ (giảm độ vọt lố,
giảm thời gian quá độ), giảm sai số xác lập.
21
Bảng 2.1: Tác động của việc thay đổi một trong các thông số Kp, KI, KD độc lập.
Thông số
Thời gian
khởi động Quá độ Thời gian xác lập
Sai số ổn định
Độ ổn định
Kp Giảm Tăng Thay đổi
nhỏ Giảm Giảm cấp
KI Giảm Tăng Tăng Giảm đáng
kể Giảm cấp
KD Giảm ít Tăng Giảm ít
Về lý thuyết không tác động Cải thiện nếu KD nhỏ Kết quả của bộ điều khiển thƣờng đƣợc đánh giá thông qua việc đo đạc đáp ứng của bộ điều khiển trong một hệ thống nhất định nhƣ hình 2.12.
Hình 2.12: Đáp ứng của các hệ thống điều khiển.5
Từ hình 2.12 ta thấy đáp ứng của hệ thống điều khiển theo sự thay đổi và đáp ứng của ba phƣơng pháp điều khiển P, PI, PID. Trong đó đáp ứng của bộ PID là tốt nhất với độ vọt lố ít và thời gian ổn định nhanh.
5Nguồn https://vi.wikipedia.org/wiki/PID.
22 Bộ PID có thể làm việc trên dữ liệu liên tục hoặc rời rạc. Hiện nay, PID rời rạc đƣợc ứng dụng rộng rãi trong các hệ điều khiển do hầu hết các hệ thống hiện nay đều là hệ thống số. Công thức bộ PID có thể đƣợc mô tả ở công thức số (2.6):
u(n) = 𝐊𝐩e(n) + 𝐊𝐢 𝒏 𝒆(𝒌)
𝒌=𝟎 + 𝐊𝐝(y(n) – y(n – 1)) (2.6) Cách tính toán của bộ PID nhƣ hình 2.13.
Hình 2.13: Cách tính toán PID6.
Bộ điều khiển PID có hàm truyền dạng liên tục trong công thức số (2.7):
G(s) = 𝐊𝐩 + 𝐊𝐈
𝑺+ 𝐊𝐃.s(2.7)
Biến đổi Z công thức số (2.7) ta đƣợc công thức số (2.3):
G(z) = 𝐊𝐩 + 𝐊𝐈 .𝐓
𝑺 (𝐳 + 𝟏
𝒛 − 𝟏) + 𝐊𝐃
𝑻(𝐳 − 𝟏
𝒛 ) (2.8) Viết lại G(z) ta đƣợc công thức số (2.9):
G(z) =(𝐊𝐩 + 𝐊𝐈.𝐓𝟐 + 𝐊𝐃𝑻)+(−𝐊𝐩 + 𝐊𝐈.𝐓𝟐 – 𝟐.𝐊𝐃𝑻).𝒛−𝟏+𝐊𝐃𝑻.𝒛−𝟐 𝟏 − 𝒛−𝟏 (2.9) Đặt: 𝐚𝟎 = 𝐊𝐩 + 𝐊𝐈.𝐓𝟐 + 𝐊𝐃 𝑻 𝐚𝟏 = −𝐊𝐩 + 𝐊𝐈.𝐓 𝟐 – 𝟐.𝐊𝐃 𝑻 𝐚𝟐 = 𝐊𝐃 𝑻 Suy ra (2.10): G(z) = 𝐚𝟎 + 𝐚𝟏.𝒛 −𝟏+𝐚𝟐.𝒛−𝟐 𝟏 − 𝒛−𝟏 (2.10)
Từ đó, ta tính đƣợc tín hiệu điều khiển u(k) khi tín hiệu vào e(k) nhƣ công thức (2.11): u(k) = G(z).e(k) = 𝐚𝟎 + 𝐚𝟏.𝒛 −𝟏+𝐚𝟐.𝒛−𝟐 𝟏 − 𝒛−𝟏 .e(k) (2.11) 6Nguồn https://vi.wikipedia.org/wiki/PID
23 Suy ra (2.12):
u(k) = u(k – 1) +𝐚𝟎.e(k) + 𝐚𝟏.e(k – 1) + 𝐚𝟐.e(k – 2) (2.12) Trong đó: T là thời gian lấy mẫu
e(k) là sai số hiện tại: e(k) = giá trị đặt – giá trị ngõ ra e(k – 1) và e(k – 2) là các sai số tích lũy
u(k) là ngõ ra bộ điều khiển
𝐊𝐩,𝐊𝐈,𝐊𝐃là các thông số P, I, D của bộ điều khiển Cách điều chỉnh các hệ số𝐊𝐩,𝐊𝐈, 𝐊𝐃:
Có rất nhiều cách cân chỉnh hệ số Kp, KI, KD cho phù hợp ví dụ cân chỉnh bằng tay, bằng phần mềm… và bên dƣới là phƣơng pháp cân chỉnh Ziegler – Nichols.
Đầu tiên, ta chỉnh cho 𝐊𝐩 = 1 và𝐊𝐃, 𝐊𝐈 = 0 nghĩa là chỉ điều khiển P. Sau đó, tăng 𝐊𝐩lên dần dần và quan sát động cơ. Khi thấy động cơ dao động (nghĩa là nó lúc nhanh lúc chậm). Đặt Kp tại đó là Kc,đo chu kì giao động PC và tính các thông số Kp, KI, KD theo bảng 2.2.
Bảng 2.2 : Thông số điều chỉnh Kp, Ki, Kd. Ziegler – Nichols Method
Control Type Kp Ki Kd
P 0.5KC - -
PI 0.45KC 1.2KP / PC -
24
Chƣơng 3
TÍNH TOÁN VÀ THIẾT KẾ
3.1. Sơ đồ khối hệ thống
Một phần rất quan trọng và là yếu tố quyết định cho việc đề tài hoàn thành và mô hình chạy ổn định là phải nghiên cứu và tính toán để chọn ra đƣợc những thiết bị để từ đó thiết kế ra đƣợc mô hình thực hiện đúng yêu cầu của đề tài. Trong đề tài “Lập trình Robot 3 bánh di chuyển theo con ngƣời sử dụng cảm biến Kinect” yêu cầu là phải thi công đƣợc một Robot có chức năng di chuyển theo sự chuyển động của con ngƣời. Từ yêu cầu này, việc tính toán thiết kế làm sao để chọn đƣợc một thiết bị theo dõi và đọc đƣợc toạ độ của con ngƣời, một thiết bị để xử lý toàn bộ hoạt động của hệ thống, khối thiết bị kết nối để thực thi hoạt động của Robot và phần quan trọng là những thiết bị cấu hình nên khung Robot.
Vậy cụ thể trong đề tài này việc tính toán và thiết kế bao gồm các phần nhƣ sau: - Thiết kế sơ đồ khối cho hệ thống.
- Tính toán thiết kế hệ thống. Khối điều khiển. Camera Kinect. Khối chấp hành. Khối cảm biến. Khối công suất. Khối nguồn. - Sơ đồ kết nối toàn hệ thống.
25 MÁY TÍNH KHỐI NGUỒN KHỐI ĐIỀU KHIỂN KHỐI CÔNG SUẤT 2 KHỐI CHẤP HÀNH 2 CẢM BIẾN KINECT ENCODER KHỐI CÔNG SUẤT 1 KHỐI CHẤP HÀNH 1 ENCODER Hình 3.1: Sơ đồ khối hệ thống
Chức năng của từng khối:
- Khối nguồn: cung cấp nguồn điện cho các khối điều khiển, khối công suất, khối chấp hành, encoder và camera hoạt động.
- Khối điều khiển: có nhiệm vụ giao tiếp truyền nhận dữ liệu với máy tính thông qua hệ điều hành ROS để điều khiển khối công suất.
- Khối công suất: có nhiệm vụ nhận tín hiệu điều khiển từ khối điều khiển để thực hiện khuếch đại công suất ngõ ra cho phù hợp để điều khiển động cơ. - Khối chấp hành: thực hiện chuyển động của Robot.
- Encoder: đọc giá trị xung từ khối chấp hành và phản hồi về cho khối điều khiển.
- Camera: nhận biết vị trí con ngƣời để truyền dữ liệu về máy tính xử lý.
- Máy tính: là trung tâm xử lý các tín hiệu để thực hiện điều khiển chung cho cả hệ thống.
Hoạt động của hệ thống: Ở đây máy tính đóng vai trò làm trung tâm điều khiển và giám sát cho cả hệ thống, camera Kinect thu thập dữ liệu vị trí toạ độ của con ngƣời rồi truyền về cho máy tính, thông qua hệ điều hành ROS máy tính sẽ xử lý tín hiệu đó rồi tính toán đƣa ra đƣợc giá trị vận tốc để truyền xuống khối điều khiển, sau khi nhận và xử lý tín hiệu này khối điều khiển sẽ xuất tín hiệu ngõ ra truyền cho khối
26 công suất để thực hiện việc điều khiển khối chấp hành đúng theo yêu cầu đặt ra, còn phần cảm biến Encoder sẽ nhận dữ liệu khối chấp hành trả về khối điều khiển để so sánh giá trị hiện tại với giá trị đặt ra để điều chỉnh cho hợp lí. Sau đó, khối điều khiển sẽ truyền tín hiệu về máy tính để thực hiện giám sát dữ liệu. Và hoạt động này của hệ thống sẽ diễn ra liên tục.
3.2. Tính toán và thiết kế hệ thống
3.2.1. Khối điều khiển – Arduino DUE
Hình 3.2: Mặt trước và mặt sau board Arduino DUE
Arduino Due là board phát triển sử dụng nhân ARM đầu tiên của series board arduino, sử dụng CPU Atmel SAM3X8E ARM Cortex-M3 và đƣợc nạp bootloader