RRT算法簡單介紹

1. RRT算法定義

RRT（Rapidly-Exploring Random Tree）算法是一種基于采樣的路徑規劃算法，常用于移動機器人路徑規劃，適合解決高維空間和復雜約束下的路徑規劃問題。基本思想是以產生隨機點的方式通過一個步長向目標點搜索前進，有效躲避障礙物，避免路徑陷入局部極小值，收斂速度快。本文通過matlab實現RRT算法，解決二維平面的路徑規劃問題。
?

2. RRT算法基本步驟

1）確定起點start和終止點goal；

2）在空間中隨機生成新的點r(50%為隨機點，50%為目標點，目的是增強RRT向goal點生成的導向性)；

3）判斷點r與軌跡樹中哪一個節點的歐氏距離最小，記該點為最近父節點closetNode；

4）沿生長向量方向（最近父節點closetNode指向隨機點r的方向）按照步長stepSize生成新的子節點newNode；

5）碰撞檢測：檢測closetNode到newNode的連線是否會與障礙物發生碰撞。若是，返回步驟2，重新生成隨機點；若否，則將newNode添加到軌跡樹中。

6）檢測是否到達goal附近。若是，結束搜索；若否，返回步驟2繼續搜索。
?

三維軌跡Matlab仿真效果：

障礙物我們只設置了球形障礙物，也可以設置成立方體、圓柱體等等。

實施效果1：

? ? ?

?左：RRT隨機樹? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ?右：貪心算法優化后的隨機樹

?優化前，RRT樹有80個節點，并且路徑并不平滑。優化后，隨機樹只有三個點，路徑只有兩條直線組成。當然還可以繼續對路徑進行平滑，本文沒有再繼續做這些工作了。

?實施效果2：

?

以下為RRT算法的Matlab實現代碼：

mian函數：

%created by MW %date: 2022/5/25 %func: RRT avoiding obstacleclc; clear;%% 創建并繪制障礙物 circleCenter = [100 200 100;200 700 100;200 500 500;700 700 300;900 200 100]; radius = [100;100;200;200;300]; %繪制球形障礙物 figure(1); [x, y, z] = sphere; %創建一個坐標在原點，半徑為1的標準圓，用于繪制自定義的圓 for i = 1: length(radius)mesh(radius(i)*x + circleCenter(i,1), radius(i)*y + circleCenter(i,2), radius(i)*z + circleCenter(i,3));hold on; end axis equal;%% 創建初始位置和目標位置并繪制 start = [0 0 0]; goal = [700 800 1000]; hold on; scatter3(start(1),start(2),start(3),'filled','g'); scatter3(goal(1),goal(2),goal(3),'filled','b');%% 圖片標注 text(start(1),start(2),start(3),'起點'); text(goal(1),goal(2),goal(3),'終點'); view(3); grid on; axis equal; axis([0 1000 0 1000 0 1000]); xlabel('x'); ylabel('y'); zlabel('z');%% RRT方法生成避障軌跡 path = RRT(start,goal,radius,circleCenter);%% 貪心算法優化RRT軌跡 newPath = GreedyOptimize(path,radius,circleCenter);figure(2); %繪制球形障礙物 [x, y, z] = sphere; %創建一個坐標在原點，半徑為1的標準圓，用于繪制自定義的圓 for i = 1: length(radius)mesh(radius(i)*x + circleCenter(i,1), radius(i)*y + circleCenter(i,2), radius(i)*z + circleCenter(i,3));hold on; end axis equal;%繪制原RRT樹 plot3(path(:,1), path(:,2), path(:,3),'LineWidth',1,'Color','r');%繪制優化后的RRT樹 for k1 =1: length(newPath)point = newPath(k1,:);scatter3(point(1),point(2),point(3),'filled','k'); end plot3(newPath(:,1), newPath(:,2), newPath(:,3),'LineWidth',2,'Color','y');%圖片標注 text(start(1),start(2),start(3),'起點'); text(goal(1),goal(2),goal(3),'終點'); view(3); grid on; axis equal; axis([0 1000 0 1000 0 1000]); xlabel('x'); ylabel('y'); zlabel('z');

RRT算法：

function path = RRT(start,goal,radius,circleCenter) %% 定義RRT參數 stepSize = 20; %步長 maxIterTimes = 5000; %最大迭代步數 iterTime = 0; %當前迭代次數 threshold = 20; %閾值 searchSize = [1000 1000 1000]; %空間尺寸 RRTree = double([start -1]); %創建RRT樹,共4列。前3列為節點坐標，第4列為當前節點的父節點的索引。初始點的索引為-1%多個點到某一點歐式距離計算方法 calcDis = @(a,b) sqrt((b(:,1)-a(1,1)).^2 + (b(:,2)-a(1,2)).^2 + (b(:,3)-a(1,3)).^2);%% 尋找RRT路徑 tic % tic-toc函數，用于計時，記錄完成整個RRT樹的運行時間 pathFound = false; %標志物，記錄是否正確找到避障路徑 while iterTime <= maxIterTimesiterTime = iterTime +1;%step1 - 生成隨機點%為了提高RRT擴展的導向性，以50%的概率在空間中隨機生成生成新的隨機點，50%的概率以目標點為新的隨機點if rand < 0.5 sample = rand(1,3) .* searchSize + start;elsesample = goal;end%step2 - 尋找樹上最近父節點[val,nearIndex] = min(calcDis(sample, RRTree(:,1:3)),[],1); %計算樹上每個節點到隨機點的歐氏距離，并返回最短距離的值和indexclosestNode = RRTree(nearIndex,1:3);%step3 - 沿生長向量方向按照步長生成新的子節點growVec = sample - closestNode;growVec = growVec/sqrt(sum(growVec.^2));newPoint = closestNode + growVec*stepSize;%step4 - 碰撞檢測feasible = collisionDetec(newPoint,closestNode,radius,circleCenter);if ~feasiblecontinue; %如果發生碰撞，則重新尋找隨機點 end%為樹添加新節點RRTree = [RRTree; newPoint nearIndex];plot3([closestNode(1) newPoint(1)],[closestNode(2) newPoint(2)],[closestNode(3) newPoint(3)],'LineWidth',1,'Color','b');pause(0.01);%檢測是否到達goal附近if sqrt(sum((newPoint - goal).^2)) <= thresholdpathFound = true;break; %如果節點已到達goal附近，則結束搜尋endend%搜索結束后如果搜索失敗，顯示錯誤信息 if ~pathFounddisp('no path found. maximum attempts reached'); endtoc%% 繪制回溯路徑 path = goal; lastNode = nearIndex; %父節點索引，這里的nearIndex是goal的父節點索引 while lastNode >= 0path =[RRTree(lastNode,1:3); path]; %將當前節點的上一個節點添加進回溯路徑中lastNode = RRTree(lastNode, 4); %更新父節點索引 end plot3(path(:,1), path(:,2), path(:,3),'LineWidth',1,'Color','r');end

碰撞檢測算法：

線段檢測：

%新的生長向量是否發生碰撞 function feasible = collisionDetec(newPoint,closestNode,radius,circleCenter) feasible = true; checkVec = newPoint - closestNode; %檢測向量 %將檢測向量以0.5為步長等比均分為無數個檢測點，檢測每個檢測點是否與球發生碰撞 for i = 0:0.5:sqrt(sum(checkVec.^2))checkPoint = closestNode + i.*(checkVec/sqrt(sum(checkVec.^2))); %生成檢測點checkPointFeasible = pointCollisionCheck(checkPoint,radius,circleCenter);if ~checkPointFeasiblefeasible = false;break;end end end

點檢測：

%監測點是否發生碰撞 function pointFeasible = pointCollisionCheck(checkPoint,radius,circleCenter) pointFeasible = true; for s = 1:length(radius)if sqrt(sum((checkPoint - circleCenter(s,:)).^2)) <= radius(s)pointFeasible = false;break;end end end

貪心算法簡單介紹

1. 貪心算法定義：

貪心算法，是指在對問題求解時，總是做出再當前看來是最好的選擇。也就是說，不從整體最優上加以考慮，他所做出的僅是某種意義上的局部最優解。

貪心算法沒有固定算法框架，算法設計的關鍵是貪心策略的選擇。必須注意的是，貪心算法不是對所有問題都能得到整體最優解，選擇的貪心策略必須具備無后效性，即某個狀態以后的過程不會影響以前的狀態，只與當前狀態有關。

2. 貪心算法基本步驟：

步驟1：從某個初始解出發；
步驟2：采用迭代的過程，當可以向目標前進一步時，就根據局部最優策略，得到一部分解，縮小問題規模；
步驟3：將所有解綜合起來。

本問題下的貪心算法：

1）連接目標點和初始點。

2）檢測是否發生碰撞。

3）若是，表明從初始點到該點中間不能被省略，沿目標點往上回溯到上一級父節點（上一級父節點更新為新的目標點），回到步驟1；若否，表明從初始點到該點中間的所有節點均可被省略，將該點添加到新的隨機樹中，并將該點更新為新的初始點，目標點復位到最后一個節點，回到步驟1.

4）檢測直到start更新至goal結束。

function newPath = GreedyOptimize(path,radius,circleCenter)startIndex = 1; goalIndex = length(path(:,1)); detectTimes = length(path(:,1)) - 1; %檢測次數 newPath = [path(startIndex,:)]; %添加初始位置while detectTimes >0;detectTimes = detectTimes-1;start = path(startIndex,:);goal = path(goalIndex,:);%碰撞檢測feasible = collisionDetec(goal,start,radius,circleCenter);if ~feasiblegoalIndex = goalIndex - 1; %若碰撞，index減1，繼續向上探索父節點continue; elsenewPath = [newPath; goal]; %若未發生碰撞，表示找到一個最優節點，則從該節點往上的所有父節點均可省略，將該節點添加至路徑中detectTimes = length(path(:,1)) - goalIndex; %檢測次數更位startIndex = goalIndex; %將當前節點索引更新為新的樹的起點goalIndex = length(path(:,1)); %將終點索引復位endendnewPath = [newPath; path(end,:)]; %添加目標位置end

總結

以上是生活随笔為你收集整理的Matlab RRT算法三维轨迹规划及贪心算法轨迹优化的全部內容，希望文章能夠幫你解決所遇到的問題。

如果覺得生活随笔網站內容還不錯，歡迎將生活随笔推薦給好友。