區域性路徑規劃 01 曲線插值法

2022-10-04 03:09:10 字數 2981 閱讀 7349

\[\begin

x\left( t \right)= a_0 +a_1t+a_2t^2+a_3t^3\\

y\left( t \right)=b_0 +b_1t+b_2t^2+b_3t^3\\

\end\]

\[\begin

x\left( t \right)= a_0 +a_1t+a_2t^2+a_3t^3 +a_4t^4+a_5t^5\\

y\left( t \right)=b_0 +b_1t+b_2t^2+b_3t^3+b_4t^4+b_5t^5\\

\end\]

\[\begin

x\left( t \right)= a_0 +a_1t+a_2t^2+a_3t^3 +a_4t^4+a_5t^5 + a_6t^6+a_7t^7\\

y\left( t \right)=b_0 +b_1t+b_2t^2+b_3t^3+b_4t^4+b_5t^5 +b_6t^6+b_7t^7\\

\end\]

首先,我們定義如下的五次多項式:

\[\begin

x\left( t \right)= a_0 +a_1t+a_2t^2+a_3t^3 +a_4t^4+a_5t^5\\

y\left( t \right)=b_0 +b_1t+b_2t^2+b_3t^3+b_4t^4+b_5t^5\\

\end\]

我們將起始時刻定義為\(t_0\),起始時刻的位置、速度和加速度均已知,我們構造如下的縱向和橫向方程:

\[\begin

x\left( t_0 \right)= a_0 +a_1t_0+a_2t_0^2+a_3t_0^3 +a_4t_0^4+a_5t_0^5\\

y\left( t_0 \right)=b_0 +b_1t_0+b_2t_0^2+b_3t_0^3+b_4t_0^4+b_5t_0^5\\

\end\]

\[\begin

x^\prime\left( t_0 \right)= a_1 +2a_2t_0+3a_3t_0^2 +4a_4t_0^3+5a_5t_0^4\\

y^\prime\left( t_0 \right)=b_1 +2b_2t_0+3b_3t_0^2+4b_4t_0^3+5b_5t_0^4\\

\end\]

\[\begin

x^\left( t_0 \right)= 2a_2+6a_3t_0 + 12a_4t_0^2+20a_5t_0^3\\

y^\left( t_0 \right)=2b_2+6b_3t_0 + 12b_4t_0^2+20b_5t_0^3\\

\end\]

同理,我們可以構造終止時刻\(t_1\)的的橫向和縱向方程,這裡就不再詳細描述;

我們將起始位置和終止位置的橫縱向方程統一用矩陣表達為:

\[x=\left[

\begin

x_0 \\

x_0^\prime\\

x_0^\\

x_1\\

x_1^\prime\\

x_1^\\

\end

\right] = \left[

\begin

t_0^5 & t_0^4 & t_0^3 & t_0^2 & t_0 & 1 \\

5t_0^4 & 4t_0^3 & 3t_0^2 & 2t_0^1 & 1 & 0 \\

20t_0^3 & 12t_0^2 & 6t_0 & 2 & 0 & 0 \\

t_1^5 & t_1^4 & t_1^3 & t_1^2 & t_1 & 1 \\

5t_1^4 & 4t_1^3 & 3t_1^2 & 2t_1^1 & 1 & 0 \\

20t_1^3 & 12t_1^2 & 6t_1 & 2 & 0 & 0 \\

\end

\right]

\left[

\begin

a_5 \\

a_4\\

a_3\\

a_2\\

a_1\\

a_0\\

\end

\right]=t\times a\]

\[y=\left[

\begin

y_0 \\

y_0^\prime\\

y_0^\\

y_1\\

y_1^\prime\\

y_1^\\

\end

\right] = \left[

\begin

t_0^5 & t_0^4 & t_0^3 & t_0^2 & t_0 & 1 \\

5t_0^4 & 4t_0^3 & 3t_0^2 & 2t_0^1 & 1 & 0 \\

20t_0^3 & 12t_0^2 & 6t_0 & 2 & 0 & 0 \\

t_1^5 & t_1^4 & t_1^3 & t_1^2 & t_1 & 1 \\

5t_1^4 & 4t_1^3 & 3t_1^2 & 2t_1^1 & 1 & 0 \\

20t_1^3 & 12t_1^2 & 6t_1 & 2 & 0 & 0 \\

\end

\right]

\left[

\begin

b_5 \\

b_4\\

b_3\\

b_2\\

b_1\\

b_0\\

\end

\right]=t\times b\]

對上述矩陣進行求解(\(a=t^ \times x\),\(b=t^ \times y\)),我們可以得到唯一的係數矩陣解,即得到唯一的多項式曲線,該曲線上每一點的導數代表著車輛經過該點時的速度。多項式曲線插值法軌跡規劃出的曲線是路徑+速度的耦合結果。

注意:曲線插值法得到的軌跡曲線是關於時間\(t\)的函式,而非座標\(y\)關於座標\(x\)的函式。

與雙圓弧段換道軌跡進行對比:

待更新,後續會上傳到github上。

題解0003 曲線

題目描述 乙個函式f x ma x si x f x max si x si x ax 2 bx c i 1,2.n i 1,2.n i 1,2.n 輸出函式f x f x 的在區間 0 1000 0 1000 上的最小值0,1000 題目思路 三分求解,將範圍分成3份,判斷左右哪邊比較小,然後縮小...

測試趨勢6曲線解讀

1 背景 2 名詞解釋 3 趨勢圖 4 解讀 1 背景 2 名詞解釋 1 提交的bug指提交的有效bug,重複的,非錯的不算在內 2 開啟的bug指提交的bug中被開啟的bug 狀態不一定是待改,比如分配,分析,都算開啟的bug 3 待處理bug指不是關閉狀態所有bug 4 修改bug指狀態是己改的...

Part 7 曲線積分

分割,取近似,作和,取極限。極限存在,與分割法無關 空間曲線弧長 加權 線密度 的平面 權連續的 曲線。總結成一般的點函式形式 int f p mathrm ds lim limits sum limits nf p i delta s i sum limits n m m k 的上確界 分段光滑曲...