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

在轨迹规划中，规划起始点与终点的加速度连续非常重要，如果在终点或起始点的端点处，加速度有跳变，对于乘坐舒适性有很大影响。采用五次多项式进行轨迹规划时，需要六个条件才能求解出五次多项式的6个系数，因此很容易找出这6个边值条件，分别是起始点的位置、速度、加速度以及终点的位置、速度、加速度。

​ 五次多项式形式：q(t)表示与t0时刻相距（t-t0）距离时多项式的值

\[\mathrm{q}(\mathrm{t})=\mathrm{a}_{0}+\mathrm{a}_{1}\left(\mathrm{t}-\mathrm{t}_{0}\right)+\mathrm{a}_{2}\left(\mathrm{t}-\mathrm{t}_{0}\right)^{2}+\mathrm{a}_{3}\left(\mathrm{t}-\mathrm{t}_{0}\right)^{3}+\mathrm{a}_{4}\left(\mathrm{t}-\mathrm{t}_{0}\right)^{4}+\mathrm{a}_{5}\left(\mathrm{t}-\mathrm{t}_{0}\right)^{5} \tag{1-1} \]

​ 起始点及终止点的条件：

\[\begin{array}{c} \mathrm{q}\left(\mathrm{t}_{0}\right)=\mathrm{q}_{0}, \quad \mathrm{q}\left(\mathrm{t}_{1}\right)=\mathrm{q}_{1} \\ \dot{\mathrm{q}}\left(\mathrm{t}_{0}\right)=\mathrm{v}_{0}, \quad \dot{\mathrm{q}}\left(\mathrm{t}_{1}\right)=\mathrm{v}_{1} \\ \ddot{\mathrm{q}}\left(\mathrm{t}_{0}\right)=\mathrm{a}_{0}, \quad \ddot{\mathrm{q}}\left(\mathrm{t}_{1}\right)=\mathrm{a}_{1} \end{array} \]

这里定义T = t1 − t0 ,求得多项式系数为h = q1 - q0。

\[\left\{\begin{array}{lcc} \mathrm{a}_{0}= & \mathrm{q}_{0} \\ \mathrm{a}_{1}= & \mathrm{v}_{0} \\ \mathrm{a}_{2}= & \frac{1}{2} \mathrm{a}_{0} \\ \mathrm{a}_{3}= & \frac{1}{2 \mathrm{~T}^{3}}\left[20 \mathrm{~h}-\left(8 \mathrm{v}_{1}+12 \mathrm{v}_{0}\right) \mathrm{T}-\left(3 \mathrm{a}_{0}-\mathrm{a}_{1}\right) \mathrm{T}^{2}\right] \\ \mathrm{a}_{4}= & \frac{1}{2 \mathrm{~T}^{4}}\left[-30 \mathrm{~h}+\left(14 \mathrm{v}_{1}+16 \mathrm{v}_{0}\right) \mathrm{T}+\left(3 \mathrm{a}_{0}-2 \mathrm{a}_{1}\right) \mathrm{T}^{2}\right] \\ \mathrm{a}_{5} = & \frac{1}{2 \mathrm{~T}^{5}}\left[12 \mathrm{~h}-6\left(\mathrm{v}_{1}+\mathrm{v}_{0}\right) \mathrm{T}+\left(\mathrm{a}_{1}-\mathrm{a}_{0}\right) \mathrm{T}^{2}\right] \end{array}\right. \tag{1-2} \]

例一：

\[\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} \]

clc;
clear
%轨迹定义条件
%% 左图
% t0s时刻初始条件
t0=0; q0=0; v0=0; acc0=0;
%位置和速度（a）
t1=8; q1=10; v1=0; acc1=0;
%利用公式（1-2）求系数
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=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:10;
%位置
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