Python实现-BIT*-Batch Informed Tree 运动规划算法-蒲公英云

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*")