【三维路径规划】基于快速扩展随机树（RRT）实现无人机三维路径规划算法附matlab代码

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;endaxis 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 TimeRRTree = 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; endif ~pathFound, error('no path found. maximum attempts reached'); end​%% retrieve path from parent informationpath = 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 lengthfprintf('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;endaxis equalhold 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代码问题可私信交流。

部分理论引用网络文献，若有侵权联系博主删除。