STT
NHÓM 3
ĐỒ ÁN THIẾT KẾ HỆ THỐNG CƠ ĐIỆN TỬ GVHD: TS. Phùng Trí Cơng.
Hình 4.4 Đồ thị giữa PWM(%) và RPM vận tốc quay của động cơ 1 Bảng 4.4 Số liệu thử nghiệm L298 với động cơ 2:
STT
NHÓM 3
ĐỒ ÁN THIẾT KẾ HỆ THỐNG CƠ ĐIỆN TỬ GVHD: TS. Phùng Trí Cơng.
Hình 4.14 Đồ thị giữa PWM(%) và RPM vận tốc quay của động cơ 2
Tiếp theo ta tuyến tính hóa đường quan hệ này của 2 động cơ. Phương trình thể hiện mối quan hệ giữa áp đầu vào Driver (thể hiện qua giá trị duty cycle của tần số PWM) và vận tốc đầu ra của động cơ bên trái và bên phải lần lượt là:
Y1=3,1422X1 + 16,1176 Y2=3,1X2 + 20,7647
Trong đó:
+Y1, Y2 lần lượt là vận tốc đầu ra (vòng/phút) của động cơ trái, phải.
+ X1, X2: lần lượt là giá trị duty cycle của PWM cấp vào 2 động cơ trái, phải. Nhận xét:
Ta thấy quan hệ giữa tốc độ và xung PWM gần như tuyến tính đối vs cả hai động cơ.
4.3.2 Tìm hàm truyền động cơ:
Trước hết ta xác định gần đúng hàm truyền của động cơ thông qua việc xác định thời gian đáp ứng. Việc xác định này được tiến hành theo các bước sau:
- Cấp cho động cơ một mức điện áp cố định, ở thí nghiệm này chọn mức điện áp là 12V ứng với giá trị duty cycle của PWM là 100%.
NHÓM 3
ĐỒ ÁN THIẾT KẾ HỆ THỐNG CƠ ĐIỆN TỬ GVHD: TS. Phùng Trí Cơng. - Ghi lại giá trị vận tốc của động cơ theo thời gian. Thời gian lấy mẫu được chọn ở thí nghiệm này chọn là 0,05s.
Kết quả của thí nghiệm được thể hiện qua hình dưới đây:
Hình 4.15 Đáp ứng của động cơ 1 theo thời gian
Hình 4.16 Đáp ứng của động cơ 2 theo thời gian NHÓM 3
ĐỒ ÁN THIẾT KẾ HỆ THỐNG CƠ ĐIỆN TỬ GVHD: TS. Phùng Trí Cơng.
Dựa vào đồ thị ta thấy hệ thống có dạng bậc nhất P(s)
với Km (rpm/pwm) là hằng số độ lợi của động cơ và T(sec) là hằng số thời gian đáp ứng thể hiện qua:
Hình 4.17 Đồ thị đường cong đáp của hệ bậc nhất Từ đó ta có được hàm truyền của hai động cơ:
Khối driver-động cơ 1:
G (s)
Khối driver-động cơ 2:
G (s)
ĐỒ ÁN THIẾT KẾ HỆ THỐNG CƠ ĐIỆN TỬ GVHD: TS. Phùng Trí Cơng. Thiết kế bộ điều khiển PID cho khối driver và động cơ:
Hình 4.18 Sơ đồ khối hệ thống driver và động cơ Tiêu chí bộ điều khiển:
+ Settling time: Ts < 0.1s + Steady state error: ess = 5% + Overshoot <10%
Sử dụng PID tuner trong matlab ta tìm đc các hệ số của bộ điều khiển PID như sau: • Khối driver-động cơ 1
Mơ phỏng matlab với các hệ số PID thỏa tiểu chí đề ra là: Kp =0.0093231,Ki = 0.21262, Kd = 1.3554.10-6
Hình 4.19 Đáp ứng của động cơ 1 sau khi sử dụng bộ điều khiển pid
NHÓM 3
ĐỒ ÁN THIẾT KẾ HỆ THỐNG CƠ ĐIỆN TỬ GVHD: TS. Phùng Trí Cơng. Đáp ứng mơ phỏng là:
Settling time: Ts =0.0908 Steady state error: ess =0 Overshoot =8.8%
•
Khối driver-động cơ 2
Kp =0.010758; Ki = 0.23514; Kd = 1.1824.10-6
Hình 4.20 Đáp ứng của động cơ 2 sau khi sử dụng bộ điều khiển pid Đáp ứng mô phỏng là:
+Settling time: Ts =0.0843 +Steady state error: ess =0 +Overshoot =6.58%
4.4 Nguồn cấp:
Yêu cầu:
Áp của pin phải lớn hơn hoặc bằng áp lớn nhất của thiết bị trong hệ thống (động cơ GA25 V1 -12V)
NHÓM 3
ĐỒ ÁN THIẾT KẾ HỆ THỐNG CƠ ĐIỆN TỬ GVHD: TS. Phùng Trí Cơng.
- Tính tốn cường độ dịng điện cần cung cấp: Bảng 4.5 Thiết bị Động cơ Cảm biến Driver L298 PIC 18F4550 Tổng Cộng
Từ yêu cầu kĩ thuật và nguồn điện cần cung cấp, mắc nối tiếp 4 pin sạc 18650 Pananasonic 3,7V,3,7A .
-Về điện áp tiêu thụ, dùng 2 mạch giảm áp DC LM2596 để tạo ra áp 12V cấp vào driver L298 để điều khiển động cơ.
Nguồn 5V được sử dụng lấy từ Driver L298.
Nguồn 3.3 V cấp cho encoder được sử dụng từ kit pic.
NHÓM 3
ĐỒ ÁN THIẾT KẾ HỆ THỐNG CƠ ĐIỆN TỬ GVHD: TS. Phùng Trí Cơng.
CHƯƠNG 5 : MƠ HÌNH HĨA 5.1. Mơ hình động học robot:
Để thực hiện việc điều khiển cho xe bám line tốt hơn, ta tiến hành thiết lập mơ hình động học của hệ thống. Mơ hình này bao gồm các điểm quan trọng: Điểm P: điểm tham chiếu cho robot; Điểm A: trung điểm của hai bánh chủ động; Điểm C: Điểm tracking của robot. Mơ hình này được thể hiện bên dưới.
Hình 5.1 Mơ hình động học của mobile robot
Chọn hệ trục tọa độ tuyệt đối là 0 0 và hệ trục tương đối gắn với tọa độ tâm vận tốc tức thời I trên hệ trục 0 0 :
x I x D sin y I y D cos
NHÓM 3
ĐỒ ÁN THIẾT KẾ HỆ THỐNG CƠ ĐIỆN TỬ GVHD: TS. Phùng Trí Cơng.
w
Với D là khoảng cách từ tâm vận tốc tức thời đến tâm hai bánh xe
VL,R lần lượt là vận tốc của bánh trái và bánh phải của Mobile Platform Ta có vận tốc của mobile robot:
Ta có phương trình động học của Mobile robot :
x
Với v là vận tốc dài, là vận tốc góc. Ta biểu diễn phương trình trên theo vận tốc
của hai bánh xe
x
Với L là khoảng cách giữa hai bánh xe.
Tại điểm C( , ) tọa độ trackingpoint, ta có:
xC x a cos Phương trình động học tại C: xc ' x ' a sin . yc' x ' a cos . ' ' c ' ' NHÓM 3
ĐỒ ÁN THIẾT KẾ HỆ THỐNG CƠ ĐIỆN TỬ GVHD: TS. Phùng Trí Cơng. Với a là khoảng cách từ tâm đường nối 2 bánh xe đến điểm tracking
Tại điểm P( , ) – điểm mong muốn của điểm C trên đường line, ta có :
x
P
y P ' vP sin P
Ta tìm được phương trình động học cho sự sai lệch giữa vị trí tâm xe và vị trí của điểm mong muốn trên đường line:
x P y
P P
Với 1, 2 , 3 lần lượt là sai số về vị trí theo tọa độ x, y và góc di chuyển trên hệ trục tọa độ tương đối gắn với mobile platform của trọng tâm C với điểm mong muốn của nó trên đường line.
Suy ra : e1 e sin 2 e 3
Tiếp theo ta xác định các sai số để đưa vào bộ điều khiển:
Hình 5.2 Mơ hình xác định sai số trong việc mơ phỏng
NHĨM 3
ĐỒ ÁN THIẾT KẾ HỆ THỐNG CƠ ĐIỆN TỬ GVHD: TS. Phùng Trí Cơng. Để xe bám theo được line, cơng việc đầu tiên là xác định vị trí của xe so với
line. Ta đặt hệ toạ độ như hình 5.2.2, trong đó M là trung điểm giữa 2 bánh xe, C là trung điểm của dãy sensor. Từ đó ta có sai số 1 bị triệt tiêu nên khơng cần xét đến. Mục đích của giải thuật điều khiển là làm cho điểm C bám theo điểm tham chiếu P. Để làm điều đó, ta cần xác định các sai số 2 và 3. Trên thực tế, sai số 2 được đo từ dãy sensor nên ta chỉ cần tính tốn sai số 3 . Để tính sai số này, ta cho xe di chuyển theo phương trước đó của xe một đoạn sao cho ds đủ nhỏ để khi nối 2 điểm R và R’ ta được phương tiếp tuyến với đường cong.
Khi đó ta có cơng thức xác định 3:
e arctan e2 e2'
3d
s
Ta thấy : tại một vị trí đường line, ta sẽ xác định được giá trị góc giữa đường tâm xe so với trục ngang Ox. Từ mối quan hệ giữa tọa độ tâm hai bánh xe, tọa độ tâm cảm biến và góc giữa đường tâm xe so với trục ngang ta sẽ xác định được phương trình đường thẳng đi qua các cảm biến – được dùng trong mô phỏng để xác định sai số 2.
Dựa theo phần trình bày trên, ta sẽ tìm được các giá trị sai số đưa vào bộ điều khiển vị trí.
Hình 5.3 Sơ đồ khối hệ thống.
v vr cos e1 k1e1
Bộ điều khiển tracking:
k 2 vr e2 r k3 sin e3
NHÓM 3
ĐỒ ÁN THIẾT KẾ HỆ THỐNG CƠ ĐIỆN TỬ GVHD: TS. Phùng Trí Cơng.
5.2 Mơ phỏng tìm khoảng cách d:
Bảng 5.1 Thơng số đầu vào mơ phỏng tìm khoảng cách d Đại lượng
Bán kính cong Vận tốc lớn nhất
Khoảng cách hai bánh xe Đường kính bánh xe Thời gian lấy mẫu
Thời gian di chuyển đoạn nhỏ tìm e2’ Chiều dài cụm sensor
Dựa vào các thông số trên, với khoảng cách d thay đổi , ứng với mỗi giá trị của d chọn bộ hệ số k sao cho sai số max là nhỏ nhất . Kết quả mô phỏng như hình :
Hình 5.4 Kết quả mơ phỏng sai số max ứng với các giá trị khoảng cách
Theo kết quả mô phỏng ta thấy với d nằm trong khoảng từ 50 đến 80 thì sai số max là nhỏ nhất. Sai số tăng dần khi vượt qua khoảng này. Do đó nhóm chọn d= 51 mm vì với giá trị này, xe chạy hết đường line.
NHÓM 3
ĐỒ ÁN THIẾT KẾ HỆ THỐNG CƠ ĐIỆN TỬ GVHD: TS. Phùng Trí Cơng.
Hình 5.5 Kết quả bám line khoảng cách dược chọn là 51 mm
5.3 Mô phỏng sa bàn:
Quy ước : đường vẽ màu xanh lá thể hiện vận tốc gốc bánh xe phải, đường màu đỏ thể hiện vận tốc gốc bánh xe trái.
Với khoảng cách từ tâm cảm biến C đến tâm 2 bánh chủ động M được chọn là 51 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à [1 600 0].
5.3.1 Kết quả bám line ở đoạn A-B-C-D:
Hình 5.6 Sa bàn di chuyển đoạn A-B-C-DNHÓM 3 NHÓM 3
ĐỒ ÁN THIẾT KẾ HỆ THỐNG CƠ ĐIỆN TỬ GVHD: TS. Phùng Trí Cơng.
Hình 5.7 Đồ thị sai số trong quá trình di chuyển
Hình 5.8 Vận tốc quay hai bánh trên đoạn A-B-C-D
Hình 5.9 Vận tốc dài robot trên đoạn A-B-C-D
NHÓM 3
ĐỒ ÁN THIẾT KẾ HỆ THỐNG CƠ ĐIỆN TỬ GVHD: TS. Phùng Trí Cơng. Trong đoạn A-B-C-D, sai số lớn e2 lớn nhất là khúc giao tại B, tại đây có sự chuyển hướng đột ngột dẫn đến sai số lớn, tuy nhiên sai số trong đoạn này là 20 mm ,nhỏ hơn sai số yêu cầu của đề bài. Trên đoạn A-B và B-D sai số tương đối ổn định (Hình 5.7). Nhận thấy trong q trình chạy, tốc độ góc 2 bánh xe khơng vượt quá giới hạn cho phép của động cơ.
5.3.2 Đoạn D-E-F-C
a.Sa bàn di chuyển đoạn D-E-F-C b. Sai số e2, e3 Hình 5.10 Mơ phỏng bám line đoạn D-E-F-C và sai số e2 ,e3
Hình 5.11 Vận tốc 2 bánh xe trên đoạn D-E-F-C
NHÓM 3
ĐỒ ÁN THIẾT KẾ HỆ THỐNG CƠ ĐIỆN TỬ GVHD: TS. Phùng Trí Cơng.
Hình 5.12 Vận tốc dài robot trên đoạn D-E-F-C
Trong đoạn D-E-F-C, sai số lớn e2 lớn nhất là khúc giao tại G với giá trị 13 mm, tại đây có sự chuyển hướng đột ngột từ line cong sang line thẳng dẫn đến sai số lớn. Trên đoạn D-E-F và F-C sai số tương đối ổn định (Hình 5.10 b).
5.3.3 Đoạn C-G-A
a.Sa bàn di chuyển đoạn C-G-A b. Sai số e2 e3
Hình 5.13 Mơ phỏng bám line đoạn C-G-A và sai số e2 ,e3
NHÓM 3
ĐỒ ÁN THIẾT KẾ HỆ THỐNG CƠ ĐIỆN TỬ GVHD: TS. Phùng Trí Cơng.
Hình 5.14 Vận tốc hai bánh trên đoạn C-G-A
Hình 5.15 Vận tốc xe trên đoạn C-G-A
Các thơng số mô phỏng về sự chuyển động của sensor cảm biến, sai số e2, e3, vận tốc xe và số vòng quay của bánh xe trong quá trình chuyển động đoạn C-G-A được mơ tả trong hình 5.13 đến 5.15
Trong đoạn C-G-A, sai số lớn e2 lớn nhất là khúc giao tại G với giá trị -17 mm, tại đây có sự chuyển hướng đột ngột dẫn đến sai số lớn. Trên đoạn C-G và G-A sai số tương đối ổn định (Hình 5.13 b).
NHĨM 3
ĐỒ ÁN THIẾT KẾ HỆ THỐNG CƠ ĐIỆN TỬ GVHD: TS. Phùng Trí Cơng.
5.3.3 Đoạn G-A-C-E
a.Sa bàn di chuyển đoạn G-A-C-E b. Sai số e2, e3
Hình 5.16 Mơ phỏng bám line đoạn G-A-C-E và sai số e2 ,e3
Hình 5.17 Vận tốc hai bánh trên đoạn G-A-C-E
NHÓM 3
ĐỒ ÁN THIẾT KẾ HỆ THỐNG CƠ ĐIỆN TỬ GVHD: TS. Phùng Trí Cơng.
Hình 5.18 Vận tốc xe trên đoạn G-A-C-E
Các thơng số mô phỏng về sự chuyển động của sensor cảm biến, sai số e2, e3, vận tốc xe và số vịng quay của bánh xe trong q trình chuyển động đoạn G-A-C-E được mơ tả trong Hình 5.4.4.
Trong đoạn G-A-C-E , có khúc cua 900 làm cho sai số e2 tại vị trí này khá lớn -23 mm. Tuy nhiên sai số này là có thể chấp nhận trong q trình chạy.
Trong quá trình di chuyển, vận tốc xe thay đổi (Hình 5.18 ) và tại vị trí bẻ cua 900 vận tốc xe giảm nhiều nhất, đây cũng là điều cần lưu ý khi áp dụng giải thuật cho mơ hình xe thật.
NHĨM 3
ĐỒ ÁN THIẾT KẾ HỆ THỐNG CƠ ĐIỆN TỬ GVHD: TS. Phùng Trí Cơng.
CHƯƠNG 6 : BỘ ĐIỀU KHIỂN. 6.1 Lựa chọn vi điều khiển:
Giải thuật điều khiển tập trung sử dụng 1 con vi điều khiển đảm nhận tất cả các chức năng, bao gồm: đọc và xử lí tín hiệu từ cảm biến , điều khiển 2 động cơ thông qua driver và đọc giá trị encoder.
Đặt ra yều cầu vi điều khiển đáp ứng được nhu cầu đặt ra cho robot, cụ thể:
Có 7 kênh đọc giá trị analog, có 2 kênh điều khiển độ rộng xung PWM và có thể đọc tín hiệu từ encoder tới tần số cao.
Nhóm quyết định chọn vi điều khiển Pic 18F4550, thỏa mãn tất cả các yêu cầu trên.
Hình 6.1 PIC 18F4550
6.2 Giải thuật điều khiển:
6.2.1 Chương trình điều khiển chính:
NHĨM 3
ĐỒ ÁN THIẾT KẾ HỆ THỐNG CƠ ĐIỆN TỬ GVHD: TS. Phùng Trí Cơng.
Hình 6.2 Chương trình điều khiển chính
NHĨM 3
ĐỒ ÁN THIẾT KẾ HỆ THỐNG CƠ ĐIỆN TỬ GVHD: TS. Phùng Trí Cơng.
6.2.2 Chương trình con:
Hình 6.3 Chương trình con điều khiển
NHÓM 3
ĐỒ ÁN THIẾT KẾ HỆ THỐNG CƠ ĐIỆN TỬ GVHD: TS. Phùng Trí Cơng.
CHƯƠNG 7 : THỰC NGHIỆM VÀ KẾT QUẢ 7.1 Hình ảnh xe thực tế:
Hình 7.1 Ảnh xe thực tế 1.
Hình 7.2 Ảnh xe thực tế 2.
NHĨM 3
ĐỒ ÁN THIẾT KẾ HỆ THỐNG CƠ ĐIỆN TỬ GVHD: TS. Phùng Trí Cơng.
7.2 Kết quả thực nghiệm bám line:
Hình 7.3 Ảnh xe bám line 1-ơm cua tại vị trí G.
Hình 7.4 Ảnh xe bám line 2- tới vị trí B.
NHÓM 3
ĐỒ ÁN THIẾT KẾ HỆ THỐNG CƠ ĐIỆN TỬ GVHD: TS. Phùng Trí Cơng.
7.3 Nhận xét:
Kết quả chạy mơ phỏng và thực tế của của xe theo vR = 0,9 m/s trên đường line được thể hiện trên Hình 7.3, Hình 7.4 và Hình 7.5. Ba hình đều thể hiện dạng bám line của xe giống nhau: trên các đoạn đường thẳng (B→D, F→G, A→E), 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