三次多项式代码实现
%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=(3h-(2v0+v1)T)/(TT);
a3=(-2h+(v0+v1)T)/(TTT);
%轨迹生成
t=t0:0.1:t1;
%位置
q=a0+a1power((t-t0),1)+a2power((t-t0),2)+a3power((t-t0),3);
%速度
v=a1+2a2power((t-t0),1)+3a3power((t-t0),2);
%加速度
acc=2a2+6a3power((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=(3h-(2v0+v1)T)/(TT);
a3=(-2h+(v0+v1)T)/(TTT);
%轨迹生成
t=t0:0.1:t1;
%位置
q=a0+a1power((t-t0),1)+a2power((t-t0),2)+a3power((t-t0),3);
%速度
v=a1+2a2power((t-t0),1)+3a3power((t-t0),2);
%加速度
acc=2a2+6a3power((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
五次多项式代码实现
clc
clear
close all
%%场景定义
%换道场景相关参数定义
d = 3.5;%道路标准宽度
len_line = 30;%直线长度
W =1.75;%车宽
L = 4.7;%车长
x1 = 20;%1号车x坐标
%车辆换道初始状态
t0 = 0;
t1 = 3;
state_t0 = [0, -d/3; 5, 0; 0, 0];%x,y,vx,vy,ax,ay
state_t1 = [20,-d/3; 5, 0; 0, 0];
x2=state_t0(1);
%%画场景示意图
figure(1)
%画灰色路面图
GreyZone = [-5,-d-0.5; -5,d+0.5; len_line,d+0.5; len_line,-d-0.5];
fill(GreyZone(:,1),GreyZone(:,2),[0.5 0.5 0.5]);
hold on
%画小车
fill([x1,x1,x1+L,x1+L],[-d/2-W/2,-d/2+W/2,-d/2+W/2,-d/2-W/2],‘b’)%一号车
%fill([x2,x2,x2-L,x2-L],[-d/2-W/2,-d/2+W/2,-d/2+W/2,-d/2-W/2],‘y’)%二号车
%画分界线
plot([-5,len_line],[0,0],‘w–’,‘linewidth’,2);
plot([-5,len_line],[d,d],‘w’,‘linewidth’,2);
plot([-5,len_line],[-d,-d],‘w’,‘linewidth’,2);
%设置坐标轴显示范围
axis equal
set(gca, ‘XLim’,[-5 len_line]);
set(gca, ‘YLim’,[-4 4]);
%%五次多项式轨迹生成
%计算A和B两个矩阵
X = [state_t0(:,1);state_t1(:,1)];
Y = [state_t0(:,2);state_t1(:,2)];
T = [ t0^5 t0^4 t0^3 t0^2 t0 1;
5t0^4 4t0^3 3t0^2 2t0 1 1;
20t0^3 12t0^2 6t0 1 0 0;
t1^5 t1^4 t1^3 t1^2 t1 1;
5t1^4 4t1^3 3t1^2 2t1 1 1;
20t1^3 12t1^2 6t1 1 0 0];
A = T X;
B = T Y;
%将时间t0到t1离散化，获得离散时刻的坐标
t=(t0:0.05:t1)’;
path=zeros(length(t),4);%1-4列分别存放x,y,vx,vy
for i = 1:length(t)
%纵向位置坐标
path(i,1) = [t(i)^5, t(i)^4, t(i)^3, t(i)^2, t(i), 1] * A;
end
%画换道轨迹
plot(path(:,1),path(:,2),‘r–’,‘linewidth’,1.5);
%分析速度
%横向速度
figure
plot(t, path(:,4),‘k’);
xlabel(‘时间/s’);
ylabel(‘横向速度/ m/s’);
%纵向速度
figure
plot(t, path(:,3),‘k’);
xlabel(‘时间/s’);
ylabel(‘纵向速度/ m/s’);
%分析加速度
%横向速度% figure
% plot(t, path(:,6),‘k’);
% xlabel(‘时间/s’);
% ylabel(‘横向加速度/ m/s^2’);
%纵向速度
% figure
% plot(t, path(:,5),‘k’);
% xlabel(‘时间/s’);
% ylabel(‘纵向加速度/ m/s^2’);