基于RRT算法的最优动力学路径规划(Matlab代码实现)

 ‍个人主页:研学社的博客 

 

欢迎来到本博客❤️❤️

博主优势:博客内容尽量做到思维缜密,逻辑清晰,为了方便读者。

⛳️座右铭:行百里者,半于九十。

本文目录如下:

目录

1 概述

2 运行结果

3 Matlab代码实现

4 参考文献


1 概述

RRT是Steven M. LaValle和James J. Kuffner Jr.提出的一种通过随机构建Space Filling Tree实现对非凸高维空间快速搜索的算法。该算法可以很容易的处理包含障碍物和差分运动约束的场景,因而广泛的被应用在各种机器人的运动规划场景中。

RRT 的一个弱点是难以在有狭窄通道的环境找到路径。因为狭窄通道面积小,被碰到的概率低,找到路径需要的时间要看运气了。下图展示的例子是 RRT 应对一个人为制作的很短的狭窄通道,有时RRT很快就找到了出路,有时则一直被困在障碍物里面。 

2 运行结果

基于RRT算法的最优动力学路径规划(Matlab代码实现)_第1张图片

基于RRT算法的最优动力学路径规划(Matlab代码实现)_第2张图片

部分代码:


clc
clear all
close all
clf


%% ----------- Simulation Setup -----------%

simulation = Simulation;
setSim(simulation, 0.2, 0, 200, 0.5);


%% ----------- Environment Setup ------------%

environment = Environment;
setBound(environment,[0 200 0 200]);
dispField(environment);

%% ----------- Vehicle Setup ------------%

vehicle = Vehicle;
setTalos(vehicle);
setInitialVehicleState(vehicle,[50 50 0.25*pi 0],[0 0 0 0],[0 0 0 0]);
dispVehicle(vehicle);

%% ------------ Look Ahead Point Setup --------------%

control = Control;
setLookAheadDistance(control,vehicle);
setControlTalos(control);

%% --------------- PID Setup ---------------%

setPID(control,0.2,0.04,0);

%% --------------- Algorithm Proceeding ---------------%

% evaluateSim(simulation, environment, vehicle, control)


%% ------------ RRT Test -------------%
rrtPlanner = RRTPlanner;
setRRT(rrtPlanner,vehicle) 
TreeExpansion(rrtPlanner,environment,vehicle, control,simulation) 


%% --------------- Result Plotting ----------------%

% PlotVehicleTrajectory(vehicle);


dt = simulation.deltaT;
Speed = vehicle.hisSpeed;
Vel = vehicle.hisVel(:,1:2);
for i=1:length(vehicle.hisSpeed); VelCar(i) = norm(Vel(i,:));end
VelCmd = control.hisRefVel;

Time=0:dt:(length(vehicle.hisSpeed)-1)*dt;

figure(2)
plot(Time,VelCmd,Time,Speed);
legend('VelCmd','Speed');xlabel('Time (sec)');ylabel('Speed (m/s)');


% figure(3)
% plot(Time,Speed,Time,VelCar);
% legend('Speed','VelCar');xlabel('Time (sec)');ylabel('Speed (m/s)');


% figure(3);plot(vehicle.hisPos(:,1),vehicle.hisPos(:,2),'r');axis([-100 100 -100 100])
% figure(2);plot(Time,Vel,Time,VelCmd);legend('Vel','VelCmd');xlabel('Time (sec)');ylabel('Speed (m/s)');
% figure(3);plot(Time,Vel,Time,Speed);legend('Vel','Speed');xlabel('Time (sec)');ylabel('Speed (m/s)');

3 Matlab代码实现

4 参考文献

部分理论来源于网络,如有侵权请联系删除。

[1]樵永锋,王瀚鑫,周淑文,杨贵军.改进RRT算法的无人驾驶车辆路径规划研究[J/OL].机械设计与制造:1-8[2022-12-06].DOI:10.19356/j.cnki.1001-3997.20221103.046.

你可能感兴趣的:(路径规划/机器人,算法,路径规划,RRT算法,最优动力学,Matlab)