最优控制理论 三、两点边值问题求解

前面一章最优控制理论 二、哈密尔顿函数法，我们已经利用Hamilton函数法对连续控制系统的Euler方程和横截条件进行了推导。这篇文章讲述如何求解

Hamilton函数法回顾

对于一个包含终端等式约束、Lagrange型性能指标的最优控制问题：
$\begin{aligned} \dot{x}&=f[x(t), u(t), t]\\ \min_{u(t)}J&=\int_{t_{0}}^{t_{f}} L[x(t), u(t), t] d t \\ \quad x\left(t_{0}\right)&=x_0\quad t_{0} \leq t \leq t_{f}\\ &\psi(x(t_f),t_f)=0 \tag 1\end{aligned}$
Hamilton函数法的Euler方程和横截条件为：
$\begin{aligned} \dot{x}&=f(x,u,t)\\ x(t_0)&=x_0,\psi(x(t_f),t_f)=0\\ \dot{\lambda}&=-\frac{\partial H}{\partial x}=-\frac{\partial L}{\partial x}-\lambda^T\frac{\partial f}{\partial x}\\ \lambda(t_f)&=\mu^T\frac{\partial\psi}{\partial x}\\ 0&=\frac{\partial H}{\partial u}=\frac{\partial L}{\partial u}+\lambda^T\frac{\partial f}{\partial u} \end{aligned}\tag{2}$
公式 $(2)$的前四个构成一个两点边值问题，控制 $u(t)$由状态变量 $x(t)$和协态变量 $\lambda(t)$决定。

二维火箭上升问题

二维火箭上升，推力加速度 $a$恒定。求最优的喷射角度 $\beta(t)$，使指定时刻达到预定高度，并水平速度 $V_x$最大（Dreyfus Rocket Ascent Problem）
$\begin{aligned} \min_{\beta(t)}J&=-V_x(t_f)=V_x(0)-\int_{0}^{t_{f}} a\cos(\beta) d t \\ \quad x\left(t_{0}\right)&=[0,0,0,0]^T\quad (0 \leq t \leq t_{f})\\ \psi(x(t_f),t_f)&=\begin{bmatrix}y(t_f)-h\\V_y(t_f)\end{bmatrix}=0 \end{aligned}$

其中参数设定 $t_f=100s,h=30000m,g=10m/s^2,a=20m/s^2$，
$H=-a \cos \beta+\lambda_{r}^{T} v+\lambda_{3} a \cos \beta +\lambda_{4}(a \sin \beta-g)$

${\dot{\lambda}=-\frac{\partial H}{\partial x}}\implies {\lambda_{1}=0 \quad \lambda_{2}=0\quad\lambda_{3}=-\lambda_{1} \quad \lambda_{4}=-\lambda_{2}}\\ \lambda\left(t_{f}\right)=\mu^{\top} \frac{\partial \psi}{\partial x}=\mu_{1}\left[\begin{array}{l} 0\\1\\0\\0 \end{array}\right]+\mu_{2}\left[\begin{array}{l} 0 \\0 \\0\\1 \end{array}\right]\\ \lambda_{1}\left(t_{f}\right)=0, \lambda_{2}\left(t_{f}\right)=\mu_{1} \quad\lambda_{3}\left(t_{1}\right)=0 \quad \lambda_{4}\left(t_{1}\right)=\mu_{2}^{*} \\ \lambda_{1}=0\quad \lambda_{2}=\mu_{1} \quad\lambda_{3}=0, \quad \lambda_{4}=-\lambda_2 t+\mu_{2}$

$\frac{\partial H}{\partial\beta}=a\{\sin\beta-\lambda_3\sin\beta+\lambda_4\cos\beta\}=0\\ \implies\tan\beta=\frac{\lambda_4}{\lambda_3-1}=-\lambda_4$

共有6个未知变量，构成两点边值问题。

两点边值问题求解

两点边值问题（Two Point Boundary Value Problem）是对常微分方程组( $n$维)而言的，如果定解问题的边界条件既有左边界的初值( $p$个)、又有右边界的终端值( $q$个)，且问题定解（ $p+q=n$）。这样的常微分方程是不能直接按照求解常微分方程组的初值问题的方法（如4阶Runge-Kutta法）来求解的。两点边值问题在轨迹规划里有很多典型的例子。
TPBVP问题求解的典型方法有1. 打靶法2. 有限差分法3. 有限元法，算法可以参考百度文库-第5章两点边值问题求解方法介绍。下面给出上面的问题对应的MATLAB代码

function sol = RocketAscentIndirectBVP
%Edited by NICAI001
a=20;tf=100;h=30000;g=10;
ti=linspace(0,tf,101);
solinit=bvpinit(ti,@init);
opts=bvpset('Stats','on','RelTol',1e-6);% ,'BCJacobian',@pFpXf
sol=bvp4c(@ode,@bc,solinit,opts);
t=sol.x;    y=sol.y;
figure(1)
plot(y(1,:),y(2,:),'k-'),title('trj'),xlabel('x/m'),ylabel('y/m');
figure(2)
plot(t,y(3:4,:));xlabel('t/s'),ylabel('velocity');
legend('V_x','V_y')
figure(3)
plot(t,-57.3*atan(y(6,:)));xlabel('t/s'),ylabel('\beta°');

    function dydx=ode(t,y,p)
        u=-atan(y(6));
        dydx=[y(3);
            y(4);
            a*cos(u);
            a*sin(u)-g;
            0;
            -y(5)];
    end
    function [dbcdya,dbcdyb,dbcdp]=pFpXf(t,y,p)
        
        
    end
    function res=bc(ya,yb,p)
        res=[ya(1);
            ya(2);
            ya(3);
            ya(4);
            yb(2)-h;
            yb(4)];
%             yb(4)];
    end

    function y=init(t,p)
        y=[400*(t); % y(-1)=-1; y(1)=1;
            300*(t);
            8*t;
            6*t;
            1.2*ones(size(t));
            -0.1*(120*ones(size(t))-t)];
    end
end