4.3.1. Lựa chọn cảm biến
- Dựa vào phương án thiết kế, ta nhận thấy đường line được sử dụng với 2 màu trắng và đen. Nên nhóm đề xuất lựa chọn sử dụng loại cảm biến thu phát hồng ngoại được đóng gói sẵn vì lý do: Đảm bảo được kích thước giữa các cặp cảm biến, có được đặc tính của IR phát và thu. Nhóm lựa chọn loại cảm biến: TCRT5000.
Bảng 4.2 : Thông số kỹ thuật cảm biến TCRT 5000
Kích thước (LxWxH) Bước sóng IR phát Phạm vi hoạt động
Dòng hoạt động IC/IF tối đa Điện áp hoạt động
Công suất tiêu thụ Góc phát
Góc thu
Hình 4.3 : Sơ đồ nguyên lý cảm biến TCRT5000
4.3.2. Tính toán giá trị điện trở
- Với sơ đồ nguyên lý trên, ta kết hợp với các giá trị được cho trong datasheet của TCRT5000 :
{ = 1.25
≤ 60 - Điện trở R1 được tính theo công thức
Ω Chọn R1=220 Ω - Tính điện trở R2 : = Từ Hình 4.2 a) , ta tìm được= 1.2 , và từ đồ thị Hình 4.2 b) ta được = 0.6 2 = −=5−0.6=3666,67Ω 1,2.10−3 Chọn 2= 10 Ω 30
ĐỒ ÁN THIẾT KẾ HỆ THỐNG CƠ ĐIỆN TỬ GVHD : TS.LÊ ĐỨC HẠNH
a) b)
Hình 4.4 : Đồ thị thể hiện giữa dòng và áp qua LED
a) Mối quan hệ giữa và b) Mối quan hệ giữa điện áp vào và dòng qua LED thu
4.3.3. Cách bố trí cảm biến :
- Có 2 cách đặt cảm biến trên mảng các cảm biến : đặt dọc (position 2 Hình 4.3 ) và đặt ngang (position 1 Hình 4.3 ).
- Khi đi từ nền trắng vào nền đen, cảm biến phải di chuyển một khoảng Xd thì giá trị analog của nó mới thật sự xác định. Ta chọn cách bố trí cảm biến sao cho giá trị Xd nhỏ để đảm bảm cảm biến ổn định và sẽ cho kết quả chính xác hơn
- Dựa vào đồ thị Hình 4.3 ta sẽ chọn cảm biến bố trí nằm ngang (position 1)
Hình 4.5 : Ảnh hưởng của cách đặt cảm biến đến switching distance
4.3.4. Chọn khoảng cách giữa cảm biến và sàn :
Hình 4.6 : Vùng hoạt động của mỗi cảm biến TCRT5000
- Để đảm bảo rằng phototransistor có thể nhận được tín hiệu từ led phát khi robot bám theo line,khoảng cách từ sensor đến line được tính như sau :
( + ). (tan(45 ) + tan(31 )) =
=
tan(45 )+tan(31 )−
- Dựa trên datasheet của TCRT5000, ta biết được rằng = 0.7 và = 3.5, nên
ta có :
3.5
=
tan(45 )+tan(31 )− 0.7 = 1.48mm
- Với kết quả trên, nhóm quyết định làm thực nghiệm để tìm ra khoảng cách thích hợp nhất. Từ đây ta tiến hành đo bằng thực nghiệm: ta cho cảm biến tịnh tiến vuông góc từ thấp lên cao, ghi lại giá trị ADC đọc về vi điều khiển tại từng vị trí. Ta có được đồ thị quan hệ khoảng cách và giá trị ADC như hình :
Hình 4.7 : Sự thay đổi giá trị ADC trong khoảng 0-15mm
ĐỒ ÁN THIẾT KẾ HỆ THỐNG CƠ ĐIỆN TỬ GVHD : TS.LÊ ĐỨC HẠNH
- Từ đồ thị Hình 4.5 ta thấy rằng tại các vị trí h=4-6 mm, giá trị ADC đọc về là nhỏ nhất, có nghĩa là ánh sáng phản xạ nhiều nhất. Vì vậy ta chọn h=6mm là chiều cao thích hợp của cảm biến
4.3.5. Chọn khoảng cách giữa 2 led :
Yêu cầu :
- Phạm vi hoạt động của 2 led không chồng lên nhau
- Tối thiểu sai số
Để cảm biến hoạt động tốt thì các led phải tách biệt nhau, các led không được giao thoa với nhau, vì sẽ gây nên sai số khi hoạt động. Giả sử ta đặt hai cảm biến sát nhau sao cho vùng hoạt động của chúng vừa chạm nhau như hình
Hình 4.8 : Phạm vi quét của led thu và led phát ở 2 cảm biến đặt liền kề nhau
- Khoảng cách giữa 2 led phát và thu liền kề phải đảm bảo:
≥ ℎ ∗ 16 + ℎ ∗ 30 = 5.18
- Khoảng cách giữa 2 led trong 1 cảm biến là 3.5 mm. Do đó khoảng cách tối thiểu giữa 2 cảm biến sẽ là:
= + 3.5 ≥ 8.68
- Ngoài ra khi hoạt động sẽ có trường hợp cảm biến nằm trong vùng bất định thì giá trị
analog của cảm biến đưa về sẽ như nhau. Do đó sẽ không xác định được chính xác vị trí cảm biến so với tâm đường line. Vùng bất định của cảm biến được mô tả lại như sau:
Hình 4.9 : Vùng bất định của cảm biến
- Trên Hình 4.7 ta thấy khi cảm biến dịch chuyển sang phải một đoạn 26-d thì luôn có 2 led nằm trong đường line, và do đó tín hiệu analog đo được sẽ như nhau, tương tự khi cảm biến di chuyển sang trái đoạn 2d-26 thì chỉ có 1 led nằm trong đường line. Phải chọn giá trị của d sao cho các khoảng cách này là nhỏ nhất và thỏa điều kiện ≥ 8.68
- Xác định sự ảnh hưởng của đường line đến khoảng cách giữa 2 cảm biến
- Cách tiến hành thực nghiệm :
Cố định khoảng cách giữa cảm biến đến mặt sàn là h=6mm.Thay đổi khoảng cách giữa tâm cảm biến và tâm line từ tâm line ra đến vị trí cách tâm line 20mm theo 2 bước : + Dùng một cảm biến đơn di chuyển từ tâm ra vị trí cách tâm 20mm rồi vẽ lại đồ
thị quan hệ giữa khoảng cách và giá trị ADC
+ Cố định một cảm biến ở tâm line, sau đó dùng một cảm biến khác đặt sát cảm biến cố
định. Di chuyển cảm biến thứ hai đó ra xa đến vị trí cách tâm line 20mm. Vẽ lại đồ thị quan hệ khoảng cách và giá trị ADC của cảm biến thứ hai. Từ đó ta có được
đồ thị :
Hình 4.10 : Đồ thị quan hệ ảnh hưởng line đến khoàng cách hai cảm biến
ĐỒ ÁN THIẾT KẾ HỆ THỐNG CƠ ĐIỆN TỬ GVHD : TS.LÊ ĐỨC HẠNH
- Dựa vào đồ thị ta thấy rằng trong khoảng 14-18mm hai đường gần như trùng nhau. Điều này có nghĩa là trong khoảng 14-18mm, dù có hay không có cảm biến đặt kế bên thì giá trị ADC vẫn không đổi, chứng tỏ cảm biến không bị ảnh hưởng đến nhau. Ta chọn khoảng cách giữa hai cảm biến liên tiếp là 15 mm.
- Do sử dụng giải thuật xấp xỉ trọng số nên chọn ta đặt dãy cảm biến cách đều nhau.
4.3.6. Tuyến tính hóa cảm biến :
- Từ kết quả mô hình hóa nhóm lựa chọn chiều dài thanh cảm biến 120mm, từ đó sử dụng 7 cảm biến. Sử dụng phương pháp xấp xỉ để tính ra vị trí tâm cảm biến so với đường lineThông số test:
Chiều cao cảm biến : 7mm
Khoảng cách giữa hai cảm biến : 15mm Số lượng cảm biến : 7
- Việc thu nhận tín hiệu analog giúp ta nhận biết được sự thay đổi của giá trị mỗi khi cảm
biến di chuyển lại gần hay ra xa đường line. Tuy nhiên, tín hiệu analog cho mỗi cảm biến khác nhau, làm việc trong cùng điều kiện như nhau, cho ra giá trị khác nhau kể cả các phần cảm biến như nhau. Đây là lý do tại sao hiệu chuẩn (calibration) là cần thiết cho việc lắp đặt các cảm biến trên robot dò line. Do đó ta phải tiến hành calib cảm biến
- Công thức calib cảm biến :
− 0= + , − , ( , − , )
Trong đó:
, và , là giá trị nhỏ nhất và lớn nhất đọc được từ cảm biến thứ
và là giá trị lớn nhất và nhỏ nhất mà ta mong muốn cho tất cả cảm biến , là giá trị đọc được lần thứ j của cảm biến thứ
0giá trị sau khi điều chỉnh ,
Công thức calib cho 7 cảm biến :
Cảm biến 1
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
- Xác định vị trí tâm đường line
Các cảm biến0,1,2,3,4,5,6 tương ứng với các tọa độ -3,-2,-1,0,1,2,3 và giá trị trả về các cảm biến tương ứng là0,1,2,3,4,5,6
Hình 4.11 : Phương pháp xấp xỉ theo trọng số
- Từ giá trị khi thực hiện calib cảm biến, áp dụng công thức tính giá trị trung bình theo phương pháp xấp xỉ theo trọng số :
x =
Trong đó: + : trọng số của cảm biến so với line.
+ : giá trị ADC đọc về của cảm biến thứ . + : tọa độ của tâm thanh cảm biến so với tâm line.
+ : Khoảng cách giữa hai cảm biến.
- Tọa độ tâm đường line được xác định như sau :
ĐỒ ÁN THIẾT KẾ HỆ THỐNG CƠ ĐIỆN TỬ GVHD : TS.LÊ ĐỨC HẠNH 6 x = ∑xi y i i =0 ∑ yi i=0
Hình 4.12 : Đồ thị quan hệ giá trị tính toán và giá trị thực tâm đường line
- Sử dụng phương pháp nội suy hàm bậc nhất đề tìm ra phương trình thể hiện mối quan hệ giữa tọa độ thực và tọa độ đọc về cùa cảm biến .
Công thức nội suy : y = 0,7835x – 0,0941
trong đó : x là giá trị tọa độ thực tế của tâm cảm biến so với tâm line
y là giá trị tọa độ tính toán tâm cảm biến so với tâm line Sai số : ∆=4,8647
4.3.7. Thiết kế mạch cảm biến :
Dựa vào kết quả tính toán và thực nghiệm ở trên thì nhóm tiến hành thiết kế mạch cảm biến.
Hình 4.13 : Sơ đồ nguyên lý mạch cảm biến
Hình 4.14 : Sơ đồ PCB của mạch cảm biến
Hình 4.15 : Mạch cảm biến sau khi thi công
4.4. Nguồn điện
Bảng 4.3 : Công suất điện cần cung cấp cho các thiết bị
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à luôn 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
- Kích thước: 43 × 21 × 14
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:
Hình 4.19 : Liên hệ giữa vận tốc động cơ 1 và PWM
= 0.2876 + 2.0663
Độ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.22 : Cấp PWM 75%
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ếtkếbộđiềukhiểnPID.
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ình 4.26 : Mô hình của hệ điều khiển
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
P
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.30 : Đáp ứng động cơ với setpoint 200rpm
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ó:
50