④Pybullet之Informed RRT*算法介绍及示例

1、Informed RRT*算法介绍

Informed RRT* 是 RRT*（Rapidly-exploring Random Tree Star）算法的改进版本，属于 RRT 算法家族。RRT* 算法通过引入局部优化（枝剪机制）和全局最优路径搜索，解决了传统 RRT 算法路径质量不高的问题。然而，RRT* 在复杂环境中搜索效率较低，因此 Informed RRT*通过启发式信息（如椭圆区域采样）进一步优化了搜索效率。

Informed RRT*的核心思想是利用已知的启发式信息（如起始点和目标点之间的最短路径长度）来限制采样范围，从而减少无效采样点，提高搜索效率。具体而言，它通过以下方式实现：

• 椭圆区域采样：以起始点和目标点为焦点，构建一个椭圆区域，采样点仅在该椭圆内生成。椭圆的长轴为当前已知的最优路径长度（cbest），短轴则根据起始点和目标点的距离（cmin）确定。
• 路径优化：通过不断更新路径长度，椭圆区域逐渐缩小，最终找到接近全局最优的路径。

2、Informed RRT*算法示例

如下图所示，由于取样点基本限制在起点和终点之间连线建立的一个椭球区域内，并且椭球区域会越来越小，因此最终生成的路径会基本确保为一个相对较优（较短）的路径，而不会出现较大波动。

参考程序代码如下所示：

import numpy as np
import matplotlib.pyplot as plt
from mpl_toolkits.mplot3d import Axes3D
import pybullet as p
import time,math

class Node3D:
    def __init__(self, x, y, z):
        self.x = x
        self.y = y
        self.z = z
        self.cost = 0.0  # 从起点到该节点的路径代价
        self.parent = None

def distance_3d(node1, node2):
    return np.sqrt((node1.x - node2.x)**2 + (node1.y - node2.y)**2 + (node1.z - node2.z)**2)

def steer_3d(from_node, to_node, step_size):
    # 计算从 from_node 到 to_node 的方向向量
    direction = np.array([to_node.x - from_node.x, to_node.y - from_node.y, to_node.z - from_node.z])
    norm = np.linalg.norm(direction)
    if norm == 0:
        return None
    direction = direction / norm  # 单位化
    
    new_node = Node3D(
        from_node.x + direction[0] * step_size,
        from_node.y + direction[1] * step_size,
        from_node.z + direction[2] * step_size
    )
    new_node.cost = from_node.cost + step_size
    new_node.parent = from_node
    return new_node

def is_collision_3d(node, obstacles):
    global collision_cnt,no_collision_cnt

    basePosition=(node.x,node.y,node.z)
    sphere_id=create_SPHERE(basePosition)
    p.stepSimulation() 
    # 干涉检查
    contacts = p.getContactPoints(sphere_id, obstacle_id)
    if contacts:
        print('q_new与障碍物发生干涉')
        collision_cnt+=1
        p.removeBody(sphere_id)
        return True
    else:
        print('q_new与障碍物未发生干涉')
        no_collision_cnt+=1
        p.removeBody(sphere_id)
        return False

def is_collision_line_3d(node1, node2, obstacles, radius=0.5):
    global line_collision_cnt,line_no_collision_cnt

    """
    在两点之间生成圆柱体
    :param point1: 起点坐标 (x1, y1, z1)
    :param point2: 终点坐标 (x2, y2, z2)
    :param radius: 圆柱体半径
    :return: 圆柱体的 PyBullet 对象 ID
    """
    # 计算中点位置
    
    point1=(node1.x,node1.y,node1.z)
    point2=(node2.x,node2.y,node2.z)
    
    midpoint = np.array([
        (point1[0] + point2[0]) / 2,
        (point1[1] + point2[1]) / 2,
        (point1[2] + point2[2]) / 2
    ])

    #print("midpoint=",midpoint)
    # 计算两点之间的向量和距离（高度）
    direction = np.array(point2) - np.array(point1)
    height = np.linalg.norm(direction)
    #print("height=",height)
    if height < 1e-6:
        raise ValueError("两点重合，无法生成圆柱体")

    # 归一化方向向量
    direction_normalized = direction / height

    # 计算旋转四元数（将Z轴对齐到方向向量）
    z_axis = np.array([0, 0, 1])
    rotation_axis = np.cross(z_axis, direction_normalized)
    rotation_axis_norm = np.linalg.norm(rotation_axis)
    
    if rotation_axis_norm < 1e-6:
        # 方向向量与Z轴平行，无需旋转
        quaternion = [0, 0, 0, 1]
    else:
        rotation_angle = np.arccos(np.dot(z_axis, direction_normalized))
        quaternion = p.getQuaternionFromAxisAngle(rotation_axis, rotation_angle)

    collision_shape_id = p.createCollisionShape(
        p.GEOM_CYLINDER,
        radius=radius,
        height=height
    )
    
    visual_shape_id = p.createVisualShape(
        p.GEOM_CYLINDER,
        radius=radius,
        length=height,
        rgbaColor=[0.8, 0.2, 0.2, 1]  # 红色圆柱体
    )

    # 创建刚体对象
    cylinder_id = p.createMultiBody(
        baseMass=1,
        baseCollisionShapeIndex=collision_shape_id,
        baseVisualShapeIndex=visual_shape_id,
        basePosition=midpoint,
        baseOrientation=quaternion
    )
    #print("cylinder_id=",cylinder_id) 
    p.stepSimulation() 
    # 干涉检查
    contacts = p.getContactPoints(cylinder_id, obstacle_id)
    if contacts:
        print('机器人与障碍物发生干涉')
        line_collision_cnt+=1
        p.removeBody(cylinder_id)
        return True
    else:
        print('机器人与障碍物未发生干涉')
        line_no_collision_cnt+=1
        p.removeBody(cylinder_id)
        return False

def rrt_star_3d(start, goal, obstacles, max_iter=2000, step_size=20, goal_threshold=20, search_radius=40):
    tree = [Node3D(*start)]
    
    for _ in range(max_iter):
        # 随机采样（10%概率偏向目标点）
        if np.random.rand() < 0.1:
            q_rand = Node3D(*goal)
        else:
            q_rand = Node3D(
                np.random.uniform(0, 100),
                np.random.uniform(-100, 100),
                np.random.uniform(0, 100)
            )
        
        # 寻找最近邻节点
        q_near = min(tree, key=lambda node: distance_3d(node, q_rand))
        
        # 扩展新节点
        q_new = steer_3d(q_near, q_rand, step_size)
        if q_new is None:
            continue
        
        # 碰撞检测
        if is_collision_3d(q_new, obstacles):
            continue
        
        # 搜索邻近节点（三维球形邻域）
        neighbors = [node for node in tree if distance_3d(node, q_new) <= search_radius]
        
        # 选择最优父节点
        min_cost = q_new.cost
        best_parent = q_near
        for neighbor in neighbors:
            if neighbor.cost + distance_3d(neighbor, q_new) < min_cost:
                if not is_collision_line_3d(neighbor, q_new, obstacles):
                    min_cost = neighbor.cost + distance_3d(neighbor, q_new)
                    best_parent = neighbor
        q_new.parent = best_parent
        q_new.cost = min_cost
        
        # 添加新节点到树
        tree.append(q_new)
        
        # 重新连接（优化邻近节点）
        for neighbor in neighbors:
            if neighbor != best_parent and q_new.cost + distance_3d(q_new, neighbor) < neighbor.cost:
                if not is_collision_line_3d(q_new, neighbor, obstacles):
                    neighbor.parent = q_new
                    neighbor.cost = q_new.cost + distance_3d(q_new, neighbor)
        
        # 检查是否接近目标
        if distance_3d(q_new, Node3D(*goal)) < goal_threshold:
            print("Path found!")
            path = []
            current = q_new
            while current is not None:
                path.append((current.x, current.y, current.z))
                current = current.parent
            return path[::-1], tree
    
    print("Path not found.")
    return None, tree

def get_path_len(path,point_end):
    path.append(point_end)
    pathLen = 0
    for i in range(1, len(path)):
        node1_x = path[i][0]
        node1_y = path[i][1]
        node1_z = path[i][2]
        
        node2_x = path[i - 1][0]
        node2_y = path[i - 1][1]
        node2_z = path[i - 1][2]
        pathLen += math.sqrt((node1_x - node2_x)
                             ** 2 + (node1_y - node2_y) ** 2+(node1_z - node2_z) ** 2)

    return pathLen


def compute_rotation(node1, node2):
    # 计算中点位置
    
    point1=(node1.x,node1.y,node1.z)
    point2=(node2.x,node2.y,node2.z)
    
    midpoint = np.array([
        (point1[0] + point2[0]) / 2,
        (point1[1] + point2[1]) / 2,
        (point1[2] + point2[2]) / 2
    ])

    #print("midpoint=",midpoint)
    # 计算两点之间的向量和距离（高度）
    direction = np.array(point2) - np.array(point1)
    height = np.linalg.norm(direction)
    #print("height=",height)
    if height < 1e-6:
        raise ValueError("两点重合，无法生成圆柱体")

    # 归一化方向向量
    direction_normalized = direction / height
    #=========================================================
    # 计算旋转四元数
    x_axis = np.array([1, 0, 0])  
    rotation_axis = np.cross(x_axis, direction_normalized)
    print("rotation_axis=",rotation_axis)
    rotation_axis_norm = np.linalg.norm(rotation_axis)
    
    if rotation_axis_norm < 1e-6:
        # 方向向量与X轴平行，无需旋转
        quaternion = [0.7071067690849304, 0.7071067690849304, 0, 0]
    else:
        rotation_angle = np.arccos(np.dot(x_axis, direction_normalized))
        quaternion = p.getQuaternionFromAxisAngle(rotation_axis, rotation_angle)

    print("quaternion=",quaternion)
    direction_Matrix=np.array(p.getMatrixFromQuaternion(quaternion)).reshape(3, 3)
    
    return direction_Matrix

def sample_in_ellipsoid(start, goal, c_best):
    global a,b,c
    # 椭圆参数计算
    c_min = distance_3d(start, goal)  # 起点到终点的直线距离
    x_center = (start.x + goal.x) / 2, (start.y + goal.y) / 2, (start.z + goal.z) / 2
    rotation =compute_rotation(Node3D(*point_start), Node3D(*point_end))# compute_rotation(start, goal)  # 计算旋转矩阵（对齐主轴到起点-终点连线）
    print('rotation:',rotation)
    # 椭球半轴长度（三维椭圆）
    a = c_best / 2
    b = np.sqrt(c_best**2 - c_min**2) / 2
    c = b  # 假设第三轴与第二轴相同（可根据场景调整）
    print('ellipsoid parameter:',"a=",a,"b=",b,"c=",c)
    
    # 在单位球内采样，再变换到椭圆坐标系
    while True:
        u = np.random.uniform(-1, 1, 3)
        if np.linalg.norm(u) <= 1:
            break
    # 缩放和旋转
    print('u:',u)
    q_rand = rotation @ np.diag([a, b, c]) @ u + x_center
    #return q_rand
    print('rotation @ np.diag([a, b, c]) @ u:',rotation @ np.diag([a, b, c]) @ u)
    print('x_center:',x_center)
    print('q_rand:',q_rand)
    
    return Node3D(q_rand[0], q_rand[1], q_rand[2])


def Informed_rrt_star_3d(start, goal, obstacles, c_best,max_iter=2000, step_size=20, goal_threshold=20, search_radius=40):
    tree = [Node3D(*start)]
    
    for _ in range(max_iter):
        # 随机采样（10%概率偏向目标点）
        if np.random.rand() < 0.1:
            q_rand = Node3D(*goal)
        else:
            q_rand=sample_in_ellipsoid(Node3D(*start), Node3D(*goal), c_best)
        
        q_near = min(tree, key=lambda node: distance_3d(node, q_rand))
        
        # 扩展新节点
        q_new = steer_3d(q_near, q_rand, step_size)
        if q_new is None:
            continue
        
        # 碰撞检测
        if is_collision_3d(q_new, obstacles):
            continue
        
        # 搜索邻近节点（三维球形邻域）
        neighbors = [node for node in tree if distance_3d(node, q_new) <= search_radius]
        
        # 选择最优父节点
        min_cost = q_new.cost
        best_parent = q_near
        for neighbor in neighbors:
            if neighbor.cost + distance_3d(neighbor, q_new) < min_cost:
                if not is_collision_line_3d(neighbor, q_new, obstacles):
                    min_cost = neighbor.cost + distance_3d(neighbor, q_new)
                    best_parent = neighbor
        q_new.parent = best_parent
        q_new.cost = min_cost
        
        # 添加新节点到树
        tree.append(q_new)
        
        # 重新连接（优化邻近节点）
        for neighbor in neighbors:
            if neighbor != best_parent and q_new.cost + distance_3d(q_new, neighbor) < neighbor.cost:
                if not is_collision_line_3d(q_new, neighbor, obstacles):
                    neighbor.parent = q_new
                    neighbor.cost = q_new.cost + distance_3d(q_new, neighbor)
        
        # 检查是否接近目标
        if distance_3d(q_new, Node3D(*goal)) < goal_threshold:
            print("Path found!")
            path = []
            current = q_new
            while current is not None:
                path.append((current.x, current.y, current.z))
                current = current.parent
            return path[::-1], tree
    
    print("Path not found.")
    return None, tree



def plot_rrt_star_3d(path, tree, start, goal, obstacles):
    fig = plt.figure(figsize=(10, 8))
    ax = fig.add_subplot(111, projection='3d')
    
    # 绘制树
    for node in tree:
        if node.parent:
            ax.plot([node.x, node.parent.x], [node.y, node.parent.y], [node.z, node.parent.z], 
                    'g-', lw=0.5, alpha=0.3)
    
    # 绘制路径
    if path:
        ax.plot([p[0] for p in path], [p[1] for p in path], [p[2] for p in path], 'r-', lw=2)
    
    # 绘制起点和终点
    ax.scatter(start[0], start[1], start[2], c='blue', s=100, marker='o', label='Start')
    ax.scatter(goal[0], goal[1], goal[2], c='red', s=100, marker='o', label='Goal')

    # 长方体的顶点坐标
    vertices = [
        [25, -5, 0],
        [75, -5, 0],
        [25, -5, 60],
        [75, -5, 60],
        [25, 5, 0],
        [75, 5, 0],
        [25, 5, 60],
        [75, 5, 60]
    ]

    # 连接顶点以形成长方体的边缘
    edges = [
        [vertices[0], vertices[1]],
        [vertices[0], vertices[2]],
        [vertices[0], vertices[4]],
        [vertices[1], vertices[3]],
        [vertices[1], vertices[5]],
        [vertices[2], vertices[3]],
        [vertices[2], vertices[6]],
        [vertices[3], vertices[7]],
        [vertices[4], vertices[5]],
        [vertices[4], vertices[6]],
        [vertices[5], vertices[7]],
        [vertices[6], vertices[7]]
    ]

    # 绘制每条边
    for edge in edges:
        ax.plot(*zip(*edge), color='b') 
    ax.set_xlim(0, 100)
    ax.set_ylim(-100, 100)
    ax.set_zlim(0, 100)
    ax.set_xlabel('X')
    ax.set_ylabel('Y')
    ax.set_zlabel('Z')
    plt.legend()
    plt.show()

def create_SPHERE(basePosition):
    # 创建球体碰撞形状
    collision_shape_id = p.createCollisionShape(p.GEOM_SPHERE, radius=5)

    # 创建球体视觉形状
    visual_shape_id = p.createVisualShape(p.GEOM_SPHERE, radius=1, rgbaColor=[0.8, 0.2, 0.2, 1])

    # 创建球体刚体对象，初始位置为 (1, 2, 3)
    sphere_id = p.createMultiBody(baseMass=1, baseCollisionShapeIndex=collision_shape_id, baseVisualShapeIndex=visual_shape_id, basePosition=basePosition)

    return sphere_id


global collision_cnt,no_collision_cnt
no_collision_cnt=0
collision_cnt=0

global line_collision_cnt,line_no_collision_cnt
line_no_collision_cnt=0
line_collision_cnt=0

global a,b,c

# 参数设置（起点、终点、障碍物）
point_start = (50, 40, 50)#(0.5, 0.4, 0.5)
point_end = (50, -40, 50)# (0.5, -0.4, 0.5)
obstacles = [
    (40, 40, 40, 15),
    (60, 30, 50, 10),
    (30, 70, 60, 12)
]

# 连接物理引擎
p.connect(p.GUI)  # 使用 p.DIRECT 可关闭图形界面

obstacle_shape = p.createCollisionShape(p.GEOM_BOX, halfExtents=[25, 5, 30])
obstacle_pose = [50, 0, 30]  # 设置障碍物位置

obstacle_id = p.createMultiBody(baseMass=0, baseCollisionShapeIndex=obstacle_shape, basePosition=obstacle_pose)

# 运行三维RRT*
path, tree = rrt_star_3d(point_start, point_end, obstacles, max_iter=3000, step_size=20, search_radius=40)
print("path=",path)
print("collision_cnt=",collision_cnt,"no_collision_cnt=",no_collision_cnt)
print("line_collision_cnt=",line_collision_cnt,"line_no_collision_cnt=",line_no_collision_cnt)

# 可视化
if path:
    plot_rrt_star_3d(path, tree, point_start,point_end, obstacles)
else:
    print("No path found.")

# 运行三维informed RRT*
pathLen=get_path_len(path,point_end)
print("pathLen=",pathLen)
direction_Matrix=compute_rotation(Node3D(*point_start), Node3D(*point_end))
print("direction_Matrix=",direction_Matrix)

c_best=pathLen
path_Length=[]
path_Length.append(c_best)

# 记录所有最短路径及其长度
path_length_dict = {}

for i in range(3):
    
    path_Informed, tree_Informed = Informed_rrt_star_3d(point_start, point_end, obstacles, c_best,max_iter=2000, step_size=20, goal_threshold=20, search_radius=40)
    # 可视化
    if path:
        plot_rrt_star_3d(path_Informed, tree_Informed, point_start,point_end, obstacles)
    else:
        print("No path found.")
    
    pathLen=get_path_len(path_Informed,point_end)
    print("pathLen=",pathLen)
    c_best=pathLen
    path_Length.append(c_best)
    c_best=min(path_Length)
    print("c_best=",c_best)
    path_length_dict[pathLen] = path_Informed
    
print("path_Length=",path_Length)
# 查找最小路径长度
min_length = min(path_length_dict.keys())

# 提取具有最小长度的路径
min_paths = path_length_dict[min_length]#[list(path) for path, length in path_length_dict.items() if length == min_length]

print("Minimum Path(s):", min_paths, "with length", min_length)


# 可视化
if path:
    plot_rrt_star_3d(min_paths, tree_Informed, point_start,point_end, obstacles)
else:
    print("No path found.")

Informed RRT*算法整体感觉对于路径的优化还是有着非常不错的效果。

大家感兴趣的话，还可以进一步参考下文，针对Informed RRT*算法有着详细的说明并提出了进一步优化的AS InformedRRT*算法。

改进AS Informed RRT*算法，在机器人无人机中，怎样提高算法鲁棒|步长|鲁棒性|自适应|视频生成模型_网易订阅