TRƯỜNG ĐẠI HỌC SƯ PHẠM KỸ THUẬT TP. HỒ CHÍ MINH
KHOA CƠ KHÍ CHẾ TẠO MÁY
BỘ MƠN CƠ ĐIỆN TỬ

BÁO CÁO CUỐI KỲ NHĨM 21
MƠN HỌC: HỆ THỐNG TRUYỀN ĐỘNG SERVO

MATLAB SIMULATION AND PID
ALGORITHM OF SERVO CONTROL

GVHD: Th.S Võ Lâm Chương
SVTH:

MSSV

Trần Thành Nhơn

18145196

Võ Quang Nhất

18146182

Tp. Hồ Chí Minh, tháng 6 /2021


MỤC LỤC
YÊU CẦU ĐỀ BÀI ............................................................................................................. 1
Bảng thông số của động cơ M4-4207 .................................................................................. 2
1. Mô phỏng 3 cấp vận tốc cho NL=4 và NL=7.................................................................. 5
1.1 TH1: NL=4 ................................................................................................................. 6

1.1.1 Tốc độ thấp tương đương với hệ bậc 1 ................................................................ 6
1.1.2 Tốc độ trung bình tương đương với hệ bậc 2 ...................................................... 8
1.1.3 Tốc độ cao tương đương với hệ bậc 4 ............................................................... 10
1.2 TH2: NL=7 ............................................................................................................... 12
1.2.1 Tốc độ thấp tương đương với hệ bậc 1 .............................................................. 12
1.2.2 Tốc độ trung bình tương đương với hệ bậc 2 .................................................... 14
1.2.3 Tốc độ cao tương đương với hệ bậc 4 ............................................................... 16
1.3 Nhận xét đáp ứng vị trí và vận tốc đối với 3 cấp vận tốc: ....................................... 18
2. Viết thuật toán PID ........................................................................................................ 19
Nhận xét ......................................................................................................................... 26


Hệ thống truyền động sevro

YÊU CẦU ĐỀ BÀI
Mỗi nhóm phải chọn các thông số tải trọng bao gồm độ cứng khớp nối mềm (KL),
qn tính moment (JL), và giảm xóc nhớt (DL) để thỏa mãn điều kiện: 3 ≤ 𝑁𝐿 ≤ 10 (ít
nhất 2 khác nhau tải trong phạm vi này) và hệ số tắt dần cho tất cả các tải: 𝜉 = 0,02
Mỗi nhóm sử dụng động cơ ở câu hỏi 1. Viết thuật toán PID bằng m-file của
Matlab để mô phỏng để điều khiển vận tốc của động cơ. Thuật toán phải bao gồm bộ lọc
đạo hàm cho số hạng D và bộ lọc chống gió cho số hạng I. Hãy minh họa tác dụng của
việc chống gió giật trong kết quả của bạn?

Bảng thông số động cơ
Em xin trình bày lại bảng thơng số động cơ như sau:

1


Hệ thống truyền động sevro


Bảng thông số của động cơ M4-4207
Motor

M4-4207

Winding

D

Basic Motor Constants
Power, P (W)

2686

Max Speed, Nmax (rpm)

2,600

Cont. Torque Mo (Nm)

21.9

Peak Torque, Mmax (Nm)

54

Current @ Cont. Torque, Io (A)

46.30


Current @ Peak Torque, Ip (A)

120

Max Terminal Voltage, VT (V)

140

Back EMF Const, Ke (V/krpm)

49.7

Torque Const., Kt (Nm/A)

0.475

DC Resistance, R (Ω)

0.065

Inductance, L (mH)

0.38

Weight, M (kg)

37

Inertia, JM (kg.m2)


0.186

Static Friction, TF (Nm)

0.75

Viscous Damping, F1 (Nm/krpm)

0.23

Tachogenerator Option
Tachogenerator Model

TGF2030-147

Voltage, KG (V/krpm)

19v / 57v / 38v / 47v

Ripple (pk-pk/avg)

2%

Inertia, JT (kg.cm2)

0.64

Weight, MT (grams)


640
2


Hệ thống truyền động sevro

Encoder Option
Encoder Type

Incremental (A quad B with index Z &
complements)

Power Supply

5V DC ± 5%, 200mA max

Output

Line Driver, 20mA Max source/sink

Resolution (ppr)

1024 / 2048 / 4096

Frequency Response

200 kHz max

Brake Option
Barke Open Voltage (Volts D.C.)


24

Power (Watts)

28

Brake Static Torque (Nm)

20

Weight (brake + housing) Kg

13

Bản vẽ động cơ M4-4207

3


Hệ thống truyền động sevro

rated output of motor

kW

0.2

rated torque of motor


kgm

0.195

rated velocity of motor

rev/min

1000

inertia moment of motor axis J m

kg m2

0.00224

inertia moment of mechanism part J L

kg m2

0.00653

natural angel frequency of mechanism part L

rad/s

94.2

damping rate of mechanism part  L


0.002

encoder resolution

pulse/rev

gear deceleration ratio NG

2000
1

Bảng 1.1 Slide bài giảng
Như thống nhất ở trên lớp chọn độ cứng khớp nối mềm như bảng 1.1 :

L =

KL
JL

→ K L = L 2 .J L = 94, 22.0.00653 = 57.95 (N.m)
Chọn độ cứng của khớp nối K L = 58 N.m
Động cơ có thể có hộp số nhưng trong mơ phỏng chọn N G = 1 .
Yêu cầu chọn ít nhất hai tải trọng trong phạm vi từ 3 đến 10. Chọn NL=4 và NL=7

4


Hệ thống truyền động sevro

1. Mô phỏng 3 cấp vận tốc cho NL=4 và NL=7

Bảng thông số cần thiết cho mô phỏng của động cơ M4-4207D
Power, P (W)

2686

Max Speed, Nmax (rpm)

2,600

Inertia, J M ( kg.m2 )

0,186

damping rate of mechanism part ζ L

0,02

Spring constant of mechanism part K L (N.m)

58

Gear deceleration ratio N G

1

Inertia moment of mechanism part J L ( kg.m 2 )

+Với NL = 4

J L = N L .N G2 .J M = 4.0,186 = 0,744

+Với NL = 7

J L = N L .N G2 .J M = 7.0,186 = 1,302

Natural angle frequence of mechanism part ωL
(rad/s)

+Với NL = 4

L =

KL
58
=
= 8,829
JL
0,744

+Với NL = 7

L =

5

KL
58
=
= 6.674
JL
1,302



Hệ thống truyền động sevro

Để thể hiện rõ ràng các cấp vận tốc, chúng em chọn position là 400 rad để đồ thị thể
hiện rõ các đáp ứng với từng cấp vận tốc
1.1 TH1: NL=4
1.1.1 Tốc độ thấp tương đương với hệ bậc 1

V

1
1
VMax  .2600  130 rpm
20
20

→ V  13,6 rad/s. Chọn V=10 rad/s =95,5 rpm
c p = 0, 24 ; cv = 0,82
b0 = (1 + N L )c p c v = (1 + 4).0,82.0, 24 = 0,984

b1 = (1 + N L )(c v + 2cpc v L )
= (1 + 4)(0,82 + 2.0,82.0, 24.0,02) = 4,139
cp1 =

b0 0,984
=
= 0, 237
b1 4,139


K p1 = c p1.L = 0, 237.8,829 = 2,092

Sơ đồ khối mô phỏng trên matlab simulink
Code mô phỏng:

NL=4;
Ng=1;
cp=0.24;
cv=0.82;
psi=0.02;
Jm=0.186;
KL=58;
%cong thuc o duoi

JL = NL*Jm*(Ng^2);
wL = sqrt(KL/JL);
DL = 2*psi*sqrt(KL*JL);
b0=(1+NL)*cp*cv;
b1=(1+NL)*(cv+2*cp*cv*psi);
cp1=b0/b1;
Kp1 = cp1*wL
sim('Bac1_NL4')

6


Hệ thống truyền động sevro

%ve do thi matlab
%hold on

%plot(out.input.Time,
out.input.Data, '--k',
'LineWidth',1)
%plot(out.y1st.Time,
out.y1st.Data, '-r',
'LineWidth',1)

%legend('Input','Vitri')
plot(out.vantoc.Time,
out.vantoc.Data, '-r',
'LineWidth',1)
grid on

Đồ thị
vị trí
cấp
vận
tốc
thấp

Đồ thị
vận
tốc
cấp
vận
tốc
thấp

7



Hệ thống truyền động sevro

1.1.2 Tốc độ trung bình tương đương với hệ bậc 2

1
1
VMax  V  VMax
20
5
→

1
1
.2600  V  .2600 → 130  V  520 rpm
20
5

→ 13,6  V  54,5 rad/s
Chọn V=30 rad/s = 286,5 rpm
Ta có c p2 = 0, 24 , cv2 = 0,96
K p2 = 0, 24.8,829 = 2,119

K v2 = 0,96.8,829 = 8,476

Sơ đồ khối mô phỏng trên matlab simulink
Code:
cp2=0.24;
cv2=0.96;
KL=58;

NL=4;
Jm=0.186;
Ng=1;
JL = NL*Jm*(Ng^2);
wL = sqrt(KL/JL);
Kp2=cp2*wL;
Kv2=cv2*wL;
sim('Bac2_NL4')
%ve do thi matlab

hold on
plot(out.input.Time,
out.input.Data, '--k',
'LineWidth',1)
plot(out.y2st.Time,
out.y2st.Data, '-r',
'LineWidth',1)
legend('Input','Vitri')
%plot(out.vantoc.Time,
out.vantoc.Data, '-r',
'LineWidth',1)
grid on

8


Hệ thống truyền động sevro

Đồ thị
vị trí

cấp
vận
tốc
trung
bình

Đồ thị
vận
tốc
cấp
vận
tốc
trung
bình

9


Hệ thống truyền động sevro

1.1.3 Tốc độ cao tương đương với hệ bậc 4

1
1
V  .VMax → V  .2600 → V  520 rpm
5
5

→ V  54,5 rad/s
Chọn V=80 rad/s =764 rpm

c p = 0, 24 ; cv = 0,82
J L = N L .N G2 .J M = 4.1.0,186 = 0,744

D L =  L .2. J L .K L = 0,02.2. 0,744.58 = 0, 263

JT = JM +

JL
0,744
= 0,186 +
= 0,93
2
1
NG

K p = c p .L = 0, 24.8,829 = 2,119

K v = cv .L = 0,82.8,829 = 7,240
K gv = K v .J T = 7, 240.0,93 = 6,733

Sơ đồ khối mô phỏng trên matlab simulink
Code:
cp=0.24;
cv=0.82;
KL=58;
Ng=1;
Jm=0.186;
psi=0.02;
NL=4;
%cong thuc cho bac 4


JL = NL*Jm*(Ng^2);
wL = sqrt(KL/JL);
DL=psi*2*sqrt(JL*KL);
JT=Jm+(JL/(Ng^2));
Kp=cp*wL;
Kv=cv*wL;
Kvg=Kv*JT;
sim('Bac4_NL4')
10


Hệ thống truyền động sevro

%ve do thi matlab
%hold on
%plot(out.input.Time,
out.input.Data, '--k',
'LineWidth',2)
%plot(out.y4st.Time,
out.y4st.Data, '-r',
'LineWidth',2)

%legend('Input','Vitri')
plot(out.vantoc.Time,
out.vantoc.Data, '-r',
'LineWidth',2)
grid on

Đồ thị

vị trí
cấp
vận
tốc
cao

Đồ thị
vận
tốc
cấp
vận
tốc
cao

11


Hệ thống truyền động sevro

1.2 TH2: NL=7
1.2.1 Tốc độ thấp tương đương với hệ bậc 1

V

1
1
VMax  .2600  130 rpm
20
20


→ V  13,6 rad/s. Chọn V=10 rad/s =95,5 rpm
b0 = (1 + N L )c p c v = (1 + 7).0,82.0, 24 = 1,574

b1 = (1 + N L )(c v + 2cp c v  L ) = (1 + 7)(0,82 + 2.0,82.0, 24.0,02) = 6,623

cp1 =

b0 1,574
=
= 0, 237
b1 6,623

K p1 = c p1.L = 0, 237.6,674 = 1,582

Sơ đồ khối mô phỏng trên matlab simulink
Code matlab
NL=7;
Ng=1;
cp=0.24;
cv=0.82;
psi=0.02;
Jm=0.186;
KL=58;
%cong thuc o duoi
JL = NL*Jm*(Ng^2);
wL = sqrt(KL/JL);
DL = 2*psi*sqrt(KL*JL);
b0=(1+NL)*cp*cv;
b1=(1+NL)*(cv+2*cp*cv*psi);
cp1=b0/b1;

Kp1 = cp1*wL

sim('Bac1_NL5')
%ve do thi matlab
hold on
plot(out.input.Time,
out.input.Data, '--k',
'LineWidth',1)
plot(out.y1stNL7.Time,
out.y1stNL7.Data, '-r',
'LineWidth',1)
legend('Input','Vitri')
%plot(out.vantoc.Time,
out.vantoc.Data, '-r',
'LineWidth',1)
grid on
12


Hệ thống truyền động sevro

Đồ thị
vị trí
cấp
vận
tốc
thấp

Đồ thị
vận

tốc
cấp
vận
tốc
thấp

13


Hệ thống truyền động sevro

1.2.2 Tốc độ trung bình tương đương với hệ bậc 2

1
1
VMax  V  VMax
20
5
→

1
1
.2600  V  .2600 → 130  V  520 rpm
20
5

→ 13,6  V  54,5 rad/s
Chọn V=30 rad/s = 286,5 rpm
Ta có c p2 = 0, 24 , cv2 = 0,96
K p2 = 0, 24.6,674 = 1,602


K v2 = 0,96.6,674 = 6,407

Sơ đồ khối mô phỏng trên matlab simulink
Code:
cp2=0.24;
cv2=0.96;
KL=58;
NL=7;
Jm=0.186;
Ng=1;
JL = NL*Jm*(Ng^2);
wL = sqrt(KL/JL);
Kp2=cp2*wL;
Kv2=cv2*wL;

sim('Bac2_NL7')
%ve do thi matlab
hold on
plot(out.input.Time,
out.input.Data, '--k',
'LineWidth',1)
plot(out.y2stNL7.Time,
out.y2stNL7.Data, '-r',
'LineWidth',1)
legend('Input','Vitri')
14


Hệ thống truyền động sevro


%plot(out.vantoc.Time,
out.vantoc.Data, '-r',
'LineWidth',1)

grid

Đồ thị
vị trí
cấp
vận
tốc
trung
bình

Đồ thị
vận
tốc
cấp
vận
tốc
trung
bình

15


Hệ thống truyền động sevro

1.2.3 Tốc độ cao tương đương với hệ bậc 4


1
1
V  .VMax → V  .2600 → V  520 rpm
5
5

→ V  54,5 rad/s
Chọn V=80 rad/s =764 rpm
c p = 0, 24 ; cv = 0,82
J L = N L .N G2 .J M = 7.1.0,186 = 1,302

D L =  L .2. J L .K L = 0,02.2. 1,302.58 = 0,348

JT = JM +

JL
1,302
= 0,186 +
= 1, 488
2
1
NG

K p = c p .L = 0, 24.6,674 = 1,602

K v = cv .L = 0,82.6,674 = 5,472
K gv = K v .J T = 5, 472.1, 488 = 8,142

Sơ đồ khối mô phỏng trên matlab simulink

Code:
cp=0.24;
cv=0.82;
KL=58;
Ng=1;
Jm=0.186;
psi=0.02;
NL=7;

%cong thuc cho bac 4
JL = NL*Jm*(Ng^2);
wL = sqrt(KL/JL);
DL=psi*2*sqrt(JL*KL);
JT=Jm+(JL/(Ng^2));
Kp=cp*wL;
Kv=cv*wL;
16


Hệ thống truyền động sevro

Kvg=Kv*JT;
sim('Bac4_NL7')
%ve do thi matlab
%hold on
%plot(out.input.Time,
out.input.Data, '--k',
'LineWidth',2)

%plot(out.y4st.Time,

out.y4st.Data, '-r',
'LineWidth',2)
%legend('Input','Vitri')
plot(out.vantoc.Time,
out.vantoc.Data, '-r',
'LineWidth',2)
grid on

Đồ thị
vị trí
cấp
vận
tốc
cao

Đồ thị
vận
tốc
cấp
vận
tốc
cao

17


Hệ thống truyền động sevro

1.3 Nhận xét đáp ứng vị trí và vận tốc đối với 3 cấp vận tốc:
-Vận tốc trong 3 trường hợp ứng với từng cấp tốc độ: “Thấp, trung bình, cao” là 10,

30,80 rad/s. Vận tốc càng cao thì vị trí vị trí mong muốn là 400 rad càng nhanh đạt được.
trong cả 3 trường hợp chúng ta có thể thấy đều khơng có tình trạng xảy ra vọt lố. Đúng với
yêu cầu cần thiết của động cơ sevro.
- Đối với NL=4 và NL=7 chúng ta có thể thấy được khơng có sự khác biệt đối với 3
cấp vận tốc : “Thấp, trung bình, cao”. Rõ ràng thơng số của động cơ có thể đáp ứng trong
trường hợp 3  NL  10 . Thỏa mãn cơ sở lý thuyết mà chúng ta đã được học.

18


Hệ thống truyền động sevro

2. Viết thuật toán PID
Viết thuật tốn PID sử dụng m-file của Matlab để mơ phỏng điều khiển vận tốc
của động cơ. Thuật toán phải bao gồm bộ lọc đạo hàm cho số hạng D và bộ lọc ngược
cho số hạng I. Hãy minh họa tác dụng của việc chống gió giật trong kết quả của bạn?
Bảng thông số cần thiết cho câu 2
Motor

M4-4207

Winding

D

Back EMF Const, Ke (V/krpm)

49.7

Torque Const., Kt (Nm/A)


0.475

DC Resistance, R (Ω)

0.065

Inductance, L (mH)

0.38

Inertia, JM (kg.m2)

0.186

Viscous Cofficient (b)

0

19


Hệ thống truyền động sevro

Đổi đơn vị Ke (V/krpm) thành V/rad/s
Ke=

49,7.60
= 0, 475 V/rad/s.
1000.2


Nhận thấy Ke=Kt đúng với cơ sở lý thuyết đã được học
Hệ số K của hàm truyền động cơ DC

K=

Kt
1
1
=
=
= 2,105
K t .K e K e 0, 475

Thời gian đáp ứng

=

R.J M
0,065.0,186
=
= 0,05358 (s)
K t .K e 0, 475.0, 475

Thời gian đáp ứng mong muốn là c = 0,02 (s)
Nhận xét:
Trong mơ phỏng thuật tốn PID nhóm lựa chọn bỏ khâu D trong điều khiển động
cơ DC sevro đặc tính động cơ có cơng thức

K

là khâu bậc 1, nên không cần sử dụng
M s + 1

khâu D. Trong bài tốn mơ phỏng này nhóm chỉ sử dụng khâu PI do khâu D bằng 0.
Nhưng trong mô phỏng sẽ viết bộ lọc đạo hàm cho số hạng D đúng với yêu cầu của
đề bài.

20


Hệ thống truyền động sevro

Tìm thơng số PID với PID tuner của matlab simulink:

Kd=0
N=100

Nhận thấy thông số Kd của PID tuner matlab simulink trả về kết quả bằng 0 đúng
với cơ sở lý thuyết của hệ sevro động cơ DC.
+ Thông số N trả về bằng 100

21


Hệ thống truyền động sevro

Khâu P được tính tốn như sau
u pk = K p .e k

( Công thức 3.37 slide bài giảng)


Anti-windup:

u ik = u ik −1 + K I .T.e k + K b .T.e kreset ( Công thức 3.41 slide bài giảng)

Low pass filter:

u fD (k) =

N
KD
u fD (k − 1) +
(e(k) − e(k − 1))
N+T
N+T

Thông số bộ PID bao gồm bộ lọc đạo hàm cho số hạng D và bộ lọc ngược cho số hạng
I
u k = u kI + u kP + u fD

22


Hệ thống truyền động sevro

Mạch mô phỏng matlab simulink
Code:
Hàm PID_control
global Kp Ki Kd Kb Ts b
err_p ui_p err_reset N ud_p

err_p=0;
ui_p=0;
ud_p=0;
err_reset=0;

b=0;
Ts=0.02;
Kp=tau/(K*tauc);
Ki=Kp/tau;
Kb=1/tau;
Kd=0;
N=100;
sim('PID');
plot(y_out.Time,y_out.Data,
'-r','lineWidth',2)
title('The output speed of
position control');
ylabel('Speed (rad/s)');

K=2.105;
tau=0.05358;
tauc=0.02;
Km=0.475;
Ke=0.475;
L= 0.38*10^(-3);
R=0.065;
Jm=0.186;
xlabel('Time (s)');
grid on
Hàm PIDfunction:

function y = PIDfunction(u)
global Ts Kp Kd Ki Kb
err_p ui_p err_reset ud_p N

err = wd - wf;
up = Kp*err;
%Anti-windup
ui = ui_p + Ki*Ts*err +
Kb*Ts*err_reset;
%Low pass filter

wd = u(1); % Gia tri
ban dau
wf = u(2); % Gia tri
tra ve
23