自動駕駛路徑規(guī)劃算法學(xué)習(xí)-RRT算法及matlab實(shí)現(xiàn)

參考?手把手教用matlab做無人駕駛（六）-路徑規(guī)劃RRT

? ? ? ??RRT路徑規(guī)劃算法在二維仿真環(huán)境中的應(yīng)用 -- Python代碼實(shí)現(xiàn)

? ? ? ??RRT路徑規(guī)劃算法在二維仿真環(huán)境中的應(yīng)用 -- Python代碼實(shí)現(xiàn)

? ? ? ??RRT 算法原理以及過程演示

RRT快速隨機(jī)數(shù)算法 Rapid Random Tree

是基于采樣的規(guī)劃方法的一種。

快速搜索隨機(jī)樹，就是在環(huán)境中隨機(jī)撒一些點(diǎn)，這些點(diǎn)經(jīng)過算法運(yùn)算，最終可以連接起來，變成車輛可以運(yùn)行的軌跡。

1.算法原理

RRT 適用于涉及非完整約束場合下的路徑規(guī)劃問題。

過程中，算法不斷在搜索空間中隨機(jī)生成狀態(tài)點(diǎn)，如果該點(diǎn)位于無碰撞位置，則尋找搜索樹中離該節(jié)點(diǎn)最近的結(jié)點(diǎn)為基準(zhǔn)結(jié)點(diǎn)，由基準(zhǔn)結(jié)點(diǎn)出發(fā)以一定步長朝著該隨機(jī)結(jié)點(diǎn)進(jìn)行延伸，延伸線的終點(diǎn)所在的位置被當(dāng)做有效結(jié)點(diǎn)加入搜索樹中。這個搜索樹的生長過程一直持續(xù)，直到目標(biāo)結(jié)點(diǎn)與搜索樹的距離在一定范圍以內(nèi)時終止。隨后搜索算法在搜索樹中尋找一條連接起點(diǎn)到終點(diǎn)的最短路徑。
計算實(shí)例參考?RRT 算法原理以及過程演示，這篇博客講的非常清楚，如下：

1.1計算實(shí)例

Step 1.初始化一個環(huán)境，包括地圖，起點(diǎn)，終點(diǎn)。如下圖所示,黑色物體為障礙物，藍(lán)色飛機(jī)位于起點(diǎn)位置，紅色五角星為目標(biāo)終點(diǎn)位置

Step2:從環(huán)境中隨機(jī)采樣狀態(tài)點(diǎn)，如下圖所示，采樣點(diǎn)為?Xrand。

Step3: 從所構(gòu)建的樹中尋找距離采樣點(diǎn)?Xrand最近的結(jié)點(diǎn)?Xnear。現(xiàn)在樹中只有起點(diǎn)一個結(jié)點(diǎn)，所有最近的結(jié)點(diǎn)就是起點(diǎn)

Step4:開始樹的生長過程。首先連接?Xnear?和?Xrand連接起來，這個連接線的方向就是樹生長的方向。設(shè)置一個步長?Stepsize作為樹一次生長的步長，在樹生長的這個方向上生長一個步長，然后就會在生長的末端會產(chǎn)生一個新的結(jié)點(diǎn)?Xnew。

Step5:判斷從?Xnear?到?Xrand是否穿過障礙物，如果穿過，則放棄該新的結(jié)點(diǎn)，如果沒有，則將?Xnew?結(jié)點(diǎn)加入到樹中。

Step6:從步驟 2 開始再循環(huán)執(zhí)行，從環(huán)境中隨機(jī)采樣狀態(tài)點(diǎn)。

.........

重復(fù)上述樹的生長過程，直到樹新生成的結(jié)點(diǎn)到目標(biāo)點(diǎn)的距離小于一個步長，則終止樹的生長。直接將該新結(jié)點(diǎn)與目標(biāo)點(diǎn)相連。

整個步驟動圖如下：

1.2算法偽代碼

概述之：

輸入地圖，起點(diǎn)，終點(diǎn)→起點(diǎn)先加入樹節(jié)點(diǎn)nodes表→在地圖隨機(jī)采樣一個點(diǎn)xrand(同時保證有一定的概率會選擇到目標(biāo)點(diǎn)，保證有節(jié)點(diǎn)會向著目標(biāo)點(diǎn)的方向擴(kuò)展)→找到樹節(jié)點(diǎn)中離xrand最近的點(diǎn)xnearest→xnearest朝著xrand前進(jìn)一個步長得到新的點(diǎn)xnew→判斷xnearest到xnew連線進(jìn)行碰撞檢測，若有碰撞則放棄該節(jié)點(diǎn)重新選擇，若無碰撞則將該節(jié)點(diǎn)加入樹節(jié)點(diǎn)→重復(fù)xnew的擴(kuò)展過程直到擴(kuò)展的xnew在目標(biāo)點(diǎn)附近。

2.算法Matlab實(shí)現(xiàn)

這里只介紹了關(guān)鍵代碼的實(shí)現(xiàn)，非完整程序，完整代碼鏈接附在文末。

2.1 二維環(huán)境的搭建? CreateMap.m

CreateMap.m的主要作用輸入起點(diǎn)終點(diǎn)障礙物等信息，障礙物是一一個個實(shí)心圓表示。繪制起點(diǎn)終點(diǎn)障礙物信息，代碼如下：

%CreateMap.m start = [0,0]; goal = [10,14]; %障礙物(x,y,radiu) obstacle_list=[3,3,1.5;12,2,3;3,9,2;9,11,2]; %畫出地圖框及障礙物 axis([-2,18,-2,15]) hold on for i=1:length(obstacle_list(:,1)) %調(diào)用畫障礙物函數(shù) plot_obstacle(obstacle_list(i,1),obstacle_list(i,2),obstacle_list(i,3)) end plot(start(1),start(2),'og') hold on plot(goal(1),goal(2),'or') hold on

2.2 隨機(jī)采樣

隸屬于RRT算法程序RRT_planning.m的一部分。

在狀態(tài)空間中隨機(jī)采樣p_rand（這里采樣的是一個坐標(biāo)點(diǎn)）, 有一定的概率采樣到目標(biāo)點(diǎn)，確保路徑能以一定的概率向著目標(biāo)點(diǎn)前進(jìn)。這里隨機(jī)采樣取得目標(biāo)點(diǎn)的概率是0.3，這個參數(shù)越大，越快找到路徑，但障礙物較多時可能反而要耗費(fèi)更多時間繞開。

%隨機(jī)采樣取得目標(biāo)點(diǎn)的概率是0.3，這個參數(shù)越大，越快找到路徑，但障礙物較多時可能反而要耗費(fèi)更多時間繞開 p=0.3 %在環(huán)境中采樣p_rand,p_int是start p_randx = randi(16)-1; %x隨機(jī)采樣范圍0-15 p_randy = randi(13)-1; %x隨機(jī)采樣范圍0-12 p_rand=[p_randx,p_randy]; %一定概率采樣點(diǎn)取得目標(biāo)點(diǎn) if rand(1)<pp_rand=goal; end

2.3 FindNearest.m

從節(jié)點(diǎn)樹中找到距隨機(jī)采樣點(diǎn)p_rand最近的節(jié)點(diǎn)p_nearest的程序FindNearest.m程序如下：

這里有一個要注意的細(xì)節(jié)，運(yùn)行時出錯，因為nodes節(jié)點(diǎn)樹很可能存在好幾個點(diǎn)同時到p_rand隨機(jī)采樣點(diǎn)距離最小，這里設(shè)置的返回值必須只有一個，如果有多個最近點(diǎn)，只取第一個返回。

function minID=FindNearest(p_rand,nodes) %dist矩陣存放p_rand到nodes節(jié)點(diǎn)每個節(jié)點(diǎn)的距離 %nodes的節(jié)點(diǎn)數(shù) nodes_num = length(nodes(:,1)); prand_matx=ones(nodes_num,1)*p_rand(1); prand_maty=ones(nodes_num,1)*p_rand(2); nodes_matx=nodes(:,1); nodes_maty=nodes(:,2); dist=((prand_matx-nodes_matx).^2+(prand_maty-nodes_maty).^2).^0.5; minID=find(dist==min(dist)); minID=minID(1); %萬一有多個同樣小的，只取其中一個 end

2.4 擴(kuò)展新節(jié)點(diǎn)

最近點(diǎn)朝著隨機(jī)點(diǎn)走一個步長得到新節(jié)點(diǎn)。

先求出隨機(jī)點(diǎn)p_rand和最近點(diǎn)p_nearest連線與x軸所成角theta，然后計算pnew新節(jié)點(diǎn)，代碼如下：

%隨機(jī)點(diǎn)和樹中最近點(diǎn)連線與x軸夾角 theta = atan2(p_rand(2)-p_nearest(2),p_rand(1)-p_nearest(1)); %計算新節(jié)點(diǎn) pnew(1)= p_nearest(1)+ RRT_stepsize*cos(theta); pnew(2)= p_nearest(2)+ RRT_stepsize*sin(theta); %看該隨機(jī)點(diǎn)是否已在隨機(jī)樹nodes中，是的話重新選擇,防止pnew在nodes里出現(xiàn)兩次 father=FindFather(pnew, nodes); if ~isempty(father) %如果father非空說明能找到父節(jié)點(diǎn)說明在nodes里，重復(fù)了，避免出錯continue end

特別注意，我在跑程序時開始跑很多次總有一兩次會陷入死循環(huán)，怎么都找不到錯誤所在，后來發(fā)現(xiàn)是在回溯路徑時出現(xiàn)了兩個節(jié)點(diǎn)互為父節(jié)點(diǎn)father的情況，原因是在擴(kuò)展新節(jié)點(diǎn)pnew時,沒有判斷pnew是否已經(jīng)在nodes樹節(jié)點(diǎn)中了，如果已經(jīng)在nodes樹節(jié)點(diǎn)中，就不應(yīng)再作為新的擴(kuò)展點(diǎn)，本次隨機(jī)采樣放棄，進(jìn)入下一次隨機(jī)采樣。下面用圖說明：

假設(shè)

P2是由父節(jié)點(diǎn)P0擴(kuò)展出；P1是由父節(jié)點(diǎn)P2擴(kuò)展出；此時新一次的擴(kuò)展P1擴(kuò)展出的pnew正好是P2

我們程序里樹節(jié)點(diǎn)存放在nodes中，是一層層往上堆的，后擴(kuò)展的放在上面，但是在nodes中找父節(jié)點(diǎn)時又是從上往下，則會出現(xiàn)下圖的情況

對擴(kuò)展的新節(jié)點(diǎn)判斷是否已經(jīng)在nodes樹節(jié)點(diǎn)中，若是則放棄本次采樣，pnew也不加入樹節(jié)點(diǎn)nodes中，就不會陷入死循環(huán)：

2.5 碰撞檢測 collision_check.m

計算障礙物圓心o到線段p_nearest-pnew的最短距離是否小于半徑，是則會發(fā)生碰撞。

障礙物的圓心o和線段p_nearest-pnew的距離計算三種情況（垂點(diǎn)在線段之間，垂點(diǎn)在線段下方，垂點(diǎn)在線段上方）：

點(diǎn)到線段最短距離d的計算方法為點(diǎn)到直線的距離

點(diǎn)到線段最短距離d的計算方法為圓心o到p_nearest的距離

點(diǎn)到線段最短距離d的計算方法為圓心o到p_new的距離

點(diǎn)到線段最短距離的實(shí)現(xiàn)封裝為distance_squared_point_to_segment.m，其返回值為最短距離的平方，其代碼如下：

程序中? x1--p_nearest, x2--pnew, x3--圓心

向量（x3-x1）乘向量（x2-x1）求到O-pnearest在pnearest-pnew上的投影，投影/l2求到垂足在線段長度中的百分比，可能超過1或為負(fù)數(shù)。超過1時，最短距離取x3x2,小于0時距離取x3x1

distance_squared_point_to_segment.m

function dd=distance_squared_point_to_segment(x1,x2,x3) %""" 計算線段 vw 和 點(diǎn) p 之間的最短距離""",x1 near v; x2 new w; x3 obstacle_圓心 p %點(diǎn) v 和 點(diǎn) w 重合的情況 if isequal(x1,x2)dd=(x3(1)-x1(1))^2+(x3(2)-x1(2))^2;return end %線段 vw 長度的平方 l2=(x2(1)-x1(1))^2+(x2(2)-x1(2))^2; t = max(0, min(1, (x3 - x1)*(x2 - x1)' / l2)); %向量（x3-x1）乘向量（x2-x1）求到O-pnearest在pnearest-pnew上的投影，投影/l2求導(dǎo)垂足在線段長度中的百分比，可能超過1或為負(fù)數(shù)。超過1時，最短距離取x3x2,小于0時距離取x3x1 projection = x1 + t * (x2 - x1); dd = (x3 - projection)*(x3 - projection)'; end

整個碰撞檢測函數(shù)collision_check.m代碼如下：

function collisionflag=collision_check(pnew,p_nearest,obstacle_list) collisionflag=0; for i=1:length(obstacle_list(:,1)) x0=p_nearest; x1=pnew; x2=[obstacle_list(i,1),obstacle_list(i,2)]; dd = distance_squared_point_to_segment(x0,x1,x2); if dd<(obstacle_list(i,3))^2 %%距離小于障礙物半徑collisionflag=1;return end end

如果發(fā)生碰撞，就放棄本次采樣，直接進(jìn)入下一次

如果沒有發(fā)生碰撞，計算出的新節(jié)點(diǎn)pnew加入節(jié)點(diǎn)樹nodes，并在nodes存入pnew父節(jié)點(diǎn)為p_nearest

2.6 判斷是否到達(dá)目標(biāo)點(diǎn)is_near_goal.m

判斷的原理就是計算通過碰撞檢測的pnew新擴(kuò)展節(jié)點(diǎn)到目標(biāo)點(diǎn)距離是否小于一個步長RRT_stepsize,是的話，

則說明達(dá)到，并將目標(biāo)點(diǎn)加入節(jié)點(diǎn)樹nodes，記錄其父節(jié)點(diǎn)為此時的pnew,實(shí)現(xiàn)代碼如下：

function goalflag=is_near_goal(pnew,goal,RRT_stepsize) goalflag=0; dist=pdist([pnew;goal],'euclidean'); if dist<RRT_stepsizegoalflag=1;return end end

3. 運(yùn)行結(jié)果

RRT算法的幾個可調(diào)節(jié)參數(shù)? ?步長RRT_stepsize，隨機(jī)采樣取得目標(biāo)點(diǎn)概率p

RRT_stepsize越大，計算路徑的速度越快，當(dāng)步長過大可能來回震蕩往復(fù)無法達(dá)到目標(biāo)點(diǎn)附近；

p越大，計算路徑的速度越快，節(jié)點(diǎn)更快向目標(biāo)點(diǎn)生長，但p過大時，遇到障礙物時要花費(fèi)更多的時間才能繞開，反而使搜索速度下降。

從運(yùn)行結(jié)果來看，搜索到的并非最優(yōu)路徑，可以了解RRT相關(guān)的改進(jìn)算法如RRT*等，考慮到路徑代價的優(yōu)化。

運(yùn)行視頻：https://www.bilibili.com/video/BV1wK4y1R7H7/

運(yùn)行代碼：https://download.csdn.net/download/weixin_39199083/18932919?spm=1001.2014.3001.5501

總結(jié)

以上是生活随笔為你收集整理的自动驾驶路径规划算法学习-RRT算法及matlab实现的全部內(nèi)容，希望文章能夠幫你解決所遇到的問題。

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