一、獲取代碼方式

獲取代碼方式1：
通過訂閱紫極神光博客付費專欄，憑支付憑證，私信博主，可獲得此代碼。

獲取代碼方式2：
完整代碼已上傳我的資源：【三維路徑規劃】基于matlab RRT算法無人機三維路徑規劃【含Matlab源碼 1270期】

備注：
訂閱紫極神光博客付費專欄，可免費獲得1份代碼（有效期為訂閱日起，三天內有效）；

二、RRT算法簡介

1 RRT定義
RRT（Rapidly-Exploring Random Tree）算法是一種基于采樣的路徑規劃算法，常用于移動機器人路徑規劃，適合解決高維空間和復雜約束下的路徑規劃問題。基本思想是以產生隨機點的方式通過一個步長向目標點搜索前進，有效躲避障礙物，避免路徑陷入局部極小值，收斂速度快。本文通過matlab實現RRT算法，解決二維平面的路徑規劃問題。
2 地圖
為了方便算法的實現，使用離散量來表達環境地圖。其中，數值0表示無障礙物的空區域，數值1表示該區域有障礙物。

RRT算法中搜索到的頂點坐標為連續點，在地圖中產生隨機點，算法將通過連續的點構建樹。此過程中，對樹枝和頂點進行檢測，檢測頂點所處位置是否是空區域。下載附錄中.dat文件，繪制地圖。

colormap=[1 1 1; 0 0 0; 1 0 0; 0 1 0; 0 0 1]; imshow(uint8(map),colormap)

note:數據中的列為x軸，行為y軸

3 RRT算法原理
通過matlab程序構建從起始位置到目標位置的樹，并生成連接兩個點的路徑。使用一顆中心點在起始點的樹，而不是兩顆樹（一個中心點在起始位置，一個中心點在目標位置）。
編寫一個matlab函數，輸入和輸出有相同的形式。

function [vertices, edges, path] = rrt(map, q_start, q_goal, k, delta_q, p)

其中：
map：.mat文件中的地圖矩陣
q_start：起點的x和y坐標
q_goal：目標點的x和y坐標
k: 在目標點無法找到是，控制產生搜索樹的最大迭代次數為k次
delta_q : q_new 和 q_near之間的距離
p: 將q_goal 作為q_rand 的概率，當隨機產生的隨機數小于p，將目標點作為隨機點q_rand，當隨機產生的數大于p時，產生一個隨機點作為q_rand
vertices：頂點的x和y坐標，生成隨機樹的過程中產生的所有的點的坐標都存儲在這個矩陣中，第一個點為起點，最后一個點為目標點。是一個2行n列的矩陣
deges：生成隨機樹的所有樹枝，一共有n-1個樹枝，因此該矩陣有n-1行，每一行的兩列分別表示兩個點的索引號。一旦搜索到目標點，最后一行將表示目標點，沿著目標點回溯，即可找到路徑
path: 從起始點到目標點的索引，是一個行向量
下面用一個圖來表示上面提到的算法里的一些變量：

4 障礙物檢測
檢測樹枝（即q_near和q_new之間的edge）是否處于自由空間，可以使用增量法或者等分法，示意圖如下(假設兩點之間有10個點，左圖為為增量檢測法，右圖為等分法，從示意圖中可以看出使用等分法檢測次數更少)：

在本文中，使用k=10000，delta_q=50,p=0.3, 我們將獲得如下結果：

5 路徑平滑處理
完成基本的RRT算法之后，我們獲得了一條從起點到終點的路徑，現在對這條路徑進行平滑和降噪處理，處理完成之后，我們將得到一條更短的路徑。
采用貪心算法：
連接q_start和q_goal,如果檢測到兩個點之間有障礙物，則將q_goal替換為前一個點，直到兩個點能連接上（中間無障礙物）為止。一旦q_goal被連接上，
在matlab中定義平滑函數：

function [path_smooth] = smooth(map, path, vertices, delta)

其中：
path: 從起始點到目標位置的路徑索引號
vertices：樹中所有的頂點坐標
delta：增量距離，用來檢測路徑頂點之間的直接連接是否在自由空間之內，每個edge都被delta分割成幾段
path_smooth：經過平滑處理之后，路徑點將會減少，用path_smooth記錄平滑之后的路徑,仍然是一個行向量，記錄路徑的索引號

平滑處理之后的路徑為：

6 總結
RRT算法是一種增量式的搜索算法，基于概率的思想，它是一種概率完備的路徑優化算法，具有求解速度上的優勢。RRT基本算法有其自身缺陷，求解得到的路徑通常質量不好，帶有棱角，不夠光滑。因此需要對路徑進行平滑處理，才能得到適合機器人路徑跟蹤的路徑曲線。

三、部分源代碼

function c; clc clear all close all %map1 隨機地表。 % a=10; % b=0.2; % c=0.1; % d=0.6; % e=1; % f=0.1; % g=0.1; % for x=1:80 % for y=1:80 % Z1=sin(y+a)+b*sin(x)+cos(d*(x^2+y^2)^(1/2))+e*cos(y)+f*sin(f*(x^2+y^2)^(1/2))+g*cos(y); % % Z1=SquareDiamond(6,2,8); % figure(1); % surf(Z1); %畫出三維曲面 % shading flat; %各小曲面之間不要網格 % %map2 山峰圖 tic; % h=[20,35,25,38,20,25]; % x0=[20,40,45,60,20,20]; % y0=[10,25,50,30,45,10]; % xi=[5.5,8,5,4.5,5.5,3.5]; % yi=[5,7,6,5.5,6,4.5]; h=[20,35,25,38,20,25]; x0=[20,40,45,60,20,20]; y0=[10,25,50,30,45,10]; xi=[5.5,8,5,4.5,5.5,3.5]; yi=[5,7,6,5.5,6,4.5]; Z2=CeatHill(6,h,x0,y0,xi,yi,80); figure(2); surf(Z2); %畫出三維曲面 shading flat; %各小曲面之間不要網格 %map3 合成圖 % Z3=max(Z1,Z2); % figure(3); % surf(Z3); %畫出三維曲面 % shading flat; %各小曲面之間不要網格 segmentLength =5; start_node = [10,80,5,0,0,0]; end_node =[60,0,5,1,0,0]; hold on plot3(start_node(:,1),start_node(:,2),start_node(:,3),'r*'); plot3(end_node(:,1),end_node(:,2),end_node(:,3),'r*'); tree = start_node; if ( (norm(start_node(1:3)-end_node(1:3))<segmentLength )...&(collision(start_node,end_node)==0) )path = [start_node; end_node];elsenumPaths = 0;while numPaths<1,[tree,flag] = extendTree(tree,end_node,segmentLength,Z2);numPaths = numPaths + flag;end endpath = findMinimumPath(tree);plot3(path(:,1),path(:,2),path(:,3),'r'); toc;function [data]=CeatHill(N,h,x0,y0,xi,yi,num) x=1:1:num;y=1:1:num; for m=1:numfor n=1:numSum=0;for k=1:Ns=h(k)*exp(-((x(m)-x0(k))/xi(k))^2-((y(n)-y0(k))/yi(k))^2);Sum=Sum+s;enddata(m,n)=Sum;end end

四、運行結果

五、matlab版本及參考文獻

1 matlab版本
2014a

2 參考文獻
[1] 包子陽,余繼周,楊杉.智能優化算法及其MATLAB實例（第2版）[M].電子工業出版社，2016.
[2]張巖,吳水根.MATLAB優化算法源代碼[M].清華大學出版社，2017.
[3]RRT路徑規劃算法

總結

以上是生活随笔為你收集整理的【RRT三维路径规划】基于matlab RRT算法无人机三维路径规划【含Matlab源码 1270期】的全部內容，希望文章能夠幫你解決所遇到的問題。

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