# Solving optimal control problem when input is constrained

Given a linear time-invariant system: $$x'(t)=Ax(t)+Bu(t)$$ with initial state $x(0)=x0$ and final state $x(T)=xT$.

The performance measure to be minimized is: $$∫_0^Tu(t)^2dt$$

The most important thing is to constrain the input $u$.

What would be the optimal control trajectories in this specific case? I know NDSolve can solve TVBVP, but I don't know how to deal with the constraint of input $u$.

I would appreciate any help on this!

The settings are inside the code. The codes:

A = {{0, 0, 0, 1, 0, 0},{0, 0, 0, 0, 1, 0},{0, 0, 0, 0, 0, 1},{-10.169, 1.406, 10.848, 0, 0, 0},{-15.135, -17.618, 16.146, 0, 0, 0},{26.186, -3.62, -10.883, 0, 0, 0}};

B = {{0, 0, 0},{0, 0, 0},{0, 0, 0},{-0.03, -0.045,0.0789},{-0.0456,0.571, 0.117},{0.0789, 0.1174, -0.0791}};

boundary condition are

x0 = {{-0.2}, {0.2}, {0.2}, {0}, {0}, {0}};
xT = {{0.2}, {-0.2}, {0.2}, {0}, {0}, {0}};


terminal time is

T = 1;


cost funtion

L[t_] = 1/2 (u1[t]^2 + u2[t]^2 + u3[t]^2);


state and costate and input

lambda[t_] := {{l1[t]}, {l2[t]}, {l3[t]}, {l4[t]}, {l5[t]}, {l6[t]}};
x[t_] := {{x1[t]}, {x2[t]}, {x3[t]}, {x4[t]}, {x5[t]}, {x6[t]}};
u[t_] := {{u1[t]}, {u2[t]}, {u3[t]}};


state space form

f[t_] = A.x[t] + B .u[t];


Hamilton

H[t_] = Flatten[L[t] + lambda[t]\[Transpose].f[t]][[1]];


Calculate the u*

u1Sol = First@Solve[0 == -D[H[t], u1[t]], u1[t]];
u2Sol = First@Solve[0 == -D[H[t], u2[t]], u2[t]];
u3Sol = First@Solve[0 == -D[H[t], u3[t]], u3[t]];


Define the state and costate

TableForm[ eqn1 = Table[D[lambda[t][[i, 1]], t] == -D[H[t] /. u1Sol /. u2Sol /. u3Sol, x[t][[i, 1]]], {i, 1, 6, 1}]]; TableForm[ eqn2 = Table[D[x[t][[i, 1]], t] == D[H[t] /. u1Sol /. u2Sol /. u3Sol, lambda[t][[i, 1]]], {i, 1, 6, 1}]]

Define boundary condition

bc1 = Table[x[0][[i, 1]] == x0[[i, 1]], {i, 1, 6, 1}]
bc2 = Table[x[T][[i, 1]] == xT[[i, 1]], {i, 1, 6, 1}]


Calculate the TVBVP

sol = NDSolve[Flatten[{eqn1, eqn2, bc1, bc2}], {x1[t], x2[t], x3[t], x4[t], x5[t], x6[t], l1[t], l2[t], l3[t], l4[t], l5[t], l6[t]}, {t, 0, T}]

and I want to constrain $u$ as $-100<u1<100$,$-50<u2<50$,$-50<u3<50$.

Where should I insert the constraints?

• Do you have a meaningful example as test case? I hate coming up with my own one. So far, I can only say that you have to formulate your problem as an optimization problem with quadratic objective, affine-linear equality constraints, and affine-linear inequality constraints, a so-called quadratic program. In principle NMinimize should be able to solve that if you don't use a too fine discretization of the ODE. In general, I would advice the semi-smooth Newton algorithm. I have an implementation on it on my hard drive. Interested? Jun 24 '18 at 20:41
• Yes, I'm interested in it. Thank you
– sun
Jun 24 '18 at 22:46
• Would you please post a test case? Jun 24 '18 at 23:54
• I already posted it. Thank you!
– sun
Jun 25 '18 at 0:33

Semi-smooth Newton solver

This is supposed to solve constrained optimization problems of the form

$$\text{Minimize } F(x) \text{ subject to } \varPhi(x) = 0 \text{ and } \varPsi(x) \leq 0.$$

More precisely, it attempts to solve the KKT conditions for $$x$$ and the Lagrange multipliers $$\lambda$$ and $$\mu$$:

$$\begin{array}{rcl} DF(x) + \lambda^T \, D\varPhi(x) + \mu^T \, D\varPsi &=&0,\\ \varPhi(x) &= &0,\\ \varPsi(x) &\leq &0,\\ \mu & \geq &0,\\ \mu^T \varPsi(x) &=&0. \end{array}$$

These conditions are necessary conditions for a local minimum if $$\varPhi$$ and $$\varPsi$$ satisfy certain constraint qualifications (e.g., Mangasarian-Fromovitz constraint qualification or Slater condition (for convex problems)) and are sufficient if the optimization problem is convex. The current problem is a quadratic program with strictly convex objective, so it is convex.

For some more mathematical background of the algorithm, see chapter 2 in these great lecture notes by Michael Hintermüller.

ClearAll[SemiSmoothNewton];
Options[SemiSmoothNewton] = {
"EqualityMultiplier" -> Automatic,
"InequalityMultiplier" -> Automatic,
"MaxIterations" -> 1000,
"Tolerance" -> 10^-8,
"ArmijoSlope" -> 0.001,
"BacktrackingFactor" -> 0.25,
"InitialStepSize" -> 1.,
"MaxBacktrackingIterations" -> 20,
"PrintReport" -> True
};

SemiSmoothNewton[x0_, F_, Φ_, Ψ_, OptionsPattern[]] :=
Module[{iter, biter, x, y, z, λ, μ, τ, xτ, λτ, μτ, n, mΦ, mΨ, TOL, τ0, residual, maxiter, maxbiter, σ, γ, Fval, ΦQ, ΨQ, Θ0, DΘ0, Θτ, DΘτ, ϕ0, ϕτ, u, δx, δλ, δμ, timing, maxstepsize},

ΦQ = Φ =!= None;
ΨQ = Ψ =!= None;

x = x0;
n = Length[x];
If[ΦQ, mΦ = Length[Φ[x]], mΦ = 0];
If[ΨQ, mΨ = Length[Ψ[x]], mΨ = 0];

λ = OptionValue["EqualityMultiplier"];
If[λ === Automatic, λ = ConstantArray[0., mΦ]];
μ = OptionValue["InequalityMultiplier"];
If[μ === Automatic, μ = ConstantArray[0., mΨ]];

iter = 0;
maxstepsize = 0.;
TOL = OptionValue["Tolerance"];
maxiter = OptionValue["MaxIterations"];
maxbiter = OptionValue["MaxBacktrackingIterations"];
σ = OptionValue["ArmijoSlope"];
γ = OptionValue["BacktrackingFactor"];
τ0 = OptionValue["InitialStepSize"]/γ;
residual = 2 TOL; (*enforce first iteration*)
timing = AbsoluteTiming[
While[
residual > TOL && iter < maxiter
,
iter++;
{Θ0, DΘ0} = SSNΘDΘ[x, λ, μ, F, Φ, Ψ];
ϕ0 = Θ0.Θ0;
u = -LinearSolve[DΘ0, Θ0];
δx = u[[1 ;; n]];
δλ = u[[n + 1 ;; n + mΦ]];
δμ = u[[n + mΦ + 1 ;; n + mΦ + mΨ]];

(*backtracking line search*)
biter = 0;
τ = τ0;
ϕτ = 2 ϕ0; (*enforce first iteration*)
While[
ϕτ >= (1. - σ τ) ϕ0  && biter < maxbiter,
biter++;
τ = γ τ;
xτ = x + τ δx; λτ = λ + τ δλ; μτ = μ + τ δμ;
Θτ = SSNΘ[xτ, λτ, μτ, F, Φ, Ψ];
ϕτ = Θτ.Θτ;
];
residual = Sqrt[ϕτ/n];
If[biter === maxbiter, Print["Oops. Backtracking was interrupted."]];
x = xτ; λ = λτ; μ = μτ;
maxstepsize = Max[maxstepsize, τ];
Fval = F[x];
];
][[1]];
If[iter === maxiter, Print["Oops. Maximal number of iterations reached without satisfying the tolerance goal."]];
Association[
"Solution" -> x,
"EqualityMultiplier" -> λ,
"InequalityMultiplier" -> μ,
"ObjectiveValue" -> Fval,
"Iterations" -> iter,
"Timing" -> timing,
"Residual" -> residual,
"MaxStepSize" -> maxstepsize
]
];

SSNΘ[x_, λ_, μ_,F_, Φ_, Ψ_] := Join[F'[x] + λ.Φ'[x] + μ.Ψ'[x], Φ[x], Ramp[Ψ[x] + μ] - μ]
SSNΘDΘ[x_, λ_, μ_, F_, Φ_, Ψ_] := With[{A = Φ'[x], B = Ψ'[x], zμ = Ψ[x] + μ},
With[{a = SparseArray[UnitStep[zμ - $$MachineEpsilon] +$$MachineEpsilon]},
{
Join[F'[x] + λ.A + μ.B, Φ[x], Ramp[zμ] - μ],
ArrayFlatten[{
{F''[x] + λ.Φ''[x] + μ.Ψ''[x], A\[Transpose], B\[Transpose]},
{A, 0., 0.},
{a B, 0., DiagonalMatrix[a - 1.]}
}]
}
]
];

SSNΘ[x_, λ_, {}, F_, Φ_, Ψ_] := Join[F'[x] + λ.Φ'[x], Φ[x]]
SSNΘDΘ[x_, λ_, {}, F_, Φ_, Ψ_] := With[{A = Φ'[x]},
{
Join[F'[x] + λ.A, Φ[x]],
ArrayFlatten[{
{F''[x] + λ.Φ''[x], A\[Transpose]},
{A, 0.}
}]
}
];

SSNΘ[x_, {}, μ_, F_, Φ_, Ψ_] := Join[F'[x] + μ.Ψ'[x], Ramp[Ψ[x] + μ] - μ]
SSNΘDΘ[x_, {}, μ_, F_, Φ_, Ψ_] := With[{B = Ψ'[x], zμ = Ψ[x] + μ},
With[{a =  SparseArray[UnitStep[zμ - $$MachineEpsilon] +$$MachineEpsilon]},
{
Join[F'[x] + μ.B, Ramp[zμ] - μ],
ArrayFlatten[{
{F''[x] + μ.Ψ''[x], B\[Transpose]},
{a B, DiagonalMatrix[a - 1.]}
}]
}
]
];

SSNΘ[x_, {}, {}, F_, Φ_, Ψ_] := F'[x];
SSNΘDΘ[x_, {}, {}, F_, Φ_, Ψ_] := {F'[x], F''[x]};


#Casting the problem into a constrained optimization problem

Problem specifications.

ToPack = DeveloperToPackedArray;
A = ToPack[N[{{0, 0, 0, 1, 0, 0}, {0, 0, 0, 0, 1, 0}, {0, 0, 0, 0, 0,
1}, {-10.169, 1.406, 10.848, 0, 0, 0}, {-15.135, -17.618, 16.146, 0, 0, 0}, {26.186, -3.62, -10.883, 0, 0, 0}}]];
B = ToPack[N[{{0, 0, 0}, {0, 0, 0}, {0, 0, 0}, {-0.03, -0.045,0.0789}, {-0.0456, 0.571, 0.117}, {0.0789, 0.1174, -0.0791}}]];
x0 = ToPack@N@{-0.2, 0.2, 0.2, 0, 0, 0};
xT = ToPack@N@{0.2, -0.2, 0.2, 0, 0, 0};
umin = {-100., -50., -50.};
umax = {100., 50., 50.};
T = 1.;


Discretization of ODE (n time steps, Crank-Nicolson scheme, control piecewise-constant in time).

n = 1000;
τ = N[T/n];
ndofs = n Dimensions[B][[2]];
dim = Length[A];

uumin = Flatten[ConstantArray[umin, n]];
uumax = Flatten[ConstantArray[umax, n]];
AA = SparseArraySparseBlockMatrix[{
{1, 1} -> IdentityMatrix[dim, SparseArray],
Band[{2, 2}] -> IdentityMatrix[dim, SparseArray] + τ/2 SparseArray[A],
Band[{2, 1}] -> -IdentityMatrix[dim, SparseArray] + τ/2 SparseArray[A]
},
{n + 1, n + 1}, 0.
];
BB = SparseArraySparseBlockMatrix[{Band[{2,2}] -> τ SparseArray[B]}, {n + 1, n + 1}, 0.];

AAinv = LinearSolve[AA];
A1 = Transpose[AAinv[Join[ConstantArray[0., {(n + 1) dim - dim, dim}], N@IdentityMatrix[dim]], "T"]];
A2 = A1[[All, dim + 1 ;;]].BB;
b = A1[[All, 1 ;; dim]].x0;

trajectory[u_] := Partition[AAinv[Join[x0, BB.u]], dim];


Defining objective funtion F, equality constraint mapping Φ and inequality constraint mapping Ψ along with their first two derivatives.

F[u_?VectorQ] := 1/(2 n) u.u;
F'[u_?VectorQ] := u/n;
F''[u_?VectorQ] = N[1/n IdentityMatrix[ndofs, SparseArray]];

Φ[u_?VectorQ] := b + A2.u - xT;
Φ'[u_?VectorQ] = SparseArray[A2];
Φ''[u_?VectorQ] = SparseArray[{}, {dim, ndofs, ndofs}, 0.];

Ψ[u_?VectorQ] := Join[u - uumax, uumin - u];
Ψ'[u_?VectorQ] = Join[N@IdentityMatrix[ndofs, SparseArray], -N@ IdentityMatrix[ndofs, SparseArray]];
Ψ''[u_?VectorQ] = SparseArray[{}, {2 ndofs, ndofs, ndofs}, 0.];


Creating a starting point for Newton search and performing the actual search.

u0 = LeastSquares[A2, xT];
data = SemiSmoothNewton[u0, F, Φ, Ψ, "Tolerance" -> 10^-8]; // AbsoluteTiming // First
u = data[["Solution"]];


0.306972

Plotting the results.

ListLinePlot[Transpose[{Rest@Subdivide[0., T, n], #}] & /@ Transpose[Partition[u, 3]],
AxesLabel -> {"t", "u"},
PlotLegends -> Table["u" <> ToString[i], {i, 1, Dimensions[B][[2]]}],
PlotLabel -> "Controls"
]

ListLinePlot[
Transpose[{Subdivide[0., T, n], #}] & /@ Transpose[trajectory[u]],
AxesLabel -> {"t", "u"},
PlotLegends -> Table["x" <> ToString[i], {i, 1, dim}],
PlotLabel -> "Trajectories"
]


• It's the timing of the whole optimization process. The result is data[["Solution"]]. Whether this is long or not lies in the eye of the beholder. Maybe it can be made a bit faster with another linear solver. Inspecting Dataset@data will tell you that only 6(!) steps of Newton's method were needed. Simply solving the ODE once with NDSolve needs 0.047 seconds, so there is actually not much discrepancy. Notice also that NDSolve` computes only 70 time steps (though probably with a higher order method). Jun 26 '18 at 13:37
• I drew most of what I know about the semismooth Newton algorithm from this script by by Michael Hintermüller, basically chapter 2. Maybe that will also help you. Jun 27 '18 at 6:28
• Would you mind explaining further or show me the code? How to change the cost function to the form of $u1∗x4+u2∗x5+u3∗x6$ ? I don' t really understand how to apply such a step.
– sun
Jun 27 '18 at 6:49
• In total, this is a fairly standard problem in optimal control and there are plenty of tools around for solving them. I just posted the semi-smooth Newton alrgorithm, because it is not so well-known and because I already had code for it. I don't mean to be rude, but implementing another method would take me several hours and nobody is going to pay me for that. Jul 6 '18 at 6:55
• @Xminer Yes, of course. It is the Fréchet derivative (also called total derivative or Jacobian). Mar 20 '19 at 11:53