1 简介
移动机器人运动规划技术是自主移动机器人导航的核心技术之一,而路径规划技术是导航技术研究的一个关键课题.路径规划的任务是:依据一定的评价准则(如距离最短,时间最短,工作代价最小等等),在一个存在障碍物的工作环境内,寻求一条从初始点开始到目标点结束的较优的无碰撞路径.本文旨在结合实际环境基于快速扩展随机树(Rapidly-Exploring Random Tree, RRT)算法实现自主移动机器人的路径规划。
2 部分代码
%% 绘制障碍物(以球为例,主要是方便计算)
x0=100; y0=100; z0=100;%球心
circleCenter = [100,100,100;50,50,50;100,40,60;150,100,100;60,130,50];
r=[50;20;20;15;15];%半径
%下面开始画
figure(1);
[x,y,z]=sphere;
for i = 1:length(circleCenter(:,1))
mesh(r(i)*x+circleCenter(i,1),r(i)*y+circleCenter(i,2),r(i)*z+circleCenter(i,3));hold on;
end
axis equal
%% 参数
source=[10 10 10];
goal=[150 150 150];
stepsize = 10;
threshold = 10;
maxFailedAttempts = 10000;
display = true;
searchSize = [250 250 250]; %探索空间六面体
%% 绘制起点和终点
hold on;
scatter3(source(1),source(2),source(3),'filled','g');
scatter3(goal(1),goal(2),goal(3),'filled','b');
tic; % tic-toc: Functions for Elapsed Time
RRTree = double([source -1]);
failedAttempts = 0;
pathFound = false;
%% 循环
while failedAttempts <= maxFailedAttempts % loop to grow RRTs
%% chooses a random configuration
if rand < 0.5
sample = rand(1,3) .* searchSize; % random sample
else
sample = goal; % sample taken as goal to bias tree generation to goal
end
%% selects the node in the RRT tree that is closest to qrand
[A, I] = min( distanceCost3(RRTree(:,1:3),sample) ,[],1); % find the minimum value of each column
closestNode = RRTree(I(1),1:3);
%% moving from qnearest an incremental distance in the direction of qrand
movingVec = [sample(1)-closestNode(1),sample(2)-closestNode(2),sample(3)-closestNode(3)];
movingVec = movingVec/sqrt(sum(movingVec.^2)); %单位化
newPoint = closestNode + stepsize * movingVec;
if ~checkPath3(closestNode, newPoint, circleCenter,r) % if extension of closest node in tree to the new point is feasible
failedAttempts = failedAttempts + 1;
continue;
end
if distanceCost3(newPoint,goal) < threshold, pathFound = true; break; end % goal reached
[A, I2] = min( distanceCost3(RRTree(:,1:3),newPoint) ,[],1); % check if new node is not already pre-existing in the tree
if distanceCost3(newPoint,RRTree(I2(1),1:3)) < threshold, failedAttempts = failedAttempts + 1; continue; end
RRTree = [RRTree; newPoint I(1)]; % add node
failedAttempts = 0;
if display, plot3([closestNode(1);newPoint(1)],[closestNode(2);newPoint(2)],[closestNode(3);newPoint(3)],'LineWidth',1); end
pause(0.05);
end
if display && pathFound, plot3([closestNode(1);goal(1)],[closestNode(2);goal(2)],[closestNode(3);goal(3)]); end
if display, disp('click/press any key'); waitforbuttonpress; end
if ~pathFound, error('no path found. maximum attempts reached'); end
%% retrieve path from parent information
path = goal;
prev = I(1);
while prev > 0
path = [RRTree(prev,1:3); path];
prev = RRTree(prev,4);
end
pathLength = 0;
for i=1:length(path(:,1))-1, pathLength = pathLength + distanceCost3(path(i,1:3),path(i+1,1:3)); end % calculate path length
fprintf('processing time=%d \nPath Length=%d \n\n', toc, pathLength);
figure(2)
for i = 1:length(circleCenter(:,1))
mesh(r(i)*x+circleCenter(i,1),r(i)*y+circleCenter(i,2),r(i)*z+circleCenter(i,3));hold on;
end
axis equal
hold on;
scatter3(source(1),source(2),source(3),'filled','g');
scatter3(goal(1),goal(2),goal(3),'filled','b');
plot3(path(:,1),path(:,2),path(:,3),'LineWidth',2,'color','r');
3 仿真结果
4 参考文献
[1]马蓉. 基于改进RRT算法的无人机航路规划与跟踪方法研究[J]. 导航定位与授时, 2020, 7(1):6.
博主简介:擅长智能优化算法、神经网络预测、信号处理、元胞自动机、图像处理、路径规划、无人机等多种领域的Matlab仿真,相关matlab代码问题可私信交流。
部分理论引用网络文献,若有侵权联系博主删除。