精华内容
下载资源
问答
  • 多项式轨迹--五次多项式轨迹

    千次阅读 2020-07-28 22:37:38
    #多项式轨迹–五次多项式 1.5 Polynomial of degree five 利用三次多项式,根据过q0,q1,…,qnq_0,q_1,\ldots,q_nq0​,q1​,…,qn​确定的轨迹的特征是位置和速度连续,但是加速度不连续(参见上一篇博客轨迹规划–三...

    #多项式轨迹–五次多项式

    1.5 Polynomial of degree five

    利用三次多项式,根据过q0,q1,,qnq_0,q_1,\ldots,q_n确定的轨迹的特征是位置和速度连续,但是加速度不连续(参见上一篇博客轨迹规划–三次多项式轨迹)。尽管三次多项式轨迹确定的轨迹有一定“平滑”,但是对于一些应用的动力学和惯性载荷会产生一些不期待的影响。为了获得一个加速度连续的轨迹,位置和速度需要合适的初始和终止条件,也需要合适的初始和终止加速度值。这样共有六个边界条件,因此需要采用五次多项式:
    q(t)=q0+a1(tt0)+a2(tt0)2+a3(tt0)3+a4(tt0)4+a5(tt0)5(1-24) q(t)=q_0+a_1(t-t_0)+a_2(t-t_0)^2+a_3(t-t_0)^3+a_4(t-t_0)^4+a_5(t-t_0)^5 \tag{1-24}
    根据条件
    q(t0)=q0,q(t1)=q1q˙(t0)=v0,q˙(t1)=v1q¨(t0)=a0,q¨(t1)=a1. \begin{matrix} 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)=\text a_0, & \ddot{q}(t_1)=\text a_1. \end{matrix}
    这里定义T=t1t0T=t_1-t_0,求得多项式系数为
    {a0=a0a1=v0a2=12a0a3=12T3[20h(8v1+12v0)T(3a0a1)T2]a4=12T4[30h+(14v1+16v0)T+(3a02a1)T2]a5=12T5[12h6(v1+v0)T+(a1a0)T2].(1-25) \begin{cases} a_0&=&a_0\\ a_1&=&v_0\\ a_2&=&\frac{1}{2}\text a_0\\ a_3&=&\frac{1}{2T^3}\left[20h-\left(8v_1+12v_0\right)T-(3\text a_0-\text a_1)T^2\right]\\ a_4&=&\frac{1}{2T^4}[-30h+(14v_1+16v_0)T+(3\text a_0-2\text a_1)T^2]\\ a_5&=&\frac{1}{2T^5}[12h-6(v_1+v_0)T+(\text a_1-\text a_0)T^2]. \end{cases} \tag{1-25}
    Example2.9:使用五次多项式确定运动轨迹:

    (a)
    t0=0,t1=8,q0=0,q1=10,v0=0,v1=0,a0=0,a1=0. \begin{matrix} t_0=0,& t_1=8, \\ q_0=0,& q_1=10, \\ v_0=0, &v_1=0,\\ \text a_0=0, &\text a_1=0. \end{matrix}

    (b)
    $$
    \begin{matrix}

    t_0&=&0,& t_1&=&8, \

    q_0&=&0,& q_1&=&10, \

    v_0&=&-5, &v_1&=&-10,\

    \text a_0&=&0, & \text a_1&=&0.

    \end{matrix}
    $$
    可以注意到,使用三次多项式是无法指定加速度的边界条件的。对(a)和(b)条件使用五次多项式确定轨迹如下图所示。可以与上一篇博文的三次多项式(轨迹规划–三次多项式轨迹)比较一下,三次多项式是无法指定加速度的边界条件的。
    在这里插入图片描述
    图7 五次多项式轨迹

    matlab仿真代码参见examplesCode文件夹下的example2_9.m文件

    %example2.9
    clc
    clear
    %轨迹定义条件
    %时间
    t0=0;
    t1=8;
    %位置和速度(a)
    q0=0;
    q1=10;
    v0=0;
    v1=0;
    acc0=0;
    acc1=0;
    %利用公式(1-25)求系数
    h=q1-q0;
    T=t1-t0;
    a0=q0;
    a1=v0;
    a2=1.0/2*acc0;
    a3=1.0/(2*T*T*T)*(20*h-(8*v1+12*v0)*T+(acc1-3*acc0)/(T*T));
    a4=1.0/(2*T*T*T*T)*(-30*h+(14*v1+16*v0)*T+(3*acc0-2*acc1)/(T*T));
    a5=1.0/(2*T*T*T*T*T)*(12*h-6*(v1+v0)*T+(acc1-acc0)/(T*T));
    %轨迹生成
    t=t0:0.1:t1;
    %位置
    q=a0+a1*power((t-t0),1)+a2*power((t-t0),2)+a3*power((t-t0),3)+...
        a4*power(t-t0,4)+a5*power(t-t0,5);
    %速度
    v=a1+2*a2*power((t-t0),1)+3*a3*power((t-t0),2)+4*a4*power(t-t0,3)+...
        5*a5*power(t-t0,4);
    %加速度
    acc=2*a2+6*a3*power((t-t0),1)+12*a4*power(t-t0,2)+20*a5*power(t-t0,3);
    %绘图
    subplot(3,2,1)
    plot(t,q,'r');
    axis([0,8,0,11])
    ylabel('position')
    grid on
    subplot(3,2,3)
    plot(t,v,'b');
    axis([0,8,-1,2.5])
    ylabel('velocity')
    grid on
    subplot(3,2,5)
    plot(t,acc,'g');
    xlabel('(a)');
    ylabel('acceleration')
    grid on
    
    %时间
    t0=0;
    t1=8;
    %位置和速度(a)
    q0=0;
    q1=10;
    v0=-5;
    v1=-10;
    acc0=0;
    acc1=0;
    %利用公式(1-25)求系数
    h=q1-q0;
    T=t1-t0;
    a0=q0;
    a1=v0;
    a2=1.0/2*acc0;
    a3=1.0/(2*T*T*T)*(20*h-(8*v1+12*v0)*T+(acc1-3*acc0)/(T*T));
    a4=1.0/(2*T*T*T*T)*((-30*h+(14*v1+16*v0)*T)+(3*acc0-2*acc1)/(T*T));
    a5=1.0/(2*T*T*T*T*T)*(12*h-6*(v1+v0)*T+(acc1-acc0)/(T*T));
    %轨迹生成
    t=t0:0.1:t1;
    %位置
    q=a0+a1*power((t-t0),1)+a2*power((t-t0),2)+a3*power((t-t0),3)+...
        a4*power(t-t0,4)+a5*power(t-t0,5);
    %速度
    v=a1+2*a2*power((t-t0),1)+3*a3*power((t-t0),2)+4*a4*power(t-t0,3)+...
        5*a5*power(t-t0,4);
    %加速度
    acc=2*a2+6*a3*power((t-t0),1)+12*a4*power(t-t0,2)+20*a5*power(t-t0,3);
    %绘图
    subplot(3,2,2)
    plot(t,q,'r');
    axis([0,8,-5,30])
    ylabel('position')
    grid on
    subplot(3,2,4)
    plot(t,v,'b');
    ylabel('velocity')
    grid on
    subplot(3,2,6)
    plot(t,acc,'g');
    xlabel('(b)');
    ylabel('acceleration')
    grid on
    

    Example 2.10:对于example2.7、example2.8 的条件(参见上一篇博客轨迹规划–三次多项式轨迹)使用五次多项式确定轨迹,如下图所示。并与example2.8对比,发现“平滑度”得到了改善。

    给定条件
    t0=0,t1=2,t2=4,t3=8,t4=10,q0=10,q1=20,q2=0,q3=30,q4=40,v0=0,v1=10,v2=10,v3=3,v4=0. \begin{matrix} {{t}_{0}}=0,{{t}_{1}}=2, &{{t}_{2}}=4,{{t}_{3}}=8,{{t}_{4}}=10, \\ {{q}_{0}}=10,{{q}_{1}}=20, &{{q}_{2}}=0,{{q}_{3}}=30,{{q}_{4}}=40,\\ {{v}_{0}}=0,{{v}_{1}}=-10, &{{v}_{2}}=10,{{v}_{3}}=3,{{v}_{4}}=0. \\ \end{matrix}
    由此系列点确定的轨迹如下图所示。
    在这里插入图片描述

    图8 Example2.7的条件用五次多项式确定的轨迹

    matlab仿真代码参见examplesCode文件夹下的example2_10.m文件

    %example2.10
    clc
    clear
    close('all')
    %轨迹定义条件
    %时间、位置和速度(a)
    t_array=[0,2,4,8,10];
    q_array=[10,20,0,30,40];
    v_array=[0,-10,10,3,0];
    %起点和终点加速度假设为0,中间点加速度都初始化为0,
    acc_array=[0,0,0,0,0];
    %计算轨迹
    %初始位置
    t=t_array(1);
    q=q_array(1);
    v=v_array(1);
    v_array2=v_array;
    acc=acc_array(1);
    
    for k=1:length(t_array)-1
        %按照式(1-23)式确定中间点的速度值
        if(k>1)
            dk1=(q_array(k)-q_array(k-1))/(t_array(k)-t_array(k-1));
            dk2=(q_array(k+1)-q_array(k))/(t_array(k+1)-t_array(k));
            if((dk2>=0 && dk1>=0) || (dk2<=0 && dk1<=0))
                v_array2(k)=1.0/2.0*(dk1+dk2);
            else
                v_array2(k)=0;
            end  
        end
    end
    
    for k=1:length(t_array)-1
        %利用公式(1-25)求系数
    
        %计算各段多项式的系数
        h(k)=q_array(k+1)-q_array(k);
        T(k)=t_array(k+1)-t_array(k);
        a0(k)=q_array(k);
        a1(k)=v_array2(k);
        a2(k)=1.0/2*acc_array(k);
        a3(k)=1.0/(2*T(k)*T(k)*T(k))*(20*h(k)-(8*v_array2(k+1)+12*v_array2(k))*T(k)+(acc_array(k+1)-3*acc_array(k))/(T(k)*T(k)));
        a4(k)=1.0/(2*T(k)*T(k)*T(k)*T(k))*(-30*h(k)+(14*v_array2(k+1)+16*v_array2(k))*T(k)+(3*acc_array(k)-2*acc_array(k+1))/(T(k)*T(k)));
        a5(k)=1.0/(2*T(k)*T(k)*T(k)*T(k)*T(k))*(12*h(k)-6*(v_array2(k+1)+v_array2(k))*T(k)+(acc_array(k+1)-acc_array(k))/(T(k)*T(k)));
        
        %生成各段轨迹密化的数据点
        %局部时间坐标
        tau=t_array(k):T(k)/100:t_array(k+1);
        %全局时间坐标,由局部时间坐标组成
        t=[t,tau(2:end)];
        %位置
        qk=a0(k)+a1(k)*power(tau-tau(k),1)+a2(k)*power(tau-tau(k),2)+a3(k)*power(tau-tau(k),3)+...
            a4(k)*power(tau-tau(k),4)+a5(k)*power(tau-tau(k),5);
         %全局位置坐标
        q=[q,qk(2:end)];
        %速度
        vk=a1(k)+2*a2(k)*power(tau-tau(k),1)+3*a3(k)*power(tau-tau(k),2)+4*a4(k)*power(tau-tau(k),3)+...
            5*a5(k)*power(tau-tau(k),4);
         v=[v,vk(2:end)];
        %加速度
        acck=2*a2(k)+6*a3(k)*power(tau-tau(k),1)+12*a4(k)*power(tau-tau(k),2)+20*a5(k)*power(tau-tau(k),3);
        acc=[acc,acck(2:end)];
    
    end
    %绘图
    subplot(3,1,1)
    plot(t,q,'r');
    hold on
    plot(t_array,q_array,'o');
     axis([0,10,-5,45])
    ylabel('position')
    grid on
    subplot(3,1,2)
    plot(t,v,'b');
    hold on
    plot(t_array,v_array2,'o');
    axis([0,10,-20,15])
    ylabel('velocity')
    grid on
    subplot(3,1,3)
    plot(t,acc,'g');
    axis([0,10,-35,35])
    ylabel('acceleration')
    grid on
    

    接下来将介绍七次多项式和更高阶次的多项式轨迹。


    参考文献

    Biagiotti L, Melchiorri C. Trajectory Planning for Automatic Machines and Robots[M]. Springer Berlin Heidelberg, 2009.

    展开全文
  • 轴机器人轨迹规划之五次多项式插值

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

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

    {θ(t)=a0+a1t+a2t2+a3t3+a4t4+a5t5θ˙(t)=a1+2a2t+3a3t2+4a4t3+5a5t4θ¨(t)=2a2+6a3t+12a4t2+20a5t3

    约束条件如下,相对三次多项式插值,增加了对起止点角速度的约束(为简便计算设t0为0)
    {θ(t0)=θ0=a0θ(tf)=θf=a0+a1tf+a2tf2+a3tf3+a4tf4+a5tf5θ˙(t0)=θ˙0=a1θ˙(tf)=θ˙f=a1+2a2tf+3a3tf2+4a4tf3+5a5tf4θ¨(t0)=θ¨0=2a2θ¨(tf)=θ¨f=2a2+6a3tf+12a4tf2+20a5tf3

    求解得
    {a0=θ0a1=θ˙0a2=θ¨02a3=20θf20θ0(8θ˙f+12θ˙0)tf(3θ¨0θ¨f)tf22tf3a4=30θ030θf+(14θ˙f+16θ˙0)tf+(3θ¨02θ¨f)tf22tf4a5=12θf12θ0(6θ˙f+6θ˙0)tf(θ¨0θ¨f)tf22tf5

    3.matlab代码实现

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

    这里写图片描述
    PS:相对于三次多项式插值,加速度也是平滑的曲线,并没有出现跳变的情况。然而在机器人系统中,单纯的多项式规划有一个非常严重的问题:没有匀速段,无法根据期望速度提供匀速控制,而在大部分机器人应用中,对加工的速度控制都是有要求的。另一个问题就是,次数越高的多项式,加速过程越慢,整个运动过程中的平均速度越小,影响效率。下次将会为大家介绍能约束速度的三段s曲线加减速直线插值方式。

    展开全文
  • 轴机器人轨迹规划之三次多项式轨迹插值

    万次阅读 多人点赞 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:这种三次多项式插值,只要给定离散点位置、速度和时间,就能插补出一段连续平滑的曲线。但是角加速度并不连续,下次为大家介绍高阶多项式插值则可以解决这个问题。

    展开全文
  • 次多项式次多项式表达式如下: ...可以定义起始和结束的位置、速度、加速度(因为一共有个约束,所以需要是五次多项式) 举例: q0=0q_0=0q0​=0 q1=1q_1=1q1​=1 v0=0v_0=0v0​=0 v1=0v_1=

    机器人轨迹

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

    • 点到点轨迹(P2P)

    • 在线轨迹规划

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

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

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

    这一篇文章讲五次多项式插值轨迹。

    五次多项式

    表达式

    位置:

    q(t)=k0+k1(tt0)+k2(tt0)2+k3(tt0)t3+k4(tt0)4+k5(tt0)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)=k1+2k2(tt0)+3k3(tt0)2+4k4(tt0)3+5k5(tt0)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)=2k2+6k3(tt0)+12k4(tt0)2+20k5(tt0)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}

    特点:

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

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

    把方程(4)结合(1)、(2)、(3)可以得到6个线性方程组,最终可以得到如下解:
    T=t1t0h=q1q0k0=q0k1=v0k2=a02k3=12T3[20h(8v1+12v0)T(3a0a1)T2]k4=12T4[30h+(14v1+16v0)T+(3a02a1)T2]k5=12T5[12h6(v1+v0)T+(a1a0)T2](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}

    代码实现

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

    举例

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

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

    五次多项式轨迹

    总结

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

    展开全文
  • 实验3:取-放轨迹规划——7次多项式轨迹的规划 文章目录一、实验目的和要求1.1 目的1.2 要求二、实验手段三、轨迹规划的推导过程(7次多项式)3.1 边界条件3.2 连续条件3.3 七次多项式推导四、仿真结果与比较分析4.1...
  • 轨迹规划方法一:Cubic Polynomials(三次多项式) 每两个点之间用不同参数的函数来规划,找到函数把两个点串起来 任何两端相接的轨迹都要满足 smooth 的条件,也就是说每段函数头尾处的条件要定义清楚,所以一段函数...
  • 多项式回归

    万次阅读 2018-10-31 12:24:45
    1.多项式回归简介 2.scikit-learn中的多项式回归于pipeline ...考虑下面的数据,虽然我们可以使用线性回归来拟合这些数据,但是这些数据更像是一条二曲线,相应的方程是y=ax2+bx+c,这是式子虽然可...
  • 多项式专题

    2019-10-02 08:00:23
    除法、开根、exp、多点插值、快速求值...\(1.F(x)\)表示一个普通的项数为\(2\)的幂次多项式,\(F_D(x)\)是他的点值表示。 \(2.w\)代表单位根,\(w_m\)表示\(m\)次单位根。 \(3.A\)代表一个数列。 \(4.g\)表示原根。 ...
  • 多项式小结

    2021-07-23 14:06:04
    多项式 文章目录多项式一 简介二 ...多项式求逆 多项式开方七 多项式除法|取模八 多项式对数函数|指数函数九 牛顿迭代法十 多项式三角函数十二 多项式反三角函数 一 简介 **多项式的度:**对于一个多项式f(x)f(x
  • 一、函数的近似表示—高次多项式 二、误差函数—最小二乘法 三、引出案例函数曲线 四、目标函数 五、优化目标函数 、优化目标函数—梯度下降法 七、优化目标函数—求解线性方程组 八、python编程实现拟合...
  • n次多项式共有n+1项。直观地,可以定义一个数组来存储这n+1个系数。以多项式 p ( x ) = − 3.4 x   10   − 9.6 x   8   + 7.2 x   2   + x   为例,存储这个多项式的数组如下图:  可以看出,...
  • 章 线性回归的拓展 - 多项式回归
  • 机器学习算法之多项式回归

    千次阅读 2019-05-12 21:05:14
    考虑下面的数据,虽然我们可以使用线性回归来拟合这些数据,但是这些数据更像是一条二曲线,相应的方程是y=ax2+bx+c,这个式子虽然可以理解为二方程,但是我们呢可以从另外一个角度来理解这个式子: 如果...
  • 1、多项式回归 1.1、前言 简单线性回归:输入特征为一维 多元线性回归:输入特征为多为 但在线性回归中有很强的假设条件,数据存在线性关系。而在实际场景中,大多数数据并不具有明显的线性关系,因此对线性回归法...
  • 次多项式插值轨迹 五次多项式插值轨迹 多项式轨迹实战 梯形速度曲线轨迹 双S形速度曲线轨迹 多个自由度轨迹的时间同步 在线轨迹规划 多项式在线轨迹规划 梯形在线轨迹规划 双S形在线轨迹规划 非线性实时轨迹...
  • 多项式(Polynomial)的运算——利用单链表 1. 多项式的概念 1.1 多项式的定义 有限的单项式之和称为多项式,其中每...通常在数学中对一元n次多项式可表示成如下的形式,设多项式的最高可能阶数为maxDegree,当前的最
  • 数据结构—— 一元多项式的加法运算

    万次阅读 多人点赞 2019-09-24 12:01:56
    在数学上,一个一元n次多项式 可按降序写成: 它由n+1个系数唯一确定,因此,在计算机里他可以用一个线性表表示: 设Pn(x)和Qn(x)分别为两个一元多项式,请求出两个一元多项式的加法运算的结果,要求元素按照...
  • 文章目录多项式插值例题性能Lagrange 形式因式定理推导 Lagrange 插值多项式的表达式例题Lagrange 基函数性质:Newton 插值多项式差商的性质:差商性质 1 :对称性例题差商性质 2带导数条件的插值多项式例题例题插值...
  • 一.简述  利用链表表示稀疏多项式,并基于之前的一些操作(编程实现上还是有所... 1 抽象数据类型一元多项式定义 2 ADT Polyomail{ 3 数据对象:D = {a[i]|a[i]属于TermSet, i = 1,2,3...,m,m>=0 4 ...
  • 值得注意的是,这里很多奇怪的多项式运算本来是不存在模xnx^nxn意义下的定义的,但是由于生成函数是形式幂级数,所以直接取泰勒展开后的结果就是了. 多项式乘法作为最基础的多项式运算,这里不再介绍,具体参见快速...
  • 【快速傅里叶变换】FFT 参考:从多项式乘法到快速傅里叶...点值表示法:一个n-1次多项式在复数域中有n个根,即n个(x,y)可以唯一确定一个n-1次多项式。 对于一个多项式,从其系数表示法到其点值表示法的变换称为...
  • cubic:三次多项式插值,根据已知数据求出一个三次多项式,然后根据该多项式进行插值 %spline:三次样条插值,在每个分段区间内构造一个三次多项式,使得其插值函数满组成插值条件外,还要求在各节点处具有光滑的...
  • qwq。。。好像很久没有写博客了的样子。...然后发现luogu上有很多多项式的板子。。。就去做了一点。。。这样子。。。然后发现好久没更博客了。。。就准备放上来一些。。。也算是水一水博客了。。。反正老年选手已...
  • 一元稀疏多项式计算器

    千次阅读 2005-10-31 18:39:00
    一元稀疏多项式计算器班级:02计2 姓名:刘晓明 学号:200201020219完成日期:2004年9月30日 一,需求分析: 1, 输入并建立多项式2, 输出多项式3, 相同指数的同类项合并4, 对于没有指数的项,把指数设为...

空空如也

空空如也

1 2 3 4 5 ... 20
收藏数 5,366
精华内容 2,146
关键字:

六次多项式的定义