Partitioned control  trajectory following (stable, linear system, perfect model)

Differential equation

Define simple second-order differential equation with input f

diffeq = m  Dt[x, {t, 2}] + b Dt[x, t] + k x == f ;

Display differential equation

diffeq // TraditionalForm

k x + b x/t + m ^2x/t^2f

Numeric system values

system = {m->1, b1, k1} ;

Numerical model values

model = {m->1, b1, k1} ;

Controller specification

Controller type

controlLaw = PartitionedControlLinear ;

Controller frequency (Hz)

freq = 50 ;

Simulation parameters

Total simulation time

RowBox[{RowBox[{tmax, , =, , 15.}], ;}]

Desired trajectory (cubic interpolation)

path = {{0, 0}, {5, 4}, {7, 2}} ;

Create desired trajectory as interpolation function

desired = CreateDesiredTrajectory[path, {0, tmax, 1/freq}] ;

Control simulation #1 (small gains)

Control gain vectors

kp = 9// N ; kv = 6 // N ; ki = 0 // N ;

Control simulation

{actual, torques} = ControlAll[{diffeq/. system}, {x}, {f}, {{path[[1, 2]]}, {0}}, tmax, N[1/freq], {controlLaw, desired, kp, kv, ki, model}] ;

Visualization of results #1

Join trajectories

gdesired = PlotMultipleTrajectory[desired[[1]], {0, tmax}, {Blue}, nS] ; gactual = PlotMultipleTrajectory[actual, {0, tmax}, {Red}, nS] ; gtraj1 = Show[gdesired, gactual, yS, imgSize] ;

[Graphics:../HTMLFiles/index_95.gif]

Error plot

error = CombineInterpolationFunctions[actual, desired[[1]], {0, tmax}, Subtract] ; gerror1 = PlotMultipleTrajectory[error, {0, tmax}, {Green}, imgSize] ;

[Graphics:../HTMLFiles/index_97.gif]

Applied torques

gtorques1 = PlotMultipleTrajectory[torques, {0, tmax}, {Magenta}, imgSize] ;

[Graphics:../HTMLFiles/index_99.gif]

Control simulation #2 (larger gains)

Control gain vectors

kp = 25// N ; kv = 10 // N ; ki = 0 // N ;

Control simulation

{actual, torques} = ControlAll[{diffeq/. system}, {x}, {f}, {{path[[1, 2]]}, {0}}, tmax, N[1/freq], {controlLaw, desired, kp, kv, ki, model}] ;

Visualization of results #2

Join trajectories

gdesired = PlotMultipleTrajectory[desired[[1]], {0, tmax}, {Blue}, nS] ; gactual = PlotMultipleTrajectory[actual, {0, tmax}, {Red}, nS] ; gtraj2 = Show[gdesired, gactual, yS, imgSize] ;

[Graphics:../HTMLFiles/index_103.gif]

Error plot

error = CombineInterpolationFunctions[actual, desired[[1]], {0, tmax}, Subtract] ; gerror2 = PlotMultipleTrajectory[error, {0, tmax}, {Green}, imgSize] ;

[Graphics:../HTMLFiles/index_105.gif]

Applied torques

gtorques2 = PlotMultipleTrajectory[torques, {0, tmax}, {Magenta}, imgSize] ;

[Graphics:../HTMLFiles/index_107.gif]

Compare the two simulations

Show[GraphicsArray[{{gtraj1, gtraj2}, {gerror1, gerror2}, {gtorques1, gtorques2}}], ImageSize500] ;

[Graphics:../HTMLFiles/index_109.gif]

Created by Mathematica  (November 12, 2003)