二次計画法 (QP) スプラインパスの最適化

元のアドレス: https://daobook.github.io/apollo/docs/specs/qp_spline_path_optimizer_cn.html
二次計画法 (QP) + スプライン補間

1. 目的関数

1.1 経路長の取得

パスはステーション横座標系で定義されます。変化間隔sは、車両の現在位置からデフォルトの経路までの長さです。

1.2 スプラインセグメントの取得

パスをn個のセグメントに分割し、パスの各セグメントは多項式で表されます。

1.3 スプラインセグメント関数の定義

各スプラインセグメントi は、基準線に沿った累積距離 $d_iを持ちます。$ 。各セグメントのパスは、デフォルトでは 5 方向多項式で表されます。

$f_i(s ) = a_{i0} + a_{i1} \cdot s + a_{i2} \cdot s^2 + a_{i3} \cdot s^3 + a_{i4} \cdot s^4 + a_{i5} \ cdot s^5 (0 \leq s \leq d_{i})$

1.4 各スプラインセグメントの最適化目的関数を定義する

$\sum_{i=1}^{n} \Big( w_1 \cdot \int\limits_{0}^{d_i} (f_i')^2(s) ds + w_2 \cdot \int\limits_{0}^{d_i} (f_i'')^2(s) ds + w_3 \cdot \int\limits_{0}^{d_i} (f_i^{\prime\prime\prime })^2(s) ds \Big)$

1.5 コスト関数を QP 式に変換する

QP公式:
$\begin{aligned} minimum & \frac{1}{2} \cdot x^T \cdot H \cdot x + f^T \cdot x \\ st \qquad & LB \leq x \leq UB \\ & A_{eq}x = b_{eq} \\ & Ax \geq b \end{aligned}$
以下は、コスト関数を QP 式に変換する例です。
$f_i(s) ＝ \begin{vmatrix} 1 & s & s^2 & s^3 & s^4 & s^5 \end{vmatrix} \cdot \begin{vmatrix} a_{i0} \\ a_{i1} \\ a_ {i2} \\ a_{i3} \\ a_{i4} \\ a_{i5} \end{vmatrix}$

また
$f_i'(s) = \begin{vmatrix} 0 & 1 & 2s & 3s^2 & 4s^3 & 5s^4 \end{vmatrix} \cdot \begin{vmatrix} a_{i0} \\ a_{i1} \\ a_{i2} \\ a_{i3} \ \ a_{i4} \\ a_{i5} \end{vmatrix}$

また
$f_i'(s)^2 = \begin{vmatrix} a_{i0} & a_{i1} & a_{i2} & a_{i3 } & a_{i4} & a_{i5} \end{vmatrix} \cdot \begin{vmatrix} 0 \\ 1 \\ 2s \\ 3s^2 \\ 4s^3 \\ 5s^4 \end{vmatrix} \cdot \begin{vmatrix} 0 & 1 & 2s & 3s^2 & 4s^3 & 5s^4 \end{vmatrix} \cdot \begin{vmatrix} a_{i0} \\ a_{i1} \\ a_{ i2} \\ a_{i3} \\ a_{i4} \\ a_{i5} \end{vmatrix}$
その後、
$\int\limits_{0}^{d_i} f_i'(s)^2 ds ＝ \int \limits_{0}^{d_i} \begin{vmatrix} a_{i0} & a_{i1} & a_{i2} & a_{i3} & a_{i4} & a_{i5} \end{vmatrix} \cdot \begin{vmatrix} 0 \\ 1 \\ 2s \\ 3s^2 \\ 4s^3 \\ 5s^4 \end{vmatrix} \cdot \begin{vmatrix} 0 & 1 & 2s & 3s^2 & 4s ^3 & 5s^4 \end{vmatrix} \cdot \begin{vmatrix} a_{i0} \\ a_{i1} \\ a_{i2} \\ a_{i3} \\ a_{i4} \\ a_{ i5} \end{vmatrix} ds$

結合関数中引出常量得，
$\int\limits_{0}^{d_i} f'(s) ^2 ds ＝ \begin{vmatrix} a_{i0} & a_{i1} & a_{i2} & a_{i3} & a_{i4} & a_{i5} \end{vmatrix} \cdot \int\limits_{ 0}^{d_i} \begin{vmatrix} 0 \\ 1 \\ 2s \\ 3s^2 \\ 4s^3 \\ 5s^4 \end{vmatrix} \cdot \begin{vmatrix} 0 & 1 & 2s & 3s^2 & 4s^3 & 5s^4 \end{vmatrix} ds \cdot \begin{vmatrix} a_{i0} \\ a_{i1} \\ a_{i2} \\ a_{i3} \\ a_ {i4} \\ a_{i5} \end{vmatrix}$

$＝\begin{vmatrix} a_{i0} & a_{i1} & a_{i2} & a_{i3} & a_{i4} & a_{i5} \end{vmatrix} \cdot \int\limits_{0}^{d_i} \begin{vmatrix} 0 & 0 &0&0&0&0\\ 0 & 1 & 2s & 3s^2 & 4s^3 & 5s^4 \\ 0 & 2s & 4s^2 & 6s^3 & 8s^4 & 10s^5\\ 0 & 3s^2 & 6s^3 & 9s^4 & 12s^5&15s^6 \\ 0 & 4s^3 & 8s^4 &12s^5 &16s^6&20s^7 \\ 0 & 5s^4 & 10s^5 & 15s^6 & 20s^7 & 25s^8 \end{vmatrix} ds \cdot \begin{vmatrix} a_{i0 } \\ a_{i1} \\ a_{i2} \\ a_{i3} \\ a_{i4} \\ a_{i5}\end{vmatrix}$

ついに手に入れた、

$\int\limits_{0}^{d_i} f'_i(s) ^2 ds =\begin{vmatrix} a_{i0} & a_{i1} & a_{i2} & a_{i3} & a_{i4} & a_{i5} \end{vmatrix} \cdot \begin{vmatrix} 0 & 0 & 0 & 0 &0&0\\ 0 & d_i & d_i^2 & d_i^3 & d_i^4&d_i^5\\ 0& d_i^2 & \frac{4}{3}d_i^3& \frac{6} {4}d_i^4 & \frac{8}{5}d_i^5&\frac{10}{6}d_i^6\\ 0& d_i^3 & \frac{6}{4}d_i^4 & \frac {9}{5}d_i^5 & \frac{12}{6}d_i^6&\frac{15}{7}d_i^7\\ 0& d_i^4 &\frac{8}{5}d_i^5 & \frac{12}{6}d_i^6 & \frac{16}{7}d_i^7&\frac{20}{8}d_i^8\\ 0& d_i ^5 & \frac{10}{6}d_i^6 & \frac{15}{7}d_i^7 & \frac{20}{8}d_i^8&\frac{25}{9}d_i^9 \ end{vmatrix} \cdot \begin{vmatrix} a_{i0} \\ a_{i1} \\ a_{i2} \\ a_{i3} \\ a_{i4} \\ a_{i5} \end{vmatrix}$

最終的には、5 次元スプライン補間の導出コストを表す 6 次元行列になることに注意してください。
同じ推論方法を適用すると、2 次元および 3 次元のスプライン補間の導出コストを取得できます。

2 制約

2.1 初期点の制約

最初の点が ( $s_0であると仮定します)$ 、 $l_0$ )、( $s_0$ 、 $l'_0$ ) と ( $s_0$ 、 $l'_0$ )、ここで $l_0$ 、 $l'_0$ そして $l''_0$ $f_i(s)$ から取得できます。 $f (s)$ 、 $f'_i(s)$ 、 $f_i(s)''$ と計算されます。

次の式を使用して、上記の制約を QP 制約式に変換します。

$A_{eq}x = b_{eq}$

変換の具体的な手順は次のとおりです。

$f_i(s_0) = \begin{vmatrix } 1 & s_0 & s_0^2 & s_0^3 & s_0^4&s_0^5 \end{vmatrix} \cdot \begin{vmatrix} a_{i0} \\ a_{i1} \\ a_{i2} \\ a_{ i3} \\ a_{i4} \\ a_{i5}\end{vmatrix} = l_0$
また
$f'_i( s_0) = \begin{vmatrix} 0& 1 & 2s_0 & 3s_0^2 & 4s_0^3 &5 s_0^4 \end{vmatrix} \cdot \begin{vmatrix} a_{i0} \\ a_{i1} \\ a_{ i2} \\ a_{i3} \\ a_{i4} \\ a_{i5} \end{vmatrix} = l'_0$
また
$f''_i(s_0) = \begin{vmatrix} 0&0& 2 & 3\times2s_0 & 4\times3s_0^2 & 5\times4s_0^3 \end{vmatrix} \cdot \begin{vmatrix} a_{i0} \\ a_{i1} \\ a_{i2} \\ a_{i3} \\ a_{i4} \\ a_{i5} \end{vmatrix} = l''_0$
その中に、i には $s_0が含まれています$ スプラインセグメントのインデックス値。

2.2 エンドポイントの制約

始点、終点と同じ $s_e, l_e)$ は、開始点の計算方法に従って制約も生成する必要があります。

開始点と終了点を組み合わせると、制約式は次のようになります。

$\begin{vmatrix} 1 & s_0 & s_0^2 & s_0^3 & s_0^4&s_0^5 \\ 0&1 & 2s_0 & 3s_0^2 & 4s_0^3 & 5s_0^4 \\ 0& 0&2 & 3\times2s_0 & 4\times3s_0^2 & 5\times4s_0^3 \\ 1 & s_e & s_e^2 & s_e^3 & s_e^ 4&s_e ^5 \\ 0&1 & 2s_e & 3s_e^2 & 4s_e^3 & 5s_e^4 \\ 0& 0&2 & 3\times2s_e & 4\times3s_e^2 & 5\times4s_e^3 \end{vmatrix} \cdot \begin{; vmatrix} a_{i0}\\a_{i1}\\a_{i2}\\a_{i3}\\a_{i4}\\a_{i5}\end{vmatrix}=\begin{vmatrix}l_0\\ l'_0\\l'_0\\ l_e\\ l_e\\ l'_e\\ \end{vmatrix}$

2.3 スムーズノード制約

このコンストレイントの目的は、スプラインのノードをより滑らかにすることです。2 つのセグメント $seg_k を想定します$ および $seg_{k+1}$ 相互に接続され、 $seg_k$ 累積値 s は $s_k$ 。制約を計算する式は次のとおりです。

$f_k(s_k) = f_{k+1} (s_0)$
テーブルの数を指定します:
$\begin{vmatrix} 1 & s_k & s_k^2 & s_k^ 3 & s_k^4&s_k^5 \\ end{vmatrix} \cdot \begin{vmatrix} a_{k0} \\ a_{k1} \\ a_{k2} \\ a_{k3} \\ a_{k4}\ \a_ {k5}\end{vmatrix} = \begin{vmatrix}1&s_{0}&s_{0}^2&s_{0}^3&s_{0}^4&s_{0}^5\\ \end{vmatrix} \cdot \begin {vmatrix} a_{k+1,0} \\ a_{k+1,1} \\ a_{k+1,2} \\ a_{k+1,3} \\a_{k+1.4}\ \a_{k+1.5}\end{vmatrix}$
∣
$\begin{vmatrix} 1 & s_k & s_k^2 & s_k^3 & s_k^4&s_k ^ 5 & -1 & -s_{0} & -s_{0}^2 & -s_{0}^3 & -s_{0}^4&-s_{0}^5\\\end{vmatrix}\ cdot \begin{vmatrix} a_{k0} \\ a_{k1} \\ a_{k2} \\ a_{k3} \\ a_{k4} \\ a_{k5} \\ a_{k+1,0} \ \ a_{k+1,1} \\ a_{k+1,2} \\ a_{k+1,3} \\ a_{k+1,4} \\ a_{k+1,5} \終了 { vmatrix } = 0$
意志 $s_0$ = 0 が式に代入されます。

同様に、制約方程式は次のように計算できます
$f'_k(s_k) = f'_{k+1} (s_0) \\ f''_k(s_k) = f''_{k+1} ( s_0) \\ f''_k(s_k) = f''''_{k+1} (s_0)$

2.4 ポイントサンプリング境界制約

パス上のm点を均等にサンプリングし、これらの点で障害物の境界を確認します。次の不等式を使用して、これらの制約を QP 制約の不等式に変換します。

$\geq b$

まず、道路幅と周囲の障害物に基づいて点 $(s_j, l_j)を見つけます。$ 下限の $l_{lb,j}$ ，しかも $j\in[0, m]$ 。制約を計算する不等式は次のとおりです。

$\begin{vmatrix} 1 & s_0 & s_0^2 & s_0^3 & s_0^4&s_0^5 \\ 1 & s_1 & s_1^2 & s_1^3 & s_1^4&s_1^5 \\ ... &...&...&...&...&... \\ 1 & s_m & s_m^2 & s_m^3 & s_m^4&s_m^5 \\ \end{vmatrix} \cdot \begin {vmatrix}a_{i0} \\ a_{i1} \\ a_{i2} \\ a_{i3} \\ a_{i4} \\ a_{i5} \end{vmatrix} \geq \begin{vmatrix} l_ {lb,0}\\ l_{lb,1}\\ ...\\ l_{lb,m}\\ \end{vmatrix}$

同様に、上限 $l_{ub,j}$ 次を解きます:
$\begin{vmatrix} -1 & -s_0 & -s_0^2 & -s_0^3 & -s_0^4&-s_0^5 \\ -1 & -s_1 & -s_1^2 & -s_1^3 & -s_1^4&-s_1^5 \\ ...&...-&...&...&...&... \\ -1 & -s_m & -s_m^2 & -s_m ^3 & -s_m^4&-s_m^5 \\ end{vmatrix} \cdot \begin{vmatrix} a_{i0} \\ a_{i1} \\ a_{i2} \\ a_{i3} \\ a_ { i4} \\ a_{i5} \end{vmatrix} \geq -1 \cdot \begin{vmatrix} l_{ub,0}\\ l_{ub,1}\\ ...\\ l_{ub, m }\\\end{vmatrix}$