Optimal Control: A Simple Example

H. G. Kwatny
MEM Department
Drexel University

Introduction

We will consider variations of the following problem. The dunamics are goven by

f = {x2, -x2 + u};

We wish to steer from an arbitrary intial state to a the origin while minimizing cost

J[u_]:=(∫_(Subscript[t, 1])^(Subscript[t, 2]))(1+(1/2)(u^2))\[DifferentialD]t

No Constraints

H=1+u^2/2+λ1 x2 - λ2 x2 +λ2 u;
λ1dot=-D[H,x1]
λ2dot=-D[H,x2]

solu=Solve[D[H,u]==0,u]

Fixed Terminal Time

Traj=FullSimplify[DSolve[{D[x1[t],t]==x2[t],D[x2[t],t]==-x2[t]+λ2[t],D[λ1[t],t]==0,D[λ2[t],t]==-λ1[t]+λ2[t],x1[0]== a,x2[0]==b,x1[5]== 0,x2[5]==0},{x1[t],x2[t],λ1[t],λ2[t]},t]];
SolRules=Traj/.{a->1,b->1};
Plot[{x1[t]/.SolRules,x2[t]/.SolRules,-λ2[t]/.SolRules},{t,0,5},AxesLabel->{"t","(Subscript[x, 1]),(Subscript[x, 2]),u"},PlotStyle->Thick]

Traj=FullSimplify[DSolve[{D[x1[t],t]==x2[t],D[x2[t],t]==-x2[t]+λ2[t],D[λ1[t],t]==0,D[λ2[t],t]==-λ1[t]+λ2[t],x1[0]== a,x2[0]==b,x1[2]== 0,x2[2]==0},{x1[t],x2[t],λ1[t],λ2[t]},t]];
SolRules=Traj/.{a->1,b->1};
Plot[{x1[t]/.SolRules,x2[t]/.SolRules,-λ2[t]/.SolRules},{t,0,2},AxesLabel->{"t","(Subscript[x, 1]),(Subscript[x, 2]),u"},PlotStyle->Thick]

Traj=FullSimplify[DSolve[{D[x1[t],t]==x2[t],D[x2[t],t]==-x2[t]+λ2[t],D[λ1[t],t]==0,D[λ2[t],t]==-λ1[t]+λ2[t],x1[0]== a,x2[0]==b,x1[2]== 0,x2[2]==0},{x1[t],x2[t],λ1[t],λ2[t]},t]]

SolRules=Traj/.{a->1,b->1};

Plot[{x1[t]/.SolRules,x2[t]/.SolRules,-λ2[t]/.SolRules},{t,0,2},AxesLabel->{"t","(Subscript[x, 1]),(Subscript[x, 2]),u"}]

Traj=FullSimplify[DSolve[{D[x1[t],t]==x2[t],D[x2[t],t]==-x2[t]+λ2[t],D[λ1[t],t]==0,D[λ2[t],t]==-λ1[t]+λ2[t],x1[0]== a,x2[0]==b,x1[1]== 0,x2[1]==0},{x1[t],x2[t],λ1[t],λ2[t]},t]];
SolRules=Traj/.{a->1,b->1};
Plot[{x1[t]/.SolRules,x2[t]/.SolRules,-λ2[t]/.SolRules},{t,0,1},AxesLabel->{"t","(Subscript[x, 1]),(Subscript[x, 2]),u"}]

K=Join[Map[(({x1[t],x2[t]}/.Traj[[1]])/.{a->#,b->1})&,Range[0,1,0.2]],Map[(({x1[t],x2[t]}/.Traj)/.{a->#,b->-1})&,Range[-1,0,0.2]]];

K[[1]]

gr1=ParametricPlot[K,{t,0,2},AxesLabel->{"(Subscript[x, 1])","(Subscript[x, 2])"}]

K=Join[Map[(({x1[t],x2[t]}/.Traj[[1]])/.{a->#,b->1})&,Range[-5,1,0.2]],Map[(({x1[t],x2[t]}/.Traj)/.{a->#,b->-1})&,Range[-1,5,0.2]]];

K=Map[(({x1[t],x2[t]}/.Traj[[1]])/.{a->#,b->0})&,Range[-5,5,0.2]];

gr2=ParametricPlot[K,{t,0,2},PlotRange->{{-1,1},{-1,1}}]

Show[gr1,gr2]

Free Terminal Time

Solve[((H/.solu[[1]])/.{x1->0,x2->0})==0,λ2]

ψ=x1^2+x2^2-ϵ^2;
K=NullSpace[{{D[ψ,x1],D[ψ,x2]}}]

K.{λ1,λ2}

Reduce[-((x2 λ1)/x1)+λ2==0,{λ1,λ2}]/.{x1->γ Cos[θ],x2->γ Sin[θ]}

ψ=x1^2+x2^2-γ^2;

Solve adjoint backwards:

solλ=DSolve[{D[λ1[t],t]==0,D[λ2[t],t]==λ1[t]-λ2[t],λ2[0]==Sin[θ] (Sqrt[2])/Abs[Sin[θ]],λ1[0]==Cos[θ]  (Sqrt[2])/Abs[Sin[θ]]},{λ1[t],λ2[t]},t]

Eqns={D[x1[t],t]==-x2[t],D[x2[t],t]==x2[t]+λ2[t]/.solλ[[1]],x1[0]==0,x2[0]==0}

solx=FullSimplify[DSolve[Eqns,{x1[t],x2[t]},t],Reals]

ParametricPlot[Map[(({x1[t],x2[t]}/.solx)/.θ->#)&,Range[-π,π,π/40]],{t,0,4},PlotRange->{{-1,1},{-1,1}},AxesLabel->{"(Subscript[x, 1])","(Subscript[x, 2])"}]

PlotList=Join[Range[π/40,π (1-1/40),π/40],Range[-π/40,-π (1-1/40),-π/40]];
ParametricPlot[Map[(({x1[t],x2[t]}/.solx)/.θ->#)&,PlotList],{t,0,4},PlotRange->{{-1,1},{-1,1}},AxesLabel->{"(Subscript[x, 1])","(Subscript[x, 2])"}]

Control Constraints

Now, we impose the constraint Abs[u]≤1. Thus, we need to minimize

H = 1 + u^2/2 + λ1 x2 - λ2 x2 + λ2 u;

subject to the control constraint.

solu=Solve[D[H,u]==0,u]

Implies u=-Sat[(Subscript[λ, 2])]

<<ProPac`

?Sat

Eq=1+Sat[μ]^2/2-μ Sat[μ]==0;
Reduce[Eq,μ,Reals]

solλ=DSolve[{D[λ1[t],t]==0,D[λ2[t],t]==λ1[t]-λ2[t],λ2[0]==Sin[θ](3/ 2)/Abs[Sin[θ]],λ1[0]==Cos[θ]  (3/ 2)/Abs[Sin[θ]]},{λ1[t],λ2[t]},t]

PlotList=Join[Range[π/20,π (1-1/20),π/20],Range[-π/20,-π (1-1/20),-π/20]];
gr1=Plot[Map[(({λ2[t]}/.solλ)/.θ->#)&,PlotList],{t,0,5},PlotRange->{{0,5},{-3,3}},AxesLabel->{"t","(Subscript[λ, 2])"}]

Plot[Map[(({λ2[t]}/.solλ)/.θ->#)&,{π/20,-π  (1-1/20)}],{t,0,5},PlotRange->{{0,5},{-3,3}},AxesLabel->{"t","(Subscript[λ, 2])"}]

PlotList=Range[π/20,π (1-1/20),π/20];
gr1=Plot[Map[(({λ2[t]}/.solλ)/.θ->#)&,PlotList],{t,0,5},PlotRange->{{0,5},{-2,3}},AxesLabel->{"t","(Subscript[λ, 2])"}]

gr2=Plot[{1,-1},{t,0,5},PlotStyle->{{Red}}]

Show[gr1,gr2]

Plot[({Sat[λ2[t]]}/.solλ)/.θ->8 π/20,{t,0,5},PlotRange->{{0,5},{-1.5,1.5}},AxesLabel->{"t","Sat[(Subscript[λ, 2])]"}]

Plot[({Sat[λ2[t]]}/.solλ[[1]])/.θ->16 π/20,{t,0,5},PlotRange->{{0,5},{-1.5,1.5}},AxesLabel->{"t","Sat[(Subscript[λ, 2])]"}]

u=(Sat[λ2[t]]/.solλ[[1]])/.θ->16 π/20;
Eqns16={D[x1[t],t]==-x2[t],D[x2[t],t]==x2[t]+u,x1[0]==0,x2[0]==0};
SolEqn=DSolve[Eqns16,{x1[t],x2[t]},t];
s=NDSolve[Eqns16,{x1,x2},{t,0,5}];

Plot[Evaluate[{x1[t],x2[t]}]/.s,{t,0,5},PlotRange->{{0,5},{-5,5}},AxesLabel->{"t","(Subscript[x, 1]),(Subscript[x, 2])"}]

grss1=ParametricPlot[Evaluate[{{x1[t],x2[t]}}/.s],{t,0,5},PlotRange->{{-2,2},{-2,2}},AxesLabel->{"(Subscript[x, 1])","(Subscript[x, 2])"}]

u=(Sat[λ2[t]]/.solλ[[1]])/.θ->8 π/20;
Eqns16={D[x1[t],t]==-x2[t],D[x2[t],t]==x2[t]+u,x1[0]==0,x2[0]==0};
SolEqn=DSolve[Eqns16,{x1[t],x2[t]},t];
s=NDSolve[Eqns16,{x1,x2},{t,0,5}];
Plot[Evaluate[{x1[t],x2[t]}]/.s,{t,0,5},PlotRange->{{0,5},{-5,5}},AxesLabel->{"t","(Subscript[x, 1]),(Subscript[x, 2])"}]
grss2=ParametricPlot[Evaluate[{{x1[t],x2[t]}}/.s],{t,0,5},PlotRange->{{-2,2},{-2,2}},AxesLabel->{"(Subscript[x, 1])","(Subscript[x, 2])"}]

u=(Sat[λ2[t]]/.solλ[[1]])/.θ->-π(1-8/20);
Eqns16={D[x1[t],t]==-x2[t],D[x2[t],t]==x2[t]+u,x1[0]==0,x2[0]==0};
SolEqn=DSolve[Eqns16,{x1[t],x2[t]},t];
s=NDSolve[Eqns16,{x1,x2},{t,0,5}];
Plot[Evaluate[{x1[t],x2[t]}]/.s,{t,0,5},PlotRange->{{0,5},{-5,5}},AxesLabel->{"t","(Subscript[x, 1]),(Subscript[x, 2])"}]
grss3=ParametricPlot[Evaluate[{{x1[t],x2[t]}}/.s],{t,0,5},PlotRange->{{-2,2},{-2,2}},AxesLabel->{"(Subscript[x, 1])","(Subscript[x, 2])"}]

u=(Sat[λ2[t]]/.solλ[[1]])/.θ->-π(1-16/20);
Eqns16={D[x1[t],t]==-x2[t],D[x2[t],t]==x2[t]+u,x1[0]==0,x2[0]==0};
SolEqn=DSolve[Eqns16,{x1[t],x2[t]},t];
s=NDSolve[Eqns16,{x1,x2},{t,0,5}];
Plot[Evaluate[{x1[t],x2[t]}]/.s,{t,0,5},PlotRange->{{0,5},{-5,5}},AxesLabel->{"t","(Subscript[x, 1]),(Subscript[x, 2])"}]
grss4=ParametricPlot[Evaluate[{{x1[t],x2[t]}}/.s],{t,0,5},PlotRange->{{-2,2},{-2,2}},AxesLabel->{"(Subscript[x, 1])","(Subscript[x, 2])"}]

u=(Sat[λ2[t]]/.solλ[[1]])/.θ->15 π/20;
Eqns16={D[x1[t],t]==-x2[t],D[x2[t],t]==x2[t]+u,x1[0]==0,x2[0]==0};
SolEqn=DSolve[Eqns16,{x1[t],x2[t]},t];
s=NDSolve[Eqns16,{x1,x2},{t,0,5}];
Plot[Evaluate[{x1[t],x2[t]}]/.s,{t,0,5},PlotRange->{{0,5},{-5,5}},AxesLabel->{"t","(Subscript[x, 1]),(Subscript[x, 2])"}]
grss5=ParametricPlot[Evaluate[{{x1[t],x2[t]}}/.s],{t,0,5},PlotRange->{{-2,2},{-2,2}},AxesLabel->{"(Subscript[x, 1])","(Subscript[x, 2])"}]

u=(Sat[λ2[t]]/.solλ[[1]])/.θ->-π(1-15/20);
Eqns16={D[x1[t],t]==-x2[t],D[x2[t],t]==x2[t]+u,x1[0]==0,x2[0]==0};
SolEqn=DSolve[Eqns16,{x1[t],x2[t]},t];
s=NDSolve[Eqns16,{x1,x2},{t,0,5}];
Plot[Evaluate[{x1[t],x2[t]}]/.s,{t,0,5},PlotRange->{{0,5},{-5,5}},AxesLabel->{"t","(Subscript[x, 1]),(Subscript[x, 2])"}]
grss6=ParametricPlot[Evaluate[{{x1[t],x2[t]}}/.s],{t,0,5},PlotRange->{{-2,2},{-2,2}},AxesLabel->{"(Subscript[x, 1])","(Subscript[x, 2])"}]

Show[grss1,grss2,grss3,grss4,grss5,grss6]