3次スプライン補完をやってみた

研究室で複数のデータを滑らかに補完するためにスプライン曲線を生成するプログラムを作成しました。線形補間でもよかったのですが、微妙に直線に乗らなかったので3次スプラインを用いました。せっかくなので実装方法を説明します。

指定点を通る滑らかな曲線を生成する3次スプラインをProcessingで実装してみた pic.twitter.com/NpgtoNK3pj

— だん (@kogakudanshi) 2022年10月27日

3次スプライン補完とは

数学的な解説は合ってるか怪しいので表現などに問題があったらコメントで教えてほしいのですがなんとなくで説明します。

小さい順に並んだ とそれに対応するがあったときの区間で

 

という3次式の係数およびを適切に決めると全ての点をを通る滑らかな曲線を描くことができます。

「滑らかな」というのは1階微分、2階微分が連続であるという意味です。

たくさんある近似曲線の中でも

1. 指定した点を確実に通る
2. 速度と加速度が連続である
3. パラメーターを定めてしまえば計算が簡単

と言った点が特徴かとおもいます。特に1,2は速度、加速度変化がゆるやかなためロボット工学における経路生成などで利点となります。また計算自体はただの3次関数なのでCPU負荷は小さいです。一方で指定した点の数の5倍のパラメーターが必要なのでメモリ的にはあまり優しくないです。

係数を求める

実際の求め方はこちらのサイトがわかりやすかったです。

3次スプライン補間で軌跡生成：３次多項式のパラメータを求める（その１）｜Tajima Robotics

3次スプライン補間で軌跡生成：３次多項式のパラメータを求める（その２）｜Tajima Robotics

端点において2階微分が0となるようにし、各点におけるが連続となる条件からパラメーターを決めていきます。

ポイントとなるのはを求める際に逆行列を用いるところです。との間隔を1に固定して簡略化する方法がよく紹介されているのですが、任意の点を用いたかったので行列から求めることにしました。

上記記事中の係数ベクトル、行列、連立方程式の右辺が

となるので逆行列を求めて

とします。

逆行列を求める

ここは言語によってライブラリがあったりなかったりいろいろだと思います。

数学的には余因子行列を用いるのが一般的なのかもしれませんが、高次になると行列式を求めるだけで一苦労です。

ここでは掃き出し法を使いましたが値が極端に大きい・小さいと問題が出てくるかもしれません。よくわかってませんが個数が多くなるとLU分解が良いらしいです。

float[][] inv(float[][] A){
  int n = A.length;
  float[][] I = new float[n][n];
  for(int i = 0;i < n;i++)
    for(int j = 0;j < n;j++)
          I[j][i] = (i == j ? 1 : 0);

  for(int i = 0; i < n; i++){
    float buf = 1 / A[i][i];
    for(int j = 0;j < n;j++){
      A[i][j] *= buf;
      I[i][j] *= buf;
    }
    for(int j = 0; j < n; j++){
      if(i != j){
        buf = A[j][i];
        for(int k = 0; k < n; k++){
          A[j][k] -= A[i][k] * buf;
          I[j][k] -= I[i][k] * buf;
        }
      }
    }
  }
  return I;
}

 

実装

なぜか実験用、研究用、Processing用にC++→C#→Javaで再々実装する羽目になりました。

点が増えるたびに係数配列を増やさないといけないのですが、それぞれ例えば行列はstd::vector<std::vector<float>>、float[,]、floatで微妙に違うので地味に面倒でした。

以下はProcessingで使ったJava用のソースコードになります。

class Spline{
  float[] x;
  float[] a;
  float[] b;
  float[] c;
  float[] d;
  int n;
  
  Spline(int num,ArrayList<Float> xa,ArrayList<Float> ya){
    n = num;
    x = new float[n];
    a = new float[n];
    b = new float[n];
    c = new float[n];
    d = new float[n];
    float[] h = new float[n - 1];
    
    //copy x
    for(int i = 0;i < n;i++){
      x[i] = xa.get(i);
    }
    
    //calc a
    for(int i = 0;i < n;i++){
      a[i] = ya.get(i);
    }

    //calc h
    for(int i = 0;i < n - 1;i++){
      h[i] = x[i + 1] - x[i];
    }

    //calc c
    float[][] A = new float[n][n];
    A[0][0] = 1;
    A[n - 1][n - 1] = 1;

    for(int i = 1;i < n - 1;i++){
        A[i][i - 1] = h[i - 1];
        A[i][i] = 2 * (h[i - 1] + h[i]);
        A[i][i + 1] = h[i];
    }

    float[] v = new float[n];
    for(int i = 1;i < n - 1;i++){
        v[i] = 3 / h[i] * (a[i + 1] - a[i]) - 3 / h[i - 1] * (a[i] - a[i - 1]);
    }

    float[][] invA = inv(A);

    for(int i = 0;i < n;i++){
      float t = 0;
      for(int j = 0;j < n;j++){
        t += invA[i][j] * v[j];
      }
      c[i] = t;
    }

    //calc b
    for(int i = 0;i < n - 1;i++){
      b[i] = (a[i + 1] - a[i]) / h[i] - h[i] * (c[i + 1] + 2 * c[i]) / 3;
    }
    b[n - 1] = 0;

    //calc d
    for (int i = 0; i < n - 1; i++){
        d[i] = (c[i + 1] - c[i]) / 3 / h[i];
    }
    d[n - 1] = 0;
  }
  
  float f(float t){
    float _x = 0;
    int j = 0;
    for(int i = n - 1;i >= 0;i--){
      if(x[i] <= t){
        _x = x[i];
        j = i;
        break;
      }
    }

    float dx = t - _x;
    return a[j] + (b[j] + (c[j] + d[j] * dx) * dx) * dx;
  }
}

xが昇順に並んでいる必要がある点に注意してください。

経路生成などで二次元にしたい場合は媒介変数をtなどとして2つSplineを生成すれば良いはずです。

というわけで3次スプライン補正のパラメータ導出方法について解説しました。xが比較的均等でもともと滑らかめなデータだときれいに補完できるんですが、そうじゃないデータだと結構飛んだり跳ねたりする印象です。

こういったソースコードを載せる記事を書くのは久しぶりなので問題点等あればコメントで教えていただけると助かります。

ご覧いただきありがとうございました。