Post on 13-Dec-2015
Introduction to the first laboratory exerciseContinuous neural network sliding mode controller
DSP2 realization
Andreja Rojkoandreja.rojko@uni-mb.si
TARET PROJECT
Andreja Rojko 2Beljak, February 2007
OUTLINE• Overview• Dynamic model of the experimental mechanism• Simulation model of the experimental mechanism• Sin2 velocity profile – reference trajectory• PI position control• Controller algorithm – NN• Controller algorithm - C code • Controller algorithm - test• Complete simulation scheme• Experiment – C code, test on DSP2• Experiment
Andreja Rojko 3Beljak, February 2007
Dynamic model of the experimental mechanism• General dynamic model of mechanism:
• Inertia:
• Spring torque:
,)()(),().(nTF
fGhMT
][2
22
mkgrm
M C
springspring FdT
sin xkrTspring
Andreja Rojko 4Beljak, February 2007
Dynamic model of the experimental mechanism• Dynamic model of the experimental mechanism:
• Gear ratio:
d
lrllrrrlrkr
rmT C
CCC )sin(
cos2)(2
222
shaftmotor
C
r
rN
_
N
TTmotor
N
N
N
motor
motor
motor
Andreja Rojko 5Beljak, February 2007
Simulation model of the experimental mechanism
tau_controller
Parameters are defined in m-file model_cylinder_spring_parameters.m
sin
TrigonometricFunction1
cos
TrigonometricFunction
delta_x
To Workspace4
time
To Workspace3
theta_ddot
To Workspace2
theta_dot
To Workspace1
theta
To WorkspaceStep
Product1
Product
sqrt
MathFunction1
1
u
MathFunction
1s
Integrator1
1s
Integrator
1/J
Inertia
l+rcGain3
r*k
Gain22*r*(rc+l)
Gain1
12:34
Digital Clock
l
Constant1
(rc+l)^2+r^2
Constant
thetaddot thetadot
d
theta
delta_xTau_spring
sin(alf a)
1/d
(l+r)/d
Andreja Rojko 6Beljak, February 2007
Sin2 velocity profile – reference trajectory• For position and velocity control of mechanisms we need
reference trajectory: desired position, desired velocity and desired acceleration. The reference trajectory is defined with the following data:– initial position( theta_initial), end position (theta_final)– maximum velocity (vmax) – maximum acceleration,(amax)
C code is written insin2_profi l_wrapper.c.
(1 5)
vmax, amax
(0 pi)
theta_initial, theta_final
time
To Workspace3
theta_ddot_ref
To Workspace2
theta_dot_ref
To Workspace1
theta_ref
To Workspace
Scope
sin2_profi l
S-Function
12:34
Digital Clock
Demux
t
theta_ref
theta_dot_ref
theta_ddot_ref
Andreja Rojko 7Beljak, February 2007
Sin2 velocity profile – reference trajectory
0 0.5 1 1.5 2 2.5 3 3.5 4 4.5 50
2
4
0 0.5 1 1.5 2 2.5 3 3.5 4 4.5 50
0.5
1
0 0.5 1 1.5 2 2.5 3 3.5 4 4.5 5-5
0
5
t [s]
theta ref [rad]
thetadot ref [rad/s]
thetaddot ref [rad/s2]
Andreja Rojko 8Beljak, February 2007
PI position control
Position control of DC motor - realization on DSP-2
(11 11)
vmax, Amax3
[0 1000*pi]
theta_initial, theta_final
DSP-2 TTpos_error
DSP-2 TTi_ref
DSP-2 TTtheta_ref
DSP-2 TTw_filt
UpdateParameters
EditParameters
Build
Scope3
sin2_profi l
S-Function
PI positionregulator PI current
regulator
FDATool
Low pass fi lterFc=200Hz
Ke
[reset] [reset]12:34
Digital Clock
Demux
Ua
TL
f i
w
Ia
DCmotor
DSP-2Interface
DC motorDSP-2 interface
i_reftheta_ref
theta_ref
theta_dot_ref
theta_ddot_ref
•PI position controller, PI current controller.
•Set the parameters of PI position controller.
•'trial and error' procedure.
Andreja Rojko 9Beljak, February 2007
Controller algorithm - NN• Neural netwok with two layers will be constructed for the use in
CSMNN control algorithm. • Nonlinear threshold function will be used for the hidden layer, so
that its output will have values between -1 and 1. • Linear threshold function will be used for the output layer.• Sketch the NN structure! Mark the lines with the names of
variables.• For learning algorithm, the equations form E-book will be used.• Case study from e-book ‘direct drive robot’ -> rewrite into
equations for our one axis experimental mechanism.• We will use different names because we cannot use Greek
letters in C code...table of the preferable variables names and their sizes is in the instructions.
Andreja Rojko 10Beljak, February 2007
Controller algorithm - C code• For programming the controller algorithm in C, we will use S-
Function Builder block.• NN’s weights should be declared as global variables and
initialized to small numbers between -1 and 1 (Example1).• Writing the C code:
– Don’t try to write the whole algorithm at once!
– First write and test the code for calculation of the output from NN.
– Then add the learning algorithm for the output layer and then, when this is working, also for the hidden layer.
– It is advisable to check some parts of the program (specially loops) in Matlab m-file!
Andreja Rojko 11Beljak, February 2007
Controller algorithm - test
[1 2 3 1.07 2.02 3.1]
theta, theta_dot, theta_ddot,theta_ref, theta_dot_ref, theta_ddot_ref
(20 1e-3)
m, epsilon
(1)
enableNN
Scope1
CSMNN_control_sim
S-Function
(10 1 10 10)
Kp, Kv, M, D
TAU
wJ(2,1)
wJ(1,0)
wL(1,0)
wL(1,1,)
Andreja Rojko 12Beljak, February 2007
Controller algorithm - test
Andreja Rojko 13Beljak, February 2007
Complete simulation scheme
wJ(1,0)
Model parameters are inmodel_cylinder_spring_parameters.m
tau_controller
(1 5)
vmax, amax
(0 pi)
theta_initial, theta_final
[1 2 3 1.07 2.02 3.1]
theta, theta_dot, theta_ddot,theta_ref, theta_dot_ref, theta_ddot_ref
(10 1e-6)
m, epsilon
(1)
enableNN1
sin
TrigonometricFunction1
cos
TrigonometricFunction
tau
To Workspace5
time
To Workspace4
theta_ref
To Workspace3
theta_dot_ref
To Workspace2
theta_dot
To Workspace1
theta
To WorkspaceStep
Scope2
Scope1
Scopesin2_profi l
S-Function, sin2
CSMNN_control_sim
S-Function
Product1
Product
Memory3
sqrt
MathFunction1
1
u
MathFunction
[12 0.8 1e-3 18]
Kp, Kv, M, D1
1s
Integrator1
1s
Integrator
l+rcGain3
r*k
Gain2
2*r*(rc+l)
Gain1
1/J
Gain
12:34
Digital Clock
Demux
l
Constant1
(rc+l)^2+r^2
Constant
theta_ref
theta_reftheta_dot_ref
theta_dot_ref
theta_ddot_ref
TAUwJ(2,1)
wL(1,0)
wL(2,1)
d
q
t
ref erence
actual
Andreja Rojko 14Beljak, February 2007
Experiment – C code• Declaration and initialization of the global variables should be
altered for C compiler on DSP2 board!• Reason: Matlab uses different C code compiler then it is used on
DSP2 board. • First test, if algorithm’s C code is working on DSP2 (without
mechanism).
Andreja Rojko 15Beljak, February 2007
Experiment – testTest of neural network algorithm - realization on DSP-2
wJ(1,0)
(6 1e-1)
m, epsilon
(1)
enableNN1
DSP-2 TTWJ2
DSP-2 TTWL2
DSP-2 TTWL
DSP-2 TTWJ
DSP-2 TTTorque
UpdateParameters
EditParameters
Build
Sine Wave5
Sine Wave4
Sine Wave3
Sine Wave2
Sine Wave1
Sine Wave
Scope2
CSMNN_control1input output
S-Function Builder
(10 0.8 1e-3 10)
Kp, Kv, M, D1
0
Constant
DSP-2AO 0
TAUwJ(2,1)
wL(1,0)
wL(2,1)
Andreja Rojko 16Beljak, February 2007
ExperimentCSMNN Position control of DC motor - realization on DSP-2
wJ
(1 1)
vmax, amax
Kv
velocity gain
(0 600*pi)
theta_initial, theta_final
Kp
position gain
m
m, num neurons
epsilon
lernaing rate
M
inertia
D
gain
enableNN
enable NN
z-1
zderivative
DSP-2 TTwL2
DSP-2 TTwL1
DSP-2 TTwJ2
DSP-2 TTwJ1
DSP-2 TTerror
DSP-2 TTtauNN
DSP-2 TTtheta_ref
DSP-2 TTw_filt
UpdateParameters
EditParameters
Build
Scope3
Scope2
CSMNN_control1
S-Function1
sin2_profil
S-Function
PI currentregulator
FDATool
Low pass filterFc=200Hz
Ke
reset12:34
Digital ClockDemux
Ua
TL
f i
w
Ia
DCmotor
DSP-2Interface
DC motorDSP-2 interface
theta_dot, f iltered
theta_dot, f iltered
theta_dot, f iltered
theta_ref
theta_dot_ref
thetaddot_ref
TAU
TAUwJ
wJ
wL
wL
theta
theta_ddot
Andreja Rojko 17Beljak, February 2007
Conclusion• Read the instructions for each step carefully before you begin to
work.• Between work write the report: explain each step, give the
results, explain the results!• Required part: Experiment with PI position control, Simulation of
the CSMNN control algorithm with the neural network, teleoperation by using PI position control.
• Desired: Experiment with CSMNN (even if it is not working perfectly).
• NOTE! In the experimental part don’t let the motor oscillating for too long, because it can be damaged!