Partitioned control  trajectory following (stable, linear system, imperfect 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

RowBox[{RowBox[{system, , =, , RowBox[{{, RowBox[{RowBox[{m, ->, 1.1}], ,, , RowBox[{b, ->, 1.05}], ,, , RowBox[{k, ->, 0.95}]}], }}]}], ;}]

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_124.gif]

Error plot

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

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

Applied torques

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

[Graphics:../HTMLFiles/index_128.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_132.gif]

Error plot

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

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

Applied torques

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

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

Compare the two simulations

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

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

Control simulation #3 (integral gain as well)

Control gain vectors

kp = 25// N ; kv = 10 // N ; ki = 3 // 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 #3

Join trajectories

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

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

Error plot

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

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

Applied torques

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

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

Compare the las two simulations

Show[GraphicsArray[{{gtraj2, gtraj3}, {gerror2, gerror3}, {gtorques2, gtorques3}}], ImageSize500] ;

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

Created by Mathematica  (November 12, 2003)