bài tập lớn hệ thống điều khiển đa biến điều khiển vị trí tay máy hai bậc tự do

CHƯƠNG 2 : TÍNH ỔN ĐỊNH VÀ CHẤT LƯỢNG HỆ THỐNG ĐABIẾN...2.1 Tính điều khiển được của hệ tuyến tính đa biến.... 3.3 Thiết kế bộ điều khiển gán điểm cực sử dụng phản hồi tín hiệura.... 3.4

TRƯỜNG ĐẠI HỌC GIAO THÔNG VẬN TẢI

KHOA CƠ KHÍBỘ MÔN CƠ ĐIỆN TỬ

: NGUYỄN VĂN TOÀN : NGUYỄN KHÁNH DUY : NGUYỄN XUÂN QUYẾT

CHƯƠNG 2 : TÍNH ỔN ĐỊNH VÀ CHẤT LƯỢNG HỆ THỐNG ĐABIẾN

2.1 Tính điều khiển được của hệ tuyến tính đa biến

2.2 Tính quan sát được của hệ tuyến tính đa biến

2.3 điểm cực (poles) và điểm không (zeros) của hệ đa biến

CHƯƠNG 3 : THIẾT KẾ BỘ ĐIỀU KHIỂN

3.1 Thiết kế bộ điều khiển gán điểm cực

3.2 Bộ quan sát trạng thái Luenbergen

3.3 Thiết kế bộ điều khiển gán điểm cực sử dụng phản hồi tín hiệura

3.4 thiết kế bộ điều khiển toàn phương tuyến tính

CHƯƠNG 1 ĐỘNG LỰC HỌC VÀ PHƯƠNG TRÌNH TRẠNG THÁI1.1 Thông số vật lý của cánh tay mô phỏng

Hình 1.1 Mô hình cánh tay Robot m , m khối lượng của hai khớp(m =1kg, m = 2 kg)12 12

1.2 Thành lập phương trình động lực học và phương trình trạng thái

- Vị trí của m1:[x1

y1]=[L1cosθ1L1sinθ1] (1.1) vận tốc của m1

[˙x1

˙y1]=[−L1sinθ1

L1cosθ1] 1 (1.2) - Vị trí của m2:

[x2

y2]=[[L1cosθ1+ L2cos(θ1+θ2)

L1sinθ1+ L2sin(θ1+θ2)]] (1.3)- vận tốc của m2:

[˙X2

˙Y2]=[−L1sinθ1−L2sin(θ1+θ2) −L2sin(θ1+θ2)

L1cosθ1+L2cos(θ1+θ2) L2cos(θ1+θ2)][˙θ1

˙θ2] (1.4)- Động năng của hệ (K=1

2mv2

)

K1=12m1v1=1

2m1( ˙x1+ ˙y1) = 12m1L1˙θ1 (1.5)

K2=12m2v2=1

2m2(˙x2+ ˙y2) (1.6) = 12m2¿) +22L1L2˙θ1(˙θ1+ ˙θ2)cosθ2

- Thế năng của hệ ( P = m.g.h)

P1=m1g L1sinθ1 (1.7)

P 2=m2g¿+L2sin(θ1+θ2) (1.8)

- Hàm lagrangeL = K-P (1.9) - Thay (1.5),(1.6),(1.7),(1.8) vào (1.9) ta được :

L=12m1L1˙θ1+1

2m2¿)2+2L1L2cosθ2˙θ1(˙θ1+ ˙θ2)−¿+L2sin(θ1+θ2)]) (1.10)- Phương trình động lực học của hệ cánh tay robot 2 bậc tự do:

Fi=d

dt

∂ L∂ ˙θi−∂ L

∂θi

,i=1,2 + TÍNH F1 :

F1 =dtd(∂ L∂ ˙θ 1)−∂ L

∂ θ 1

Trong đó :

∂ L∂ ˙θ 1 = (m + m ).(l121) 2˙θ1 + m2.(l (2)2 ˙θ1+ ˙θ2¿ +m2.l l cos1 2 θ2 (2 ˙θ1+ ˙θ2¿

∂ L∂θ 1 = -m1.l g.cos1 θ1- m g ( l cos21 θ1 + l cos(2 θ1+θ2))

ddt(∂ L

-+[ m1.l g.cos1 θ1+ m g ( l cos21 θ1 + l cos(2 θ1+θ2

))]

+ TÍNH F2 :

F2 =dtd(∂ L∂ ˙θ 2)−∂ L

∂ θ 2

Trong đó :

∂ L∂ ˙θ 2 = [m (l22)2.(˙θ1+ ˙θ2) + m2.l l cos1 2 θ2.1]

∂ L∂θ 2 = -m2.l l sin1 2 θ2.˙θ1(˙θ1+ ˙θ2)−¿m2.g.l cos(2 θ1+θ2)

ddt(∂ L

∂ ˙θ 2) = [m2.(l2)2 + m

2.l l cos1 2 θ2].θ¨1 + m2.(l 2)2θ¨2 -m2.l l 1 2sinθ2.˙θ1.2

F2= [m 2.(l )2 + m2.l l cos1 2 θ2].¨θ1 + m2.(l ) 2 ¨θ2 -m2.l l 1 2sinθ2.˙θ1.˙θ2 +[m l l sin2 1 2 θ2.˙θ1( ˙θ1+ ˙θ2)+¿m2.g.l cos(2 θ1+θ2)]

- Phương trình động lực học của robot 2 bậc tự do là:[F1

U2=(km 2

R2)V2=10

d1

'= 2m2.l l1 2 cosθ2θ¨1+m2.l1.l cos2 θ2θ¨2 -2.m2.l l sin1 2 θ2.1.˙θ2 –m2.l l1 2 sinθ2.(˙θ2)2

[ m1.l g.cos1 θ1+ m g ( l cos21 θ1 + l cos(2 θ1+θ2))]

d2

'

=¿m2.l l cos1 2 θ2.θ¨1 -m2.l l 1 2sinθ2.˙θ1.˙θ2 – [-m l l sin2 1 2 θ2.˙θ1(˙θ1+ ˙θ2)−¿m2.g.l cos(2 θ1+θ2)]Suy ra :

x2

x3

x4] + [0000][u1

u2]Thay các giá trị vào suy ra không gian trạng thái của hệ

01

θm 2] = [10000010][x1

x2x3

x4] + [0000][u1

u2]

CHƯƠNG 2 : TÍNH ỔN ĐỊNH VÀ CHẤT LƯỢNG HỆ THỐNG ĐA BIẾN2.1 Tính điều khiển được của hệ tuyến tính đa biến.

2.1.1 Tiêu chuẩn Kalman:

Hệ tuyến tính cho bởi phương trình trạng thái là điều khiển được khi và chỉ khi ma trận sau đây:

P(A,B) = [B|AB|….|An-1B]Có hạng bằng n , tức rank (P) = n.Hệ tuyến tính:

0−1,5][x1

x2x3

A=[0100

00

01

Hình 2.1 Kết quả tính ma trận và hạng ma trận P

Kết luận : Rank (P) = 4 = n vậy hệ trên điều khiển được

2.2 Tính quan sát được của hệ tuyến tính đa biến 2.2.1 Xác định tính quan sát được

Tính quan sát được (observable) của hệ tuyến tính bất biến là tính chất mà giá trịcủa các biến trạng thái có thể xác định được duy nhất từ giá trị đo được của tín hiệuvào và tín hiệu ra

Định nghĩa :hệ tuyến tính với mô hình trạng thái biểu diễn bởi phương trình trên.

Hệ này được gọi là quan sát được nếu với bất kỳ thời gian cuối t > 0 nào thì trạngfthái ban đầu x(0) có thể được xác định duy nhất từ giá trị theo thời gian của tín hiệuvào u(t) và đầu ra y(t) với 0 <= t <= t Ngược lại, hệ được gọi là không quan sátfđược

Các xác định :Hệ tuyến tính cho bởi phương trình là quán sát được khi và chỉ khi ma trận sau đây:

Có hạng bằng n, tức rank(L) = n.Mô hình trạng thái:

01

[θm 1θm 2] = [1000

0010][x1

x2

x3

x4] + [0000][u1

u2]Ta có :

A=[0100

00

01

2.2.2 Code matlab

clc;clear all;A= [0,1,0,0;0, -1,0,1;0,0,0,1;0,1,0, -1.5]C= [1,0,0,0;0,0,1,0]

L = [C; C*A; C*A*A; C*A*A*A]rank(L)

Hình 2.2 Xác định ma trận và hạng ma trận L

Kết Luận : Rank (L) = 4 vậy hệ trên là quan sát được

2.3 điểm cực (poles) và điểm không (zeros) của hệ đa biến.2.3.1 Điểm cực

Các điểm cực (poles) và điểm không(zeros) của hệ tuyến tính đa biến (A, B, C, D)dẽ được xác định với giả thiết biểu diễn (A, B, C, D) trên là tối thiểu

Định nghĩa: các điểm cực p của một hệ tuyến tính có biểu diễn tối thiểu (A, B, C,iD) là các trị riêng λ (A), i= 1,2,… n của ma trận A i Đa thức đặc trưng (đa thứcđiểm cực) được định nghĩa là :

Áp dụng định nghĩa về điểm cực ta đi tìm các trị riêng của ma trận A

A=[0100

00

01

0−1

( λ2

+2,5 λ+0,5)

Giải hệ :

λ1 = 0

Suy ra hệ không có điểm không bất biếnTìm hàm truyền

Code matlabclc;clear all;A = [0,1,0,0; 0, -1,0,1;0,0,0,1;0,1,0, -1.5]B = [0,0; 5, -5; 0,0; 5,15]

C = [1,0,0,0;0,0,1,0]D = [0,0;0,0]syms sI = [ 1,0,0,0;0,1,0,0;0,0,1,0;0,0,0,1]G= C*(s*I-A) ^ (-1) *B+Dsimplify(G)

pretty(ans)

Hình 2.3 Kết quả tìm hàm truyền

¿ = [ ] có rank lớn nhất là 4 ta xét định thức của tất cả các ma trận con kích thước 4x4 của ma trận ¿

det [ ] = (s*(2*s^3 + 5*s^2 + s))/2det [ ] = -s*(s + 1)

det [ ] = -sdet [ ] =0det [ ]=s^3 + (5*s^2)/2 + s/2

det[ ] =-1det [ ]=0det [ ] =s^3 + (5*s^2)/2 + s/2`

det[ ]=s + 3/2

det[ ] =(s*(2*s^2 + 5*s + 1))/2det [ ] =0

det [ ] =s + 1det [ ] =- s^2 - (5*s)/2 - 1/2det [ ] =1

Xét ma trận :[sI−A B] =[ ] có rank lớn nhất là 4det [ ] =(s*(2*s^3 + 5*s^2 + s))/2

det [ ] =5*s*(s^2 + 2*s)det [ ]=5*s*(3*s^2 + 4*s)det[] = - 15*s^2 - 20*sdet[ ]= - 5*s^2 - 10*sdet[ ]=0

det[ ]=(5*s*(2*s^2 + 5*s))/2det[ ]= (5*s*(2*s^2 + 9*s))/2det[ ]= -50*s^2

det[ ]=50*sdet[ ]=- 5*s^2 - (45*s)/2det[ ]=- 5*s^2 - (25*s)/2

det[ ]=50*sdet[ ]=-50det[ ]= 0vậy hệ không có điểm không hệ thốngCode tìm điểm cực và điểm không của hệclc;

clear all;A = [0,1,0,0;0, -1,0,1;0,0,0,1;0,1,0,-1.5]B = [0,0;5, -5;0,0;5,15]

C = [1,0,0,0;0,0,1,0]D = [0,0;0,0]sys=ss (A, B, C, D)zeros =zero(sys)poles = pole( sys )

CHƯƠNG 3 : THIẾT KẾ BỘ ĐIỀU KHIỂN3.1 Thiết kế bộ điều khiển gán điểm cực

Hệ tuyến tính được mô tả bởi phương trình:

˙X =Ax +Bu

Xây dựng bộ điều khiển gán điểm cực sử dụng phản hồi biến trạng thái Với bộ điều khiển gán điểm cực phản hồi trạng thái, tín hiệu điều khiển u được thiết kế như sau : u = −Kx

Với u = [u1 u2 u3 u4] là ma trận khuếch đại trạng thái cần xác định Các vecto các điểm cực mong muốn là : p = [-5,-10,-2+4j,-2-4j]

Giả sử cần điều khiển vị trí tay máy lệch ra khỏi vị trí ban đầu một góc pi/3

Code matlab

clc;clear all;A=[0,1,0,0;0,-1,0,1;0,0,0,1;0,1,0,-1.5]B=[0,0;5,-5;0,0;5,15]

P=[-5,-10,-2+4j,-2-4j]place(A,B,P)

Hình 3.1 Kết quả ma trận K

- Bộ điều khiển gán điểm cực

Hình 3.2 Mô phỏng bộ điều khiển gán điểm cực

Hình 3.3 Kết quả mô phỏng thetal 1

Hình 3.4 Kết quả mô phỏng thetal 2

3.2 Bộ quan sát trạng thái Luenbergen

Tín hiệu điều khiển: u=-K*x Công thức tính ma trận khuếch đại K : K=place(A',C',p) Các vecto điểm cực mong muốn : p=[-5,-10,-2+4j,-2-4j]

Code matlab

clc;clear all;A=[0,1,0,0;0,-1,0,1;0,0,0,1;0,1,0,-1.5]C=[1,0,0,0;0,0,1,0]

P=[-5,-10,-2+4j,-2-4j]place(A',C',P)

Hình 3.5 Kết quả ma trận khuêch đại

Vậy ma trận khuếch đại của bộ quan sát luenberger :

Kq = [11,85692,931

3.3 Thiết kế bộ điều khiển gán điểm cực sử dụng phản hồi tín hiệu ra

Giả sử cần điều khiển giữ cho tay máy ở vị trí cân bằng

Hình 3.6 Mô phỏng bô điều khiển gán điểm cực sử dụng phản hồi tín hiệu ra

Kết quả mô phỏng

Hình 3.7 kết quả mô phỏng góc thetal 1

Hình 3.8 Kết quả mô phỏng góc thetal 2

3.4 thiết kế bộ điều khiển toàn phương tuyến tính

Hình 3.9 Mô Phỏng bộ điều khiển toàn phương tuyến tính

Tín hiệu điều khiển thiết kế của bộ điều khiển toàn phương tuyến tính với mục tiêu cực tiểu hóa hàm chất lượng:

J=∫

0

∞(xT¿Qx+uT

Ru)dt¿

Trong đó Q và R là các ma trận định dương, khi đó tín hiệu điều khiển được chọn như sau:

u = -KxVới K là ma trận khuếch đại trạng thái xác định theo chỉ tiêu cực tiểu hóa hàm chấtlượng J :

Q=eye(4)R=eye(2)k=lqr(A,B,Q,R)

Hình 3.10 Kết quả ma trận khuêch đạiVậy ma trận khuếch đại của bộ điều khiển toàn phương tuyến tính là:

Giả sử cần điều khiển vị trí tay máy lệch ra khỏi vị trí ban đầu một góc pi/4

Kết quả mô phỏng

Hình 3.11 Kết quả mô phỏng góc thetal 1

Hình 3.12 Kết quả mô phỏng thetal 2