Python實現-BIT*-Batch Informed Tree 運動規劃算法

RRT系列的採樣規劃算法，其隨機採樣特性可以有效解決維度災難的問題

RRT*通過改進RRT的節點擴展方式，加入重連的機制，可以實現路徑質量的漸近最優

BIT*結合了採樣規劃算法和搜索規劃算法的優勢，引入節點排序和邊排序，在超橢球子集中執行排序搜索，可以避免將無價值的節點和邊加入到生成樹中，下面是BIT*的僞代碼

僞代碼參考文獻：

J. D. Gammell, S. S. Srinivasa, and T. D. Barfoot, “Batch informed trees BIT*: Sampling-based optimal planning via the heuristically guided search of implicit random geometric graphs,” in 2015 IEEE International Conference on Robotics and Automation (ICRA), May 2015, pp. 3067– 3074.

在僞代碼中，有幾個部分比較難以理解：

1、line10-11. 這兩行實質是在對邊隊列進行擴展，如論文所說：

This value (vertex-queue value) is a lower bound estimate of the edge-queue values from a vertex

就是說，點的排序值，是經過該點的邊的排序值的下界

line 10 對應只有某個點的排序值，比當前邊的最好排序值還要好，這個點纔會被用於去擴展邊。否則，這個點擴展出來的邊肯定不會比當前 edge-queue 裏的最好邊要好，那目前就沒有擴展它的必要

2、line14-16. 這三行實質是在判斷邊的價值，是否應該把它加入到生成樹中

line14，不等式左邊其實就是這條邊的排序值，也是目前 edge-queue 中的最小值，如果這個最小值都比當前路徑長度要大，說明 edge-queue 裏已經沒有好的邊了，也說明這是當前採樣節點可以找到的最好的路徑，因此需要進入下一個batch 加入新的採樣節點

line15，主要是計算這條邊之間是否可以到達，一般要通過碰撞檢測這樣的比較耗時的方式來計算，因此上一個不等式判斷確實可以減少不必要的碰撞檢測計算

line16，爲了判斷經過Vm是否可以對原節點價值帶來提升

在滿足上面三個條件的情況下，這條邊纔會真正加入到生成樹中，如果Xm已經在樹中，那就對應邊從重連過程，相反就是邊的擴展。

更多的細節可以參考原論文，下面給出上述算法的python實現主函數：

def plan(self, pathLengthLimit, choise):
        init_time = time.time()
        radius_constant = self.setup_planning()
        time_and_cost_set = []
        path = []
        for i in range(self.maxIter):
            if not self.vertex_queue and not self.edge_queue:
                c_best = self.g_scores[self.goal]
                path = self.get_best_path()
                self.prune(c_best)
                self.samples.extend(self.informed_sample(c_best, self.batch_size))

                self.old_vertices = set(self.vertices)
                self.vertex_queue = [(self.get_point_value(point),point) for point in self.vertices]
                heapq.heapify(self.vertex_queue)    # change to op priority queue
                q = len(self.vertices)+len(self.samples)
                self.r = radius_constant * ((math.log(q) / q) ** (1.0/self.dimension))

            while self.bestVertexQueueValue() <= self.bestEdgeQueueValue():
                _, point = heapq.heappop(self.vertex_queue)
                self.expand_vertex(point)

            best_edge_value, bestEdge = heapq.heappop(self.edge_queue)

            # Check if this can improve the current solution
            if best_edge_value < self.g_scores[self.goal]:
                actual_cost_of_edge = self.actual_edge_cost(bestEdge[0], bestEdge[1])
                actual_f_edge = self.heuristic_cost(self.start, bestEdge[0]) + actual_cost_of_edge + self.heuristic_cost(bestEdge[1], self.goal)
                if actual_f_edge < self.g_scores[self.goal]:
                    actual_g_score_of_point = self.get_g_score(bestEdge[0]) + actual_cost_of_edge
                    if actual_g_score_of_point < self.get_g_score(bestEdge[1]):
                        self.g_scores[bestEdge[1]] = actual_g_score_of_point
                        self.edges[bestEdge[1]] = bestEdge[0]
                        if bestEdge[1] not in self.vertices:
                            self.samples.remove(bestEdge[1])
                            self.vertices.append(bestEdge[1])
                            heapq.heappush(self.vertex_queue,(self.get_point_value(bestEdge[1]),bestEdge[1]))

                        self.edge_queue = [ item for item in self.edge_queue if item[1][1]!=bestEdge[1] or \
                                            self.get_g_score(item[1][0]) + self.heuristic_cost(item[1][0],item[1][1])<self.get_g_score(item[1][0]) ]
                        heapq.heapify(self.edge_queue)      # Rebuild the priority queue because it will be destroyed after the element is removed

                        # self.plot_function()
                        # exit()
            else:
                self.vertex_queue = []
                self.edge_queue = []
                print("Step:", i, "Path Length:", self.g_scores[self.goal], "Planer: BIT*")

完整代碼參考：https://github.com/hichenway/Batch-Informed-Trees