多项式轨迹–三次轨迹

1.4 Cubic trajectory

图 3 三次多项式轨迹

一旦指定了 t 0 , t 1 { {t}_{0}},{ {t}_{1}} t0​,t1​时刻的位置和速度的值 ( q 0 , q 1 v 0 , v 1 ) \left({ {q}_{0}},{ {q}_{1}}{ {v}_{0}},{ {v}_{1}} \right) (q0​,q1​v0​,v1​) ，那么有四个条件需要满足，所以必须使用三次多项式
q ( t ) = a 0 + a 1 ( t − t 0 ) + a 2 ( t − t 0 ) 2 + a 3 ( t − t 0 ) 3 , t 0 ≤ t ≤ t 1 (1-21) q\left(t \right)={ {a}_{0}}+{ {a}_{1}}\left( t-{ {t}_{0}} \right)+{ {a}_{2}}{ {\left(t-{ {t}_{0}} \right)}^{2}}+{ {a}_{3}}{ {\left( t-{ {t}_{0}}\right)}^{3}},{ {t}_{0}}\le t\le { {t}_{1}} \tag{1-21} q(t)=a0​+a1​(t−t0​)+a2​(t−t0​)2+a3​(t−t0​)3,t0​≤t≤t1​(1–21)
根据给定条件，可求得四个系数分别为
{ a 0 = q 0 a 1 = v 0 a 2 = 3 h − ( 2 v 0 + v 1 ) T T 2 a 3 = − 2 h + ( v 0 + v 1 ) T T 3 a 0 = q 0 a 1 = v 0 a 2 = 3 h − ( 2 v 0 + v 1 ) T T 2 a 3 = − 2 h + ( v 0 + v 1 ) T T 3 (1-22) \begin{cases} \begin{matrix} { {a}_{0}}&=&{ {q}_{0}} \\ { {a}_{1}}&=&{ {v}_{0}} \\ { {a}_{2}}&=&\frac{3h-\left( 2{ {v}_{0}}+{ {v}_{1}} \right)T}{ { {T}^{2}}} \\ { {a}_{3}}&=&\frac{-2h+\left( { {v}_{0}}+{ {v}_{1}} \right)T}{ { {T}^{3}}} \\ { {a}_{0}}&=& { {q}_{0}} \\ { {a}_{1}}&=& { {v}_{0}} \\ { {a}_{2}}&=& \frac{3h-\left( 2{ {v}_{0}}+{ {v}_{1}} \right)T}{ { {T}^{2}}} \\ { {a}_{3}}&=& \frac{-2h+\left( { {v}_{0}}+{ {v}_{1}} \right)T}{ { {T}^{3}}} \\ \end{matrix} \end{cases} \tag{1-22} ⎩⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎨⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎧​a0​a1​a2​a3​a0​a1​a2​a3​​========​q0​v0​T23h−(2v0​+v1​)T​T3−2h+(v0​+v1​)T​q0​v0​T23h−(2v0​+v1​)T​T3−2h+(v0​+v1​)T​​​(1–22)
通过使用这个结果，可很容易计算出n个点的序列的速度连续的轨迹。整个运动可分为n-1段，每段连接了时刻 t k { {t}_{k}} tk​ 和 t k + 1 { {t}_{k+1}} tk+1​ 对应的点 q k { {q}_{k}} qk​ 和 q k + 1 { {q}_{k+1}} qk+1​ ，并且分别具有起始速度和终点速度 v k , v k + 1 { {v}_{k}},{ {v}_{k+1}} vk​,vk+1​。上式可以用来定义这些曲线段的4（n-1）个参数 a 0 k , a 1 k , a 2 k , a 3 k { {a}_{0}}_{k},{ {a}_{1k}},{ {a}_{2k}},{ {a}_{3k}} a0​k​,a1k​,a2k​,a3k​ 。

Example2.6:（a） t 0 = 0 , t 1 = 8 , q 0 = 0 , q 1 = 10 , v 0 = v 1 = 0 { {t}_{0}}=0,{ {t}_{1}}=8,{ {q}_{0}}=0,{ {q}_{1}}=10,{ {v}_{0}}={ {v}_{1}}=0 t0​=0,t1​=8,q0​=0,q1​=10,v0​=v1​=0

（b） t 0 = 0 , t 1 = 8 , q 0 = 0 , q 1 = 10 , v 0 = − 5 , v 1 = − 10 { {t}_{0}}=0,{ {t}_{1}}=8,{ {q}_{0}}=0,{ {q}_{1}}=10,{ {v}_{0}}=-5,{ {v}_{1}}=-10 t0​=0,t1​=8,q0​=0,q1​=10,v0​=−5,v1​=−10

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

​ 图 4 Example 2.6

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

%example2.6
clc
clear
%轨迹定义条件
%时间
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 on
subplot(3,2,3)
plot(t,v,'b');
ylabel('velocity')
grid on
subplot(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 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.7 :由多个点定义的运动轨迹
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,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.​

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

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

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

%example2.7
clc
clear
%轨迹定义条件
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 on
axis([0,10,-45,45]);
ylabel('acceleration')
grid on;

通过一系列点 q 0 , q 1 , ⋯   , q n { {q}_{0}},{ {q}_{1}},\cdots,{ {q}_{n}} q0​,q1​,⋯,qn​ 定义轨迹时，中间点的速度并不总是满足指定的速度。这时中间点可根据直观判断确定合适的速度值，例如
v 0 ( a s s i g n e d ) v k = { 0 , s i g n ( d k ) ≠ s i g n ( d k + 1 ) 1 2 ( d k + d k + 1 ) , s i g n ( d k ) = s i g n ( d k + 1 ) v n ( a s s i g n e d ) (1-23) \begin{matrix} & { {v}_{0}}\left( assigned \right) \\ & { {v}_{k}}= \begin{cases} \begin{matrix} 0, &sign\left( { {d}_{k}} \right)\ne sign\left( { {d}_{k+1}} \right) \\ \frac{1}{2}\left( { {d}_{k}}+{ {d}_{k+1}} \right), &sign\left( { {d}_{k}}\right)=sign\left( { {d}_{k+1}} \right) \\ \end{matrix} \\ \end{cases}\\ & { {v}_{n}}\left( assigned \right) \\ \end{matrix} \tag{1-23} ​v0​(assigned)vk​={ 0,21​(dk​+dk+1​),​sign(dk​)​=sign(dk+1​)sign(dk​)=sign(dk+1​)​​vn​(assigned)​(1–23)
这里 d k = ( q k − q k − 1 ) / ( t k − t k – 1 ) { {d}_{k}}=\left({ {q}_{k}}-{ {q}_{k-1}} \right)/\left( { {t}_{k}}-{ {t}_{k\text{-}1}} \right) dk​=(qk​−qk−1​)/(tk​−tk–1​) ， s i g n ( ⋅ ) sign\left( \cdot \right) sign(⋅) 是函数的符号。

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

图 6 中间点的速度不一定等于指定速度

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

%example2.8
clc
clear
%轨迹定义条件
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  
    end
end

%计算各段轨迹
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.