MATLAB 局部“自主移动机器人AMR”路径规划算法一

技术干货贩卖机

于 2025-03-26 16:59:33 发布

阅读量1.2k

点赞数 16

分类专栏： # AMR移动机器人路径规划文章标签： matlab 学习笔记动态规划

本文链接：https://blog.youkuaiyun.com/2302_77523399/article/details/146534704

版权

AMR移动机器人路径规划专栏收录该内容

4 篇文章

订阅专栏

曲线插值法

进行路径的曲线拟合，常见的曲线插值有：

多项式曲线	双圆弧段曲线	贝塞尔曲线
B样条曲线		正弦函数曲线

核心思想：基于预先构造的曲线类型，根据车辆期望达到的状态

（比如：要求车辆达到某点的速度和加速度为期望值），将此期望值作为边界条件带入曲线类型进行方程式求解

**多项式曲线（一般都是奇数，由边值条件决定）**
项数	期望点维度
三次多项式	位置和速度
五次多项式	位置、速度、加速度
七次多项式	位置、速度、加速度、加加速度（冲击度）

下述是三、五、七次多项式曲线的公式以及在模拟环境下的示意

小车图MATLAB代码

clc
clear
close all

d=3.5;
len_line=30;
W=1.75;
L=4.7;
x1=20;

t0=0;
t1=3; 
state_t0=[0,-d/2;5,0;0,0];
state_t1=[20,d/2;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;
5*t0^4 4*t0^3 3*t0^2 2*t0 1 0;
20*t0^3 12*t0^2 6*t0 1 0 0;
t1^5 t1^4 t1^3 t1^2 t1 1;
5*t1^4 4*t1^3 3*t1^2 2*t1 1 0;
20*t1^3 12*t1^2 6*t1 1 0 0];
A= T \ X;
B= T \ Y;

%将实践从t0到t1离散化，获得离散时刻的轨迹坐标
t=(t0:0.05:t1)';
path=zeros(length(t),4);
for i=1:length(t)
%纵向位置坐标
path(i,1)=[t(i)^5,t(i)^4,t(i)^3,t(i)^2,t(i),1]*A;
%横向位置坐标
path(i,2)=[t(i)^5,t(i)^4,t(i)^3,t(i)^2,t(i),1]*B;
%纵向速度
path(i,3)=[5*t(i)^4,4*t(i)^3,3*t(i)^2,2*t(i),1,0]*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');