Code covered by the BSD License  

Highlights from
Optimal Control Using Control Vector Parameterization

image thumbnail
from Optimal Control Using Control Vector Parameterization by Chang Duan
An example of using control vector parameterization to solve an optimal control problem

Optimal Control Using Control Vector Parameterization

Optimal Control Using Control Vector Parameterization

Control vector parameterization, also known as direct sequential method, is one of the direct optimization methods for solving optimal control problems. The basic idea of direct optimization methods is to discretize the control problem, and then apply nonlinear programming (NLP) techniques to the resulting finite-dimensional optimization problem.

Contents

Problem Statement

The problem is that you wish to steer from point $A=(0,0)$ at time $t = 0$ to close to point $B=(4,4)$ at time T. The motion takes place in the $x_1, x_2$ plane. Your control variables are thrust $u$ and angle of thrust $\theta$. The angle $\theta$ is measured from the $x_1$ axis. To make life interesting there is large mass at (3,0) that exerts a force proportional to the inverse of the square of the distance you are from the mass. The force points toward the large mass. This force has a parameter $\beta$. The Newtonian formulation in Cartesian coordinates, assuming your mass is 1 and there is no resistance is:

$$\dot{x}_1 = x_3$$

$$\dot{x}_2 = x_4$$

$$\dot{x}_3 = \cos(\theta)u + q_1(x)$$

$$\dot{x}_4 = \sin(\theta)u + q_2(x)$$

where

$$q_1(x) = \frac{\beta(3-x_1)}{(x_2^2+(x_1-3)^2)^{3/2}}, \quad q_2(x) = \frac{\beta(-x_2)}{(x_2^2+(x_1-3)^2)^{3/2}}$$

The cost functional is:

$$J=\rho((x_1(T)-4)^2+(x_2-4)^2)+\int_0^T(10u^2+4\theta^2)dt$$

In addtion, trust is bounded, $|u|\leq 1$, no bound on $\theta$. $T=6$.

Parameter Configurations

Remarks: $N$, $\rho$ and $\beta$ are chosen arbitrarily. One can vary these parameters to see its impact on the results.

T = 6; % terminal time
N = 3; % number of control stages
rho = 100; % weight on missing the final target
beta = 1; % parameters of the external force
x0 = zeros(5,1); % initial state
ts = 0:(T/N):T;

% Options for ODE & NLP Solvers
optODE = odeset( 'RelTol', 1e-9, 'AbsTol', 1e-9 );
optNLP = optimset( 'LargeScale', 'off', 'GradObj','off', 'GradConstr','off',...
    'DerivativeCheck', 'off', 'Display', 'iter', 'TolX', 1e-9,...
    'TolFun', 1e-9, 'TolCon', 1e-9, 'MaxFunEvals',5000,...
    'DiffMinChange',1e-5,'Algorithm','interior-point');

Piecewise constant control

In this section, control $u$ and $\theta$ are assumed to be piecewise constant in each stage of uniform length.

Remarks: Due to the nonconvexity of this problem, you may end up with a optimal point that is local. Changing the initial guess dvar0, you may get completely different solution. Thus to find global otpimal or multiple local optimal, one can make use of the multistart function from Global Optimization Toolbox. main_multistart.m contains a script as a starting point.

dvar0 = [repmat(0.5,1,N),repmat(-0.1,1,N),4,4]; % design variables contains $N$ pieces of
% $u$, $N$ pieces of $\theta$ and the final position
lb = -Inf(1,2*N+2);
lb(1:N) = -1; % enforce lower bound on control signal $u$
ub = Inf(1,2*N+2);
ub(1:N) = 1; % enforce upper bound on control signal $u$

% Sequential Approach of Dynamic Optimization
[dvarO,JO] = fmincon(@(dvar) costfun1(x0,ts,dvar,rho,N,beta,optODE),...
    dvar0,[],[],[],[],lb,ub,...
    @(dvar) confun1(x0, dvar, ts, N, beta, optODE),optNLP);

% plot
[topt,xopt,uopt,thetaopt] = plotopt1( x0,N,ts,dvarO,beta,rho,optODE );
% animation
M = animateopt( topt,xopt,uopt,thetaopt,beta,N,rho );
movie2avi(M, ['betaN_' int2str(beta) '_' int2str(N) '.avi'],...
    'compression','Cinepak','fps',10)

                                            First-order      Norm of
 Iter F-count            f(x)  Feasibility   optimality         step
    0       9   1.524000e+001   1.076e+000   1.965e+001
    1      19   9.377437e+000   3.601e+000   4.060e+001   1.206e+000
    2      36   6.895389e+000   3.736e+000   8.706e+000   1.903e-001
    3      53   6.653659e+000   3.622e+000   1.660e+001   2.211e-002
    4      63   6.592960e+000   3.597e+000   1.868e+001   5.468e-002
    5      78   6.751868e+000   3.690e+000   8.957e+000   2.405e-002
    6      88   8.602348e+000   3.566e+000   1.537e+002   1.889e-001
    7     100   4.129357e+001   2.993e+000   9.278e+001   1.567e+000
    8     112   4.226601e+001   2.909e+000   9.492e+001   3.401e-001
    9     122   4.453187e+001   1.620e+000   1.022e+002   1.048e+000
   10     133   3.759971e+001   1.384e+000   3.679e+002   4.669e-001
   11     147   4.582005e+001   6.931e-001   1.846e+001   4.948e-001
   12     162   4.521022e+001   4.104e-001   2.101e+001   2.515e-001
   13     175   5.333200e+001   3.633e-001   2.127e+001   4.422e-001
   14     185   5.133203e+001   3.594e-001   1.187e+002   6.080e-001
   15     195   5.352301e+001   3.626e-002   4.705e+001   8.170e-001
   16     205   5.021449e+001   4.616e-002   2.800e+001   4.668e-001
   17     215   4.560641e+001   7.372e-002   1.288e+002   2.386e-001
   18     224   4.423655e+001   9.180e-003   8.926e+001   9.880e-002
   19     236   4.359363e+001   7.345e-003   1.167e+002   8.885e-002
   20     246   4.119136e+001   9.065e-003   1.324e+002   5.370e-001
   21     255   3.987785e+001   1.592e-002   8.740e+001   8.321e-002
   22     265   3.912088e+001   2.083e-002   7.251e+000   1.632e-001
   23     274   3.865954e+001   9.783e-003   7.136e+001   8.962e-002
   24     283   3.873452e+001   2.254e-003   4.549e+001   1.882e-002
   25     296   3.866053e+001   8.360e-004   4.281e+001   1.457e-002
   26     310   3.817942e+001   1.834e-003   4.222e+000   6.578e-002
   27     322   3.808927e+001   2.001e-003   6.734e+000   1.605e-002
   28     336   3.765888e+001   5.454e-004   5.128e+000   6.461e-002
   29     354   3.652951e+001   3.545e-003   8.029e+000   1.158e-001
   30     367   3.624417e+001   3.250e-003   1.143e+001   2.373e-002

                                            First-order      Norm of
 Iter F-count            f(x)  Feasibility   optimality         step
   31     384   3.336932e+001   2.050e-002   1.626e+001   2.085e-001
   32     395   3.257211e+001   1.888e-002   1.909e+001   4.739e-002
   33     412   2.697912e+001   2.877e-002   3.479e+001   4.484e-001
   34     423   2.586951e+001   1.577e-002   3.425e+001   5.599e-002
   35     442   2.335252e+001   3.223e-002   3.594e+001   4.331e-001
   36     453   2.252318e+001   3.134e-002   3.521e+001   2.948e-002
   37     463   1.019554e+001   3.040e-002   1.813e+001   5.099e-001
   38     472   6.148550e+000   1.913e-002   1.162e+001   3.979e-001
   39     481   5.869631e+000   1.967e-002   1.114e+001   9.169e-002
   40     490   5.693761e+000   1.296e-002   1.201e+001   1.062e-001
   41     499   5.609284e+000   4.660e-003   1.260e+001   4.562e-002
   42     509   5.537935e+000   3.856e-005   1.277e+001   4.076e-002
   43     519   5.501603e+000   1.702e-006   1.271e+001   1.486e-002
   44     529   5.400289e+000   4.766e-005   1.058e+001   3.196e-002
   45     539   5.243929e+000   1.624e-005   8.720e+000   4.521e-002
   46     549   5.011358e+000   1.948e-004   6.854e+000   7.628e-002
   47     559   4.868786e+000   1.598e-004   3.580e+000   5.682e-002
   48     568   4.833245e+000   3.222e-004   7.751e-001   1.915e-002
   49     577   4.829065e+000   1.159e-004   2.745e-001   1.074e-002
   50     586   4.828938e+000   2.558e-004   8.950e-002   6.112e-003
   51     595   4.828795e+000   1.332e-006   7.973e-002   1.354e-003
   52     604   4.828772e+000   8.349e-006   2.849e-002   8.795e-004
   53     613   4.828774e+000   5.068e-007   4.034e-003   1.947e-004
   54     622   4.828774e+000   2.488e-009   4.280e-004   1.462e-005
   55     631   4.828774e+000   1.896e-011   1.401e-005   1.020e-006
   56     640   4.828774e+000   3.428e-012   3.724e-006   4.898e-008
   57     649   4.828774e+000   4.405e-013   1.720e-007   1.104e-008
   58     658   4.828774e+000   3.464e-014   1.614e-008   7.163e-010

Local minimum possible. Constraints satisfied.

fmincon stopped because the size of the current step is less than
the selected value of the step size tolerance and constraints are 
satisfied to within the selected value of the constraint tolerance.



Optimal cost for N=3: 4.822457e+000

Continuous linear spline control

In this section, control $u$ and $\theta$ are assumed to be continuous linear spline in each stage of uniform length.

dvar0 = [repmat(0.5,1,N+1),repmat(-0.1,1,N+1),4,4]; % design variables contains $N+1$
% start and end points of control signal $u$, $\theta$ and the final
% position
lb = -Inf(1,2*(N+1)+2);
lb(1:N+1) = -1; % enforce lower bound on control signal $u$
ub = Inf(1,2*(N+1)+2);
ub(1:N+1) = 1; % enforce upper bound on control signal $u$

% Sequential Approach of Dynamic Optimization
[dvarO,JO] = fmincon(@(dvar) costfun2(x0,ts,dvar,rho,N,beta,optODE),...
    dvar0,[],[],[],[],lb,ub,...
    @(dvar) confun2(x0, dvar, ts, N, beta, optODE),optNLP);

% plot
[topt,xopt,uopt,thetaopt] = plotopt2( x0,N,ts,dvarO,beta,rho,optODE );
% animation
M = animateopt( topt,xopt,uopt,thetaopt,beta,N,rho );
movie2avi(M, ['betaN2_' int2str(beta) '_' int2str(N) '.avi'],...
    'compression','Cinepak','fps',10)

                                            First-order      Norm of
 Iter F-count            f(x)  Feasibility   optimality         step
    0      11   1.524000e+001   1.076e+000   1.913e+001
    1      23   6.628266e+000   3.580e+000   3.646e+001   1.177e+000
    2      43   5.211388e+000   3.600e+000   1.863e+001   1.274e-001
    3      55   3.748959e+000   3.536e+000   1.296e+001   2.791e-001
    4      72   4.967599e+000   3.419e+000   6.662e+000   1.307e-001
    5      84   6.928451e+000   3.181e+000   2.944e+001   2.682e-001
    6      97   1.281084e+001   2.791e+000   3.654e+001   6.935e-001
    7     113   1.971879e+001   1.336e+000   5.193e+001   6.732e-001
    8     124   1.591789e+001   1.609e+000   1.967e+001   1.844e+000
    9     137   1.119402e+001   5.256e-001   2.191e+001   1.028e+000
   10     149   8.334419e+000   3.809e-001   3.054e+001   1.049e+000
   11     161   6.284996e+000   1.508e-001   4.504e+000   1.119e+000
   12     175   5.373304e+000   2.105e-001   3.418e+000   1.846e-001
   13     188   4.901003e+000   2.147e-001   5.815e+000   5.549e-001
   14     200   4.698544e+000   5.094e-002   2.636e+000   1.908e-001
   15     211   4.706038e+000   3.628e-002   8.827e-001   2.800e-001
   16     222   4.685036e+000   3.664e-002   6.517e-001   1.319e-001
   17     233   4.677286e+000   4.450e-004   6.289e-001   9.156e-003
   18     245   4.669954e+000   2.969e-004   5.368e-001   5.228e-002
   19     257   4.664877e+000   1.638e-005   2.247e-001   2.060e-002
   20     268   4.664341e+000   1.261e-004   4.499e-002   7.897e-003
   21     280   4.664340e+000   6.198e-005   2.348e-002   7.504e-004
   22     291   4.664343e+000   1.077e-006   2.003e-003   8.749e-004
   23     302   4.664342e+000   1.597e-009   2.530e-004   2.403e-005
   24     313   4.664342e+000   5.778e-011   8.000e-006   3.268e-006
   25     324   4.664342e+000   7.390e-012   5.807e-007   3.379e-007
   26     335   4.664342e+000   1.803e-013   3.412e-008   8.638e-009

Local minimum possible. Constraints satisfied.

fmincon stopped because the size of the current step is less than
the selected value of the step size tolerance and constraints are 
satisfied to within the selected value of the constraint tolerance.



Optimal cost for N=3: 4.659332e+000


Published with MATLAB® 7.12

Contact us