④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,mathclass Node3D:def __init__(self, x, y, z):self.x = xself.y = yself.z = zself.cost = 0.0  # 从起点到该节点的路径代价self.parent = Nonedef 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 Nonedirection = 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_sizenew_node.parent = from_nodereturn new_nodedef is_collision_3d(node, obstacles):global collision_cnt,no_collision_cntbasePosition=(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+=1p.removeBody(sphere_id)return Trueelse:print('q_new与障碍物未发生干涉')no_collision_cnt+=1p.removeBody(sphere_id)return Falsedef 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+=1p.removeBody(cylinder_id)return Trueelse:print('机器人与障碍物未发生干涉')line_no_collision_cnt+=1p.removeBody(cylinder_id)return Falsedef 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.costbest_parent = q_nearfor 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 = neighborq_new.parent = best_parentq_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_newneighbor.cost = q_new.cost + distance_3d(q_new, neighbor)# 检查是否接近目标if distance_3d(q_new, Node3D(*goal)) < goal_threshold:print("Path found!")path = []current = q_newwhile current is not None:path.append((current.x, current.y, current.z))current = current.parentreturn path[::-1], treeprint("Path not found.")return None, treedef get_path_len(path,point_end):path.append(point_end)pathLen = 0for 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 pathLendef 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_Matrixdef 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) / 2rotation =compute_rotation(Node3D(*point_start), Node3D(*point_end))# compute_rotation(start, goal)  # 计算旋转矩阵（对齐主轴到起点-终点连线）print('rotation:',rotation)# 椭球半轴长度（三维椭圆）a = c_best / 2b = np.sqrt(c_best**2 - c_min**2) / 2c = 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_randprint('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.costbest_parent = q_nearfor 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 = neighborq_new.parent = best_parentq_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_newneighbor.cost = q_new.cost + distance_3d(q_new, neighbor)# 检查是否接近目标if distance_3d(q_new, Node3D(*goal)) < goal_threshold:print("Path found!")path = []current = q_newwhile current is not None:path.append((current.x, current.y, current.z))current = current.parentreturn path[::-1], treeprint("Path not found.")return None, treedef 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_idglobal collision_cnt,no_collision_cnt
no_collision_cnt=0
collision_cnt=0global line_collision_cnt,line_no_collision_cnt
line_no_collision_cnt=0
line_collision_cnt=0global 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=pathLenpath_Length.append(c_best)c_best=min(path_Length)print("c_best=",c_best)path_length_dict[pathLen] = path_Informedprint("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*算法，在机器人无人机中，怎样提高算法鲁棒|步长|鲁棒性|自适应|视频生成模型_网易订阅