4.2.4 Thiết kế bộ điều khiển trung tâm4.2.4.1Yêu cầu vi điều khiển 4.2.4.1Yêu cầu vi điều khiển
Lựa chọn vi điều khiển
Theo như sơ đồ khối của hệ thống điều khiển cần 3 vi điều khiển bao gồm 1 slave điều khiển động cơ, 1 slave nhận tín hiệu từ cảm biến và xử lý và 1 master. Yêu cầu của vi điều khiển là:
- Đọc giá trị analog của 7 cảm biến.
- Đọc tín hiệu trả về từ encoder của động cơ. - Chân PWM để điều khiển động cơ.
- Giao tiếp giữa master và slave.
- Ngắt timer và ngắt ngồi để đọc tín hiệu từ encoder
Từ những yêu cầu trên nhóm sử dụng vi điều khiển Atmega328 với các thơng số: - 8 kênh ADC 10 bit
- Giao tiếp: 1 UART, 1 I2C, 2 SPI
- 6 kênh PWM trong đó có 2 kênh tần số cao (62500Hz) - 2 kênh ngắt ngoài và 3 timer
Hình 4.48 Sơ đồ chân vi điều khiển Atmega 328
+ Giao tiếp Slaver 1: 7 chân đọc tín hiệu analog và giao tiếp
+ Giao tiếp Slaver 2: 4 chân logic và 2 chân PWM cho driver, giao tiếp và 2 chân ngắt ngoài encoder.
+ 1 Master giao tiếp với 2 Slaver
Kết luận: Nhóm chọn slaver 1 là atmega328P dạng TQFQ, master và slaver 2 là atmega328P dạng PDIP.
4.2.3.2 Chuẩn giao tiếp
- Giao tiếp master và slaver 1 là: RX, TX - Giao tiếp master và slaver 2 là: I2C
Hình 4.51 Model 3D mạch điều khiển trung tâm
4.3 Mơ hình hố hệ thống
Vì xe có trọng lượng khơng đổi trong quá trình di chuyển nên phần lực tác dụng theo phương thẳng đứng không đổi và xe chỉ chịu tác dụng của ngoại lực là lực ma sát, xe di chuyển trên sa bàn phẳng, không gồ ghề nên hầu như ngoại lực tác động vào xe khơng đổi => Chọn phân tích động học cho hệ thống.
4.3.1 Mơ hình động học của robot
Để thực hiện việc điều khiển xe bám được line tốt, ta tiến hành thực hiện mơ hình động học của hệ thống. Ta chọn mơ hình động học vì: sa bàn bằng phẳng, ngoại lực tác dụng xấp xỉ bằng 0, trọng lượng xe khơng đổi.
Trong mơ hình này bao gồm: Điểm tham chiếu R: chứa các thông tin về vị trí và hướng tiếp tuyến của line để robot bám theo, điểm M: trung điểm của đoạn thẳng nối tâm 2 bánh xe, điểm C là điểm tracking có thơng tin về vị trí và hướng hiện tại của robot
Phương trình động học tại điểm M: [x˙ ˙ y ˙ φ]=[cosφ 0 sinφ 0 0 1][v ω]
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
{x˙C=vcosφ − ωdsinφ
˙
yC=vsinφ+ωdcosφ
˙
φ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) {x˙R=vRcosφR
˙
yR=vRsinφR
˙
φR=ωR
vR 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:
{ e1=(xR− xC)cosφ+(yR− yC)sinφ e2=−(xR− xC)sinφ+(yR− yC)cosφ
e3=φR− φ Đạo hàm sai số động học, ta được:
{ e1=˙ (xR˙ −xC˙ )cosφ−(xR− xC)sinφ+(yR˙ −˙yC)sinφ+(yR− yC)cosφ
˙
e2=−(xR˙ −xC˙ )sinφ−(xR− xC)cosφ+(˙yR−yC˙ )cosφ −(yR− yC)sinφ
˙
⇒[e˙1 ˙ e2 ˙ e3]=[vRcose3 vRsine3 ωR ]+[−1 e2 0 −(d+e1) 0 −1 ][v ω]
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.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.
e3=arctan(e2− e2
'
ds )
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 fh = 14/3 vòng/s
Số lần lấy mẫu cần thiết:
N .Ts=m.Th=¿m=N .Ts
Th =N .0.023 14
=N .757
Với Ts=0.02s là thời gian lấy mẫu vận tốc động cơ
Th= 1
(143 )=
3
14 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 N=75 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
tốc khi tăng) đến khi động cơ đạt được vận tốc cực đại. Ta được hình sau:
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à:
y=0.659x+93.456
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
Bước 3: Đo vận tốc theo thời gian
Hình 4.57 Đồ thị vận tốc theo thời gian khi chưa có bộ điều khiển PID
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
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 420.5
s2+50.07s+416.2
Độ chính xác xấp xỉ: 87.2% %Os (%) 0
Tsettling(s) 0.4011
Động cơ phải Hàm truyền 773.3
s2+77.55s+767.2
Độ chính xác xấp xỉ: 91%
%Os(%) 0
Tsettling(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%.
Tiêu chí chọn bộ điều khiển tracking: - 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ự đố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à: Value=Kp.e(t)+Ki∫
0
t
e(τ)dτ+Kd de(t) dt
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:
Value(k)=Value(k −1)+Kp(e(k)− e(k−1))+Kie(k)+Kd¿
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 Tsettling<0.15s (để đả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
Hình 4.62 Dữ liệu và đồ thị đáp ứng động cơ sau khi có 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à:
1767
s2+62.66s+1747 đối với động cơ trái.s2+66.991611s+1583 đố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 1767 s2+62.66s+1747 Độ chính xác xấp xỉ: 88.27% y=0.659x+93.456 Kp = 1.4268 %Os (%) 2.85 Ki = 28.9187
Tsettling(s) 0.1375 Kd = 0 Động cơ phải Hàm truyền 1611 s2+66.99s+1583 Độ chính xác xấp xỉ: 91.72% y=0.69512x+87.514 Kp = 3.975 %Os(%) 0.7441 Ki = 49.1952 Tsettling(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 Vr
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 [v ω]T điều khiển hệ thống hội tụ về điểm làm việc [e1 e2 e3]T
=[0 0 0]T
.
Định lý: Một hệ được mơ tả bởi phương trình trạng thái x=˙ f(x1,…,xn). Nếu tìm được
hàm V(x) 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.
Chứng minh: Chọn hàm Lyapunov xác định dương: V=1 2e12+1 2e22+1−cose3 k2 ⇒V˙=e1(vRcose3− v)+sine3 k2 (k2vRe2+ωR−ω)
Bộ điều khiển (1) làm giá trị V˙ â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ố e1, e2, e3 sẽ hội tụ về 0 khi t →∞
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 tố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:
1767
s2+62.66s+1747
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:
1611
s2+66.99s+1583
Settling Time: 0,12 s %Os = 0,74%
Hình 5.7 Tốc độ 2 bánh sau khi có bộ điều khiển
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 e2 là cộng thêm một số ngẫu nhiên trong khoảng ±3,3mm(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ố K1,K2, K3có đượ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ố e2tố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ỆM6.1 Lắp ráp phần cơ 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