PID trajectory following control (nonlinear system, no 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

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

Controller specification

Controller type

controlLaw = PIDNonlinear ;

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}] ;

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

Error plot

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

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

Applied torques

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

[Graphics:../HTMLFiles/index_60.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}] ;

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

Error plot

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

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

Applied torques

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

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

Compare the two simulations

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

[Graphics:../HTMLFiles/index_70.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}] ;

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

Error plot

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

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

Applied torques

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

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

Compare the las two simulations

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

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

Created by Mathematica  (November 12, 2003)