文章目录

参考资料

前文回顾

1. 基本概念

1.1 基于随机采样的路径规划算法

  • 基于随机采样的路径规划算法适用于高维度空间，它们以概率完备性(当时间接近无限时一定有解)来代替完备性，从而提高搜索效率。

  • 基于随机采样的路径规划算法又分为单查询算法(single-query path planning)以及渐近最优算法(asymptotically optimal path planning)，前者只要找到可行路径即可，侧重快速性，后者还会对找到的路径进行逐步优化，慢慢达到最优，侧重最优性。单查询方法包括概率路图算法(Probabilistic Road Map, PRM)、快速随机扩展树算法(Rapidly-exploring Random Tree, RRT)、RRT-Connect算法等，渐近最优算法有RRT*算法等。

1.2 RRT*算法概述

RRT*算法是一种渐近最优算法，属于RRT算法的优化。

渐近最优的意思是随着迭代次数的增加，得出的路径是越来越优化的，因此要想得出相对满意的优化路径，需要一定的运算时间。

算法流程与RRT算法流程基本相同，不同之处主要在于两个地方：

  • 首先，重新为 x n e w x_{new} xnew​选择父节点。

    • 不同于RRT中直接选择 x n e a r e s t x_{nearest} xnearest​作为 x n e w x_{new} xnew​的父节点，我们需要重新为 x n e w x_{new} xnew​选择父节点，使得 x n e w x_{new} xnew​到起点的cost能够最小。至于cost的定义，可以是路径的长度。
    • 父节点的选择可以是该节点附近相连的所有点，一般是在新产生的节点 x n e w x_{new} xnew​ 附近以定义的半径范围 r r r内寻找所有的近邻节点 X n e a r X_{near} Xnear​，作为替换 x n e w x_{new} xnew​ 原始父节点 x n e a r e s t x_{nearest} xnearest​ 的备选
    • 我们需要依次计算起点到每个近邻节点 X n e a r X_{near} Xnear​ 的路径代价 加上 近邻节点 X n e a r X_{near} Xnear​ 到 x n e w x_{new} xnew​ 的路径代价，取路径代价最小的近邻节点 x m i n x_{min} xmin​作为 x n e w x_{new} xnew​ 新的父节点

  • 其次，就是在重新选完父节点后，为该节点的所有近邻节点重新布线，即rewire，布线的原则是使所有节点到起点的cost最小

示意过程如下：

1.3 算法伪代码

算法伪代码如下

伪代码中的一些必要解释：

  • SampleFree：在可行的区域内随机采样
  • Nearest(G,x_{rand})：在已生成的树中利用欧氏距离判断距离 x r a n d x_{rand} xrand​ ​最近的点 x n e a r e s t x_{nearest} xnearest​​。
  • Steer(x_{nearest},x_{rand}): 从 x n e a r e s t x_{nearest} xnearest​与 x r a n d x_{rand} xrand​的连线方向上扩展固定步长得到新节点 x n e w x_{new} xnew​（如果 x n e a r e s t x_{nearest} xnearest​与 x r a n d x_{rand} xrand​间的距离小于步长，则直接将 x r a n d x_{rand} xrand​作为新节点 x n e w x_{new} xnew​）
  • Card(V)：cardinality of V——应该理解成集合 V V V的大小
  • Line(x_nearest, x_new)：两个点之间的直线距离（欧氏距离）
  • CollisionFree和ObstacleFree：都是判断两个节点之间是否有障碍物

2. python代码实现

代码主体来自pythonRobotics，由于许多代码跟RRT的代码是重复的，所以直接继承了RRT类。完整的代码见github仓库。

import math
import os
import sys

import matplotlib.pyplot as plt
from celluloid import Camera  # 保存动图时用，pip install celluloid
sys.path.append("../RRT")
try:
    from rrt_planning import RRT
except ImportError:
    raise

show_animation = True


class RRTStar(RRT):
    """
    Class for RRT Star planning
    """

    class Node(RRT.Node):
        def __init__(self, x, y):
            super().__init__(x, y)
            self.cost = 0.0

    def __init__(self,
            start,
            goal,
            obstacle_list,
            rand_area,
            expand_dis=3.0,
            goal_sample_rate=20,
            max_iter=500,
            connect_circle_dist=50.0,
            search_until_max_iter=False,
            robot_radius=0.0):
        """
        Setting Parameter

        start:Start Position [x,y]
        goal:Goal Position [x,y]
        obstacleList:obstacle Positions [[x,y,size],...]
        randArea:Random Sampling Area [min,max]

        """
        super().__init__(start, goal, obstacle_list, rand_area, expand_dis,
                          goal_sample_rate, max_iter,
                         robot_radius=robot_radius)
        self.connect_circle_dist = connect_circle_dist
        self.goal_node = self.Node(goal[0], goal[1])
        self.search_until_max_iter = search_until_max_iter

    def planning(self, animation=True, camera=None):
        """
        rrt star path planning

        animation: flag for animation on or off .
        """

        self.node_list = [self.start]
        for i in range(self.max_iter):
            print("Iter:", i, ", number of nodes:", len(self.node_list))
            rnd = self.sample_free()
            nearest_ind = self.get_nearest_node_index(self.node_list, rnd)
            new_node = self.steer(self.node_list[nearest_ind], rnd,
                                  self.expand_dis)
            near_node = self.node_list[nearest_ind]
            # 计算代价,欧氏距离
            new_node.cost = near_node.cost + math.hypot(new_node.x-near_node.x, new_node.y-near_node.y)

            if self.obstacle_free(new_node, self.obstacle_list, self.robot_radius):
                near_inds = self.find_near_nodes(new_node) # 找到x_new的邻近节点
                node_with_updated_parent = self.choose_parent(new_node, near_inds) # 重新选择父节点
                # 如果父节点更新了
                if node_with_updated_parent:
                    # 重布线
                    self.rewire(node_with_updated_parent, near_inds)
                    self.node_list.append(node_with_updated_parent)
                else:
                    self.node_list.append(new_node)

            if animation and i % 5 ==0:
                self.draw_graph(rnd, camera)

            if ((not self.search_until_max_iter) and new_node):  # if reaches goal
                last_index = self.search_best_goal_node()
                if last_index is not None:
                    return self.generate_final_course(last_index)

        print("reached max iteration")

        last_index = self.search_best_goal_node()
        if last_index is not None:
            return self.generate_final_course(last_index)

        return None

    def choose_parent(self, new_node, near_inds):
        """
        在新产生的节点 $x_{new}$ 附近以定义的半径范围$r$内寻找所有的近邻节点 $X_{near}$，
        作为替换 $x_{new}$ 原始父节点 $x_{near}$ 的备选
	    我们需要依次计算起点到每个近邻节点 $X_{near}$ 的路径代价 加上近邻节点 $X_{near}$ 到 $x_{new}$ 的路径代价，
        取路径代价最小的近邻节点$x_{min}$作为 $x_{new}$ 新的父节点
        Arguments:
        --------
            new_node, Node
                randomly generated node with a path from its neared point
                There are not coalitions between this node and th tree.
            near_inds: list
                Indices of indices of the nodes what are near to new_node
        Returns.
        ------
            Node, a copy of new_node
        """
        if not near_inds:
            return None

        # search nearest cost in near_inds
        costs = []
        for i in near_inds:
            near_node = self.node_list[i]
            t_node = self.steer(near_node, new_node)
            if t_node and self.obstacle_free(t_node, self.obstacle_list, self.robot_radius):
                costs.append(self.calc_new_cost(near_node, new_node))
            else:
                costs.append(float("inf"))  # the cost of collision node
        min_cost = min(costs)

        if min_cost == float("inf"):
            print("There is no good path.(min_cost is inf)")
            return None

        min_ind = near_inds[costs.index(min_cost)]
        new_node = self.steer(self.node_list[min_ind], new_node)
        new_node.cost = min_cost

        return new_node

    def search_best_goal_node(self):
        dist_to_goal_list = [self.calc_dist_to_goal(n.x, n.y) for n in self.node_list]
        goal_inds = [
            dist_to_goal_list.index(i) for i in dist_to_goal_list
            if i <= self.expand_dis
        ]

        safe_goal_inds = []
        for goal_ind in goal_inds:
            t_node = self.steer(self.node_list[goal_ind], self.goal_node)
            if self.obstacle_free(t_node, self.obstacle_list, self.robot_radius):
                safe_goal_inds.append(goal_ind)

        if not safe_goal_inds:
            return None

        min_cost = min([self.node_list[i].cost for i in safe_goal_inds])
        for i in safe_goal_inds:
            if self.node_list[i].cost == min_cost:
                return i

        return None

    def find_near_nodes(self, new_node):
        """
        1) defines a ball centered on new_node
        2) Returns all nodes of the three that are inside this ball
            Arguments:
            ---------
                new_node: Node
                    new randomly generated node, without collisions between
                    its nearest node
            Returns:
            -------
                list
                    List with the indices of the nodes inside the ball of
                    radius r
        """
        nnode = len(self.node_list) + 1
        r = self.connect_circle_dist * math.sqrt((math.log(nnode) / nnode))
        # if expand_dist exists, search vertices in a range no more than
        # expand_dist
        if hasattr(self, 'expand_dis'):
            r = min(r, self.expand_dis)
        dist_list = [(node.x - new_node.x)**2 + (node.y - new_node.y)**2
                     for node in self.node_list]
        near_inds = [dist_list.index(i) for i in dist_list if i <= r**2]
        return near_inds

    def rewire(self, new_node, near_inds):
        """
            For each node in near_inds, this will check if it is cheaper to
            arrive to them from new_node.
            In such a case, this will re-assign the parent of the nodes in
            near_inds to new_node.
            Parameters:
            ----------
                new_node, Node
                    Node randomly added which can be joined to the tree

                near_inds, list of uints
                    A list of indices of the self.new_node which contains
                    nodes within a circle of a given radius.
            Remark: parent is designated in choose_parent.

        """
        for i in near_inds:
            near_node = self.node_list[i]
            edge_node = self.steer(new_node, near_node)
            if not edge_node:
                continue
            edge_node.cost = self.calc_new_cost(new_node, near_node)

            no_collision = self.obstacle_free(
                edge_node, self.obstacle_list, self.robot_radius)
            improved_cost = near_node.cost > edge_node.cost

            if no_collision and improved_cost:
                near_node.x = edge_node.x
                near_node.y = edge_node.y
                near_node.cost = edge_node.cost
                near_node.path_x = edge_node.path_x
                near_node.path_y = edge_node.path_y
                near_node.parent = edge_node.parent
                self.propagate_cost_to_leaves(new_node)

    def calc_new_cost(self, from_node, to_node):
        d, _ = self.calc_distance_and_angle(from_node, to_node)
        return from_node.cost + d

    def propagate_cost_to_leaves(self, parent_node):

        for node in self.node_list:
            if node.parent == parent_node:
                node.cost = self.calc_new_cost(parent_node, node)
                self.propagate_cost_to_leaves(node)


def main():
    print("Start " )
    fig = plt.figure(1)

    camera = Camera(fig) # 保存动图时使用
    # camera = None # 不保存动图时，camara为None
    show_animation = True
    # ====Search Path with RRT====
    obstacle_list = [
        (5, 5, 1),
        (3, 6, 2),
        (3, 8, 2),
        (3, 10, 2),
        (7, 5, 2),
        (9, 5, 2),
        (8, 10, 1),
        (6, 12, 1),
    ]  # [x,y,size(radius)]

    # Set Initial parameters
    rrt_star = RRTStar(
        start=[0, 0],
        goal=[6, 10],
        rand_area=[-2, 15],
        obstacle_list=obstacle_list,
        expand_dis=3,
        robot_radius=0.8)
    path = rrt_star.planning(animation=show_animation,camera=camera)

    if path is None:
        print("Cannot find path")
    else:
        print("found path!!")

        # Draw final path
        if show_animation:
            rrt_star.draw_graph(camera=camera)
            plt.plot([x for (x, y) in path], [y for (x, y) in path], 'r--')
            plt.grid(True)
            if camera!=None:
                camera.snap()
                animation = camera.animate()
                animation.save('trajectory.gif')
    plt.show()


if __name__ == '__main__':

    main()

实现效果如下：
在这里插入图片描述

3. c++实现

由于在自动驾驶中算法实现一般使用C++，所以我也使用C++实现了相关功能，代码结构与python代码实现类似，这边就不再做相关代码解释了。完整代码详见另一个github仓库。

# python # 自动驾驶 # 算法
Logo
开放原子开发者工作坊

开放原子开发者工作坊旨在鼓励更多人参与开源活动，与志同道合的开发者们相互交流开发经验、分享开发心得、获取前沿技术趋势。工作坊有多种形式的开发者活动，如meetup、训练营等，主打技术交流，干货满满，真诚地邀请各位开发者共同参与！

更多推荐

一维高斯分布与多维高斯分布及 matlab 实现

一维高斯分布与多维高斯分布一维高斯分布高斯分布（Gaussian distribution），又称正态分布（Normal distribution）。若随机变量X服从一个数学期望为μ、方差为σ^2的正态分布，记为N(μ，σ^2)。其概率密度函数为正态分布的期望值μ决定了其位置，其标准差σ决定了分布的幅度。当μ = 0,σ = 1时的正态分布是标准正态分布。正态分布使用下列参数：参数说明范围mu (

avatar 开放原子开发者工作坊

图像传感器与信号处理——自动曝光算法

图像传感器与信号处理——自动曝光算法图像传感器与信号处理——自动曝光算法1. 如何实现自动曝光？2. 基于直方图统计的算法2.1 Automatic Camera Exposure Control，20073. 基于熵的算法4. 基于梯度的算法图像传感器与信号处理——自动曝光算法在很久之前我和一位视觉SLAM大佬交流过，视觉SLAM难以落地原因有很多，其中之一就是环境光照对SLAM系统有较大的..

avatar 开放原子开发者工作坊
cover

论文解读：BIT | Remote Sensing Image Change Detection with Transformers

avatar 开放原子开发者工作坊