????????在我以前的作品里有關(guān)于RRT算法的視頻和代碼，今天主要講解一下RRT*算法的原理。RRT*算法主要是在RRT算法的基礎(chǔ)上加上了重寫父節(jié)點(diǎn)和隨機(jī)重連的兩個步驟。具體的實(shí)現(xiàn)方式我想以視頻的方式向大家講解，個人感覺講解的十分詳細(xì)。視頻連接在這里，希望大家看完有任何不懂的地方直接指出。
??????? 視頻中還講述了RRT*算法的整套代碼實(shí)現(xiàn)流程以及代碼的手把手教學(xué)。代碼內(nèi)容清晰易懂，且低耦合。而且可以通過注釋掉相應(yīng)模塊后直接變?yōu)閭鹘y(tǒng)RRT算法。下面是效果圖。
????????其中左側(cè)是傳統(tǒng)RRT算法，右側(cè)是RRT*算法，通過圖片可以看出，RRT*算法相比于傳統(tǒng)RRT算法節(jié)點(diǎn)收斂性好。下面是全部代碼展示，還是挺多的。。。。。
????????下面是部分代碼，代碼獲取方式我放在了評論區(qū)（不用復(fù)制了，指定是運(yùn)行不成功，我就是單純湊字?jǐn)?shù)，混個播放量）。
首先是main函數(shù)，通過增加路徑點(diǎn)可以實(shí)現(xiàn)多目標(biāo)點(diǎn)路徑規(guī)劃。
%% 清空變量
clear;
clc;
%% 變量定義
axisStart = [0 0 0]; %空間起始坐標(biāo)
axisLWH = [1000 1000 1000]; %空間長寬高
cubeInfo.exist = 0; %長方體是否存在，若存在則置為1
cylinderInfo.exist = 0; %圓柱體是否存在，若存在則置為1
sphereInfo.exist = 0; %球是否存在，若存在則置為1
pathPoint = [0 0 0;
600 200 300;
600 600 200]; %一系列的路徑點(diǎn)
cubeInfo = CreateCubeObject(cubeInfo); %創(chuàng)建長方體障礙物信息
cylinderInfo = CreateCylinderObject(cylinderInfo); %創(chuàng)建圓柱體障礙物信息
sphereInfo = CreateSphereObject(sphereInfo); %創(chuàng)建圓柱障礙物信息
%% 畫圖
DrawPicture(cubeInfo,cylinderInfo,sphereInfo,pathPoint,axisStart,axisLWH);
%% 尋找路徑
totalPath = [];
for k1 = 1:size(pathPoint,1)-1
startPoint = pathPoint(k1,:);
goalPoint = pathPoint(k1+1,:);
[path,T] = RRTStar(axisStart,axisLWH,startPoint,goalPoint,cubeInfo,cylinderInfo,sphereInfo);
if ~isempty(path)
for k2 = 1:size(path,1)-1
line([path(k2,1),path(k2+1,1)],[path(k2,2),path(k2+1,2)],[path(k2,3),path(k2+1,3)],'LineWidth',1,'Color','red');
end
totalPath = [totalPath ;path];
end
end
pathLength = 0;
for k1 = 1:size(totalPath,1)-1
pathLength = pathLength+CalcuDistance([totalPath(k1,1) totalPath(k1,2) totalPath(k1,3)],[totalPath(k1+1,1) totalPath(k1+1,2) totalPath(k1+1,3)]);
end
disp(['路徑長度為:',num2str(pathLength)]);接下來是最核心代碼，RRTStar函數(shù)，內(nèi)部可修改一些參數(shù)實(shí)現(xiàn)不同效果。
function [path,T] = RRTStar(axisStart,axisLWH,startPoint,goalPoint,cubeInfo,cylinderInfo,sphereInfo)
%% 變量定義
iterMax = 5000; %最大迭代次數(shù)
iter = 0; %當(dāng)前迭代次數(shù)
step = 10; %步長
count = 1; %計(jì)數(shù)器
Thr = 10; %閾值
randProbability = 0.8; %隨機(jī)采樣概率，范圍0-1之間，越大隨機(jī)性越大。越小導(dǎo)向性越大，收斂快,但是容易找不到路徑
r = 5*step; %影響范圍，若大一點(diǎn)路徑規(guī)劃效果好，但是迭代慢。若小一點(diǎn)，路徑規(guī)劃效果和rrt算法越貼近
flag = 0; %路徑規(guī)劃參數(shù)，當(dāng)規(guī)劃失敗時返回0，規(guī)劃成功返回1
%%%%%%%%%%% 配置樹的信息 %%%%%%%%%%
T.x(1) = startPoint(1);
T.y(1) = startPoint(2);
T.z(1) = startPoint(3);
T.pre(1) = 0;
T.cost(1) = 0;
path = [];
while iter<=iterMax
%% 迭代次數(shù)加1
iter = iter+1;
%% 空間中隨機(jī)采樣
randCoor = RandSample(axisStart,axisLWH,goalPoint,randProbability);
%% 尋找樹上最近的點(diǎn)
[nearestCoor,parentIndex] = FindNearstPoint(T,randCoor);
%% 根據(jù)指定步長擴(kuò)展新的點(diǎn)
newCoor = ExpandPoint(nearestCoor,randCoor,step);
%% 重寫
% parentIndex = RewriteFunction(T,newCoor,r,parentIndex);
%% 碰撞檢測
A = [T.x(parentIndex),T.y(parentIndex),T.z(parentIndex)];
B = newCoor;
collisionFlag = CollisionDetection(cubeInfo,cylinderInfo,sphereInfo,A,B,step);
if collisionFlag
continue;
end
%% 將新點(diǎn)插入進(jìn)來
count = count + 1;
T.x(count) = newCoor(1);
T.y(count) = newCoor(2);
T.z(count) = newCoor(3);
T.pre(count) = parentIndex;
T.cost(count) = CalcuDistance(A,B)+T.cost(parentIndex);
line([A(1),B(1)],[A(2),B(2)],[A(3),B(3)],'LineWidth',1); % 要是想只畫最終的路徑圖，不畫擴(kuò)展圖就把這行注釋掉
pause(0.01) %不想看動畫，就把這行注釋
%% 隨機(jī)重連
% T = RandRelink(T,newCoor,cubeInfo,cylinderInfo,sphereInfo,step,r);
if CalcuDistance(newCoor,goalPoint)<Thr
flag = 1;
break;
end
end
%% 路徑規(guī)劃失敗直接返回
if ~flag
disp('路徑規(guī)劃失敗');
return;
else
disp('路徑規(guī)劃成功');
end
%% 尋找路徑
path = FindPath(T,startPoint,goalPoint);接下來是RRT*算法核心部分，重寫和隨機(jī)重連部分
function parentIndex = RewriteFunction(T,newCoor,r,parentIndex)
%% 重寫函數(shù)，將新點(diǎn)newCoor重新連接到代價最小
% 下面是尋找到潛在的父節(jié)點(diǎn)，在變量potentialParent里
potentialParent = -ones(1,size(T.x,2));
count = 1;
for k1 = 1:size(T.x,2)
if CalcuDistance(newCoor,[T.x(k1),T.y(k1),T.z(k1)])<r
potentialParent(count) = k1;
count = count+1;
end
end
potentialParent(potentialParent==-1)=[];
%迭代尋找最小代價，并重新選擇新節(jié)點(diǎn)newCoor的父節(jié)點(diǎn)，并把parentIndex改為新父節(jié)點(diǎn)對應(yīng)的索引
for k2 = 1:size(potentialParent,2)
% potentialParent(k2)潛在父節(jié)點(diǎn)的序號
pp = [T.x(potentialParent(k2)),T.y(potentialParent(k2)),T.z(potentialParent(k2))];
p = [T.x(parentIndex),T.y(parentIndex),T.z(parentIndex)];
if (CalcuDistance(pp,newCoor)+T.cost(potentialParent(k2)))<(CalcuDistance(p,newCoor)+T.cost(parentIndex))
parentIndex = potentialParent(k2);
end
endfunction T = RandRelink(T,newCoor,cubeInfo,cylinderInfo,sphereInfo,step,r)
%% 隨機(jī)重連，將新節(jié)點(diǎn)newCoor周圍節(jié)點(diǎn)的父節(jié)點(diǎn)嘗試改為新節(jié)點(diǎn)，若代價小于原來的代價值，則確認(rèn)更改
% 尋找需要需要修改父節(jié)點(diǎn)的節(jié)點(diǎn)放入potentialParent里面。
potentialParent = -ones(1,size(T.x,2));
count = 1;
for k1 = 1:size(T.x,2)
if CalcuDistance(newCoor,[T.x(k1),T.y(k1),T.z(k1)])<r
potentialParent(count) = k1;
count = count+1;
end
end
potentialParent(potentialParent==-1)=[];
% 找到新節(jié)點(diǎn)的父節(jié)點(diǎn)集合，存在變量Index里
Index = -ones(1,size(T.x,2));
index = T.pre(end);
count = 1;
while T.pre(index)~=0
Index(count) = index;
index = T.pre(index);
count = count+1;
end
Index(Index==-1) = [];
potentialParent(ismember(potentialParent,Index)==1) = []; %這行的意思是把需要修改父節(jié)點(diǎn)的節(jié)點(diǎn)集合，剔除新節(jié)點(diǎn)的父節(jié)點(diǎn)，以免出現(xiàn)環(huán)
% 根據(jù)代價決定是否修改父節(jié)點(diǎn)
for k2 = 1:size(potentialParent,2)
pp = [T.x(potentialParent(k2)),T.y(potentialParent(k2)),T.z(potentialParent(k2))];
if T.cost(potentialParent(k2))>( T.cost(end)+ CalcuDistance(pp,newCoor))
if ~CollisionDetection(cubeInfo,cylinderInfo,sphereInfo,pp,newCoor,step)
T.pre(potentialParent(k2)) = size(T.x,2)+1;
T.cost(potentialParent(k2)) = CalcuDistance(newCoor,pp);
end
end
end隨機(jī)采樣函數(shù)
function randCoor = RandSample(axisStart,axisLWH,goalPoint,randProbability)
%% 隨機(jī)采樣，當(dāng)小于某一概率時，采用隨機(jī)采樣，其他情況直接將目標(biāo)點(diǎn)作為采樣點(diǎn)
if rand<randProbability
randX = rand*axisLWH(1)+axisStart(1);
randY = rand*axisLWH(2)+axisStart(2);
randZ = rand*axisLWH(3)+axisStart(3);
randCoor = [randX,randY,randZ];
else
randCoor = goalPoint;
end?找到最近節(jié)點(diǎn)函數(shù)
function [nearestCoor,parentIndex] = FindNearstPoint(T,randCoor)
%% 遍歷整個樹，尋找距離隨機(jī)點(diǎn)randCoor最近的點(diǎn)，標(biāo)記為nearestCoor
tempDis = inf;
parentIndex = -1;
for k1 = 1:size(T.x,2)
dis = CalcuDistance(randCoor,[T.x(k1),T.y(k1),T.z(k1)]);
if dis<tempDis
tempDis = dis;
parentIndex = k1;
end
end
nearestCoor = [T.x(parentIndex),T.y(parentIndex),T.z(parentIndex)];?根據(jù)步長擴(kuò)展點(diǎn)文章來源：http://www.zghlxwxcb.cn/news/detail-854247.html
function newCoor = ExpandPoint(nearCoor,randCoor,step)
%% 按照指定步長，隨機(jī)點(diǎn)方向擴(kuò)展新的點(diǎn)，命名為newCoor。這里按步長擴(kuò)展點(diǎn)采用球坐標(biāo)與直角坐標(biāo)的轉(zhuǎn)化方式
deltaX = randCoor(1)-nearCoor(1);
deltaY = randCoor(2)-nearCoor(2);
deltaZ = randCoor(3)-nearCoor(3);
r = sqrt(deltaX^2+deltaY^2+deltaZ^2);
fai = atan2(deltaY,deltaX);
theta = acos(deltaZ/r);
x = step*sin(theta)*cos(fai);
y = step*sin(theta)*sin(fai);
z = step*cos(theta);
newCoor = [x+nearCoor(1),y+nearCoor(2),z+nearCoor(3)];?這些是代碼中最核心的部分，因?yàn)榇a太多，不在這里展示過多，具體獲取方式可以看我b站視頻（up主名字叫“-不禿頭-”）。代碼實(shí)際上是不免費(fèi)的，是因?yàn)橛腥四弥议_源代碼去賣（很難理解）。但是如果你要是真想學(xué)代碼，幫我公眾號和b站點(diǎn)個關(guān)注，我也能免費(fèi)給你，但你不能拿去賺錢，就當(dāng)我謝謝你了。文章來源地址http://www.zghlxwxcb.cn/news/detail-854247.html
到了這里，關(guān)于手把手教學(xué)RRT*（RRTSTAR）三維算法MATLAB仿真（代碼可直接運(yùn)行，視頻手把手教學(xué)）的文章就介紹完了。如果您還想了解更多內(nèi)容，請?jiān)谟疑辖撬阉鱐OY模板網(wǎng)以前的文章或繼續(xù)瀏覽下面的相關(guān)文章，希望大家以后多多支持TOY模板網(wǎng)！
