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

1.5 Polynomial of degree five

利用三次多项式，根据过 q 0 , q 1 , … , q n q_0,q_1,\ldots,q_n q0​,q1​,…,qn​确定的轨迹的特征是位置和速度连续，但是加速度不连续（参见上一篇博客轨迹规划–三次多项式轨迹）。尽管三次多项式轨迹确定的轨迹有一定“平滑”，但是对于一些应用的动力学和惯性载荷会产生一些不期待的影响。为了获得一个加速度连续的轨迹，位置和速度需要合适的初始和终止条件，也需要合适的初始和终止加速度值。这样共有六个边界条件，因此需要采用五次多项式：
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​)=q1​q˙​(t1​)=v1​q¨​(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} ⎩⎪⎪⎪⎪⎪⎪⎪⎪⎨⎪⎪⎪⎪⎪⎪⎪⎪⎧​a0​a1​a2​a3​a4​a5​​======​a0​v0​21​a0​2T31​[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)条件使用五次多项式确定轨迹如下图所示。可以与上一篇博文的三次多项式（轨迹规划–三次多项式轨迹）比较一下，三次多项式是无法指定加速度的边界条件的。

图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对比，发现“平滑度”得到了改善。

给定条件
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.​
由此系列点确定的轨迹如下图所示。

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