51
Bảng 4.7 : Bảng hàm truyền động cơ và PWM tương ứng
PWM
(đầu vào là các giá trị điện áp thay đổi từ -u đến u) Hàm truyền động cơ G(s) = (1+𝜏 𝐾 1∗𝑠)(1+𝜏2∗𝑠); 75% với 𝐾 = 30, ; 𝜏1 = 0,037536; 𝜏2 = 0,0055367 85% với K= 30; 𝜏1 = 0,037253; 𝜏2 = 0,0066754 100% với K= 30; 𝜏1 = 0,048733; 𝜏2 = 0,0096374 PWM Random {32, 80, 160, 240, 400, 320, 200, 100, 50, 0}; với 𝐾 = 30; 𝜏1= 0,039536; 𝜏2 = 0,0058532
Dạng đồ thị điện áp cấp vào để Estimate và Validate:
52
Hình 4.34 : Cấp PWM 75%
53
Hình 4.36 : Cấp PWM 100%
Hình 4.37 : Độ chính xác của giá trị hàm truyền tìm được và giá trị dùng để Validate với PWM
85%
Thiết kế bộ điều khiển PID cho động cơ
Với các hàm truyền tìm được, nhóm sử dụng hàm truyền
𝐺𝑑𝑐(𝑠)= 30
(1+0,037253𝑠)(1+0,0066754𝑠) để thiết kế bộ điều khiển PID. Với Setpoint = 300, ta có 𝑇𝑔ℎ = 0,1𝑠, 𝐾𝑔ℎ = 0.4. Ta có :
54 Sử dụng PID Tune Toolbox của matlab:
Hình 4.38 : Hệ số kp, ki, kd khi sử dụng PID Tune Toolbox
55
Hình 4.40 : Đáp ứng động cơ với setpoint 200rpm
56
Hình 4.42 :Đáp ứng động cơ với setpoint 300rpm 4.8. Thiết kế board mạch tổng kết nối các thiết bị
Việc thiết kế board này nhằm mục đích lắp ráp kit vi điều khiển với board driver motor, đồng thời đặt các ngõ ra vào phù hợp với kết cấu xe.
4.8.1.Thiết kế board mạch nguồn
57
Hình 4.44 : Mạch nguồn sau khi thi công
4.8.2.Thiết kế board mạch ra chân cho STM32F4
58
59
CHƯƠNG 5 : MÔ HÌNH HÓA
5.1. Mô hình động học
Hình 5.1 : Mô hình động học của robot trong hệ tọa độ Oxy
Gọi R là điểm tham chiếu của robot, M là trung điểm hai động cơ, C là trung điểm của cảm biến và cũng là điểm tracking của robot.
Phương trình động học tại R:
{
𝑥̇𝑅 = 𝑣𝑅𝑐𝑜𝑠𝜑𝑅 𝑦̇𝑅 = 𝑣𝑅𝑠𝑖𝑛𝜑𝑅
𝜑̇𝑅 = 𝜔𝑅
Trong đó: 𝑣𝑅 là vận tốc mong muốn của robot Phương trình động học robot tại M:
[ 𝑥̇𝑀 𝑦̇𝑀 𝜑̇ ] = [ 𝑐𝑜𝑠𝜑 0 𝑠𝑖𝑛𝜑 0 0 1 ] [𝜔𝑣]
Trong đó 𝑣 và 𝜔 là vận tốc dài và vận tốc góc của xe. Phương trình động học của robot tại C:
{
𝑥̇𝐶 = 𝑥̇𝑀 − 𝜑.̇ 𝑑. 𝑠𝑖𝑛𝜑 𝑦̇𝐶 = 𝑦̇𝑀+ 𝜑.̇ 𝑑. 𝑐𝑜𝑠𝜑
𝜑̇𝐸 = 𝜔
60
5.2. Thiết kế luật điều khiển
Đối với bài toán tracking của robot bám line, luật điều khiển để robot có thể bám theo quỹ đạo cho trước. Có nhiều luật điều khiển được sử dụng như luật điều khiển tracking như back stepping dựa vào tiêu chuẩn ổn định Lyapunov hay bộ điều khiển fuzzy,…. Ở đây, nhóm sử dụng dụng bộ điều khiển tracking.
Hình 5.2 : Mô hình sai số xe dò line
Bộ điều khiển được thiết kế cho điểm C bám theo điểm R trên quỹ đạo với vận tốc 𝑣𝑟. Sai số của C so với điểm tham chiếu:
[ 𝑒1 𝑒2 𝑒3] = [ 𝑐𝑜𝑠𝜑 𝑠𝑖𝑛𝜑 0 −𝑠𝑖𝑛𝜑 𝑐𝑜𝑠𝜑 0 0 0 1 ] [ 𝑥𝑅− 𝑥𝐶 𝑦𝑅− 𝑦𝐶 𝜑𝑅− 𝜑𝐶 ]
Sai số động học có thể được viết:
[ 𝑒̇1 𝑒̇2 𝑒̇3 ] = [ 𝑣𝑅𝑐𝑜𝑠𝑒3 𝑣𝑅𝑠𝑖𝑛𝑒3 𝜔𝑅 ] − [ −1 𝑒2 0 −𝑑 − 𝑒1 0 −1 ] [𝑣 𝜔]
Do hệ thống phototransitor chỉ xác định được sai số theo phương pháp tuyến với phương chuyển động của xe. Nên ta chọn điểm tracking là C trung điểm sensor. Giải sử xe chạy với vận tốc bằng vận tốc tham chiếu, vì vậy 𝑒1 = 0. Tuy nhiên, ta chỉ xác định được một sai số là 𝑒2 nên ta sẽ chọn lại hàm Lyapunov cho bộ điều khiển.
𝑉 =1 2𝑒1 2+1 2𝑒2 2+1 − 𝑐𝑜𝑠𝑒3 𝑘3 ≥ 0 Đạo hàm ta được:
61
𝑉̇ = 𝑒1(𝑣𝑅𝑐𝑜𝑠𝑒3− 𝑣) + 1
𝑘2𝑠𝑖𝑛𝑒3(𝜔𝑅− 𝜔 + 𝑘2𝑒2𝑣𝑅) − 𝑑𝑒2𝜔
Để hệ ổn định thì 𝑉̇ < 0
Mà ta không thể xác định được 𝑒1 và 𝑒3 nên cho 𝑒1 = 0, 𝑒3 ≈ 0. Từ đó ta có:
𝑉̇ =−𝑑𝑒2𝜔 < 0 Chọn:
𝑑. 𝜔 = 𝑘2. 𝑒2
Với 𝑘2 > 0
Từ đó: 𝑉̇ = −𝑘2. 𝑒22 < 0 thỏa điều kiện.
Để xe chạy với vận tốc lớn nhất ta chọn 𝑣= 𝑣𝑅 và 𝜔 = 𝑘2𝑒2/𝑑
Tiến hành mô phỏng quá trình bám sa bàn của robot, sử dụng các thông số đầu vào:
Bảng 5.1 : Các thông số đầu vào cần thiết cho robot khi mô phỏng :
Đại lượng Giá trị Đơn vị
Bán kính cong 𝑅 500 mm Vận tốc cần đạt 𝑣𝑟 0.5 m/s Khoảng cách từ tâm xe đến bánh xe chủ động 𝑏 89 mm Bán kính bánh xe 𝑟 40 mm
Thời gian lấy mẫu động cơ 𝑑𝑡 0.01 s
62
Đồ thị khảo sát khoảng cách 𝑑 và sai số dò line 𝑒2 lớn nhất
Hình 5.3 : Quan hệ giữa khoảng cách 𝒅 và sai số lớn nhất Từ đồ thị trên ta chọn 𝑑 = 100𝑚𝑚
63
5.3. Thiết kế giải thuật điều khiển cho robot
Vị trí ban đầu
Đo sai số e2
Tính vận tốc góc lí thuyết của robot
Bắt đầu
Tính vận tốc hai bánh lí thuyết
Bộ điều khiển PID cho hai động cơ
Vận tốc hai bánh
Vận tốc hai bánh
Tính vận tốc góc của robot
Tính toán vị trí xe
Tới điểm cuối
Kết thúc Tính toán vị trí điểm tracking T_system T_motor S Đ
64
5.4. Mô phỏng robot dò line
Phần này trình bày kết quả mô phỏng robot dò line theo luật điều khiển trên và khoảng cách từ điểm tracking đến tâm xe 𝑑 = 100𝑚𝑚, chọn hệ số 𝑘2 = 8 Phần mềm mô phỏng sử dụng là MATLAB.
65
Hình 5.6 : Sai số bám line 𝒆𝟐
66
Hình 5.8 : Vận tốc góc của robot
Hình 5.9 : Vận tốc góc hai động cơ
Nhận xét:
- Đồ thị sai số bám line có dạng bậc thangsau một khoảng thời gian 𝑡𝑠𝑦𝑠𝑡𝑒𝑚 thì hệ thống mới tính giá trị sai số, từ đó được sử dụng để tìm vận tốc góc của xe.
- Sai số lớn nhất trong quá trình bám line ±32𝑚𝑚
- Trong quá trình chạy đường thẳng sai số lớn nhất là 6𝑚𝑚 sau đó giảm về 0, trên đường cong 24𝑚𝑚 và có giá trị xác lập khi ổn định là 14𝑚𝑚
67
CHƯƠNG 6 : THỰC NGHIỆM VÀ ĐÁNH GIÁ
Hình 6.1 : Mô hình thực tế xe dò line
68
Hình 6.3 : Sai số e2 thực tế thuc được trong quá trình xe bám line
Kết quả thực tế của của xe trên đường là 0.55 m/s đáp ứng được 69% yêu cầu của đề bài đặt ra 0.8 m/s. Thời gian thực hiện hết một vòng chạy là 20s. Xe mô phỏng và chạy thực tế thể hiện dạng bám line giống nhau: trên các đoạn đường thẳng (B→D, F→G), xe thể hiện dao động hai bên đường line; trên các đoạn đường cong (A→B, D→F, G→A), xe đều nằm ở một bên so với đường line. Sai số thực tế của xe lại không thỏa được yêu cầu đề ra về sai số trên đoạn thẳng và cong. Các lý do dẫn đến việc tang sai số này bao gồm:
- Sai số lắp đặt ảnh hưởng đến độ đồng trục của hai động cơ. - Sai số tốc độ của hai động cơ.
- Ảnh hướng của hệ số ma sát của bánh mắt trâu. - Quán tính của xe trong quá trình quẹo cua.
- Sai số của hệ thống cảm biến cảm biến do sự khác biệt giữa môi trường thí nghiệm và thực nghiệm.
- Sai số của hệ thống cảm biến do chiều cao gá đặt và độ song song với mặt phẳng xe. Để khắc phục các sai số này, các giải pháp được đề ra bao gồm:
- Khung xe và đồ gá được chế tạo bằng nhôm, thay cho mica trong mô hình thực nghiệm, để có thể kiểm soát được các sai số trong quá trình gia công và lắp đặt. - Thực hiện thí nghiệm nhằm đánh giá sai số vận tốc của động cơ để có thể đưa vào
mô phỏng.
- Thực nghiệm thí nghiệm để đánh giá ảnh hưởng của môi trường thực nghiệm lên giá trị đọc của hệ thống cảm biến để có thể thực hiện các biện pháp xử lý phù hợp.
69
TÀI LIỆU THAM KHẢO
[1] DAIFUKU, SMARTCART, Automatic Guided Cart Installed Systems
Richard T. Vannoy II, M.S.I.T., B.S.E.E.T. Designing and Building a Line Following Robot.
[2] Andrew Reed Bacha, Line Detection and Lane Following for an Autonomous Mobile Robot, MS diss., Virginia Polytechnic Institute and State University, 2005.
[3] Ramiro Velázquez et. al., A Review of Models and Structures for Wheeled Mobile Robots: Four Case Studies, The 15th International Conference on Advanced Robotics, Estonia, June 20-23, 2011.
[4] G. H. Lee et. al., Line Tracking Control of a Two-Wheeled Mobile Robot Using Visual Feedback,International Journal of Advanced Robotic Systems, DOI: 10.5772/53729, received 4 Apr 2012; Accepted 24 Sep 2012.
[5] Huu Danh Lam et. al., Smooth tracking controller for AGV through junction using
CMU camera,Hội nghị Toàn quốc lần thứ 7 về Cơ điện tử - VCM-2014.
[6] A. H. Ismail et. al.,Vision-based System for Line Following Mobile Robot,IEEE Symposium on Industrial Electronics and Applications (ISIEA 2009), October 4-6, 2009, Kuala Lumpur, Malaysia.
[7] Mustafa Engin, Dilúad Engin, Path Planing of Line Follower Robot,Proceedings of the 5th European DSP Education and Research Conference, 2012.
[8] F. Kaiser et.al., Line Follower Robot: Fabrication and accuracy measurement by data acquisition,International Conference on Electrical Engineering and Information & Communication Technology (ICEEICT) 2014.
[9] Hsin-Hsiung Huang et. al., Hands-on intelligent mobile robot laboratory with support from the industry, IEEE EDUCON Education Engineering 2010 – The future of Global Learing Engineering Education.
[10] Deepak Punetha, et. al. Development and Applications of Line Following Robot Based Health Care Management System. International Journal of Advanced Research in Computer Engineering & Technology (IJARCET) Volume 2, Issue 8, pp.2446-2449, August 2013.
[11] Lý thuyết điều khiển tự động – Nguyễn Thị Phương Hà (chủ biên )
[12] Trịnh Chất & Lê Văn Uyển. Tính Toán Thiết Kế Hệ Dẫn Động Cơ Khí Tập 2, 116- 117. Việt Nam, Công ty In Công Đoàn Việt Nam, 2006
[13] Ninh Đức Tốn, Dung Sai và Lắp Ghép, Việt Nam, Công ty Cổ Phần In Thái Nguyên, 2010
[14] VISHAY, TCRT5000, TCRT5000L Datasheet, Document number: 80112, Rev. 1.1, 02-Jul-09
70
[15] VISHAY, Application of Optical Reflex SensorsTCRT1000, TCRT5000, CNY70,
Document number: 80107, Rev. 1.1, 02-02 [16] LM2586 Datasheet