机器人轨迹规划——五次多项式插值轨迹

• 点到点轨迹（Point To Point Trajectory）
  • 三次多项式插值轨迹
  • 五次多项式插值轨迹
  • 梯形速度曲线轨迹
  • 双S形速度曲线轨迹

---

五次多项式

表达式

位置：

$$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\text{\hspace{1em}(1)}$$

求一阶导数可以得到速度：
$$\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\text{\hspace{1em}(2)}$$

求二阶导数可以得到加速度：

$$\ddot{q}(t)=2k_2+6k_3(t-t_0)+12k_4(t-t_0{)}^2+20k_5(t-t_0{)}^3\text{\hspace{1em}(3)}$$

特点：

• 加速度连续
• 可以定义起始和结束的位置、速度、加速度

计算
给定起始和结束的位置、速度、加速度以及轨迹时间$T$，可以构造六个方程，解出五次多项式的6个系数。
满足的约束方程为：
$$\begin{gathered} q(t_0)=q_0,q(t_1)=q_1 \\ \dot{q}(t_0)=v_0,\dot{q}(t_1)=v_1 \\ \ddot{q}(t_0)=a_0,\ddot{q}(t_1)=a_1\text{\hspace{1em}(4)} \end{gathered}$$

把方程（4）结合（1）、（2）、（3）可以得到6个线性方程组，最终可以得到如下解：
$$\begin{array}{rl}& 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{array}\text{\hspace{1em}(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()

举例

$$\begin{gathered} 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 \end{gathered}$$

这段轨迹是从速度、加速度为0开始，再到速度、加速度为0停止，称作rest-to-rest，其轨迹形状如下：

总结

五次多项式是一种经常使用的插值方式，它可以保证加速度连续，可以定义轨迹两个端点的速度、加速度，有比较好的平滑性。