轨迹规划——五次多项式曲线拟合

最新推荐文章于 2024-07-26 11:36:13 发布

mhrobot

最新推荐文章于 2024-07-26 11:36:13 发布

阅读量2w

点赞数 6

分类专栏：规划算法数学基础

本文链接：https://blog.csdn.net/qq_36458461/article/details/110007656

版权

本文探讨了如何使用五次多项式进行轨迹规划，以确保加速度连续。通过设置六个边界条件（包括位置、速度和加速度的初始与终止值），求解五次多项式的系数。介绍了从起点(x0, v0, a0)到终点(x1, v1, a1)的计算过程，并阐述了如何确定时间区间[t0, t1]内的系数。同时，提供了代码实现来辅助理解这一数学方法。" 116155427,9711861,Python实现二叉树数据结构与遍历,"['数据结构与算法', '二叉树', 'Python']

摘要由CSDN通过智能技术生成

1.五次多项式求解

为了获得一个加速度连续的轨迹，位置和速度需要合适的初始和终止条件，也需要合适的初始和终止加速度值。这样共有六个边界条件，因此需要采用五次多项式：
函数关系式为：
在这里插入图片描述
注意：v(t)公式错误，系数c1后面没有t

已知起点(x0, v0, a0), 终点(x1, v1, a1),
两点之间，一段轨迹规划的起始时间是t0，结束时间是t1。
一般情况下，起点时刻默认为t0 = 0 ，根据五次多项式及其一阶和二阶导，可求得系数：c0, c1,c2
将求得的c0, c1,c2再带回原式及其一阶和二阶导，可求出 c3, c4, c5

2. 代码实现

import math

import matplotlib.pyplot as plt
import numpy as np

# parameter
MAX_T = 100.0  # maximum time to the goal [s]
MIN_T = 5.0  # minimum time to the goal[s]

show_animation = True


class QuinticPolynomial:

    def __init__(self, xs, vxs, axs, xe, vxe, axe, time):
        # calc coefficient of quintic polynomial
        # See jupyter notebook document for derivation of this equation.
        self.a0 = xs
        self.a1 = vxs
        self.a2 = axs / 2.0

        A = np.array([[time ** 3, time ** 4, time ** 5],
                      [3 * time ** 2, 4 * time ** 3, 5 * time ** 4],
                      [6 * time, 12 * time ** 2, 20 * time ** 3]])
        b = np.array([xe - self.a0 - self.a1 * time - self.a2 * time ** 2,
                      vxe - self.a1 - 2 * self.a2 * time,
                      axe - 2 * self.a2])
        x = np.linalg.solve(A, b)

        self.a3 = x[0]
        self.a4 = x[1]
        self.a5 = x[2]

    def calc_point(self, t):
        xt = self.a0 + self.a1 * t + self.a2 * t ** 2 + \
             self.a3 * t ** 3 + self.a4 * t ** 4 + self.a5 * t ** 5

        return xt

    def calc_first_derivative(self, t):
        xt = self.a1 + 2 *<