Bộ điều khiển Cơ cấu robot Cảm biến qd
q
Hình 2.6. Sơ đồ khối tổng quát của hệ thống điều khiển phản hồi
Trong sơ đồ trên, q là vectơ tín hiệu đặt vị trí của các khớp (q = d d
d
đối với khớp quay và q = r đối với khớp tịnh tiến), q là vectơ vị trí thực của các d d
khớp robot tương ứng là với khớp quay và r với khớp tịnh tiến, là vectơ mômen đối với khớp quay và lực đối với khớp tịnh tiến.
Phương trình động lực học tổng quát của robot có dạng: τ = M(q)q + V(q,q) + G(q)&& &
(2- 4a)
hoặc
1
τ = M(q)q + M(q) + S(q,q) q + G(q) 2 &
&& & &
(2- 4b) trong đó q = [q1, q2, …, q ]nT Rn , τ R n ; M(q) Rnxn là ma trận dương đối xứng; V(q,q) R& n , G(q) Rn , S(q,q) R& nxn là ma trận đối xứng ngược.
Từ chương này, ta sẽ dùng q thay cho và q thay cho i
i
để kí hiệu cho các biến khớp nhằm làm tiện lợi hơn trong việc lập trình và mơ phỏng với Matlab và Simulink.
Luật điều khiển có cấu trúc dạng tỷ lệ - đạo hàm (PD):
+) τ = K e - K q + G(q)p d& (2-5) +) τ = K e + K e + G(q)p d& (2-6) trong đó:
Kp = diag(kp1, kp2, …, kpn) là ma trận đường chéo các hệ số khuếch đại của từng khớp riêng biệt.
Kd = diag(kd1, kd2, …, k ) là ma trận đường chéo các hệ số đạo hàm dn
của từng khớp riêng biệt. e = q - qd
là sai số vị trí của khớp robot, với q = [q , q , …, qd d1 d2 dn]T
Rn
là vectơ vị trí đặt của các khớp robot. e = q - q
& &d &
là sai số tốc độ khớp robot.
Hệ thống điều khiển với cấu trúc điều khiển (2-5), (2-6) đã được chứng minh là ổn định tuyệt đối xung quanh điểm cân bằng, không phụ thuộc vào khối lượng thanh nối và tải dựa vào lý thuyết ổn định Lyapunov. Thật vậy, sau đây ta sẽ trình bày cách chứng minh đối với luật điều khiển (2-5).
Đặt biến trạng thái của hệ thống là: &
T T T [e , q ] . Chọn hàm Lyapunov có dạng: VL = 1 [e K e + q M(q)q] 2 T &T & p (2-7)
Hàm V biểu thị tổng năng lượng của hệ thống robot: thành phần L
1 e K e 2
T p p
biểu thị thế năng tích lũy trong hệ thống có hệ số tỷ lệ là K , thành phần p
1
q M(q)q 2& &
T
là động năng robot.
Do K , M(q) là các ma trận đối xứng dương nên V > 0 với p L
e,q 0&
Tính đạo hàm cấp 1 của V ta được:L
1 T 1 1 1 1
V = e K e + e K e + q M(q)q + q M(q)q + q M(q)qL p p
2 2 2 2 2
& & T & &&T & &T& & &T &&
(2-8)
Do q là hằng số nên d
& &
e q
.
Vì K , M(q) là các ma trận đối xứng dương nên:p
e K e = e K e &T T &
p p
và
q M(q)q = q M(q)q
&&T & &T &&
T T T
L p
1
V = -q K e + q M(q)q + q M(q)q2 2
& & & & & & &&
(2-9) Cân bằng phương trình luật điều khiển (2-5) và phương trình động lực học robot (2-4b):
1
K e - K q + G(q) = M(q)q + M(q) + S(q,q) q + G(q) 2 &
& && & &
p d
1
M(q)q = K e - K q - M(q) + S(q,q) q 2 &
&& p d& & &
(2-10) Thay (2-9) vào (2-10) được:
1 1
V = -q K e + q M(q)q + q K e - K q - M(q) + S(q,q) q
2 2
& &T &T& & &T & & & &
L p p d
V = -q K q + q S(q,q)q& &T & &T & &
L d
(2-11)
Vì S(q,q&) là ma trận đối xứng ngược nên
q S(q,q)q = 0&T & &
do đó (2-11) trở thành:
V = -q K q 0& &T &
L d
(2-12)
Từ (2-7) và (2-12), theo tiêu chuẩn ổn định Lyapunov, ta có &
q 0
khi t . Như vậy ở trạng thái xác lập, các thành phần tốc độ và gia tốc đều bằng 0 do đó thay vào phương trình hệ thống kín (2-10) sẽ có:
p d
K e 0 e 0 q q