本文主要基于以下参考：

[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.

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

积分约束


在前面涉及到的最优控制问题中的约束仅对终端状态进行了约束，而工程中常见的问题经常包括多种类型的约束，从本节开始讨论涉及各种约束的最优控制问题。

本部分涉及到的内容为[1]中的3.1节部分，考虑最优控制问题：

此外，还包括有积分约束即某个积分必须满足某一固定值，比如：

其中N 为某给定标量函数。

最直接的处理方法就是额外增加一个系统方程，也就是：

以及边界条件：

于是，欧拉—拉格朗日方程则变为

例子：

最优控制问题(Isoperimetric constraint problem)

积分约束：

边界条件：

根据前面的理论知识，将积分约束转化为额外的状态量，可以得到：

边界条件：

对于该问题首先采用psopt进行求解，得到结果如下：

psopt求解-状态

psopt求解-控制量

然后同样，类似于之前采用matlab中的bvp函数进行求解：

% given the initial guess
solinit = bvpinit(linspace(0,1,10),[1 0 0.5 0.0450]);
% solve the problem
options = bvpset('RelTol', 1e-8, 'AbsTol', 1e-8);
sol = bvp4c(@sysdyn,@sysbc,solinit, options);

%% eval
t = linspace(0, 1, 50);
y = deval(sol,t);
% plot states
figure(1);
subplot(211);
plot(t, y(1,:), 'o-');
grid on;
xlabel('time/s');
legend('state1');

subplot(212);
plot(t, y(2,:),'o-');
grid on;
xlabel('time/s');
legend('state 2');

% plot costates
figure(2);
plot(t, y(3,:), 'o-',t, y(4,:),'o-');
grid;
xlabel('time/s');
legend('costate 1', 'costate 2');
% plot control
figure(3);
plot(t, -y(3,:) ./ 2 ./ y(4, :) ,'o-');
grid;
xlabel('time/s');
legend('control');

function dydx = sysdyn(t,y)
    u=-y(3)/2/y(4);
    dydx = [ -sin(y(1))+u; u^2; -1+y(3)*cos(y(1)); 0];
end

function res = sysbc(y0,y1)
    res = [ y0(1)-1; y0(2); y1(1); y1(2)-10 ];
end

上述代码得到的结果如下：

bvp求解-状态

bvp求解-协态

bvp求解-控制

注：上述bvp求解过程中存在几个问题，如下：

  1. 对协态初值较为敏感，在设置不合理的情况下会导致bvp返回雅可比奇异性的问题，导致无法求解。因此对该问题求解是通过首先采用直接法求解得到协态以后再赋值给bvp的初值。
  2. 该bvp存在两个满足条件的解，这两个解对应于两个极值解，可能一个是极大值，一个是极小值，需要进一步采用二阶必要条件来进行验证。

等式控制约束

本部分涉及到的内容为[1]中的3.2节部分，考虑最优控制问题：

此外，考虑控制变量满足等式约束：

其中， u(t) 为 m 维控制向量， m≥2 ，并且 C 为标量函数(为何标量，不过可以有多个标量函数也可以施加多个控制约束)。

对这种约束的处理方法可以将该约束通过拉格朗日乘子 μ(t) 加入到哈密顿中，得到如下：

这种处理仅导致了最优控制条件的改变，如下：

其中，与式(8)联合得到 m+1 个条件来确定 m 维的控制向量 u(t) 和标量函数 μ(t) 。

下面给出一个具体的例子：

最优控制问题(Bryson’s maximum range problem)：

满足路径约束：

边界约束：

其中

 [公式] 。

与前面类似，首先写出最优必要条件：

相应的边界条件如下：

同样，以上问题构成了一个两点边值问题，下面给出具体的代码实现。

clear all;

g = 1.0;
% 初始化，时间区间为[0,1]

solinit = bvpinit(linspace(0, 2),@BVP2Init);
tol = 1E-4;
options = bvpset('RelTol',tol,'AbsTol',[tol tol tol tol tol tol],'Nmax', 5000);

sol = bvp4c(@BVP2Ode,@BVP2bc,solinit,options);

time = sol.x;  % get time
state = sol.y([1 2 3], :);
adjoint = sol.y([4 5 6],:);

len = length(time);
uu = zeros(2, len);
for i=1:len
    x = state(1,i);     y = state(2,i);     v = state(3,i);
    cx1 = adjoint(1,i);     cx2 = adjoint(2,i);     cx3 = adjoint(3,i);

    if cx1*v == 0, % may mu==0 or u1 == 0
        if cx2*v-g*cx3 == 0, % mu == 0
            u1 = 0;
            u2 = -1;
            mu = 0;
        else, % u1 == 0
            % u2 can be +1 or -1
            u1 = 0;
            u2 = -1;
            mu = 0;
        end
    elseif cx2*v-g*cx3 == 0,  % u2 == 0
        u2 = 0;
        u1 = -1;
        mu = 0;
    else, % u1,u2 != 0
        mu = sqrt(((cx1*v)^2+(g*cx3-cx2*v)^2)/4);
        u1 = -cx1*v/2/mu;
        u2 = -(cx2*v-g*cx3)/2/mu;
    end

    uu(:, i) = [u1;u2];
end

control = uu;

mm.time= time;
mm.state = state;
mm.costate= adjoint;
mm.control = control;

%% plotting
figure(1); clf
plot(mm.time,mm.state,'LineWidth',2)
legend('x','y','v','Location','NorthWest')
grid on;
%axis([0 0.9 0 7])
title('HBVP Solution')
xlabel('Time');ylabel('States')


figure(2); clf
plot(mm.time,mm.costate,'LineWidth',2)
legend('lam1','lam2','lam3','Location','NorthWest')
grid on;
%axis([0 0.9 -0.5 0])
title('HBVP Solution')
xlabel('Time');ylabel('Costates')

figure(3); clf
plot(mm.time,mm.control,'LineWidth',2)
legend('u1','u2','Location','NorthWest')
grid on;
%axis([0 0.9 -0.5 0])
title('HBVP Solution')
xlabel('Time');ylabel('control')

%-------------------------------------------------------------------------­
function dydt=BVP2Ode(t, state)
a = 0.5;    g = 1.0;

x=state(1); y=state(2); v=state(3);
cx1=state(4);   cx2=state(5);   cx3=state(6);

if cx1*v == 0, % may mu==0 or u1 == 0
    if cx2*v-g*cx3 == 0, % mu == 0
        u1 = 0;
        u2 = -1;
        mu = 0;
    else, % u1 == 0
        % u2 can be +1 or -1
        u1 = 0;
        u2 = -1;
        mu = 0;
    end
elseif cx2*v-g*cx3 == 0,  % u2 == 0
    u2 = 0;
    u1 = -1;
    mu = 0;
else, % u1,u2 != 0
    mu = sqrt(((cx1*v)^2+(g*cx3-cx2*v)^2)/4);
    u1 = -cx1*v/2/mu;
    u2 = -(cx2*v-g*cx3)/2/mu;
end

dydt = [
                    v*u1;
                    v*u2;
                    a-g*u2;
                    0;
                    0;
                    -(cx1*u1+cx2*u2);];
end
%-------------------------------------------------------------------------­

function res=BVP2bc(ya,yb)

g = 9.8;

x0=ya(1); y0=ya(2); v0=ya(3);
cx10=ya(4);   cx20=ya(5);   cx30=ya(6);

xf=yb(1); yf=yb(2); vf=yb(3);
cx1f=yb(4);   cx2f=yb(5);   cx3f=yb(6);

res = [x0;
          y0;
          v0;
          cx1f+1;
          yf-0.1;
          cx3f];

end
%-------------------------------------------------------------------------­