離散化された最適軌跡 (問題ベース)

ライブスクリプトを開く

この例では、問題ベースのアプローチを使用して、離散化された最適軌跡問題を解く方法を示します。軌跡の重力は一定とし、印加される力には限度があり、空気抵抗は考慮しません。この解法プロセスは、固定期間 $T$ の間の最適な軌跡を求め、その解を使用して最適な $T$ 、つまりコストを最小限にする時間を見つけます。この例では、次に、空気抵抗を導入する方法を示し、最後に、オブジェクトが燃料を消費する際の可変質量の影響を導入する方法を示します。

最適化変数を使用して ODE のパラメーターを当てはめるの例のような離散化されていない最適化と比較して、離散化された最適化は常微分方程式 (ODE) を解く場合正確ではありません。しかし、離散化された最適化は、シミュレーションまたは常微分方程式の最適化の説明にあるとおり、可変ステップ ODE ソルバーに含まれるノイズの影響を受けません。また、離散化された最適化の方がカスタマイズしやすくなることがあり、モデル化も簡単です。最後に、離散化された最適化では、最適化プロセスで自動微分を活用できます。問題ベースの最適化における自動微分の効果を参照してください。

問題の説明

問題は、制御されたジェット機を使用して、時間 $0$ における位置 $p_{0}$ から時間 $T$ における位置 $p_{F}$ までオブジェクトを移動することです。位置はベクトル $p (t)$ 、速度はベクトル $v (t)$ 、印加された加速度はベクトル $a (t)$ で表します。連続時間では、重力を含めた運動方程式は次のようになります。

$\frac{d p}{d t} = v (t)$

$\frac{d v}{d t} = a (t) + g * [0, 0, - 1]$ .

ジェット機は加速度 $a (t)$ を印加します。結果として得られる加速度には重力の項が追加されます。

ジェット機が印加できる最大加速度は $A_{m a x} = 25$ です。

時間を離散化して問題を解きます。時間をサイズ $t = T / N$ で $N$ 個に等分割します。タイムステップ $i$ における位置はベクトル $p (i)$ 、速度はベクトル $v (i)$ 、印加された加速度はベクトル $a (i)$ となります。ODE モデルをかなり正確に表現する、一連の方程式を作成できます。近似運動方程式は次のようになります。

$v (i) = v (i - 1) + t (a (i - 1) + g), i = 1 . . . N$

$p (i) = p (i - 1) + t (\frac{v (i - 1) + v (i)}{2}), i = 1 . . . N .$

前述の方程式では、2 点 (台形則) 近似を使用して速度を積分し、1 点 (オイラー) 近似を使用して加速度を積分します。加速度 $a (i)$ が時間間隔の中心 $i$ に印加されるとみなした場合、積分法が加速度の中点則となります。積分法全体で得られる方程式は簡単なもので、ステップ $i$ における位置および速度は、ステップ $i - 1$ における位置、速度、および加速度のみに依存しています。この方程式は、空気抵抗を考慮するように変更するのも容易です。

境界条件は、 $p (0) = p_{0}$ 、 $p (N) = p_{F}$ 、および $v (0) = v (N) = [0, 0, 0]$ です。初期位置および最終位置は次のように設定します。

p0 = [0 0 0];
pF = [5 10 3];

MATLAB® のインデックスは 0 ではなく 1 から開始します。インデックス付けを容易にするために、区間数を $N - 1$ と指定し、位置と速度のインデックスを、 $0$ から $N$ ではなく、 $1$ から $N$ とします。このインデックス付けにより、加速度のインデックスは $1$ から $N - 1$ となります。

ジェット機を使用して、時間 $t$ の間に力 $a$ を発生させるのにかかるコストは、 $‖ a ‖ t$ です。総コストは、加速度と $t$ の積のノルムの和です。

$C o s t = \sum_{i = 1}^{N - 1} ‖ a (i) ‖ t .$

このコストを最適化変数の線形コストに変換するには、変数 $s (i)$ と、関連する 2 次錐制約を作成します。

$\begin{array}{l} C o s t = \sum_{i = 1}^{N - 1} s (i) t \\ ‖ a (i) ‖ \leq s (i) . \end{array}$

すべてのタイムステップについて、加速度のノルムが定数 Amax によって制限される追加制約を課します。

$‖ a (i) ‖ \leq A m a x .$

これらの制約も 2 次錐制約です。制約が線形または 2 次錐制約であり、目的関数が線形であるため、solve は coneprog ソルバーを呼び出して問題を解きます。

この例の最後の補助関数 setupproblem1 は、固定の時間 T の最適化問題を作成します。このコードでは、運動方程式を問題の制約として組み込んでいます。この関数には空気抵抗の引数が含まれています。空気抵抗を考慮するモデルの場合は、air = true と設定してください。空気抵抗の定義については、空気抵抗の導入のセクションを参照してください。

印加された加速度 $a$ が、この問題のメインの最適化変数となります。Vanderbei [1] が提案するように、この問題では速度 v と位置 p を最適化変数としてとる必要はありません。運動方程式と印加された加速度 $a$ の値から、それらの値を導き出すことができます。N ステップの場合、 $a$ の次元は N – 1 行 3 列となります。また、この問題には、線形目的関数を許可する最適化変数として、ベクトル変数 s が含まれています。

T = 20 について問題を解く

時間 $T = 20$ の軌跡問題を作成し、解きます。この問題では数値の厳密さが求められるため、信頼性を確保するために、最適性の許容誤差は 1e-9 に設定します。

[trajprob,p] = setupproblem1(20);

この問題の既定のソルバーは何でしょうか。

defaultsolver = solvers(trajprob)

defaultsolver = 
"coneprog"

coneprog ソルバーのオプションを設定して、問題を解きます。

opts = optimoptions(defaultsolver,Display="none",OptimalityTolerance=1e-9);
[sol,fval,eflag,output] = solve(trajprob,Options=opts)

sol = struct with fields:
    a: [49×3 double]
    s: [49×1 double]

fval = 192.2812

eflag = 
    OptimalSolution

output = struct with fields:
           iterations: 12
    primalfeasibility: 4.4823e-10
      dualfeasibility: 3.3916e-09
           dualitygap: 3.2421e-11
            algorithm: 'interior-point'
         linearsolver: 'augmented'
              message: 'Optimal solution found.'
               solver: 'coneprog'

補助関数 plottrajandaccel を呼び出して、軌跡および加速度のノルムをプロットします。

N = 50;
plottrajandaccel(sol,p,p0,pF)

Figure contains an axes object. The axes object with xlabel x, ylabel y contains 3 objects of type line. One or more of the lines displays its values using only markers These objects represent Steps, Initial Point, Final Point.

Figure contains an axes object. The axes object with xlabel Time step, ylabel Norm(acceleration) contains a line object which displays its values using only markers.

加速度は、期間の開始時点と終了時点でほぼ最大、中間時点でほぼゼロになります。このように、加速度が最大か 0 のいずれかになるような解を「バンバン」と呼びます。

最小コストの特定

時間 $T$ をいくつにすればコストが最小になるでしょうか。 $T = 1$ など時間が短すぎる場合、問題は実行不可能であり、したがって解はありません。

myprob = setupproblem1(1);
opts.Display = "final"; % To see the solution status
[solm,fvalm,eflagm,outputm] = solve(myprob,Options=opts);

Solving problem using coneprog.
Problem is infeasible.

時間を 1.5 にすると問題が実行可能になります。

myprob = setupproblem1(1.5);
[solm,fvalm,eflagm,outputm] = solve(myprob,Options=opts);

Solving problem using coneprog.
Optimal solution found.

補助関数 tomin は、時間 T の問題を設定してから、solve を呼び出して解となるコストを計算します。tomin 上で fminbnd を呼び出し、区間 $1.5 \leq T \leq 10$ における最適な時間 (できる限り低いコスト) を特定します。

[Tmin,Fmin] = fminbnd(@(T)tomin(T,false),1.5,10)

Tmin = 1.9517

Fmin = 24.6095

最適時間 Tmin の軌跡を求めます。

[minprob,p] = setupproblem1(Tmin);
sol = solve(minprob,Options=opts);

Solving problem using coneprog.
Optimal solution found.

最小となる軌跡と加速度をプロットします。

plottrajandaccel(sol,p,p0,pF)

Figure contains an axes object. The axes object with xlabel Time step, ylabel Norm(acceleration) contains a line object which displays its values using only markers.

最小となる解はほぼ「バンバン」解であり、加速度は 2 つの値を除くすべての値で最大かゼロのいずれかとなります。

最小とならない軌跡のプロット

さまざまな時間の軌跡をプロットします。

figure
hold on
options = optimoptions("coneprog",Display="none",OptimalityTolerance=1e-9);
g = [0 0 -9.81];
for i = 1:10
    T = 2*i;
    t = T/N;
    prob = setupproblem1(T);
    sol = solve(prob,"Options",options);
    asol = sol.a;
    vsol = cumsum([[0 0 0];t*(asol+repmat(g,size(asol,1),1))]);
    N = size(vsol,1);
    np = 2:N;
    n0 = 1:(N-1);
    psol = cumsum([p0;t*(vsol(np,:) + vsol(n0,:))/2]);
    plot3(psol(:,1),psol(:,2),psol(:,3),'rx',"Color",[256-25*i 20*i 25*i]/256)
end
view([18 -10])
xlabel("x")
ylabel("y")
zlabel("z")
legend("T = 2","T = 4","T = 6","T = 8","T = 10","T = 12","T = 14","T = 16","T = 18","T = 20")
hold off

Figure contains an axes object. The axes object with xlabel x, ylabel y contains 10 objects of type line. One or more of the lines displays its values using only markers These objects represent T = 2, T = 4, T = 6, T = 8, T = 10, T = 12, T = 14, T = 16, T = 18, T = 20.

最も短い時間 (2) の軌跡は、このスケールではほぼ一直線となっています。時間が長くなるほど直線から大きくずれており、 $T = 20$ のパスは 300 付近の高さに達しています。

空気抵抗の導入

モデルのダイナミクスを変更して、空気抵抗を導入します。線形空気抵抗により、時間 $t$ 後に速度が係数 $\exp (- t)$ だけ変化します。運動方程式は次のようになります。

$v (i) = v (i - 1) \exp (- t) + t (a (i - 1) + g)$

$\begin{array}{l} p (i) = p (i - 1) + t (\frac{v (i - 1) + v (i)}{2}) \\ = p (i - 1) + t (\frac{1 + \exp (- t)}{2}) v (i - 1) + t^{2} \frac{a (i - 1) + g}{2} . \end{array}$

air = true の場合の関数 setupproblem1(T,air) の問題の定式化には、速度定数を定義する行および位置定数を定義する行の両方に exp(-t) の係数があります。

vcons(2:N,:) = v(2:N,:) == v(1:(N-1),:)*exp(-t) + t*(a(1:(N-1),:) + repmat(g,N-1,1));
pcons(2:N,:) = p(2:N,:) == p(1:(N-1),:) + t*(1+exp(-t))/2*v(1:(N-1),:) + t^2/2*(a(1:(N-1),:) + repmat(g,N-1,1));

空気抵抗を含む問題に対する最適な時間を求めます。

[Tmin2,Fmin2] = fminbnd(@(T)tomin(T,true),1.5,10)

Tmin2 = 1.9398

Fmin2 = 28.7967

最適な時間は、空気抵抗がない問題の時間よりわずかに小さいだけ (1.95 ではなく 1.94) ですが、コスト Fmin は約 17% 上昇します (24.6 ではなく 28.8)。

compartable = table([Tmin;Tmin2],[Fmin;Fmin2],'VariableNames',["Time" "Cost"],'RowNames',["Original" "Air Resistance"])

compartable=2×2 table
                       Time      Cost 
                      ______    ______

    Original          1.9517    24.609
    Air Resistance    1.9398    28.797

空気抵抗を含む場合の最適解の軌跡および加速度をプロットします。

[minprob,p] = setupproblem1(Tmin2,true);
sol = solve(minprob,Options=opts);

Solving problem using coneprog.
Optimal solution found.

plottrajandaccel(sol,p,p0,pF)

Figure contains an axes object. The axes object with xlabel Time step, ylabel Norm(acceleration) contains a line object which displays its values using only markers.

空気抵抗があると、ゼロ加速度の時間が後ろのタイムステップに移動し、短縮されます。ただし、解はほぼ「バンバン」のままです。

可変質量を導入するためにモデルを拡張

ジェット機は質量を放出することで動作します。ジェット機が動作する際に質量が減少する影響を含める場合、運動方程式は 2 次錐問題のフレームワークに当てはまらなくなります。問題を数値的に解くのが難しくなり、求解時間が長くなったり、解の精度が低下したりします。

モデルを変更して、印加される力 $F (t)$ と質量 $m (t)$ を含めます。正味加速度は以下です。

$a (t) = F (t) / m (t) + g [0, 0, - 1]$ .

質量の変化率は以下です。

$\frac{d m}{d t} = - r ‖ F (t) ‖,$

ここで、 $r$ は定数です。

$m (0) = 2$ 、 $r = 1 / 1000$ 、最大力が $F_{m a x} = 50$ と仮定します。これらの仮定から、時間 0 における印加された最大加速度は $F_{m a x} / m (0) = 25$ となり、前のモデルと同じ最大加速度であることがわかります。

関数 setupproblemfuel1 は、これらの方程式とパラメーターに基づいて問題を作成します。可変質量モデルのこのインスタンスには空気抵抗を導入しません。

[trajprob,p] = setupproblemfuel1(20);

結果として得られる問題の既定のソルバーは何でしょうか。

defaultsolver = solvers(trajprob)

defaultsolver = 
"fmincon"

fmincon ソルバーは初期点を必要とします。T = 20 でランダムな初期点を試してみてください。

rng default
y0 = randn(49,3);
x0.F = y0;

ソルバーが進行状況に応じてプロットを生成し、既定よりも多くの関数評価と反復を行えるようにソルバーのオプションを指定します。StepTolerance を既定の 1e-10 から 1e-11 に減らして精度を上げてみてください。

opts = optimoptions(defaultsolver,Display="none",PlotFcn="optimplotfvalconstr",...
    MaxFunctionEvaluations=1e5,MaxIterations=1e4,StepTolerance=1e-11);

ソルバーを呼び出します。

[sol,fval,eflag,output] = solve(trajprob,x0,Options=opts)

Figure Optimization Plot Function contains an axes object. The axes object with title Best Function Value: 348.149, xlabel Iteration, ylabel Function value contains 2 objects of type scatter. These objects represent Best function value, Best function value (infeasible).

sol = struct with fields:
    F: [49×3 double]

fval = 348.1493

eflag = 
    StepSizeBelowTolerance

output = struct with fields:
              iterations: 2393
               funcCount: 8148
         constrviolation: 2.7960e-12
                stepsize: 1.8893e-11
               algorithm: 'interior-point'
           firstorderopt: 0.0719
            cgiterations: 76125
                 message: 'Local minimum possible. Constraints satisfied.↵↵fmincon stopped because the size of the current step is less than↵the value of the step size tolerance and constraints are ↵satisfied to within the value of the constraint tolerance.↵↵<stopping criteria details>↵↵Optimization stopped because the relative changes in all elements of x are↵less than options.StepTolerance = 1.000000e-11, and the relative maximum constraint↵violation, 1.474630e-15, is less than options.ConstraintTolerance = 1.000000e-06.'
            bestfeasible: [1×1 struct]
     objectivederivative: "reverse-AD"
    constraintderivative: "reverse-AD"
                  solver: 'fmincon'

軌跡と加速度をプロットします。関数 plottrajandaccel は解の構造体の a フィールドにこの値を使用するため、まず F フィールドから a フィールドに解をコピーします。

sol.a = sol.F;
plottrajandaccel(sol,p,p0,pF)

Figure contains an axes object. The axes object with xlabel Time step, ylabel Norm(acceleration) contains a line object which displays its values using only markers.

可変加速度のコストを追加して解を平滑化

印加される力は基本的に「バンバン」ですが、この力には振動運動があります。Vanderbei [1] はこのタイプの動作を「リンギング」と呼びます。リンギングをなくすには、振動加速度のコストを追加し、再度解きます。補助関数 setupproblemfuel2 には、目的関数の追加項 t*1e-4*sum(diff(fnorm).^2 が含まれており、加速度のノルム変位にペナルティを設定します。

 % Add cost for acceleration changes
 trajectoryprob.Objective = sum(fnorm)*t + t*1e-4*sum(diff(fnorm).^2);

問題を再度解きます。

[trajprob,p] = setupproblemfuel2(20);
[sol,fval,eflag,output] = solve(trajprob,x0,Options=opts)

Figure Optimization Plot Function contains an axes object. The axes object with title Best Function Value: 348.562, xlabel Iteration, ylabel Function value contains 2 objects of type scatter. These objects represent Best function value, Best function value (infeasible).

sol = struct with fields:
    F: [49×3 double]

fval = 348.5616

eflag = 
    StepSizeBelowTolerance

output = struct with fields:
              iterations: 3755
               funcCount: 7927
         constrviolation: 3.2683e-11
                stepsize: 1.6872e-10
               algorithm: 'interior-point'
           firstorderopt: 0.1728
            cgiterations: 46335
                 message: 'Local minimum possible. Constraints satisfied.↵↵fmincon stopped because the size of the current step is less than↵the value of the step size tolerance and constraints are ↵satisfied to within the value of the constraint tolerance.↵↵<stopping criteria details>↵↵Optimization stopped because the relative changes in all elements of x are↵less than options.StepTolerance = 1.000000e-11, and the relative maximum constraint↵violation, 1.723720e-14, is less than options.ConstraintTolerance = 1.000000e-06.'
            bestfeasible: [1×1 struct]
     objectivederivative: "reverse-AD"
    constraintderivative: "reverse-AD"
                  solver: 'fmincon'

sol.F データを使用して軌跡と加速度をプロットします。

sol.a = sol.F;
plottrajandaccel(sol,p,p0,pF)

Figure contains an axes object. The axes object with xlabel Time step, ylabel Norm(acceleration) contains a line object which displays its values using only markers.

より適切な初期点の指定

ほとんどのリンギングはなくなりましたが、解にはまだ力がゼロの区間が複数あります。

別の初期点を指定することで、より満足のいく解を得られる可能性があります。ソルバーの中間時点の加速度を小さくするには、インデックス 10 から 40 の場合は 0 に等しい初期点を、インデックス 1 から 9 および 41 から 49 の場合は 20 に等しい初期点を指定します。初期点にランダムノイズを追加します。

rng default
y0 = 20*ones(49,3);
y0(10:40,:) = 0;
y0 = y0 + randn(size(y0));
x0.F = y0;

この新しい点から始めて問題を解きます。

[sol,fval,eflag,output] = solve(trajprob,x0,Options=opts)

Figure Optimization Plot Function contains an axes object. The axes object with title Best Function Value: 348.277, xlabel Iteration, ylabel Function value contains 2 objects of type scatter. These objects represent Best function value, Best function value (infeasible).

sol = struct with fields:
    F: [49×3 double]

fval = 348.2770

eflag = 
    StepSizeBelowTolerance

output = struct with fields:
              iterations: 1653
               funcCount: 5422
         constrviolation: 7.6383e-14
                stepsize: 2.2832e-11
               algorithm: 'interior-point'
           firstorderopt: 0.0318
            cgiterations: 26724
                 message: 'Local minimum possible. Constraints satisfied.↵↵fmincon stopped because the size of the current step is less than↵the value of the step size tolerance and constraints are ↵satisfied to within the value of the constraint tolerance.↵↵<stopping criteria details>↵↵Optimization stopped because the relative changes in all elements of x are↵less than options.StepTolerance = 1.000000e-11, and the relative maximum constraint↵violation, 6.578331e-17, is less than options.ConstraintTolerance = 1.000000e-06.'
            bestfeasible: [1×1 struct]
     objectivederivative: "reverse-AD"
    constraintderivative: "reverse-AD"
                  solver: 'fmincon'

軌跡と加速度をプロットします。

sol.a = sol.F;
plottrajandaccel(sol,p,p0,pF)

Figure contains an axes object. The axes object with xlabel Time step, ylabel Norm(acceleration) contains a line object which displays its values using only markers.

満足のいく解が得られたようです。これは基本的に、印加された加速度がゼロの区間が 1 つだけの「バンバン」の加速度です。

オブジェクトの移動が終わったとき、最初の 2 からどれだけの質量が残っているでしょうか。

T = 20;
N = 50;
t = T/N;
fnorm = zeros(N-1,1);
r = 1/1000;
for i = 1:length(fnorm)
    fnorm(i) = norm(sol.F(i,:));
end
m = 2 - r*t*sum(fnorm)

m = 1.6518

このように質量が減少した場合、印加された最大加速度はどのくらいになるでしょうか。

Fmax = 50;
Fmax/m

ans = 30.2700

モデルに可変質量の影響が導入されている場合、軌跡の間の印加された最大加速度は 25 から 30 に変わります。

空気抵抗と可変質量の導入

モデルに再度空気抵抗を導入します。この場合、補助関数 setupproblemfuel2 が fcn2optimexpr を使用して補助関数 airResistance を呼び出し、空気抵抗を考慮した速度を評価します。別の関数を使用して for ループをコーディングし、fcn2optimexpr を使用してその関数を呼び出すと、より効率的な問題が作成されます。詳細については、静的解析のための for ループの作成と最適化式の静的解析を参照してください。

[trajprob2,p] = setupproblemfuel2(20,true);

空気抵抗を導入した先ほどのモデルの結果に基づき、力がゼロの初期区間を、10 から 40 ではなく、15 から 45 に設定します。

rng default
y0 = 20*ones(49,3);
y0(15:45,:) = 0;
y0 = y0 + randn(size(y0));
x0.F = y0;

問題を解きます。

[sol2,fval2,eflag2,output2] = solve(trajprob2,x0,Options=opts)

Figure Optimization Plot Function contains an axes object. The axes object with title Best Function Value: 352.714, xlabel Iteration, ylabel Function value contains 2 objects of type scatter. These objects represent Best function value, Best function value (infeasible).

sol2 = struct with fields:
    F: [49×3 double]

fval2 = 352.7138

eflag2 = 
    StepSizeBelowTolerance

output2 = struct with fields:
              iterations: 6686
               funcCount: 12620
         constrviolation: 2.1938e-13
                stepsize: 1.5226e-11
               algorithm: 'interior-point'
           firstorderopt: 0.2178
            cgiterations: 93737
                 message: 'Local minimum possible. Constraints satisfied.↵↵fmincon stopped because the size of the current step is less than↵the value of the step size tolerance and constraints are ↵satisfied to within the value of the constraint tolerance.↵↵<stopping criteria details>↵↵Optimization stopped because the relative changes in all elements of x are↵less than options.StepTolerance = 1.000000e-11, and the relative maximum constraint↵violation, 1.532271e-15, is less than options.ConstraintTolerance = 1.000000e-06.'
            bestfeasible: [1×1 struct]
     objectivederivative: "reverse-AD"
    constraintderivative: "reverse-AD"
                  solver: 'fmincon'

sol2.a = sol2.F;
plottrajandaccel(sol2,p,p0,pF)

Figure contains an axes object. The axes object with xlabel Time step, ylabel Norm(acceleration) contains a line object which displays its values using only markers.

残った燃料の質量はどのくらいになるでしょうか。

for i = 1:length(fnorm)
    fnorm(i) = norm(sol2.F(i,:));
end
m2 = 2 - r*t*sum(fnorm)

m2 = 1.6474

このように質量が減少した場合、印加された最大加速度はどのくらいになるでしょうか。

Fmax/m2

ans = 30.3517

空気抵抗を導入しても燃料消費量は大きく変わりません。この軌跡では空気抵抗を利用してオブジェクトを減速させているようで、オブジェクトは移動の最後の部分では減速にあまり燃料を使用していません。空気抵抗のある軌跡の最大の高さは 130 未満ですが、空気抵抗のない軌跡の最大の高さは約 300 であることに注意してください。現在のパラメーターでは、空気抵抗によって軌跡に有意差が出ますが、燃料消費量には有意差が出ません。

参考文献

[1] Vanderbei, R. J. "Case Studies in Trajectory Optimization: Trains, Planes, and Other Pastimes." Optimization and Engineering 2, 215–243 (2001). https://vanderbei.princeton.edu/tex/trajopt/trajopt.pdf から入手可能。

補助関数

次のコードは、補助関数 setupproblem1 を作成します。

function [trajectoryprob,p] = setupproblem1(T,air)
    if nargin == 1
        air = false;
    end
    N = 50;
    g = [0 0 -9.81];
    p0 = [0 0 0];
    pF = [5 10 3];
    Amax = 25;
    t = T/N;
    a = optimvar("a",N-1,3,"LowerBound",-Amax,"UpperBound",Amax);
    v = optimexpr(N,3);
    trajectoryprob = optimproblem;
    s = optimvar("s",N-1,"LowerBound",0,"UpperBound",3*Amax);
    trajectoryprob.Objective = sum(s)*t;
    scons = optimconstr(N-1);
    for i = 1:(N-1)
        scons(i) = norm(a(i,:)) <= s(i);
    end
    acons = optimconstr(N-1);
    for i = 1:(N-1)
        acons(i) = norm(a(i,:)) <= Amax;
    end
    np = 2:N;
    n0 = 1:(N-1);
    v0 = [0 0 0];
    if air
        v(1, :) = v0;
        for i = 2:N
            v(i,:) = v(i-1,:)*exp(-t) + t*(a(i-1,:) + g);
        end
    else
        gbig = repmat(g,size(a,1),1);
        v = cumsum([v0; t*(a + gbig)]);
    end
    p = cumsum([p0;t*(v(np,:) + v(n0,:))/2]); % Independent of "air"
    endcons = v(N,:) == [0 0 0];
    pcons2 = p(N,:) == pF;
    trajectoryprob.Constraints.acons = acons;
    trajectoryprob.Constraints.scons = scons;
    trajectoryprob.Constraints.endcons = endcons;
    trajectoryprob.Constraints.pcons2 = pcons2;
end

次のコードは、補助関数 plottrajandaccel を作成します。

function plottrajandaccel(sol,p,p0,pF)
figure
psol = evaluate(p, sol);
plot3(psol(:,1),psol(:,2),psol(:,3),'rx')
hold on
plot3(p0(1),p0(2),p0(3),'ks')
plot3(pF(1),pF(2),pF(3),'bo')
hold off
view([18 -10])
xlabel("x")
ylabel("y")
zlabel("z")
legend("Steps","Initial Point","Final Point")
figure
asolm = sol.a;
nasolm = sqrt(sum(asolm.^2,2));
plot(nasolm,"rx")
xlabel("Time step")
ylabel("Norm(acceleration)")
end

次のコードは、補助関数 tomin を作成します。

function F = tomin(T,air)
    if nargin == 1
        air = false;
    end
    problem = setupproblem1(T,air);
    opts = optimoptions("coneprog",Display="none",OptimalityTolerance=1e-9);
    [~,F] = solve(problem,"Options",opts);
end

次のコードは、補助関数 setupproblemfuel1 を作成します。

function [trajectoryprob,p] = setupproblemfuel1(T,air)
    r = 1/1000;
    if nargin == 1
        air = false;
    end
    N = 50;
    g = [0 0 -9.81];
    p0 = [0 0 0];
    pF = [5 10 3];
    Fmax = 50;
    t = T/N;
    F = optimvar("F",N-1,3,"LowerBound",-Fmax,"UpperBound",Fmax);
    v = optimexpr(N,3);
    fnorm = sqrt(sum(F(1:N-1,:).^2,2));
    m = 2 - r*t*cumsum(fnorm);
    trajectoryprob = optimproblem;
    trajectoryprob.Objective = sum(fnorm)*t;
    Fcons = fnorm <= Fmax;
    v0 = [0 0 0];
    if air
        v = fcn2optimexpr(@airResistance, v, v0, N, t, F, m, g);
    else
        gbig = repmat(g,size(F,1),1);
        mbig = repmat(m,1,3);
        v = cumsum([v0; t*(F./mbig + gbig)]);
    end
    np = 2:N;
    n0 = 1:(N-1);
    p = cumsum([p0;t*(v(np,:) + v(n0,:))/2]); % Independent of "air"
    endcons = v(N,:) == [0 0 0];
    pcons2 = p(N,:) == pF;
    trajectoryprob.Constraints.Fcons = Fcons;
    trajectoryprob.Constraints.endcons = endcons;
    trajectoryprob.Constraints.pcons2 = pcons2;
end

次のコードは、補助関数 setupproblemfuel2 を作成します。

function [trajectoryprob,p] = setupproblemfuel2(T,air)
    r = 1/1000;
    if nargin == 1
        air = false;
    end
    N = 50;
    g = [0 0 -9.81];
    p0 = [0 0 0];
    pF = [5 10 3];
    Fmax = 50;
    t = T/N;
    F = optimvar("F",N-1,3,"LowerBound",-Fmax,"UpperBound",Fmax);
    v = optimexpr(N,3);
    fnorm = sqrt(sum(F(1:N-1,:).^2,2));
    m = 2 - r*t*cumsum(fnorm);
    trajectoryprob = optimproblem;
    % Add cost for acceleration changes
    trajectoryprob.Objective = sum(fnorm)*t + t*1e-4*sum(diff(fnorm).^2);
    Fcons = fnorm <= Fmax;
    v0 = [0 0 0];
    if air
        v = fcn2optimexpr(@airResistance, v, v0, N, t, F, m, g);
    else
        gbig = repmat(g,size(F,1),1);
        mbig = repmat(m,1,3);
        v = cumsum([v0; t*(F./mbig + gbig)]);
    end
    np = 2:N;
    n0 = 1:(N-1);
    p = cumsum([p0;t*(v(np,:) + v(n0,:))/2]); % Independent of "air"
    endcons = v(N,:) == [0 0 0];
    pcons2 = p(N,:) == pF;
    trajectoryprob.Constraints.Fcons = Fcons;
    trajectoryprob.Constraints.endcons = endcons;
    trajectoryprob.Constraints.pcons2 = pcons2;
end

次のコードは、setupproblemfuel1 と setupproblemfuel2 で使用される補助関数 airResistance を作成します。

function v = airResistance(v, v0, N, t, F, m, g)
v(1, :) = v0;
for i = 2:N
    v(i,:) = v(i-1,:)*exp(-t) + t*(F(i-1,:)/m(i-1) + g);
end
end

参考

coneprog | solve

離散化された最適軌跡 (問題ベース)

問題の説明

T = 20 について問題を解く

最小コストの特定

最小とならない軌跡のプロット

空気抵抗の導入

可変質量を導入するためにモデルを拡張

可変加速度のコストを追加して解を平滑化

より適切な初期点の指定

空気抵抗と可変質量の導入

参考文献

補助関数

参考

関連するトピック