原创力文档- 1、本文档共18页，可阅读全部内容。
- 2、原创力文档（book118）网站文档一经付费（服务费），不意味着购买了该文档的版权，仅供个人/单位学习、研究之用，不得用于商业用途，未经授权，严禁复制、发行、汇编、翻译或者网络传播等，侵权必究。
- 3、本站所有内容均由合作方或网友上传，本站不对文档的完整性、权威性及其观点立场正确性做任何保证或承诺！文档内容仅供研究参考，付费前请自行鉴别。如您付费，意味着您自己接受本站规则且自行承担风险，本站不退款、不进行额外附加服务；查看《如何避免下载的几个坑》。如果您已付费下载过本站文档，您可以点击 这里二次下载。
- 4、如文档侵犯商业秘密、侵犯著作权、侵犯人身权等，请点击“版权申诉”（推荐），也可以打举报电话：400-050-0827(电话支持时间：9:00-18:30)。
1三次多项式轨迹规划;三次多项式角度位置：;q0=30,q1=75，v0=v1=0时：;五次多项式角度位置：;q0=30,q1=75，v0=v1=0，a0=a1=0时：;第一段初始位置（起始点）：;第二段3次多项式终端当地时间：;%三次多项式源码
clc
clear
%轨迹定义条件
%时间
t0=0;
t1=6;
v_max=8;%定义最大速度
%位置和速度（a）
q0=30;
q1=75;
v0=0;
v1=0;
%运用公式（1-22）求系数
h=q1-q0;%最大位置差
T=t1-t0;%最大时间差
a0=q0;%第一种参数
a1=v0;%第二个参数;%位置
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);%速度
fori=1:1:length(v)
if(v(i)v_max)
v(i)=v_max;
end
end
%加速度
acc=2*a2+6*a3*power((t-t0),1);
%绘图
subplot(3,2,1);%五次多项式源码
clc
clear
%轨迹定义条件
%时间
t0=0;
t1=10;
%位置和速度（a）
q0=0;
q1=150;
v0=0;
v1=0;
v_max=8;%定义最大速度
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: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);
%fori=1:1:length(v)
%if(v(i)v_max)
%v(i)=v_max;
%end
%end
%加速度
acc=2*a2+6*a3*power((t-t0),1)+12*a4*power(t-t0,2)+20*a5*power(t-t0,3);
%绘图
figure(1),
subplot(3,1,1)
%plot(t,q,r);
t1=1:1:t1;
i=1;
num=1;;fori=1:i+10:length(q)
q1(num)=q(i);
num=num+1;
end
plot(t1,q1,ro);
%axis([0,8,0,11])
ylabel(position)
gridon
subplot(3,1,2)
plot(t,v,b);
%axis([0,8,-1,2.5])
ylabel(velocity)
gridon
subplot(3,1,3)
plot(t,acc,k);
xlabel((a));
ylabel(acceleration)
gridon;%四三四次多项式轨迹规划
clc
clear
ToDeg=180/pi;
ToRad=pi/180;%转化为弧度制
%定义初始角度
Th1=30;%30
Th1_1=0;
Th1_2=0;
Th2=50;%50
Th3=90;%90
Th4=70;%70
Th4_1=0;
Th4_2=0;
%定义初始当地起始时间
Tao1_i=0;
Tao2_i=0;
Tao3_i=0;
%定义初始当地终端时间
Tao1_f=2;
Tao2_f=4;
Tao3_f=2;
%定义全局起始、终止时间;t_i=0;
t_f=Tao1_f+Tao2_f+Tao3_f-(Tao1_i+Tao2_i+Tao3_i);
TH=[Th1Th1_1Th1_2Th2Th200Th3Th300Th4Th4_1Th4_2];
%C=[a0a1a2a3a4b0
文档评论（0）