【啟發式算法】RRT*算法詳細介紹（Python）

????????📢本篇文章是博主人工智能（AI）領域學習時，用于個人學習、研究或者欣賞使用，并基于博主對相關等領域的一些理解而記錄的學習摘錄和筆記，若有不當和侵權之處，指出后將會立即改正，還望諒解。文章分類在👉啟發式算法專欄：

? ? ? ?【啟發式算法】（9）---《RRT*算法詳細介紹（Python）》

【啟發式算法】RRT*算法詳細介紹（Python）

1. RRT*算法

2.算法原理

RRT*與RRT的區別

3.算法步驟

步驟詳細說明

4.RRT*的關鍵原理

1. 樹的擴展

2. 路徑優化

3. 連接最短路徑

4. 漸進最優性

[Python] RRT*算法實現

[Results] 運行結果

[Notice]? 注意事項

5.優點與不足

6.總結

1. RRT*算法

????????RRT*算法（Rapidly-exploring Random Tree Star）是一種用于機器人路徑規劃的算法，旨在為機器人找到從起點到目標的最短路徑，同時避免障礙物。它是基于RRT（Rapidly-exploring Random Tree）算法的改進版，具有更高的路徑質量和優化能力。RRT*的關鍵特點是它能夠在搜索過程中逐漸優化路徑，最終找到一條接近最短的路徑。

2.算法原理

????????想象你在一個森林里，從某個點出發（起點），想找到一條通向小屋（終點）的路徑。森林里有很多樹（障礙物），你看不到小屋在哪里。你開始隨機地朝不同方向走，記錄你走過的路，如果碰到障礙就繞開。

這就是 RRT 的基本思路：“快速而隨機地擴展路徑”。

而 RRT* 在這個基礎上更聰明：每走一步，它還會看一下有沒有更短的路線可以替換原來的路線。

**RRT*與RRT的區別**

????????RRT*算法與普通的RRT算法相比，最大的區別在于優化過程。RRT只能快速探索空間，但不保證找到最優路徑。而RRT*則通過不斷優化已經找到的路徑，保證能找到一條接近最優的路徑。

3.算法步驟

????????RRT*的核心思想是通過不斷擴展一個隨機樹，探索搜索空間，同時在擴展過程中優化路徑。算法的基本步驟如下：

**初始化樹**：從起點出發，初始化一個樹（可以是RRT樹），樹的根節點是起點。
?**隨機采樣**：在空間中隨機采樣一個點，這個點是機器人可能需要經過的位置。
?**擴展樹**：通過找到樹中距離該隨機采樣點最近的節點，然后朝這個隨機點擴展一小步。樹會不斷向外擴展，探索新的區域。
**連接路徑**：擴展樹的過程中，算法會檢查是否能夠找到更優的路徑。如果新的節點能夠連接到樹中某個更接近目標的節點，或者可以替換掉原本的路徑，那么就會更新樹的結構，尋找更短的路徑。
**優化路徑**：算法會在樹擴展的過程中，反復優化路徑，以確保最終路徑是最優的。

步驟詳細說明

????????假設我們有一個機器人，起點在`(0, 0)`，目標點在`(10, 10)`，空間中有障礙物。我們來一步步看RRT*算法是如何工作的：

?1. 初始化樹

????????我們從起點`(0, 0)`開始，構建一個初始樹。這個樹只有一個節點，即起點。

2. 隨機采樣

????????我們從空間中隨機選擇一個點，假設選擇的是`(7, 4)`。這個點并不一定在樹上，但我們希望樹可以通過某種方式擴展到這個點。

3. 擴展樹

????????找到樹中離`(7, 4)`最近的節點，比如起點`(0, 0)`，然后從`(0, 0)`朝著`(7, 4)`方向擴展，假設擴展的步長是1，所以新節點的位置可能是`(1, 0.57)`。這個新節點被加入到樹中，樹就變得更大了。

4. 檢查路徑優化

????????如果新的節點在擴展時發現了更短的路徑，或者有可能替代之前的路徑，算法就會更新路徑。例如，如果擴展的過程中，我們發現從`(1, 0.57)`到目標點`(10, 10)`的路徑比原來的路徑更短，算法就會更新路徑。

5. 重復以上步驟

????????算法會繼續重復以上步驟，直到找到一條從起點到目標點的路徑。隨著樹的擴展，路徑不斷優化，最終得到一條最短的路徑。

6. 結束

????????當樹擴展到目標點附近，算法停止，輸出路徑。

4.RRT*的關鍵原理

????????RRT*的核心原理不僅僅是樹的擴展，而是在每次擴展時進行路徑優化。為了保證路徑的最優性，RRT*會在每次擴展時，檢查是否能找到更短的路徑。這個過程實際上是一個漸進最優的過程。

1. 樹的擴展

????????樹的擴展是RRT*的基本操作，在空間中隨機選擇一個點，并擴展樹的結構。這一過程類似于RRT算法中的“樹擴展”步驟，但RRT*在擴展時增加了路徑優化步驟。

給定一個隨機采樣點 $q_{\text{rand}}$ ，樹中某個節點 $q_{\text{nearest}}$ ，RRT*的目標是從 $q_{\text{nearest}}$ 朝 $q_{\text{rand}}$ ?的 $q_{\text{new}}$

$q_{\text{new}} = q_{\text{nearest}} + \Delta t \cdot \frac{q_{\text{rand}} - q_{\text{nearest}}}{\|q_{\text{rand}} - q_{\text{nearest}}\|}$

其中， $\Delta t$ 是擴展步長， $q_{\text{new}}$ 是新節點。

??????計算?新節點的代價：假設在某一時刻，樹中有一個節點 $q_{\text{parent}}$ ，我們從這個節點擴展到新節點 $q_{\text{new}}$ ，則新節點的代價可以定義為：

$C(q_{\text{new}}) = C(q_{\text{parent}}) + \text{cost}(q_{\text{parent}}, q_{\text{new}})$
其中， $\text{cost}(q_{\text{parent}}, q_{\text{new}})$ 是從父節點到新節點的代價，通常計算為兩點之間的歐幾里得距離：

$\text{cost}(q_{\text{parent}}, q_{\text{new}}) = \|q_{\text{new}} - q_{\text{parent}}\|$

2. 路徑優化

????????在RRT*中，路徑優化是一個至關重要的步驟，它能夠保證路徑的最優性。優化過程的核心思想是檢查新擴展的節點是否可以通過某些已有節點來形成更短的路徑。具體來說，如果我們有兩個節點 $q_{\text{new}}$ ?和 $q_{\text{neighbour}}$ ，那么如果經過 $q_{\text{new}}$ 到 $q_{\text{neighbour}}$ ?的路徑比原有路徑更短，就更新路徑。

????????在這個過程中，RRT*需要計算從新節點到樹中某些節點的路徑長度，并選擇能夠帶來路徑優化的節點。這個步驟在每次擴展時都進行，最終產生一條近似最優的路徑。

????????假設新節點 $q_{\text{new}}$ ?通過與樹中的節點 $q_{\text{neighbour}}$ 連接來優化路徑。如果經過 $q_{\text{neighbour}}$ 的路徑比原路徑更短，則更新路徑。即，若有：

$C(q_{\text{neighbour}}) > C(q_{\text{new}}) + \text{cost}(q_{\text{new}}, q_{\text{neighbour}})$

則更新 $q_{\text{neighbour}}$ ?的父節點為 $q_{\text{new}}$ ，并更新路徑。

3. 連接最短路徑

????????RRT*的優化不僅體現在路徑本身，還包括通過連接新節點和樹中的某些節點，生成最短路徑。假設已經有了一條路徑 $\pi_1$ ，從起點到目標點。現在我們希望通過添加新的節點來更新這條路徑。若新的路徑 $\pi_2$ 比原路徑更短，則替換原路徑。更新過程可以通過“Rewiring”來完成，即調整樹結構，使路徑變得更短。

4. 漸進最優性

????????RRT*的一個顯著特征是它的漸進最優性。隨著擴展步數的增加，RRT*會不斷優化路徑，最終趨近于最優路徑。這種漸進最優性可以通過以下公式描述：

????????在給定的空間中，如果我們對樹進行足夠多的擴展，路徑的代價 $C_{\text{path}}$ 會趨近于最優路徑的代價 $C^*$ ，滿足：

$C_{\text{path}} \to C^* \quad \text{as} \quad n \to \infty$

其中， $n$ $是擴展的次數。

[Python] RRT*算法實現

????????RRT*算法我也是基于一位大佬的算法應用的，在大佬的代碼基礎上做了一點修改。

?項目代碼我已經放入下面鏈接里面，可以通過下面鏈接跳轉：🔥

【啟發式算法】--- RRT*算法?

若是下面代碼復現困難或者有問題，也歡迎評論區留言。

"""《RRT*算法實現》時間：2025.06.25"""
import math
import os
import sysimport matplotlib.pyplot as plt
from celluloid import Camera  # 保存動圖時用，pip install celluloid
sys.path.append("../RRT")
try:from rrt_planning import RRT
except ImportError:raiseshow_animation = Trueclass RRTStar(RRT):"""Class for RRT Star planning"""class Node(RRT.Node):def __init__(self, x, y):super().__init__(x, y)self.cost = 0.0def __init__(self,start,goal,obstacle_list,rand_area,expand_dis=3.0,goal_sample_rate=20,max_iter=50000,connect_circle_dist=50.0,search_until_max_iter=False,robot_radius=0.0):"""Setting Parameterstart:Start Position [x,y]goal:Goal Position [x,y]obstacleList:obstacle Positions [[x,y,size],...]randArea:Random Sampling Area [min,max]"""super().__init__(start, goal, obstacle_list, rand_area, expand_dis,goal_sample_rate, max_iter,robot_radius=robot_radius)self.connect_circle_dist = connect_circle_distself.goal_node = self.Node(goal[0], goal[1])self.search_until_max_iter = search_until_max_iterdef planning(self, animation=True, camera=None):"""rrt star path planninganimation: flag for animation on or off ."""self.node_list = [self.start]for i in range(self.max_iter):if i % 100 == 0:print("Iter:", i, ", number of nodes:", len(self.node_list))# print("Iter:", i, ", number of nodes:", len(self.node_list))rnd = self.sample_free()nearest_ind = self.get_nearest_node_index(self.node_list, rnd)new_node = self.steer(self.node_list[nearest_ind], rnd,self.expand_dis)near_node = self.node_list[nearest_ind]# 計算代價,歐氏距離new_node.cost = near_node.cost + math.hypot(new_node.x-near_node.x, new_node.y-near_node.y)if self.obstacle_free(new_node, self.obstacle_list, self.robot_radius):near_inds = self.find_near_nodes(new_node) # 找到x_new的鄰近節點node_with_updated_parent = self.choose_parent(new_node, near_inds) # 重新選擇父節點# 如果父節點更新了if node_with_updated_parent:# 重布線self.rewire(node_with_updated_parent, near_inds)self.node_list.append(node_with_updated_parent)else:self.node_list.append(new_node)if animation and i % 100 ==0:self.draw_graph(rnd, camera)if ((not self.search_until_max_iter) and new_node):  # if reaches goallast_index = self.search_best_goal_node()if last_index is not None:return self.generate_final_course(last_index)print("reached max iteration")last_index = self.search_best_goal_node()if last_index is not None:return self.generate_final_course(last_index)return Nonedef choose_parent(self, new_node, near_inds):"""在新產生的節點 $x_{new}$ 附近以定義的半徑范圍$r$內尋找所有的近鄰節點 $X_{near}$，作為替換 $x_{new}$ 原始父節點 $x_{near}$ 的備選我們需要依次計算起點到每個近鄰節點 $X_{near}$ 的路徑代價 加上近鄰節點 $X_{near}$ 到 $x_{new}$ 的路徑代價，取路徑代價最小的近鄰節點$x_{min}$作為 $x_{new}$ 新的父節點Arguments:--------new_node, Noderandomly generated node with a path from its neared pointThere are not coalitions between this node and th tree.near_inds: listIndices of indices of the nodes what are near to new_nodeReturns.------Node, a copy of new_node"""if not near_inds:return None# search nearest cost in near_indscosts = []for i in near_inds:near_node = self.node_list[i]t_node = self.steer(near_node, new_node)if t_node and self.obstacle_free(t_node, self.obstacle_list, self.robot_radius):costs.append(self.calc_new_cost(near_node, new_node))else:costs.append(float("inf"))  # the cost of collision nodemin_cost = min(costs)if min_cost == float("inf"):print("There is no good path.(min_cost is inf)")return Nonemin_ind = near_inds[costs.index(min_cost)]new_node = self.steer(self.node_list[min_ind], new_node)new_node.cost = min_costreturn new_nodedef search_best_goal_node(self):dist_to_goal_list = [self.calc_dist_to_goal(n.x, n.y) for n in self.node_list]goal_inds = [dist_to_goal_list.index(i) for i in dist_to_goal_listif i <= self.expand_dis]safe_goal_inds = []for goal_ind in goal_inds:t_node = self.steer(self.node_list[goal_ind], self.goal_node)if self.obstacle_free(t_node, self.obstacle_list, self.robot_radius):safe_goal_inds.append(goal_ind)if not safe_goal_inds:return Nonemin_cost = min([self.node_list[i].cost for i in safe_goal_inds])for i in safe_goal_inds:if self.node_list[i].cost == min_cost:return ireturn Nonedef find_near_nodes(self, new_node):"""1) defines a ball centered on new_node2) Returns all nodes of the three that are inside this ballArguments:---------new_node: Nodenew randomly generated node, without collisions betweenits nearest nodeReturns:-------listList with the indices of the nodes inside the ball ofradius r"""nnode = len(self.node_list) + 1r = self.connect_circle_dist * math.sqrt((math.log(nnode) / nnode))# if expand_dist exists, search vertices in a range no more than# expand_distif hasattr(self, 'expand_dis'):r = min(r, self.expand_dis)dist_list = [(node.x - new_node.x)**2 + (node.y - new_node.y)**2for node in self.node_list]near_inds = [dist_list.index(i) for i in dist_list if i <= r**2]return near_indsdef rewire(self, new_node, near_inds):"""For each node in near_inds, this will check if it is cheaper toarrive to them from new_node.In such a case, this will re-assign the parent of the nodes innear_inds to new_node.Parameters:----------new_node, NodeNode randomly added which can be joined to the treenear_inds, list of uintsA list of indices of the self.new_node which containsnodes within a circle of a given radius.Remark: parent is designated in choose_parent."""for i in near_inds:near_node = self.node_list[i]edge_node = self.steer(new_node, near_node)if not edge_node:continueedge_node.cost = self.calc_new_cost(new_node, near_node)no_collision = self.obstacle_free(edge_node, self.obstacle_list, self.robot_radius)improved_cost = near_node.cost > edge_node.costif no_collision and improved_cost:near_node.x = edge_node.xnear_node.y = edge_node.ynear_node.cost = edge_node.costnear_node.path_x = edge_node.path_xnear_node.path_y = edge_node.path_ynear_node.parent = edge_node.parentself.propagate_cost_to_leaves(new_node)def calc_new_cost(self, from_node, to_node):d, _ = self.calc_distance_and_angle(from_node, to_node)return from_node.cost + ddef propagate_cost_to_leaves(self, parent_node):for node in self.node_list:if node.parent == parent_node:node.cost = self.calc_new_cost(parent_node, node)self.propagate_cost_to_leaves(node)def main():print("Start " )fig = plt.figure(1)camera = Camera(fig) # 保存動圖時使用# camera = None # 不保存動圖時，camara為Noneshow_animation = True# ====Search Path with RRT====# # 生成100個隨機元組的列表import random# obstacle_list = [(random.randint(0, 200), random.randint(0, 200), random.randint(1, 10)) for _ in range(100)]# 分別生成100個x、y坐標對和100個size值positions = [(random.randint(0, 180), random.randint(0, 180)) for _ in range(100)]sizes = [random.randint(1, 10) for _ in range(100)]# 合并為100個三元組obstacle_list = [(x, y, size) for (x, y), size in zip(positions, sizes)]while True:goal = [random.randint(180, 199), random.randint(180, 199)]goal_ = tuple(goal)if goal_ not in positions:break# Set Initial parametersrrt_star = RRTStar(start=[0, 0],goal=goal,rand_area=[0, 200],obstacle_list=obstacle_list,expand_dis=3,robot_radius=0.8)path = rrt_star.planning(animation=show_animation,camera=camera)if path is None:print("Cannot find path")else:print(f"found path!!,路徑長度{len(path)},路徑{path}")# Draw final pathif show_animation:rrt_star.draw_graph(camera=camera)plt.plot([x for (x, y) in path], [y for (x, y) in path], 'r--')plt.grid(True)if camera!=None:camera.snap()animation = camera.animate()animation.save('trajectory.gif')plt.show()if __name__ == '__main__':main()

如果需要完整代碼實現，可在下面自行前往大佬的github，鏈接。?

[Results] 運行結果

[Notice]? 注意事項

?# 環境配置
Python                  3.11.5
torch                   2.1.0
torchvision             0.16.0
gym                     0.26.2

5.優點與不足

?優點：

1. **最優性**：通過不斷優化路徑，RRT\*能夠提供最短路徑（在有限的計算時間內）。
2. **靈活性**：適用于高維空間和復雜環境，能夠處理障礙物的影響。
3. **漸進性**：隨著搜索的進行，路徑會越來越優化，適合動態環境。

不足：

1. **計算量大**：由于需要在每一步優化路徑，計算量較大，可能不適合實時性要求高的應用。
2. **收斂速度慢**：對于復雜環境，算法可能需要較長時間才能找到最優路徑。

6.總結

????????RRT*算法是一種高效的路徑規劃算法，其關鍵在于通過路徑優化和漸進最優性來生成接近最優的路徑。通過在樹擴展過程中不斷進行路徑優化，RRT*能夠解決復雜環境中的路徑規劃問題。它通過多次擴展和優化，在計算資源允許的情況下，可以產生最優或近似最優的路徑。RRT*算法特別適用于復雜的環境。雖然計算量較大，但它的最優性使得它在許多應用中仍然很有價值。