RRT算法簡單介紹

1. RRT算法定義

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

2. RRT算法基本步驟

1）確定起點(diǎn)start和終止點(diǎn)goal；

2）在空間中隨機(jī)生成新的點(diǎn)r(50%為隨機(jī)點(diǎn)，50%為目標(biāo)點(diǎn)，目的是增強(qiáng)RRT向goal點(diǎn)生成的導(dǎo)向性)；

3）判斷點(diǎn)r與軌跡樹中哪一個(gè)節(jié)點(diǎn)的歐氏距離最小，記該點(diǎn)為最近父節(jié)點(diǎn)closetNode；

4）沿生長向量方向（最近父節(jié)點(diǎn)closetNode指向隨機(jī)點(diǎn)r的方向）按照步長stepSize生成新的子節(jié)點(diǎn)newNode；

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

6）檢測是否到達(dá)goal附近。若是，結(jié)束搜索；若否，返回步驟2繼續(xù)搜索。
?

三維軌跡Matlab仿真效果：

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

實(shí)施效果1：

? ? ?

?左：RRT隨機(jī)樹? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ?右：貪心算法優(yōu)化后的隨機(jī)樹

?優(yōu)化前，RRT樹有80個(gè)節(jié)點(diǎn)，并且路徑并不平滑。優(yōu)化后，隨機(jī)樹只有三個(gè)點(diǎn)，路徑只有兩條直線組成。當(dāng)然還可以繼續(xù)對路徑進(jìn)行平滑，本文沒有再繼續(xù)做這些工作了。

?實(shí)施效果2：

?

以下為RRT算法的Matlab實(shí)現(xiàn)代碼：

mian函數(shù)：

%created by MW %date: 2022/5/25 %func: RRT avoiding obstacleclc; clear;%% 創(chuàng)建并繪制障礙物 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; %創(chuàng)建一個(gè)坐標(biāo)在原點(diǎn)，半徑為1的標(biāo)準(zhǔn)圓，用于繪制自定義的圓 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;%% 創(chuàng)建初始位置和目標(biāo)位置并繪制 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');%% 圖片標(biāo)注 text(start(1),start(2),start(3),'起點(diǎn)'); text(goal(1),goal(2),goal(3),'終點(diǎn)'); 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);%% 貪心算法優(yōu)化RRT軌跡 newPath = GreedyOptimize(path,radius,circleCenter);figure(2); %繪制球形障礙物 [x, y, z] = sphere; %創(chuàng)建一個(gè)坐標(biāo)在原點(diǎn)，半徑為1的標(biāo)準(zhǔn)圓，用于繪制自定義的圓 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');%繪制優(yōu)化后的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');%圖片標(biāo)注 text(start(1),start(2),start(3),'起點(diǎn)'); text(goal(1),goal(2),goal(3),'終點(diǎn)'); 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參數(shù) stepSize = 20; %步長 maxIterTimes = 5000; %最大迭代步數(shù) iterTime = 0; %當(dāng)前迭代次數(shù) threshold = 20; %閾值 searchSize = [1000 1000 1000]; %空間尺寸 RRTree = double([start -1]); %創(chuàng)建RRT樹,共4列。前3列為節(jié)點(diǎn)坐標(biāo)，第4列為當(dāng)前節(jié)點(diǎn)的父節(jié)點(diǎn)的索引。初始點(diǎn)的索引為-1%多個(gè)點(diǎn)到某一點(diǎn)歐式距離計(jì)算方法 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函數(shù)，用于計(jì)時(shí)，記錄完成整個(gè)RRT樹的運(yùn)行時(shí)間 pathFound = false; %標(biāo)志物，記錄是否正確找到避障路徑 while iterTime <= maxIterTimesiterTime = iterTime +1;%step1 - 生成隨機(jī)點(diǎn)%為了提高RRT擴(kuò)展的導(dǎo)向性，以50%的概率在空間中隨機(jī)生成生成新的隨機(jī)點(diǎn)，50%的概率以目標(biāo)點(diǎn)為新的隨機(jī)點(diǎn)if rand < 0.5 sample = rand(1,3) .* searchSize + start;elsesample = goal;end%step2 - 尋找樹上最近父節(jié)點(diǎn)[val,nearIndex] = min(calcDis(sample, RRTree(:,1:3)),[],1); %計(jì)算樹上每個(gè)節(jié)點(diǎn)到隨機(jī)點(diǎn)的歐氏距離，并返回最短距離的值和indexclosestNode = RRTree(nearIndex,1:3);%step3 - 沿生長向量方向按照步長生成新的子節(jié)點(diǎn)growVec = sample - closestNode;growVec = growVec/sqrt(sum(growVec.^2));newPoint = closestNode + growVec*stepSize;%step4 - 碰撞檢測feasible = collisionDetec(newPoint,closestNode,radius,circleCenter);if ~feasiblecontinue; %如果發(fā)生碰撞，則重新尋找隨機(jī)點(diǎn) end%為樹添加新節(jié)點(diǎn)RRTree = [RRTree; newPoint nearIndex];plot3([closestNode(1) newPoint(1)],[closestNode(2) newPoint(2)],[closestNode(3) newPoint(3)],'LineWidth',1,'Color','b');pause(0.01);%檢測是否到達(dá)goal附近if sqrt(sum((newPoint - goal).^2)) <= thresholdpathFound = true;break; %如果節(jié)點(diǎn)已到達(dá)goal附近，則結(jié)束搜尋endend%搜索結(jié)束后如果搜索失敗，顯示錯(cuò)誤信息 if ~pathFounddisp('no path found. maximum attempts reached'); endtoc%% 繪制回溯路徑 path = goal; lastNode = nearIndex; %父節(jié)點(diǎn)索引，這里的nearIndex是goal的父節(jié)點(diǎn)索引 while lastNode >= 0path =[RRTree(lastNode,1:3); path]; %將當(dāng)前節(jié)點(diǎn)的上一個(gè)節(jié)點(diǎn)添加進(jìn)回溯路徑中l(wèi)astNode = RRTree(lastNode, 4); %更新父節(jié)點(diǎn)索引 end plot3(path(:,1), path(:,2), path(:,3),'LineWidth',1,'Color','r');end

碰撞檢測算法：

線段檢測：

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

點(diǎn)檢測：

%監(jiān)測點(diǎn)是否發(fā)生碰撞 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. 貪心算法定義：

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

貪心算法沒有固定算法框架，算法設(shè)計(jì)的關(guān)鍵是貪心策略的選擇。必須注意的是，貪心算法不是對所有問題都能得到整體最優(yōu)解，選擇的貪心策略必須具備無后效性，即某個(gè)狀態(tài)以后的過程不會(huì)影響以前的狀態(tài)，只與當(dāng)前狀態(tài)有關(guān)。

2. 貪心算法基本步驟：

步驟1：從某個(gè)初始解出發(fā)；
步驟2：采用迭代的過程，當(dāng)可以向目標(biāo)前進(jìn)一步時(shí)，就根據(jù)局部最優(yōu)策略，得到一部分解，縮小問題規(guī)模；
步驟3：將所有解綜合起來。

本問題下的貪心算法：

1）連接目標(biāo)點(diǎn)和初始點(diǎn)。

2）檢測是否發(fā)生碰撞。

3）若是，表明從初始點(diǎn)到該點(diǎn)中間不能被省略，沿目標(biāo)點(diǎn)往上回溯到上一級(jí)父節(jié)點(diǎn)（上一級(jí)父節(jié)點(diǎn)更新為新的目標(biāo)點(diǎn)），回到步驟1；若否，表明從初始點(diǎn)到該點(diǎn)中間的所有節(jié)點(diǎn)均可被省略，將該點(diǎn)添加到新的隨機(jī)樹中，并將該點(diǎn)更新為新的初始點(diǎn)，目標(biāo)點(diǎn)復(fù)位到最后一個(gè)節(jié)點(diǎn)，回到步驟1.

4）檢測直到start更新至goal結(jié)束。

function newPath = GreedyOptimize(path,radius,circleCenter)startIndex = 1; goalIndex = length(path(:,1)); detectTimes = length(path(:,1)) - 1; %檢測次數(shù) 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，繼續(xù)向上探索父節(jié)點(diǎn)continue; elsenewPath = [newPath; goal]; %若未發(fā)生碰撞，表示找到一個(gè)最優(yōu)節(jié)點(diǎn)，則從該節(jié)點(diǎn)往上的所有父節(jié)點(diǎn)均可省略，將該節(jié)點(diǎn)添加至路徑中detectTimes = length(path(:,1)) - goalIndex; %檢測次數(shù)更位startIndex = goalIndex; %將當(dāng)前節(jié)點(diǎn)索引更新為新的樹的起點(diǎn)goalIndex = length(path(:,1)); %將終點(diǎn)索引復(fù)位endendnewPath = [newPath; path(end,:)]; %添加目標(biāo)位置end

總結(jié)

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

如果覺得生活随笔網(wǎng)站內(nèi)容還不錯(cuò)，歡迎將生活随笔推薦給好友。