本文主要基于以下参考：

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

最优控制问题数值方法-间接法/变分法 (8)0 赞同 · 0 评论文章

本节首先对后续涉及到的一个例题进行介绍，该例题位于[4] 2.7节 Example1，并包括了相关的代码实现。

下面给出一个例子：

考虑一艘船必须穿过一片有较强水流的区域，该水流的幅值和方向为位置的已知函数：

其中， (x,y) 为平面坐标， (u,v) 为水流在 x 和 y 方向的速度分量，船体相对于水流的速度为 V (常数)。该最优控制问题是指控制该船使得在最短时间内从 A 点到达 B 点。

运动方程如下：

其中， θ 为船头与坐标系之间的夹角， (x,y) 代表船在水中的位置。

于是，最优必要条件如下：

此外，由于哈密顿函数(10)不是时间的显式函数，因此哈密顿在整个过程中为常数。此外，根据之前的最优条件：

因此，哈密顿函数必须为零，即 H=0 ，因此根据式(3)和式(4)最后一式可以解得：

由上式我们得到协态与控制之间的关系，但是协态的微分关系还不满足，因此将式(6-7)代入到式(4)前两式中，得：

类比于斯涅尔定律

如果有 u=u(y) ， v=v(y) ，则式(4)变为：

方程(6)变为：

上述公式类似于光学中的斯涅尔定律，因为它给出了在不同水流条件下船体的航向角 θ （类似于光线在穿过不同介质时由于传播速度不同发生折射）。

特殊情况

水流速度线性变化。如果考虑u=-V(y/h)， [公式] ，我们希望找到从点 x0,y0 到原点的最短路径，可以采用式(10)来描述最优航向角的根据终端航向角 θ 以及当前的坐标 y ，即：

采用 θ 作为自变量，根据上式可得：

因此，结合已知得水流表达式，式(8)变为：

最后，式(2)结合式(12)(13)可以得到：

上式积分可以得到：

数值计算：

重新对问题进行表达，一般性的问题：

满足约束：

正常来说和前面一样首先写出最优条件：

然后是边界条件：

以及时间条件：

于是上式构成了一个两点边值问题，下面在给定具体数值的基础上对其进行计算，考虑某种特殊情况：

 clear all

global x0 x1 V h

x0=[36.6 -18.6]';   % 初始位置
x1=[0 0]';              % 终端位置
V = 2;                    % 船体速度
h = 10;                  % 比例因子

mm=TPBVPzermelo;

%%
time = mm(1, :);
state = mm([2 3],:);
control = mm(6, :);
costate = mm([4 5], :);
% 对状态规范化
state = state / h;
xp0 = x0 / h;
xp1 = x1 / h;
% 对控制量规范化
control(control < 0) = control(control < 0) + 2*pi;

figure(1);clf
plot( state(1,:), state(2,:),'LineWidth',2);
axis('square'); grid on
axis([0 6 -2 1.5])
xlabel('x','FontSize',12);ylabel('y','FontSize',12);
hold on;
plot(xp0(1),xp0(2),'rs'); plot(xp1(1),xp1(2),'bs');
text(xp0(1),xp0(2),'Initial Point','FontSize',12)
text(xp1(1),xp1(2),'Final Point','FontSize',12)
len = length(time);
Nstep = 20;
for i=1:Nstep:len
    xi = state(1, i);   yi = state(2,i);
    arrow_len = 0.5;
    xf = state(1, i)+arrow_len*cos(control(i));   yf = state(2, i)+arrow_len*sin(control(i));
    plot_arrow(xi, yi, xf, yf, 0.05,'m','-');
end
hold off


figure(2);clf
plot(time, rad2deg(control), 'LineWidth',2);grid on;axis('square')
xlabel('t','FontSize',12);ylabel('control','FontSize',12);

figure(3);clf

plot(time, costate(1,:),time, costate(2,:),'LineWidth',2);grid on;axis('square')
xlabel('t','FontSize',12);ylabel('costates','FontSize',12);
 

function m = TPBVPzermelo(p1,p2)

global x0 x1 V h

solinit = bvpinit(linspace(0,1),@TPBVPinit);
tol = 1E-8;
options = bvpset('RelTol',tol,'AbsTol',[tol tol tol tol tol],'Nmax', 5000);
sol = bvp4c(@TPBVPode,@TPBVPbc,solinit,options);

time = sol.y(5)*sol.x;
state = sol.y([1 2],:);
adjoint = sol.y([3 4],:);
control = atan2(sol.y([4],:),sol.y([3],:));

m(1,:) = time;
m([2 3],:) = state;
m([4 5],:) = adjoint;
m(6,:) = control;

end

%-------------------------------------------------------------------------?
function dydt=TPBVPode(t,y)

global x0 x1 V h

% 计算控制量

sinth  = y(4)/sqrt(y(3)^2+y(4)^2);
costh = y(3)/sqrt(y(3)^2+y(4)^2);

dydt=y(5)*[V*costh-V*y(2)/h; V*sinth;0;y(3)*V/h; 0];

%-------------------------------------------------------------------------?
end

function res=TPBVPbc(ya,yb)
global x0 x1 V h

costhb = -yb(3)/sqrt(yb(3)^2+yb(4)^2);
sinthb  = -yb(4)/sqrt(yb(3)^2+yb(4)^2);

res=[ya(1) - x0(1);
        ya(2)-x0(2);
        yb(1) - x1(1);
        yb(2)-x1(2);
1+(V*costhb-V*yb(2)/h)*yb(3)+V*sinthb*yb(4)];

%-------------------------------------------------------------------------
end

function v=TPBVPinit(t)
global x0 x1 V h

tvec = linspace(0,1);
ind = find(tvec==t);

xini1 = linspace(x0(1), x1(1));
xini2 = linspace(x0(2), x1(2));

v=[xini1(ind);xini2(ind);-1;-1;20];

end

另一种方法(打靶法)

前面提到了根据哈密顿函数为常数以及最优控制量满足的关系，推导得到了协态与控制量之间的关系，并将该关系代入到协态微分方程中得到了控制量满足的关系，下面基于该推导进行求解，前面提到我们只需要确定参数

 [公式] ，然后对如下的三个式子

积分并使得其满足终端约束到达 B 点即可，这个过程有些类似于打靶法（因为我们猜测初始时子弹射出的角度，并希望它命中目标），打靶法是用来求解边值问题的一种方法。

将式(31)在本题中的具体表达式写出来:

首先采用打靶法来进行求解：

% 
global x0 x1 V h

x0=[36.6 -18.6]';   % 初始位置
x1=[0 0]';              % 终端位置
V = 2;                    % 船体速度
h = 10;                  % 比例因子

% 迭代变量 \thetaA tf
% 约束量: x(t_f) = x_f
xini = [deg2rad(10); 25];             % 初始猜测: 10°/18s

options = optimoptions('fsolve','MaxFunctionEvaluations', 1000);
[x,feval] = fsolve(@cons,xini,options)

%% plotting
% plot cons vs params
xparams = deg2rad(0:10:180);   yparams = 10:30;
len1 = length(xparams);   len2 = length(yparams);
ret1 = zeros(len1,len2);
ret2 = zeros(len1,len2);
for i=1:len1
    for j=1:len2
        xi = xparams(i);    yj = yparams(j);
        res = cons([xi;yj]);
        ret1(i,j) = res(1);
        ret2(i,j) = res(2);
    end
end


%%
[X, Y] = meshgrid(yparams, xparams);
subplot(121);
surf(X, Y, ret1)
xlabel('time'); ylabel('theta_A'); zlabel('residual');
subplot(122);
surf(X, Y, ret2)
xlabel('time'); ylabel('theta_A'); zlabel('residual');

%%

contour(X,Y,ret2, 20)

function dydt=zermeloODE(t, y)
    global x0 x1 V h

dydt = y(4)*[V*cos(y(3))-V*y(2)/h;
                V*sin(y(3));
                cos(y(3))^2*V/h
                0
                ];

end

function res = cons(x)
    global x0 x1 V h
    thetaA = x(1);
    tf = x(2);
    y0 = [x0;thetaA;tf];
    opts = odeset('RelTol',1e-8,'AbsTol',1e-8);
    [~,y] = ode45(@zermeloODE, [0 1], y0, opts);
    
    res = [y(end, 1);
               y(end, 2)];

end

因此，如果通过打靶法来求解这种问题通常有几种解决思路：

1. 采用进化算法，比如说遗传算法这种不依赖于梯度信息的；

2. 通过对参数区间快速扫描，画出类似于上图，然后通过读图来确定参数的大致区间，然后在给定较好的初值条件下进行求解。

由第一张图大致可以得出，时间位于 [22.5,30]，角度位于 [0.5,2]，根据得出的信息，此时优化就容易多了。

另一种方法(解析方法）

在特殊情况里，推导得到了 x,y 关于 θ 的表达式，如下：

对这两个公式采用fsolve进行求解可以得到：

这与前面计算得到的结果相同，因此不再赘述。