三次元スプライン補完とは
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$の時には
S_i(x_i)=y_i
と、3次多項式の値はyiになります。
条件２：位置の連続性
離散点$x_i$と$x_{i+1}$を3次多項式を用いて補間された経路において、変数$x$が終点$x_{i+1}$の時には
S_i(x_{i+1})=y_{i+1}
と、3次多項式の値は$y_{i+1}$になります。
また、先ほどの条件１でも示したように、次の離散点$x_{i+1}$と$x_{i+2}$の区間を補間した経路について、変数$x$が始点$x_{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}
の関係式が導かれます。
このように各区間の境界での条件として、各区間の位置は必ず連続する必要があります。
条件３：速度の連続性
\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})
が条件として導かれます。
条件５：軌跡の始点と終点
S^{(2)}_0(0)=S^{(2)}_{end−1}(x_{end})=0
と、加速度を0に設定します。
このような始点と終点の加速度についての条件を用いた補間手法を自然3次スプライン補間と言います。
パラメータ導出
３次スプライン補完の５つの条件式からパラメータを導出します。
導出過程は以下のサイトを参照してください。
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