Partitioned control  trajectory following (nonlinear system, imperfect model)

Differential equation

Define simple second-order differential equation with input f

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

Display differential equation

diffeq // TraditionalForm

q x^3 + b x/t + m ^2x/t^2f

Numeric system values

RowBox[{RowBox[{system, , =, , RowBox[{{, RowBox[{RowBox[{m, ->, 1.02}], ,, , RowBox[{b, ->, 1.01}], ,, , RowBox[{q, ->, 0.98}]}], }}]}], ;}]

Numerical model values

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

Controller specification

Controller type

controlLaw = PartitionedControlNonlinear ;

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, no integral term)

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

Error plot

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

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

Applied torques

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

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

Error plot

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

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

Applied torques

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

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

Compare previous two simulations

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

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

Control simulation #3 (even larger gains, integral gain as well)

Control gain vectors

kp = 100// N ; kv = 20 // N ; ki = 10 // 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_181.gif]

Error plot

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

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

Applied torques

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

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

Compare the las two simulations

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

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

Created by Mathematica  (November 12, 2003)