基于Python的RRT算法實現

• 一、RRT算法原理及實現效果
• 二、RRT算法python代碼實現

本文為博主原創內容，未經博主允許不得轉載。尊重他人知識成果，共同建立良好學習分享氛圍，謝謝！

一、RRT算法原理及實現效果

??關于RRT算法的原理，博主之前看到的RRT算法原理圖解這篇文章講的很好，通俗易懂，所以若不了解RRT算法的原理，推薦各位看這個了解下。

??本文主要是基于Python實現RRT算法，先來看一下RRT算法實現后的效果圖。

圖1 RRT算法的實現效果圖

二、RRT算法python代碼實現

??具體實現代碼如下，博主寫的可能有些復雜，不過其中大部分代碼注釋的很詳細了，在這里我就不在贅述了。

import matplotlib.pyplot as plt import random import math import timedef main():starttime = time.time()# 程序運行時間計時開始region1 = region(100,100)# 定義單元區域obstacle_area1 = obstacle_area((50,0),(50,80),(60,0),(60,80))# 定義障礙物區域tree_point = [] # 存放起點、終點和所有的可用衍生點futher = {} # 存放所有可用衍生點的父節點step = 3# 步進系數breed_point = 'breed_point is invalid'draw_tree_pointx = []# 存放可用衍生點的橫坐標draw_tree_pointy = []# 存放可用衍生點的縱坐標draw_plan_pointx = []# 存放從起點到終點的計劃路徑點的橫坐標draw_plan_pointy = []# 存放從起點到終點的計劃路徑點的縱坐標initial_point = point()# 實例化起點對象initial_point.set_position(10,10)# 設定起點坐標initial_point.set_state(in_obstacle_area(initial_point,obstacle_area1,region1))# 設定起點狀態if initial_point.state == False:# 若起點在障礙物區域，直接退出print("initial point is live in obstacle region , please choose initial point again")returnterminal_point = point()# 實例化終點對象terminal_point.set_position(90,50)# 設定終點坐標terminal_point.set_state(in_obstacle_area(terminal_point,obstacle_area1,region1))# 設定終點狀態if terminal_point.state == False:# 若終點在障礙物區域，直接退出print("terminal point is live in obstacle region , please choose terminal point again")returntree_point.append([initial_point.x, initial_point.y])# 把起點加入tree_point集合列表中while (breed_point == 'breed_point is invalid') or (math.sqrt((tree_point[-1][0] - terminal_point.x) ** 2 + (tree_point[-1][1] - terminal_point.y) ** 2) > 3):radom_point = random_num(region1,obstacle_area1)# 生成有效隨機點min_point = MinPoint(tree_point, radom_point)# 從tree_point合集列表中選取距離有效隨機點最近的點breed_point = BreedPoint(min_point, radom_point, step, obstacle_area1,region1)# 在距離有效隨機點最近的點的基礎上選取有效的衍生點if breed_point != 'breed_point is invalid':futher[tuple(breed_point.get_position())] = tuple(breed_point.get_futher())# 記錄有效衍生點的父節點tree_point.append([breed_point.x, breed_point.y])# 將有效衍生點加入到衍生點集合列表draw_tree_pointx.append(tree_point[-1][0])# 記錄可用衍生點的橫坐標draw_tree_pointy.append(tree_point[-1][1])# 記錄可用衍生點的縱坐標futher[tuple(terminal_point.get_position())] = tuple(tree_point[-1])# 將終點的父節點設置為最后加入的tree_point點tree_point.append([terminal_point.x, terminal_point.y])# 把終點加入tree_point集合列表中current_cheak_point = tuple(terminal_point.get_position())while futher[current_cheak_point] != tuple(initial_point.get_position()):# 從終點開始往回尋找父節點，直到找到起點為止draw_plan_pointx.append(current_cheak_point[0])# 記錄從起點到終點的計劃路徑點的橫坐標draw_plan_pointy.append(current_cheak_point[1])# 記錄從起點到終點的計劃路徑點的縱坐標current_cheak_point = futher[current_cheak_point]draw_plan_pointx.append(current_cheak_point[0]) # 記錄父節點為起點的點的橫坐標draw_plan_pointx.append(initial_point.x)# 記錄起點的橫坐標draw_plan_pointy.append(current_cheak_point[1]) # 記錄父節點為終點的點的橫坐標draw_plan_pointy.append(initial_point.y)# 記錄終點的橫坐標plt.scatter(draw_tree_pointx, draw_tree_pointy, c='green', s=3)# 畫出所有的可用衍生點plt.plot([obstacle_area1.obstacle_point1[0],obstacle_area1.obstacle_point2[0],obstacle_area1.obstacle_point4[0],obstacle_area1.obstacle_point3[0],obstacle_area1.obstacle_point1[0]],[obstacle_area1.obstacle_point1[1],obstacle_area1.obstacle_point2[1],obstacle_area1.obstacle_point4[1],obstacle_area1.obstacle_point3[1],obstacle_area1.obstacle_point1[1]],color='r', markerfacecolor='blue', marker='o')# 畫處障礙物區域plt.plot(draw_plan_pointx, draw_plan_pointy, color='r', markerfacecolor='blue', marker='o')# 畫出從起點到終點的計劃路徑點plt.axis("equal")plt.grid(True)plt.title('initial_point' + str(initial_point.get_position()) + ' , terminal_point:' + str(terminal_point.get_position()))plt.show()endtime = time.time()# 程序運行時間計時結束dtime = endtime - starttime# 計算程序運行時長print("程序運行時間：%.8s s" % dtime) # 顯示到微秒def BreedPoint(min_point, radom_point, step,obstacle_area,region):# 在距離有效隨機點最近的點的基礎上選取有效的衍生點New_BreedPoint = point()# 實例化衍生點對象if (min_point[0] == radom_point.x):# 若有效隨機點和距離其最近的點的橫坐標一樣New_BreedPoint.set_position(min_point[0], step + min_point[1])else:k = (min_point[1] - radom_point.y) / (min_point[0] - radom_point.x)# 計算有效隨機點和距離其最近的點之間的斜率New_BreedPoint_x = math.cos(math.atan(k)) * step + min_point[0]New_BreedPoint_y = math.sin(math.atan(k)) * step + min_point[1]New_BreedPoint.set_position(New_BreedPoint_x, New_BreedPoint_y)if in_obstacle_area(New_BreedPoint, obstacle_area,region) is False:# 判斷點是否在有效區域內return 'breed_point is invalid'New_BreedPoint.set_futher(min_point)# 設置衍生點的父節點return New_BreedPointdef random_num(region,obstacle_area):# 生成有效隨機點radom_point = point()# 實例化隨機點對象radom_point.set_position(random.randint(0, region.length), random.randint(0, region.width))# 生成隨機點坐標if in_obstacle_area(radom_point, obstacle_area,region) is False:# 判斷隨機點是否在無效區域內random_num(region,obstacle_area)# 重新生成隨機節點return radom_pointdef in_obstacle_area(check_point,obstacle_area,region):# 判斷點是否在有效區域內if ((check_point.x >= obstacle_area.obstacle_point1[0]) and (check_point.x <= obstacle_area.obstacle_point3[0]) and (check_point.y <= obstacle_area.obstacle_point2[1]) and (check_point.y >= obstacle_area.obstacle_point1[1])) :# 判斷點是否在障礙物區域內return Falseelif ((check_point.x >= 0) and (check_point.x <= region.length) and (check_point.y >= 0) and (check_point.y <= region.width)):# 判斷點是否在單元區域內return Trueelse:return Falsedef MinPoint(tree_point, radom_point):# 從tree_point合集列表中選取距離有效隨機點最近的點Distance = []for i in range(0, len(tree_point)):Distance.append(math.sqrt((tree_point[i][0] - radom_point.x) ** 2 + (tree_point[i][1] - radom_point.y) ** 2))return tree_point[Distance.index(min(Distance))]class point():def set_position(self, x, y):self.x = xself.y = ydef set_state(self, state):self.state = statedef set_futher(self, futher):self.futher = futherdef get_position(self):return [self.x, self.y]def get_state(self):return self.statedef get_futher(self):return self.futherclass region():def __init__(self,length,width):self.length = lengthself.width = widthclass obstacle_area():def __init__(self,obstacle_point1,obstacle_point2,obstacle_point3,obstacle_point4):self.obstacle_point1 = obstacle_point1# 左下點self.obstacle_point2 = obstacle_point2# 左上點self.obstacle_point3 = obstacle_point3# 右下點self.obstacle_point4 = obstacle_point4# 右上點if __name__ == '__main__':main()

總結

以上是生活随笔為你收集整理的基于Python的RRT算法实现的全部內容，希望文章能夠幫你解決所遇到的問題。

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