スムーズな移動：多項式による軌道の制御

そしてもう一つ… スムーズな移動：多項式による軌道の制御 2026-08-03 ロボット / 3dprint / python / 数学 この記事は移動についてです。乗り物、ロボット、あるいは3Dプリンターのようなものの移動についてです。3Dプリンターは、複数の方向へ動かしながら溶けたプラスチックを噴出できるホットグルーガンを備えた特殊なロボットと見なすことができます。位置、速度、加速度… 私たちのロボット、あるいはプリントヘッドは、空間内の位置を持ち、また速度（どれだけ速く動いているか）と加速度（その速度がどれだけ急速に変化しているか）も持ちます。速度は位置の導関数です：位置がどれだけ速く変化しているか。加速度は速度の導関数です：速度がどれだけ急速に増加または減少しているか。これらはすべてベクトルであり、大きさも方向も持ちますが、（多くの）3Dプリンターや機械では各軸が独立した機構であるため、この記事では一時的にスカラー、つまり正または負の数として扱います。… ジャーク（Jerk）？ ジャークは加速度の導関数であり、かなり抽象的なものに思えるかもしれません。しかし、一定の加速度下にある箱の中の質量を想像してみてください。箱の中のスプリングが質量を押し、質量も加速させます。しかし、加速度の大きさや方向を変えると、質量は新しい平衡状態に達するまで滑り回ります。次に、その箱があなたの頭蓋骨であり、質量があなたの脳であると想像してください。ジャークは現実です！ 高次の微分には、Snap、Crackle、Popと呼ばれることもある高次の導関数もあります。Snap（Jounceとも呼ばれる）はジャークの変化率です。CrackleはSnapの変化率です、など。これらが物理的に何を意味するのかについての明確な例はありませんが、それら、そしておそらくさらに高次の導関数も、機構の振動などに影響を与えるという共通認識があるようで、それらも最小化されるべきです。不連続性私たちの3Dプリンターや同様の機械は、多くのセグメントからなるパスに従います。一部のセグメントは固定された位置と速度を持ちます：プリンターが押し出しを行っている間、またはCNCミルが切削を行っている間、それは設定された方向へ設定された速度で進みます。移動の他の部分はより自由です：プリンターは、実用的な限り速く、次の固定セグメントの開始位置に到達するだけでよいのです1。問題は、セグメント間の滑らかな遷移です。速度の不連続性は大きな加速度を必要とします。加速度の不連続性は大きなジャークを必要とします。そしてその逆も然りです。セグメントをパスに結合する際、これらの不連続性を防ぐために端を一致させることが重要です。したがって、私たちの見かけ上「自由な」移動セグメントは、実際には「作業」セグメントへの、そしてからの滑らかな遷移をサポートするために重要です。遷移と軌道それでは、遷移を滑らかにする方法を見てみましょう。私たちが探しているのは、適切な一般的な形状を持つ何らかのシグモイド関数です。最も明白なシグモイド関数はロジスティック関数ですが、これは確かに0から1の間を滑らかに遷移し、明確に定義された導関数を持ちますが、残念ながら0と1に収束するだけで、遷移を有限時間で完了させたいという私たちの要望には合いません。しかし、多項式を含む他の適切な関数もあります。SmoothStep… SmoothStepは多項式関数のファミリーであり、ドメイン[0,1]で範囲[0,1]をシグモイド形状で滑らかに遷移します。$ S_1(t) = egin{cases}0, & if extless extgreater 0 \ 3t^2 - 2t^3, & if 0 extless extgreater t extless extgreater 1 \ 1, & if t extgreater extgreater 1 ext{endcases} $ Smoothstepは多項式関数なので、最初の導関数 $ S'_1 $ も多項式です：$ S'_1(t) = egin{cases}0, & if t extless extgreater 0 \ -6t^2 + 6t, & if 0 extless extgreater t extless extgreater 1 \ 0, & if t extgreater extgreater 1 ext{endcases} $ そして、両端できれいにゼロになるという便利な特性を持っています。しかし、$ S''_1 $ はこの特性を持ちません。各端での私たちの速度はゼロですが、加速度はゼロではありません。… そしてSmootherStep… 軌道の開始と終了で加速度がゼロであることを望むなら、Smootherstepを使用できます。これはこの特性を持つ5次の多項式です：$ S_2(t) = egin{cases}0, & if t extless extgreater 0 \ 6t^5 - 15t^4 +10t^3, & 0 extless extgreater t extless extgreater 1 \ 1, & 1 extless extgreater t ext{endcases} $ $ S'_2(t) = egin{cases}0, & if t extless extgreater 0 \ 30t^4 - 60t^3 + 30t^2, & 0 extless extgreater t extless extgreater 1 \ 0, & 1 extless extgreater t ext{endcases} $ $ S''_2(t) = egin{cases}0, & if t extless extgreater 0 \ 120t^3 - 180t^2 + 60t, & 0 extless extgreater t extless extgreater 1 \ 0, & 1 extless extgreater t ext{endcases} $ … そしてSmoothnStep SmoothStep関数は任意の深さまで計算できます。例えば、$ S_6 $ は13次の多項式です：$ S_6(t) = egin{cases}0, & t extless extgreater 0 \ 924t^{13} - 6006t^{12} + 16380t^{11} - 24024t^{10} + 20020t^9 - 9009t^8 + 1716t^7, & 0 extless extgreater t extless extgreater 1 \ 1, & 1 extless extgreater t ext{endcases} $ $ n $ 次のSmoothstep関数では、 $ n $ 次までのすべての導関数はゼロで開始および終了します：$ S^{(m)}_n(0) = S^{(m)}_n(1) = 0 ext{ where } 1 extless extgreater m extless extgreater n $ 次のいくつかの例はすべてSmootherstep $ S_2 $ を使用します。これは少し扱いにくいですが、後で $ S_6 $ に戻ります。ロジスティック関数Smoothstepの次数が高くなるほど、ロジスティック関数に似てきます。ただし、テーパーの端は無限にテーパーオフするのではなく、正確に0と1で終了します。これは、実際に移動を完了させたい私たちにとって便利です。このグラフは、最初のいくつかのSmoothstep関数と、スケーリングされたロジスティック関数 $ rac{1}{1 + e^{6-12x}} $ を比較しています：Pythonの例これらの数式をPythonでいじりたい場合は、numpy.polynomialライブラリが非常に便利です。Polynomialオブジェクトは係数から構築でき、derivメソッドを使用して微分できます：>>> from numpy.polynomial import Polynomial >>> S_6 = Polynomial([0,0,0,0,0,0,0,1716,-9009,20020,-24024,16380,-6006,924], symbol='t') >>> print(S_6) 0.0 + 0.0·t + 0.0·t² + 0.0·t³ + 0.0·t⁴ + 0.0·t⁵ + 0.0·t⁶ + 1716.0·t⁷ - 9009.0·t⁸ + 20020.0·t⁹ - 24024.0·t¹⁰ + 16380.0·t¹¹ - 6006.0·t¹² + 924.0·t¹³ >>> jerk = S_6.deriv(3) >>> print(jerk) 0.0 + 0.0·t + 0.0·t² + 0.0·t³ + 360360.0·t⁴ - 3027024.0·t⁵ + 10090080.0·t⁶ - 17297280.0·t⁷ + 16216200.0·t⁸ - 7927920.0·t⁹ + 1585584.0·t¹⁰ >>> jerk(0) np.float64(0.0) >>> jerk(1) np.float64(0.0) そこからここへそれでは、SmootherStepを使用して軌道を計算してみましょう。$ S_2 $ の計算はWikipediaでかなりよく説明されていますが、ここでは $ x $ を位置として使用し、$ x(t) $ を時間とともに変化する位置として使用しているため、$ t $ を自由変数として使用していることに注意してください。まず、変数係数 $ a_0, a_1, a_2, a_3, a_4, a_5 $ を持つ5次の多項式を設定し、その1次および2次導関数を考えます：$ S_2(t) = a_5 t^5 + a_4 t^4 + a_3 t^3 + a_2 x^2 + a_1 x + a_0 $ $ S'_2(t) = 5 a_5 t^4 + 4 a_4 t^3 + 3 a_3 t^2 + 2 a_2 t + a_1 $ $ S''_2(t) = 20 a_5 t^3 + 12 a_4 t^2 + 6 a_3 t + 2 a_2 $ また、$ S_2 $ などについて期待されるいくつかの値を知っています：位置0から開始し、位置1に到達します：$ S_2(0) = 0 ; S_2(1) = 1 $ ゼロ速度で開始および終了します：$ S'_2(0) = 0 ; S'_2(1) = 0 $ ゼロ加速度で開始および終了します：$ S''_2(0) = 0 ; S''_2(1) = 0 $ これで、線形代数を使用して、目的の $ S_2 $, $ S'_2 $, $ S''_2 $ の開始値と終了値に対応する係数 $ a_n $ を計算できます。$ egin{bmatrix}0 & 0 & 0 & 0 & 0 & 1 \ 1 & 1 & 1 & 1 & 1 & 1 \ 0 & 0 & 0 & 0 & 1 & 0 \ 5 & 4 & 3 & 2 & 1 & 0 \ 0 & 0 & 0 & 2 & 0 & 0 \ 20 & 12 & 6 & 2 & 0 & 0 ext{endmatrix} $ $ egin{bmatrix} a_5 \ a_4 \ a_3 \ a_2 \ a_1 \ a_0 ext{endmatrix} $ = $ egin{bmatrix} S_2(0) \ S_2(1) \ S'_2(0) \ S'_2(1) \ S''_2(0) \ S''_2(1) ext{endmatrix} $ = $ egin{bmatrix} 0 \ 1 \ 0 \ 0 \ 0 \ 0 ext{endmatrix} $ これを解くと、次のようになります：$ egin{bmatrix}a_5 \ a_4 \ a_3 \ a_2 \ a_1 \ a_0 ext{endmatrix} $ = $ egin{bmatrix}6 \ -15 \ 10 \ 0 \ 0 \ 0 ext{endmatrix} $ したがって、私たちの多項式は次のようになります：$ S_2(t) = 6 t^5 - 15 t^4 + 10 t^3 $ 予想通りです。この関数は、静止状態（$ x(0) = 0 ; x'(0) = 0 ; x''(0) = 0 $）から静止状態（$ x(1) = 1 ; x'(1) = 0 ; x''(1) = 0 $）への滑らかな遷移を作成します。