多项式轨迹--五次多项式轨迹

朱雀 2023-03-02 10:28 29阅读 0赞

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

## 1.5 Polynomial of degree five ##

利用三次多项式，根据过 q 0 , q 1 , … , q n q\_0,q\_1,\\ldots,q\_n q0,q1,…,qn确定的轨迹的特征是位置和速度连续，但是加速度不连续（参见上一篇博客[轨迹规划–三次多项式轨迹][Link 1]）。尽管三次多项式轨迹确定的轨迹有一定“平滑”，但是对于一些应用的动力学和惯性载荷会产生一些不期待的影响。为了获得一个加速度连续的轨迹，位置和速度需要合适的初始和终止条件，也需要合适的初始和终止加速度值。这样共有六个边界条件，因此需要采用五次多项式：  
 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 (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(t)=q0\+a1(t−t0)\+a2(t−t0)2\+a3(t−t0)3\+a4(t−t0)4\+a5(t−t0)5(1\-24)  
根据条件  
 q ( t 0 ) = q 0 , q ( t 1 ) = q 1 q ˙ ( t 0 ) = v 0 , q ˙ ( t 1 ) = v 1 q ¨ ( t 0 ) = a 0 , q ¨ ( t 1 ) = a 1 . \\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\} q(t0)=q0,q˙(t0)=v0,q¨(t0)=a0,q(t1)=q1q˙(t1)=v1q¨(t1)=a1.  
这里定义 T = t 1 − t 0 T=t\_1-t\_0 T=t1−t0,求得多项式系数为  
 \{ a 0 = a 0 a 1 = v 0 a 2 = 1 2 a 0 a 3 = 1 2 T 3 \[ 20 h − ( 8 v 1 + 12 v 0 ) T − ( 3 a 0 − a 1 ) T 2 \] a 4 = 1 2 T 4 \[ − 30 h + ( 14 v 1 + 16 v 0 ) T + ( 3 a 0 − 2 a 1 ) T 2 \] a 5 = 1 2 T 5 \[ 12 h − 6 ( v 1 + v 0 ) T + ( a 1 − a 0 ) T 2 \] . (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\} ⎩⎪⎪⎪⎪⎪⎪⎪⎪⎨⎪⎪⎪⎪⎪⎪⎪⎪⎧a0a1a2a3a4a5======a0v021a02T31\[20h−(8v1\+12v0)T−(3a0−a1)T2\]2T41\[−30h\+(14v1\+16v0)T\+(3a0−2a1)T2\]2T51\[12h−6(v1\+v0)T\+(a1−a0)T2\].(1\-25)  
Example2.9:使用五次多项式确定运动轨迹：

(a)  
 t 0 = 0 , t 1 = 8 , q 0 = 0 , q 1 = 10 , v 0 = 0 , v 1 = 0 , a 0 = 0 , a 1 = 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\} t0=0,q0=0,v0=0,a0=0,t1=8,q1=10,v1=0,a1=0.

(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)条件使用五次多项式确定轨迹如下图所示。可以与上一篇博文的三次多项式（[轨迹规划–三次多项式轨迹][Link 1]）比较一下，三次多项式是无法指定加速度的边界条件的。  
![在这里插入图片描述][watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L0dhbGF4eV9Sb2JvdA_size_16_color_FFFFFF_t_70]  
图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 的条件（参见上一篇博客[轨迹规划–三次多项式轨迹][Link 2]）使用五次多项式确定轨迹，如下图所示。并与example2.8对比，发现“平滑度”得到了改善。

给定条件  
 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. \\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\} t0=0,t1=2,q0=10,q1=20,v0=0,v1=−10,t2=4,t3=8,t4=10,q2=0,q3=30,q4=40,v2=10,v3=3,v4=0.  
由此系列点确定的轨迹如下图所示。  
![在这里插入图片描述][watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L0dhbGF4eV9Sb2JvdA_size_16_color_FFFFFF_t_70 1]

图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.

[Link 1]: https://blog.csdn.net/Galaxy_Robot/article/details/107420469
[watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L0dhbGF4eV9Sb2JvdA_size_16_color_FFFFFF_t_70]: /images/20230208/57ad09f10db0495a95a8324c32657f92.png
[Link 2]: http://blog.csdn.net/libing403/article/details/78698322
[watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L0dhbGF4eV9Sb2JvdA_size_16_color_FFFFFF_t_70 1]: /images/20230208/975ba552d34444b79ee39c0476c8b4f7.png