精华内容
下载资源
问答
  • 六轴机器人轨迹规划三次多项式轨迹插值

    万次阅读 多人点赞 2018-02-09 14:55:09
    1.轨迹规划的定义 轨迹规划(trajectory planning)是运动规划(motion planning)研究的主要内容。运动规划指的是运动插补,在起始点和终止点之间插入中间点序列,实现沿着轨迹的平稳运动。运动控制包含路径规划(path ...

    1.轨迹规划的定义
    轨迹规划(trajectory planning)是运动规划(motion planning)研究的主要内容。运动规划指的是运动插补,在起始点和终止点之间插入中间点序列,实现沿着轨迹的平稳运动。运动控制包含路径规划(path planning)和轨迹规划,路径规划是规划位置,在起终点之间经过的路径点,轨迹规划是规划时间,将路径点与时间相对应。
    对于我们的六轴机器人而言轨迹规划可以分为:关节空间轨迹规划和笛卡尔空间轨迹规划。关节空间轨迹规划是把机器人的关节变量变换成跟时间的函数,然后对角速度和角加速度进行约束。笛卡尔空间轨迹规划是把机器人末端在笛卡尔空间的位移、速度和加速度变换成跟时间的函数关系。
    2.数学基础
    三次多项式插值(适用于起点和终点速度为零的情况,约束关节在起点和终点的角度值,规定轨迹两端点位置角速度为定值)。
    设关节角满足下式

    {θ(t)=a0+a1t+a2t2+a3t3θ˙(t)=a1+2a2t+3a3t2θ¨(t)=2a2+6a3t

    将相邻两个点看作一小段轨迹的起点和终点分别用θ0θf表示,约束起始速度为v0,终止速度为vf
    {θ(t0)=θ0θ(tf)=θfθ˙(t0)=v0θ˙(tf)=vf

    将约束条件代入函数,可以求得系数(为简便计算,设t0=0
    {a0=θ0a1=v0a2=3tf2(θfθ0)1tf(2v0+vf)a3=2tf3(θ0θf)+1tf2(v0+vf)

    三次多项式规划轨迹如下
    {θ(t)=θ0+v0t+[3tf2(θfθ0)1tf(2v0+vf)]t2+[2tf3(θ0θf)+1tf2(v0+vf)]t3θ˙(t)=v0+2[3tf2(θfθ0)1tf(2v0+vf)]t+3[2tf3(θ0θf)+1tf2(v0+vf)]t2θ¨(t)=2[3tf2(θfθ0)1tf(2v0+vf)]+6[2tf3(θ0θf)+1tf2(v0+vf)]t

    当速度为零时适用于一段轨迹的起终点,不为零时适用于一段轨迹的经过点。
    3.matlab代码实现

    序号 位置 速度 时间
    1 0 0 0
    2 100 0 3
    clear;
    clc;
    q0=0;
    q1=100; %指定起止位置
    t0=0;
    t1=3;%指定起止时间
    v0=0;
    v1=0;%指定起止速度
    a0=q0;
    a1=v0;
    a2=(3/(t1)^2)*(q1-q0)-(1/t1)*(2*v0+v1);
    a3=(2/(t1)^3)*(q0-q1)+(1/t1^2)*(v0+v1);%计算三次多项式系数
    t=t0:0.01:t1;
    q=a0+a1*t+a2*t.^2+a3*t.^3;%三次多项式插值的位置
    v=a1+2*a2*t+3*a3*t.^2;%三次多项式插值的速度
    a=2*a2+6*a3*t;%三次多项式插值的加速度
    subplot(3,1,1),plot(t,q),xlabel('t'),ylabel('position');grid on;
    subplot(3,1,2),plot(t,v),xlabel('t'),ylabel('velocity');grid on;
    subplot(3,1,3),plot(t,a),xlabel('t'),ylabel('accelerate');grid on;

    下图为插值结果
    这里写图片描述
    4.含经过点的插值

    序号 位置 速度 时间
    1 0 0 0
    2 50 10 3
    3 150 20 6
    4 100 -15 12
    5 0 0 14
    clear;
    clc;
    q_array=[0,50,150,100,0];%指定起止位置
    t_array=[0,2,4,8,10];%指定起止时间
    v_array=[0,10,20,-15,0];%指定起止速度
    t=[t_array(1)];q=[q_array(1)];v=[v_array(1)];a=[0];%初始状态
    for i=1:1:length(q_array)-1;%每一段规划的时间
         a0=q_array(i);
         a1=v_array(i);
         a2=(3/(t_array(i+1)-t_array(i))^2)*(q_array(i+1)-q_array(i))-(1/(t_array(i+1)-t_array(i)))*(2*v_array(i)+v_array(i+1));
         a3=(2/(t_array(i+1)-t_array(i))^3)*(q_array(i)-q_array(i+1))+(1/(t_array(i+1)-t_array(i))^2)*(v_array(i)+v_array(i+1));%计算三次多项式系数 
         ti=t_array(i)+0.001:0.001:t_array(i+1);
         qi=a0+a1*(ti-t_array(i))+a2*(ti-t_array(i)).^2+a3*(ti-t_array(i)).^3;
         vi=a1+2*a2*(ti-t_array(i))+3*a3*(ti-t_array(i)).^2;
         ai=2*a2+6*a3*(ti-t_array(i));
         t=[t,ti];q=[q,qi];v=[v,vi];a=[a,ai];
    end
    subplot(3,1,1),plot(t,q,'r'),xlabel('t'),ylabel('position');grid on;
    subplot(3,1,2),plot(t,v,'b'),xlabel('t'),ylabel('velocity');grid on;
    subplot(3,1,3),plot(t,a,'g'),xlabel('t'),ylabel('accelerate');grid on;

    这里写图片描述
    PS:这种三次多项式插值,只要给定离散点位置、速度和时间,就能插补出一段连续平滑的曲线。但是角加速度并不连续,下次为大家介绍高阶多项式插值则可以解决这个问题。

    展开全文
  • 三次多项式插值轨迹 梯形速度曲线轨迹 双S形速度曲线轨迹 多个自由度轨迹的时间同步 在线轨迹规划 多项式在线轨迹规划 梯形在线轨迹规划 双S形在线轨迹规划 非线性实时轨迹滤波 多点轨迹(Multi-point) 三次...

    机器人轨迹

    这一系列轨迹教程将主要包括以下内容:

    三次多项式

    三次多项式表达式

    位置:
    q(t)=k0+k1(tt0)+k2(tt0)2+k3(tt0)t3(1) q(t)=k_0+k_1(t-t_0)+k_2(t-t_0)^2+k_3(t-t_0)t^3 \tag{1}

    求一阶导数可以得到速度:
    q˙(t)=k1+2k2(tt0)+3k3(tt0)2(2) \dot q(t)=k_1+2k_2(t-t_0)+3k_3(t-t_0)^2 \tag{2}

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

    q¨(t)=2k2+6k3(tt0)(3) \ddot q(t)=2k_2+6k_3(t-t_0) \tag{3}

    特点:

    • 区间内加速度连续,但是因为不能定义两端的加速度,相邻区间会存在加速度突变
    • 可以定义起始和结束的位置、速度

    计算
    给定起始和结束的位置、速度以及轨迹时间,可以构造4个方程,解出3次多项式的4个系数。
    满足的约束方程为:
    q(t0)=q0,q(t1)=q1q˙(t0)=v0,q˙(t1)=v1(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 \tag{4}

    把方程(4)结合(1)、(2)、(3)可以得到6个线性方程组,最终可以得到如下解:
    T=t1t0h=q1q0k0=q0k1=v0k2=3h(2v0+v1)TT2k3=2h+(v0+v1)TT3(5) \begin{aligned} &T = t_1 - t_0 \\ &h = q_1 - q_0 \\ &k_0 = q_0 \\ &k_1 = v0 \\ &k_2 = \frac {3h − (2v_0 + v_1)T}{T^2} \\ &k_3 = \frac {−2h + (v_0 + v_1)T}{T^3} \\\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 PolynomialCubic:
        def __init__(self, t0, t1, q0, q1, v0=0.0, v1=0.0):
            coeffs = self.__ComputeQuinticCoeffs(t0, t1, q0, q1, v0, v1)
            self.poly = np.poly1d(coeffs[::-1])
    
        def __call__(self, ts, deriv_order=0):
            return self.poly.deriv(deriv_order)(ts)
    
        @classmethod
        def __ComputeQuinticCoeffs(cls, t0, t1, q0, q1, v0, v1):
            T = t1 - t0
            T2 = T * T
            h = q1 - q0
            k0 = q0
            k1 = v0
            k2 = (3*h-(2*v0+v1)*T) / T2
            k3 = (-2*h+(v0+v1)*T) / (T2*T)
            return (k0, k1, k2, k3)
    
    
    def PlotSpline(poly, t0, t1, title):
        ts = np.linspace(t0, t1, 200)
        qs = poly(ts, 0)
        vs = poly(ts, 1)
        dvs = poly(ts, 2)
        plt.subplots_adjust(hspace=1)
        plt.suptitle(title)
        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(f"./{title}.png")
        plt.show()
    
    
    def TestCubicPolynomial(t0, t1, q0, q1, v0, v1):
        poly = PolynomialCubic(t0, t1, q0, q1,
                               v0, v1)
        title = "CubicPolynomial"
        PlotSpline(poly, t0, t1, title)
    
    
    if __name__ == "__main__":
        t0, t1 = 0, 3.0
        q0, q1 = 0, 1
        v0, v1 = 0, 0
        a0, a1 = 0, 0
        TestCubicPolynomial(t0, t1, q0, q1, v0, v1)
    
    
    

    举例

    t0=0.0,t1=3.0q0=0.0,q1=1.0v0=0.0,v1=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

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

    三次多项式插值轨迹

    总结

    三次多项式是一种经常使用的插值方式,它可以保证加速度连续,可以定义轨迹两个端点的速度,有比较好的平滑性。计算比五次多项式要更简单,缺点是不能定义起始和结束的加速度。

    展开全文
  • 三次多项式插值轨迹 五次多项式插值轨迹 多项式轨迹实战 梯形速度曲线轨迹 双S形速度曲线轨迹 多个自由度轨迹的时间同步 在线轨迹规划 多项式在线轨迹规划 梯形在线轨迹规划 双S形在线轨迹规划 非线性实时轨迹...

    机器人轨迹

    这一系列轨迹教程将主要包括以下内容:

    • 点到点轨迹(P2P)

    • 在线轨迹规划

      • 多项式在线轨迹规划
      • 梯形在线轨迹规划
      • 双S形在线轨迹规划
      • 非线性实时轨迹滤波
    • 多点轨迹(Multi-point)

      • 三次样条曲线(cubic spline)
      • 贝赛尔曲线(Bezier Curve)
      • B样条曲线(BSpline)
    • 时间最优轨迹

      • 三次样条时间最优轨迹
      • 任意路径下的时间最优轨迹
      • 时间最优停止轨迹
        • 不约束路径
        • 有路径约束

      [TOC]

    多项式轨迹实战

    概述

    前面讲过了三次多项式插值轨迹五次多项式插值轨迹,他们具有相对来说比较简单的表达式:
    q(t)=k0+k1t+k2t2+k3t3+(1) q(t) = k_0 + k_1t + k_2t^2+k_3t^3+\cdots \tag{1}
    具体用几次的表达式,和想要施加的约束有关系,约束的个数和多项式系数的个数相同。三次多项式可以保证速度连续性,五次多项式可以保证加速度连续性,七次多项式可以保证加加速度(Jerk)连续性。

    实现

    前面几节分别给出了一个python的实现版本,在实际使用中,对轨迹计算速度要求较高,python可能难以满足,以下介绍用c++来实现一个从一次线性插值(linear)到七次插值(Septic)的多项式算法。支持的插值算法如下:

    • Linear:线性插值,两个约束,给定两端的位置
    • Quadratic:二次插值,三个约束,给定起点位置,终点的位置和速度
    • CubicFirst:三次插值,四个约束,给定两端的位置和速度
    • CubicSecond:三次插值,四个约束,给定两端的位置和加速度
    • QuarticFirstSecond:四次插值,五个约束,给定起点的位置、速度,终点的位置、速度、加速度
    • QuinticFirstSecond:五次插值,六个约束,给定两端的位置、速度、加速度
    • SepticFirstSecondThird:七次插值,八个约束,给定两端的位置、速度、加速度、加加速度

    每个算法返回一个Polynomial对象,具有以下接口:

    • Eval(t, n):计算多项式在t点的n阶导数值,如果n是0的话,就是计算当前多项式的值
    • GetAbsoluteMaximum:计算多项式在定义域内的最大的绝对值,这个后面做时间最优轨迹优化的时候会用到
    • RealRoots:返回定义域内的实数根
    • Derivative(n):求多项式的n阶导数,返回的还是一个多项式
    • Integrate(n):对多项式进行积分

    代码

    c++

    以下是c++实现的Polynomial

    #pragma once
    #include <Eigen/Eigenvalues>
    #include <algorithm>
    #include <iostream>
    #include <memory>
    #include <sstream>
    #include <vector>
    
    #include "Function.h"
    
    namespace rz {
    namespace trajectory {
    
    /**
     * A polynomial function.
     * each dof x in [x0, x1]:
     * y_i = f(x)_i = c(0, i) + c(1, i) * (x - x0) + c(2, i) * (x - x0) ^ 2 + ...
     */
    template <typename T = VecXd>
    class Polynomial : public Function<T>, public std::enable_shared_from_this<Polynomial<T>> {
    public:
        // using T = VecXd;
    
        DECLARE_MEMBER_PTR(Polynomial<T>)
    
        Polynomial(unsigned degree, unsigned dof) : Function<T>(0, 0), m_coeffs(degree + 1, dof), m_dof(dof) {}
        Polynomial(const MatXd& coeffs, double x1 = 1) : Function<T>(0, x1), m_coeffs(coeffs), m_dof(coeffs.cols()) {}
    
        virtual ~Polynomial() {}
    
        static Ptr CubicFirst(const T& y0, const T& y1, const T& yd0, const T& yd1, double x1 = 1) {
            auto f = std::make_shared<Polynomial<T>>(3, yd0.size());
            f->m_coeffs.row(0) = y0;
            f->m_coeffs.row(1) = yd0;
            f->m_coeffs.row(2) = -(2 * x1 * yd0 + 3 * y0 - 3 * y1 + x1 * yd1) / std::pow(x1, 2);
            f->m_coeffs.row(3) = (2 * y0 + x1 * yd0 - 2 * y1 + x1 * yd1) / std::pow(x1, 3);
            f->m_x1 = x1;
            return f;
        }
    
        static Ptr CubicSecond(const T& y0, const T& y1, const T& ydd0, const T& ydd1, double x1 = 1) {
            auto f = std::make_shared<Polynomial<T>>(3, ydd0.size());
            f->m_coeffs.row(0) = y0;
            f->m_coeffs.row(1) = -(2 * std::pow(x1, 2) * ydd0 + std::pow(x1, 2) * ydd1 + 6 * y0 - 6 * y1) / x1 / 6;
            f->m_coeffs.row(2) = ydd0 / 2;
            f->m_coeffs.row(3) = -(ydd0 - ydd1) / x1 / 6;
            f->m_x1 = x1;
            return f;
        }
    
        static Ptr Linear(const T& y0, const T& y1, double x1 = 1) {
            auto f = std::make_shared<Polynomial<T>>(1, y0.size());
            f->m_coeffs.row(0) = y0;
            f->m_coeffs.row(1) = (y1 - y0) / x1;
            f->m_x1 = x1;
            return f;
        }
    
        static Ptr Quadratic(const T& y0, const T& y1, const T& yd0, double x1 = 1) {
            auto f = std::make_shared<Polynomial<T>>(2, yd0.size());
            f->m_coeffs.row(0) = y0;
            f->m_coeffs.row(1) = yd0;
            f->m_coeffs.row(2) = -(x1 * yd0 + y0 - y1) / std::pow(x1, 2);
            f->m_x1 = x1;
            return f;
        }
    
        static Ptr QuarticFirstSecond(const T& y0, const T& y1, const T& yd0, const T& yd1, const T& ydd0, double x1 = 1) {
            auto f = std::make_shared<Polynomial<T>>(4, yd0.size());
            f->m_coeffs.row(0) = y0;
            f->m_coeffs.row(1) = yd0;
            f->m_coeffs.row(2) = ydd0 / 2;
            f->m_coeffs.row(3) = -(std::pow(x1, 2) * ydd0 + 3 * x1 * yd0 + x1 * yd1 + 4 * y0 - 4 * y1) / std::pow(x1, 3);
            f->m_coeffs.row(4) = static_cast<double>(0.5) *
                                 (std::pow(x1, 2) * ydd0 + 4 * x1 * yd0 + 2 * x1 * yd1 + 6 * y0 - 6 * y1) / std::pow(x1, 4);
            f->m_x1 = x1;
            return f;
        }
    
        static Ptr QuinticFirstSecond(const T& y0, const T& y1, const T& yd0, const T& yd1, const T& ydd0, const T& ydd1,
                                      double x1 = 1) {
            auto f = std::make_shared<Polynomial<T>>(5, yd0.size());
            f->m_coeffs.row(0) = y0;
            f->m_coeffs.row(1) = yd0;
            f->m_coeffs.row(2) = ydd0 / 2;
            f->m_coeffs.row(3) =
                -(3 * std::pow(x1, 2) * ydd0 + 12 * x1 * yd0 - std::pow(x1, 2) * ydd1 + 20 * y0 + 8 * x1 * yd1 - 20 * y1) /
                (2 * std::pow(x1, 3));
            f->m_coeffs.row(4) = (16 * x1 * yd0 - 2 * std::pow(x1, 2) * ydd1 + 30 * y0 + 14 * x1 * yd1 - 30 * y1 +
                                  3 * std::pow(x1, 2) * ydd0) /
                                 (2 * std::pow(x1, 4));
            f->m_coeffs.row(5) =
                -(12 * y0 + 6 * x1 * yd0 + 6 * x1 * yd1 - 12 * y1 + std::pow(x1, 2) * ydd0 - std::pow(x1, 2) * ydd1) /
                (2 * std::pow(x1, 5));
            f->m_x1 = x1;
    
            return f;
        }
    
        static Ptr SepticFirstSecondThird(const T& y0, const T& y1, const T& yd0, const T& yd1, const T& ydd0,
                                          const T& ydd1, const T& yddd0, const T& yddd1, double x1 = 1) {
            auto f = std::make_shared<Polynomial<T>>(7, yd0.size());
    
            f->m_coeffs.row(0) = y0;
            f->m_coeffs.row(1) = yd0;
            f->m_coeffs.row(2) = ydd0 / 2;
            f->m_coeffs.row(3) = yddd0 / 6;
            f->m_coeffs.row(4) = -(std::pow(x1, 3) * (yddd1 + 4 * yddd0) + std::pow(x1, 2) * (30 * ydd0 - 15 * ydd1) +
                                   x1 * (90 * yd1 + 120 * yd0) - 210 * y1 + 210 * y0) /
                                 (6 * std::pow(x1, 4));
            f->m_coeffs.row(5) = (std::pow(x1, 3) * (yddd1 + 2 * yddd0) + std::pow(x1, 2) * (20 * ydd0 - 14 * ydd1) +
                                  x1 * (78 * yd1 + 90 * yd0) - 168 * y1 + 168 * y0) /
                                 (2 * std::pow(x1, 5));
            f->m_coeffs.row(6) = -(std::pow(x1, 3) * (3 * yddd1 + 4 * yddd0) + std::pow(x1, 2) * (45 * ydd0 - 39 * ydd1) +
                                   x1 * (204 * yd1 + 216 * yd0) - 420 * y1 + 420 * y0) /
                                 (6 * std::pow(x1, 6));
            f->m_coeffs.row(7) = (std::pow(x1, 3) * (yddd1 + yddd0) + std::pow(x1, 2) * (12 * ydd0 - 12 * ydd1) +
                                  x1 * (60 * yd1 + 60 * yd0) - 120 * y1 + 120 * y0) /
                                 (6 * std::pow(x1, 7));
            f->m_x1 = x1;
    
            return f;
        }
    
        static Ptr SexticFirstSecondThird(const T& y0, const T& y1, const T& yd0, const T& yd1, const T& ydd0,
                                          const T& ydd1, const T& yddd0, double x1 = 1) {
            auto f = std::make_shared<Polynomial<T>>(6, yd0.size());
    
            f->m_coeffs.row(0) = y0;
            f->m_coeffs.row(1) = yd0;
            f->m_coeffs.row(2) = ydd0 / 2;
            f->m_coeffs.row(3) = yddd0 / 6;
            f->m_coeffs.row(4) = -(std::pow(x1, 3) * yddd0 + std::pow(x1, 2) * (6 * ydd0 - ydd1) +
                                   x1 * (10 * yd1 + 20 * yd0) - 30 * y1 + 30 * y0) /
                                 (2 * std::pow(x1, 4));
            f->m_coeffs.row(5) = (std::pow(x1, 3) * yddd0 + std::pow(x1, 2) * (8 * ydd0 - 2 * ydd1) +
                                  x1 * (18 * yd1 + 30 * yd0) - 48 * y1 + 48 * y0) /
                                 (2 * std::pow(x1, 5));
            f->m_coeffs.row(6) = -(std::pow(x1, 3) * yddd0 + std::pow(x1, 2) * (9 * ydd0 - 3 * ydd1) +
                                   x1 * (24 * yd1 + 36 * yd0) - 60 * y1 + 60 * y0) /
                                 (6 * std::pow(x1, 6));
            f->m_x1 = x1;
    
            return f;
        }
    
        Ptr Derivative(unsigned deriv_order = 1) const {
            if (deriv_order <= 0) return std::const_pointer_cast<Polynomial<T>>(this->shared_from_this());
            Ptr f = std::make_shared<Polynomial<T>>(this->Degree() > 0 ? this->Degree() - 1 : 0, this->m_dof);
            f->m_x0 = this->m_x0;
            f->m_x1 = this->m_x1;
            if (0 == this->Degree()) {
                f->m_coeffs.row(0) = T::Zero(this->m_dof);
            } else {
                for (unsigned i = 0; i < this->Degree(); ++i) {
                    f->m_coeffs.row(i) = static_cast<double>(i + 1) * this->m_coeffs.row(i + 1);
                }
            }
            return deriv_order <= 1 ? f : f->Derivative(deriv_order - 1);
        }
    
        T GetAbsoluteMaximum() const {
            Ptr derivative = this->Derivative();
    
            T maximum = this->Eval(this->m_x0);
            T y1 = this->Eval(this->m_x1);
            unsigned degree = derivative->Degree();
            std::vector<T> roots_of_all_dof = this->RealRoots();
            for (unsigned k = 0; k < this->m_dof; ++k) {
                maximum[k] = std::max(maximum[k], y1[k]);
                const T& roots = roots_of_all_dof[k];
                for (unsigned i = 0; i < roots.size(); ++i) {
                    if (this->m_x0 < roots[i] && roots[i] < this->m_x1) {
                        double y = std::abs((*this)(roots[i])[k]);
                        if (y > maximum[k]) {
                            maximum[k] = y;
                        }
                    }
                }
            }
    
            return maximum;
        }
    
        T operator()(double x, unsigned deriv_order = 0) const {
            if (deriv_order > 0) {
                return (*this->Derivative(deriv_order))(x, 0);
            } else {
                x = std::clamp(x, this->m_x0, this->m_x1);
                x -= this->m_x0;
                unsigned degree = this->Degree();
                T y = this->m_coeffs.row(degree);
                for (unsigned i = 1; i < degree + 1; ++i) {
                    y *= x;
                    y += this->m_coeffs.row(degree - i);
                }
                return y;
            }
        }
        std::vector<T> operator()(const std::vector<double> xs, unsigned deriv_order = 0) const {
            if (deriv_order > 0) {
                return (*this->Derivative(deriv_order))(xs, 0);
            } else {
                std::vector<T> ys;
                for (auto x : xs) {
                    x = std::clamp(x, this->m_x0, this->m_x1);
                    x -= this->m_x0;
                    unsigned degree = this->Degree();
                    T y = this->m_coeffs.row(degree);
                    for (unsigned i = 1; i < degree + 1; ++i) {
                        y *= x;
                        y += this->m_coeffs.row(degree - i);
                    }
                    ys.push_back(y);
                }
                return ys;
            }
        }
    
        std::vector<T> RealRoots() const {
            std::vector<T> roots_of_all_dof;
    
            for (unsigned k = 0; k < this->m_dof; ++k) {
                T roots_of_one_dof(0);
                auto c = this->m_coeffs.col(k);
                unsigned degree = this->Degree();
                for (unsigned i = 0; i < c.size(); ++i) {
                    if (std::abs(c[c.size() - 1 - i]) <= std::numeric_limits<double>::epsilon()) {
                        degree = degree > 0 ? degree - 1 : degree;
                    } else {
                        break;
                    }
                }
                switch (degree) {
                    case 0: {
                        roots_of_one_dof.resize(0);
                    } break;
                    case 1: {
                        roots_of_one_dof.resize(1);
                        roots_of_one_dof[0] = -c[0] / c[1];
                    } break;
                    case 2: {
                        double det = c[1] * c[1] - 4 * c[0] * c[2];
                        if (det > 0) {
                            roots_of_one_dof.resize(2);
                            double sqrt_det = std::sqrt(det);
                            roots_of_one_dof[0] = (-c[1] + sqrt_det) / (2 * c[2]);
                            roots_of_one_dof[1] = (-c[1] - sqrt_det) / (2 * c[2]);
                        } else if (std::abs(det) <= std::numeric_limits<double>::epsilon()) {
                            roots_of_one_dof.resize(1);
                            roots_of_one_dof[0] = -c[1] / (2 * c[2]);
                        }
                    } break;
                    case 3: {
                        double p = (3 * c[3] * c[1] - std::pow(c[2], 2)) / (3 * std::pow(c[3], 2));
                        double q = (2 * std::pow(c[2], 3) - 9 * c[3] * c[2] * c[1] + 27 * std::pow(c[3], 2) * c[0]) /
                                   (27 * std::pow(c[3], 3));
    
                        double back = c[2] / (3 * c[3]);
    
                        if (std::abs(p) <= std::numeric_limits<double>::epsilon() &&
                            std::abs(q) <= std::numeric_limits<double>::epsilon()) {
                            roots_of_one_dof.resize(1);
                            roots_of_one_dof[0] = 0 - back;
                        } else if (std::abs(p) <= std::numeric_limits<double>::epsilon() &&
                                   std::abs(q) > std::numeric_limits<double>::epsilon()) {
                            roots_of_one_dof.resize(1);
                            roots_of_one_dof[0] = std::cbrt(-q) - back;
                        } else if (std::abs(p) > std::numeric_limits<double>::epsilon() &&
                                   std::abs(q) <= std::numeric_limits<double>::epsilon()) {
                            double sqrt_p = std::sqrt(-p);
                            roots_of_one_dof.resize(3);
                            roots_of_one_dof[0] = 0 - back;
                            roots_of_one_dof[1] = sqrt_p - back;
                            roots_of_one_dof[2] = -sqrt_p - back;
                        } else if (std::abs(4 * std::pow(p, 3) + 27 * std::pow(q, 2)) <=
                                       std::numeric_limits<double>::epsilon() &&
                                   std::abs(p) > std::numeric_limits<double>::epsilon()) {
                            roots_of_one_dof.resize(2);
                            roots_of_one_dof[0] = -(3 * q) / (2 * p) - back;
                            roots_of_one_dof[1] = (3 * q) / p - back;
                        } else {
                            double det = std::pow(q, 2) / 4 + std::pow(p, 3) / 27;
                            if (det < 0) {
                                double tmp1 = 2 * std::sqrt(-p / 3);
                                double tmp2 = std::acos((3 * q) / (2 * p) * std::sqrt(-3 / p)) / 3;
                                roots_of_one_dof.resize(3);
                                roots_of_one_dof[0] = tmp1 * std::cos(tmp2) - back;
                                roots_of_one_dof[1] = tmp1 * std::cos(tmp2 - 2 * M_PI / 3) - back;
                                roots_of_one_dof[2] = tmp1 * std::cos(tmp2 - 4 * M_PI / 3) - back;
                            } else {
                                double sqrt_det = std::sqrt(det);
                                double u = std::cbrt(-q / 2 + sqrt_det);
                                double v = std::cbrt(-q / 2 - sqrt_det);
                                roots_of_one_dof.resize(1);
                                roots_of_one_dof[0] = u + v - back;
                            }
                        }
                    } break;
                    default: {
                        MatXd companion(degree, degree);
                        companion.topLeftCorner(1, degree).setZero();
                        companion.bottomLeftCorner(degree - 1, degree).setIdentity();
                        companion.rightCols(1) = -1 / c[degree] * c.segment(0, degree);
    
                        Eigen::EigenSolver<MatXd> solver(companion);
                        std::vector<double> roots;
    
                        for (unsigned i = 0; i < solver.eigenvalues().size(); ++i) {
                            if (std::abs(solver.eigenvalues()[i].imag()) < Eigen::NumTraits<double>::dummy_precision()) {
                                roots.push_back(solver.eigenvalues()[i].real());
                            }
                        }
                        roots_of_one_dof = T::Map(roots.data(), roots.size());
                    } break;
                }
                if (roots_of_one_dof.size() > 0) {
                    roots_of_all_dof.push_back(roots_of_one_dof);
                }
            }
            return roots_of_all_dof;
        }
    protected:
        MatXd m_coeffs; ///< polynomial coefficients, (degree + 1) x dof
        unsigned m_dof{0};
    };
    } // namespace trajectory
    } // namespace rz
    

    python bindings

    为了能够在python中使用,利用pybind11来进行封装:

    #include <pybind11/eigen.h>
    #include <pybind11/operators.h>
    #include <pybind11/pybind11.h>
    
    #include "Polynomial.h"
    #include "Polynomial1d.h"
    
    namespace rz {
    namespace rzpy {
    using namespace rz::trajectory;
    namespace py = pybind11;
    using namespace py::literals;
    
    template <typename T>
    void BindPolynomial(py::module &m, const char *name) {
        py::class_<Polynomial<T>, std::shared_ptr<Polynomial<T>>> poly(m, name);
        poly.def_static("Linear", &Polynomial<T>::Linear, "y0"_a, "y1"_a, "x1"_a = 1);
        poly.def_static("Quadratic", &Polynomial<T>::Quadratic, "y0"_a, "y1"_a, "yd0"_a, "x1"_a = 1);
        poly.def_static("CubicFirst", &Polynomial<T>::CubicFirst, "y0"_a, "y1"_a, "yd0"_a, "yd1"_a, "x1"_a = 1);
        poly.def_static("CubicSecond", &Polynomial<T>::CubicSecond, "y0"_a, "y1"_a, "ydd0"_a, "ydd1"_a, "x1"_a = 1);
        poly.def_static("QuarticFirstSecond", &Polynomial<T>::QuarticFirstSecond, "y0"_a, "y1"_a, "yd0"_a, "yd1"_a,
                        "ydd0"_a, "x1"_a = 1);
        poly.def_static("QuinticFirstSecond", &Polynomial<T>::QuinticFirstSecond, "y0"_a, "y1"_a, "yd0"_a, "yd1"_a,
                        "ydd0"_a, "ydd1"_a, "x1"_a = 1);
        poly.def_static("SexticFirstSecondThird", &Polynomial<T>::SexticFirstSecondThird, "y0"_a, "y1"_a, "yd0"_a, "yd1"_a,
                        "ydd0"_a, "ydd1"_a, "yddd0"_a, "x1"_a = 1);
        poly.def_static("SepticFirstSecondThird", &Polynomial<T>::SepticFirstSecondThird, "y0"_a, "y1"_a, "yd0"_a, "yd1"_a,
                        "ydd0"_a, "ydd1"_a, "yddd0"_a, "yddd1"_a, "x1"_a = 1);
        poly.def(py::init<const MatXd &, double>(), "coeffs"_a, "x1"_a = 1,
                 "Construct from coeffs [(degree+1) x DoF] and duration");
        poly.def("Eval", &Polynomial<T>::Eval, "t"_a, "deriv_order"_a = 0);
        poly.def("Evals", &Polynomial<T>::Evals, "ts"_a, "deriv_order"_a = 0);
        poly.def("DoF", &Polynomial<T>::DoF);
        poly.def("Degree", &Polynomial<T>::Degree);
        poly.def("Duration", &Polynomial<T>::Duration);
        poly.def("GetAbsoluteMaximum", &Polynomial<T>::GetAbsoluteMaximum);
        poly.def("GetMinimumMaximum", &Polynomial<T>::GetMinimumMaximum);
        poly.def("Coefficients", py::overload_cast<>(&Polynomial<T>::Coefficients, py::const_));
        poly.def("Upper", py::overload_cast<>(&Polynomial<T>::Upper, py::const_));
        poly.def("Lower", py::overload_cast<>(&Polynomial<T>::Lower, py::const_));
        poly.def("Derivative", &Polynomial<T>::Derivative, "deriv_order"_a = 0);
        poly.def("RealRoots", &Polynomial<T>::RealRoots);
        poly.def("ScaledX", &Polynomial<T>::ScaledX, "factor"_a);
        poly.def("TranslatedX", &Polynomial<T>::TranslatedX, "translation"_a);
        poly.def(py::self += T());
        poly.def(py::self -= T());
        poly.def(py::self *= double());
        poly.def(py::self /= double());
        poly.def("__repr__", &Polynomial<T>::Repr);
        poly.def(
            "__call__", [](Polynomial<T> &self, double t, unsigned deriv_order) { return self(t, deriv_order); }, "t"_a,
            "deriv_order"_a = 0);
        poly.def(
            "__call__",
            [](Polynomial<T> &self, const std::vector<double> &ts, unsigned deriv_order) { return self(ts, deriv_order); },
            "ts"_a, "deriv_order"_a = 0);
    }
    
    PYBIND11_MODULE(PyPolynomial, m) {
        BindPolynomial<VecXd>(m, "PolynomialRn");
        BindPolynomial<double>(m, "Polynomial1d");
    }
    
    } // namespace rzpy
    } // namespace rz
    
    

    测试

    from PyPolynomial import *
    import numpy as np
    import matplotlib.pyplot as plt
    from IPython import embed
    
    
    def PlotPolynomial(poly, number_points=100, title="Plot"):
        ts = np.linspace(poly.Lower(), poly.Upper(), number_points)
        qs = np.asarray([poly(t, 0) for t in ts])
        dqs = np.asarray([poly(t, 1) for t in ts])
        ddqs = np.asarray([poly(t, 2) for t in ts])
        dddqs = np.asarray([poly(t, 3) for t in ts])
        shape = 3, 2
        plt.subplots_adjust(hspace=1)
        plt.subplot2grid(shape, (0, 0))
        plt.plot(ts, qs)
        plt.title("q(t)")
        plt.subplot2grid(shape, (1, 0))
        plt.plot(ts, dqs)
        plt.title("$\dot q(t)$")
        plt.subplot2grid(shape, (2, 0))
        plt.plot(ts, ddqs)
        plt.title("$\ddot q(t)$")
        plt.subplot2grid(shape, (0, 1))
        plt.plot(ts, dddqs)
        plt.title("$\dddot q(t)$")
        plt.subplot2grid(shape, (1, 1), rowspan=2)
        if poly.DoF() >= 2:
            plt.plot(*qs.T)
            plt.title("curve")
        plt.suptitle(title)
        plt.show()
    
    
    if __name__ == "__main__":
        dof = 2
        duration = 10
        c = np.array([
            [1., 2.],
            [2., -2.],
            [-2.3, 4.],
        ])
        poly = PolynomialRn(c, duration)
        PlotPolynomial(poly, title="FromCoeffients")
        poly = Polynomial1d(c[:, 0], duration)
        PlotPolynomial(poly, title="FromCoeffients1d")
    
        q0 = np.array([0., 0.])
        q1 = np.array([10., 4.])
        dq0 = np.zeros(dof)
        dq1 = np.zeros(dof)
        ddq0 = np.zeros(dof)
        ddq1 = np.zeros(dof)
        dddq0 = np.zeros(dof)
        dddq1 = np.zeros(dof)
        poly1d = Polynomial1d.CubicFirst(0, 1, 0, 0, duration)
        PlotPolynomial(poly1d, title="1d")
        poly = PolynomialRn.Linear(q0, q1, duration)
        PlotPolynomial(poly, title="Linear")
        poly = PolynomialRn.CubicFirst(q0, q1, dq0, dq1, duration)
        PlotPolynomial(poly, title="Cubic")
        poly = PolynomialRn.QuinticFirstSecond(
            q0, q1, dq0, dq1, ddq0, ddq1, duration)
        PlotPolynomial(poly, title="Quintic")
        poly = PolynomialRn.SepticFirstSecondThird(
            q0, q1, dq0, dq1, ddq0, ddq1, dddq0, dddq1, duration)
        PlotPolynomial(poly, title="Septic")
    
    

    Linar
    cubicquintic

    septic

    总结

    本篇内容是对机器人点到点多项式插值轨迹的一个总结,同时实现了一个比较好的框架,后续的样条轨迹、梯形轨迹、双S曲线轨迹,都是分段的多项式轨迹,所以实现一个高效易用的多项式轨迹对后面的学习和实现都很重要。

    本篇中的代码不是完整显示的,参阅完整的代码

    另外需要注意的是,以上的编译、测试都是在linux系统下完成的,Windows下没有进行测试。编译依赖Eigen,pybind11,linux下使用以下命令安装:

    sudo apt install libeigen3-dev
    sudo apt install pybind11-dev
    

    当然也可以去官网下载最新的eigenpybind11

    展开全文
  • 具有中间点路径的三次多项式轨迹 五次多项式 例子1:简单三次曲线和五次曲线 (a)是三次多项式曲线,已知用户指定初始点和终止点的位置和速度及整个运动时间,则根据公式(7-1)~(7-5)进行计算,代码及...

    学习代码都记录在个人github上,欢迎关注~

    三次多项式

    在这里插入图片描述在这里插入图片描述在这里插入图片描述

    具有中间点路径的三次多项式轨迹

    在这里插入图片描述在这里插入图片描述在这里插入图片描述在这里插入图片描述在这里插入图片描述

    五次多项式

    在这里插入图片描述在这里插入图片描述

    例子1:简单三次曲线和五次曲线

    在这里插入图片描述
    (a)是三次多项式曲线,已知用户指定初始点和终止点的位置和速度及整个运动时间,则根据公式(7-1)~(7-5)进行计算,代码及运算结果如下

    % 单关节多项式关节空间轨迹
    %% 三阶多项式theta(t) = a0 + a1*t + a2*t^2 + a3*t^3
    % 指定初始和终止点的位置theta_s theta_f,同时速度均为0
    theta_s = 120; theta_f = 60; tf = 1;
    a0_3 = theta_s;
    a1_3 = 0;
    a2_3 = (3/tf^2)*(theta_f - theta_s);
    a3_3 = (-2/tf^3)*(theta_f - theta_s);
    j = 1;
    for t = 0: 0.02: 1
       theta_3(j) = a0_3 + a1_3*t + a2_3*t^2 + a3_3*t^3;
       theta_3d(j) = a1_3 + 2*a2_3*t + 3*a3_3*t^2;
       theta_3dd(j) = 2*a2_3 + 6*a3_3*t;
       theta_3ddd(j) = 6*a3_3;
       j = j + 1;
    end
    figure(1)
    subplot(4, 1, 1)
    plot([0:0.02:1], theta_3)
    grid on
    title('关节角(°)')
    subplot(4, 1, 2)
    plot([0:0.02:1], theta_3d)
    grid on
    title('角速度(°/s)')
    subplot(4, 1, 3)
    plot([0:0.02:1], theta_3dd)
    grid on
    title('角加速度(°/s^2)')
    subplot(4, 1, 4)
    plot([0:0.02:1], theta_3ddd)
    grid on
    title('角加速度变化率')
    hold on
    
    

    在这里插入图片描述
    由上图可以看出,三次曲线在初始时刻和终止时刻时,加速度不连续,会存在冲击,这是三次曲线的缺点。五次曲线可以解决这个问题。

    (b)是五次曲线,此时用户给定初始时刻和终止时刻的位置、速度和加速度,这是该曲线的约束条件,同时给定运动时间。代码及运算结果如下:

    %% 五阶多项式theta(t) = a0 + a1*t + a2*t^2 + a3*t^3 + a4*t^4 + a5*t^5
    % 指定初始和终止点的位置,另外速度和加速度均为0
    theta_s = 120; theta_f = 60; tf = 1;
    theta_fd = 0; theta_fdd = 0; theta_sd = 0; theta_sdd = 0;
    a0_5 = theta_s; a1_5 = theta_sd; a2_5 = theta_sdd / 2;
    a3_5 = (20*theta_f - 20*theta_s - (8*theta_fd + 12*theta_sd)*tf - (3*theta_sdd - theta_fdd)*tf^2) / (2*tf^3);
    a4_5 = (30*theta_s - 30*theta_f + (14*theta_fd + 16*theta_sd)*tf + (3*theta_sdd - 2*theta_fdd)*tf^2) / (2*tf^4);
    a5_5 = (12*theta_f - 12*theta_s - (6*theta_fd + 6*theta_sd)*tf - (theta_sdd - theta_fdd)*tf^2) / (2*tf^5);
    k = 1;
    for t = 0: 0.02: 1
       theta_5(k) = a0_5 + a1_5*t + a2_5*t^2 + a3_5*t^3 + a4_5*t^4 + a5_5*t^5;
       theta_5d(k) = a1_5 + 2*a2_5*t + 3*a3_5*t^2 + 4*a4_5*t^3 + 5*a5_5*t^4;
       theta_5dd(k) = 2*a2_5 + 6*a3_5*t + 12*a4_5*t^2 + 20*a5_5*t^3;
       theta_5ddd(k) = 6*a3_5 + 24*a4_5*t + 60*a5_5*t^2;
       k = k + 1;
    end
    figure(2)
    subplot(4, 1, 1)
    plot([0:0.02:1], theta_5)
    grid on
    title('关节角(°)')
    subplot(4, 1, 2)
    plot([0:0.02:1], theta_5d)
    grid on
    title('角速度(°/s)')
    subplot(4, 1, 3)
    plot([0:0.02:1], theta_5dd)
    grid on
    title('角加速度(°/s^2)')
    subplot(4, 1, 4)
    plot([0:0.02:1], theta_5ddd)
    grid on
    title('角加速度变化率')
    

    在这里插入图片描述
    由上图可知,五次曲线解决了三次曲线在初始时刻和终止时刻加速度不连续的问题。

    (c)是两段带有中间点路径的三次曲线。此时用户给定初始时刻和终止时刻的位置、速度和加速度,给定了中间点的位置,但是没有指定中间时刻的速度,因此在两条三次曲线的连接处,用速度和加速度均连续作为新的约束条件,并根据公式(7-12)~(7-15)进行计算。代码及计算结果如下:

    %% 两段带有中间点的三项多项式(约束条件为连接点的速度和加速度相等)
    % theta(t)_1 = a10 + a11*t1 + a12*t1^2 + a13*t1^3
    % theta(t)_2 = a20 + a21*t2 + a22*t2^2 + a23*t2^3
    theta_s_ = 60; theta_v_ = 120; theta_f_ = 30;
    t = 1; tf = 2;
    theta_s_d_ = 0; theta_s_dd_ = 0; 
    theta_f_d_ = 0; theta_f_dd_ = 0;
    a10 = theta_s_; a11 = 0;
    a12 = (12*theta_v_ - 3*theta_f_ - 9*theta_s_) / (4*t^2);
    a13 = (-8*theta_v_ + 3*theta_f_ + 5*theta_s_) / (4*t^3);
    a20 = theta_v_; 
    a21 = (3*theta_f_ - 3*theta_s_) / (4*t);
    a22 = (-12*theta_v_ + 6*theta_f_ + 6*theta_s_) / (4*t^2);
    a23 = (8*theta_v_ - 5*theta_f_ - 3*theta_s_) / (4*t^3);
    s = 1;
    for T = 0: 0.02: 1
        theta_1(s) = a10 + a11*T + a12*T^2 + a13*T^3;
        theta_d_1(s) = a11 + 2*a12*T + 3*a13*T^2;
        theta_dd_1(s) = 2*a12 + 6*a13*T;
        theta_ddd_1(s) = 6*a13;
        s = s + 1;
    end
    s = 1;
    for T = 0: 0.02: 1
        theta_2(s) = a20 + a21*T + a22*T^2 + a23*T^3;
        theta_d_2(s) = a21 + 2*a22*T + 3*a23*T^2;
        theta_dd_2(s) = 2*a22 + 6*a23*T;
        theta_ddd_2(s) = 6*a23;
        s = s + 1;
    end
    % 去掉首尾
    theta_ = [theta_1, theta_2(2: 51)];
    theta_d_ = [theta_d_1, theta_d_2(2: 51)];
    theta_dd_ = [theta_dd_1, theta_dd_2(2: 51)];
    theta_ddd_ = [theta_ddd_1, theta_ddd_2(2: 51)];
    figure(3)
    subplot(4, 1, 1)
    plot([0:0.02:2], theta_)
    grid on
    title('关节角(°)')
    subplot(4, 1, 2)
    plot([0:0.02:2], theta_d_)
    grid on
    title('角速度(°/s)')
    subplot(4, 1, 3)
    plot([0:0.02:2], theta_dd_)
    grid on
    title('角加速度(°/s^2)')
    subplot(4, 1, 4)
    plot([0:0.02:2], theta_ddd_)
    grid on
    title('角加速度变化率')
    

    在这里插入图片描述
    由上图可知,三次曲线本身的问题仍然存在。

    例子2:多段带有中间点的三次多项式

    用户指定初始点、终止点、多个中间点的时刻以及各点对应的位置、速度
    指定各点的时间及其对应的位置、速度:
    t0 = 0, t1 = 2, t2 = 4, t3 = 8, t4 = 10
    p0 = 10, p1 = 20, p2 = 0, p3 = 30, p4 = 40
    v0 = 0, v1 = -10, v2 = 10, v3 = 3, v4 = 0
    此时用户指定了中间点的速度,因此可以根据公式(7-9)~(7-11)确定各项系数,并得到轨迹。代码及运算结果如下:

    %% 多段带有中间点的三次多项式
    % 指定各点的时间及其对应的位置、速度
    % t0 = 0, t1 = 2, t2 = 4, t3 = 8, t4 = 10
    % p0 = 10, p1 = 20, p2 = 0, p3 = 30, p4 = 40
    % v0 = 0, v1 = -10, v2 = 10, v3 = 3, v4 = 0
    t = [0, 2, 4, 8, 10];
    p = [10, 20, 0, 30, 40];
    v = [0, -10, 10, 3, 0];
    % 初始化
    T = t(1); P = p(1); V = v(1);
    for i = 1:4
        % 四个三阶多项式的各项参数
        a0(i) = p(i);
        a1(i) = v(i);
        Tf = t(i+1) - t(i);
        a2(i) = (3/Tf^2)*(p(i+1) - p(i)) - (2/Tf)*v(i) - (1/Tf)*v(i+1);
        a3(i) = (-2/Tf^3)*(p(i+1) - p(i)) + (1/Tf^2)*(v(i+1) + v(i));
        % 时间均分100份,并累加时间横坐标
        N = linspace(t(i), t(i+1), 100);
        T = [T, N(2: 100)];% 累加时间坐标
        % 计算各多项式
        pk = a0(i) + a1(i)*(N - N(1)) + a2(i)*power(N - N(1), 2) + a3(i)*power(N - N(1), 3);
        vk = a1(i) + 2*a2(i)*(N - N(1)) + 3*a3(i)*power(N - N(1), 2);
        qk = 2*a2(i) + 6*a3(i)*(N - N(1));
        P = [P, pk(2: 100)];
        V = [V, vk(2: 100)];
        if (i == 1)
            Q = 2*a2(i);
        end
        Q = [Q, qk(2: 100)];
    end
    figure(4)
    subplot(3, 1, 1);
    plot(T, P, 'r')
    grid on
    hold on
    plot(t, p, 'or')
    title('关节角(°)')
    subplot(3, 1, 2)
    plot(T, V, 'b')
    grid on
    hold on
    plot(t, v, 'ob')
    title('角速度(°/s)')
    subplot(3, 1, 3)
    plot(T, Q, 'g')
    grid on
    title('角加速度(°/s^2)')
    

    在这里插入图片描述
    由上图可知,三次曲线中各时间点的加速度不连续,存在冲击,同时速度波动太大,这可能是中间点速度选取不合理造成、下面利用五次多项式,提出一种简单的解决办法。

    例子3:多段带中间点的五次多项式,同时中间速度平滑处理(和三次多项式进行比较)

    用户指定初始点、终止点、多个中间点的时刻以及各点对应的位置、速度
    指定各点的时间及其对应的位置、速度。使用五次多项式,还需要指定各点的加速度:
    t0 = 0, t1 = 2, t2 = 4, t3 = 8, t4 = 10
    p0 = 10, p1 = 20, p2 = 0, p3 = 30, p4 = 40
    v0 = 0, v1 = -10, v2 = 10, v3 = 3, v4 = 0
    a0 = 0, a1 = 0, a2 = 0, a3 = 0, a4 = 0
    针对例子2中速度选取不得当导致波动较大的问题,本例采用如下办法。一般情况下不会指定中间点的速度,只指定起点和终点的速度,这时候就可以使用下面方法规划轨迹。有时候定义轨迹时,指定的中间点的速度不合理,会导致速度曲线波动过大,这是时候如果不要求中间位置的速度都必须与指定相等,也可以使用下面的规划方式。
    在这里插入图片描述
    代码及运算结果如下:

    %% 多段带中间点的五次多项式,同时中间速度平滑处理(和三次多项式进行比较)
    % 该方法由于对中间点的速度进行平滑处理,因此中间点速度不一定是期望速度,适用于对中间点速度无要求的情况
    % 指定各点的时间及其对应的位置、速度
    % t0 = 0, t1 = 2, t2 = 4, t3 = 8, t4 = 10
    % p0 = 10, p1 = 20, p2 = 0, p3 = 30, p4 = 40
    % v0 = 0, v1 = -10, v2 = 10, v3 = 3, v4 = 0
    % 初始、中间点及终止点的加速度均为0
    t = [0, 2, 4, 8, 10];
    p = [10, 20, 0, 30, 40];
    v = [0, -10, 10, 3, 0];
    a = [0, 0, 0, 0, 0];
    % 初始化
    T = t(1); P_ = p(1); V_ = v(1); A_ = a(1);
    % 处理中间速度
    for k = 1: 4
        if (k == 1)
            v2(k) = v(k);
        else
            dk1 = (p(k) - p(k-1)) / (t(k) - t(k-1));
            dk2 = (p(k+1) - p(k)) / (t(k+1) - t(k));
            if (dk1 >= 0 && dk2 >= 0 || dk1 <= 0 && dk2 <= 0)
                v2(k) = (1/2)*(dk1 + dk2);
            else
                v2(k) = 0;
            end
        end
    end
    v2(5) = v(5);
    for i = 1: 4
        % 计算四个五次多项式的各项系数
        tf = t(i+1) - t(i);
        a0(i) = p(i); 
        a1(i) = v2(i); 
        a2(i) = a(i) / 2;
        a3(i) = (20*p(i+1) - 20*p(i) - (8*v2(i+1) + 12*v2(i))*tf - (3*a(i) - a(i+1))*tf^2) / (2*tf^3);
        a4(i) = (30*p(i) - 30*p(i+1) + (14*v2(i+1) + 16*v2(i))*tf + (3*a(i) - 2*a(i+1))*tf^2) / (2*tf^4);
        a5(i) = (12*p(i+1) - 12*p(i) - (6*v2(i+1) + 6*v2(i))*tf - (a(i) - a(i+1))*tf^2) / (2*tf^5);
        % 时间均分100份,并累加时间横坐标
        N = linspace(t(i), t(i+1), 100);
        T = [T, N(2: 100)]; % 累加时间坐标
        % 计算各多项式theta(t) = a0 + a1*t + a2*t^2 + a3*t^3 + a4*t^4 + a5*t^5
        % position
        pk = a0(i) + a1(i)*(N - N(1)) + a2(i)*power(N - N(1), 2) + a3(i)*power(N - N(1), 3) + a4(i)*power(N - N(1), 4) + a5(i)*power(N - N(1), 5);
        % velocity
        vk = a1(i) + 2*a2(i)*(N - N(1)) + 3*a3(i)*power(N - N(1), 2) + 4*a4(i)*power(N - N(1), 3) + 5*a5(i)*power(N - N(1), 4);
        % acceleration
        ak = 2*a2(i) + 6*a3(i)*(N - N(1)) + 12*a4(i)*power(N - N(1), 2) + 20*a5(i)*power(N - N(1), 3);
        P_ = [P_, pk(2: 100)];
        V_ = [V_, vk(2: 100)];
        A_ = [A_, ak(2: 100)];
    end
    figure(5)
    subplot(3, 1, 1);
    plot(T, P_, 'r')
    grid on
    hold on
    plot(T, P, 'r--')
    plot(t, p, 'or')
    title('关节角(°)')
    subplot(3, 1, 2)
    plot(T, V_, 'b')
    grid on
    hold on
    plot(T, V, 'b--')
    plot(t, v2, 'ob')
    title('角速度(°/s)')
    subplot(3, 1, 3)
    plot(T, A_, 'g')
    hold on
    plot(T, Q, 'g--')
    grid on
    title('角加速度(°/s^2)')
    

    与带有多个中间点的三次多项式轨迹进行比较,图中实线为本例方法,虚线为三次多项式方法。
    在这里插入图片描述
    改进之后,可以发现速度变化更为平滑,各时间点的加速度冲击得到改善。

    附加

    关于拐点对称的抛物线轨迹

    在这里插入图片描述在这里插入图片描述

    %% 关于拐点对称的抛物线轨迹
    % 已知起始和结束时刻的位置和速度
    t0 = 0; t = 8; T = t - t0;
    tf = T / 2; % 拐点
    v0 = 0; v1 = 0;
    theta_s = 0; theta = 10; theta_f = (theta_s + theta) / 2;
    h = theta - theta_s;
    % theta_a(t) = a10 + a11*(t - t0) + a12*(t - t0)^2 加速段
    a10 = theta_s; a11 = v0; a12 = (2/T^2)*(h - v0*T);
    % matlab中这样计算是很方便的,但是c里面无法直接合并向量,得借助循环遍历
    Ta = linspace(t0, tf, 400);
    theta_a = a10 + a11*(Ta - t0) + a12*power(Ta - t0, 2);
    theta_ad = a11 + 2*a12*(Ta - t0);
    theta_add = 2*a12;
    % theta_b(t) = a20 + a21*(t - tf) + a22*(t - tf)^2 减速段
    Tb = linspace(tf, t, 400);
    Tn = [Ta, Tb(2: 400)];
    a20 = theta_f; a21 = 2*(h/T) - v1; a22 = (2/T^2)*(v1*T - h); 
    theta_b = a20 + a21*(Tb - tf) + a22*power(Tb - tf, 2);
    theta_bd = a21 + 2*a22*(Tb - tf);
    theta_bdd = 2*a22;
    theta = [theta_a, theta_b(2: 400)];
    theta_d = [theta_ad, theta_bd(2: 400)];
    figure(1)
    subplot(3, 1, 1)
    plot(Tn, theta, 'r')
    ylabel('position')
    hold on
    plot(tf, theta_f, 'or')
    grid on
    subplot(3, 1, 2)
    plot(Tn, theta_d, 'b')
    ylabel('velocity')
    hold on
    plot(tf, theta_d(400), 'ob')
    grid on
    for i = 1: 799
        theta_dd(i) = theta_add;
        if (i > 400)
            theta_dd(i) = theta_bdd;
        end
    end
    subplot(3, 1, 3)
    plot(Tn, theta_dd, 'g')
    ylabel('acceleration')
    hold on
    plot(tf, theta_dd(400), 'og')
    grid on
    

    在这里插入图片描述

    更一般的情况,拐点两边不对称,但是加速度对称恒定

    在这里插入图片描述在这里插入图片描述

    % 已知起始和结束时刻的位置和速度,同时拐点的位置和速度具有连续性
    t0 = 0; t1 = 8; T = t - t0; tf = 4;
    ta = tf - t0; tb = t - tf;
    v0 = 0.1; v1 = -1; 
    q0 = 0; q1 = 10; h = q1 - q0;
    % theta_a(t) = a10 + a11*(t - t0) + a12*(t - t0)^2 加速段
    a10 = q0; a11 = v0; a12 = (2*h - v0*(T + ta) - v1*tb) / (2*T*tb);
    Ta = linspace(t0, tf, (ta/T)*800);
    q_a = a10 + a11*(Ta - t0) + a12*power(Ta - t0, 2);
    q_ad = a11 + 2*a12*(Ta- t0);
    q_add = 2*a12;
    % theta_b(t) = a20 + a21*(t - tf) + a22*(t - tf)^2 减速段
    a20 = (2*q1*ta + tb*(2*q0 + ta*(v0 - v1))) / (2*T);
    a21 = (2*h - v0*ta - v1*tb) / T;
    a22 = -(2*h - v0*ta - v1*(T + tb)) / (2*T*tb);
    Tb = linspace(tf, t1, (tb/T)*800);
    q_b = a20 + a21*(Tb - tf) + a22*power(Tb - tf, 2);
    q_bd = a21 + 2*a22*(Tb - tf);
    q_bdd = 2*a22;
    Tn = [Ta, Tb(2: (tb/T)*800)];
    q = [q_a, q_b(2: (tb/T)*800)];
    q_d = [q_ad, q_bd(2: (tb/T)*800)];
    for i = 1:799
       q_dd(i) = q_add;
       if (i > (ta/T)*800)
           q_dd(i) = q_bdd;
       end
    end
    figure(2)
    subplot(3, 1, 1)
    plot(Tn, q, 'r');
    hold on
    plot(tf, q_a((ta/T)*800), 'or')
    grid on
    ylabel('position')
    subplot(3, 1, 2)
    plot(Tn, q_d, 'b')
    hold on
    plot(tf, q_ad((ta/T)*800), 'ob')
    grid on
    subplot(3, 1, 3)
    plot(Tn, q_dd, 'g')
    grid on
    
    

    在这里插入图片描述

    展开全文
  • 实验3:取-放轨迹规划——7次多项式轨迹的规划 文章目录一、实验目的和要求1.1 目的1.2 要求二、实验手段轨迹规划的推导过程(7次多项式)3.1 边界条件3.2 连续条件3.3 七次多项式推导四、仿真结果与比较分析4.1...
  • 背景知识轨迹规划方法分为两个方面:对于移动机器人偏向于意指移动的路径轨迹规划...轨迹规划常用的多项式方法在轨迹规划中,有一个类别是多项式轨迹规划:抛物线、三次多项式、五次多项式、七次多项式、N次多项式。...
  • 机器人学之运动学笔记【5】—— 机械臂轨迹规划【1】 轨迹:机械臂的末端点或操作点的位置、速度、加速度对时间的历程
  • 并没有告诉机器人应该以怎样的速度、加速度运动,这就需要进行带时间参数的轨迹规划处理,也就是对这条空间轨迹进行速度、加速度约束,并且计算运动到每个路点的时间,高级的算法有TOPP等,一般的呢就是贝塞尔、三次...
  •     这是《机器人技术基础》个人课程实验之一,按照学号尾数不同分配给每人的取放轨迹规划方式也不同,包括3-4-3、4-3-4等轨迹规划方法,而我抽中的是七次多项式实现,不存在优劣之分,特地说明一下。...
  • 三次多项式插值轨迹 梯形速度曲线轨迹 双S形速度曲线轨迹 多个自由度轨迹的时间同步 在线轨迹规划 多项式在线轨迹规划 梯形在线轨迹规划 双S形在线轨迹规划 非线性实时轨迹滤波 多点轨迹(Multi-point) 三次...
  • 2009收稿日期:2008-10-12作者简介:凌家良(1977-),男,江西定南人,中南大学软件工程...21惠州学院电子科学系,广东惠州516015)摘要:首先介绍了工业机器人轨迹规划的概念及涉及到的问题,并改进关节空间的三次多项式插...
  • 、五、七次多项式

    2018-11-09 10:56:44
    机器人运动控制领域,轨迹规划是很关键的一步,决定了机器人运动效率,常用的轨迹规划方法是多项式插值。
  • 3. 工业机器人轨迹规划-关节空间 ...三次多项式为(即关节角随时间变换的方程): θt=c0+c1t+c2t2+c3t3 (θ,为θ的一阶导数,θ,,为θ的二阶导数) 初始和终止条件: θt0=θ0 θt1=θ1 θ,t0=0 θ,t1=0 代
  • 建立机器人动力学模型,通过将机器人运动学约束转化为三次样条插值曲线的控制关键点约束的非线性约束优化问题,利用三次多项式插值拟合曲线,通过粒子群算法和惩罚函数法相结合,采用自适应函数控制插值收敛速度,得到一...
  • 实际应用中,我们也会经常使用到直线轨迹,然而(5)中讲的三次多项式并不能满足我们的直线轨迹。然而如果单纯地使用直线轨迹,线段间的转折点会让速度不连续,如何又能使用直线轨迹又能满足速度连续呢?在这里引出...
  • 上一篇博客所学内容是使用三次多项式轨迹规划,规划出来的曲线都是曲线,在实际的应用当中,会遇到直线轨迹的情况,所以需要掌握能够实现直线轨迹的方法。 轨迹中若包含多个直线段轨迹,线段间转折点速度就会不...
  • 来看这个部分,主要有左右两边的M函数,左边的M函数主要实现的功能是利用矩阵求解三次多项式4个参数。右边的M函数就是将求解出来的参数代入三次多项式,得到规划的轨迹曲线。 可能说的有点快,首先要了解一下轨迹...
  • 基于coin3D搭建的机器人可视化平台,vs2010+mfc+coin3d+eigen,含正逆运动学,两点5次多项式轨迹规划
  • 目前,学者们对关节空间的轨迹规划算法进行了大量研究,较常用的有三次多项式和五次多项式插值法,计算量小且速度连续。马睿等在满足机器人速度和加速度等约束条件下,利用三次多项式函数对机器人各个运动轨迹点进行...

空空如也

空空如也

1 2 3
收藏数 43
精华内容 17
关键字:

机器人三次多项式轨迹规划