MyEnigma

とある自律移動システムエンジニアのブログです。#Robotics #Programing #C++ #Python #MATLAB #Vim #Mathematics #Book #Movie #Traveling #Mac #iPhone

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

 

目次

はじめに

3次スプライン補間は、

計算がそこまで複雑ではなく、

また二次微分までの連続性が担保されているため、

様々な用途に利用されています。

 

今回の記事では、

この3次スプライン曲線の概要と、

3次スプライン曲線を作成する

C++, Pythonのサンプルコードを作成したので、

公開したいと思います。

 

各種スプラインにおける連続性

使用するスプラインのモデルに応じて、

様々な補間曲線を生成することができますが、

その補間曲線のなめらかを表すのに

C_0, C_1といいったような表現をします。

 

この表現は、状態空間の各次元での連続性を表しており、

例えば、二次元空間でのスプライン曲線では、

下記の図のように、

C_0はx-yの位置のみが連続

C_1は位置を微分した方位連続、

C_2は方位を微分した曲率の連続を担保していることを意味します。

 

今回の記事で取り扱う3次スプラインは、

C0, C1, C2を担保しているといえます。

 

3次スプライン補間とは?

3次スプライン補間は下記の図のように、

複数のサンプル点が与えられた時に、

そのサンプル点の間を3次の多項式で近似し、

滑らかに補間する手法です。

各データの間を区切り、

各区間において、それぞれ別の3次多項式で近似する部分が特徴的です。

 

3次多項式なので、各データ区間は下記の式で近似されます。

但し、それぞれの区間によって、多項式のパラメータは変わるので、

x_j< x <x_j+1となります。

上記の式の通り、一つの区間に対して、4つの未知パラメータ(a,b,c,d)があるので、

データ数をNとすると、4Nの未知パラメータを決定することで補間が可能になります。

 

一般的に、上記の4Nの未知パラメータを推定するために、

下記の5つの条件を用います。

これらの5つの条件が、3次スプライン曲線を特徴付けています。

 

条件1

区間の端点の拘束条件です。

それぞれの区間の多項式はxを入力として、yを出力としています。 

条件2

こちらは各区間の境界の連続性条件です。

各区間の境界は必ず繋がるように連続性が決められます。

条件3

こちらは、各区間の境界の一次微分の連続性条件です。

これにより、各区間の境界は傾きまでを含めて同じように連結されます。

条件4

上記は二次微分の連続性条件です。

これにより、傾きの変化率まで含めて、

滑らかに連結されるため、

非常に滑らかに補間をすることができます。

条件5

こちらの条件は、始点と終点の二次微分の境界条件です。

この始点と終点の境界条件には、いくつか流派があり、

上記のように、曲線の始点と終点の二次微分が0である境界条件を

使うものをNatural Cubic Spline(自然3次スプライン)と呼ぶようです。

しかし、この方式を利用する場合が多いため、

この方式を単に3次スプラインと呼んでいる場合も多い気がします。

 

3次スプライン補間を手計算+pythonで解く

上記の3次スプライン補間のパラメータを求める方法はいくつかあるのですが、

まず初めに3次スプライン補間の理解を深めるため、

下記の記事を参考にして

手計算とpythonで解いてみようと思います。

 

上記の記事の通り、

4Nのパラメータを解くには、4N個の方程式が必要です。

まず、区間一つに対して、両端はかならずサンプリング点を通ることから、

それぞれの区間に対して2つの方程式を立てることができ、

その結果、2Nの方程式を立てることができます。

 

続いて、上記の条件3と条件4により、

2つの区間を挟むサンプリング点の分だけ、

それぞれ方程式を立てることができるので、

2N-2の方程式を立てることができます。

(始点と終点は方程式を立てることができません。)

 

最後に条件5により、始点と終点で一つづつ方程式を立てられるので、

すべてを合わせると4Nの方程式を作ることができ、

これでパラメータを計算することができます。

 

それぞれの方程式を先程の記事のように、

線形方程式にし、あとは逆行列と行列演算で解くだけです。

下記がそのコードになります。

 

import matplotlib.pyplot as plt
import numpy as np

def CubicFunc(x,p):
    y=p[0]*(x**3)+p[1]*(x**2)+p[2]*x+p[3]
    return y

#input
x=[1,2,3,4]
y=[2.7,6,5,6.5]

A=np.array([[1,1,1,1, 0,0,0,0,0,0,0,0],
            [8,4,2,1, 0,0,0,0,0,0,0,0],
            [0,0,0,0, 8,4,2,1,0,0,0,0],
            [0,0,0,0,27,9,3,1,0,0,0,0],
            [0,0,0,0,0,0,0,0,27,9,3,1],
            [0,0,0,0,0,0,0,0,64,16,4,1],
            [12,4,1,0,-12,-4,-1,0,0,0,0,0],
            [0,0,0,0,27,6,1,0,-27,-6,-1,0],
            [12,2,0,0,-12,-2,0,0,0,0,0,0],
            [0,0,0,0,18,2,0,0,-18,-2,0,0],
            [6,2,0,0,0,0,0,0,0,0,0,0],
            [0,0,0,0,0,0,0,0,24,2,0,0],
            ])

b=np.array([2.7,6,6,5,5,6.5,0,0,0,0,0,0])

xs=np.linalg.solve(A,b)

fx,fy=[],[]
for i in range(3):
    xt=np.arange(x[i],x[i+1]+0.1,0.1)
    p=xs[4*i:4*(i+1)]
    yt=[CubicFunc(xi,p) for xi in xt]
    fx.extend(xt)
    fy.extend(yt)

plt.plot(x,y,"xb")
plt.plot(fx,fy,"-r")
plt.grid(True)
plt.axis("equal")
plt.show()

上記のコードでxsというベクトルが、

求めたいパラメータベクトルであり、

その後のコードでそのパラメータを元に補間データを復元しています。

上記のコードを実行すると下記のようなグラフが表示されます。

 

ちゃんと滑らかに補間できていることが確認できました。

 

入力データ数が不定な場合の3次Spline補間

前述のような、手計算の方法だと、

入力データの数が変わると行列のサイズなどが変更になり、

コードを修正しないといけないので、

ツールとしてはあまり使い勝手がよくありません。

 

そこで、媒介変数を使った

下記の記事の手法を使うことで、

入力データ数が可変でも

同じコードで3次スプライン補間をすることができます。

 

上記の手法をPythonとC++で実装してみました。

 

Pythonサンプルコード

Pythonでのコードはpycubicsplineという名前で、

下記で公開しています。

github.com

 

基本的には一つのファイルで完結し、

numpyのみに依存しているため、

こちらのファイルをダウンロードして、importすれば、

簡単に利用できると思います。

 

上記のコードのREADMEのサンプルコードを実行すると、

下記のグラフを得られます。

きちんと補間できていることがわかりました。

  

C++サンプルコード

続いて、

上記のPythonコードをC++に移植しました。

下記が3次元スプライン補間用のヘッダライブラリです。

/**
 *  @brief Cubic Spline header library
 *
 *  @author Atsushi Sakai
 *
 **/

#include <vector>
#include <math.h>

using namespace std;

/**
 *  @brief Cubic Spline header library
 *
 *  @usage 
 *    vector<double> sy{2.7,6,5,6.5};
 *    CppCubicSpline cppCubicSpline(sy);
 *    vector<double> rx;
 *    vector<double> ry;
 *    for(double i=0.0;i<=3.2;i+=0.1){
 *       rx.push_back(i);
 *       ry.push_back(cppCubicSpline.Calc(i));
 *    }
 */
class CppCubicSpline{
  public:
    CppCubicSpline(const vector<double> &y){

      InitParameter(y);

    }

    double Calc(double t){
        int j=int(floor(t));
        if(j<0){
            j=0;
        }
        else if(j>=a_.size()){
            j=(a_.size()-1);
        }

        double dt=t-j;
        double result=a_[j]+(b_[j]+(c_[j]+d_[j]*dt)*dt)*dt;
        return result;
    }

  private:
    vector<double> a_;
    vector<double> b_;
    vector<double> c_;
    vector<double> d_;
    vector<double> w_;

    void InitParameter(const vector<double> &y){
      int ndata=y.size()-1;

      for(int i=0;i<=ndata;i++){
        a_.push_back(y[i]);
      }

      for(int i=0;i<ndata;i++){
        if(i==0){
          c_.push_back(0.0);
        }
        else if(i==ndata){
          c_.push_back(0.0);
        }
        else{
          c_.push_back(3.0*(a_[i-1]-2.0*a_[i]+a_[i+1]));
        }
      }

      for(int i=0;i<ndata;i++){
        if(i==0){
          w_.push_back(0.0);
        }
        else{
          double tmp=4.0-w_[i-1];
          c_[i]=(c_[i]-c_[i-1])/tmp;
          w_.push_back(1.0/tmp);
        }
      }

      for(int i=(ndata-1);i>0;i--){
        c_[i]=c_[i]-c_[i+1]*w_[i];
      }

      for(int i=0;i<=ndata;i++){
        if(i==ndata){
          d_.push_back(0.0);
          b_.push_back(0.0);
        }
        else{
          d_.push_back((c_[i+1]-c_[i])/3.0);
          b_.push_back(a_[i+1]-a_[i]-c_[i]-d_[i]);
        }
      }

    }

};

 

このクラスを使って、

こんな感じでスプライン補間をすることができます。

 

#include<iostream>
#include<vector>
#include"CppCubicSpline.h"
#include"matplotlibcpp.h"

namespace plt=matplotlibcpp;

using namespace std;

int main(void){
  cout<<"cpp spline sample"<<endl;
  vector<double> sx{0,1,2,3};
  vector<double> sy{2.7,6,5,6.5};

  CppCubicSpline cppCubicSpline(sy);
  vector<double> rx;
  vector<double> ry;
  for(double i=0.0;i<=3.2;i+=0.1){
    rx.push_back(i);
    ry.push_back(cppCubicSpline.Calc(i));
  }

  plt::named_plot("Truth",sx,sy, "xb");
  plt::named_plot("interporation",rx,ry, "-r");
  plt::legend();
  plt::axis("equal");
  plt::grid(true);
  plt::show();
}

上記のコードを実行すると、

下記のようなグラフが得られます。

また、c++コードのグラフ作成には、

下記のツールを使いました。

myenigma.hatenablog.com

 

3次スプラインにおける曲率の計算方法

下記のWikipediaの記事にある通り、

ある方程式f(x)の各xにおける曲率は、

下記の式で計算することができます。

 

3次スプラインは3次多項式ですので、

先程の式における一次微分と二次微分は下記のようになります。

 

あとは上記の式を先程の曲率の式に入れれば、

各点における曲率が計算できます。

 

ためしに、

先程の3次スプライン補間のデータの曲率を計算してみました。

上記のライブラリのcalc_curvature関数を使うことで、

各点の曲率を計算できます。

 

下記が、その曲率の計算結果です。

上記の補間結果を比べると、

ちゃんとカーブのきつい部分の曲率が大きくなっていることがわかります。

また曲率も3次スプラインの特徴通り、

滑らかに繋がっていることがわかります。

 

x-y座標系における点群のスプライン補間

二次元の平面上の点群をスプライン補間して、

滑らかな補間を実施したい場合、

スプライン補間の入力を各入力点間の距離にし、

x座標とy座標をそれぞれスプライン補間することで、

なめらかなコースを作ることができます。

 

pycubicsplineのSpline2Dというクラスを使うことで、

上記のように簡単に、

二次元のx-y点列を補間した3次スプラインを生成することができます。

 

方位の計算方法

幾何学的関係より、各点の方位θは下記の式で計算できます。

xとyの微分値は前述の式より計算できるので、

各点のおける方位は、pycubicsplineのcalc_yaw関数を使います。

github.com

 

下記は直線と円弧で構成されるコースにおいて、

方位の推定を実施したものです。

真値に近い方位が、

スプライン補間の結果から計算できていることがわかります。

 

曲率の計算方法

このコースにおける曲率を計算したい場合は、

下記の二次元曲線の曲率の定義と、

xとyのそれぞれの1次微分値と2次微分値(前述の式参照)を使うことで、

計算できます。

(元の曲率が不連続な部分で振動してしまっていますが)

 

曲率を計算するには、pycubicsplineのcalc_curvature関数を使います。

github.com

  

参考資料

myenigma.hatenablog.com

myenigma.hatenablog.com

myenigma.hatenablog.com

myenigma.hatenablog.com

myenigma.hatenablog.com

 

MyEnigma Supporters

もしこの記事が参考になり、

ブログをサポートしたいと思われた方は、

こちらからよろしくお願いします。

myenigma.hatenablog.com