【路徑規劃】基于Matlab的改進RRT算法二維/三維路徑規劃

基于Matlab的改進RRT算法二維/三維路徑規劃

一、引言

在機器人學、自動駕駛等領域，路徑規劃是一個關鍵問題，它旨在為機器人或車輛找到一條從起始點到目標點的安全、高效的路徑。RRT（Rapidly-exploring Random Trees）算法作為一種基于采樣的路徑規劃算法，因其能夠有效處理高維空間和復雜環境下的路徑規劃問題而受到廣泛關注。然而，傳統的RRT算法存在收斂速度慢、路徑質量不高等問題。為了克服這些問題，人們提出了RRT算法以及各種改進的RRT算法。本文將詳細介紹基礎RRT算法、RRT算法和改進RRT算法的原理，并通過Matlab代碼實現這三種算法，對比它們的路徑規劃結果。

二、基礎RRT算法原理

2.1 算法概述

RRT算法是由Steven M. LaValle于1998年提出的一種基于采樣的增量式路徑規劃算法。它的核心思想是通過在狀態空間中隨機采樣，并以當前構建的隨機樹為基礎，逐步向新的采樣點進行擴展，從而快速探索未知區域。RRT算法的優點在于其無需顯式地對整個狀態空間進行離散化，能夠有效地處理高維空間，并且具有概率完備性，即在無限時間內，如果存在解，RRT算法以概率1能夠找到一條路徑。然而，標準的RRT算法通常只能找到一條可行路徑，而不能保證路徑是最優的（例如，最短路徑）。

2.2 算法步驟

RRT算法的主要步驟如下：

  1. 初始化：在環境中隨機生成一個起始節點，并將其作為樹的根節點。
  2. 擴展樹：隨機生成一個新的節點，并將其與樹中最近的節點連接起來，形成新的分支。
  3. 檢查碰撞：檢查新分支是否與障礙物發生碰撞。如果發生碰撞，則放棄該分支。
  4. 更新目標節點：如果新分支與目標節點的距離小于閾值，則將目標節點與樹連接起來，完成路徑規劃。
  5. 重復步驟2 - 4：重復步驟2 - 4，直到找到一條連接起點和終點的路徑。

2.3 算法示意圖

基礎RRT算法原理示意圖

三、RRT*算法原理

3.1 算法概述

RRT算法是RRT算法的改進版本，它在標準RRT算法的基礎上引入了兩個關鍵機制：選擇父節點和重布線機制，以實現最優或近似最優的路徑規劃。通過這兩個機制，RRT算法能夠在構建隨機樹的同時，不斷優化已存在的路徑，使得整棵樹趨向于包含最優路徑。RRT*算法具有漸近最優性，即當迭代次數趨近于無窮時，算法以概率1能夠找到最優路徑。

3.2 算法步驟

RRT*算法的主要步驟如下：

  1. 初始化：設定起始點start和目標點goal，并創建一個只包含start的RRT樹T。
  2. 重復步驟直到找到路徑或達到最大迭代次數：
    a. 隨機采樣：在環境空間中隨機采樣一個點x_rand。
    b. 擴展樹：從樹T中找到最近的節點x_near，以x_near為起點，在方向上延伸一定的距離，得到新的節點x_new。
    c. 碰撞檢測：檢測路徑x_near到x_new之間是否與障礙物發生碰撞，如果發生碰撞，則返回步驟2繼續下一次迭代。
    d. 尋找最優連接：在樹T中找到與x_new最近的節點x_min，并計算從x_min到x_new的代價cost(x_min, x_new)。
    e. 重新連接：對于樹T中與x_new距離在一定范圍內的節點x_near_neighbors，計算通過x_near連接到x_new的代價cost(x_near, x_new)，選擇代價最小的連接方式。
    f. 更新樹：將x_new加入樹T，并更新節點間的連接關系和代價信息。
    g. 優化路徑：對樹T中的部分節點，以目標點goal為目標進行優化，通過調整連接關系和代價信息，改善路徑的質量。
    h. 判斷終止條件：如果新加入的節點x_new接近目標點goal，則找到了一條可行路徑，算法結束。

四、改進RRT算法原理

4.1 算法概述

為了進一步提高RRT算法的性能，人們提出了多種改進策略。這些改進策略從多個方面對傳統RRT算法進行優化，旨在提升算法的收斂速度、路徑質量以及避障能力等。以下將詳細介紹幾種常見的改進策略。

4.2 多步長策略

4.2.1 原理

在障礙物稀疏區域使用大步長，在障礙物密集區域使用小步長，以提高搜索效率。通過動態調整步長，算法可以在不同環境下更靈活地擴展搜索樹，加速搜索過程，減少無效擴展。

4.2.2 方法
  • 計算當前節點附近障礙物的密度（但不直接依賴障礙物密度計算）。
  • 根據路徑連續碰撞情況動態調整步長：
    • 大步長 step3（自由區域）
    • 中步長 step2（一般區域）
    • 小步長 step1（障礙物密集區域）
4.2.3 優勢

加速搜索過程，減少無效擴展，提高避障能力。

4.3 目標偏置概率P與權重系數W聯合優化

4.3.1 原理

偏置概率P用于控制隨機點采樣，使算法有一定概率直接朝向目標點，提高收斂速度。權重系數W在生成新節點時，動態調整目標點與隨機點的影響權重，使搜索更加平穩，減少不必要的波動。

4.3.2 計算方式
  • 計算隨機點Q1和目標點Q2方向的位移：
    • (Q_1=(1 - W)cdot(randNode - nearestNode)+nearestNode)
    • (Q_2=Wcdot(goal - nearestNode)+nearestNode)
  • 最終采樣點：
    • (sample = Q_1+Q_2)

4.4 路徑與障礙物安全距離safe_dis約束

4.4.1 原理

路徑點應始終與障礙物保持安全距離safe_dis以上，避免路徑緊貼障礙物，提高魯棒性。

4.4.2 方法
  • 采用dis_fun函數，在路徑點生成時檢查是否滿足安全距離。
  • 若不滿足，則重新采樣，避免無效擴展。

4.5 拉伸與剪枝策略

4.5.1 拉伸

若路徑點之間可以直接相連（無碰撞），則移除中間點，提高路徑簡潔性。例如，原路徑點為(P_1→P_2→P_3→P_4→P_5)，若(P_1→P_4)直接可行，則移除(P_2)，(P_3)，得到優化路徑(P_1→P_4→P_5)。

4.5.2 剪枝

從目標點向起點回溯，移除多余拐點，使路徑更順暢。

4.6 碰撞檢測的改進

4.6.1 傳統方法

采用固定分割點數進行路徑檢查，精度受限。

4.6.2 改進方案
  • 動態調整采樣點數：(n = round(min(max(baseN-(dist / maxDist), minN), maxN)))
  • 逐點檢測：
    • 生成插值點：(node_lin = linspace(node1, node2, n))
    • 遍歷檢測：若(dis_fun(temp_point, map, row, col, safe_dis)==1)，則認為碰撞
4.6.3 優勢

碰撞檢測更加精細，減少漏檢情況，提高路徑安全性。

4.7 終點判斷的優化

4.7.1 方法

在新節點生成時，檢查新節點是否能直接連通目標點（無障礙）。若可直連，則直接連接目標點，結束搜索。

4.7.2 優勢

避免無意義的繼續搜索，提高路徑收斂速度。

4.8 生成新節點時優先選擇“離目標最近的路徑點”

4.8.1 原理

傳統RRT隨機選擇父節點，可能導致路徑較長。本方法確保新節點生成在“當前路徑中最接近目標的點”附近，提高收斂效率。

4.8.2 實現
  • 計算所有已生成路徑點到目標點的距離：(nearestToGoal = arg min(dist(path.Nodes, goal)))
  • 以該點作為新節點的父節點，提高搜索的導向性。

五、三種算法的路徑規劃結果對比

5.1 對比指標

為了對比基礎RRT算法、RRT*算法和改進RRT算法的性能，我們可以從以下幾個方面進行評估：

  1. 路徑長度：路徑長度越短，說明算法找到的路徑越優。
  2. 收斂速度：收斂速度越快，說明算法能夠在更短的時間內找到可行路徑。
  3. 路徑平滑度：路徑平滑度越高，說明機器人或車輛在行駛過程中越穩定。
  4. 避障能力：避障能力越強，說明算法在復雜環境下的可靠性越高。

5.2 對比結果

通過Matlab代碼實現這三種算法，并在相同的環境下進行路徑規劃實驗，得到以下對比結果：

算法路徑長度收斂速度路徑平滑度避障能力
基礎RRT算法較長較慢較差一般
RRT*算法較短較快較好較好
改進RRT算法最短最快最好最強

5.3 結果示意圖

5.3.1 二維結果圖

在這里插入圖片描述
請添加圖片描述
請添加圖片描述
各個算法性能指標如下：
在這里插入圖片描述

5.3.2 三維結果圖

在這里插入圖片描述
在這里插入圖片描述
在這里插入圖片描述

在這里插入圖片描述

六、Matlab代碼實現

6.1 基礎RRT算法代碼示例

% %***************************************
% %RRT
function [trace_coo,dis_yuan]=RRT3D(S_coo,E_coo,radius,circleCenter,mapsize)
tic
x_I=S_coo(1); y_I=S_coo(2);            z_I=S_coo(3);  
x_G=E_coo(1); y_G=E_coo(2);        z_G=E_coo(3);  
Thr=100;                 
Delta= 50;              xL=mapsize(1)';
yL=mapsize(2)';
zL=mapsize(3)';
figure;
[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;
hold on;
scatter3(S_coo(1),S_coo(2),S_coo(3),'filled','g');
scatter3(E_coo(1),E_coo(2),E_coo(3),'filled','b');
%% 圖片標注
text(S_coo(1),S_coo(2),S_coo(3),'起點');
text(E_coo(1),E_coo(2),E_coo(3),'終點');
view(3);
grid on;
axis equal;
axis([0 xL 0 yL 0 zL]);
xlabel('x');
ylabel('y');
zlabel('z');%% 
T.v(1).x = x_I;         
T.v(1).y = y_I; 
T.v(1).z = z_I; T.v(1).xPrev = x_I;   
T.v(1).yPrev = y_I;
T.v(1).zPrev = z_I;T.v(1).dist=0;         
T.v(1).indPrev = 0;     
%% 
count=1;
goal = [x_G,y_G,z_G];
for iter = 1:30000x_rand=[];x_rand(1) = xL*rand; x_rand(2) = yL*rand;x_rand(3) = zL*rand;x_near=[];min_dist = 1000000;near_iter = 1;%%=======尋找x_near===========%%[~,N]=size(T.v);for j = 1:Nx_near(1) = T.v(j).x;x_near(2) = T.v(j).y;x_near(3) = T.v(j).z;dist = norm(x_rand - x_near);if min_dist > distmin_dist = dist;near_iter = j;endendx_near(1) = T.v(near_iter).x;x_near(2) = T.v(near_iter).y;x_near(3) = T.v(near_iter).z;%%========獲取x_new============%%x_new=[];near_to_rand = [x_rand(1)-x_near(1),x_rand(2)-x_near(2),x_rand(3)-x_near(3)];normlized = near_to_rand / norm(near_to_rand) * Delta;x_new = x_near + normlized;%%=======障礙檢測===============%%% if ~collisionChecking(x_near,x_new,Imp) if ~collisionDetec(x_near,x_new,radius,circleCenter,0)continue;endcount=count+1;%%========將X_NEW增加到樹中========%%T.v(count).x = x_new(1);T.v(count).y = x_new(2); T.v(count).z = x_new(3); T.v(count).xPrev = x_near(1);     T.v(count).yPrev = x_near(2);T.v(count).zPrev = x_near(3);T.v(count).dist= norm(x_new - x_near);          T.v(count).indPrev = near_iter;   plot3([x_near(1),x_new(1)],[x_near(2),x_new(2)],[x_near(3),x_new(3)],'-k');hold on;%========判斷是否找到路徑==========%%if norm(x_new - goal) < Thrbreak;end% pause(0.1);
end
%%elsedisp('Error, no path found!');
end%%
trace_coo=[path.pos.x;path.pos.y;path.pos.z]';
toc
time_all=toc;disp(['原始RRT算法采樣點數：',num2str(count)])
disp(['原始RRT算法運行時間：',num2str(time_all)])
disp(['原始RRT算法路徑總距離：',num2str(dis_yuan)])
disp('-----------------------------------------------------------------------' )

6.2 RRT*算法代碼示例

function [trace_coo,dis_yuan]=RRTstar3D(S_coo,E_coo,radius,circleCenter,mapsize)
tic
x_I=S_coo(1); y_I=S_coo(2);            z_I=S_coo(3);  
x_G=E_coo(1); y_G=E_coo(2);        z_G=E_coo(3);  
Thr=100;                 
Delta= 100;              xL=mapsize(1)';
yL=mapsize(2)';
zL=mapsize(3)';
figure;
[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;
hold on;
scatter3(S_coo(1),S_coo(2),S_coo(3),'filled','g');
scatter3(E_coo(1),E_coo(2),E_coo(3),'filled','b');
%% 圖片標注
text(S_coo(1),S_coo(2),S_coo(3),'起點');
text(E_coo(1),E_coo(2),E_coo(3),'終點');
view(3);
grid on;
axis equal;
axis([0 xL 0 yL 0 zL]);
xlabel('x');
ylabel('y');
zlabel('z');%% 
T.v(1).x = x_I;         
T.v(1).y = y_I; 
T.v(1).z = z_I; T.v(1).xPrev = x_I;   
T.v(1).yPrev = y_I;
T.v(1).zPrev = z_I;T.v(1).dist=0;         
T.v(1).indPrev = 0;     
%%
% figure(1);
% ImpRgb=imread('map6.bmp');
% if size(ImpRgb, 3) == 3
%     Imp = rgb2gray(ImpRgb);
% else
%     Imp = ImpRgb;
% end
% imshow(Imp)
% xL=size(Imp,1);
% yL=size(Imp,2);
% hold on
% plot(x_I, y_I, 'ro', 'MarkerSize',10, 'MarkerFaceColor','r');
% plot(x_G, y_G, 'go', 'MarkerSize',10, 'MarkerFaceColor','g');
count=1;
goal = [x_G,y_G,z_G];
start_goal_dist = 1000000;
% path.pos(1).x = 700;
% path.pos(1).y = 700;
% path.pos(1).z = 700;
dis_yuan=0;for iter = 1:30000x_rand=[];x_rand(1) = xL*rand;x_rand(2) = yL*rand;x_rand(3) = zL*rand;%%=======尋找x_near===========%%x_near=[];min_dist = 1000000;near_iter = 1;near_iter_tmp = 1;[~,N]=size(T.v);for j = 1:Nx_near(1) = T.v(j).x;x_near(2) = T.v(j).y;x_near(3) = T.v(j).z;dist = norm(x_rand - x_near);if min_dist > distmin_dist = dist;near_iter = j;endendx_near(1) = T.v(near_iter).x;x_near(2) = T.v(near_iter).y;x_near(3) = T.v(near_iter).z;%%========獲取x_new============%%x_new=[];near_to_rand = [x_rand(1)-x_near(1),x_rand(2)-x_near(2),x_rand(3)-x_near(3)];normlized = near_to_rand / norm(near_to_rand) * Delta;x_new = x_near + normlized;%%=======障礙檢測===============%%if ~collisionDetec(x_near,x_new,radius,circleCenter,0)continue;end%%=======  nearC && chooseParent  =========%%nearptr = [];nearcount = 0;neardist =  norm(x_new - x_near) + T.v(near_iter_tmp).dist;for j = 1:Nif j == near_iter_tmpcontinue;endx_neartmp(1) = T.v(j).x;x_neartmp(2) = T.v(j).y;x_neartmp(3) = T.v(j).z;dist = norm(x_new - x_neartmp) + T.v(j).dist;norm_dist = norm(x_new - x_neartmp);if norm_dist < 120%nearCif collisionDetec(x_near,x_new,radius,circleCenter,0)nearcount = nearcount + 1;nearptr(nearcount,1) = j;if neardist > distneardist = dist;near_iter = j;endendendendx_near(1) = T.v(near_iter).x;x_near(2) = T.v(near_iter).y;x_near(3) = T.v(near_iter).z;count=count+1;%%========將X_NEW增加到樹中========%%T.v(count).x = x_new(1);T.v(count).y = x_new(2); T.v(count).z = x_new(3); T.v(count).xPrev = x_near(1);     T.v(count).yPrev = x_near(2);T.v(count).zPrev = x_near(3);T.v(count).dist= norm(x_new - x_near) + T.v(near_iter).dist;T.v(count).indPrev = near_iter;%%========  rewirte  =========%%[M,~] = size(nearptr);for k = 1:Mx_1(1) = T.v(nearptr(k,1)).x;x_1(2) = T.v(nearptr(k,1)).y;x_1(3) = T.v(nearptr(k,1)).z;x1_prev(1) = T.v(nearptr(k,1)).xPrev;x1_prev(2) = T.v(nearptr(k,1)).yPrev;x1_prev(3) = T.v(nearptr(k,1)).zPrev;if T.v(nearptr(k,1)).dist >  (T.v(count).dist + norm(x_1-x_new))T.v(nearptr(k,1)).dist = T.v(count).dist + norm(x_1-x_new);T.v(nearptr(k,1)).xPrev = x_new(1);T.v(nearptr(k,1)).yPrev = x_new(2);T.v(nearptr(k,1)).yPrev = x_new(3);T.v(nearptr(k,1)).indPrev = count;% plot([x_1(1),x1_prev(1)],[x_1(2),x1_prev(2)],'-w');% hold on;plot3([x_1(1),x_new(1)],[x_1(2),x_new(2)],[x_1(3),x_new(3)],'-k');hold on;% drawnowendendplot3([x_near(1),x_new(1)],[x_near(2),x_new(2)],[x_near(3),x_new(3)],'-k');hold on;% plot(x_new(1),x_new(2),'*r');% hold on;% if norm(x_new - goal) < Thrif norm(x_new - goal) < Thrbreak;end
end% if (T.v(count).dist + norm(x_new - goal)) < start_goal_dist
%     start_goal_dist = (T.v(count).dist + norm(x_new - goal));
%     % if length(path.pos) > 2
%     %     for j = 2 : length(path.pos)
%     %         plot([path.pos(j).x; path.pos(j-1).x;], [path.pos(j).y; path.pos(j-1).y], 'w', 'Linewidth', 3);
%     %     end
%     % end
%     path.pos = [];
if iter < 30000path.pos(1).x = x_G; path.pos(1).y = y_G;path.pos(1).z = z_G;path.pos(2).x = T.v(end).x; path.pos(2).y = T.v(end).y;path.pos(2).z = T.v(end).z;pathIndex = T.v(end).indPrev; % ??????·??j=0;while 1path.pos(j+3).x = T.v(pathIndex).x;path.pos(j+3).y = T.v(pathIndex).y;path.pos(j+3).z = T.v(pathIndex).z;pathIndex = T.v(pathIndex).indPrev;if pathIndex == 1breakendj=j+1;endpath.pos(end+1).x = x_I; path.pos(end).y = y_I; path.pos(end).z = z_I; for j = 2:length(path.pos)% plot([path.pos(j).x; path.pos(j-1).x;], [path.pos(j).y; path.pos(j-1).y], 'b', 'Linewidth', 3);dis_yuan=dis_yuan+...norm([path.pos(j).x-path.pos(j-1).x,path.pos(j).y- path.pos(j-1).y,path.pos(j).z- path.pos(j-1).z]);end% else% disp('Error, no path found!');% end
end
% continue;
% end
% pause(0.01);
% end%%
trace_coo=[path.pos.x;path.pos.y;path.pos.z]';
toc
time_all=toc;
% dis_yuan=0;
% trace_coo=[tracebackNode.xCoord,tracebackNode.yCoord];
% while tracebackNode.parentNode~=0
%     dis_yuan=dis_yuan+norm([tracebackNode.xCoord-nodeArray(tracebackNode.parentNode).xCoord,tracebackNode.yCoord-nodeArray(tracebackNode.parentNode).yCoord]);
%     tracebackNode =nodeArray(tracebackNode.parentNode);
%     trace_coo=[[tracebackNode.xCoord,tracebackNode.yCoord];trace_coo];
% end% plot(trace_coo(:,1),trace_coo(:,2),'r','LineWidth',2);disp(['RRT*算法采樣點數：',num2str(count)])
disp(['RRT*算法運行時間：',num2str(time_all)])
disp(['RRT*算法路徑總距離：',num2str(dis_yuan)])
disp('-----------------------------------------------------------------------' )

6.3 改進RRT算法代碼示例

function [smoothedPath,dis_smooth]=imRRT3D(S_coo,E_coo,radius,circleCenter,mapsize)
row=mapsize(1);
col=mapsize(2);
height=mapsize(3);figure;
[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;
hold on;
scatter3(S_coo(1),S_coo(2),S_coo(3),'filled','g');
scatter3(E_coo(1),E_coo(2),E_coo(3),'filled','b');
%% 圖片標注
text(S_coo(1),S_coo(2),S_coo(3),'起點');
text(E_coo(1),E_coo(2),E_coo(3),'終點');
view(3);
grid on;
axis equal;
axis([0 row 0 col 0 height]);
xlabel('x');
ylabel('y');
zlabel('z');
% title('本文改進RRT算法搜索樹')car_R = 3;  % 車的大小
safe_dis1 = 60;  % 與障礙物的一類安全距離
safe_dis2 = 110;  % 與障礙物的二類安全距離
step1 = 50;% 小步長
step2 = 100;  % 正常步長
step3 = 150;  % 大步長
P = 0.6;  % 概率參數
goalRadius = 100;nodeIndex = 1;  % 樹枝節點編號初始值
goalFound = 0;
% 起始節點初始化
initialNode.xCoord = S_coo(1);
initialNode.yCoord = S_coo(2);
initialNode.zCoord = S_coo(3);initialNode.parentNode = 0;
goalNode.xCoord = E_coo(1);
goalNode.yCoord = E_coo(2);
goalNode.zCoord = E_coo(3);nodeArray(nodeIndex).xCoord = initialNode.xCoord;
nodeArray(nodeIndex).yCoord = initialNode.yCoord;
nodeArray(nodeIndex).zCoord = initialNode.zCoord;nodeArray(nodeIndex).parentNode = initialNode.parentNode;
nodeArray(nodeIndex).ind = nodeIndex;
nodeIndex = nodeIndex + 1;%% 主要路徑規劃部分，可以改：隨機生成幾個點，引入代價線，從起點到終點的連線直線，查看離直線最短的點。
tic
while goalFound == 0% === (1) 在已有節點中選擇最接近目標的節點 ===nearestToGoal = findNearestToGoal(nodeArray, goalNode);% === (2) 以該最近節點附近生成新節點 ===randomNode.xCoord = nearestToGoal.xCoord + (rand - 0.5) * 200; % 隨機偏移，范圍 [-50, 50]randomNode.yCoord = nearestToGoal.yCoord + (rand - 0.5) * 200;randomNode.zCoord = nearestToGoal.zCoord + (rand - 0.5) * 200;% if rand < 0.1  % 目標偏置%     randomNode.xCoord = E_coo(1);%     randomNode.yCoord = E_coo(2);% else%     randomNode.xCoord = randi([1, col]);%     randomNode.yCoord = randi([1, row]);% end% === (3) 找到最近鄰節點 ===nearestNode = nodeArray(1);for node = nodeArrayif (getDistanceBetween3D(randomNode, node) < getDistanceBetween3D(randomNode, nearestNode))nearestNode = node;  % 選擇距離新生成節點最近的已有節點endend% === (4) 目標點偏置 + 權重調整 ===% flag = dis_fun3D(nearestNode, map, row, col, safe_dis1);flag =pointCollisionCheck(nearestNode,radius,circleCenter,safe_dis1);if flag == 0  % 障礙少distToGoal = getDistanceBetween3D(nearestNode, goalNode);w = min(1, distToGoal / (row + col));  % 根據目標距離動態調整權重if rand < P  % 目標點方向randomNode.xCoord = goalNode.xCoord;randomNode.yCoord = goalNode.yCoord;randomNode.yCoord = goalNode.zCoord;else  % 目標點與隨機點的平衡d1 = getDistanceBetween3D(randomNode, nearestNode);d2 = getDistanceBetween3D(goalNode, nearestNode);Q1(1) = (randomNode.xCoord - nearestNode.xCoord) * (1 - w) / d1 + nearestNode.xCoord;Q1(2) = (randomNode.yCoord - nearestNode.yCoord) * (1 - w) / d1 + nearestNode.yCoord;Q1(3) = (randomNode.zCoord - nearestNode.zCoord) * (1 - w) / d1 + nearestNode.zCoord;Q2(1) = (goalNode.xCoord - nearestNode.xCoord) * w / d2 + nearestNode.xCoord;Q2(2) = (goalNode.yCoord - nearestNode.yCoord) * w / d2 + nearestNode.yCoord;Q2(3) = (goalNode.zCoord - nearestNode.zCoord) * w / d2 + nearestNode.zCoord;Q = Q1 + Q2;randomNode.xCoord = Q(1);randomNode.yCoord = Q(2);randomNode.zCoord = Q(3);endend% === (5) 選擇合適步長 ===flag = pointCollisionCheck(nearestNode,radius,circleCenter,safe_dis2);if flag == 0step = step3;elseflag = pointCollisionCheck(nearestNode,radius,circleCenter,safe_dis2);if flag == 0step = step2;elsestep = step1;endend% === (6) 生成新節點 ===angleAlpha = atan2(randomNode.yCoord - nearestNode.yCoord, randomNode.xCoord - nearestNode.xCoord);newNode.xCoord = nearestNode.xCoord + step * cos(angleAlpha);newNode.yCoord = nearestNode.yCoord + step * sin(angleAlpha);newNode.zCoord = nearestNode.zCoord + step * sin(angleAlpha);growVec = [randomNode.xCoord - nearestNode.xCoord,randomNode.yCoord - nearestNode.yCoord,randomNode.zCoord - nearestNode.zCoord];growVec = growVec/sqrt(sum(growVec.^2));newNode.xCoord = nearestNode.xCoord + step * growVec(1);newNode.yCoord = nearestNode.yCoord + step * growVec(2);newNode.zCoord = nearestNode.zCoord + step * growVec(3);flag1 = ~pointCollisionCheck(newNode,radius,circleCenter,car_R*1.1);% flag2 = checkConstraint(newNode, nearestNode, map, row, col, car_R * 1.1);flag2= ~collisionDetec([newNode.xCoord,newNode.yCoord,newNode.zCoord],...[nearestNode.xCoord,nearestNode.yCoord,nearestNode.zCoord],radius,circleCenter,car_R * 1.1);if flag1||flag2  % 遇到障礙則跳過continue;endif newNode.xCoord <= 0 || newNode.xCoord >= col || newNode.yCoord <= 0 || newNode.yCoord >= row||...newNode.zCoord <= 0 || newNode.zCoord >= heightcontinue;end% === (7) 記錄并繪制路徑 ===plot3([newNode.xCoord nearestNode.xCoord], [newNode.yCoord nearestNode.yCoord], [newNode.zCoord nearestNode.zCoord], 'k-');% drawnow;nodeIndex = nodeIndex + 1;nodeArray(nodeIndex).xCoord = newNode.xCoord;  % 記錄新節點nodeArray(nodeIndex).yCoord = newNode.yCoord;nodeArray(nodeIndex).zCoord = newNode.zCoord;nodeArray(nodeIndex).parentNode = nearestNode.ind;nodeArray(nodeIndex).ind = nodeIndex;% === (8) 目標點直連檢測 ===if collisionDetec([newNode.xCoord,newNode.yCoord,newNode.zCoord],...[goalNode.xCoord,goalNode.yCoord,goalNode.zCoord],radius,circleCenter,car_R * 1.1)plot3([newNode.xCoord goalNode.xCoord], [newNode.yCoord goalNode.yCoord], [newNode.zCoord goalNode.zCoord], 'k-');goalFound = 1;break;end% === (9) 判斷是否到達目標點 ===if getDistanceBetween3D(newNode, goalNode) <= goalRadiusgoalFound = 1;break;end
end
toc
time_all=toc;
%%
%判斷最后點是不是終點
if nodeArray(nodeIndex).xCoord~=goalNode.xCoord||nodeArray(nodeIndex).yCoord~=goalNode.yCoord||nodeArray(nodeIndex).zCoord~=goalNode.zCoordnodeIndex=nodeIndex+1;nodeArray(nodeIndex).xCoord=goalNode.xCoord;nodeArray(nodeIndex).yCoord=goalNode.yCoord;nodeArray(nodeIndex).zCoord=goalNode.zCoord;nodeArray(nodeIndex).parentNode=nodeIndex-1;nodeArray(nodeIndex).ind=nodeIndex;plot3([nodeArray(nodeIndex-1).xCoord nodeArray(nodeIndex).xCoord],[nodeArray(nodeIndex-1).yCoord nodeArray(nodeIndex).yCoord],...[nodeArray(nodeIndex-1).zCoord nodeArray(nodeIndex).zCoord],'-k');
end
tracebackNode = nodeArray(nodeIndex);
dis_yuan=0;
trace_coo=[tracebackNode.xCoord,tracebackNode.yCoord,tracebackNode.zCoord];
while tracebackNode.parentNode~=0dis_yuan=dis_yuan+norm([tracebackNode.xCoord-nodeArray(tracebackNode.parentNode).xCoord,...tracebackNode.yCoord-nodeArray(tracebackNode.parentNode).yCoord,...tracebackNode.zCoord-nodeArray(tracebackNode.parentNode).zCoord]);tracebackNode =nodeArray(tracebackNode.parentNode);trace_coo=[[tracebackNode.xCoord,tracebackNode.yCoord,tracebackNode.zCoord];trace_coo];
endbeacon_coo= prunePath3D(trace_coo,radius,circleCenter,1.1*car_R);
% plot3(beacon_coo(:,1),beacon_coo(:,2),beacon_coo(:,3),'b--','LineWidth',1);k = 0.5; % 控制圓弧大小的參數
smoothedPath = smooth_path3D(beacon_coo, k);
dis_smooth=0;
for i=2:size(smoothedPath,1)dis_smooth=dis_smooth+norm(smoothedPath(i,:)-smoothedPath(i-1,:));
end
% plot3(smoothedPath(:,1),smoothedPath(:,2),smoothedPath(:,3),'r-','LineWidth',2);disp(['本文改進RRT算法采樣點數：',num2str(nodeIndex)])
disp(['本文改進RRT算法運行時間：',num2str(time_all)])
disp(['本文改進RRT算法路徑總距離：',num2str(dis_smooth)])
title('本文改進RRT算法路徑圖')
% disp('-----------------------------------------------------------------------' )

七、結論

本文詳細介紹了基礎RRT算法、RRT算法和改進RRT算法的原理，并通過Matlab代碼實現了這三種算法，對比了它們的路徑規劃結果。實驗結果表明，基礎RRT算法雖然能夠快速找到一條可行路徑，但路徑質量不高；RRT算法通過引入選擇父節點和重布線機制，能夠找到漸近最優的路徑，但收斂速度較慢；改進RRT算法通過引入多步長策略、目標偏置概率與權重系數聯合優化等多種改進策略，能夠在提高收斂速度的同時，顯著提升路徑質量和避障能力。在實際應用中，我們可以根據具體的需求選擇合適的算法。

本人擅長各類優化模型的建模和求解，具有近400個優化項目的建模仿真經驗，擅長模型構建，算法設計，算法實現和算法改進。累計指導各類建模/算法比賽和SCI寫作超過100人次。
本人長期提供： ①源碼分享（近1000個本人手寫項目） ②輔導答疑（遠程桌面一對一語音+文檔指導，可以錄屏反復觀看）
③項目定制（根據您的現實問題，針對性建模求解，提供完整方案+代碼實現） 長期在線，歡迎咨詢，一般晚上看消息！！

本文來自互聯網用戶投稿，該文觀點僅代表作者本人，不代表本站立場。本站僅提供信息存儲空間服務，不擁有所有權，不承擔相關法律責任。
如若轉載，請注明出處：http://www.pswp.cn/web/86036.shtml
繁體地址，請注明出處：http://hk.pswp.cn/web/86036.shtml
英文地址，請注明出處：http://en.pswp.cn/web/86036.shtml

如若內容造成侵權/違法違規/事實不符，請聯系多彩編程網進行投訴反饋email:809451989@qq.com，一經查實，立即刪除！

相關文章

PHP的命名空間與自動加載機制

PHP的命名空間與自動加載機制

在PHP 5.3版本之后&#xff0c;引入了命名空間的概念&#xff0c;這為解決全局命名沖突和促進代碼的模塊化提供了強有力的工具。命名空間允許開發者將類、函數和常量封裝在不同的命名空間中&#xff0c;從而避免了全局范圍內的名稱沖突問題。 命名空間基礎 命名空間在PHP中是…
閱讀更多...
OpenSIPS 邂逅 Kafka：構建高效 VoIP 消息處理架構

OpenSIPS 邂逅 Kafka：構建高效 VoIP 消息處理架構

使用場景使用步驟 引入模塊組裝&發送數據消費數據故障轉移 使用場景 異步日志處理&#xff1a;將 OpenSIPS 中的 SIP 信令日志、通話記錄&#xff08;CDR&#xff09;等數據發送到 Kafka 隊列中。 事件通知與監控&#xff1a;利用 OpenSIPS 的 event_interface 模塊將 S…
閱讀更多...
《AI大模型應用技術開發工程師》學習總結

《AI大模型應用技術開發工程師》學習總結

以下是對你提供的《AI大模型應用技術開發工程師》課程內容的系統梳理&#xff0c;已去除所有廣告、價格、報名、個人信息等內容&#xff0c;并補全了技術要點&#xff0c;最后給出客觀的學習建議和個人感想&#xff0c;適合公開分享或自我學習參考。 AI大模型應用技術開發工程師…
閱讀更多...
Python爬蟲實戰：研究LOSO相關技術

Python爬蟲實戰：研究LOSO相關技術

1. 引言 1.1 研究背景與意義 隨著互聯網數據的爆炸式增長，個性化推薦系統成為提升用戶體驗的關鍵技術。準確捕捉用戶興趣需要大量多維度數據，但獲取高質量標注數據面臨隱私保護、數據分散等挑戰。網絡爬蟲技術為自動采集用戶行為數據提供了解決方案，而如何有效評估模型在個…
閱讀更多...
stm32萬年歷仿真+keil5程序

stm32萬年歷仿真+keil5程序

stm32萬年歷 本設計是利用單片機實現一個簡易萬年歷系統&#xff0c;能夠準確顯示時、分、秒信息。用戶可通過特定按鍵對時間進行設置調整&#xff0c;具備基本的時間校準功能&#xff0c;可滿足日常簡易計時需求。運用了stm32單片機模塊內部定時器 / 計數器功能來實現精確計時…
閱讀更多...
操作系統--名稱解釋

操作系統--名稱解釋

第一章: 操作系統:位于硬件層之上,所有軟件層之下的一個系統軟件,是管理系統中各種軟硬件資源,方便用戶使用計算機系統的程序集合 并發:宏觀上是同時發生,但是再微觀是交替發生的(若干事件在同一時間間隔內發生,單CPU) 并行:微觀上同時發生(要求多個CPU) 共享:系統的資源可以…
閱讀更多...
2025.6.16-實習

2025.6.16-實習

2025.6.18--2025.6.23 1.使用Cocos&#xff0c;從0開發老虎棒子雞2D游戲。實現&#xff1a;AI自動選擇&#xff0c;倒計時&#xff0c;對戰邏輯&#xff0c;播放動畫&#xff0c;設置背景音樂等功能。 2.使用Cocos&#xff0c;開發2D手術游戲。實現&#xff1a;視頻、音頻控制播…
閱讀更多...
構建你的 AI 模塊宇宙：Spring AI MCP Server 深度定制指南

構建你的 AI 模塊宇宙：Spring AI MCP Server 深度定制指南

引言&#xff1a;當模塊化遇見 AI 在微服務架構的海洋中&#xff0c;MCP&#xff08;Module Communication Protocol&#xff09;就像一艘智能帆船&#xff0c;它讓不同 AI 模塊的通信變得優雅而高效。本文將帶你構建一艘屬于自己的 AI 智能帆船——自定義 Spring AI MCP Serv…
閱讀更多...
從數據到洞察：UI前端如何利用大數據優化用戶體驗

從數據到洞察：UI前端如何利用大數據優化用戶體驗

hello寶子們...我們是艾斯視覺擅長ui設計、前端開發、數字孿生、大數據、三維建模、三維動畫10年經驗!希望我的分享能幫助到您!如需幫助可以評論關注私信我們一起探討!致敬感謝感恩! 在當今數字化時代&#xff0c;數據如同蘊藏著無限價值的寶藏&#xff0c;源源不斷地產生并積累…
閱讀更多...
SQLite3 在嵌入式C環境中存儲音頻/視頻文件的專業方案

SQLite3 在嵌入式C環境中存儲音頻/視頻文件的專業方案

SQLite3 在嵌入式C環境中存儲音頻/視頻文件的專業方案 在嵌入式系統中存儲大型媒體文件需要平衡存儲效率、訪問速度和資源限制。以下是針對嵌入式C環境的優化方案&#xff1a; 一、存儲策略選擇 1. 直接存儲 vs 文件路徑存儲 方法優點缺點適用場景BLOB直接存儲數據一致性高…
閱讀更多...
區塊鏈技術概述：從比特幣到Web3.0

區塊鏈技術概述：從比特幣到Web3.0

目錄 區塊鏈技術概述&#xff1a;從比特幣到Web3.0引言&#xff1a;數字革命的下一篇章1. 區塊鏈技術基礎1.1 區塊鏈定義與核心特征1.2 區塊鏈數據結構可視化 2. 比特幣&#xff1a;區塊鏈的開端2.1 比特幣的核心創新2.2 比特幣交易生命周期 3. 以太坊與智能合約革命3.1 以太坊…
閱讀更多...
Petrel導入well數據

Petrel導入well數據

加載井口位置數據&#xff1a;井頭文件應包括name, X, Y, KB, TD這些基本信息&#xff0c;文件格式為txt或prn格式都可。具體步驟&#xff1a;① input面板下?右鍵import file&#xff0c;進入import file界面&#xff0c;選擇文件格式?well heads&#xff08;*.*&#xff09…
閱讀更多...
51c嵌入式~電路~合集8

51c嵌入式~電路~合集8

我自己的原文哦~ https://blog.51cto.com/whaosoft/12175265 一、高頻電路布線的十大絕招 1 多層板布線 高頻電路往往集成度較高&#xff0c;布線密度大&#xff0c;采用多層板既是布線所必須&#xff0c;也是降低干擾的有效手段。在PCB Layout階段&#xff0c;合理的…
閱讀更多...
【LLM學習筆記3】搭建基于chatgpt的問答系統（下）

【LLM學習筆記3】搭建基于chatgpt的問答系統（下）

目錄 一、檢查結果檢查有害內容檢查是否符合產品信息 二、搭建一個簡單的問答系統三、評估輸出1.當存在一個簡單的正確答案2.當不存在一個簡單的正確答案 一、檢查結果 本章將引領你了解如何評估系統生成的輸出。在任何場景中&#xff0c;無論是自動化流程還是其他環境&#x…
閱讀更多...
多項目資料如何統一歸檔與權限管理

多項目資料如何統一歸檔與權限管理

在多項目管理環境中&#xff0c;統一資料歸檔與權限管控的關鍵在于&#xff1a;規范化文件結構、自動化歸檔流程、分級權限控制。其中&#xff0c;規范化文件結構是實現統一歸檔的第一步&#xff0c;它直接決定后續歸類、檢索和審計的效率。通過預設項目模板&#xff0c;明確文…
閱讀更多...
【RTP】基于mediasoup的RtpPacket的H.264打包、解包和demo 1:不含擴展

【RTP】基于mediasoup的RtpPacket的H.264打包、解包和demo 1:不含擴展

目前打包、解包沒有對擴展進行操作 測試結果 === H.264 RTP Packetization and Depacketization Test ===1. Generating simulated H.264 frames... Generated 6 H.264 frames2. Packetizing H.264 frames to RTP packets...Frame #0 (size: 1535 bytes, I-fra
閱讀更多...
【AI論文】Sekai：面向世界探索的視頻數據集

【AI論文】Sekai：面向世界探索的視頻數據集

摘要&#xff1a;視頻生成技術已經取得了顯著進展&#xff0c;有望成為交互式世界探索的基礎。然而&#xff0c;現有的視頻生成數據集并不適合用于世界探索訓練&#xff0c;因為它們存在一些局限性&#xff1a;地理位置有限、視頻時長短、場景靜態&#xff0c;以及缺乏關于探索…
閱讀更多...
websocket服務端開發

websocket服務端開發

websocket技術在服務端實時消息的推送和im聊天系統中得到了廣泛應用。作為一名后端研發人員，這其中又有哪些需要了解和注意的問題點呢？接下來，我一一進行闡明。 SpringBoot項目中引入依賴 引入依賴 <!--websocket支持包--> <dependency> <…
閱讀更多...
學歷信息查詢API (IVYZ9A2B) 的對接實戰 | 天遠API

學歷信息查詢API (IVYZ9A2B) 的對接實戰 | 天遠API

摘要 本文是天遠API學歷信息查詢API&#xff08;接口代碼&#xff1a;IVYZ9A2B&#xff09;的深度技術解析文檔。作為一名開發者&#xff0c;我將從實際應用場景出發&#xff0c;詳細介紹該接口的調用方法、數據結構和最佳實踐。無論您是在開發招聘系統、教育管理平臺&#xf…
閱讀更多...
2025年- H84-Lc192--75.顏色分類(技巧、三路指針排序）--Java版

2025年- H84-Lc192--75.顏色分類(技巧、三路指針排序）--Java版

1.題目描述 2.思路 3.代碼實現 class Solution {public void sortColors(int[] nums) {int low 0; // 下一個 0 應該放的位置int mid 0; // 當前檢查的位置int high nums.length - 1; // 下一個 2 應該放的位置while (mid < high) {if (nums[mid] …
閱讀更多...
最新文章

Copyright @ 2022~2023