%%?簡單的RRT算法
%%?初始化
map=im2bw（imread（‘map2.bmp‘））;?%?bmp無損壓縮圖像500x500im2bw把灰度圖轉(zhuǎn)換成二值圖像01
source=[10?10];?%?起始點(diǎn)位置
goal=[490?490];?%?目標(biāo)點(diǎn)位置
stepsize=20;?%?RRT每步步長
disTh=20;?%?直到qnearest和目標(biāo)點(diǎn)qgaol距離小于一個(gè)閾值
maxFailedAttempts?=?10000;??%?最大嘗試次數(shù)
display=true;?%?RRT是否展示

%%??%%%%?參數(shù)?%%%%%

tic;??%?保存當(dāng)前時(shí)間
if?~feasiblePoint（sourcemap）?error（‘source?lies?on?an?obstacle?or?outside?map‘）;?end
if?~feasiblePoint（goalmap）?error（‘goal?lies?on?an?obstacle?or?outside?map‘）;?end
if?display?imshow（map）;rectangle（‘position‘[1?1?size（map）-1]‘edgecolor‘‘k‘）;?end??%展示圖像，并創(chuàng)建帶有尖角的矩形邊框
RRTree=double（[source?-1]）;?%?RRT?從起點(diǎn)開始（索引為-1），經(jīng)過的結(jié)點(diǎn)和索引
failedAttempts=0;??%?已經(jīng)嘗試失敗的次數(shù)
counter=0;??%?循環(huán)計(jì)數(shù)
pathFound=false;??%?是否找到路徑的flag
while?failedAttempts<=maxFailedAttempts??%?RRT循環(huán)
????if?rand?????????sample=rand（12）?.*?size（map）;???%?50%幾率隨機(jī)采點(diǎn)
????else
????????sample=goal;?%?50%幾率向目標(biāo)前進(jìn)
????end
????
????%?每一個(gè)分支都會繼續(xù)分支
????[A?I]=min（?distanceCost（RRTree（:1:2）sample）?[]1）;?%?發(fā)現(xiàn)結(jié)點(diǎn)和隨機(jī)采樣點(diǎn)最小距離的一行，并返回對應(yīng)索引[]1可以去掉
????closestNode?=?RRTree（I（1）1:2）;?%樹結(jié)點(diǎn)最近點(diǎn)坐標(biāo)，最近點(diǎn)可能多個(gè)（1）不可取
????theta=atan2（sample（1）-closestNode（1）sample（2）-closestNode（2））;??%?產(chǎn)生新結(jié)點(diǎn)的方向
????newPoint?=?double（int32（closestNode（1:2）?+?stepsize?*?[sin（theta）??cos（theta）]））;??%?產(chǎn)生新結(jié)點(diǎn)，先計(jì)算縱坐標(biāo)，再計(jì)算橫坐標(biāo)
????
????if?~checkPath（closestNode（1:2）?newPoint?map）?%?檢測最近結(jié)點(diǎn)到新結(jié)點(diǎn)的路徑是否可行
????????failedAttempts=failedAttempts+1;
????????continue;
????end
????if?distanceCost（newPointgoal）????[A?I2]=min（?distanceCost（RRTree（:1:2）newPoint）?[]1）;?%?檢測檢點(diǎn)是否已經(jīng)存在樹結(jié)點(diǎn)中
????if?distanceCost（newPointRRTree（I2（1）1:2））????RRTree=[RRTree;newPoint?I（1）];?%?將新結(jié)點(diǎn)介入到如結(jié)點(diǎn)中
????failedAttempts=0;
????%?每擴(kuò)展一個(gè)新結(jié)點(diǎn)，畫一條線
????if?display?
????????line（[closestNode（2）;newPoint（2）][closestNode（1）;newPoint（1）]）;
????????counter=counter+1;M（counter）=getframe;
????end
end
%?補(bǔ)充最后一個(gè)新結(jié)點(diǎn)和終點(diǎn)的連線
if?display?&&?pathFound?
????line（[closestNode（2）;goal（2）][closestNode（1）;goal（1）]）;
????counter=counter+1;M（counter）=getframe;
end
if?display?
????disp（‘click/press?any?key‘）;
????waitforbuttonpress;?
end
if?~pathFound?error（‘no?path?found.?maximum?attempts?reached‘）;?end

%%?重現(xiàn)原軌跡
path=[goal];
prev=I（1）;
while?prev>0
????path=[RRTree（prev1:2）;path];
????prev=RRTree（prev3）;
end
pathLength=0;
for?i=1:length（path）-1?pathLength=pathLength+distanceCost（path（i1:2）path（i+11:2））;?end
fprintf（‘processing?time=%d?\nPath?Length=%d?\n\n‘?tocpathLength）;???%?打印運(yùn)行時(shí)間toc和路徑長度
imshow（map）;rectangle（‘position‘[1?1?size（map）-1]‘edgecolor‘‘k‘）;
line（path（:2）path（:1））;

?屬性????????????大小?????日期????時(shí)間???名稱
-----------?---------??----------?-----??----

?????文件???????3037??2019-01-07?20:41??RRT\RRT\astart.m

?????文件????????620??2019-01-07?16:36??RRT\RRT\checkPath.m

?????文件????????123??2019-01-07?15:53??RRT\RRT\distanceCost.m

?????文件????????258??2019-01-07?15:11??RRT\RRT\feasiblePoint.m

?????文件?????251078??2014-06-06?15:35??RRT\RRT\map1.bmp

?????文件?????251078??2014-06-06?15:35??RRT\RRT\map2.bmp

?????文件?????251078??2014-06-06?15:35??RRT\RRT\map3.bmp

?????文件?????251078??2014-06-06?15:35??RRT\RRT\map4.bmp

?????文件?????251078??2014-06-06?15:35??RRT\RRT\map5.bmp

?????文件?????210658??2014-06-06?15:35??RRT\RRT\RRT.pdf

?????目錄??????????0??2019-01-07?20:55??RRT\RRT

?????目錄??????????0??2019-01-07?10:36??RRT

-----------?---------??----------?-----??----

??????????????1470086????????????????????12