融合A*改进RRT算法的路径规划代码仿真 全局路径规划 - RRT算法原理 RRT算法，即快速随机树算法（Rapid Random Tree），是LaValle在1998年首次提出的一种高效的路径规划算法。 RRT算法以初始的一个根节点，通过随机采样的方法在空间搜索，然后添加一个又一个的叶节点来不断扩展随机树。 当目标点进入随机树里面后，随机树扩展立即停止，此时能找到一条从起始点到目标点的路径。 算法的计算过程如下： step1：初始化随机树。 将环境中起点作为随机树搜索的起点，此时树中只包含一个节点即根节点； stpe2：在环境中随机采样。 在环境中随机产生一个点，若该点不在障碍物范围内则计算随机树中所有节点到的欧式距离，并找到距离最近的节点，若在障碍物范围内则重新生成并重复该过程直至找到; stpe3：生成新节点。 在和连线方向，由指向固定生长距离生成一个新的节点，并判断该节点是否在障碍物范围内，若不在障碍物范围内则将添加到随机树 中，否则的话返回step2重新对环境进行随机采样； step4：停止搜索。 当和目标点之间的距离小于设定的阈值时，则代表随机树已经到达了目标点，将作为最后一个路径节点加入到随机树中，算法结束并得到所规划的路径 。

在路径规划领域，RRT（快速随机树算法，Rapid Random Tree）可谓是一颗耀眼的“明星”算法。它由LaValle在1998年首次提出，是一种极为高效的路径规划算法，主要应用于全局路径规划。

RRT算法原理剖析

RRT算法就像是在一片未知的空间中，从一个起始点开始，像种树一样，不断长出新的“树枝”（叶节点），直到触碰到目标点。

初始化随机树

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


def rrt_init(start_x, start_y):
    start_node = Node(start_x, start_y)
    tree = [start_node]
    return tree

上述代码定义了一个Node类来表示树中的节点，每个节点包含其坐标x、y以及父节点信息。rrt_init函数则以环境中的起点作为随机树搜索的起点，此时树中仅包含这个根节点。

在环境中随机采样

import random


def sample_env(obstacles):
    while True:
        sample_x = random.uniform(0, 100)  # 假设环境范围是0 - 100
        sample_y = random.uniform(0, 100)
        sample_point = Node(sample_x, sample_y)
        if not is_in_obstacle(sample_point, obstacles):
            return sample_point


def is_in_obstacle(point, obstacles):
    for obstacle in obstacles:
        if (point.x >= obstacle[0] and point.x <= obstacle[2]) and (point.y >= obstacle[1] and point.y <= obstacle[3]):
            return True
    return False

sampleenv函数在环境中随机生成一个点，这里假设环境范围是0 - 100。它通过isin_obstacle函数判断该点是否在障碍物范围内，如果不在则返回该点，否则重新生成，直到找到合适的点。

生成新节点

import math


def nearest_node(sample_point, tree):
    min_dist = float('inf')
    nearest = None
    for node in tree:
        dist = math.sqrt((node.x - sample_point.x) ** 2 + (node.y - sample_point.y) ** 2)
        if dist < min_dist:
            min_dist = dist
            nearest = node
    return nearest


def new_node(nearest, sample_point, step_size):
    theta = math.atan2(sample_point.y - nearest.y, sample_point.x - nearest.x)
    new_x = nearest.x + step_size * math.cos(theta)
    new_y = nearest.y + step_size * math.sin(theta)
    new_node = Node(new_x, new_y)
    new_node.parent = nearest
    return new_node


def add_to_tree(tree, new_node):
    tree.append(new_node)
    return tree

nearestnode函数计算随机树中所有节点到采样点的欧式距离，并找到距离最近的节点。newnode函数在最近节点和采样点的连线方向，由最近节点指向采样点，按照固定生长距离stepsize生成一个新的节点，并设置其父节点为最近节点。addto_tree函数将新节点添加到随机树中。

停止搜索

def stop_search(new_node, goal_node, goal_threshold):
    dist = math.sqrt((new_node.x - goal_node.x) ** 2 + (new_node.y - goal_node.y) ** 2)
    if dist < goal_threshold:
        new_node.parent = goal_node
        return True
    return False

stopsearch函数判断新生成的节点和目标点之间的距离是否小于设定的阈值goalthreshold，如果小于则代表随机树已经到达了目标点，将目标点作为最后一个路径节点加入到随机树中，算法结束并得到所规划的路径。

融合A*改进RRT算法的路径规划代码仿真 全局路径规划 - RRT算法原理 RRT算法，即快速随机树算法（Rapid Random Tree），是LaValle在1998年首次提出的一种高效的路径规划算法。 RRT算法以初始的一个根节点，通过随机采样的方法在空间搜索，然后添加一个又一个的叶节点来不断扩展随机树。 当目标点进入随机树里面后，随机树扩展立即停止，此时能找到一条从起始点到目标点的路径。 算法的计算过程如下： step1：初始化随机树。 将环境中起点作为随机树搜索的起点，此时树中只包含一个节点即根节点； stpe2：在环境中随机采样。 在环境中随机产生一个点，若该点不在障碍物范围内则计算随机树中所有节点到的欧式距离，并找到距离最近的节点，若在障碍物范围内则重新生成并重复该过程直至找到; stpe3：生成新节点。 在和连线方向，由指向固定生长距离生成一个新的节点，并判断该节点是否在障碍物范围内，若不在障碍物范围内则将添加到随机树 中，否则的话返回step2重新对环境进行随机采样； step4：停止搜索。 当和目标点之间的距离小于设定的阈值时，则代表随机树已经到达了目标点，将作为最后一个路径节点加入到随机树中，算法结束并得到所规划的路径 。

然而，RRT算法也并非十全十美，比如其搜索过程具有一定的盲目性，可能导致路径不够优化。这时候，融合A算法来改进RRT算法就显得尤为重要，通过A算法的启发式搜索特性，让RRT算法在扩展树的时候更有“方向感”，从而更快地找到更优路径，后续我们可以进一步深入探讨这种融合改进的实现细节和效果优化。