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

%%??%%%%?參數?%%%%%

tic;??%?保存當前時間
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??%展示圖像，并創建帶有尖角的矩形邊框
RRTree=double（[source?-1]）;?%?RRT?從起點開始（索引為-1），經過的結點和索引
failedAttempts=0;??%?已經嘗試失敗的次數
counter=0;??%?循環計數
pathFound=false;??%?是否找到路徑的flag
while?failedAttempts<=maxFailedAttempts??%?RRT循環
????if?rand?????????sample=rand（12）?.*?size（map）;???%?50%幾率隨機采點
????else
????????sample=goal;?%?50%幾率向目標前進
????end
????
????%?每一個分支都會繼續分支
????[A?I]=min（?distanceCost（RRTree（:1:2）sample）?[]1）;?%?發現結點和隨機采樣點最小距離的一行，并返回對應索引[]1可以去掉
????closestNode?=?RRTree（I（1）1:2）;?%樹結點最近點坐標，最近點可能多個（1）不可取
????theta=atan2（sample（1）-closestNode（1）sample（2）-closestNode（2））;??%?產生新結點的方向
????newPoint?=?double（int32（closestNode（1:2）?+?stepsize?*?[sin（theta）??cos（theta）]））;??%?產生新結點，先計算縱坐標，再計算橫坐標
????
????if?~checkPath（closestNode（1:2）?newPoint?map）?%?檢測最近結點到新結點的路徑是否可行
????????failedAttempts=failedAttempts+1;
????????continue;
????end
????if?distanceCost（newPointgoal）????[A?I2]=min（?distanceCost（RRTree（:1:2）newPoint）?[]1）;?%?檢測檢點是否已經存在樹結點中
????if?distanceCost（newPointRRTree（I2（1）1:2））????RRTree=[RRTree;newPoint?I（1）];?%?將新結點介入到如結點中
????failedAttempts=0;
????%?每擴展一個新結點，畫一條線
????if?display?
????????line（[closestNode（2）;newPoint（2）][closestNode（1）;newPoint（1）]）;
????????counter=counter+1;M（counter）=getframe;
????end
end
%?補充最后一個新結點和終點的連線
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

%%?重現原軌跡
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）;???%?打印運行時間toc和路徑長度
imshow（map）;rectangle（‘position‘[1?1?size（map）-1]‘edgecolor‘‘k‘）;
line（path（:2）path（:1））;

?屬性????????????大小?????日期????時間???名稱
-----------?---------??----------?-----??----

?????文件???????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