我想结合RRT*算法和APF对船舶自动航行的路径规划进行研究，以下是我的代码，你能帮我改进一下吗 import numpy as np import random import matplotlib.pyplot as plt from matplotlib.animation import FuncAnimation from math import log # ================== 参数配置 ================== vmax = 2.0 k_goal = 1.5 k_obs = 40.0 radius_obs = 0.5 radius_safe = 1.0 boundary = (0, 10) max_iterations = 1000 # 调整为较小的值以便快速测试 step_size = 1.5 goal_sample_rate = 0.3 gamma = 50.0 # 环境设置 start = np.array([0.0, 0.0]) goal = np.array([9.0, 9.0]) obstacles = [(1, 2), (4, 5), (7, 8)] # ================== 基础类定义 ================== class Node: def __init__(self, point, parent=None): self.point = np.array(point, dtype=np.float64) self.parent = parent self.cost = parent.cost + distance(parent.point, self.point) if parent else 0.0 def distance(p1, p2): return np.linalg.norm(np.array(p1) - np.array(p2)) def is_collision(p1, p2, obstacles, radius_safe): p1 = np.array(p1) p2 = np.array(p2) line_vec = p2 - p1 line_len =

### 船舶路径规划中的RRT*与APF融合优化 为了实现船舶路径规划中RRT*与人工势场法（APF）的有效融合，可以从以下几个方面入手： #### 1. **算法框架设计** 将RRT*作为主要路径生成工具，负责探索未知环境并构建初始可行路径；而APF则用于局部优化和微调路径。具体来说，可以在每一步扩展过程中利用APF引导节点朝向目标点移动，从而提高收敛速度。 在实际编码时可以采用如下逻辑： - 使用RRT*的核心机制来处理全局路径规划。 - 利用APF提供额外的吸引力或排斥力作用于当前节点的选择过程，使得新生成的节点更加贴近期望轨迹[^1]。 ```python import numpy as np from scipy.spatial import cKDTree class Node: def __init__(self, position): self.position = position self.parent = None def rrt_star_apf(start_position, goal_position, obstacles, max_iterations=5000, step_size=0.5, gamma_rrt=1.0, eta=0.2): nodes = [Node(np.array(start_position))] for _ in range(max_iterations): q_rand = sample_free_space(goal_position) # 随机采样函数 # 寻找最近邻居 tree_positions = np.array([node.position for node in nodes]) kdtree = cKDTree(tree_positions) _, nearest_index = kdtree.query(q_rand) q_near = nodes[nearest_index].position # 执行增量扩展 q_new_pos = extend_towards(q_near, q_rand, step_size) if not check_collision(q_new_pos, obstacles): # 碰撞检测 new_node = Node(q_new_pos) # 应用APF调整位置 apf_adjusted_pos = apply_apf(new_node.position, goal_position, obstacles, eta) new_node.position = apf_adjusted_pos # 计算成本并连接到最佳父节点 rewire_tree(nodes, new_node, gamma_rrt) final_path = extract_final_path(nodes, goal_position) return final_path def sample_free_space(goal_position): """随机采样优先靠近目标""" bias_prob = 0.1 # 目标偏置概率 if np.random.rand() < bias_prob: return tuple(goal_position + (np.random.randn(len(goal_position)) * 0.1)) else: return tuple((np.random.rand(len(goal_position)) - 0.5) * 10) def extend_towards(current_point, target_point, step_size): direction_vector = np.array(target_point) - current_point norm_direction = direction_vector / np.linalg.norm(direction_vector) next_step = current_point + norm_direction * min(step_size, np.linalg.norm(direction_vector)) return tuple(next_step) def apply_apf(node_position, goal_position, obstacles, eta): attractive_force = compute_attractive_force(node_position, goal_position, eta) repulsive_forces_sum = sum(compute_repulsive_force(node_position, obs, eta) for obs in obstacles) adjusted_position = np.array(node_position) + attractive_force + repulsive_forces_sum return tuple(adjusted_position) def compute_attractive_force(position, goal, eta): distance_to_goal = np.linalg.norm(np.array(goal) - position) force_magnitude = eta * distance_to_goal unit_direction = (goal - position) / distance_to_goal if distance_to_goal != 0 else 0 return force_magnitude * unit_direction def compute_repulsive_force(position, obstacle_center, eta): d_obstacle = np.linalg.norm(obstacle_center - position) if d_obstacle <= eta: magnitude = ((eta - d_obstacle)**2) / (d_obstacle * eta**2) direction = -(obstacle_center - position) / d_obstacle if d_obstacle != 0 else 0 return magnitude * direction else: return 0 def rewire_tree(nodes, candidate_node, gamma_rrt): neighbors_indices = find_neighbors_within_radius(candidate_node.position, nodes, gamma_rrt) optimal_parent = select_optimal_parent(neighbors_indices, candidate_node, nodes) candidate_node.parent = optimal_parent update_children_costs(optimal_parent, candidate_node.cost) def extract_final_path(nodes, goal_position): closest_node = min(nodes, key=lambda n: np.linalg.norm(n.position - goal_position)) path = [] while closest_node is not None: path.append(closest_node.position) closest_node = closest_node.parent return list(reversed(path)) # 辅助功能定义省略... ``` 此代码片段展示了如何结合两种技术完成基本的功能开发。其中`apply_apf()`实现了对每一个新增加节点的位置修正操作[^3]。 #### 2. **参数调节建议** 对于不同的应用场景可能需要适当修改某些超参比如吸引因子强度(`eta`)以及最大迭代次数等设置以满足特定需求下的性能表现要求[^2]。 --- ####