自動運転アルゴリズム(10)：多項式軌道とMinimun Snap閉形式解の原理とコード解説

1 多項式軌道とミニムンスナップ閉形式解法原理

        前回説明した Minimun Snap は、実際には 2 次関数の最適解を探しています。

        制約 $p^TQp$ の下での最適化関数の最小値にすることができます。 > $AP=b$ 。

        しかし、これは分析的な最適解ではなく漸近的な最適解です。分析的な最適解は存在するのでしょうか?

        $Ap = d$ の形式を使用して次のことを表現します (d は、旅行の各セクションの開始点と終了点の位置、速度、加速度を表します)。たとえば、< /span>。各セグメントには開始点と終了点があります。開始点と終了点には、位置、速度の 3 つの変数があります。と加速度なので、次元は 2*3k = 6k です。      セグメントの数は k セグメントです $p_{1}(t_0)、v_{1}(t_0)、a_{1}(t_0)、p_{1}(t_1)、v_{1}(t_1)、a_{1}(t_1)$ は、最初のセグメントの始点の位置、速度、加速度、および最初のセグメントの終点の位置、速度、加速度を意味します。

        したがって、特定の p を見つけることができ、 $p^TQp$ を追加した後、最小値を計算できます。

        行列 A の次元は K 個のセグメントがあり、各セグメントは次数 n+1 のパラメータであり、各セグメントの位置は 6 であるため、次元は k(n+1) * 6K です。

                                                 $A_{合計} \begin{bmatrix} p_1\\p_2 \\... \\... \\p_k \end{bmatrix} = \begin{bmatrix} d_1\\d_2 \\... \\.. . \\d_k \end{bmatrix}$

        さらに遠く：

         $p = A^{-1}d$ これが私たちが解きたい方程式です。

        まず 1 つの段落、つまり最初の段落と 2 番目の段落を見てみましょう。 $p_i$ は i 番目の段落を表します。絵を描きました:

        そして $t^{(i)}$ は段落 1 の終わりと段落 2 の始まりです。

        重複する変数を削除します。

        合計 k+1 のウェイポイントがあり、各ウェイポイントには p、v、a があります。したがって、行列のサイズは 3(k+1) になります。

        d = Md' を取得します。

        d=C[dF dP]T、ここで dF は、始点と終点の変位、速度、加速度を含む既知の量と、中点間の既知の量を表します (これらを通過させたいと考えています)ポイント）。 dP は未知の量を表します。簡単な例を見てみましょう。

        ?

        dP が未知であることがわかっているので、dP の導関数を導出し、dP の解析解を見つけることができます。

        衝突を避けるためにウェイポイントを手動で追加できます。

2 コード分析

        前回のブログとの違いのみ説明します。

        最初に実行されている結果を見てみましょう。

        コードを再現してみましょう。
function polys = minimum_snap_single_axis_close_form(wayp,ts,n_order,v0,a0,v1,a1)
% 参数个数 = 阶数+1
n_coef = n_order+1;
% 多项式的段数:航路点-1
n_poly = length(wayp)-1;
% 计算Q 和原来的一样
Q_all = [];
for i=1:n_poly
    Q_all = blkdiag(Q_all,computeQ(n_order,3,ts(i),ts(i+1)));
end
ここでも、n_coef はパラメータの数を表すために引き続き使用され (数値はフィッティング曲線の最大次数 + 1)、n_poly は多項式セグメントの数を表すために使用され、サイズはウェイポイントの数 - 1 です。 Qのやり方は前回のブログと同じなので割愛します。

閉形式解のステップを見てみましょう。まず A 行列を計算しましょう。
% compute A (n_continuous*2*n_poly) * (n_coef*n_poly)
n_continuous = 3;  % 1:p  2:pv  3:pva  4:pvaj  5:pvajs
% A的维度是 3*2*K（航路点个数） 阶数（N+1） * K
A = zeros(n_continuous*2*n_poly,n_coef*n_poly);
% 遍历每一段航路点
for i = 1:n_poly
    % 位置、速度、加速度
    for j = 1:n_continuous
        % 计算具体参数 j--参数个数（阶数+1）
        for k = j:n_coef
            if k==j
                t1 = 1;
                t2 = 1;
            else %k>j
                t1 = tk(i,k-j+1);
                t2 = tk(i+1,k-j+1);
            end
            % 每一段填充首和尾
            A(n_continuous*2*(i-1)+j,n_coef*(i-1)+k) = prod(k-j+1:k-1)*t1;
            A(n_continuous*2*(i-1)+n_continuous+j,n_coef*(i-1)+k) = prod(k-j+1:k-1)*t2;
        end
    end
end
        ウェイポイントがセグメント K の場合、近似したい多項式の次数は N であることがわかります。したがって、A 行列のサイズは、ウェイポイントが K セグメントであることです。各セグメントには開始点と終了点があります。各開始点と終了点には、X、V、A の 3 つの要素があります。したがって、行列のサイズはは K*2*3 * (N+1 )*K です。

        まずは0に初期化します。各ウェイポイントを移動し、最初にこのウェイポイントの変位、速度、加速度を計算します。

        次に、M 行列を計算します。
M = zeros(n_poly*2*n_continuous,n_continuous*(n_poly+1));
for i = 1:n_poly*2
    j = floor(i/2)+1;
    rbeg = n_continuous*(i-1)+1;
    cbeg = n_continuous*(j-1)+1;
    M(rbeg:rbeg+n_continuous-1,cbeg:cbeg+n_continuous-1) = eye(n_continuous);
end
M
C 行列と K 行列を計算します。
% compute C
num_d = n_continuous*(n_poly+1);
C = eye(num_d);
df = [wayp,v0,a0,v1,a1]';% fix all pos(n_poly+1) + start va(2) + end va(2) 
fix_idx = [1:3:num_d,2,3,num_d-1,num_d];
free_idx = setdiff(1:num_d,fix_idx);
C = [C(:,fix_idx) C(:,free_idx)];

% K
AiMC = inv(A)*M*C;
R = AiMC'*Q_all*AiMC;

n_fix = length(fix_idx);
Rff = R(1:n_fix,1:n_fix);
Rpp = R(n_fix+1:end,n_fix+1:end);
Rfp = R(1:n_fix,n_fix+1:end);
Rpf = R(n_fix+1:end,1:n_fix);
p の回復軌道、位置、加速度を計算します。
dp = -inv(Rpp)*Rfp'*df;

p = AiMC*[df;dp];

polys = reshape(p,n_coef,n_poly);

自動運転アルゴリズム(10)：多項式軌道とMinimun Snap閉形式解の原理とコード解説

1 多項式軌道とミニムンスナップ閉形式解法原理

2 コード分析

おすすめ