最优控制理论 三+、间接法参数化方法（带内点约束）

数值求解最优控制的各种方法分类如下：

间接法求解最优控制问题的思路是：先通过解析推导得到必要条件，然后将OCP问题参数化或离散化再求解。
前面最优控制理论 三、两点边值问题求解给了一个例子，这个例子只包含终端约束。本小节在它的基础上考虑了一个内点约束(interior point constraint)，将问题参数化后求解，需要注意间接法的特点就是不需要离散化$x(t),u(t)$。本小节给出了这样一个简单的例子。

问题描述

最短时间拦截且飞行中途经过一个点的问题，如下
$$\begin{gathered} \underset{\theta (t)}{\min }J=t_f \\ \text{s.t.}\{\begin{array}{c}\dot{x}=u,\dot{y}=v\\ \dot{u}=a\cos \theta ,\dot{v}=a\sin \theta \end{array} \end{gathered}$$

已知初始点状态、落点位置、经过点的位置，即
$$\begin{gathered} \mathbf{x}(0)=[x(0),y(0),u(0),v(0){]}^{\mathrm{T}}=0, \\ N(\mathbf{x}(t_1),t_1)=\left[\begin{array}{c}x(t_1)-x_1\\ y(t_1)-y_1\end{array}\right]=0,t_1\in (0,t_f) \\ \psi (\mathbf{x}(t_f),t_f)=\left[\begin{array}{c}x(t_f)-x_f\\ y(t_f)\end{array}\right]=0 \end{gathered}$$

且$t_1$未知。求最优控制${\theta }^{\ast }(t)$

间接法一阶必要条件推导

首先写出哈密尔顿函数$H=1+{\lambda }_{1}u+{\lambda }_{2}v+{\lambda }_{3}a\cos \theta +{\lambda }_{4}a\sin \theta$，协态方程
$$\dot{\lambda }(t)=-\frac{\partial H}{\partial \mathbf{x}}=\left[\begin{array}{c}0\\ 0\\ -{\lambda }_1\\ -{\lambda }_2\end{array}\right]$$

控制方程$$\begin{gathered} \frac{\partial H}{\partial \theta }=-{\lambda }_3\sin \theta +{\lambda }_{4}a\cos \theta =0 \\ \Rightarrow \tan {\theta }^{\ast }=\frac{{\lambda }_4}{{\lambda }_3} \end{gathered}$$

可见只要求出协态变量就可以得到最优控制。对终端约束引入拉格朗日乘子$\mu \in {\mathbb{R}}^2,\Phi =\varphi +{\mu }^{\mathrm{T}}\psi$，终端约束与终端时间满足：
$$\begin{gathered} \lambda (t_f)=\frac{\partial \Phi }{\partial \mathbf{x}}=[{\mu }_1,{\mu }_2,0,0{]}^{\mathrm{T}} \\ H(t_f)+\frac{\partial \Phi }{\partial t_f}=1+{\mu }_{1}u(t_f)+{\mu }_{2}v(t_f)=0 \end{gathered}$$

对内点约束引入拉格朗日乘子$\pi \in {\mathbb{R}}^2$，内点约束满足：
$$\lambda (t_{1-})=\lambda (t_{1+})+{\pi }^{\mathrm{T}}\frac{\partial N}{\partial \mathbf{x}}$$

推导可得
$$\begin{gathered} {\lambda }_{1-}={\lambda }_{1+}+{\pi }_1={\mu }_1+{\pi }_1 \\ {\lambda }_{2-}={\lambda }_{2+}+{\pi }_2={\mu }_2+{\pi }_2 \end{gathered}$$

${\lambda }_{3,4}$在$t_1$处连续，且可得$\theta (t)$连续。由于协态变量是分段的，在两个阶段都不变而${\lambda }_{1,2}$只在$t_1$处跳变，从$t_f$时刻往前推导可以得到
$$\begin{gathered} {\lambda }_1(t)=\{\begin{array}{ll}{\mu }_1+{\pi }_1& t\in [0,t_1)\\ {\mu }_1& t\in (t_1,t_f]\end{array} \\ {\lambda }_2(t)=\{\begin{array}{ll}{\mu }_2+{\pi }_2& t\in [0,t_1)\\ {\mu }_2& t\in (t_1,t_f]\end{array} \\ {\lambda }_3(t)=\{\begin{array}{ll}{\mu }_1{t}_f+t_1({\mu }_1+{\pi }_1)-(2{\mu }_1+{\pi }_1)t& t\in [0,t_1)\\ {\mu }_1(t_f-t)& t\in (t_1,t_f]\end{array} \\ {\lambda }_4(t)=\{\begin{array}{ll}{\mu }_2{t}_f+t_1({\mu }_2+{\pi }_2)-(2{\mu }_2+{\pi }_2)t& t\in [0,t_1)\\ {\mu }_2(t_f-t)& t\in (t_1,t_f]\end{array} \end{gathered}$$

另外，内点时间$H({\cdot }^{\ast },t)$连续，即$$\begin{gathered} H({\cdot }^{\ast },t_{1-})=H({\cdot }^{\ast },t_{1+})+{\mu }^{\mathrm{T}}\frac{\partial \psi }{\partial t_1} \\ \Rightarrow {\lambda }_{1-}u(t_1)+{\lambda }_{2-}v(t_1)={\lambda }_{1+}u(t_1)+{\lambda }_{2+}v(t_1) \\ \Rightarrow u(t_1){\pi }_1=-v(t_1){\pi }_2 \end{gathered}$$

把以上这些条件代入解析表达式进行数值求解，问题转化为参数$[t_1,t_f,{\mu }_1,{\mu }_2,{\pi }_1,{\pi }_2]$最优化问题，需要满足微分方程约束KaTeX parse error: Unexpected end of input in a macro argument, expected '}' at position 5: \dot\̲m̲a̲t̲h̲b̲f̲ ̲x=\mathbf f[\ma…以及代数方程约束:
$$\begin{array}{rl}1+{\mu }_{1}u(t_f)+{\mu }_{2}v(t_f)& =0\\ ({\mu }_1+{\pi }_1)u(t_1)+({\mu }_2+{\pi }_2)v(t_1)& ={\mu }_{1}u(t_1)+{\mu }_{2}v(t_1)\end{array}$$

数值求解

事实上，经过如上间接法的推导过程，已经将原来的两点边值问题转化成参数寻优问题，只需要求解$[t_1,t_f,{\mu }_1,{\mu }_2,{\pi }_1,{\pi }_2]$就可以得到最优控制。这是间接法最突出的优点，也就是有可能（不一定所有问题都行）将函数空间中的无穷维优化问题转化为有限个参数的最优化问题，问题的规模大大降低。计算最难的地方在于如何猜测协态变量相关的参数，由于本文这个例子参数较少因此容易收敛。直接给出MATLAB程序，欢迎交流学习、批评指正。

function [p,X]=Indirect_DSS_InteriorPoint
% 最短时间拦截，带有一个内点约束
% 间接法推导必要条件，把协态变量函数、以及OCP问题本身
% 用(mu1,mu2,pi1,pi2,t1,tf)几个参数刻画，求解参数优化问题即可
% 采用直接打靶法
% 计算量小于直接法
% edited by: Siyang Meng in Northwestern Polytechnical University

%% problem parameters
    p.ns = 4; p.nu = 1; % number of states and controls
    p.t0 = 0; 
    p.tf = 12; % tf为性能指标
    p.acc=60;
    p.X10 = 0; p.X20 = 0; p.X30=0; p.X40=0;% initial conditions
    p.X11 = 1500; p.X21=300; % interior point the trajectory passes
    p.X1f = 2000; p.X2f = 0; % boundary conditions
    
%% shooting parameters
    p.nt1 = 30;     % number of node points in Phase 1
    p.nt2 = 10;      % number of node points in Phase 2( t1 not included )

    
%% initial guess of decision variables, based on emprical evidance
    decVar0 = [6;   % t1 initial Guess 
               2;  % tf-t1 initial Guess 
               -0.3;% mu1
               0.2;% mu2
               1.1;% pi1
               1];% pi2
%           % u 不是决策变量

    
%% solve the problem with NLP method
    options = optimoptions(@fmincon,'Display','Iter','MaxIter',1e3); % option
    [decVar_NLP,minJ] = fmincon(@(decVar) objective(decVar,p),decVar0,...
        [],[],[],[],[],[],@(x) constraints(x,p),options);
    minJ
%% obtain the optimal solution
    [~,ceq,p] = constraints(decVar_NLP,p)
    [~,X] = ode45(@(t,y) odeCon(t,y,p),p.t,[p.X10,p.X20,p.X30,p.X40]); 
    y1 = X(:,1); y2 = X(:,2); % extract states
    
%% plots
    figure(1)
    plot(y1,y2);hold on,plot(p.X11,p.X21,'bo',p.X1f,p.X2f,'rx')
    xlabel('x/m');ylabel('y/m');
    title('trajectory');
    figure(2);
    plot(p.t,p.u);
    xlabel('t/s');ylabel('θ(t)/rad');title('Control');
end

%% objective function
function f = objective(decVar,p)
    f=decVar(1)+decVar(2);
%     u = x(p.u_IDX);      % extract
%     L = u.^2/2;       % integrand
%     f = trapz(p.t,L); % calculate objective
end

%% constraint function, c<=0, ceq=0
function [c,ceq,p] = constraints(decVar,p)
    p.t1 = decVar(1);
    p.dtf = decVar(2);
    p.mu1= decVar(3);
    p.mu2= decVar(4);
    p.pi1= decVar(5);
    p.pi2= decVar(6);
%     p.u = decVar(p.u_IDX); % extract
    p.t = [linspace(p.t0,p.t1,p.nt1)';
           linspace(p.t1,p.t1+p.dtf,p.nt2)']; % time horizon
    p.t(p.nt1)=p.t1-0.001;
    p.t(p.nt1+1)=p.t1+0.001;
    % 由解析表达式得到最优控制
    [p.u,p.lambda]=necessaryOptimal(p.t,p);
    [~,Y] = ode45(@(t,y) odeCon(t,y,p),p.t,[p.X10,p.X20,p.X30,p.X40]); % simulation
    % path and terminal constraints
    ceq1 = Y(p.nt1,1) - p.X11; % passes position
    ceq2 = Y(p.nt1,2) - p.X21;
    ceq3 = Y(end,1) - p.X1f; % final state conditions
    ceq4 = Y(end,2) - p.X2f;
    % interoir time and terminal time constraints
    ceq5 = p.pi1*( Y(p.nt1,3)-Y(p.nt1+1,3) )+ p.pi1*Y(p.nt1,3) + ...
            p.pi2*( Y(p.nt1,4)-Y(p.nt1+1,4) )+ p.pi2*Y(p.nt1,4);
    ceq6 = 1+p.pi1*Y(end,3)+p.pi2*Y(end,4);
    ceq = [ ceq1;
            ceq2;
            ceq3;
            ceq4;
            ceq5;
            ceq6];
    % inequality constraints
    c1= 0.1-p.t1;
    c2= -p.dtf;
    c3= p.u-pi;% Active Constraints
    c4=-p.u-pi;
    c=[ c1;
        c2;
        c3;
        c4];
end

%% indirect method result
function [theta,lambda]=necessaryOptimal(t,p)
    t1=p.t1; tf=t1+p.dtf; mu1=p.mu1; mu2=p.mu2; pi1=p.pi1; pi2=p.pi2;
    t_phase1=t(1:p.nt1);
    t_phase2=t(p.nt1+1:p.nt1+p.nt2);
    lambda1=[repmat(mu1+pi1,p.nt1,1);
             repmat(mu1    ,p.nt2,1)];
    lambda2=[repmat(mu2+pi2,p.nt1,1);
             repmat(mu2    ,p.nt2,1)];
    lambda3=[mu1*tf+t1*(mu1+pi1)-(2*mu1+pi1)*t_phase1;
             mu1*(tf-t_phase2)];
    lambda4=[mu2*tf+t1*(mu2+pi2)-(2*mu2+pi2)*t_phase1;
             mu2*(tf-t_phase2)];
    theta=atan2(lambda4,lambda3);
    theta(end)=theta(end-1);% for case that lambda3=lambda4=0
    lambda=[lambda1,lambda2,lambda3,lambda4];
end

%% derivative function
function dydt = odeCon(t,X,p)
    theta = interp1(p.t,p.u,t); % u
    dydt(1,1) = X(3);
    dydt(2,1) = X(4);
    dydt(3,1) = p.acc*cos(theta);
    dydt(4,1) = p.acc*sin(theta);
end

求解结果如下

参考文献

[1] Bryson A E , Ho Y C ,Applied optimal control : optimization, estimation, and control. Sec 3.5[J]. IEEE Transactions on Systems Man & Cybernetics, 1975
[2] Moritz Diehl, Numerical Optimal Control (draft), Sec 8.2. 2011