Introduction to the first laboratory exercise Continuous neural network sliding mode controller DSP2...

Introduction to the first laboratory exerciseContinuous neural network sliding mode controller

DSP2 realization

Andreja [email protected]

TARET PROJECT

mailto:[email protected]

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


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


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


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


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


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]


PI position control

Position control of DC motor - realization on DSP-2

(11 11)

vmax, Amax3

[0 1000*pi]


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.


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.

http://www.ro.feri.uni-mb.si/predmeti/int_reg/Predavanja/Lab_exercises/CSMNN_DSP2_realization.pdf


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!


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,)


Controller algorithm - test


Complete simulation scheme

wJ(1,0)

Model parameters are inmodel_cylinder_spring_parameters.m

tau_controller

(1 5)

vmax, amax

(0 pi)


[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


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).


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)


ExperimentCSMNN Position control of DC motor - realization on DSP-2

wJ

(1 1)

vmax, amax

Kv

velocity gain

(0 600*pi)


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_ref

theta_dot_ref

thetaddot_ref

TAU

TAUwJ

wJ

wL

wL

theta

theta_ddot


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!

Introduction to the first laboratory exercise Continuous neural network sliding mode controller DSP2...

Documents

Transcript of Introduction to the first laboratory exercise Continuous neural network sliding mode controller DSP2...