CHƯƠNG IV: MÔ PHỎNG HOẠT ĐỘNG CỦA HỆ THỐNG BÀN MÁY

START

Nhập xd và yd

Hàm quỹ đạo đặt

X = x(t); Y = y(t)

Bộ điều khiển

S
Quỹ đạo thực với
sai số cho phép

END
Hình 19: Lưu đồ thuật toán
Một hệ điều khiển phản hồi vòng kín có dạng như sau:
 Hình 20: Hệ điều khiển phản hồi vòng kín
Các tín hiều phải hồi về bộ điều khiển sẽ được dùng để tính toán các sai lệch và
điều chỉnh các tín hiệu điều khiển để các sai lệch đó thỏa mãn các điều kiện về độ quá
điều chỉnh, thời gian quá độ.
Cấu trúc điều khiển PID có dạng:
𝑡
Mdk = Kp (qd – q) + Kd (q̇ d - q̇ ) + Ki ∫0 ( qd (𝜏) – q(𝜏) ) 𝑑𝜏

4.1. Gia công theo đường thẳng

Thiết kế quỹ đạo chuyển động với di chuyển là đa thức bậc 3 theo thời gian.
Ta có phương trình đường thẳng đi qua 2 điểm A (X0 ,Y0); B (Xe ,Ye)
𝑋 − 𝑋0 𝑌 − 𝑌0
=
𝑋𝑒 − 𝑋0 𝑌𝑒 − 𝑌0
Với X, Y là tọa độ điểm đầu dao
𝑌𝑒 −𝑌0 𝑌0 𝑋𝑒 −𝑌𝑒 𝑋0
 Phương trình đường thẳng AB: 𝑌 = 𝑋 +
𝑋𝑒 −𝑋0 𝑋𝑒 −𝑋0

Để thỏa mãn điều kiện quỹ đạo và vận tốc, ta thiết lập quan hệ X=x(t) là đa thức
bậc 3 theo thời gian: X=a0 + a1t + a2t2 + a3t3
Ta có điều kiện:
 𝑆(0) = 𝐴(𝑋0 , 𝑌0 )
𝑣(0) = 𝑣0 = 0
𝑆(𝑡𝑒 ) = 𝐵(𝑋𝑒 , 𝑌𝑒 )
𝑣(𝑡𝑒 ) = 𝑣𝑒 = 0
{
Trong đó:

S(0) : Tọa độ ban đầu với t=0

V(0) : Vận tốc ban đầu với t=0
S(te) : Tọa độ điểm cuối
V(te): Vận tốc tại điểm cuối
Ta lại có: Y = a.X+b
𝑋(0) = 𝑥0
𝑉𝑋(0) = 0
Suy ra X=x(t) cũng có điều kiện như quỹ đạo vậy (*)
𝑋(𝑡𝑒) = 𝑥𝑒
{ 𝑉𝑋(𝑡𝑒) = 0
Trong đó:
X(0): tạo độ của đầu dao theo phương x tại thời điể ban đầu
VX(0): vận tốc đầu dao theo phương x tại thời điểm ban đầu
X(te): Tọa độ đầu dao theo phương x tại thời điểm cuối
VX(te): vận tốc đầu dao theo phương x tại thời điểm cuối

𝑋0 = 𝑎0 + 𝑎1 𝑡0 + 𝑎2 𝑡0 2 + 𝑎3 𝑡0 3
0 = 𝑎1 + 2𝑎2 𝑡0 + 3𝑎3 𝑡0 2
Phương trình (*) tương đương:
𝑋𝑒 = 𝑎0 + 𝑎1 𝑡𝑒 + 𝑎2 𝑡𝑒 2 + 𝑎3 𝑡𝑒 3
2
{ 0 = 𝑎1 + 2𝑎2 𝑡𝑒 + 3𝑎3 𝑡𝑒

𝑎0 = 𝑥0
𝑎1 = 0
3(𝑥𝑒 −𝑥0 )
Giải hệ phương trình 4 ẩn: a0,a1,a2,a3 ta được: 𝑎2 =
𝑡𝑒 2
−2(𝑥𝑒 −𝑥0 )
{𝑎3 = 𝑡𝑒 3

𝑌𝑒 −𝑌0 𝑌0 𝑋𝑒 −𝑌𝑒 𝑋0
Suy ra: 𝑌 = 𝑋(𝑡) +
𝑋𝑒 −𝑋0 𝑋𝑒 −𝑋0
 Khi chạy chương trình ta được quỹ đạo đồ thị chuyển động của bàn máy và đồ thị
sai số quỹ đạo:

Hình 21a: Quỹ đạo thực Hình 21b: Quỹ đạo mong muốn

Hình 22a: Sai số tọa độ bàn X Hình 22b: Sai số tọa độ bàn Y
Kết luận: Từ kết quả mô phỏng ta thấy kết quả mô phỏng và quỹ đạo mong muốn
không sai lệch nhiều chứng tỏ bộ điều khiển PID đáp ứng được yêu cầy bài toán
4.2. Gia công theo đường tròn
 Ta có phương trình đường tròn đi qua 2 điểm A (X0,Y0), B (Xe,Ye) nhận AB làm
đường kính có phương trình [(x – xi )2 + (y - yi)2 = R2]
𝑋0 +𝑋𝑒 𝑌0 +𝑌𝑒
Với I (xi,yi) = I ( , ) là trung điểm của AB
2 2

1
𝑅= √𝑋𝑒 2 − 2𝑋𝑒 𝑋0 + 𝑋0 2 + 𝑌𝑒 2 − 2𝑌𝑒 𝑌0 + 𝑌0 2
2
Ta viết phương trình đường tròn dưới dạng tham số như sau:
𝑥 = 𝑥𝑖 + 𝑅𝑠𝑖𝑛(𝑎(𝑡))
{ (*)
𝑦 = 𝑦𝑖 + 𝑅𝑐𝑜𝑠(𝑎(𝑡))
Cũng để thỏa mãn điều kiện về quỹ đạo và vận tốc quỹ đạo đặt cũng phải là bậc 3:
𝑋(𝑡) = 𝑎0 + 𝑎1 𝑡 + 𝑎2 𝑡 2 + 𝑎3 𝑡 3
𝑆(0) = 𝐴(𝑥0 , 𝑦0 )
𝑉(0) = 0
Và phải thỏa mãn điều kiện: {
𝑆(𝑡𝑒 ) = 𝐵(𝑥𝑒 , 𝑦𝑒 )
𝑉(𝑡𝑒 ) = 0
Trong đó:
S(0): tọa độ ban đầu ở thời điểm t=0
V(0): vận tốc ban đầu tại thời điểm t=0
S(te): tọa đọ điểm cuối tại thời điểm te
V(te): vận tốc điểm cuối tại thời điểm te
𝑋(0) = 𝑥0
𝑉𝑥(0) = 0
Suy ra:
𝑋(𝑡𝑒 ) = 𝑥𝑒
{ 𝑉𝑥(𝑡𝑒) = 0
Trong đó:
X(0): tọa độ đầu dao theo phương x tại thời điểm ban đầu
VX(0): vận tốc đầu dao theo phương x tại thời điểm ban đầu
X(te): tọa độ đầu dao theo phương x tại thời điểm cuối
VX(te): tọa độ đầu dao theo phương x tại thời điểm cuối
Phương trinh (*) tương đương:
 𝑥0 = 𝑥𝑖 + 𝑅𝑠𝑖𝑛(𝑎0 + 𝑎1 𝑡0 + 𝑎2 𝑡0 2 + 𝑎3 𝑡0 3
0 = (𝑎1 + 2𝑎2 𝑡0 + 3𝑎3 𝑡0 2 )𝑅𝑐𝑜𝑠(𝑎0 + 𝑎1 𝑡0 + 𝑎2 𝑡0 2 + 𝑎3 𝑡0 3 )
𝑥𝑒 = 𝑥𝑖 + 𝑅𝑠𝑖𝑛(𝑎0 + 𝑎1 𝑡𝑒 + 𝑎2 𝑡𝑒 2 + 𝑎3 𝑡𝑒 3 )
2 2 3
{ 0 = (𝑎1 + 2𝑎2 𝑡𝑒 + 3𝑎3 𝑡𝑒 )𝑅𝑐𝑜𝑠(𝑎0 + 𝑎1 𝑡𝑒 + 𝑎2 𝑡𝑒 + 𝑎3 𝑡𝑒
Giải phương trình 4 ẩn: a0 , a1 , a2 , a3 ta được:
𝑥0 −𝑥𝑖
a1 = 0 ; 𝑎0 = 𝑎𝑟𝑐𝑠𝑖𝑛 ( )
𝑅

𝑥𝑒 −𝑥𝑖 𝑥0 −𝑥𝑖
Đặt 𝑤 = 𝑎𝑟𝑐𝑠𝑖𝑛 ( ) − 𝑎𝑟𝑐𝑠𝑖𝑛 ( )
𝑅 𝑅
−2𝑤 −3𝑤
Suy ra: 𝑎3 = , 𝑎2 =
𝑡𝑒 3 𝑡𝑒 2

Khi chạy chương trình ta được quỹ đạo đồ thị chuyển động của bàn máy và đồ thị
sai số quỹ đạo:

Hình 23a: Quỹ đạo thực Hình 23b: Quỹ đạo mong muốn
 Hình 24: Đồ thị sai số
4.3. Gia công theo đường vuông góc
Khi chạy chương trình ta được quỹ đạo đồ thị chuyển động của bàn máy và đồ thị
sai số quỹ đạo:
Ta thấy sai số gia công trong khoảng (0,1.10-4  0,9.10-4)

Hình 25: Đồ thị gia công vuông góc

Hình 26:Sai số gia công vuông góc