我将为你实现一个基于Matplotlib可视化的Informed RRT*算法，并包含圆形障碍物环境。这个算法在找到初始路径后会持续优化，使用椭圆采样区域来加速收敛。
python
import numpy as np
import math
import random
import matplotlib.pyplot as plt
from matplotlib.patches import Circle, Ellipse
import matplotlib.animation as animation
class Node:
"""RRT树节点类"""
def __init__(self, x, y):
self.x = x
self.y = y
self.parent = None
self.cost = 0.0
class InformedRRTStar:
"""Informed RRT*算法实现"""
def __init__(self, start, goal, bounds, obstacles,
max_iter=2000, step_size=20, neighbor_radius=30):
"""
初始化算法
参数:
start: 起点坐标 (x, y)
goal: 终点坐标 (x, y)
bounds: 地图边界 [x_min, x_max, y_min, y_max]
obstacles: 障碍物列表，每个障碍物为 (x, y, radius)
max_iter: 最大迭代次数
step_size: 步长
neighbor_radius: 邻居搜索半径
"""
self.start = Node(start[0], start[1])
self.goal = Node(goal[0], goal[1])
self.bounds = bounds
self.obstacles = obstacles
self.max_iter = max_iter
self.step_size = step_size
self.neighbor_radius = neighbor_radius
# RRT树
self.tree = [self.start]
# 路径和成本
self.best_path = []
self.best_cost = float('inf')
self.found_initial = False
# 可视化数据
self.visualization_data = {
'nodes': [],
'edges': [],
'best_paths': []
}
def is_collision_free(self, node1, node2):
"""检查两点之间的线段是否与障碍物碰撞"""
for (ox, oy, r) in self.obstacles:
# 计算点到线段的最短距离
distance = self.point_to_line_distance(ox, oy,
node1.x, node1.y,
node2.x, node2.y)
if distance <= r:
return False
return True
def point_to_line_distance(self, px, py, x1, y1, x2, y2):
"""计算点(px,py)到线段(x1,y1)-(x2,y2)的距离"""
# 线段长度
line_len = math.hypot(x2 - x1, y2 - y1)
if line_len == 0:
return math.hypot(px - x1, py - y1)
# 投影比例
t = ((px - x1) * (x2 - x1) + (py - y1) * (y2 - y1)) / (line_len ** 2)
t = max(0, min(1, t))
# 最近点坐标
closest_x = x1 + t * (x2 - x1)
closest_y = y1 + t * (y2 - y1)
return math.hypot(px - closest_x, py - closest_y)
def random_point(self):
"""随机采样点"""
if not self.found_initial:
# 初始阶段在整个空间均匀采样
return (random.uniform(self.bounds[0], self.bounds[1]),
random.uniform(self.bounds[2], self.bounds[3]))
else:
# Informed阶段：在椭圆内采样
return self.sample_in_ellipse()
def sample_in_ellipse(self):
"""在椭圆采样区域内生成随机点"""
# 椭圆参数
c_min = self.best_cost
if c_min == float('inf'):
return self.random_point()
# 起点和终点中心
x_center = [(self.start.x + self.goal.x) / 2,
(self.start.y + self.goal.y) / 2]
# 旋转角度
angle = math.atan2(self.goal.y - self.start.y,
self.goal.x - self.start.x)
# 椭圆半长轴和半短轴
a = c_min / 2
c = math.sqrt((self.goal.x - self.start.x) ** 2 +
(self.goal.y - self.start.y) ** 2) / 2
b = math.sqrt(a ** 2 - c ** 2)
# 在单位圆内采样
while True:
r = math.sqrt(random.random())
theta = random.uniform(0, 2 * math.pi)
# 单位圆内的点
x = r * math.cos(theta)
y = r * math.sin(theta)
# 变换到椭圆
x_rot = x * math.cos(angle) - y * math.sin(angle)
y_rot = x * math.sin(angle) + y * math.cos(angle)
# 缩放和平移
x_sample = x_center[0] + a * x_rot
y_sample = x_center[1] + b * y_rot
# 检查是否在边界内
if (self.bounds[0] <= x_sample <= self.bounds[1] and
self.bounds[2] <= y_sample <= self.bounds[3]):
return (x_sample, y_sample)
def nearest_node(self, point):
"""找到最近的节点"""
min_dist = float('inf')
nearest = None
for node in self.tree:
dist = math.hypot(node.x - point[0], node.y - point[1])
if dist < min_dist:
min_dist = dist
nearest = node
return nearest
def steer(self, from_node, to_point):
"""从from_node向to_point方向生长一步"""
# 计算方向
dx = to_point[0] - from_node.x
dy = to_point[1] - from_node.y
dist = math.hypot(dx, dy)
if dist < self.step_size:
return Node(to_point[0], to_point[1])
# 按步长生长
ratio = self.step_size / dist
new_x = from_node.x + dx * ratio
new_y = from_node.y + dy * ratio
return Node(new_x, new_y)
def find_near_nodes(self, new_node):
"""找到新节点附近的邻居节点"""
near_nodes = []
for node in self.tree:
dist = math.hypot(node.x - new_node.x, node.y - new_node.y)
if dist <= self.neighbor_radius:
near_nodes.append(node)
return near_nodes
def choose_parent(self, new_node, near_nodes):
"""为new_node选择最优父节点"""
min_cost = float('inf')
best_parent = None
for node in near_nodes:
# 检查是否无碰撞
if self.is_collision_free(node, new_node):
# 计算成本
cost = node.cost + math.hypot(node.x - new_node.x,
node.y - new_node.y)
if cost < min_cost:
min_cost = cost
best_parent = node
return best_parent, min_cost
def rewire(self, new_node, near_nodes):
"""重新连接附近的节点"""
for node in near_nodes:
# 检查通过new_node是否能为node提供更短路径
if node == new_node.parent:
continue
new_cost = new_node.cost + math.hypot(new_node.x - node.x,
new_node.y - node.y)
if new_cost < node.cost:
# 检查新路径是否无碰撞
if self.is_collision_free(new_node, node):
# 重新连接
node.parent = new_node
node.cost = new_cost
def check_goal_connection(self, node):
"""检查是否可以直接连接到目标点"""
if math.hypot(node.x - self.goal.x, node.y - self.goal.y) <= self.step_size:
if self.is_collision_free(node, self.goal):
# 设置目标点的父节点和成本
self.goal.parent = node
self.goal.cost = node.cost + math.hypot(node.x - self.goal.x,
node.y - self.goal.y)
# 更新最佳路径
self.update_best_path()
return True
return False
def update_best_path(self):
"""更新最佳路径"""
if self.goal.parent is None:
return
# 回溯得到路径
path = []
node = self.goal
while node is not None:
path.append((node.x, node.y))
node = node.parent
path.reverse()
path_cost = self.goal.cost
if path_cost < self.best_cost:
self.best_cost = path_cost
self.best_path = path
self.found_initial = True
def plan(self):
"""执行路径规划主循环"""
for i in range(self.max_iter):
# 1. 随机采样
rand_point = self.random_point()
# 2. 找到最近节点
nearest = self.nearest_node(rand_point)
# 3. 向采样点生长
new_node = self.steer(nearest, rand_point)
# 4. 检查碰撞
if not self.is_collision_free(nearest, new_node):
continue
# 5. 找到附近节点
near_nodes = self.find_near_nodes(new_node)
# 6. 选择最优父节点
parent, cost = self.choose_parent(new_node, near_nodes)
if parent is None:
continue
# 设置新节点的父节点和成本
new_node.parent = parent
new_node.cost = cost
# 7. 添加到树中
self.tree.append(new_node)
# 8. 重新连接
self.rewire(new_node, near_nodes)
# 9. 检查是否到达目标
self.check_goal_connection(new_node)
# 10. 记录可视化数据（每100次迭代记录一次）
if i % 100 == 0 or i == self.max_iter - 1:
self.record_visualization_data(i)
# 显示进度
if i % 500 == 0:
print(f"Iteration: {i}, Best Cost: {self.best_cost:.2f}")
return self.best_path, self.best_cost
def record_visualization_data(self, iteration):
"""记录可视化数据"""
# 记录节点
nodes = [(node.x, node.y) for node in self.tree]
# 记录边
edges = []
for node in self.tree:
if node.parent:
edges.append([(node.parent.x, node.parent.y),
(node.x, node.y)])
# 记录最佳路径
best_path = self.best_path
self.visualization_data['nodes'].append(nodes.copy())
self.visualization_data['edges'].append(edges.copy())
self.visualization_data['best_paths'].append(best_path.copy())
def create_environment():
"""创建测试环境"""
# 地图边界 [x_min, x_max, y_min, y_max]
bounds = [0, 100, 0, 100]
# 起点和终点
start = (10, 10)
goal = (90, 90)
# 圆形障碍物 (x, y, radius)
obstacles = [
(30, 30, 8),
(50, 20, 6),
(60, 60, 10),
(20, 70, 7),
(70, 40, 8),
(40, 80, 9),
(80, 20, 7),
(35, 50, 5)
]
return start, goal, bounds, obstacles
def visualize_animation(rrt_star):
"""创建可视化动画"""
fig, ax = plt.subplots(figsize=(10, 10))
# 设置边界
ax.set_xlim(rrt_star.bounds[0], rrt_star.bounds[1])
ax.set_ylim(rrt_star.bounds[2], rrt_star.bounds[3])
ax.set_aspect('equal')
ax.set_xlabel('X')
ax.set_ylabel('Y')
ax.set_title('Informed RRT* Path Planning with Circular Obstacles')
# 绘制障碍物
for (ox, oy, r) in rrt_star.obstacles:
circle = Circle((ox, oy), r, color='red', alpha=0.6)
ax.add_patch(circle)
# 绘制起点和终点
ax.plot(rrt_star.start.x, rrt_star.start.y, 'go', markersize=12,
label='Start', markeredgecolor='black', markeredgewidth=2)
ax.plot(rrt_star.goal.x, rrt_star.goal.y, 'ro', markersize=12,
label='Goal', markeredgecolor='black', markeredgewidth=2)
# 可视化元素
tree_scatter = ax.scatter([], [], s=10, c='blue', alpha=0.6,
label='RRT Tree Nodes')
tree_lines = [ax.plot([], [], 'gray', linewidth=0.5, alpha=0.3)[0]
for _ in range(len(rrt_star.visualization_data['edges'][0]))]
path_line, = ax.plot([], [], 'yellow', linewidth=3, label='Best Path')
ellipse_patch = None
# 添加图例
ax.legend(loc='upper right')
def init():
"""初始化动画"""
tree_scatter.set_offsets(np.empty((0, 2)))
for line in tree_lines:
line.set_data([], [])
path_line.set_data([], [])
return [tree_scatter, *tree_lines, path_line]
def update(frame):
"""更新动画帧"""
# 更新树节点
nodes = np.array(rrt_star.visualization_data['nodes'][frame])
if len(nodes) > 0:
tree_scatter.set_offsets(nodes)
# 更新树边
edges = rrt_star.visualization_data['edges'][frame]
for i, line in enumerate(tree_lines):
if i < len(edges):
edge = edges[i]
line.set_data([edge[0][0], edge[1][0]],
[edge[0][1], edge[1][1]])
else:
line.set_data([], [])
# 更新最佳路径
best_path = rrt_star.visualization_data['best_paths'][frame]
if len(best_path) > 1:
path_x, path_y = zip(*best_path)
path_line.set_data(path_x, path_y)
else:
path_line.set_data([], [])
# 移除旧的椭圆
nonlocal ellipse_patch
if ellipse_patch is not None:
ellipse_patch.remove()
# 绘制椭圆采样区域（如果已找到初始路径）
if rrt_star.found_initial and frame > 0:
c_min = rrt_star.best_cost
if c_min < float('inf'):
# 椭圆参数
x_center = [(rrt_star.start.x + rrt_star.goal.x) / 2,
(rrt_star.start.y + rrt_star.goal.y) / 2]
angle = math.atan2(rrt_star.goal.y - rrt_star.start.y,
rrt_star.goal.x - rrt_star.start.x)
a = c_min / 2
c = math.sqrt((rrt_star.goal.x - rrt_star.start.x) ** 2 +
(rrt_star.goal.y - rrt_star.start.y) ** 2) / 2
b = math.sqrt(max(0.1, a ** 2 - c ** 2)) # 避免b为负数
# 创建椭圆
ellipse_patch = Ellipse(xy=x_center, width=2*a, height=2*b,
angle=math.degrees(angle),
color='orange', alpha=0.2,
label='Sampling Ellipse')
ax.add_patch(ellipse_patch)
# 更新标题
iteration = frame * 100
ax.set_title(f'Informed RRT* - Iteration: {iteration}, '
f'Path Cost: {rrt_star.best_cost:.2f}')
return [tree_scatter, *tree_lines, path_line]
# 创建动画
frames = len(rrt_star.visualization_data['nodes'])
anim = animation.FuncAnimation(fig, update, frames=frames,
init_func=init, blit=False,
interval=100, repeat=False)
plt.tight_layout()
plt.show()
return anim
def visualize_final_result(rrt_star, best_path, best_cost):
"""可视化最终结果"""
fig, (ax1, ax2) = plt.subplots(1, 2, figsize=(16, 8))
# 子图1：最终规划结果
ax1.set_xlim(rrt_star.bounds[0], rrt_star.bounds[1])
ax1.set_ylim(rrt_star.bounds[2], rrt_star.bounds[3])
ax1.set_aspect('equal')
ax1.set_xlabel('X')
ax1.set_ylabel('Y')
ax1.set_title('Final Planned Path')
# 绘制障碍物
for (ox, oy, r) in rrt_star.obstacles:
circle = Circle((ox, oy), r, color='red', alpha=0.6)
ax1.add_patch(circle)
# 绘制RRT树
for node in rrt_star.tree:
if node.parent:
ax1.plot([node.parent.x, node.x], [node.parent.y, node.y],
'gray', linewidth=0.5, alpha=0.3)
# 绘制树节点
nodes_x = [node.x for node in rrt_star.tree]
nodes_y = [node.y for node in rrt_star.tree]
ax1.scatter(nodes_x, nodes_y, s=10, c='blue', alpha=0.6,
label='RRT Tree Nodes')
# 绘制最佳路径
if best_path:
path_x, path_y = zip(*best_path)
ax1.plot(path_x, path_y, 'yellow', linewidth=3, label='Best Path')
# 绘制起点和终点
ax1.plot(rrt_star.start.x, rrt_star.start.y, 'go', markersize=12,
label='Start', markeredgecolor='black', markeredgewidth=2)
ax1.plot(rrt_star.goal.x, rrt_star.goal.y, 'ro', markersize=12,
label='Goal', markeredgecolor='black', markeredgewidth=2)
# 绘制椭圆采样区域
if rrt_star.found_initial and best_cost < float('inf'):
# 椭圆参数
x_center = [(rrt_star.start.x + rrt_star.goal.x) / 2,
(rrt_star.start.y + rrt_star.goal.y) / 2]
angle = math.atan2(rrt_star.goal.y - rrt_star.start.y,
rrt_star.goal.x - rrt_star.start.x)
a = best_cost / 2
c = math.sqrt((rrt_star.goal.x - rrt_star.start.x) ** 2 +
(rrt_star.goal.y - rrt_star.start.y) ** 2) / 2
b = math.sqrt(max(0.1, a ** 2 - c ** 2))
ellipse = Ellipse(xy=x_center, width=2*a, height=2*b,
angle=math.degrees(angle),
color='orange', alpha=0.2, label='Sampling Ellipse')
ax1.add_patch(ellipse)
ax1.legend(loc='upper right')
ax1.grid(True, alpha=0.3)
# 子图2：收敛曲线
ax2.set_xlabel('Iteration')
ax2.set_ylabel('Best Path Cost')
ax2.set_title('Convergence Curve')
ax2.grid(True, alpha=0.3)
# 提取收敛数据
iterations = list(range(0, len(rrt_star.visualization_data['best_paths']) * 100, 100))
costs = []
for path in rrt_star.visualization_data['best_paths']:
if len(path) > 1:
# 计算路径成本
cost = 0
for i in range(len(path) - 1):
cost += math.hypot(path[i+1][0] - path[i][0],
path[i+1][1] - path[i][1])
costs.append(cost)
else:
costs.append(float('inf'))
# 绘制收敛曲线
valid_indices = [i for i, cost in enumerate(costs) if cost < float('inf')]
if valid_indices:
valid_iterations = [iterations[i] for i in valid_indices]
valid_costs = [costs[i] for i in valid_indices]
ax2.plot(valid_iterations, valid_costs, 'b-', linewidth=2,
label='Path Cost')
ax2.fill_between(valid_iterations, 0, valid_costs, alpha=0.2)
ax2.legend(loc='upper right')
plt.tight_layout()
plt.show()
def main():
"""主函数"""
print("=== Informed RRT* Path Planning Demo ===")
print("Creating environment...")
# 创建环境
start, goal, bounds, obstacles = create_environment()
# 初始化算法
print("Initializing Informed RRT* algorithm...")
rrt_star = InformedRRTStar(start, goal, bounds, obstacles,
max_iter=3000, step_size=15, neighbor_radius=25)
# 执行路径规划
print("Starting path planning...")
best_path, best_cost = rrt_star.plan()
# 显示结果
print("\n=== Planning Results ===")
if best_path:
print(f"Path found! Cost: {best_cost:.2f}")
print(f"Path length: {len(best_path)} nodes")
else:
print("No path found within iteration limit.")
# 可视化最终结果
print("\nGenerating visualizations...")
visualize_final_result(rrt_star, best_path, best_cost)
# 询问是否查看动画
response = input("\nDo you want to see the animation? (y/n): ")
if response.lower() == 'y':
print("Generating animation (this may take a moment)...")
anim = visualize_animation(rrt_star)
print("\nDemo completed!")
if __name__ == "__main__":
main()算法特点与使用说明
🎯 算法核心特点
- Informed RRT*：找到初始路径后，将采样区域限制在椭圆内，大大提高了收敛速度
- 圆形障碍物：使用精确的几何碰撞检测，支持多个圆形障碍物
- 双向优化：找到路径后继续优化，直到达到最佳路径
- 完整可视化：
· 实时显示RRT树的生长过程
· 动态显示椭圆采样区域
· 路径收敛曲线分析
🚀 运行方式
- 确保已安装必要的库：
bash
pip install numpy matplotlib- 直接运行脚本，程序将：
· 创建包含8个圆形障碍物的测试环境
· 执行Informed RRT*路径规划（3000次迭代）
· 显示最终规划结果和收敛曲线
· 可选择观看完整的动画过程
⚙️ 参数调整建议
· 增加max_iter：如果需要更优路径，可增加到5000-10000
· 调整step_size：较小的步长（10-15）适合复杂环境，较大的步长（20-30）适合开阔环境
· 修改neighbor_radius：通常设置为step_size的1.5-2倍
· 自定义障碍物：在create_environment()函数中添加/修改圆形障碍物
📊 输出说明
· 左侧图：显示最终路径、RRT树、椭圆采样区域
· 右侧图：显示路径成本的收敛过程
· 动画：展示算法完整的运行过程（可选）
这个实现包含了Informed RRT*的所有关键特性，并提供了丰富的可视化功能。你可以根据需要修改环境配置或算法参数来适应不同的场景。
我将重写 sample_in_ellipse 函数，使用矩阵变换的方法来实现更简洁高效的椭圆采样：
python
import numpy as np
def sample_in_ellipse(self):
"""在椭圆采样区域内生成随机点（使用矩阵变换）"""
# 如果没有找到初始路径，使用均匀采样
if not self.found_initial or self.best_cost == float('inf'):
return self.random_point()
# 计算椭圆参数
start = np.array([self.start.x, self.start.y])
goal = np.array([self.goal.x, self.goal.y])
# 椭圆中心
center = (start + goal) / 2.0
# 椭圆半长轴（沿起点到终点方向）
c_min = self.best_cost
a = c_min / 2.0 # 半长轴
# 起点到终点的距离
c = np.linalg.norm(goal - start) / 2.0 # 焦距
# 计算半短轴（确保不为负）
if a ** 2 - c ** 2 < 0:
b = 0.1 # 最小正值，避免数值问题
else:
b = np.sqrt(a ** 2 - c ** 2) # 半短轴
# 计算旋转角度
direction = goal - start
angle = np.arctan2(direction[1], direction[0])
# 构建变换矩阵
# 1. 旋转矩阵（将椭圆转正）
cos_theta = np.cos(-angle)
sin_theta = np.sin(-angle)
R = np.array([[cos_theta, -sin_theta],
[sin_theta, cos_theta]])
# 2. 缩放矩阵（将椭圆变为单位圆）
S = np.array([[1.0/a, 0.0],
[0.0, 1.0/b]])
# 3. 组合变换：先旋转再缩放
T = S @ R
# 在单位圆内采样
max_attempts = 100 # 防止无限循环
for _ in range(max_attempts):
# 方法1：使用均匀圆内采样（推荐）
# 在单位圆内均匀采样
r = np.sqrt(np.random.random())
theta = 2 * np.pi * np.random.random()
# 单位圆内的点
point_in_circle = np.array([r * np.cos(theta),
r * np.sin(theta)])
# 方法2：拒绝采样法（备用）
# point_in_circle = np.random.uniform(-1, 1, 2)
# if np.linalg.norm(point_in_circle) > 1:
# continue # 不在单位圆内，重新采样
# 逆变换：将单位圆内的点变换到椭圆
# 先进行缩放逆变换，再进行旋转逆变换
R_inv = np.array([[np.cos(angle), -np.sin(angle)],
[np.sin(angle), np.cos(angle)]])
S_inv = np.array([[a, 0.0],
[0.0, b]])
# 组合逆变换：先缩放再旋转
T_inv = R_inv @ S_inv
# 应用到采样点并平移
point_in_ellipse = T_inv @ point_in_circle + center
# 检查是否在边界内
if (self.bounds[0] <= point_in_ellipse[0] <= self.bounds[1] and
self.bounds[2] <= point_in_ellipse[1] <= self.bounds[3]):
return (float(point_in_ellipse[0]), float(point_in_ellipse[1]))
# 如果多次尝试都失败，返回一个默认采样点
print("Warning: Failed to sample within ellipse after multiple attempts")
return self.random_point()
# 为了保持代码兼容性，更新类的初始化方法以使用NumPy
def __init__(self, start, goal, bounds, obstacles,
max_iter=2000, step_size=20, neighbor_radius=30):
"""
初始化算法
参数:
start: 起点坐标 (x, y)
goal: 终点坐标 (x, y)
bounds: 地图边界 [x_min, x_max, y_min, y_max]
obstacles: 障碍物列表，每个障碍物为 (x, y, radius)
max_iter: 最大迭代次数
step_size: 步长
neighbor_radius: 邻居搜索半径
"""
self.start = Node(start[0], start[1])
self.goal = Node(goal[0], goal[1])
self.bounds = bounds
self.obstacles = obstacles
self.max_iter = max_iter
self.step_size = step_size
self.neighbor_radius = neighbor_radius
# RRT树
self.tree = [self.start]
# 路径和成本
self.best_path = []
self.best_cost = float('inf')
self.found_initial = False
# 可视化数据
self.visualization_data = {
'nodes': [],
'edges': [],
'best_paths': []
}
# 预计算一些常用值以提高性能
self.start_np = np.array([start[0], start[1]])
self.goal_np = np.array([goal[0], goal[1]])
self.center_np = (self.start_np + self.goal_np) / 2.0
self.base_distance = np.linalg.norm(self.goal_np - self.start_np)矩阵变换方法的优势
🔧 变换矩阵分解
整个变换过程可以分解为以下几个步骤：
python
# 从单位圆到椭圆的完整变换过程：
point_in_ellipse = R_inv @ S_inv @ point_in_circle + center
# 其中：
# R_inv: 旋转矩阵的逆（将点转回椭圆方向）
# S_inv: 缩放矩阵的逆（将单位圆拉伸为椭圆）
# center: 平移向量（将中心移回正确位置）📊 性能优化版本
如果需要更高的性能，可以使用预计算变换矩阵：
python
def sample_in_ellipse_optimized(self):
"""优化的椭圆采样（预计算变换矩阵）"""
if not self.found_initial or self.best_cost == float('inf'):
return self.random_point()
# 预计算变换矩阵（如果参数发生变化时重新计算）
if not hasattr(self, '_cached_transform') or self._cached_transform['cost'] != self.best_cost:
self._update_transform_matrix()
# 在单位圆内采样
r = np.sqrt(np.random.random())
theta = 2 * np.pi * np.random.random()
point_in_circle = np.array([r * np.cos(theta), r * np.sin(theta)])
# 应用预计算的变换
point_in_ellipse = self._T_inv @ point_in_circle + self.center_np
# 边界检查
if (self.bounds[0] <= point_in_ellipse[0] <= self.bounds[1] and
self.bounds[2] <= point_in_ellipse[1] <= self.bounds[3]):
return (float(point_in_ellipse[0]), float(point_in_ellipse[1]))
return self.random_point()
def _update_transform_matrix(self):
"""更新预计算的变换矩阵"""
c_min = self.best_cost
a = c_min / 2.0
c = self.base_distance / 2.0
if a ** 2 - c ** 2 < 0:
b = 0.1
else:
b = np.sqrt(a ** 2 - c ** 2)
# 计算旋转角度
direction = self.goal_np - self.start_np
angle = np.arctan2(direction[1], direction[0])
# 构建逆变换矩阵（从单位圆到椭圆）
cos_angle = np.cos(angle)
sin_angle = np.sin(angle)
R_inv = np.array([[cos_angle, -sin_angle],
[sin_angle, cos_angle]])
S_inv = np.array([[a, 0.0],
[0.0, b]])
self._T_inv = R_inv @ S_inv
self._cached_transform = {'cost': c_min, 'a': a, 'b': b, 'angle': angle}🎯 两种采样方法的对比
特性 原方法（极坐标） 新方法（矩阵变换）
数学清晰度 直观但公式分散 矩阵运算，结构清晰
代码可读性 中等 高（分离了变换步骤）
扩展性 有限 容易扩展到3D或更高维
性能 中等 优化后更高
数值稳定性 可能受角度计算影响 矩阵运算更稳定
💡 使用建议
- 基本场景：使用第一个版本（sample_in_ellipse）即可，代码清晰且性能足够
- 高性能需求：使用优化版本（sample_in_ellipse_optimized），特别适用于大量迭代
- 3D扩展：矩阵变换方法可以轻松扩展到3D空间，只需将2x2矩阵改为3x3矩阵
这个矩阵变换方法不仅代码更简洁，而且数学上更优雅，更容易理解和维护。变换矩阵的方法也更符合计算机图形学和机器人学中的标准实践。