局部路径规划--曲线插值法 五次曲线插值 matlab代码（障碍物避让）
用多项式曲线插值法完成局部路径规划（障碍物避让）
思路：(PPT图截自B站 小黎的Ally 感谢)
自己写的代码：
%% 用多项式曲线插值法完成局部路径规划（障碍物避让）
%jubobolv369
clc;
clear;
%% 初始化数据
road_width=3.5; %单车道宽
road_length=30; %路长
truck_width=1.8; %车宽
truck_length=4.5; %车长
truck1_pos=[4,road_width/2]; %后车中心位置
truck2_pos=[25,road_width/2]; %车前中心位置
%% 制图
figure(1);
%绘制路面
fill([0,road_length,road_length,0],[0,0,2*road_width,2*road_width],[0.5,0.5,0.5]);
hold on
%路面中心线、边界线
plot([0,road_length],[road_width,road_width],'w--','linewidth',2);
plot([0,road_length],[2*road_width,2*road_width],'w--','linewidth',2);
plot([0,road_length],[0,0],'w--','linewidth',2);
%绘制车辆
fill([truck1_pos(1)-truck_length/2,truck1_pos(1)+truck_length/2,truck1_pos(1)+truck_length/2,truck1_pos(1)-truck_length/2],...
[truck1_pos(2)-truck_width/2,truck1_pos(2)-truck_width/2,truck1_pos(2)+truck_width/2,truck1_pos(2)+truck_width/2],'b');
fill([truck2_pos(1)-truck_length/2,truck2_pos(1)+truck_length/2,truck2_pos(1)+truck_length/2,truck2_pos(1)-truck_length/2],...
[truck2_pos(2)-truck_width/2,truck2_pos(2)-truck_width/2,truck2_pos(2)+truck_width/2,truck2_pos(2)+truck_width/2],'y');
% xlim([0 road_length]);
% ylim([0 2*road_width]);
axis equal
set(gca,'XLim',[0 road_length])
set(gca,'YLim',[0 2*road_width])
%% 车辆初始信息、期望信息
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% X = a0 + a1*t + a2 * t^2 + a3 * t^3 + a4 * t^4 + a5 * t^5 位 移%
% X' = 0 + a1+ 2*a2 * t + 3*a3 * t^2 + 4*a4 * t^3 + 5*a5 * t^4 速 度%
% X''= 0 + 0+ 2*a2 + 6*a3 * t + 12*a4 * t^2 + 20*a5 * t^3 加速度%
% Y = b0 + b1*t + b2 * t^2 + b3 * t^3 + b4 * t^4 + b5 * t^5 位 移%
% Y' = 0 + b1+ 2*b2 * t + 3*b3 * t^2 + 4*b4 * t^3 + 5*b5 * t^4 速 度%
% Y''= 0 + 0+ 2*b2 + 6*b3 * t + 12*b4 * t^2 + 20*b5 * t^3 加速度%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
t0=0;
t1=3;
%避障前的状态
before=[truck1_pos,5,0,0,0];%x,y,vx,vy,ax,ay
before_p=[1,t0,t0^2,t0^3,t0^4,t0^5];
before_v=[0,1,2*t0,3*t0^2,4*t0^3,5*t0^4];
before_a=[0,0,2,6*t0,12*t0^2,20*t0^3];
%避障后的状态
after=[truck2_pos(1),3/2*road_width,5,0,0,0];
after_p=[1,t1,t1^2,t1^3,t1^4,t1^5];
after_v=[0,1,2*t1,3*t1^2,4*t1^3,5*t1^4];
after_a=[0,0,2,6*t1,12*t1^2,20*t1^3];
%获取时间参数矩阵，避障前后 X状态 Y状态
T=[before_p;before_v;before_a;after_p;after_v;after_a];
X=[before(1,1:2:6)';after(1,1:2:6)'];
Y=[before(1,2:2:6)';after(1,2:2:6)'];
%% TA=X--->A=T^(-1)*X(A=inv(T)*X) 求系数
A=T^(-1)*X;
B=T^(-1)*Y;
%初始化存放路径点的矩阵 和 存放速度的矩阵
x=[];
y=[];
vx=[];
vy=[];
%% 循环获取各时间点的插值坐标、速度信息
for t=0:0.1:3
x=[x,[1,t,t^2,t^3,t^4,t^5]*A];
y=[y,[1,t,t^2,t^3,t^4,t^5]*B];
vx=[vx,[0,1,2*t,3*t^2,4*t^3,5*t^4]*A];
vy=[vy,[0,1,2*t,3*t^2,4*t^3,5*t^4]*B];
end
%% 绘制路径曲线
plot(x,y,'--r','linewidth',2);
title('障碍物避让--五次曲线插值法');
xlabel('纵向位移');
ylabel('横向位移');
figure(2);
plot([0:0.1:3],vx,'linewidth',2);
ylabel('纵向速度');
xlabel('时间');
figure(3);
plot([0:0.1:3],vy,'linewidth',2);
ylabel('横向速度');
xlabel('时间');
运行结果：
