informed-rrt*代码实现

Informed-RRT* 是一种基于 RRT* 的路径规划算法，它在 RRT* 的基础上引入一个启发式函数来指导 RRT* 的生长方向，从而使得算法更加高效和优化。下面是 Informed-RRT* 的代码实现：

import numpy as np
import matplotlib.pyplot as plt
import math

class Node:
    def __init__(self, x, y):
        self.x = x
        self.y = y
        self.parent = None
        self.cost = 0.0

class RRTStar:
    def __init__(self, start, goal, obstacleList, randArea,
                 expandDis=0.5, goalSampleRate=20, maxIter=500):
        self.start = Node(start[0], start[1])
        self.goal = Node(goal[0], goal[1])
        self.minrand = randArea[0]
        self.maxrand = randArea[1]
        self.expandDis = expandDis
        self.goalSampleRate = goalSampleRate
        self.maxIter = maxIter
        self.obstacleList = obstacleList
        self.nodeList = [self.start]

    def planning(self):
        for i in range(self.maxIter):
            rnd = self.get_random_point()
            nearestInd = self.get_nearest_node_index(self.nodeList, rnd)
            nearestNode = self.nodeList[nearestInd]
            newNode = self.steer(nearestNode, rnd, self.expandDis)
            if self.check_collision(newNode, self.obstacleList):
                nearInds = self.find_near_nodes(newNode)
                newNode = self.choose_parent(newNode, nearInds)
                if newNode:
                    self.nodeList.append(newNode)
                    self.rewire(newNode, nearInds)

            if i % 5 == 0:
                self.try_goal_path()

        lastIndex = self.search_best_goal_node()
        if lastIndex is None:
            return None

        path = self.get_final_path(lastIndex)
        return path

    def steer(self, fromNode, toNode, extendLength=float("inf")):
        newnode = Node(fromNode.x, fromNode.y)
        dis, angle = self.get_distance_and_angle(newnode, toNode)
        newnode.cost = fromNode.cost + dis
        if extendLength > dis:
            extendLength = dis
        newnode.x += extendLength * math.cos(angle)
        newnode.y += extendLength * math.sin(angle)
        newnode.parent = fromNode
        return newnode

    def choose_parent(self, newNode, nearInds):
        if not nearInds:
            return None

        costs = []
        for i in nearInds:
            nearNode = self.nodeList[i]
            tNode = self.steer(nearNode, newNode)
            if tNode and self.check_collision(tNode, self.obstacleList):
                costs.append(nearNode.cost + self.get_distance_and_angle(nearNode, newNode)[0])
            else:
                costs.append(float("inf"))

        minCost = min(costs)
        if minCost == float("inf"):
            return None

        minInd = nearInds[costs.index(minCost)]
        newNode.cost = minCost
        newNode.parent = self.nodeList[minInd]
        return newNode

    def rewire(self, newNode, nearInds):
        for i in nearInds:
            nearNode = self.nodeList[i]
            tNode = self.steer(newNode, nearNode)
            if tNode and self.check_collision(tNode, self.obstacleList):
                nearCost = newNode.cost + self.get_distance_and_angle(newNode, nearNode)[0]
                if nearCost < nearNode.cost:
                    nearNode.parent = newNode
                    nearNode.cost = nearCost

    def search_best_goal_node(self):
        dist_to_goal_list = [self.get_distance_and_angle(n, self.goal)[0] for n in self.nodeList]
        goal_inds = [dist_to_goal_list.index(i) for i in dist_to_goal_list if i <= self.expandDis]
        safe_goal_inds = [ind for ind in goal_inds if self.check_collision(self.nodeList[ind], self.obstacleList)]

        if not safe_goal_inds:
            return None

        minCost = min([self.nodeList[ind].cost for ind in safe_goal_inds])
        for ind in safe_goal_inds:
            if self.nodeList[ind].cost == minCost:
                return ind

        return None

    def find_near_nodes(self, newNode):
        n = len(self.nodeList) + 1
        r = min(self.expandDis * math.sqrt((math.log(n) / n)), self.expandDis)
        dist_list = [(node.x - newNode.x) ** 2 + (node.y - newNode.y) ** 2 for node in self.nodeList]
        near_inds = [dist_list.index(i) for i in dist_list if i <= r ** 2]
        return near_inds

    def check_collision(self, node, obstacleList):
        for obs in obstacleList:
            if math.hypot(node.x - obs[0], node.y - obs[1]) <= obs[2]:
                return False
        return True

    def get_random_point(self):
        if np.random.randint(0, 100) > self.goalSampleRate:
            rand = [np.random.uniform(self.minrand, self.maxrand),
                    np.random.uniform(self.minrand, self.maxrand)]
        else:
            rand = [self.goal.x, self.goal.y]
        return rand

    def get_nearest_node_index(self, nodeList, rnd):
        dlist = [(node.x - rnd[0]) ** 2 + (node.y - rnd[1]) ** 2 for node in nodeList]
        minIndex = dlist.index(min(dlist))

        return minIndex

    def get_distance_and_angle(self, fromNode, toNode):
        dx = toNode.x - fromNode.x
        dy = toNode.y - fromNode.y
        return math.hypot(dx, dy), math.atan2(dy, dx)

    def try_goal_path(self):
        lastIndex = self.search_best_goal_node()
        if lastIndex is None:
            return False

        path = self.get_final_path(lastIndex)
        if path is None:
            return False

        dist_to_goal = self.get_distance_and_angle(self.nodeList[lastIndex], self.goal)[0]
        if dist_to_goal <= self.expandDis:
            return True

        return False

    def get_final_path(self, lastIndex):
        path = []
        while self.nodeList[lastIndex].parent is not None:
            node = self.nodeList[lastIndex]
            path.append([node.x, node.y])
            lastIndex = self.nodeList.index(node.parent)
        path.append([self.start.x, self.start.y])
        return path

在代码中，Node 类表示树中的一个节点，包含节点的坐标、父节点、以及到起点的代价 cost。RRTStar 类是整个算法的实现，包括路径规划的主要流程、启发式函数的计算以及树的生长和更新等。具体来说，算法主要分为以下几个步骤：

1. 初始化：设置起点、终点、随机采样区域、最大迭代次数、扩展步长等参数，创建一个包含起点的节点列表。
2. 随机采样：以一定概率在终点和随机采样区域中进行随机采样，得到一个随机点。
3. 最近邻节点：从节点列表中找到距离随机点最近的节点，作为新节点的父节点。
4. 扩展节点：将新节点从父节点处按照设定的步长扩展，得到一个新节点。
5. 碰撞检测：判断新节点是否与障碍物相撞，若碰撞，则跳过此节点。
6. 选择父节点：在新节点的周围一定距离内，选择一个代价最小的节点作为其父节点。
7. 更新代价：对于每个新节点的父节点，计算更新其代价。
8. 搜索最优路径：找到距离终点在一定范围内的所有节点，并计算它们到终点的代价，选择代价最小的节点作为最终目标节点。
9. 获取最终路径：从最终目标节点开始，沿着节点的父节点指针一直遍历到起点，得到最终路径。

在算法实现中，为了提高算法效率，增加了一些优化措施，如限制随机采样和碰撞检测的频率、使用启发式函数引导树的生长等。此外，要注意在实现碰撞检测时，需要将障碍物表示为圆形，判断节点是否与障碍物相撞时，需要计算节点与圆心之间的距离是否小于半径。