RRT-Connect算法

简介

RRT-connect算法是基于RRT算法的一种算法，它从起始点和终点同时生长两棵快速扩展随机树来搜索状态空间，效率会更高。

伪代码：

每一次迭代中，开始步骤与原始的RRT算法一样，都是采样随机点然后进行扩展。然后扩展完第一棵树的新节点𝑞𝑛𝑒𝑤后，以这个新的目标点作为第二棵树扩展的方向。同时第二棵树扩展的方式略有不同（15~22行），首先它会扩展第一步得到𝑞′𝑛𝑒𝑤，如果没有碰撞，继续往相同的方向扩展第二步（贪婪扩展），直到扩展失败或者𝑞′𝑛𝑒𝑤=𝑞𝑛𝑒𝑤表示与第一棵树相连了，即connect了，整个算法结束。

每次选择较小的那棵树进行拓展，搜索速度、搜索效率有了显著提高。

原始RRTConnect算法，未加入贪婪扩展：

class RRTConnect(object):
    """
    desc:两颗tree交替（其中一颗树的node_new无效时交替）进行sample,每棵树的有效node_new作为另一颗树的goal，
    直到其中一颗树的有效node_new与其goal足够近，即表明两棵树connected"""

    def __init__(self, start, goal, iscollidedfunc, jointlimits, expanddis=.5,
                 starttreesamplerate=10, endtreesamplerate=100, maxiter=5000,
                 robot = None, cdchecker = None):
        """

        :param start: nd point, list
        :param goal: nd point, list
        :param iscollidedfunc: a function returns whether collided or not
        :param jointlimits: [[join0low, joint0high], [joint1low, joint1high], ...]
        :param expandDis: how much to expand along the vector randpoint - nearestnode
        :param starttreesamplerate: bias to set randpoint to be goal
        :param endtreesamplerate: bias to set randpoint to be start
        :param maxIter:

        :param the last three parameters are for robotsim robots

        """

        self.__start = np.asarray(start)
        self.__end = np.asarray(goal)

        self.__iscollidedcallback = iscollidedfunc
        self.__jointlimits = jointlimits
        self.__expanddis = expanddis
        self.__starttreesamplerate = starttreesamplerate
        self.__endtreesamplerate = endtreesamplerate
        self.__maxiter = maxiter

        self.__robot = robot
        self.__cdchecker = cdchecker

        self.__nodeliststart = []
        self.__nodelistend = []  # 两棵树
        self.__obstaclelist = []

    @property
    def start(self):
        # read-only property
        return self.__start

    @property
    def end(self):
        # read-only property
        return self.__end

    @property
    def nodeliststart(self):
        # read-only property
        return self.__nodeliststart

    @property
    def nodelistend(self):
        # read-only property
        return self.__nodelistend

    @property
    def jointlimits(self):
        # read-only property
        return self.__jointlimits

    @property
    def iscollidedcallback(self):
        # read-only property
        return self.__iscollidedcallback

    @property
    def cdchecker(self):
        # read-only property
        return self.__cdchecker

    @property
    def robot(self):
        # read-only property
        return self.__robot

    @property
    def expanddis(self):
        # read-only property
        return self.__expanddis

    @property
    def obstaclelist(self):
        # read-only property
        return self.__obstaclelist

    def planning(self, obstaclelist=[], animation=False):
        """
        Pathplanning

        animation: flag for animation on or off

        :return path [[joint0, joint1, ...], [joint0, joint1, ...], ...]
        """

        self.__obstaclelist = obstaclelist
        itercount = 0

        # one sampled point: [point, iscollided]
        sampledpoints = []

        self.__nodeliststart = [Node(self.__start)]
        self.__nodelistend = [Node(self.__end)]

        starttreegoal = self.__end
        endtreegoal = self.__start
        while True:

            if itercount > self.__maxiter:
                print "failed to find a path"
                break

            # if self.__starttreesamplerate < 80:
            #     self.__starttreesamplerate += itercount/float(self.__maxiter)*10
            # else:
            #     self.__starttreesamplerate = 80
            # print self.__starttreesamplerate

            # Random Sampling
            randpoint = []
            if random.randint(0, 100) > self.__starttreesamplerate:
                for i,jntrng in enumerate(self.__jointlimits):
                    randpoint.append(random.uniform(jntrng[0], jntrng[1]))
                randpoint = np.asarray(randpoint)
            else:
                randpoint = copy.deepcopy(starttreegoal)

            # Find nearest node
            nind = self.getNearestListIndex(self.__nodeliststart, randpoint)
            vec = randpoint-self.__nodeliststart[nind].point
            vec = vec/np.linalg.norm(vec)

            # expand tree
            nearestnode = self.__nodeliststart[nind]
            newnode = copy.deepcopy(nearestnode)
            newnode.point += self.__expanddis * vec
            newnode.parent = nind

            if animation:
                drawwspace(self, obstaclelist, randpoint, newnode.point, '^r')

            if self.__iscollidedcallback(newnode.point, obstaclelist,
                                         self.__robot, self.__cdchecker):
                sampledpoints.append([newnode.point, True])
                bswap = False
                # if collided, try the other tree
                while True:
                    randpoint = []
                    if random.randint(0, 100) > self.__endtreesamplerate:
                        for i,jntrng in enumerate(self.__jointlimits):
                            randpoint.append(random.uniform(jntrng[0], jntrng[1]))
                        randpoint = np.asarray(randpoint)
                    else:
                        randpoint = copy.deepcopy(endtreegoal)

                    # Find nearest node
                    nind = self.getNearestListIndex(self.__nodelistend, randpoint)
                    vec = randpoint-self.__nodelistend[nind].point
                    vec = vec/np.linalg.norm(vec)

                    # expand tree
                    nearestnode = self.__nodelistend[nind]
                    newnode = copy.deepcopy(nearestnode)
                    newnode.point += self.__expanddis * vec
                    newnode.parent = nind

                    if animation:
                        drawwspace(self, obstaclelist, randpoint, newnode.point, '^r')

                    if self.__iscollidedcallback(newnode.point, obstaclelist,
                                                 self.__robot, self.__cdchecker):
                        sampledpoints.append([newnode.point, True])
                        bswap = True
                        break

                    sampledpoints.append([newnode.point, False])
                    self.__nodelistend.append(newnode)
                    starttreegoal = newnode.point  # change end tree goal

                    d = np.linalg.norm(newnode.point - endtreegoal)
                    if d <= self.__expanddis:
                        print("reaching the goal")
                        bswap = False
                        break

                    if animation:
                        drawwspace(self, obstaclelist, randpoint, newnode.point, '^g')

                if bswap:
                    continue
                else:
                    break

            sampledpoints.append([newnode.point, False])
            self.__nodeliststart.append(newnode)
            endtreegoal = newnode.point  # change start tree goal

            # check goal
            d = np.linalg.norm(newnode.point - starttreegoal)
            if d <= self.__expanddis:
                print("reaching the goal")
                break

            if animation:
                drawwspace(self, obstaclelist, randpoint, newnode.point, '^g')

            itercount += 1

        path = []
        lastindex = len(self.__nodelistend) - 1
        # WARNING the first nodelist is a numpyarray
        while self.__nodelistend[lastindex].parent is not None:
            node = self.__nodelistend[lastindex]
            path.append(node.point.tolist())
            lastindex = node.parent
        path.append(self.__end.tolist())
        path = path[::-1]
        lastindex = len(self.__nodeliststart) - 1
        # WARNING the first nodelist is a numpyarray
        while self.__nodeliststart[lastindex].parent is not None:
            node = self.__nodeliststart[lastindex]
            path.append(node.point.tolist())
            lastindex = node.parent
        path.append(self.__start.tolist())
        path = path[::-1]

        return [path, sampledpoints]

    def getNearestListIndex(self, nodelist, randpoint):
        dlist = [np.linalg.norm(randpoint-node.point) for node in nodelist]
        minind = dlist.index(min(dlist))
        return minind