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

置顶 boldyoungster

已于 2024-01-09 18:20:24 修改

阅读量2.7w

点赞数 32

分类专栏： Robotics 文章标签： Robotics 机器人学机器人轨迹轨迹规划在线轨迹

于 2021-02-27 12:00:09 首次发布

By Zen. 行行皆辛苦，三连必须有！

本文链接：https://blog.youkuaiyun.com/xiaozisheng2008_/article/details/114167606

版权

Robotics 专栏收录该内容

18 篇文章 ¥19.90 ¥99.00

订阅专栏

超级会员免费看

本文探讨了机器人轨迹规划中的一种方法——五次多项式插值轨迹。通过数学表达式，详细介绍了如何计算五次多项式轨迹的各个参数，并提供了Python代码实现示例。这种轨迹规划方式确保加速度连续，适用于定义起点和终点的速度及加速度，具有良好的平滑特性。

摘要生成于 C知道，由 DeepSeek-R1 满血版支持，前往体验 >

点到点轨迹（Point To Point Trajectory）

五次多项式

表达式

位置：

$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}$

求一阶导数可以得到速度：
$\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}$

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

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

特点：

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

计算
给定起始和结束的位置、速度、加速度以及轨迹时间 $T$ ，可以构造六个方程，解出五次多项式的6个系数。
满足的约束方程为：
$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}$

把方程（4）结合（1）、（2）、（3）可以得到6个线性方程组，最终可以得到如下解：
$\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}$

代码实现

这里用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()