Extending Mathematica to Solve Control Problems

Using the classical example of controlling an inverted pendulum, we learn how to formulate a control problem in Mathematica, solve the problem using the Control System Professional functionality, and analyze the results using the standard Mathematica functions. We will see that, by being seamlessly incorporated with the rest of Mathematica, this application package provides a convenient environment for solving typical control engineering problems. Additional solved examples will be given in later chapters when individual functions are discussed. 
The inverted pendulum shown in Figure 1 is a massive rod mounted on a cart that moves in a horizontal direction in such a way that the rod remains vertical. The vertical position of the rod is unstable, and the cart exerts a force to provide the attitude control of the pendulum. This or a similar model is considered in many textbooks on control systems.

Let us obtain a mathematical model for the system. Assume that the length of the pendulum is L, and its mass and the moment of inertia about its center of gravity are m and J, respectively. The mass of the cart is M. Then, summing the forces applied to the pendulum in horizontal and vertical directions (Figure 2a), we have

\(F\_x = m \( x\&¨\_c\)\&\)

\(F\_y - m  g = m \(\( y\&¨\_c\)\&\ \)\)

where (Subscript[F, x]) and (Subscript[F, y]) are the components of the reaction force at the support point, and (Subscript[x, c]) = X + L/2  sin  θ and (Subscript[y, c]) = L/2  cos  θ are the horizontal and vertical displacements of the center of gravity of the pendulum; (Subscript[x, c]) depends on the horizontal displacement X of the cart. Summing all the moments around the center of gravity of the pendulum gives the dynamical equation

\(TraditionalForm\`\(\(\(F\_y\) L\)\/2\) 
          sin  θ - \(\(\(F\_x\) L\)\/2\) cos  θ = 
      J \( θ\&¨\)\&\)

where J = m (L^2)/12, which corresponds to the case of uniform mass distribution along the pendulum.

Finally, for the cart we have (see Figure 2b)

\(TraditionalForm\`f\_x - F\_x = M \( X\&¨\)\&\)

where (Subscript[f, x]) is the input force applied to the wheels. 
Translating the model into Mathematica is straightforward.

This is the first equation. We will keep it as eq1 for future reference.

eq1=Fx==m xc''[t]

The next equation translates almost verbatim as well.

eq2=Fy - m g==m yc''[t]

This is the dynamical equation.

eq3=(1/2) Fy L Sin[θ[t]] - 
          (1/2) Fx L Cos[θ[t]]==J θ''[t]

This gives the definition for the moment of inertia J.

J = ((m (L^2))/12)

Here is the last equation.

eq4=fx - Fx==M X''[t]

We now define the horizontal displacement xc of the pendulum through the displacement of the cart X and the angle θ. As the two depend on time t, then so does xc. The pattern notation t_ on the left-hand side makes the formula work for any expression t.

xc[t_] = X[t] + (1/2) L Sin[θ[t]]

Here is the corresponding assignment to the vertical displacement yc.

yc[t_] = (1/2) L Cos[θ[t]]

Notice that we have defined eq1 … eq4 as logical equations (using the double equation mark ==) and the expressions for xc and yc as assignments to these symbols (using the single =). We then have the benefit of not solving the differential equation against X''[t], but simply eliminating it algebraically along with the other variables we no longer need.

Eliminate takes the list of equations and the list of variables to eliminate. The result is a nonlinear differential equation between the input force fx and the angular displacement θ and its first and second derivatives. It is solvable for θ''[t].

Eliminate[{eq1, eq2, eq3, eq4},{Fx,Fy,X''[t]}]

Solve returns a list of rules that give generic solutions to the input equation.

Solve[%,θ''[t]]

We have, in fact, just a single rule and we extract it from the lists.

sln = %[[1, 1]]

As our next step, we create a state-space model of the system and linearize it for small perturbations near the equilibrium position  θ = 0. Then, based on the linearized model, we design the state feedback controller that attempts to keep the pendulum in equilibrium. Finally, we carry out several simulations of the actual nonlinear system governed by the controller and see what such a controller can and cannot do. 
The nonlinear state-space model of the system will be presented in the form

({{(OverDot["x"])="f"("x","u")}, {"y"="h"("x","u")}})

where θ and \(TraditionalForm\`\(\(\ \)\(θ\& . \)\)\) constitute the state vector "x", (Subscript[f, x]) is the only component of the input vector "u", and θ makes up the output vector "y".

This creates the state vector in Mathematica.

x={θ[t],θ'[t]};

This sets the input and output vectors.

u = {fx}; y = {θ[t]};

To obtain "f" and "h", we observe that their Mathematica equivalents f and h are simply the derivative D[x, t] and the output vector y expressed via the state and input variables.

The expression for the derivative contains an undesirable variable, θ''[t], which is among neither state nor input variables.

D[x, t]

The replacement rule stored as sln helps to get rid of θ''[t].

f = % /. sln

The expression for function h is trivial.

h = y

So far we have used the built-in Mathematica functions. Now it's time to make accessible the library of functions provided in Control System Professional.

This loads the application.

<< ControlSystems`

For most Control System Professional functions, the input state-space model must be linear. Therefore, our first task will be to linearize the model, that is, represent it in the form

({{(OverDot["x"])="A"x+"B"u}, {"y"="C"x+"D"u}})

This is the purpose of the function Linearize, which, given the nonlinear functions f and h and the lists of state and input variables, supplied together with values at the nominal point (the point in the vicinity of which the linearization will take place), returns the control object StateSpace[a, b, c, d], where matrices a, b, c, and d are the coefficients "A", "B", "C", and "D".

This performs the linearization.

ss=Linearize[f,h,{{θ[t], 0},{θ'[t],0}},{{fx, 0}}]

Mapping the built-in Mathematica function Factor onto components of the state-space object simplifies the result somewhat. (Here /@ is a shortcut for the Map command.)

Factor /@ %

TraditionalForm often gives a more compact representation for control objects.

TraditionalForm[%]

Now let us design a state feedback controller that will stabilize the pendulum in a vertical position near the nominal point. One way to do this is to place the poles of the closed-loop system at some points (Subscript[p, 1]) and (Subscript[p, 2]) on the left-hand side of the complex plane.

In this particular case, Ackermann's formula is used. The result is a matrix comprising the feedback gains.

k = StateFeedbackGains[ss, {p1, p2}]

Note that we were able to obtain a symbolic solution to this problem and thus see immediately that, for example, only the first gain depends on g and so would be affected should our pendulum get sent to Mars (and the change would be linear in g). We also see that the first gain depends on the product of pole values, the second gain on their sum, and so on. 
To check if the pole assignment has been performed correctly, we can find the poles of the closed-loop system, that is, the eigenvalues of the matrix "A"-"B""K".

This extracts the matrices from their StateSpace wrapper.

a = ss[[1]]; 
    b = ss[[2]];

We see that the eigenvalues of the closed-loop system are indeed as required.

Eigenvalues[a - b . k]

With Control System Professional, we can also design the state feedback using the optimal linear-quadratic (LQ) regulator. This approach is more computationally intensive, so it is advisable to work with inexact numeric input. For convenience in presenting results, we switch to the control print display.

This is the particular set of numeric values (all in SI) we will use.

numericValues = {m -> 2. , M -> 8. , L -> 1. , 
        g -> 9.8, p1 -> -4. , p2 -> -5. }

Here our system is numericalized.

nss = ss /. numericValues

Let "Q" and "R" be identity matrices.

Q = IdentityMatrix[2]

R = {{1}}

LQRegulatorGains solves the Riccati equations and returns the corresponding gain matrix.

LQRegulatorGains[nss, Q, R]

Here are the poles our system will possess when we close the loop.

Eigenvalues[a - b . % /. numericValues]

Let us make some simulations of the linearized system as well as the original, nonlinear system stabilized with one of the controllers we have designed—say the one obtained with Ackermann's formula. We start with the linearized system and compute the transient response of the system for the initial values of θ(0) of 0.5, 1, and 1.2, assuming in all cases that \(TraditionalForm\`\(\(\ \)\(\(θ\& . \)(0) = 0\)\)\). The same initial conditions will then be used for the nonlinear system, and the results will be compared.

Here is the list of initial conditions for θ.

(Subscript[θ, 0]) = { .5, 1. , 1.2}

This is the linearized system after the closing state feedback. The function StateFeedbackConnect is described in Chapter 6 together with other utilities for interconnecting systems.

StateFeedbackConnect[ss, k] // Simplify

To compute how the initial condition in θ decays in the absence of an input signal, we can use OutputResponse.

In this particular case, the input arguments to OutputResponse are the system to be analyzed, the input signal (which is 0 for all t), the time variable t, and the initial conditions for the state variables supplied as an option. The initial value for θ is denoted as angle.

OutputResponse[%, 0, t, 
      InitialConditions -> {angle, 0}]

Here is the plot of the previous function for the chosen values (Subscript[θ, 0]). We store it as plot for future reference.

plot = 
        Plot[Evaluate[% /. numericValues /. angle -> (Subscript[θ, 0])], {t, 0, 4}, 
          PlotStyle -> RGBColor[1, 0, 0]];

The case of actual nonlinear system stabilized with the linear controller is more interesting, but requires some work on our part. We note that when the control loop is closed, the input variable—the force (Subscript[f, x]) applied by the motor of the cart—tracks changes in state variables θ(t) and \(TraditionalForm\`\(\(\ \)\(\(θ\& . \)(t)\)\)\).

First we prepare the input rules. As we have only one input, there is only one rule in the list.

feedbackRules = 
      Thread[u -> -k . 
                x] /. numericValues

Recall that we store the description of our nonlinear system as sln.

sln

Now we numericalize the rule, substitute the feedback rules, and, to convert the rule to an equation, apply the head Equal to it (@@ is the shorthand form of the Apply function). The resultant differential equation is labeled de.

de = Equal @@ 
            sln /. numericValues /. feedbackRules

This solves the differential equation with the initial conditions for every value in the list (Subscript[θ, 0]) one by one and returns a list of solutions. The time t is assumed to vary from 0 to 4 seconds.

(Subscript[Θ, 0])=NDSolve[{de,θ[0] == #,θ'[0]==0},{θ},{t, 0, 4}]&/@(Subscript[θ, 0])

In several graphs that follow, we show the results for θ(0) = 0 as a solid line, for θ(0) = 1 as a dashed-dotted one, and for θ(0) = 1.2 as a dashed line. This changes the Plot options to reflect that convention and adjusts a few other nonautomatic values for plot options.

SetOptions[Plot, 
        PlotStyle -> {Thickness[ .001], 
            Dashing[{ .025,  .0075,  .0075,  .0075}], Dashing[{ .01,  .01}]}, 
        Frame -> {Automatic, Automatic, None, None}, 
        FrameLabel -> {"Time (s)", None}];

The results for θ are now presented graphically. We can see that the controller succeeds in driving the pendulum to its equilibrium position for all three initial displacements. The plot is stored as plot1.

plot1 = 
        Plot[Evaluate[θ[
                t] /. (Subscript[Θ, 0])], {t, 0, 4}, 
          PlotLabel -> "θ (radian)"];

We can also see that, once the angle θ[t] has come to zero, the derivative θ'[t] vanishes as well. This means that the pendulum is not about to oscillate around its equilibrium position, at least not when driven from the displacements we are considering for now.

Plot[Evaluate[θ'[t]/.(Subscript[Θ, 0])],{t, 0, 4},PlotLabel -> "θ' (radian/s)"];

Here is the plot of input force versus time.

\(\(Plot[
        Evaluate[\(fx /. feedbackRules\) /. Θ\_0], {t, 0, 4}, 
        PlotLabel -> "Input Force (Newton)", 
        PlotRange -> All];\)\)

Finally, we compare the graphs of θ for the nonlinear and linear systems and see that the only case of smallest initial displacement is treated adequately by the linear model.

Show[plot1, plot];

The transient responses suggest that our linear feedback is not sufficiently prompt in reacting to moderate and large initial displacements θ(0), and that may cause problems for still larger angles. The case θ(0) = 1.2 rad is almost critical. Indeed, for a slightly larger displacement, θ(0) = 1.25  rad, the system becomes hard to control.

We solve the same equation for another set of initial conditions.

(Subscript[θ, Big])=NDSolve[{de,θ[0] == 1.25,θ'[0]==0},{θ},{t, 0, 4},MaxSteps -> 1000]

In the following graphs, we will plot the results for θ(0) = 1.25 as a solid line and one of our previous curves (namely, θ(0) = 1.2) as a dashed line. This sets the new options.

SetOptions[Plot, 
        PlotStyle -> {Thickness[ .001], 
            Dashing[{ .01,  .01}]}];

We find that the pendulum still could be driven from θ(0) = 1.25 to θ = 0, but now it oscillates badly around the equilibrium point.

\(\(Plot[
        Evaluate[θ[
              t] /. {θ\_Big, Θ\_0[[3]]}], {t, 0, 4}, 
        PlotLabel -> "θ (radian)"];\)\)

Of course, the cart in our particular model of the pendulum would not allow the pendulum to rotate in circles, but, for the sake of argument, we will assume that it would.

The variations in \(TraditionalForm\`\(\(\ \)\(θ\& . \)\)\) become more complex and far more intense.

Plot[Evaluate[θ'[t]/.{(Subscript[θ, Big]), (Subscript[Θ, 0])[[3]]}],{t, 0, 4},PlotLabel -> "θ' (radian/s)"];

This is the force the motor must exert to maintain the process.

\(\(Plot[
        Evaluate[\(fx /. feedbackRules\) /. {θ\_Big, Θ\_0[[3]]}], {t, 0, 4}, 
        PlotLabel -> "Input Force (Newton)", 
        PlotRange -> All];\)\)

The real actuator may not be up to the task. If the maximum force the motor can provide is, say, 1000 N, and the feedback saturates at that limit, the controller fails to balance the pendulum.

To model this situation, we create a clip function.

clip = If[Abs[#] ≤ 1000. , #, Sign[#] 1000. ] &

Here is how it works: everything beyond the interval from -1000 to 1000 gets cut off.

clip /@ {-1001, -999, 999, 1001}

We use clip to saturate the feedback.

feedbackLtd = MapAt[clip, feedbackRules, {1, 2}]

This is the new differential equation for θ under the saturated feedback.

de1 = Equal @@ 
            sln /. numericValues /. feedbackLtd

This solves it.

(Subscript[θ, Big1])=NDSolve[{de1,θ[0] == 1.25,θ'[0]==0},{θ},{t, 0, 4},MaxSteps -> 1000]

Finally, we plot the state response—for θ as a solid line and for \(TraditionalForm\`\(\(\ \)\(θ\& . \)\)\) as a dashed one. It is clear that the controller fails to return the pendulum to its equilibrium position.

Plot[Evaluate[{θ[t],θ'[t]}/.(Subscript[θ, Big1])],{t, 0, 4},PlotStyle -> {Thickness[ .001], {Dashing[{ .05,  .01}], 
                Thickness[ .001]}},PlotRange -> All,PlotLabel -> "State Response"];