三次元スプライン補完とは
3次スプライン補間とは、離散的な指令点が与えられた場合に、その離散点間を3次の多項式で補間し、滑らかな経路を作成する方法。
3次スプライン補間は、複雑な計算がなく、加速度の連続性が保たれており、自律移動ロボットの経路生成などにも相性が良い。
各離散点$x_i$と$x_{i+1}$の間を補間する3次多項式は
S_i(x)=a_i+b_i(x−x_i)+c_i(x−x_i)^2+d_i(x−x_i)^3
とパラメータ$(a_i, b_i, c_i, d_i)$を用いて定義される。
この時、変数xの取りうる範囲は
x=[x_i,x_{i+1}]
スプライン補完の条件式
スプライン補完の条件式は以下の５つ
離散点$x_i$と$x_{i+1}$の区間を3次多項式を用いて補間された経路において、変数xが区間の始点$x_i$の時には と、3次多項式の値はyiになります。条件１：各区間の始点
S_i(x_i)=y_i
離散点$x_i$と$x_{i+1}$を3次多項式を用いて補間された経路において、変数$x$が終点$x_{i+1}$の時には と、3次多項式の値は$y_{i+1}$になります。 また、先ほどの条件１でも示したように、次の離散点$x_{i+1}$と$x_{i+2}$の区間を補間した経路について、変数$x$が始点$x_{i+1}$の時には となります。 よって、 の関係式が導かれます。条件２：位置の連続性
S_i(x_{i+1})=y_{i+1}
S_{i+1}(x_{i+1})=y_{i+1}
S_i(x_{i+1})=S_{i+1}(x_{i+1})=y_{i+1}
このように各区間の境界での条件として、各区間の位置は必ず連続する必要があります。
と表します。 が成り立つ必要があります。条件３：速度の連続性
条件２で示した位置の連続性に加えて、各区間の境界では速度も連続する必要があります。
離散点を補間する3次多項式の微分を
\frac{dS_i(x)}{dx}=S_i'(x)
この時、各区間の境界条件として速度が連続するためには、S'_i(x_{i+1}) = S'_{i+1}(x_{i+1})
離散点を補間する3次多項式の2階微分を で表した時、各区間の境界条件として加速度が連続するためには、 が条件として導かれます。条件４：加速度の連続性
条件２の位置、条件３の速度と同様に、各区間の境界では加速度も連続することで、滑らかの連続した経路を作成することが出来ます。
\frac{dS_i(x)}{dx^2}=S^{(2)}_i(x_{i})
S^{(2)}_i(x_{i+1})=S^{(2)}_{i+1}(x_{i+1})
と、加速度を0に設定します。 このような始点と終点の加速度についての条件を用いた補間手法を自然3次スプライン補間と言います。条件５：軌跡の始点と終点
離散点で与えられた指令点の始点と終点について、
S^{(2)}_0(0)=S^{(2)}_{end−1}(x_{end})=0
パラメータ導出
３次スプライン補完の５つの条件式からパラメータを導出します。
導出過程は以下のサイトを参照してください。
3次スプライン補間で軌跡生成：３次多項式のパラメータを求める（その１）
3次スプライン補間で軌跡生成：３次多項式のパラメータを求める（その２）
3次多項式のパラメータの関係式
離散点$x_i$と$x_{i+1}$の間を補間する3次多項式$S_i$
S_i(x)=a_i+b_i(x−x_i)+c_i(x−x_i)^2+d_i(x−x_i)^3
- 各区間の始点
- 位置の連続性
- 速度の連続性
- 加速度の連続性
- 軌跡の始点と終点
よりパラメータ$(a_i, b_i, c_i, d_i)$について
a_i = y_i \\
h_{i+1}+c_{i+1}+2(h_{i+1}+h_i)c_{i+1}+c_ih_i=\frac{3}{h_{i+1}}(a_{i+2}-a_{i+1})-\frac{3}{h_i}(a_{i+1}-a_i)\\
b_i = \frac{a_{i+1}-a_i}{h_i}-\frac{h_i(c_{i+1}+2c_i)}{3} \\
d_i = \frac{c_{i+1}-c_i}{3h_i}
の関係が成り立つ。
離散点から３次多項式のパラメータを算出
実際にX-Y平面上に表された離散的な指令点を連続的に補間していきます。
今回の記事で補間する離散的な指令点は下記のように定義します。
| X - axis | y - axis |
|---|---|
| 0 | 0 |
| 1 | 3 |
| 4 | 4 |
| 5 | 1 |
| 8 | 2 |
この離散的に与えられた指令点について、入力をX軸、出力をY軸として3次スプライン補間を用いて連続的で滑らかな経路を生成します。
Pythonで実装
[Pythonによる科学・技術計算] 3次スプライン補間，scipyを使わせてもらいました。
scipy.interpolateのinterp1dメソッドを利用して3次スプライン補間を行うことができます。
from scipy.interpolate import interp1d
import numpy as np
import matplotlib.pyplot as plt
x = np.array([0, 1, 4 ,5, 8])
y = np.array([0, 3, 4, 1, 2])
f_line = interp1d(x, y)
f_CS = interp1d(x, y, kind='cubic')
#for plot
xnew =np.linspace(0, 8, num=50)
plt.plot(x, y, 'o')
plt.plot(xnew, f_CS(xnew), '-')
plt.legend(['Raw data','Lagrange', 'Cubic spline'], loc='best')
plt.show()
まとめ
３次スプライン補完について簡単にまとめました。
scipyを使わず、C++やpythonで一から実装する方法については
３次スプライン補間の概要とC++,Pythonサンプルコードを参考にしてみてください。
参考
３次スプライン補間の概要とC++,Pythonサンプルコード
Tajima Robotics
[Pythonによる科学・技術計算] 3次スプライン補間，scipy