Phương trình động học tại điểm M:
Trong đó v và w là vận tốc dài và vận tốc góc của xe. Phương trình động học tại điểm C
Với d là khoảng cách từ M đến điểm tracking C.
Phương trình động học tại điểm R (điểm mong muốn của C trên line)
vận tốc mong muốn của mobile platform tại điểm tham chiếu. Ta có sai số động học:
Đạo hàm sai số động học, ta được:
Do hệ thống phototransistor chỉ xác định được sai số theo phương pháp tuyến với phương chuyển động của xe. Do đó mơ hình động học của xe cần được giới thiệu lại với điểm C là tâm của dãy sensor, M là trung điểm hai bánh chủ động và là điểm tracking của xe. Giả sử xe sẽ chạy với vận tốc bằng vận tốc tham chiếu nên ta có e1 = 0. Mơ hình này được thể hiện trên Hình 4.52. Như vậy, để xác định được đầy đủ thơng tin về vị trí của điểm tracking so với tham chiếu, sai số e2 và e3 cần được xác định.
Hình 4.53 Mơ hình động học sử dụng cho robot dị line
Hình 4.54 Xác định sai số e3
Trên thực tế, e2 được xác định trực tiếp từ hệ thống sensor. Đối với e3, phương án xác định được đề xuất là cho robot di chuyển theo phương trước đó một đoạn ds đủ nhỏ để khi
nối 2 điểm RR’ tạo thành tiếp tuyến của đường cong (Hình 4.54). Khi đó, sai số e3 được xác định theo cơng thức.
4.3.2 Tìm hàm truyền động cơ
Xác định số lần lấy mẫu và chu kì lấy mẫu:
Động cơ chạy với vận tốc tối đa là 280 rpm = 280/60 vòng/s = 14/3 vòng/s Suy ra = 14/3 vịng/s
Theo tiêu chuẩn lấy mẫu Nyquist thì: = 28/3 ≈ 9.33 Hz
Chọn Hz vì lý do phụ thuộc vào độ phân giải của encoder của động cơ. Số lần lấy mẫu cần thiết:
Với là thời gian lấy mẫu vận tốc động cơ là chu kì quay của động cơ
N là số lần lấy mẫu. Chọn giá trị N để m là số nguyên
Vậy là giá trị đầu tiên để m là số nguyên => Phải lấy mẫu vận tốc 75 lần với chu kì lấy mẫu là 0.02s
Các bước tìm hàm truyền động cơ:
Bước 1: Cấp PWM từ vi điều khiển cho động cơ, tăng dần giá trị này (ghi lại các giá trị vận
Hình 4.55 Đồ thị vận tốc theo PWM
Nhận xét: đồ thị vận tốc động cơ theo PWM là khơng tuyến tính và có thể dùng
phương pháp xấp xỉ tuyến tính để xấp xỉ vận tốc theo PWM
Đồ thị trên với trục hoành là PWM (55), trục tung là tốc độ động cơ (rpm), đường màu xanh là giá trị đo được, đường màu đỏ là đường xấp xỉ tuyến tính.
Từ phương pháp xấp xỉ ta suy ra được phương trình vận tốc theo PWM là:
Từ kết quả phần mô phỏng ta thấy vận tốc động cơ cỡ khoảng 220 rpm nên ta sẽ sử dụng giá trị này để tìm hàm truyền và đáp ứng của động cơ khi chưa có bộ điều khiển PID.
Bước 2: Tìm hàm truyền khi chưa có bộ điều khiển PID.
Với v = 220 rpm thì PWM = (223.456)/0.659 = 192
Hình 4.56 Dữ liệu tìm hàm truyền khi chưa có bộ điều khiển PID
Bước 3: Đo vận tốc theo thời gian
Trục hoành là thời gian (s) trục tung là vận tốc động cơ (rpm) với vận tốc tham chiếu là 220 rpm
Bước 4: Dùng công cụ Ident của Matlab để tìm hàm truyền:
Hình 4.58 Cơng cụ Ident của Matlab
Chọn Time-Domain Signals để chọn nhập dữ liệu trong miền thời gian. Input: nhập mảng 1 chiều có giá trị của các phần tử là vận tốc tham chiếu. Output: nhập mảng 1 chiều là giá trị vận tốc của động cơ.
Hình 4.60 Hàm truyền khi chưa có bộ điều khiển PID
Ở bước này cần chọn số Poles sao cho độ chính xác xấp xỉ cao nhất có thể và ở 2 động cơ bài này có độ chính xác là 87.2% và 91%.
Bảng 9 Hàm truyền động cơ trước khi có PID
Trước khi có PID Động cơ trái Hàm truyền
Độ chính xác xấp xỉ: 87.2%
%Os (%) 0
(s) 0.4011
Động cơ phải Hàm truyền
Độ chính xác xấp xỉ: 91%
%Os(%) 0
(s) 0.3528
4.3.3 Thiết kế bộ điều khiển
4.3.3.1 Tiêu chí thiết kế bộ điều khiển
Tiêu chí chọn bộ điều khiển PID:
- Thời gian xác lập của động cơ < 0,15s (để đảm bảo chu kì lấy mẫu của bộ điều khiển bám line).
- Sai số xác lập < 5%.
- Sai số bám line = ± 18mm.
4.3.3.2 Lý do lựa chọn và phương trình bộ điều khiển
- Thiết kế bộ điều khiển PID để điều khiển vận tốc cho động cơ: Có thể mơ phỏng,
dự đoán và thay đổi được đáp ứng của động cơ khi áp dụng bộ điều khiển, là bộ điều khiển phổ biến để điều khiển realtime, dễ dàng đưa giải thuật điều khiển vào vi điều khiển.
Phương trình bộ điều khiển PID là:
Rời rạc hóa phương trình trên dể đưa giải thuật điều khiển vào code vi điều khiển:
Các bước tìm hàm truyền và hệ số PID
Bước 1: Dùng PID Tuner của Matlab để xác dịnh các hệ số PID
Tiêu chí để chọn bộ điều khiển là thời gian xác lập (để đảm bảo chu kì lấy mẫu của bộ điều khiển bám line)
Hình 4.61 Hệ số PID và đồ thị đáp ứng
Kp = 1.4268; Ki = 28.9187; Kd = 0
Bước 2: Sau khi có các hệ số PID, đưa các hệ số này vào code và nạp cho vi điều khiển và
đo lại vận tốc động cơ sau khi có bộ điều khiển PID
Đồ thị vận tốc theo thời gian khi có bộ điều khiển PID.
Sử dụng lại công cụ Ident của Matlab để xác định hàm truyền động cơ khi có bộ điều khiển PID.
Kết quả được hàm truyền là:
đối với động cơ trái. đối với động cơ phải.
Hình 4.64 Đáp ứng step của động cơ phải
Bảng 10 Hàm truyền động cơ sau khi dùng bộ điều khiển PID
Sau khi có PID Phương trình xấp xỉ vận tốc theo PWM Hệ số PID Động cơ trái Hàm truyền Độ chính xác xấp xỉ: 88.27% Kp = 1.4268 %Os (%) 2.85 Ki = 28.918 7
(s) 0.1375 Kd = 0 Động cơ phải Hàm truyền Độ chính xác xấp xỉ: 91.72% Kp = 3.975 %Os(%) 0.7441 Ki = 49.195 2 (s) 0.1234 Kd = 0
Kết luận: Dựa vào 2 đồ thị, thời gian đáp ứng 2 động cơ lần lượt là 0,13s và 0,12s <
0,15s mục tiêu thiết kế => Đạt yêu cầu.
- Giải thuật điều khiển tracking: thực hiện theo yêu cầu về vận tốc mong muốn và
sai số vị trí so với đường line từ đó tính ra vận tốc góc của từng bánh xe để điều khiển xe chạy bám line. Kết quả bám line tốt và sai số nhỏ.
Bộ điều khiển được sử dụng để từ tín hiệu đầu vào điều khiển hệ thống hội tụ về điểm làm việc .
Định lý: Một hệ được mơ tả bởi phương trình trạng thái . Nếu tìm được hàm là một
hàm xác định dương với mọi biến trạng thái, sao cho đạo hàm của nó theo thời gian là một hàm xác định dấu âm thì hệ ổn định.
Hệ thống được ổn định theo tài liệu [7] bởi phương trình:
Chọn hàm Lyapunov xác định dương:
Bộ điều khiển (1) làm giá trị âm, điều này có nghĩa hệ thống được ổn định theo tiêu chuẩn Lyapunov và những giá trị sai số , , sẽ hội tụ về 0 khi
4.3.4 Lưu đồ giải thuật điều khiển
Chương 5: MƠ PHỎNG HOẠT ĐỘNG CỦA XE ROBOT5.1 Mơ phỏng sa bàn khi chưa nhúng sai số cảm biến và hàm truyền động cơ 5.1 Mô phỏng sa bàn khi chưa nhúng sai số cảm biến và hàm truyền động cơ
Với khoảng cách từ tâm cảm biến C đến tâm 2 bánh chủ động được chọn là 96 mm. Bên cạnh đó, giá trị e2 phụ thuộc vào bộ số [k1 k2 k3] được chọn. Tiến hành mô phỏng trên đoạn đường đua với các hệ số [k1 k2 k3] lần lượt là [10 1000 1].
Bảng 11 Các thông số dùng cho mô phỏng động học
Đại lượng Giá trị Đơn vị
Chiều dài 180 mm Chiều rộng 138 mm Bán kính cong 500 mm Vận tốc lớn nhất, vmax 0,7 m/s Khoảng cách 2 bánh xe 150 mm Đường kính bánh xe 65 mm
Khoảng cách giữa tâm 2 bánh xe và điểm tracking, d 30 mm
Chiều rộng line 19 mm
b. Vận tốc góc của bánh trái và bánh phải
Hình 5.1 Kết quả mơ phỏng khi chưa nhúng sai số cảm biến và hàm truyền của động cơ
5.2 Mô phỏng sa bàn khi đã nhúng sai số cảm biến và hàm truyền động cơMô phỏng cảm biến Mô phỏng cảm biến
Với các thông số như trên tiến hành mô phỏng cảm biến với sai số của tâm cảm biến thực so với phương pháp xấp xỉ trung bình tính tốn từ các giá trị cảm biến là 3,3 mm.
Từ dữ kiện hiện tại là vị trí giữa 2 bánh xe và hướng của xe ta viết được phương trình đường thẳng đi qua dãy cảm biến. Tìm giao điểm của đường thẳng này với sa bàn, tính khoảng cách giữa tâm cảm biến và giao điểm ta có sai số e2
Vì sai số của thuật toán xấp xỉ đọc về từ cảm biến là 3,3 mm = 0,0033m. Khi ra được sai số e2 ta cộng sai số này với 1 số ngẫu nhiên từ -0,0033 đến +0,0033 (m)
Tiêu chí thời gian lấy mẫu bộ điều khiển
Điều chỉnh lại thời gian lấy mẫu là 0,16s để phù hợp với settling time của động cơ là 0,13s.
Mô phỏng kết hợp cảm biến và hàm truyền động cơ:
Nhóm sử dụng 2 động cơ để điều khiển tốc độ bánh trái và tốc độ bánh phải, các động cơ có hàm truyền và đáp ứng step như sau:
- Hàm truyền của động cơ trái:
Settling Time: 0,13 s %Os = 2,85%
Hình 5.2 Đáp ứng step của động cơ trái
- Hàm truyền động cơ phải:
Settling Time: 0,12 s %Os = 0,74%
Hình 5.3 Đáp ứng step của động cơ phải
Hình 5.6 Đồ thị sai số e2 theo thời gian
5.3 Nhận xét kết quả mô phỏng
Kết quả mô phỏng trên đã bao gồm việc nhúng sai số của: cảm biến là cộng thêm một số ngẫu nhiên trong khoảng (sai số của tâm cảm biến vật lý với giá trị tính được từ thuật tốn xấp xỉ) và hai hàm truyền của động cơ (bánh trái và bánh phải) sau khi có bộ điều khiển PID.
Kết quả mơ phỏng cho thấy có thể áp dụng được giải thuật điều khiển (bám line và PID) vào mơ hình thực tế với các hệ số có được khi mơ phỏng, vận tốc của động cơ từ 120 rpm đến 260 rpm phù hợp với động cơ đã chọn ở phần thiết kế cơ khí là 280 rpm.
Giá trị sai số tối đa khi mô phỏng là ± 16mm phù hợp với yêu cầu đặt ra là ±18 mm. Do thời gian đáp ứng của động cơ trên mơ hình thật sau khi có bộ điều khiển PID là 0.13s nên chu kì lấy mẫu của bộ điều khiển bám line lớn (để đảm bảo động cơ đạt được tốc độ mong muốn thì master mới tiếp tục cấp giá trị vận tốc mới) do đó khi vận tốc tăng cao thì xe sẽ khơng ổn định và lệch khỏi đường line. Để thực tế gần giống với kết quả mơ phỏng thì cần phải che chắn cảm biến để hạn chế nhiễu từ môi trường xung quanh và vận tốc không được quá cao (<1m/s).
Chương 6: THỰC NGHIỆM
6.1 Lắp ráp phần cơ
- Các nguyên công gia cơng:
+ Gia cơng cắt Laze CNC độ chính xác cao: tấm thân xe, 2 tấm gá động cơ, tấm gá cảm biến, tấm che cảm biến.
+ Gia công thô – cắt: Hộp pin.
+ Chấn: Hộp pin, tấm gá cảm biến, tấm che cảm biến.
+ Khoan và doa lỗ: Tấm thân xe tại vị trí lắp chốt định vị, 2 tấm gá động cơ tại vị trí lắp chốt định vị.
+ Khoang và taro ren: 2 tấm gá động cơ tại vị trí bắt vít.
- Bulong: sử dụng bulong lục giác đầu dù: giúp tăng lực xiết và giảm kích thước để dễ lắp đặt tại các vị trí chật hẹp (so với lục giác đầu trụ). Dùng bulong lục giác đầu chìm tại vị trí nối trục: giúp chống tháo lỏng vì trục động cơ quay nhanh.
Hình 6.1 Lắp ráp cơ khí xe
6.2 Thực nghiệm phần điện và điều khiển
6.2.1 Thực nghiệm cảm biến
Nhóm sử dụng cảm biến TRCT5000 5 con lắp mạch và tiến hành đo:
Hình 6.2 Hình thành cảm biến
Xác định độ cao từ cảm biến tới mặt sàn, nhóm thử cảm biến ở các độ cao từ 9mm- 15mm cho ra các đồ thị sau:
- 10mm
- 12mm
- 14mm
Hình 6.3 Kết quả thực nghiệm giá trị cảm biến
Kết luận: Nhóm chọn khoảng cách 11mm và sự ổn định và giá trị chênh lệch giữa nền
đen và trắng tốt.
Tiến hành calib cảm biến bằng cách đo giá trị lớn nhất và nhỏ nhất của từng cảm biến ta có giá trị sau:
Với phương trình calib sau:
yjo: giá trị sau khi điều chỉnh j,i
xji: giá trinh đọc được thứ j cảu cảm biến thứ i
ymax và ymin là giá trị lớn nhất và nhỏ nhất mong muốn cho tất cả các cảm biến xmaxj xminj là giá trị lớn nhất, giá trị nhỏ nhất đọc được của cảm biến thứ i
Bảng 12 Tính tốn calib cảm biến
Cảm biến GTLN GTNN Phương trình sau khi calib 1 951 243 yjo = 200 + 1,1158(xji - 243) 2 966 306 yjo = 200 + 1,197(xji - 306) 3 949 224 yjo = 200 + 1,09(xji - 224) 4 954 264 yjo = 200 + 1,145xji - 264) 5 970 273 yjo = 200 + 1,133(xji - 273) 6 967 234 yjo = 200 + 1,078(xji - 234) 7 958 287 yjo = 200 + 1,177(xji - 287) Với ymax ymin lấy lần lượt là: 990 và 200
Tiến hành thực nghiệm theo sơ đồ sau:
Hình 6.4 Sơ đồ thực nghiệm cảm biến
Hình 6.5 Đồ thị kết quả thực nghiệm cảm biến
Kết luận: Sai số lớn nhất emax = 3,3 mm đáp ứng được yêu cầu đặt ra của đề bài.
6.2.2 Thực nghiệm driver động cơ
Đã nêu ở phần 4.3.3 Thiết kế bộ điều khiển.
6.2.3 Thực nghiệm nguồn mạch giảm áp
Với mạch động lực nhóm chọn nguồn 2 cục pin vng 9V Panasonic có dung tích 600mAh. Để cấp qua 2 động cơ, một mạch driver và một mạch hạ áp, có thể sử dụng liên tục trong thời gian ít nhất là 10 phút.
Sử dụng mạch giảm áp từ 18V thành 12V cho thí nghiệm tìm hàm truyền động cơ liên tục trong 5 phút nhận ra như sau:
Điện áp 2 đầu động cơ là 11.3V động cơ hoạt động bình thường và đạt được tốc độ cần thiết.
Đầu ra Vout của mạch giảm áp là 12V (không sụt áp). Chip LM7812 và L298N khơng nóng nên hiệu suất ổn định. Kết luận: Mạch giảm áp và mạch động lực hoạt động tốt.
6.3 Hồn thành thực nghiệmCảm biến Cảm biến
Mạch chính
Hình 6.7 Mạch điện trung tâm thực tế
Tổng thể xe
KẾT LUẬN Công việc đã làm:
- Tìm hiểu các thiết kế xe dị line tương tự với mục tiêu đầu bài, tìm hiểu các kiến thức có liên quan để đưa ra phương án tốt nhất cho đầu bài.
- Thiết kế và hiệu chỉnh sơ đồ gantt phù hợp để kiểm sốt lịch trình làm việc.
- Thiết kế cơ, điện, lập trình, mơ phỏng thành cơng. Bao gồm cả mô phỏng lẫn thực