Thiết bị
Động cơ Cảm biến
ĐỒ ÁN THIẾT KẾ HỆ THỐNG CƠ ĐIỆN TỬ
Driver STM32F411
Vậy nguồn điện cần cung cấp có cơng suất tối thiểu: = 0,6.12 + 0,1422.5 = 15,11
Chọn 4 pin sạc 18650 Panasonic 3,7V( sạc đầy là 4.2V) , dung lượng 1100 mAh- = 2 , sử dụng khay lắp nối tiếp 4 pin để được được điện thế cấp cho mạch nguồn là 16,8 V. Khi đó cơng
suất nguồn tối đa là 33,6W, đủ cho hệ thống.
4.5. Mạch nguồn:
Mạch nguồn có vai trị cung cấp nguồn điện ổn đinh cho driver động cơ (12V) và vi điều khiển (12V), để đảm bảo đủ dòng theo yêu cầu từ bảng 6.1. Mạch ổn áp có chức năng điện áp nhỏ hơn điện áp đầu vào và ln duy trì mức điện áp đầu ra ổn định dù điện áp cấp vào có thay đổi.
Nhóm sử dụng hai mạch giảm áp DC LM2596 có sẵn trên thị trường để biến đổi điện áp từ nguồn cấp về 12V và 5V ổn định.
Hình 4.16 : Module LM2596
Thông số kỹ thuật:
- Điện áp đầu vào: 3-40V DC
- Điện áp đầu ra: điều chinhr trong khoảng từ 1,5-30V
- Dòng đáp ứng tối đa là: 3A
- Hiệu suất: 92%
- Cơng suất 15W
4.6. Mạch driver động cơ
Nhóm lựa chọn driver module TB6612 cho DC servo, với các yêu cầu kỹ thuật đảm bảo cho hệ thống:
Hình 4.17 : Mạch driver TB6612
Thông số kỹ thuật:
- Điện áp nuôi và cấp mức logic: 2,7-5,5 V DC
- Điện áp cấp cho động cơtối đa: 15V DC
- Hai mạch cầu H cho hai động cơ
- Dòng ngõ ra liên tục: 1,2 A cho mỗi cầu
- Bảo vệ quá nhiệt, quá áp và chống dịng ngược
- Kích thước: 20 × 20 Bảng 4.4 : Các ngõ ra/vào module TB6612 Chân VCC,GND VM PWMA,PWMB AI1,BI1 AI1,BI1 STBY
Tiến hành khảo sát không tải TB6612 và động cơ GA25 V1, điều khiển và đo vận tốc bằng vi điều khiển STM32F411.
ĐỒ ÁN THIẾT KẾ HỆ THỐNG CƠ ĐIỆN TỬ GVHD : TS.LÊ ĐỨC HẠNH
Hình 4.18 : Đồ thị quan hệ giữa PWM và tốc độ động cơ ( khơng tải)
Từ hình trên ta thấy khối driver+động cơ tuyến tính với PWM cấp vào.
Sử dụng phương pháp Least square line để tìm liên hệ giữa vận tốc động cơ và PWM Động cơ 1:
Động cơ 2:
Hình 4.20 : Liên hệ giữa vận tốc động cơ 1 và PWM
= 0.2767 + 1.7062 Với là %PWM, ( ) là vận tốc động cơ
4.7. Giải thuật PID cho động cơ:
Khi có tải, đá ứng của động cơ lâu hơn so với không tải, điều này không đảm bảo thời gian đáp ứng của hệ thống = 0,1 . Do đó cần pahir thiết kế bộ điều khiển cho động cơ, ở
đây nhóm sử dụng bộ điều khiển PID để điều khiển tốc độ động cơ, với vận tốc điều khiển trong khoảng 2/3 vận tốc tối đa.
Ta xét cụ thể cho từng động cơ:
- Tìm hàm truyền
- Thiết kế bộ điều khiển PID với: độ vọt lố= 10%, thời gian xác lập = 0,1
4.7.1. Động cơ trái
Bảng 4.5 : Bảng hàm truyền động cơ và PWM tương ứng
75% 85% 100% PWM Random {32, 80, 160, 240, 400, 320, 200, 100, 50, 0};
ĐỒ ÁN THIẾT KẾ HỆ THỐNG CƠ ĐIỆN TỬ GVHD : TS.LÊ ĐỨC HẠNH
Để việc điều khiển được tối ưu, ta chỉ nên sử dụng động cơ ở mức khoảng 2/3 tốc độ tối đa. Dựa theo các đặc tính của động cơ, nhóm chọn điều khiển động cơ ở tốc độ 85%PWM. Dạng đồ thị điện áp cấp vào để Estimate và Validate:
Hình 4.21 : Cấp PWM Random
Hình 4.23 : Cấp PWM 85%
Hình 4.24 : PWM 100%
ĐỒ ÁN THIẾT KẾ HỆ THỐNG CƠ ĐIỆN TỬ GVHD : TS.LÊ ĐỨC HẠNH
Hình 4.25 : Độ 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
29,032
( ) = (1+0,036767 )(1+0,0063069 ) đểthiết kếbộ điều khiển PID.
Các chỉ tiêu cho bộ điều khiển như sau:
Độ vọt lố= 10%.
Thời gian xác lập = 0,05 .
Mơ hình của hệ gồm bộ điều khiển PID và động cơ như sau:
( )
+ -
( )
Hàm truyền của bộ điều khiển PID:
= + +
Hàm truyền của động cơ có dạng:
= (1 + 1 )(1 + 2 )
Phương trình đặc trưng của hệ kín:
1+( +
123+( + 1+ 2)2+( +1) + =0
Vì 1 2 rất nhỏ nên ta có thể bỏ qua; khi đó phương trình đặc trưng có dạng
2
+
Phương trình đặc trưng của hệ là dạng bậc hai, vì vậy đáp ứng của hệ sẽ có dạng giống với đáp ứng của hệ dao động bậc hai.
Từ chỉ tiêu độ vọt lố [11] ta có: −
= √1− 2. 100% = 10% Suy ra: = 0,59.
Từ chỉ tiêu thời gian xác lập, tính theo tiêu chuẩn 2% [11] ta có:
ĐỒ ÁN THIẾT KẾ HỆ THỐNG CƠ ĐIỆN TỬ GVHD : TS.LÊ ĐỨC HẠNH
=
4
Suy ra:= 135,59.
Với = 0,59,= 135,59 thì dạng của phương trình đặc trưng của hệ là [11]:
2 + 2 +
Đồng nhất hệ số với (4.3) ta được hệ phương trình sau:
1+ 2+
{1+2+
Ta nhận thấy hệ (4.7) là hệ phương trình 3 ẩn – 2 phương trình nên cần chọn trước một nghiệm để giải. Qua khảo sát thực tế với động cơ, nhóm nhận thấy = 0,45 là phù hợp với động cơ nên chọn = 0,45.
Với = 0,45; giải hệ (4.7) ta được: = 55,67, = 0,0015.
Vậy, bộ điều khiển PID có hàm truyền là:
= 0,45 +
Phương pháp thực nghiệm thứ 2 của Zeigler:
Nichols: Tăng hệ số khuyếch đại đến ℎ, hệ ở trạng thái dao động ổn định với chu kì
ℎ.
Bảng 4.6 : Bảng Zeigler – Nichols
PI PID
Hình 4.27 : Vận tốc động cơ bắt đầu dao động điều hòa (time (ms),van toc (rpm)) Với Setpoint = 300, ta có ℎ = 0,1 , ℎ = 0.45. Ta
có:3
= 0,27, = 0.5, = 0.0125
Sử dụng PID Tune Toolbox của matlab:
Hình 4.28 : Hệ số kp, ki, kd khi sử dụng PID Tune Toolbox
ĐỒ ÁN THIẾT KẾ HỆ THỐNG CƠ ĐIỆN TỬ GVHD : TS.LÊ ĐỨC HẠNH
Hình 4.29 : Đáp ứng động cơ với setpoint 100rpm
Hình 4.31 : Đáp ứng động cơ với setpoint 250rpm
Hình 4.32 : Đáp ứng động cơ với setpoint 300rpm
4.7.2. Động cơ phải
Tương tự như động cơ trái, ta có:
ĐỒ ÁN THIẾT KẾ HỆ THỐNG CƠ ĐIỆN TỬ
Bảng 4.7 : Bảng hàm truyền động cơ và PWM tương ứng
75% 85% 100% PWM Random {32, 80, 160, 240, 400, 320, 200, 100, 50, 0};
ĐỒ ÁN THIẾT KẾ HỆ THỐNG CƠ ĐIỆN TỬ GVHD : TS.LÊ ĐỨC HẠNH
Hình 4.34 : Cấp PWM 75%
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ó :
= 0,24, = 0.5, = 0.0125
53
ĐỒ ÁN THIẾT KẾ HỆ THỐNG CƠ ĐIỆN TỬ
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
Hình 4.40 : Đáp ứng động cơ với setpoint 200rpm
Hình 4.41 :Đáp ứng động cơ với setpoint 250rpm
ĐỒ ÁN THIẾT KẾ HỆ THỐNG CƠ ĐIỆN TỬ GVHD : TS.LÊ ĐỨC HẠNH
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
Hình 4.44 : Mạch nguồn sau khi thi công4.8.2. Thiết kế board mạch ra chân cho STM32F4 4.8.2. Thiết kế board mạch ra chân cho STM32F4
Hình 4.45 : PCB mạch nguồn ( mặt trên và dưới )
ĐỒ ÁN THIẾT KẾ HỆ THỐNG CƠ ĐIỆN TỬ GVHD : TS.LÊ ĐỨC HẠNH
CHƯƠNG 5 : MƠ HÌNH HĨA5.1. Mơ hình động học 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:
̇
[ ̇] = [
̇
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:
̇ = ̇ − ̇ .. { ̇ = ̇ + ̇ .. ̇ = Trong đó: là khoảng cách từ M đến C. 59
ĐỒ ÁN THIẾT KẾ HỆ THỐNG CƠ ĐIỆN TỬ GVHD : TS.LÊ ĐỨC HẠNH
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:
Sai số động học có thể được viết:
[ ̇] = [3 1 − 1 ][−] 2 − 2 − − 1][ ] −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.
̇ ̇= − 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 q 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 Bán kính cong Vận tốc cần đạt Khoảng cách từ tâm xe đến bánh xe chủ động Bán kính bánh xe
Thời gian lấy mẫu động cơ Thời gian lấy mẫu hệ thống
ĐỒ ÁN THIẾT KẾ HỆ THỐNG CƠ ĐIỆN TỬ GVHD : TS.LÊ ĐỨC HẠNH
Đồ thị khảo sát khoảng cách và sai số dò line 2 lớn nhất
5.3. Thiết kế giải thuật điều khiển cho robot Bắt đầu Bắt đầu Vị trí ban đầu T_system Đo sai số e2 Tính vận tốc góc lí thuyết của robot Tính vận tốc hai bánh
lí thuyết T_motor 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 tốn vị trí xe Tính tốn vị trí điểm tracking S
Tới điểm cuối Đ Kết thúc
Hình 5.4 : Giải thuật điều khiển robot trong mô phỏng
ĐỒ ÁN THIẾT KẾ HỆ THỐNG CƠ ĐIỆN TỬ GVHD : TS.LÊ ĐỨC HẠNH
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.
Hình 5.6 : Sai số bám line
Hình 5.7 : Góc định hướng của robot
ĐỒ ÁN THIẾT KẾ HỆ THỐNG CƠ ĐIỆN TỬ GVHD : TS.LÊ ĐỨC HẠNH
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: 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.
CHƯƠNG 6 : THỰC NGHIỆM VÀ ĐÁNH GIÁ
Hình 6.1 : Mơ hình thực tế xe dị line
Hình 6.2 : Vận tốc 2 động cơ thu được trong quá trình xe chạy bám line
ĐỒ ÁN THIẾT KẾ HỆ THỐNG CƠ ĐIỆN TỬ GVHD : TS.LÊ ĐỨC HẠNH
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.
- Qn tính của xe trong q 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 sốt được các sai số trong q trình gia cơng và lắp đặt.
ĐỒ ÁN THIẾT KẾ HỆ THỐNG CƠ ĐIỆN TỬ GVHD : TS.LÊ ĐỨC HẠNH
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 Tốn Thiết Kế Hệ Dẫn Động Cơ Khí Tập 2, 116-117. Việt Nam, Công ty In Cơng Đồ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
[15] VISHAY, Application of Optical Reflex SensorsTCRT1000, TCRT5000,
CNY70, Document number: 80107, Rev. 1.1, 02-02
[16] LM2586 Datasheet