多项式轨迹–三次多项式轨迹

1.4 Cubic trajectory

​ 图 3 三次多项式轨迹

一旦指定了t0,t1时刻的位置和速度的值(q0,q1v0,v1) ，那么有四个条件需要满足，所以必须使用三次多项式

q(t)=a0+a1(t−t0)+a2(t−t0)2+a3(t−t0)3,t0≤t≤t1(1-21)

根据给定条件，可求得四个系数分别为

⎧⎩⎨⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪a0a1a2a3====q0v03h−(2v0+v1)TT2−2h+(v0+v1)TT3(1-22)

通过使用这个结果，可很容易计算出n个点的序列的速度连续的轨迹。整个运动可分为n-1段，每段连接了时刻tk 和tk+1 对应的点qk 和qk+1 ，并且分别具有起始速度和终点速度vk,vk+1。上式可以用来定义这些曲线段的4（n-1）个参数a0k,a1k,a2k,a3k 。

Example2.6:（a）t0=0,t1=8,q0=0,q1=10,v0=v1=0

（b）t0=0,t1=8,q0=0,q1=10,v0=−5,v1=−10

由（a）和（b）的条件确定的两条轨迹分别如下图所示

图 4 Example 2.6

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

%example2.6clcclear%轨迹定义条件%时间t0=0;t1=8;%位置和速度（a）q0=0;q1=10;v0=0;v1=0;%利用公式（1-22）求系数h=q1-q0;T=t1-t0;a0=q0;a1=v0;a2=(3*h-(2*v0+v1)*T)/(T*T);a3=(-2*h+(v0+v1)*T)/(T*T*T);%轨迹生成t=t0:0.1:t1;%位置q=a0+a1*power((t-t0),1)+a2*power((t-t0),2)+a3*power((t-t0),3);%速度v=a1+2*a2*power((t-t0),1)+3*a3*power((t-t0),2);%加速度acc=2*a2+6*a3*power((t-t0),1);%绘图subplot(3,2,1)plot(t,q,'r');ylabel('position')grid onsubplot(3,2,3)plot(t,v,'b');ylabel('velocity')grid onsubplot(3,2,5)plot(t,acc,'g');xlabel('(a)');ylabel('acceleration')grid on%时间t0=0;t1=8;%位置和速度（b）q0=0;q1=10;v0=-5;v1=-10;%利用公式（1-22）求系数h=q1-q0;T=t1-t0;a0=q0;a1=v0;a2=(3*h-(2*v0+v1)*T)/(T*T);a3=(-2*h+(v0+v1)*T)/(T*T*T);%轨迹生成t=t0:0.1:t1;%位置q=a0+a1*power((t-t0),1)+a2*power((t-t0),2)+a3*power((t-t0),3);%速度v=a1+2*a2*power((t-t0),1)+3*a3*power((t-t0),2);%加速度acc=2*a2+6*a3*power((t-t0),1);%绘图subplot(3,2,2)plot(t,q,'r');ylabel('position')grid onsubplot(3,2,4)plot(t,v,'b');ylabel('velocity')grid onsubplot(3,2,6)plot(t,acc,'g');xlabel('(b)');ylabel('acceleration')grid on

Example 2.7 :由多个点定义的运动轨迹

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.

利用式（1-22）确定系数，然后求导得到速度和加速的表达式。example 2.7确定的轨迹如下图所示。

图 5 由给定多个点确定的轨迹

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

%example2.7clcclear%轨迹定义条件t_array=[0,2,4,8,10];q_array=[10,20,0,30,40];v_array=[0,-10,10,3,0];%计算轨迹%初始位置t=t_array(1);q=q_array(1);v=v_array(1);%计算各段轨迹for k=1:length(t_array)-1    %计算各段多项式的系数    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_array(k);    a2(k)=(3*h(k)-(2*v_array(k)+v_array(k+1))*T(k))/(T(k)*T(k));    a3(k)=(-2*h(k)+(v_array(k)+v_array(k+1))*T(k))/(T(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);    %全局位置坐标    q=[q,qk(2:end)];    %速度    vk=a1(k)+2*a2(k)*power(tau-tau(k),1)+3*a3(k)*power(tau-tau(k),2);    v=[v,vk(2:end)];    %加速度    acck=2*a2(k)+6*a3(k)*power(tau-tau(k),1);    if(k==1)        acc=2*a2(k);    end    acc=[acc,acck(2:end)];end%绘图subplot(3,1,1);h1=plot(t,q,'-r');legend(h1,'第一种方式')hold on;plot(t_array,q_array,'or');axis([0,10,-5,45]);ylabel('position')grid on;subplot(3,1,2);plot(t_array,v_array,'ob');hold on;plot(t,v,'b');axis([0,10,-20,15]);ylabel('velocity')grid on;subplot(3,1,3);plot(t,acc,'g');hold onaxis([0,10,-45,45]);ylabel('acceleration')grid on;

通过一系列点q0,q1,⋯,qn 定义轨迹时，中间点的速度并不总是满足指定的速度。这时中间点可根据直观判断确定合适的速度值，例如

v0(assigned)vk={0,sign(dk)≠sign(dk+1)12(dk+dk+1),sign(dk)=sign(dk+1)vn(assigned)(1-23)

这里dk=(qk−qk−1)/(tk−tk-1)​ ，sign(⋅)​ 是函数的符号。

Example2.8:把Example2.7的给定点按照式（1-23）确定中间点的速度值，那么确定的轨迹如下图(虚线)所示。可见第二种方式中间点速度不等于指定点的速度。一般情况下不会指定中间点的速度，只指定起点和终点的速度，这时候就可以使用第二方式规划轨迹。有时候定义轨迹时，指定的中间点的速度不合理，会导致速度曲线波动过大，这是时候如果不要求中间位置的速度都必须与指定相等，也可以使用第二种规划方式。

图 6 中间点的速度不一定等于指定速度的轨迹规划

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

%example2.8clcclear%轨迹定义条件t_array=[0,2,4,8,10];q_array=[10,20,0,30,40];v_array=[0,-10,10,3,0];%计算轨迹%初始位置t=t_array(1);q=q_array(1);v=v_array(1);v_array2=v_array;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      endend%计算各段轨迹for k=1:length(t_array)-1    %计算各段多项式的系数    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)=(3*h(k)-(2*v_array2(k)+v_array2(k+1))*T(k))/(T(k)*T(k));    a3(k)=(-2*h(k)+(v_array2(k)+v_array2(k+1))*T(k))/(T(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);    %全局位置坐标    q=[q,qk(2:end)];    %速度    vk=a1(k)+2*a2(k)*power(tau-tau(k),1)+3*a3(k)*power(tau-tau(k),2);    v=[v,vk(2:end)];    %加速度    acck=2*a2(k)+6*a3(k)*power(tau-tau(k),1);    if(k==1)        acc=2*a2(k);    end    acc=[acc,acck(2:end)];end%绘图subplot(3,1,1);h2=plot(t,q,'--r');legend(h2,'第二种方式')hold on;plot(t_array,q_array,'^r');axis([0,10,-5,45]);ylabel('position')grid on;subplot(3,1,2);plot(t_array,v_array2,'^b');hold on;plot(t,v,'--b');axis([0,10,-20,15]);ylabel('velocity')grid on;subplot(3,1,3);plot(t,acc,'--g');axis([0,10,-45,45]);ylabel('acceleration')grid on;

小结

前面所述的三次多项式实际是速度规划里面常说的PVT算法。PVT 模 式 是 指 位 置 — 速 度 — 时 间（Position-Velocity-Time）模式。PVT模式是一种简单又有效的运动控制模式，用户只需要给定离散点的位置、速度和时间，运动控制卡的插补算法将会生成一条连续、平滑的运动路径。

PVT 算法除了给定各数据点的“位置、速度、时间”之外，还可以通过其他形式给出参数。根据输入参数的不同，PVT 算法可分为 PVT 描述方式、Complete

描述方式、Percent 描述方式和 Continuous 描述方式。

前面所述的三次多项式实际是PVT描述方式，PVT描述方式直接给定各数据点的“位置、速度、时间”。一般使用多项式插值，若用三次多项式表示位移-时间关系，每两个相邻数据点可单独确定一个三次曲线。没有限制各段之间的衔接条件，不能保证各个点处速度曲线平滑过渡，因此不能保证加速度连续。其他描述方式后面会逐渐介绍。

欢迎指正，留言交流。

---

参考文献
[1]Biagiotti L, Melchiorri C. Trajectory Planning for Automatic Machines and Robots[M]. Springer Berlin Heidelberg, 2009.
[2]东梁. 嵌入式运动控制器若干关键技术的研究与实现[D].2015.