Posted at

3次スプライン補完


三次元スプライン補完とは

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}]


スプライン補完の条件式

スプライン補完の条件式は以下の5つ

条件1:各区間の始点

離散点$x_i$と$x_{i+1}$の区間を3次多項式を用いて補間された経路において、変数xが区間の始点$x_i$の時には


S_i(x_i)=y_i

と、3次多項式の値はyiになります。


条件2:位置の連続性

離散点$x_i$と$x_{i+1}$を3次多項式を用いて補間された経路において、変数$x$が終点$x_{i+1}$の時には

S_i(x_{i+1})=y_{i+1}

と、3次多項式の値は$y_{i+1}$になります。

また、先ほどの条件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}

の関係式が導かれます。

このように各区間の境界での条件として、各区間の位置は必ず連続する必要があります。


条件3:速度の連続性




条件2で示した位置の連続性に加えて、各区間の境界では速度も連続する必要があります。

離散点を補間する3次多項式の微分を

\frac{dS_i(x)}{dx}=S_i'(x)

と表します。

この時、各区間の境界条件として速度が連続するためには、

S'_i(x_{i+1}) = S'_{i+1}(x_{i+1})

が成り立つ必要があります。


条件4:加速度の連続性




条件2の位置、条件3の速度と同様に、各区間の境界では加速度も連続することで、滑らかの連続した経路を作成することが出来ます。

離散点を補間する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})

が条件として導かれます。


条件5:軌跡の始点と終点




離散点で与えられた指令点の始点と終点について、

S^{(2)}_0(0)=S^{(2)}_{end−1}(x_{end})=0

と、加速度を0に設定します。

このような始点と終点の加速度についての条件を用いた補間手法を自然3次スプライン補間と言います。



パラメータ導出

3次スプライン補完の5つの条件式からパラメータを導出します。

導出過程は以下のサイトを参照してください。

3次スプライン補間で軌跡生成:3次多項式のパラメータを求める(その1)

3次スプライン補間で軌跡生成:3次多項式のパラメータを求める(その2)


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


  1. 各区間の始点

  2. 位置の連続性

  3. 速度の連続性

  4. 加速度の連続性

  5. 軌跡の始点と終点

よりパラメータ$(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}

の関係が成り立つ。


離散点から3次多項式のパラメータを算出

実際に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()


まとめ

3次スプライン補完について簡単にまとめました。

scipyを使わず、C++やpythonで一から実装する方法については

3次スプライン補間の概要とC++,Pythonサンプルコードを参考にしてみてください。


参考

3次スプライン補間の概要とC++,Pythonサンプルコード

Tajima Robotics

[Pythonによる科学・技術計算] 3次スプライン補間,scipy