Do điều kiện thực nghiệm thực tế giá trị sai lệch εx camera đo được chỉ là tương đối nên tác giả chỉ dùng giá trị này để làm căn cứ xác định robot đã tiến gần tới đích chưa mà không áp dụng luật điều khiển cho sai lệch này.
Sử dụng bộ điều khiển PID cho sai lệch εy và bộ điều khiển này có dạng sau: uy = kp*e1 +kd*(de1/dt) + ki*∫ 𝒆𝟏𝒅𝒕
Trong đó:
uy - là tín hiệu đầu ra điều khiển.
kp, kd, ki tương ứng là các hằng số tỷ lệ, vi phân và tích phân của bộ điều khiển. e1 là sai số cần điều khiển tức là độ lệch của đích so với tâm robot (Sai số e1 được xác định từ bộ xử lý ảnh, chính là giá trị tọa độ tương đối giữa đích và robot εy như hình 4.38 ) . (4.1) εy εx o Hướng di chuyển của robot
84
Từ đầu ra của bộ điều khiển PID trong (4.1) ta sẽ xác định giá trị PWM điều khiển động cơ hai bánh theo hai cặp phương trình tương ứng sau tùy vào dấu của sai lệch εy:
Nếu sai lệch e1 = εy có dấu dương:
PWM1= PWM0 – uy. PWM2= PWM0 + uy. Nếu sai lệch e1 = εy có dấu âm:
PWM1= PWM0 + uy. PWM2= PWM0 - uy.
Công thức của bộ điều khiển PID trình bày trong (4.1) là dạng hàm liên tục của biến e1, trong đó có cả thành phần tuyến tính, đạo hàm và tích phân. Tuy nhiên, hệ thống vi điều khiển lại là hệ thống số do đó muốn xây dựng bộ điều khiển PID cho vi điều khiển chúng ta phải tiến hành xấp xỉ phương trình liên tục trên thành phương trình dạng rời rạc.
Vì bộ điều khiển PID xây dựng trong ARM sẽ là bộ điều khiển số, chúng ta cần xấp xỉ công thức của bộ điều khiển này theo các khoảng thời gian rời rạc. Trước hết, thành phần P tương đối đơn giản vì đó là quan hệ tuyến tính Kp*e1, chúng ta chỉ cần áp dụng trực tiếp công thức này mà không cần bất kỳ xấp xỉ nào. Tiếp đến là xấp xỉ
cho đạo hàm của biến e1. Vì thời gian lấy mẫu cho các bộ điều khiển thường rất bé
nên có thể xấp xỉ đạo hàm bằng sự thay đổi của e1 trong 2 lần lấy mẫu liên tiếp: de1/dt ≈ (e1(k)–e1(k-1))/h.
Trong đó e1(k) là giá trị hiện tại của e1, e1(k-1) là giá trị của e1 trong lần lấy mẫu trước đó và h là khoảng thời gian lấy mẫu (h là hằng số) cách lấy xấp xỉ đạo hàm của biến sai số e1 được trình bày trong hình 4.38.
85