N次スプライン曲線でN+1個の離散点を滑らかに補間する方法

N次のスプラインを用いることで、N+1個の離散的な点を通る滑らかな軌跡を、曲線近似により生成することが出来ます。

今回の記事では、N＋１個の位置指令が与えられた場合に、その点を通る滑らかな曲線をN次のスプライン曲線で結ぶ方法を紹介します。

Contents

離散点補間の重要性
N次スプライン曲線による離散点補間
まとめ

離散点補間の重要性

離散的な点を滑らかに補間することは、ロボットの軌跡生成においてとても重要です。

この補間技術は、自動運転車の経路を生成する際にも用いることが出来る大事な手法です。

N次スプライン曲線による離散点補間

N+1個の点が与えられた場合は、N次のスプライン曲線（N次多項式）により滑らかに補間することが出来ます。

つまり、4つの点が与えられた場合は3次のスプライン曲線を用いて補間することが出来ます。

そして、5つの点が与えられた場合は、4次のスプライン曲線を用いることで、滑らかな離散点補間を実現できます。

今回は一般式として、N+1個の制御点が

$$ \begin{eqnarray} \left\{ \begin{array}{l} \boldsymbol{ P_0 } &=& \begin{bmatrix} X_0 & Y_0 \end{bmatrix}^T \\ \boldsymbol{ P_1 } &=& \begin{bmatrix} X_1 & Y_1 \end{bmatrix}^T \\ \boldsymbol{ P_2 } &=& \begin{bmatrix} X_2 & Y_2 \end{bmatrix}^T \\ \vdots &=& \vdots \\ \boldsymbol{ P_{N-1} } &=& \begin{bmatrix} X_{N-1} & Y_{N-1} \end{bmatrix}^T \\ \boldsymbol{ P_N } &=& \begin{bmatrix} X_N & Y_N \end{bmatrix}^T \end{array} \right. \end{eqnarray} $$

と与えられた場合について、N次スプラインによる曲線補間の式を紹介します。

N次スプライン曲線の基本式

N+1個の離散的な制御点（指令点）を曲線近似するためのN次のスプライン曲線の基本式は

$$ S (u) = a u^{N} + b u^{N-1} + c u^{N-2} + \cdots + d u+ e $$

もしくは、各次数の係数を書き換えて

$$ S (u) = a_{N} u^{N} + a_{N-1} u^{N-1} + a_{N-2} u^{N-2} + \cdots + a_1 u+ a_0 $$

と表されます。

ここで、このN次スプライン曲線の基本式に含まれる変数uは

$$ u \in \begin{bmatrix} 0, \displaystyle \sum_{ i=1 }^{ N } l_i \end{bmatrix}, \ \ l_i = \vert \boldsymbol{ P_i } – \boldsymbol{ P_{i-1} } \vert $$

のように、与えられたN+1個の制御点間の距離を用いて表されます。

このN次のスプライン曲線の基本式に含まれるパラメータを決定するために、スプライン曲線補間の制約条件を求めます。

スプライン曲線補間の制約条件

この基本式に含まれる各パラメータを算出するための制約条件は、与えられた制御点の位置情報から

$$ \begin{eqnarray} \left\{ \begin{array}{l} S (u_0) &=& S (0) &=& P_0 \\ S (u_1) &=& S (l_1) &=& P_1 \\ S (u_2) &=& S (l_1 + l_2) &=& P_2 \\ \vdots &=& \vdots &=& \vdots \\ S (u_{N-1}) &=& S (l_1 + l_2 + \cdots + l_{N-1}) &=& P_{N-1} \\ S (u_N) &=& S (l_1 + l_2 + \cdots + l_{N-1} + l_N) &=& P_N \end{array} \right. \end{eqnarray} $$

と表すことが出来ます。

この制約条件をもとに、N次のスプライン曲線の基本式に含まれるパラメータを決定します。

基本式のパラメータ算出

この制約条件から、求めたいパラメータと制約条件との関係式は

$$ \begin{bmatrix} {u_0}^N & {u_0}^{N-1} & \cdots & {u_0}^2 & {u_0} & 1 \\ {u_1}^N & {u_1}^{N-1} & \cdots & {u_1}^2 & {u_1} & 1 \\ {u_2}^N & {u_2}^{N-1} & \cdots & {u_2}^2 & {u_2} & 1 \\ \vdots & \vdots & \ddots & \vdots & \vdots & \vdots \\ {u_{N-1}}^N & {u_{N-1}}^{N-1} & \cdots & {u_{N-1}}^2 & {u_{N-1}} & 1 \\ {u_N}^N & {u_N}^{N-1} & \cdots & {u_N}^2 & {u_N} & 1 \end{bmatrix} \begin{bmatrix} a_N \\ a_{N-1} \\ \vdots \\ a_2 \\ a_1 \\ a_0 \end{bmatrix} = \begin{bmatrix} P_0 \\ P_1 \\ P_2 \\ \vdots \\ P_{N-1} \\\ P_N \end{bmatrix} $$

のように行列式の形で表すことが出来ます。

この行列式による関係式を解くことでN次スプライン曲線の基本式に含まれるパラメータ（a_N, a_N-1, …, a₁, a₀）は

$$ \begin{bmatrix} a_N \\ a_{N-1} \\ \vdots \\ a_2 \\ a_1 \\ a_0 \end{bmatrix} = \begin{bmatrix} {u_0}^N & {u_0}^{N-1} & \cdots & {u_0}^2 & {u_0} & 1 \\ {u_1}^N & {u_1}^{N-1} & \cdots & {u_1}^2 & {u_1} & 1 \\ {u_2}^N & {u_2}^{N-1} & \cdots & {u_2}^2 & {u_2} & 1 \\ \vdots & \vdots & \ddots & \vdots & \vdots & \vdots \\ {u_{N-1}}^N & {u_{N-1}}^{N-1} & \cdots & {u_{N-1}}^2 & {u_{N-1}} & 1 \\ {u_N}^N & {u_N}^{N-1} & \cdots & {u_N}^2 & {u_N} & 1 \end{bmatrix}^{-1} \begin{bmatrix} P_0 \\ P_1 \\ P_2 \\ \vdots \\ P_{N-1} \\\ P_N \end{bmatrix} $$

と求めることが出来ます。