Trajectory Planning （1）——五次、七次多项式

五次多项式

Matlab Code:

% referce:Trajectory Planning for Automatic Machines and Robots
% Page:17-Example 2.1
% time:2019/09/21
%
%*************************************************************************%
%符号方程解线性方程组
%qt=a0+a1*t+a2*t.^2+a3*t.^3+a4*t.^4+a5*t^5;

clc;clear all;
% 五次多项式的符号解-数值解
q0=10;qf=20;
v0=0;v2=2;vf=0;
alp0=0;alp8=0;
t0=0;t2=2;t8=8;
[qt,vt,alpt,jerkt]=myciduoxiangshi(q0,qf,v0,v2,vf,alp8,t0,t2,t8,tf) ; %关节位移函数
qt=subs(qt,{'q0','qf','v0','v2','vf','alp8','t0','t2','t8','tf'},{10,20,0,2,0,0,0,2,8,10})
vt=subs(vt,{'q0','qf','v0','v2','vf','alp8','t0','t2','t8','tf'},{10,20,0,2,0,0,0,2,8,10})
alpt=subs(alpt,{'q0','qf','v0','v2','vf','alp8','t0','t2','t8','tf'},{10,20,0,2,0,0,0,2,8,10})
jerkt=subs(jerkt,{'q0','qf','v0','v2','vf','alp8','t0','t2','t8','tf'},{10,20,0,2,0,0,0,2,8,10})

%%
function [qt,vt,alpt,jerkt]=myciduoxiangshi(q0,qf,v0,v2,vf,alp8,t0,t2,t8,tf)
syms t;
syms a0 a1 a2 a3 a4 a5;
eql1=sym('a0+a1*t0+a2*t0^2+a3*t0^3+a4*t0^4+a5*t0^5-q0');   % constrain t0=0  q=q0=10
eql2=sym('a1+2*a2*t0+3*a3*t0^2+4*a4*t0^3+5*a5*t0^4-v0');   % constrain t0=0  v=v0
eql3=sym('a1+2*a2*t2+3*a3*t2^2+4*a4*t2^3+5*a5*t2^4-v2');   % constrain t2=2  v=v2
eql4=sym('a0+a1*tf+a2*tf^2+a3*tf^3+a4*tf^4+a5*tf^5-qf');   % constrain tf=10 q=qf=20
eql5=sym('a1+2*a2*tf+3*a3*tf^2+4*a4*tf^3+5*a5*tf^4-vf');   % constrain tf=10  v=vf=0  
eql6=sym('2*a2+6*a3*t8+12*a4*t8^2+20*a5*t8^3-alp8');       % constrain t8=8   a=alp8=0
[a0 a1 a2 a3 a4 a5]=solve(eql1,eql2,eql3,eql4,eql5,eql6,'a0','a1','a2','a3','a4','a5');

%输出值

qt=a0+a1*t+a2*t^2+a3*t^3+a4*t^4+a5*t^5;
vt=diff(qt,'t');         %速度位移函数
alpt=diff(qt,'t',2);     %加速度函数
jerkt=diff(alpt,'t');    %加加速度函数

end

%%
%绘图   
% %位置曲线
t=0:0.05:10;
qt =- (19*t.^5)/20800 + (139*t.^4)/5200 - (1459*t.^3)/5200 + (149*t.^2)/130 + 10;
vt =- (19*t.^4)/4160 + (139*t.^3)/1300 - (4377*t.^2)/5200 + (149*t)/65;
alpt =- (19*t.^3)/1040 + (417*t.^2)/1300 - (4377*t)/2600 + 149/65;
jerkt =(417*t)/650 - (57*t.^2)/1040 - 4377/2600;

subplot(2,2,1);plot(t,qt) 
title('qt');grid on;
subplot(2,2,2);plot(t,vt) 
title('vt');grid on;
subplot(2,2,3);plot(t,alpt)
title('alpt');grid on;
subplot(2,2,4);plot(t,jerkt) 
title('jerkt');grid on;

结果：

七次多项式：
有时候需要考虑加加速度情况，需要更加高阶函数来规划曲线，例如七次多项式函数，七次多项式函数可以获得更加平滑的加速度曲线。

Matlab Code:

%% referce:Trajectory Planning for Automatic Machines and Robots
%      Page:17-Example 2.11
%      time:2019/09/22
%      Note：
%      author：easyR
% 七次多项式的求解
%***************************************************************************%
% q(t) = a0 + a1(t − t0) + a2(t − t0)^2 + a3(t − t0)^3 + a4(t − t0)^4 +  *
%      +a5(t − t0)^5 + a6(t − t0)^6 + a7(t − t0)^7                        *
%
%% 初始条件
clc;clear all;
syms t;
q0 = 0; q1 = 10;
v0 = 0;  v1 = 0;
a0 = 0;  a1=0;
j0 = 0;  j1 = 0;
t0 = 0;  t1 = 8;

%函数方程
display('输出函数方程：')
display('*_____________________________________________________________________*')
[qt vt at jt]= sevenPolyn(t0,t1,q0,q1,v0,v1,a0,a1,j0,j1)
display('*_____________________________________________________________________*')

k=0:0.05:8;
for i=1:length(k)
    qty(i)=subs(qt,{t},{k(i)});   % 不要同名qt与qty
    vty(i)=subs(vt,{t},{k(i)});
    aty(i)=subs(at,{t},{k(i)});
    jty(i)=subs(jt,{t},{k(i)});
end

%%
%绘图
subplot(2,2,1);plot(k,qty,'-.b','linewidth',0.5);title('position');grid on;
subplot(2,2,2);plot(k,vty,'--g','linewidth',0.5);title('velocity');grid on;
subplot(2,2,3);plot(k,aty,'g','linewidth',0.1);title('acceleration');grid on;
subplot(2,2,4);plot(k,jty,'--k','linewidth',0.5);title('jerk');grid on;
      

%%
function [qt vt at jt]=sevenPolyn(t0,t1,q0,q1,v0,v1,a0,a1,j0,j1)

syms t;
T=t1-t0;
h=q1-q0;

a0=q0;
a1=v0;
a2=a0/2;
a3=j0/6;
a4=(210*h-T*((30*a0-15*a1)*T+(4*j0+j1)*power(T,2)+120*v0+90*v1))/(6*power(T,4));
a5=(-168*h+T*((20*a0-14*a1)*T+(2*j0+j1)*power(T,2)+90*v0+78*v1))/(2*power(T,5));
a6=(420*h-T*((45*a0-39*a1)*T+(4*j0+3*j1)*power(T,2)+216*v0+204*v1))/(6*power(T,6));
a7=(-120*h+T*((12*a0-12*a1)*T+(j0+j1)*power(T,2)+60*v0+60*v1))/(6*power(T,7));

%位移——加速度函数
qt = a0 + a1*(t - t0) + a2*(t- t0).^2 + a3*(t- t0).^3 + a4*(t- t0).^4 +a5*(t - t0).^5 + a6*(t- t0).^6 + a7*(t- t0).^7 ;
% qt=vpa(qt);  %强制转化为小数
vt= diff(qt);
at= diff(vt);
jt= diff(at); 

end

输出结果：

*_____________________________________________________________________*
 
qt =
 
- (25*t^7)/262144 + (175*t^6)/65536 - (105*t^5)/4096 + (175*t^4)/2048
 
 
vt =
 
- (175*t^6)/262144 + (525*t^5)/32768 - (525*t^4)/4096 + (175*t^3)/512
 
 
at =
 
- (525*t^5)/131072 + (2625*t^4)/32768 - (525*t^3)/1024 + (525*t^2)/512
 
 
jt =
 
- (2625*t^4)/131072 + (2625*t^3)/8192 - (1575*t^2)/1024 + (525*t)/256
 
*_____________________________________________________________________*

输出图像：