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

1.4 Cubic trajectory

三次多项式轨迹

图 3 三次多项式轨迹

一旦指定了 ${{t}_{0}},{{t}_{1}}$ 时刻的位置和速度的值 $\left({{q}_{0}},{{q}_{1}}{{v}_{0}},{{v}_{1}} \right)$ ，那么有四个条件需要满足，所以必须使用三次多项式

q (t) = a 0 + a 1 (t - t 0) + a 2 (t - t 0) 2 + a 3 (t - t 0) 3, t 0 \leq t \leq 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}$
根据给定条件，可求得四个系数分别为

⎧ ⎩ ⎨ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ a 0 a 1 a 2 a 3 = = = = q 0 v 0 3 h - ( 2 v 0 + v 1 ) T T 2 - 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}}} \\ \end{matrix} \end{cases} \tag{1-22}$

通过使用这个结果，可很容易计算出n个点的序列的速度连续的轨迹。整个运动可分为n-1段，每段连接了时刻 $t_k$ 和 ${{t}_{k+1}}$ 对应的点 ${{q}_{k}}$ 和 ${{q}_{k+1}}$ ，并且分别具有起始速度和终点速度 ${{v}_{k}},{{v}_{k+1}}$ 。上式可以用来定义这些曲线段的4（n-1）个参数 ${{a}_{0}}_{k},{{a}_{1k}},{{a}_{2k}},{{a}_{3k}}$ 。

Example2.6:（a） ${{t}_{0}}=0,{{t}_{1}}=8,{{q}_{0}}=0,{{q}_{1}}=10,{{v}_{0}}={{v}_{1}}=0$

（b） ${{t}_{0}}=0,{{t}_{1}}=8,{{q}_{0}}=0,{{q}_{1}}=10,{{v}_{0}}=-5,{{v}_{1}}=-10$

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

Example2.6

图 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{align} &{{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{align}$

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

Example2.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}},\cdots,{{q}_{n}}$ 定义轨迹时，中间点的速度并不总是满足指定的速度。这时中间点可根据直观判断确定合适的速度值，例如

v 0 (a s s i g n e d) v k = {0, s i g n (d k) \neq 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{align} & {{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{align} \tag{1-23}$

这里 ${{d}_{k}}=\left({{q}_{k}}-{{q}_{k-1}} \right)/\left( {{t}_{k}}-{{t}_{k\text{-}1}} \right)$ ， $sign\left( \cdot \right)$ 是函数的符号。

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

Example2.8

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