Dieu khien da bien UTC Đại Học Giao Thông Vận Tải

TRƯỜNG ĐẠI HỌC GIAO THƠNG VẬN TẢI
KHOA CƠ KHÍ
BỘ MƠN CƠ ĐIỆN TỬ
----------------

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

HÀ NỘI 2023


Mục Lục
CHƯƠNG 1 ĐỘNG LỰC HỌC VÀ PHƯƠNG TRÌNH TRẠNG
THÁI.........................................................................................................................
1.1. Thông số vật lý của cánh tay mô phỏng......................................................
1.2. Thành lập phương trình động lực học và phương trình trạng
thái.........................................................................................................................
CHƯƠNG 2 : TÍNH ỔN ĐỊNH VÀ CHẤT LƯỢNG HỆ THỐNG ĐA
BIẾ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ệu
ra..........................................................................................................................
3.4 thiết kế bộ điều khiển tồn phương tuyến tính..........................................


CHƯƠNG 1 ĐỘNG LỰC HỌC VÀ PHƯƠNG TRÌNH TRẠNG THÁI

1.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
m1, m2 khối lượng của hai khớp(m1=1kg, m2 = 2 kg)

1


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:

[ ][

x 1 L1 cos θ 1
=
y 1 L1 sin θ1

]

(1.1)

vận tốc của m1

[ ][

]

x˙ 1 −L1 sin θ1
= L cos θ θ˙ 1
y˙ 1

1
1

(1.2)

- Vị trí của m2:

[ ][

x2
L1 cos θ 1+ L2 cos (θ1 +θ2 )
=
y2
L1 sinθ 1+ L2 sin(θ 1+ θ2)

]]

(1.3)

- vận tốc của m2:

[ ][

X˙ 2 −L1 sin θ1 −L2 sin(θ 1+θ 2) −L2 sin(θ 1+ θ2)
= L cos θ + L cos(θ + θ ) L cos (θ +θ )
Y˙ 2
1
1
2
1

2
2
1
2
1

][ ]
θ˙ 1
θ˙ 2

(1.4)

2

- Động năng của hệ ( K= 2 mv )
1
1
1
2
2
2
2 2
K 1= m1 v 1= m1 ( x˙ 1+ y˙ 1 ) = m1 L1 θ˙ 1
2
2
2

(1.5)

1

1
2
2
2
K 2= m2 v 2= m2 ( x˙ 2+ y˙ 2 )
2
2

(1.6)

1
= 2 m2 ¿ )2+2 L1 L2 θ˙ 1 ( θ˙ 1+ θ˙ 2 ) cos θ2

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

P1=m1 g L1 sin θ1
P 2=m2 g ¿ + L2 sin ⁡(θ1 +θ 2)

(1.8)
2


- Hàm lagrange
L = K-P

(1.9)

- Thay (1.5),(1.6),(1.7),(1.8) vào (1.9) ta được :
1

2 2 1
L= m1 L1 θ˙ 1+ m2 ¿ )2+2 L1 L2 cos θ2 θ˙ 1 ( θ˙ 1+ θ˙ 2 )−¿+ L2 sin ⁡(θ1 +θ 2)])
2
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 ∂L ∂ L
−
,i=1,2
d t ∂ θ˙ i ∂θ i

+ TÍNH F1 :

( )

d ∂L
∂L
F1 = dt ∂ θ˙1 − ∂ θ 1

Trong đó :
∂L
= (m1 + m2 ).(l1)2.θ˙ 1 + m2.(l2)2. (θ˙ 1 + θ˙ 2 ¿ +m2.l1.l2.cosθ2. (2 θ˙ 1 + θ˙ 2 ¿
˙
∂θ 1
∂L
= -m1.l1.g.cosθ1- m2.g ( l1. cosθ1 + l2 cos(θ1+θ2))

∂θ 1

( )

d ∂L
= [ (m1 +m2). (l1)2 + m2. (l2)2 + 2m2.l1. l2. cos2]ă1+[ m2.(l2)2 +m2.l1.l2. cos2]
dt 1

ă2 -2.m2.l1.l2.sin2. 1. 2 m2.l1.l2. sin2 .( θ˙ 2) 2


F1 = [ (m1 +m2).(l1)2 + m2. (l2)2 + 2m2.l1. l2. cos2]ă1+[ m2.(l2)2 +m2.l1.l2. cos2]ă2 -

2.m2.l1.l2.sin2 . 1.θ˙ 2 –m2.l1.l2. sinθ2 .( θ˙ 2) +[ m1.l1.g.cosθ1+ m2.g ( l1. cosθ1 + l2 cos(θ1+θ2
2

))]

3


+ TÍNH F2 :

( )

d ∂L
∂L
F2 = dt ∂ θ˙2 − ∂ θ 2

Trong đó :

∂L
= [m2. (l2)2.(θ˙ 1 + θ˙ 2) + m2.l1.l2.cosθ2.θ˙ 1]
∂ θ˙2

∂L
= -m2.l1.l2.sinθ2 .θ˙ 1 ( θ˙ 1 + θ˙ 2 ) −¿m2.g.l2.cos(θ1+θ2)
∂θ 2

( )

d ∂L
= [m2.(l2)2 + m2.l1.l2.cos2 ].ă1 + m2.(l2)2.ă2 -m2.l1.l2.sin 2 . 1. 2
dt 2

F2= [m2.(l2)2 + m2.l1.l2.cos2 ].ă1 + m2.(l2)2.ă2 -m2.l1.l2.sin θ2 .θ˙ 1.θ˙ 2 +
[m2.l1.l2.sinθ2.θ˙ 1 ( θ˙ 1+ θ˙ 2)+¿m2.g.l2.cos(θ1+θ2 )]
- Phương trình động lực học của robot 2 bậc tự do là:

[ ][

][ ]

( m1 +m2 ) . L21 +m2 . L22+ 2m2 . L1 . L2 . cos θ2 m2 . L22 +m2 . L1 . L2 . cos 2 ă 1
F1
=
.
2
2
F2
m2 . L2+ m2 . L1 . L2 . cos 2

m 2 . L2
ă 2

[

+

m2 . L1 . L2 . sin θ2 . θ˙ 2 −m2 . L1 . L2 . sinθ 2 .( θ˙ 1+ θ˙ 2)
m2 . L1 . L2 . sin θ2 . θ˙ 1
0

+¿

4

]

[]
θ˙ 1
θ˙ 2


- Phng trỡnh dng euler-lagrange:
J ( q ) q+
ă C ( q . q˙ )+ G ( q )=F

M (θ )=

[

(1.14)

( m1 +m2 ) . L21 +m2 . L22 +2 m2 . L1 . L2 . cos θ 2 m 2 . L22+ m2 . L1 . L2 . cos θ2
2

2

m2 . L2 +m2 . L1 . L2 .cos θ2

m2 . L2

(1.15)

[

−m 2 . L1 . L2 . sin θ2 . θ˙ 2 −m2 . L1 . L2 .sin θ 2 .(θ˙ 1+ θ˙ 2)
V ( θ , θ˙ )=
m2 . L1 . L2 . sin θ2 . θ˙ 1
0

]

(1.16)

(1.17)

G ( θ )=¿

- Hệ phương trình động học của tay mỏy 2 bc t do
J m 1 ă m 1 +J 21 ă m 2 + B e ff 1 m 1 =U 1d '1

J m 2 ă m 1 +J 22 ă m 2 + B e ff 2 θ˙ m 2=U 2−d '2

Trong đó
J m 1=¿ =3
2

J m 2=m2 l 2=1
2

J 21=m2 l 2 =1
2

J 22=m2 l 2 =1
5

]


Beff 1=Bm 1 +

kb1 k m 1
=2
R1

Beff 2=Bm 2 +

kb2 k m 2
=2
R2

U 1=

( )

U 2=

( )

km1
V 1=10
R1
km2
V =10
R2 2

2
'
d 1= 2m2.l1.l2. cos2 ă1 +m2.l1.l2. cos2 ă2 -2.m2.l1.l2.sin2 . 1. 2 –m2.l1.l2. sinθ2.( θ˙ 2)

[ m1.l1.g.cosθ1+ m2.g ( l1. cosθ1 + l2 cos(1+2))]
d '2= m2.l1.l2.cos2 .ă1 -m2.l1.l2.sin 2 . 1. 2 –

[-m2.l1.l2.sinθ2.θ˙ 1 ( θ˙ 1 + θ˙ 2 ) m2.g.l2.cos(1+2 )]
Suy ra :

(m 2 l22 ) ă m 1+(m 2 l 22) ă m2+ Be ff 2 m 2=U 2 d '2

Suy ra
ă m 1 =[Beff 1 θ˙ m 1 +Be ff 2 θ˙ m 2+U 1−U 2 + d '2−d '1]/[ m1+ m2 ¿ l 21
ă m 2 = B e ff 2 θ˙ m2 ((m¿¿ 1+m¿¿ 2)l 21+ m2 l 22 ¿ ¿ )+ B e ff 1 θ˙ m 1 m 2 l 22 + ¿¿ ]

Đặt

x 1=θm 1

x˙ 1=x 2=❑m 1= x 2=ă m 1
x 3=m 2
6


x 3=x 4 =m 2= x 4 =ă m 2

[]

x 1
x˙ 2
=¿+¿
x˙ 3
x˙ 4

[ ] [
θm 1
θm 2

[][
[ ][

x1
0 0 u
1 0 0 0 x

= 0 0 1 0 x 2 + 0 0 u1
3
2
x4

]

][ ]

Thay các giá trị vào suy ra không gian trạng thái của hệ
x˙ 1
0 1
0 0
x˙ 2
0
−1
0 1
=
0 0
0 1
x˙ 3
0
1
0
−1,5
x˙ 4

[]

x1

0 0 u
1 0 0 0 x
= 0 0 1 0 x 2 + 0 0 u1
3
2
x4

[ ] [
θm 1
θm 2

][ ] [ ][ ]
x1 0 0
x 2 5 −5 u1
+
x 3 0 0 u2
x 4 5 15

]

[ ][ ]

CHƯƠNG 2 : TÍNH ỔN ĐỊNH VÀ CHẤT LƯỢNG HỆ THỐNG ĐA BIẾN
2.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:

7


[ ][

x˙ 1
0 1
0 0
x˙ 2
0
−1
0 1
=
0 0
0 1
x˙ 3
0
1
0
−1,5
x˙ 4

Ta có:

[ ]
[ ]
[ ]

0 1

0 0
0 −1
0 1
A= 0 0
0 1
0 1 0 −1,5
0 0
5 5
B= 0 0
5 15

5
−5
0
20
AB= 5
15
−2,5 −27.5

[

0
20
−2,5 −47,5
A2B= −2,5 −27,5
3,75 61,25

[

]

−2,5
−47,5
6,25
108,75
A3B= 3,75
61,25
−8,125 −139,375

]

Thay các giá trị trên vào P(A ,B) ta được

8

][ ] [ ] [ ]
x1 0 0
x2 5 5 u1
+
x3 0 0 u2
x 4 5 15


[

0 0
5
−5
0
20 −2,5 −47,5

5 −5
0
20
−2,5
−47,5 6,25 108,75
P(A,B)= 0 0
5
15
−2,5
−27,5 3,75 61,25
5 15 −2,5 −27,5 3,75 61,25 −8,125 −139,75

]

2.1.2 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=[B,A*B,A*A*B,A*A*A*B]
rank(P)

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


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ệu
và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 f > 0 nào thì trạng
thá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ệu
vào u(t) và đầu ra y(t) với 0 <= t <= t f. Ngược lại, hệ được gọi là không quan sát
được.
Các xác định :
Hệ tuyến tính cho bởi phương trình là qn 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:

[ ][

x˙ 1
0 1
0 0
x˙ 2
0
−1
0 1
=
0 0
0 1
x˙ 3
0
1

0
−1,5
x˙ 4

10

][ ] [ ][ ]
x1 0 0
x 2 5 −5 u1
+
x 3 0 0 u2
x 4 5 15


[ ] [
θm 1
θm 2

]

Ta có :

[

0 1
0 0
0 −1
0 1
A= 0 0
0 1

0 1 0 −1,5

[1

0 0 0

C= 0 0 1 0

[0

[0

C*A2= 0

[0

]

]

1 0 0

C*A= 0 0 0 1

]

−1 0
1
1 0 −1,5

2

]

0 −2,5

C*A3= 0 −2,5 0 3,25

]

[ ]

1
0
0
0
0
1
0
0
L(A,C) = 0 −1
0
1
0
2
0 −2,5

[]

x1

0 0 u
1 0 0 0 x
= 0 0 1 0 x 2 + 0 0 u1
3
2
x4

0
0
1
0
0
0
0
1
0
1
0 −1.5
0 −2.5
0 3.25

11

[ ][ ]


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
12


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 pi của một hệ tuyến tính có biểu diễn tối thiểu (A, B, C,
D) là các trị riêng λi(A), i= 1,2,…..n của ma trận A. Đ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

[

0 1
0 0
0 −1
0 1
A= 0 0
0 1
0 1 0 −1,5

]

[

]

|

λ −1
0 0
λ +1 0
0 λ+1 0 −1
Det(λI – A) = 0 0
=λ 0 λ
λ −1
−1 0
0 −1 0 λ+ 1,5

|λ+ 1

= λ . λ . −1

|

−1
λ+1,5

= λ 2 . ¿( λ+ 1).( ( λ+ 1,5) - 1]
= λ 2( λ2 +2,5 λ+0,5)
Giải hệ :

λ1 = 0
13

|

−1
−1
λ+1,5


λ2 = 0
λ 3 = -0,219
λ 4 = -2,28

Tương ứng hệ tuyến tính trên có 4 điểm cực là :
s1=0
s2=0

s3= -0,219
s4 = -2,28

2.3.2 Điểm khơng
Hàm truyền đath G(s) có một điểm không bất biến tại s = z inếu tại s = z i ma trận

[ sI −A DB ] bị giảm hạng. Nếu ma trận [ sI−C−A DB ] là ma trận vng thì :

rosenbrock −C
det

[

zi I − A
−C

]

B
=0
D

Ma trận rosenbrock :
I–A B
=[ ]
[ s −C
D]

Ma trận rosenbrock có rank lớn nhất là 6 ta tính det của ma trận có kích thước 6x6
Ta có det [ ]=¿ -50
Suy ra ma trận

I–A B
không bị giảm hạng
[ s −C
D]
14


Suy ra hệ khơng có điểm khơng bất biến
Tìm hàm truyền
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]
C = [1,0,0,0;0,0,1,0]
D = [0,0;0,0]
syms s
I = [ 1,0,0,0;0,1,0,0;0,0,1,0;0,0,0,1]
G= C*(s*I-A) ^ (-1) *B+D
simplify(G)
pretty(ans)

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

15


Hàm truyền của hệ là:
G(s) =

[

2

2

(10 s+25)/s (2 s + 5 s+ 1) −(10 s−15)/ s(2 s +5 s +1)
2
2
(10 s+20)/s (2 s + 5 s+ 1) (30 s+20)/s (2 s + 5 s+ 1)

]

Suy ra hệ khơng có điểm khơng truyền dẫn
Tìm các điểm khơng hệ thống
Để xác định tất cả các điểm khơng hệ thống ta tìm {i .d ze ros } =
{s ∈ C | ma trận [ sI – A B] bị giảm hạng}

{

s ∈ C∨ma trận

và {o.d

ze ros } =

[ sI−−CA ] bị giảmhạng } :

Xet ma trậ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))/2
det [ ] = -s*(s + 1)
det [ ] = -s
det [ ] =0
det [ ]=s^3 + (5*s^2)/2 + s/2
det [ ] =-1

det [ ]=0

det [ ] =s^3 + (5*s^2)/2 + s/2
`
det[ ]=s + 3/2
16


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

Xét ma trận :[ sI −A B ] =[ ] có rank lớn nhất là 4
det [ ] =(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*s
det[ ]= - 5*s^2 - 10*s
det[ ]=0

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

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

det[ ]=50*s
det[ ]=-50
det[ ]= 0
vậy hệ khơng có điểm khơng hệ thống
Code 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 )

18