本文主要基于以下参考：

[1] John T. Betts. Survey of Numerical Methods for Trajectory Optimization.

[2] Anil V. Rao. A Survey of Numerical Methods For Optimal Control.

[3] John T. Betts. Practical Methods for Optimal Control and Estimation Using Nonlinear Programming 2nd.

[4] A E. Bryson. Applied Optimal Control.

[5] KIRK. Optimal Control Theory: An Introduction.

[6] Matthew Kelly. An Introduction to Trajectory Optimization: How to Do Your Own Direct Collocation.

最优控制问题数值方法-间接法/变分法 (2)

下面继续看[4] 2.5节，在前面两节中，最优控制问题描述为：

此时，最优控制问题的必要条件如下：

上面给出了必须满足的微分代数方程，下面给出必要的边界条件：

上式中一共包括有 2n 个边界条件，由于这些边界条件中包括有拉格朗日乘子 v^t ,因此额外还需要 q 个约束条件(7)。

下面举一个例子：

最优控制问题：

边界条件：

类似地，首先写出该最优控制问题的最优条件：

以及相应的控制变量的最优条件：

即：

下面写出相应的边界条件：

考虑：

此时边界条件可以写作：

下面给出求解过程的代码：

% orbit_bvp_how created by Geoff Huntington 2/21/07
% Solves the Hamiltonian Boundary Value Problem for the orbit-raising optimal
% control problem (p.66 Bryson & Ho). Computes the solution using BVP4C
% Invokes subroutines orbit_ivp and orbit_bound

clear all; % close all;
set(0, 'DefaultAxesFontSize', 14, 'DefaultAxesFontWeight','demi');
set(0, 'DefaultTextFontSize', 14, 'DefaultTextFontWeight','demi');

% Fixed final time %Tf = 3.3155;
Tf = 3.32;
four = 1; % not four means use bvp6c

% Constants
global mu m0 m1 T

mu=1; m0=1; m1=-0.07485; T= 0.1405;

n=100;
y = [  ones(1,n);            % r
         zeros(1,n);            % vr
         ones(1,n);             % vt
        -ones(1,n);            % lambda_r
        -ones(1,n);            % lambda_vr
        -ones(1,n)];           % lambda_vt

x = linspace(0,Tf,n);       % time

solinit.x = x;solinit.y = y;
% Set optimizer options
tol = 1E-10;
options = bvpset('RelTol',tol,'AbsTol',[tol tol tol tol tol tol],'Nmax', 2000);
% Solve
if four
    sol = bvp4c(@orbit_ivp,@orbit_bound,solinit,options);
    Nstep=40;
else
    sol = bvp6c(@orbit_ivp,@orbit_bound,solinit,options);
    Nstep=30;
end

figure(1); clf
plot(sol.x,sol.y(1:3,:),'LineWidth',2)
legend('r','v_r','v_t','Location','NorthWest')
grid on;
axis([0 4 0 2])
title('HBVP Solution')
xlabel('Time');ylabel('States')

figure(2);clf
plot(sol.x,sol.y(4:6,:),'LineWidth',2)
legend('p_1(t)','p_2(t)','p_3(t)','Location','NorthWest')
grid on;
axis([0 4 -3 2])
title('HBVP Solution')
xlabel('Time');ylabel('Costates')

ang2 = atan2(sol.y([5],:),sol.y([6],:))+pi;
figure(3);clf
plot(sol.x,180/pi*ang2','LineWidth',2)
grid on;
axis([0 4 0 360])
title('HBVP Solution')
xlabel('Time');ylabel('Control input angle \phi(t)')
norm([tan(ang2')-(sol.y(5,:)./sol.y(6,:))'])
 

function [dx] = orbit_ivp(t,x)

global mu m0 m1 T

% State
r = x(1);u = x(2);v = x(3);
lamr = x(4);lamu = x(5);lamv = x(6);

% Substitution for control
% smart
sinphi  = -lamu./sqrt(lamu.^2+lamv.^2);
cosphi = -lamv./sqrt(lamu.^2+lamv.^2);

% Dynamic Equations
dr = u;
du = v^2/r - mu/r^2 + T*sinphi/(m0 + m1*t);
dv = -u*v/r + T*cosphi/(m0 + m1*t);

dlamr = -lamu*(-v^2/r^2 + 2*mu/r^3) - lamv*(u*v/r^2);
dlamu = -lamr + lamv*v/r;
dlamv = -lamu*2*v/r + lamv*u/r;

dx = [dr; du; dv; dlamr; dlamu; dlamv];

end

function [res] = orbit_bound(x,x2)
global mu m0 m1 T

% Initial State
r = x(1);u = x(2);v = x(3);
lamr = x(4);lamu = x(5);lamv = x(6);

% Final State
r2 = x2(1);u2 = x2(2);v2 = x2(3);
lamr2 = x2(4);lamu2 = x2(5);lamv2 = x2(6);

% Boundary Constraints
b1 = r - 1;
b2 = u;
b3 = v - sqrt(mu/r);
b4 = u2;
b5 = v2 - sqrt(mu/r2);
b6 = lamr2 + 1 - lamv2*sqrt(mu)/2/r2^(3/2);

%Residual
res = [b1;b2;b3;b4;b5;b6];

end