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

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

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

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

? ? ? ??RRT 算法原理以及過(guò)程演示

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

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

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

1.算法原理

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

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

1.1計(jì)算實(shí)例

Step 1.初始化一個(gè)環(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)建的樹(shù)中尋找距離采樣點(diǎn)?Xrand最近的結(jié)點(diǎn)?Xnear。現(xiàn)在樹(shù)中只有起點(diǎn)一個(gè)結(jié)點(diǎn)，所有最近的結(jié)點(diǎn)就是起點(diǎn)

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

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

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

.........

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

整個(gè)步驟動(dòng)圖如下：

1.2算法偽代碼

概述之：

輸入地圖，起點(diǎn)，終點(diǎn)→起點(diǎn)先加入樹(shù)節(jié)點(diǎn)nodes表→在地圖隨機(jī)采樣一個(gè)點(diǎn)xrand(同時(shí)保證有一定的概率會(huì)選擇到目標(biāo)點(diǎn)，保證有節(jié)點(diǎn)會(huì)向著目標(biāo)點(diǎn)的方向擴(kuò)展)→找到樹(shù)節(jié)點(diǎn)中離xrand最近的點(diǎn)xnearest→xnearest朝著xrand前進(jìn)一個(gè)步長(zhǎng)得到新的點(diǎn)xnew→判斷xnearest到xnew連線進(jìn)行碰撞檢測(cè)，若有碰撞則放棄該節(jié)點(diǎn)重新選擇，若無(wú)碰撞則將該節(jié)點(diǎn)加入樹(shù)節(jié)點(diǎn)→重復(fù)xnew的擴(kuò)展過(guò)程直到擴(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)障礙物等信息，障礙物是一一個(gè)個(gè)實(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]; %畫(huà)出地圖框及障礙物 axis([-2,18,-2,15]) hold on for i=1:length(obstacle_list(:,1)) %調(diào)用畫(huà)障礙物函數(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（這里采樣的是一個(gè)坐標(biāo)點(diǎn)）, 有一定的概率采樣到目標(biāo)點(diǎn)，確保路徑能以一定的概率向著目標(biāo)點(diǎn)前進(jìn)。這里隨機(jī)采樣取得目標(biāo)點(diǎn)的概率是0.3，這個(gè)參數(shù)越大，越快找到路徑，但障礙物較多時(shí)可能反而要耗費(fèi)更多時(shí)間繞開(kāi)。

%隨機(jī)采樣取得目標(biāo)點(diǎn)的概率是0.3，這個(gè)參數(shù)越大，越快找到路徑，但障礙物較多時(shí)可能反而要耗費(fèi)更多時(shí)間繞開(kā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)樹(shù)中找到距隨機(jī)采樣點(diǎn)p_rand最近的節(jié)點(diǎn)p_nearest的程序FindNearest.m程序如下：

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

function minID=FindNearest(p_rand,nodes) %dist矩陣存放p_rand到nodes節(jié)點(diǎn)每個(gè)節(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); %萬(wàn)一有多個(gè)同樣小的，只取其中一個(gè) end

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

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

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

%隨機(jī)點(diǎn)和樹(shù)中最近點(diǎn)連線與x軸夾角 theta = atan2(p_rand(2)-p_nearest(2),p_rand(1)-p_nearest(1)); %計(jì)算新節(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ī)樹(shù)nodes中，是的話重新選擇,防止pnew在nodes里出現(xiàn)兩次 father=FindFather(pnew, nodes); if ~isempty(father) %如果father非空說(shuō)明能找到父節(jié)點(diǎn)說(shuō)明在nodes里，重復(fù)了，避免出錯(cuò)continue end

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

假設(shè)

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

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

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

2.5 碰撞檢測(cè) collision_check.m

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

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

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

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

點(diǎn)到線段最短距離d的計(jì)算方法為圓心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求到垂足在線段長(zhǎng)度中的百分比，可能超過(guò)1或?yàn)樨?fù)數(shù)。超過(guò)1時(shí)，最短距離取x3x2,小于0時(shí)距離取x3x1

distance_squared_point_to_segment.m

function dd=distance_squared_point_to_segment(x1,x2,x3) %""" 計(jì)算線段 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 長(zhǎng)度的平方 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)垂足在線段長(zhǎng)度中的百分比，可能超過(guò)1或?yàn)樨?fù)數(shù)。超過(guò)1時(shí)，最短距離取x3x2,小于0時(shí)距離取x3x1 projection = x1 + t * (x2 - x1); dd = (x3 - projection)*(x3 - projection)'; end

整個(gè)碰撞檢測(cè)函數(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)入下一次

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

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

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

則說(shuō)明達(dá)到，并將目標(biāo)點(diǎn)加入節(jié)點(diǎn)樹(shù)nodes，記錄其父節(jié)點(diǎn)為此時(shí)的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算法的幾個(gè)可調(diào)節(jié)參數(shù)? ?步長(zhǎng)RRT_stepsize，隨機(jī)采樣取得目標(biāo)點(diǎn)概率p

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

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

從運(yùn)行結(jié)果來(lái)看，搜索到的并非最優(yōu)路徑，可以了解RRT相關(guān)的改進(jìn)算法如RRT*等，考慮到路徑代價(jià)的優(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实现的全部?jī)?nèi)容，希望文章能夠幫你解決所遇到的問(wèn)題。

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