Transcript 下載/瀏覽

以FPGA為基礎三軸平台運動控
制演算法則高速化之研製
南台科技大學 電機工程系伺服控制晶片設計研究室
學生﹕劉適存
指導教授：龔應時 教授
Outline
摘要
1.緒論
2.運動控制器設計之演算法則
3.三軸馬達硬體電路演算法則高速化之研究晶片設計
4.高速演算法則伺服運動控制實驗架構及實驗結果
5.結論
2
摘 要

為了提高伺服系統的控制性能，快速的運動響應及隨環境變化的控制
參數調變能力，利用FPGA(現場可程式邏輯閘陣列)優越的硬體快速計
算及可程式能力來處理伺服系統先進的控制演算法則以提升其運動性
能，以下目標即成為本文之研究目的。

以FPGA晶片完成三軸馬達驅動器電流/速度/位置迴路之控制，包括向
量控制、空間向量脈波寬度調變(SVPWM)、速度/位置PI控制。

進行實驗測試，包括位置、速度步階響應，以及運動軌跡追蹤，以證
實其有效性與正確性。
3
一、 多軸伺服控制

一般而言，多軸伺服控制，可以被區分為兩部份，一部份為具
有伺服驅動器的機械結構，另一部分為控制多軸機械運動之控
制器。
4
螺桿
5
二、運動控制器設計之演算法則

2.1 三軸平台控制系統之實現

2.2 電流迴路向量控制演算法則

2.4空間向量脈波寬度調變(SVPWM)策略之演算法則
6
2.1 三軸平台控制系統之實現
Position loop
Position
Speed
Command
+
Speed loop
Fuzzy-PI Command
Controller
+
PI
Controller
Current
controller
iq*
—
—
id*  0
vq
PI
+
PI
Park-1
modify
Clark-1
vref 1
vref 2
a,b,c vref 3
v
, 
d,q
—
+
DC
Power
Current loop
vd
, 
v
SVPWM
PWM1
PWM2
PWM3
PWM4
PWM5
PWM6
Inverter
—
iq
Speed
Estimator
id
d,q
, 
i
, 
i
Park
a,b,c
ia
ib
ic
iu
A/D
convert
iv
Clark
e  1
sin /cos of
Flux angle
 e   2
r
QEP
A
B
Z
NiosII
Trajectory & Interpolation
implemented by software using
NiosII embedded processor
Trajectory
&
Interpolation
computation
Position loop
Position
Speed
Command
+
Speed loop
Fuzzy-PI Command
Controller
+
PI
Controller
Current
controller
iq*
—
—
id*  0
vq
PI
Park-1
modify
Clark-1
vref 1
vref 2
a,b,c vref 3
v
, 
d,q
—
+
DC
Power
Current loop
PI
+
vd
, 
v
PMSM
Encoder
SVPWM
PWM1
PWM2
PWM3
PWM4
PWM5
PWM6
Inverter
—
iq
Speed
Estimator
id
d,q
, 
i
a,b,c
Park
Speed
Command
+
—
Speed loop
Fuzzy-PI Command
Controller
+
PI
Controller
Current
controller
iq*
—
id*  0
vq
iv
Clark
 e   2
Park-1
r
QEP
A
B
Z
vd
, 
DC
Power
vref 1
vref 2
a,b,c vref 3
, 
v
PMSM
Encoder
modify
Clark-1
v
d,q
—
PI
+
ic
iu
A/D
convert
Current loop
PI
+
ia
ib
e  1
sin /cos of
Flux angle
Position loop
Position
, 
i
SVPWM
PWM1
PWM2
PWM3
PWM4
PWM5
PWM6
Inverter
—
Speed
Estimator
iq
id
d,q
, 
Park
sin /cos of
Flux angle
i
i
, 
a,b,c
ia
ib
ic
iu
A/D
convert
iv
Clark
e  1
 e   2
r
QEP
A
B
Z
Encoder
PMSM
7
2.2 永磁同步馬達電流向量控制
Rectifier
TL
Te
iq*
Kt
1
J ms  Bm

A
T
B
T
C
T
A
T
B
T
C
T
PMSM
+
Inverter
L
p
AC 220V
C
U,V,W
id  0
Equivalent block diagram for current
vector control of PMSM drives
PWM1
FPGA (DE2)
Task in FPGA:
r
1. coordinate transformation
2. PI controller (for current)
3. SVPWM (Space-vector PWM)
4. ADC interface
5. QEP interface
Clarke
*
q
i
Park
PWM6
SVPWM
Modified
-1
ADC
Te
a,b,c
QEP
a,b,c
, 
, 
Clarke
i
, 
d,q
PMSM
ib
ia
i
-1
Comparato
r circuit
d,q
, 
Park
+
-
i 0
*
d
In this Figure, the PMSM will be decoupled
if control id = 0 , and control a PMSM
is as same as to control a DC motor.
Low -pass
filter
Isolated and driving circuits
PI
iq
id
PI
+
-
Current loop vector control
Sin &Cos
θθe
e
QEP Circuit
8
座標轉換
9
2.3空間向量脈波寬度調變 (SVPWM)
CMPR3
CMPR2
CMPR1
β
PWM1
U120 (010)
S
5
S
U out
T2
U180 (011)
S
S
T1
4
S
U 240 (001)
S
T0/2
T2
PWM3
U0
O0
O0 U0 U60 O111 O111 U60
(000) (100) (110) (111) (111) (110) (100) (000)
T
T
3
U 0 (100)
T1
PWM2
U 60 (110)
1
T0/2
α
區間3
2
6
CMPR3
U 300 (101)
電壓基本向量空間
CMPR1
CMPR2
PWM1
PWM2
PWM3
T0/2
T1
T0/2
T2
O0 U120 U60 O111 O111 U60 U120 O0
(000) (010) (110) (111) (111) (110) (010) (000)
T
T
區間1
10
3.1高速演算法則控制IP之設計


Example: Y  a1 * x1  a2 * x2  a3 * x3
Parallel processing:
three multipliers
and
two adders
a1
x1
a2
x2
a3
x3
x
+
x
y
+
Near one clock
execution time
x
Multiplier is large utilities consumption in FPGA

Sequential processing using Finite state machine (FSM):
x1
one multiplier
and
one adder
a1
+
x
a2
x
x2
s1
s2
y
x3
a3
Five clocks
execution time
+
x
s3
s4
s5
11
3.2空間向量脈波調變演算法之控制IP設計
三角波產生
Generation of
Symmetric
Triangular wave
Clk
Q
Vref1
12
Vref2
Vref3
12
PWMEA_1
CMPR1
S1
12
S5
S2
S4
S3
12
CMPR3
12
12
Clk_120n
SVPWM algorithm
SVPWM 演算法
Clk_40n
Comparator
1
PWMEA_2
PWMEB_1
CMPR2
State Machine
12
Comparator
2
PWMEB_2
PWM1
Dead-band
unit
PWM2
PWM3
PWM4
PWM5
PWMEC_1
Comparator
PWMEC_2
3
PWM6
Clk_200n
比較器及PWM
訊號產生
停滯帶單元
12
3.3 電流向量控制演算法之控制IP設計
 e _ addr
LUT
sin e
cos  e
x
ia
i
x
1
i
3
ib
ic
- 1
iq* +
x
x
3
x
+
x
+
+
+
-
iq
vq
x
kp_q
ki_q
id
x
+
x
e_q
i_q
-
id*
e_q
+
ki_d
v rx
i_q
e_d
x
+
vd
x
LS,1
vd
s0
s1
Look up
Sin/Cos
Table
s2 s3 s4
Clark
s5 s6
s7
Park
s8
+
vq
i_d
s9 s10 s11 s12 s13 s14 s15 s16
d-axis PI
controller
q-axis PI
controller
s17
Park-1
3
+
x
x
-
2
+
e_d
i_d
v
v
0 +
kp_d
+
x
s18
2
x
+
-
v rz
+
v ry
+
v
s19 s20 s21 s22
s23
Modified
Clark-1
13
3.4 模糊控制演算法之控制IP設計
r* (k )
a0
m (k - 2)
m (k - 1)
b1
+
x
a1
b2
x
r* (k - 1)
 (k - 2)
+
+
+
e(k )
-
 p (k )
s1
SD
+
-
s3
s4
-
+
s6
s5
de(k )
+
s7
SD
s9
s8
 B j (de )
de j 1
-
dek
Computation of rotor speed error,
and error change
Computation of reference
model output
j
 Ai (e )
RS,1
+
RS,1
s11
s10
Fuzzification
c j ,i
j&i
Look-up
Fuzzy rule
table
c j ,i
c j ,i 1
c j 1 ,i
c j 1 ,i 1
di, j
 B j (de )
 Ai (e )
x
1
-
s12
Look-up fuzzy table
 Ai (e )
1
s13
 B j (de ) -
di, j
+
 B j 1 ( de )
x
x
d i , j 1
d i , j 1
d i 1 , j
s14
s15
x
s16
+
+
Ki
ui
x
+
uf
ui
Kp
x
iq* ( k )
c j ,i 1
d i  1, j
x
+
+
c j 1 ,i
 B j (de )
 Ai 1 ( e )
j&i
&
e( k - 1)
x
s2
e i 1
ek
e(k )
s0
i
m (k )
-
*
r
a2
-
x
x
e( k - 1)
d i 1 , j 1
x
x
+
s23
s24
c j 1 ,i 1
d i  1, j  1
x
s17
Defuzzification
s18
s19
s20
s21
s22
Computation of current
command
14
模糊控制規則庫
(e)
B1(de)
A4
A3
A5
A6
Fuzzy Inference and Output
E -600 -400 -200
dE
200
-50
600 600 400 200
600 400 200
400 200
0
de
-100 600 600 600 400 200
150 100 50
de
0
200 400 600
e
400 600
-150 600 600 600 600 400 200
B3
-50 -100 -150
1
B2(de)=1 - B1(de)
B0
A2
-600 -400 -200 0
B4
B2
A1
e
50
B5
A0
A (e)
3
0
B6
Input of de (for j=1)
B1
(de)
1
A (e)=1- A (e)
4
3
Input of e (for i=3)
100 200
150
0
0
0
0
0
0
-200
uf 
c13 * d 31  c23 * d 32  c14 * d 41  c24 * d 42
d 31  d 32  d 41  d 42
-200 -400
-200 -400 -600
-200 -400 -600 -600
-200 -400 -600 -600 -600 -600
(a)
Defuzzification
0
-200 -400 -600 -600 -600
Fuzzy Rule Table
Rule 1: e is A3 and de is B1 then uf is c13
Rule 2: e is A3 and de is B2 then uf is c23
Rule 3: e is A4 and de is B1 then uf is c14
Rule 4: e is A4 and de is B2 then uf is c24
where
d31 =A3(e)* B1(de)
d32 =A3(e)* B2(de)=  A3(e)*(1 -B1(de))
d41 =A4(e)* B1(de) =(1 - A3(e))* B1(de)
d42 = A4(e)* B2(de)=(1 - A3(e))*(1 -B1(de))
and d31 + d32 + d41 + d42 = 1
(b)
15
3.5 System-on-a-Programmable-Chip設計
Inverter

TA

A[22]
TC
ac source
A[0]
D[31]
U,V,W
C
-
TA
ia [11..0]
ib [11..0]
Nios II Embedded Processor IP

TB
-
Load
-
TC
TB
encoder
PMSM
Isolated and driving circuits
UART
CPU
D[0]
sram_be[3]
sram_be[2]
sram_be[1]
sram_be[0]
sram_oe
sram_we
sram_cs
On-chip
ROM
On-chip
RAM
Avalon Bus
L
Avalon Bus
Rectifier
ic [11..0]
PIO
iq* [11..0]
Timer
r [11..0]
SPI
 r* [11..0]
Speed control IP
Clk
Clk-cur
Clk-sp
Clk-step
Frequency
divider
CK
ADIN[11]
RS232 or
USB
PWM1
LP filter
circuit
PWM6
PC
ia
ADC
Digital circuit (PLD) of
adaptive speed control
and current vector
control for PMSM
ib
ADC
Application IP
Nios processor is
used for command
generation and
receive feedback
data
Clk
ADIN[0]
Clk-sp
Clk-step
 r* [11..0]
 p [15..0]
ADC read in
Adaptive fuzzy
controller
Clk
(AFC)
B, B
Comparator
circuit
iq* [11..0]
 p [15..0]
clk
z, z
A-pulse
B-pulse
Z-pulse
QEP detection
and
transformation
ia [11..0]
and
ib [11..0] transformation
Clk-cur
A, A
A, B
z
Embedded Processor IP
(Nios II Processor)
Clk
 e _ addr [11..0]
Current controllers
and coordinate
transformation
(CCCT)
ic [11..0]
Clk
BDIN[0]
CHA
CHB
RCA
RCB
STSA
STSB
Clk-step
Clk-cur
v rx [11..0]
SVPWM
v ry [11..0]
generation
v rz [11..0]
FPGA-based speed control IC
(a)
BDIN[11]
(b)
16
PWM 1
PWM 2
PWM 3
PWM 4
PWM 5
PWM 6
Quartus II下以SoPC架構實現
軟體架構(SoPC)
硬體架構(位置、速度及電流迴路)
17
3.6 軌跡規劃-圓軌跡
Y
r
X
Start
18
3.7 軌跡規劃-星行軌跡規劃
星形軌跡方程式如下：
a 軌跡：

b 軌跡：
c 軌跡：
d軌跡：
e軌跡：

其中S 為位置增加量，其大小可改變
運動的速度； xi為X 軸命令軌跡；
yi為Y軸命令軌跡；經由上述之軌跡方
程式，隨時間經過即可產生星形軌跡
。
19
4.1實驗硬體架構介紹
SYNCHRONOUS SERVO MOTOR
XYZ平台規格
滾珠導桿距離
滾珠導桿導程
X軸、Y軸
30cm
0.5cm
Z軸
15cm
0.5cm
Encoder – 2500 P/R
Ra : 0.63 R
La : 2.77 mH
VOLTS – 92 V
AMP-S-1.6 A
OUT : 200W
TORQUE : 0.64 (N-m)
NR.rpm - 3000-
Protection : IP65
Nmx -3000 rpm
Poles: 8
20
4.2 高速演算法則控制IP實驗之演算法則
硬體計算時間測試架構
RS232 or
USB
Digital circuit (PLD) of
adaptive speed control
and current vector
control for PMSM
PC
Embedded Processor IP
(Nios II Processor)
Application IP
oscilloscope
FPGA-based speed control IC
演算法則在FPGA執行時間之量測架構
21
IP演算法則硬體計算時間測試
SVPWM執行時間之量測結果
電流向量控制器執行時間之量測結果
5.83 s
模糊控制器執行時間之量測結果
22
位置、速度PI參數設計
速度響應使用
電流PI參數
速度PI參數
KP
kp=0.02441
(12bitQ11為800)
kp=0.9760
(16bitQ15為
32000)
KI
ki=0.0003
(12bitQ11為10)
ki=0.0003
(12bitQ11為10)
速度響應Kp、Ki參數
位置響應使用
電流PI參數
速度PI參數
位置PI參數
KP
kp=0.02441
kp=0.02441
kp=0.45776
(12bitQ11為800) (12bitQ11為800) (12bitQ11為800)
KI
ki=0.0003
(12bitQ11為10)
ki=0.0003
(12bitQ11為10)
kp=0
(12bitQ11為800)
位置響應Kp、Ki參數
23
單軸速度響應實驗結果
Speed (rpm)
1900
1800
1700
1600
1500
1400
Rotor
speed
Speed
command
0
3
6
9
12
15
18
21
24
27
30
time(s)
18
21
24
27
30
time(s)
Control effort (A)
(a)
1
iq*
0.5
0
-0.5
-1
0
3
6
9
12
15
Speed (rpm)
(b)
2000
1000
0
-1000
-2000
Rotor speed
Speed
command
0
3
Curent (A)
1
iq
0.5
6
9
(a)
12
15
18
time(s)
6
9
(b)
12
15
18
time(s)
id
0
-0.5
-1 0
3
Speed (rpm)
2000
1000
1200
900
600
Rotor
speed
3
1
Curent (A)
1500
900
300
0
1800
1200
600
500
0
1500
Speed
command
1500
300
6
9
6
9
(a)
12
15
18
21
time(s)
12
15
18
21
time(s)
iq*
0.5
0
iq
-0.5
-1
0
3
(b)
24
單軸位置響應實驗結果
Position
command
10000
Position
8000
Table
position
6000
4000
2000
0
0
4
8
12
16
time(s)
20
24
28
32
24
28
32
1.5
i
Curent(A)
1
q
0.5
0
i
-0.5
-1
0
4
d
8
12
16
time(s)
20
5
x 10
Position
2
Position
command
1.5
1
Table
position
0.5
0
0
2
4
6
8
time(s)
10
12
14
16
18
12
14
16
18
1.5
Curent(A)
1
i
0.5
d
0
-0.5
i
-1
-1.5
0
2
4
6
8
q
time(s)
10
5
Position
x 10
2
1.5
Position
command
1
Table
position
0.5
0
0
1.9
3.8
5.7
7.6
time(s)
9.5
11.4
13.3
9.5
11.4
13.3
Curent(A)
1
i
0.5
d
0
-0.5
-1
0
i
1.9
3.8
5.7
25
q
7.6
time(s)
100
X-Axis error(mm) Two axis error(mm)
雙軸位置圓軌跡響應實驗結果
Position
command
90
Table
position
80
70
(mm)
60
50
40
0.8
0.6
0.4
0.2
0
Y-Axis error(mm)
20
10
0
0
30
40
50 60
(mm)
70
80
90 100
5
x 10
X-Axis Curent(A)
Table
position
1
0.5
2
0
3.3
6.7
5
x 10
10
13.3
time(s)
16.7
Table
position
Position
command
0
10
13.3
16.7
20
0
3.3
6.7
time(s)
10
13.3
16.7
20
0
3.3
6.7
10
13.3
time(s)
16.7
20
0.2
0
-0.2
-0.4
id
0.5
0
-0.5
0
iq
3.3
6.7
1
0.5
0
time(s)
0.4
-1
20
1.5
1
6.7
1
Position
command
1.5
0
Y-Axis Position
20
Y-Axis Curent(A)
X-Axis Position
2
10
3.3
0
-0.5
30
0
0.5
3.3
6.7
10
time(s)
13.3
16.7
20
10
13.3
time(s)
16.7
20
16.7
20
id
0.5
0
-0.5
iq
-1
-1.5
0
3.3
6.7
10
time(s)
13.3
26
X-Axis error(mm) Two axis error(mm)
雙軸位置星行軌跡響應實驗結果
100
90
Position
command
80
70
(mm)
60
50
Table
position
40
10
0
10
20
30
40
50 60
(mm)
70
80
90
100
0
X-Axis Curent(A)
Position
command
1.5
1
Table
position
0.5
0
10
5
x 10
2
1.5
time(s)
20
10
-1 0
10
time(s)
20
30
20
30
20
30
20
30
20
30
0.5
0
time(s)
2
1
0
-1
-2
0
10
time(s)
id
0.5
0
-0.5
-1
30
iq
0
10
time(s)
1.5
Position
command
Table
position
1
0.5
0 0
0
1
1
Y-Axis Curent(A)
X-Axis Position
5
x 10
2
Y-Axis Position
0.5
Y-Axis error(mm)
20
0
1
-0.5
30
0
1.5
10
time(s)
20
30
1
id
0.5
0
-0.5
-1
-1.50
iq
10
time(s)
27
結論
本文應用FPGA的硬體快速計算及可程式能力來處理伺服系統
的控制演算法則以提升其運動性能。本文所完成之技術說明
如下：
(1)可應用在永磁同步伺服馬達位置控制系統上。
(2)空間向量脈波調變演算法之計算時間為2.308  s、電流向
量控制演算法之計算時間為1.948  s而模糊控制器演算法則
之計算時間為5.668  s。
(3)可在SoPC 環境下使用與性能驗證。
28
Thank you for your attention.