Chương 2: Lựa chọn phương án Arduino Nano Driver DC L298 Trụ đồng đực cái Trục động cơ Cảm biến hồng ngoại Mica 1 1 2 7 cặp
CHƯƠNG 4: MÔ HÌNH TOÁN HỌC 4.1 Mô hình toán học của xe
Vì xe chỉ chạy trên địa hình bằng phẳng, tải không đổi, không có ngoại lực khác tác dụng trong quá trình di chuyển của xe nên chỉ cần phân tích động học.
Vị trí và hướng của xe trong mặt phẳng được đặt trưng bởi ba thông số (x , y,ϕ ). Với x , y ) là tọa độ của xe tuyệt đối trong hệ tọa độ quán tính vàϕ là góc hợp bởi một vector gắn liền với xe và một vector cố định trong hệ tọa độ cố định. Ở trường hợp robot dò line,
(x , y,ϕ ) được chọn như Hình 4.1. y yT yC X Y φ C(xC,yC ) O xC xT x Hình 4.1 Mô hình động học của xe
Gọi và lần lượt là vận tốc dài và vận tốc góc của xe tại điểm trọng tâm C của xe. Phương trình động học của hệ:
[ ]=[ Vận tốc góc của các bánh xe: [ Trong đó: : tốc độ góc bánh phải : tốc độ góc bánh trái : bán kính bánh xe
: ½ khoảng cách giữa 2 tâm bánh xe Tọa độ tracking point:
= +
= +
(4.1)
Trong đó :
: Khoảng cách từ tâm đến điểm tracking point T
Để điều khiển xe bám theo lineR (xr , yr ,ϕr ) cần điều khiển sao cho trọng tâm của xe chuyển động dọc theo quỹ đạo đường line mong muốn và hướng của xe tiếp tuyến với đường line.
Xe di chuyển trên đường tham chiếu thỏa mãn
. x r . y r . vr cosϕr =vr sin ϕr (4.3) ωr
Hình 4.2 Mô hình sai số của xe bám theo line Để đạt được mục tiêu đó, ta cần 3 sai số đặc trưng cho độ sai lệch của trọng tâm C so với R là e = [e1 e2 e3]T Bằng các quan hệ hình học ta xác định được: e1 e e Đạo hàm phương trình (4.3) ta đươc:
4.2 Hàm truyền của động cơ – Driver
Định nghĩa hàm truyền: Hàm truyền của hệ thống là tỉ số giữa biến đổi Laplace của tín hiệu ra và biến đổi Laplace của tín hiệu vào khi điều kiện đầu bằng 0.
Đối với động cơ, chọn tín hiệu ra là tốc độ quay của động cơ (vòng/phút), tín hiệu vào là %PWM.
Như vậy để tìm hàm truyền động cơ ta cấp điện áp cho động cơ với dạng tín hiệu biết trước rồi tiến hành đo tốc độ quay của động cơ.
Đầu tiên ta phải xác định quan hệ giữa tín hiệu ra và tín hiệu vào là tuyến tính hay phi tuyến. Để xác định ta cho nhiều giá trị PWM rồi tìm đáp ứng của tốc độ động cơ khi xác lập. Từ các mẫu thu thập được rồi tiến hành vẽ đồ thị quan hệ giữa PWM và tốc độ.
Ở giai đoạn tìm quan hệ giữa tín hiệu ra và tín hiệu vào là tuyến tính hay phi tuyến cần chọn thời gian lấy mẫu đủ để tốc độ động cơ xác lập. Qua thực nghiệm cho thấy thời gian lấy mẫu 1s đủ để tốc độ động cơ đạt xác lập.
Đề tìm quan hệ vào- ra của động cơ số mẫu tùy chọn. Chọn số mẫu cần lấy là 22 mẫu để tìm quan hệ giữa tốc độ động cơ và % PWM cho động cơ trái và phải.
4.2.1 Động cơ trái
Tìm quan hệ vào- ra
Tiến hành lấy mẫu ta được bảng số liệu sau:
Bảng 4.1 Tốc độ động cơ theo % PWM STT 1 2 3 4 5 6
7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 Hình 4.3 Đồ thị %PWM - tốc độ 35
Từ Hình 4.3 ta thấy tốc độ quay của động cơ là hàm tuyến tính theo giá trị PWM.
Tìm hàm truyền
Tốc độ lớn nhất của động cơ khi không tải: vmax = 328 vòng/phút. Số xung Encoder: n = 374 xung/vòng.
=> Tần số lớn nhất để đọc encoder: =2 60 = 4090 Hz (nhân 2 khi đọc bằng 2 kênh)
=> Chu kì lấy mẫu nhỏ nhất: Tmin = 1/fmax = 244,5 us Chọn tín hiệu vào là giá trị PWM theo hàm số sau:
20% , 0 ≤ < 0.4
40%, 0.4 ≤ < 0.8
PWM (%) = 60%, 0.8 ≤ < 1.2 T = 2 s
80%, 1.2 ≤ < 1.6 { 100% ,1.6 ≤ < 2
Hình 4.4 Đồ thị tín hiệu PWM cấp cho động cơ
Lấy mẫu trong 1 chu kì của tín hiệu vào là 2 s => f = 1 = 0.5 .
Phân tích tín hiệu vào thành chuỗi Fourier có tần số các họa tần là fk = kf =0,5k Phân tích Fourier tín hiệu vào tới họa tần thứ k = 50.
Tần số cắt cao nhất của tín hiệu vào là fc= 25 Hz
Để khôi phục được tín hiệu thì tần số lấy mẫu fs ≥ 2fc = 50 Hz Chọn fs = 50 Hz => Ts = 0.02 s
Số mẫu cần lấy là: n = => = 02
.02 = 100 Chọn m = 1 => n = 100
mẫu.
Hình 4.5 Đáp ứng của tốc độ động cơ trái Hàm truyền động cơ trái- driver có dạng:H ( s) = s K+a
Sử dụng công cụ System Identification của Matlab để tìm hàm truyền => Hàm truyền tìm được là:H ( s) = 76.47
s + 23.6
4.2.2 Động cơ phải
Tìm quan hệ vào- ra
Tiến hành lấy mẫu ta được bảng số liệu sau:
Bảng 4.2 Tốc độ động cơ theo % PWM STT 1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18
Hình 4.6 Đồ thị %PWM - tốc độ Từ đồ thị ta thấy quan hệ giữa tốc độ góc và PWM là tuyến tính.
Tìm hàm truyền
Tương tự như động cơ trái, có các kết quả sau:
Hình 4.7 Đáp ứng của động cơ phải Hàm truyền động cơ phải- driver có dạng:H ( s) = s K+a
Hàm truyền tìm được là:H ( s) =
4.3 Hàm truyền cảm biến IR
Sử dụng cảm biển TCRT5000 để phát hiện line:
Hình 4.9 Cảm biến TCRT5000
Hình 4.10 Vùng hoạt động của cảm biến
Theo như datasheet của cảm biến TCRT5000, góc phát của Emiter là = 160, góc thu là = 300 . Khoảng cách giữa hai cảm biến = 3,5 . Từ đó ta có thể xác định được giá trị của
1, 2 .
1= 2ℎ. tan(160) ,2 = 2ℎ. tan(300) ,=1+ 22 −
Khi sử dụng module dò line tích hợp sẵn cảm biến TCRT5000, vùng hoạt động của cảm biến nằm trong khoảng 1 – 25 mm.
Chọn h = 15mm ta tính được:
= 2.15. tan(160) = 8,60 1
2= 2.15. tan(300) = 17,32
Để cảm biến không bị nhiễu bởi các cảm biến khác, khoảng cách giữa 2 cảm biến phải lớn hơn hoặc bằng một khoảng K = d + (d1 + d2)/2 =3,5 + (8,60 + 17,32)/2 = 16.46 mm
Hình 4.11 Điều kiện để cảm biến không bị nhiễu bởi cảm biến khác Chọn khoảng cách giữa 2 cảm biến là 18mm
a. Hiệu chuẩn cảm biến (Calibration)
Để có thể nhận ra giao lộ giữa 3 đường, ta sử dụng 7 module dò line đơn. Mỗi module dò line lại trả về tín hiệu analog khác nhau. Vì vậy để có thể sử dụng các tín hiệu này hiệu quả, việc hiệu chuẩn cảm biến là vô cùng cần thiết.
Với:
xmax,i và xmin,i là giá trị lớn nhất và nhỏ nhất đọc được từ cảm biến thứ I,
ymax và ymin là giá trị lớn nhất và nhỏ nhất mà ta mong muốn cho tất cả các cảm biến
xj,i = là giá trị đọc được thứ j của cảm biến thứ i
yjo = là giá trị sau khi điều chỉnh xj,i. Trước khi hiệu chuẩn
Trung bình cộng
Từ 2 giá trị trung bình cộng. Ta chọn ymax = 637, ymin = 57 Sau khi hiệu chuẩn số liệu thu thập phía trên, ta được
Cảm biến 2 Cảm biến 3 Cảm biến 4 Cảm biến 5 Cảm biến 6 Cảm biến 7
b. Xác định khoảng cách tới tâm đường line
Từ các giá trị analog của cảm biến đã qua hiệu chuẩn, ta sử dụng phương pháp trung bình trọng số để xác định vị trí của xe so với đường line.
Hình 4.12 Vị trí cảm biến ứng với trọng số của cảm biến
Theo phương pháp trung bình trọng số, tọa độ tâm đường line dọc theo các cảm biến được xác định bằng công thức: ∑ −1 =0 = . ∑ =0−1 44
Với n là số cảm biến sử dụng và L là hệ số khuếch đại. Số liệu x tìm được ta sẽ đi khảo sát so sánh với khoảng cách thật từ tâm đường line để tìm hàm truyền cảm biến
Tính số mấu cần lấy:
Khoảng cách dịch chuyển 1 lần: 6mm/lần
Thời gian giữa 2 lần di chuyển cảm biến = 1s
Tổng khoảng cách dịch chuyển = 18*6 = 108 mm Tổng thời gian : T= 108/6= 18 s => f = 1/18 Hz Tần số lấy mẫu fs >= 2*f = 2/18= 1/9 Hz N*dt = mT Chọn fs = 1 Hz => dt = 1s => N = m*18/1= 18m Chọn m = 1.
Vậy số mẫu cần lấy là 18 mẫu ứng với fs = 1 Hz
Lấy 23 mẫu ta xấp xỉ được mối quan hệ và giữa trung bình trọng số và sai số thực là y = 1.5135x + 5.5558 với y là sai số thực, x là trung bình trọng số
Mối quan hệ trung bình trọng số và độ lệch thực thựclệch -40 Đ ộ 80 y = 1.6211x + 1.4486 60 40 20 0 -30 -20 -40 -60 -80 Trung bình trọng số Hình 4.13 Hàm xấp xỉ trung bình trọng số và sai số thực Bảng số liệu ứng với đồ thị:
Bảng 4.3 Dữ liệu calib cảm biến
Trung bình trọng số -29.5 -30.2 -30.8 -33.28 -27 -26 -21.2 -20.14 -10.6
-10.42 -7.17 -5.9 -0.11 0.1 5.2 9.8 10.5 18.4 19.4 20.27 21.1 32.45 31.1
CHƯƠNG 5: THIẾT KẾ BỘ ĐIỀU KHIỂN
5.1 Yêu cầu điều khiển
Thiết kế bộ điều khiển thỏa mãn các yêu cầu sau:
Bám line tốt, trọng tâm xe lệch tâm đường line < 10 mm
Thời gian hoàn thành đường line < 15 s
Vận tốc xe đoạn chạy thẳng đạt tốc độ tối đa vmax = 1 m/s
5.2 Tín hiệu input dùng để điều khiển
Cảm biến hồng ngoại trả về giá trị analog 1024 mức. Tìm hàm truyền cảm biến để tìm mối quan hệ giữa giá trị analog với khoảng cách giữa tâm cảm biến và tâm đường line.
Theo kết quả hàm truyền cảm biến tìm được ở mục 4.3 ta được: y = 1.5135x + 5.5558 Với: y là khoảng cách giữa tâm cảm biến và tâm đường line.
x là giá trị trung bình trọng số của 7 cảm biến.
5.3 Giải thuật điều khiển
Giải thuật điều khiển của robot dò line gồm ba nội dung chính ứng với ba chương trình con:
Hiệu chuẩn giá trị cảm biến
Đọc giao lộ
Bám line
Chương trình con hiệu chuẩn giá trị cảm biến:
Hình 5.2 Chương trình con hiệu chuẩn giá trị cảm biến
Chương trình con đọc giao lộ:
Hình 5.3 Chương trình con đọc giao lộ Chương trình con bám line:
5.4 Bộ điều khiển
Sau khi mô hình hóa hệ thống xe robot dò line, nhóm chọn và thiết kế bộ điều khiển PD vì tính đơn giản và hiệu quả của bộ điều khiển này.
Tín hiệu PWM cấp cho động cơ là PWMbase =190/255 = 75.5 %
u = Kperror + Ki∫ + Kd(error – last error)
Trong đó error là sai lệch khoảng cách giữa tâm cảm biến và tâm đường line.
Nếu u < 0 thì xe rẽ trái PWM trái = PWMbase - |u| PWM phải = PWMbase + |u|
Nếu u >0 thì xe rẽ phải PWM trái = PWMbase + |u| PWM phải = PWMbase - |u|
Nếu u =0 thì xe đi thẳng PWM trái = PWMbase
PWM phải = PWMbase
5.5 Kết quả mô phỏng
Sau khi đã thiết kế bộ khiển PD như đã trình bày bên trên, tiến hành lập trình trong phần mềm Matlab và chạy mô phỏng. Kết quả mô phỏng xe dò line được thể hiện như các hình dưới đây:
Hình 5.5 Quỹ đạo xe mô phỏng
Dựa vào đồ thị quỹ đạo trên, xe dò line đã hoàn thành hết đường đua với thời gian nhỏ hơn thời gian quy định. Do quỹ đạo vẽ dựa vào tọa độ của điểm nằm giữa hai bánh xe nên khi xe dừng ở đich đến (cảm biến dò line nhận giao lộ thứ 6), tồn tại một khoảng cách giữa đích và điểm đó.
Từ đồ thị trên, ta có thể nhận thấy được đáp ứng nhanh chóng của hai bánh xe khi xe tiến đến những khúc cua quẹo gấp, đặc biệt là ở điểm A khi xe phải cua gấp một góc 900. Đồ thị vận tốc góc tồn tại những dao động lớn, nhỏ do thuật toán bám line.
Hình 5.7 Đồ thị vận tốc dài của xe
Nhận xét: Vận tốc dài tối đa của xe lên đến > 1.2 m/s, thỏa mãn yêu cầu đặt ra ban đầu. Vận tốc dài ở đoạn đường thẳng (A-C-E) tăng lên mức tối đa để xe nhanh chóng kết thúc đường đua, tối ưu hóa thời gian hoàn thành.
CHƯƠNG 6: THỰC NGHIỆM VÀ PHƯƠNG HƯỚNG PHÁT TRIỂN
6.1 Thực nghiệm
Quỹ đạo của robot trong thực nghiệm có hình dạng tương đồng với hình dạng sa bàn chứng tỏ giải thuật đề ra ban đầu là hợp lý. Tuy nhiên có những đoạn robot không bám chính xác line do bề mặt sàn có những đoạn nhấp nhô cao làm sai lệch giá trị đọc về của cảm biến.
Hình 6.1 Quỹ đạo robot dò line trên thực nghiệm
Đồ thị vận tốc dài robot trong thực nghiệm xuất hiện những nhấp nhô do trong thực tế robot dò line bị ảnh hưởng bởi nhiễu (ánh sáng thay đổi đột ngột, bề mặt line phản xạ khác nhau, ma sát giữa bánh và bề mặt,…). Vận tốc dài tối đa xe có thể đạt được thỏa yêu cầu đã đặt ra (vmax = 1.2 m/s).
Hình 6.2 Vận tốc dài của robot thực nghiệm Vận tốc góc hai bánh trên thực nghiệm so với mô phỏng:
Hình 6.3 Tốc độ 2 bánh của xe thực nghiệm
Nhận xét: Vận tốc góc hai bánh xe cũng như vận tốc dài dao động liên do nguyên nhân là trong thực tế xe luôn lắc để bám line và nhiễu từ môi trường (bề mặt sa bàn nhấp nhô, …) cũng như nhiễu tín hiệu encoder trả về.
6.2 Phương hướng phát triển:
- Phát triển thuật toán tự hiểu chỉnh cảm biến khi thay đổi điều kiện hoạt động (bề mặt/ chất liệu sa bàn, ánh sáng ngoài trời,…).
- Thiết kế mạch đo dung lượng pin còn lại để báo hết pin tự động nhằm tránh sử dụng cạn pin. Ngoài ra, kết hợp với thuật toán điều chỉnh đáp ứng của robot ổn định ứng với dung lượng pin.
- Điều khiển vận tốc trên từng đoạn đường khác nhau để tối ưu thời gian hoàn thành.
Tài liệu tham khảo
[1] Juing-Huei Su, C.-S. L.-H.-H.-Y. (2010). An intelligent line-following robot project for introductory robot courses. World Transactions on Engineering and Technology Education.
[2] Mustafa Engin, D. E. (2012). PATH PLANNING OF LINE FOLLOWER ROBOT.
The 5th European DSP Education and Research Conference.
[3] Trần Q.Cường, Trần T.Phong (2012), Điều khiển robot dò đường sử dụng bộ điều khiển PID kết hợp phương pháp PWM, Bulletin of Science & Technology, Tien Giang University, No.3, ISSN 1859-4530, Nov. 2012
[4] Ebiesuwa O. O, Adekunle Y. A, Akinyemi L. A, Oyerinde O. D. Line Follower Robot Using A Sophisticated Sensor Approach
[5] GADHVI SONAL, PUNIT RANINGA, HARDIK PATEL. (2017).Design and implementation of RGB color line following robot. International Conference on Computing Methodologies and Communication.
[6] https://www.youtube.com/watch?v=POCpNmGhXfM
[7] Deepak Punetha, Neeraj Kumar, Vartika Mehta. Development and Applications of Line Following Robot Based Health Care Management System
[8] https://www.instructables.com/id/Robot-Line-Follower
[9] Pololu Corporation, Pololu AVR Library Command Reference, 2001–2015 https://www.pololu.com/docs/0J18 [10] brooksbots.com/Expressway.html [11] https://www.pololu.com/category/76/3pi-robot-and-accessories [12] http://brooksbots.com/FireBall.html [13] https://ktmt.uit.edu.vn/tin-tuc/tin-noi-bat/899-t-ng-k-t-cu-c-thi-it-car-racing-l-n-th- vi-nam-2017 58