機器人軌迹規劃—五次多項式插值軌迹

機器人軌迹

這一系列軌迹教程将主要包括以下内容：

• 點到點軌迹（P2P）
  • 五次多項式插值軌迹
  • 三次多項式插值軌迹
  • 多項式軌迹實戰
  • 梯形速度曲線軌迹
  • 雙S形速度曲線軌迹
  • 多個自由度軌迹的時間同步
• 線上軌迹規劃
  • 多項式線上軌迹規劃
  • 梯形線上軌迹規劃
  • 雙S形線上軌迹規劃
  • 非線性實時軌迹濾波
• 多點軌迹（Multi-point)
  • 三次樣條曲線（cubic spline）
  • 貝賽爾曲線（Bezier Curve）
  • B樣條曲線（BSpline）
• 時間最優軌迹
  • 三次樣條時間最優軌迹
  • 任意路徑下的時間最優軌迹
  • 時間最優停止軌迹
    • 不限制路徑
    • 有路徑限制

這一篇文章講五次多項式插值軌迹。

文章目錄

• • 五次多項式
  • • 表達式
    • 代碼實作
    • 舉例
    • 總結

五次多項式

表達式

位置：

q ( t ) = k 0 + k 1 ( t − t 0 ) + k 2 ( t − t 0 ) 2 + k 3 ( t − t 0 ) t 3 + k 4 ( t − t 0 ) 4 + k 5 ( t − t 0 ) 5 (1) q(t)=k_0+k_1(t-t_0)+k_2(t-t_0)^2+k_3(t-t_0)t^3+k_4(t-t_0)^4+k_5(t-t_0)^5 \tag{1} q(t)=k0​+k1​(t−t0​)+k2​(t−t0​)2+k3​(t−t0​)t3+k4​(t−t0​)4+k5​(t−t0​)5(1)

求一階導數可以得到速度：

q ˙ ( t ) = k 1 + 2 k 2 ( t − t 0 ) + 3 k 3 ( t − t 0 ) 2 + 4 k 4 ( t − t 0 ) 3 + 5 k 5 ( t − t 0 ) 4 (2) \dot q(t)=k_1+2k_2(t-t_0)+3k_3(t-t_0)^2+4k_4(t-t_0)^3+5k_5(t-t_0)^4 \tag{2} q˙​(t)=k1​+2k2​(t−t0​)+3k3​(t−t0​)2+4k4​(t−t0​)3+5k5​(t−t0​)4(2)

求二階導數可以得到加速度：

q ¨ ( t ) = 2 k 2 + 6 k 3 ( t − t 0 ) + 12 k 4 ( t − t 0 ) 2 + 20 k 5 ( t − t 0 ) 3 (3) \ddot q(t)=2k_2+6k_3(t-t_0)+12k_4(t-t_0)^2+20k_5(t-t_0)^3 \tag{3} q¨​(t)=2k2​+6k3​(t−t0​)+12k4​(t−t0​)2+20k5​(t−t0​)3(3)

特點：

• 加速度連續
• 可以定義起始和結束的位置、速度、加速度

計算

給定起始和結束的位置、速度、加速度以及軌迹時間 T T T，可以構造六個方程，解出五次多項式的6個系數。

滿足的限制方程為：

q ( t 0 ) = q 0 , q ( t 1 ) = q 1 q ˙ ( t 0 ) = v 0 , q ˙ ( t 1 ) = v 1 q ¨ ( t 0 ) = a 0 , q ¨ ( t 1 ) = a 1 (4) q(t_0) = q_0 , \qquad q(t_1) = q_1 \\\dot q(t_0) = v_0, \qquad\dot q(t_1) = v_1 \\\ddot q(t_0) = a_0, \qquad\ddot q(t_1) = a_1 \tag{4} q(t0​)=q0​,q(t1​)=q1​q˙​(t0​)=v0​,q˙​(t1​)=v1​q¨​(t0​)=a0​,q¨​(t1​)=a1​(4)

把方程（4）結合（1）、（2）、（3）可以得到6個線性方程組，最終可以得到如下解：

T = t 1 − t 0 h = q 1 − q 0 k 0 = q 0 k 1 = v 0 k 2 = a 0 2 k 3 = 1 2 T 3 [ 20 h − ( 8 v 1 + 12 v 0 ) T − ( 3 a 0 − a 1 ) T 2 ] k 4 = 1 2 T 4 [ − 30 h + ( 14 v 1 + 16 v 0 ) T + ( 3 a 0 − 2 a 1 ) T 2 ] k 5 = 1 2 T 5 [ 12 h − 6 ( v 1 + v 0 ) T + ( a 1 − a 0 ) T 2 ] (5) \begin{aligned} &T = t_1 - t_0 \\ &h = q_1 - q_0 \\ &k_0 = q_0 \\ &k_1 = v0 \\ &k_2 = \frac{a_0}{2} \\ &k_3 = \frac {1}{2T^3} [20h − (8v_1 + 12v_0)T − (3a_0 − a_1)T^2] \\ &k_4 = \frac {1}{2T^4} [−30h + (14v_1 + 16v_0)T + (3a_0 − 2a_1)T^2] \\ &k_5 = \frac{1}{2T^5} [12h − 6(v_1 + v_0)T + (a_1 − a_0)T^2] \end{aligned} \tag{5} ​T=t1​−t0​h=q1​−q0​k0​=q0​k1​=v0k2​=2a0​​k3​=2T31​[20h−(8v1​+12v0​)T−(3a0​−a1​)T2]k4​=2T41​[−30h+(14v1​+16v0​)T+(3a0​−2a1​)T2]k5​=2T51​[12h−6(v1​+v0​)T+(a1​−a0​)T2]​(5)

代碼實作

這裡用python來實作五次多項式插值：

#!/usr/bin/python3
"""
Copyright © 2021 boldyoungster. All rights reserved.

@file Polynomial.py
@date: 11:06:12, February 28, 2021
"""

import numpy as np
import matplotlib.pyplot as plt


class PolynomialQuintic:
    def __init__(self, t0, t1, q0, q1, v0=0.0, v1=0.0, a0=0.0, a1=0.0):
        coeffs = self.__ComputeQuinticCoeffs(t0, t1, q0, q1, v0, v1, a0, a1)
        self.poly = np.poly1d(coeffs[::-1])

    @classmethod
    def __ComputeQuinticCoeffs(cls, t0, t1, q0, q1, v0, v1, a0, a1):
        T = t1 - t0
        T2 = T * T
        h = q1 - q0
        k0 = q0
        k1 = v0
        k2 = 0.5 * a0
        k3 = (20. * h - (8. * v1 + 12. * v0) * T -
              (3 * a0 - a1) * T2) / (2. * T * T2)
        k4 = (-30. * h + (14*v1 + 16*v0)*T+(3*a0 - 2*a1)*T2) / (2. * T2 * T2)
        k5 = (12 * h - 6*(v1 + v0) * T + (a1 - a0) * T2) / (2 * T2 * T2 * T)
        return (k0, k1, k2, k3, k4, k5)


if __name__ == "__main__":
    t0, t1 = 0, 3.0
    q0, q1 = 0, 1
    v0, v1 = 0, 0
    a0, a1 = 0, 0
    poly = PolynomialQuintic(t0, t1, q0, q1,
                             v0, v1, a0, a1)
    ts = np.linspace(t0, t1, 200)
    qs = poly.poly(ts)
    vs = poly.poly.deriv(1)(ts)
    dvs = poly.poly.deriv(2)(ts)
    plt.subplots_adjust(hspace=1)
    plt.suptitle("QuinticPolynomial")
    plt.subplot(311)
    plt.plot(ts, qs)
    plt.title("Position")
    plt.subplot(312)
    plt.plot(ts, vs)
    plt.title("Velocity")
    plt.subplot(313)
    plt.plot(ts, dvs)
    plt.title("Acceleration")
    plt.savefig("./QuinticPolynomial.png")
    plt.show()

           

舉例

t 0 = 0.0 , t 1 = 3.0 q 0 = 0.0 , q 1 = 1.0 v 0 = 0.0 , v 1 = 0.0 a 0 = 0.0 , a 1 = 0.0 t_0 = 0.0, \qquad t_1 = 3.0 \\q_0 = 0.0, \qquad q_1 = 1.0 \\v_0 = 0.0, \qquad v_1 = 0.0 \\a_0 = 0.0, \qquad a_1 = 0.0 t0​=0.0,t1​=3.0q0​=0.0,q1​=1.0v0​=0.0,v1​=0.0a0​=0.0,a1​=0.0

這段軌迹是從速度、加速度為0開始，再到速度、加速度為0停止，稱作rest-to-rest，其軌迹形狀如下：

總結

五次多項式是一種經常使用的插值方式，它可以保證加速度連續，可以定義軌迹兩個端點的速度、加速度，有比較好的平滑性。