探索路径规划算法：DWA、A*、RRT 及其融合

DWA算法与RRT*融合算法。 还有单独dwa单独A*RRTRRT*2/3d地图等路径规划算法可自定义地图。 运行效果如图所示在机器人运动规划领域路径规划算法一直是核心话题。今天咱们就唠唠 DWA 算法、A算法、RRT 算法以及 RRT算法还有它们之间的融合以及如何自定义地图来测试这些算法。DWA 算法DWADynamic Window Approach算法主要用于动态环境下的局部路径规划。它通过考虑机器人当前的速度和加速度限制在一个动态窗口内搜索最优的速度和转向角。以下是简化的 DWA 代码示例Python 伪代码def dwa_algorithm(robot_state, obstacles, goal): # 获取机器人的速度限制和加速度限制 min_vel, max_vel, min_omega, max_omega get_robot_limits(robot_state) # 定义动态窗口 dynamic_window [(v, omega) for v in np.arange(min_vel, max_vel, 0.1) for omega in np.arange(min_omega, max_omega, 0.1)] best_score -np.inf best_vel None for vel, omega in dynamic_window: # 预测下一时刻的状态 predicted_state predict_state(robot_state, vel, omega) # 计算代价函数这里简单以到目标点的距离为例 score -distance_to_goal(predicted_state, goal) - collision_cost(predicted_state, obstacles) if score best_score: best_score score best_vel (vel, omega) return best_vel这段代码首先确定机器人的速度和转向角范围形成动态窗口。然后对窗口内每个可能的速度和转向角组合进行状态预测并根据到目标点的距离和碰撞代价计算得分最终选择得分最高的组合作为下一时刻的控制指令。A* 算法A*算法是经典的全局路径规划算法它结合了 Dijkstra 算法的广度优先搜索和贪心算法的最佳优先搜索思想。通过一个启发函数比如曼哈顿距离或欧几里得距离来引导搜索朝着目标前进。import heapq def a_star(start, goal, map): open_set [] heapq.heappush(open_set, (0, start)) came_from {} g_score {node: float(inf) for node in map.nodes()} g_score[start] 0 f_score {node: float(inf) for node in map.nodes()} f_score[start] heuristic(start, goal) while open_set: _, current heapq.heappop(open_set) if current goal: path [] while current in came_from: path.append(current) current came_from[current] path.append(start) path.reverse() return path for neighbor in map.neighbors(current): tentative_g_score g_score[current] cost(current, neighbor) if tentative_g_score g_score[neighbor]: came_from[neighbor] current g_score[neighbor] tentative_g_score f_score[neighbor] tentative_g_score heuristic(neighbor, goal) if neighbor not in [i[1] for i in open_set]: heapq.heappush(open_set, (f_score[neighbor], neighbor)) return None这里通过优先队列openset来存储待扩展节点根据fscoregscore加上启发函数值来排序。gscore表示从起点到当前节点的实际代价heuristic函数估计从当前节点到目标节点的代价。不断扩展节点直到找到目标节点或者队列为空。RRT 与 RRT* 算法RRTRapidly - exploring Random Tree算法是一种基于采样的路径规划算法通过随机采样空间中的点并逐步扩展搜索树找到从起点到目标点的路径。import random class Node: def __init__(self, state): self.state state self.parent None def rrt(start, goal, map, max_iter): root Node(start) tree [root] for i in range(max_iter): sample sample_free(map) nearest nearest_neighbor(tree, sample) new_state steer(nearest.state, sample) if not collision(new_state, map): new_node Node(new_state) new_node.parent nearest tree.append(new_node) if distance(new_state, goal) goal_threshold: goal_node Node(goal) goal_node.parent new_node tree.append(goal_node) path [] current goal_node while current.parent: path.append(current.state) current current.parent path.append(start) path.reverse() return path return None上述代码中rrt函数不断在地图的自由空间采样点sample找到树中距离采样点最近的节点nearest向采样点方向前进得到新节点new_node如果新节点不与障碍物碰撞则加入树中直到找到接近目标点的路径。DWA算法与RRT*融合算法。 还有单独dwa单独A*RRTRRT*2/3d地图等路径规划算法可自定义地图。 运行效果如图所示RRT* 是 RRT 的改进版本它在扩展树的过程中通过重新布线rewiring操作来优化路径使得生成的路径更优。def rrt_star(start, goal, map, max_iter): root Node(start) tree [root] for i in range(max_iter): sample sample_free(map) nearest nearest_neighbor(tree, sample) new_state steer(nearest.state, sample) if not collision(new_state, map): new_node Node(new_state) new_node.parent nearest nearby nearby_nodes(tree, new_state) best_parent select_best_parent(new_node, nearby) new_node.parent best_parent tree.append(new_node) rewire(new_node, nearby) if distance(new_state, goal) goal_threshold: goal_node Node(goal) goal_node.parent new_node tree.append(goal_node) path [] current goal_node while current.parent: path.append(current.state) current current.parent path.append(start) path.reverse() return path return None在rrtstar函数里selectbest_parent函数从附近节点中选择一个使新节点代价最小的节点作为父节点rewire函数则尝试优化附近节点的父节点以降低整个树的代价。DWA 与 RRT* 融合算法融合 DWA 和 RRT算法可以结合全局路径规划和局部动态规划的优势。先使用 RRT算法生成全局路径然后在机器人沿着全局路径行进时使用 DWA 算法进行局部路径调整以应对动态障碍物。def dwa_rrt_star(start, goal, map): global_path rrt_star(start, goal, map, max_iter1000) if not global_path: return None robot_state start for i in range(len(global_path) - 1): sub_goal global_path[i 1] while distance(robot_state, sub_goal) sub_goal_threshold: vel, omega dwa_algorithm(robot_state, get_obstacles(map), sub_goal) robot_state update_state(robot_state, vel, omega) return global_path这段代码先用rrtstar生成全局路径然后机器人沿着全局路径分段前进每段使用dwaalgorithm调整路径以适应动态环境。自定义地图为了测试这些算法我们可以自定义地图。以简单的二维地图为例地图可以用二维数组表示0 表示自由空间1 表示障碍物。map [ [0, 0, 0, 0, 0], [0, 1, 0, 1, 0], [0, 0, 0, 0, 0], [0, 1, 1, 1, 0], [0, 0, 0, 1, 0] ]在实际应用中地图可以从文件读取或者通过传感器实时生成。以上这些算法各有优劣在实际项目中我们可以根据具体场景选择合适的算法或者算法融合方案。希望这篇博文能帮助你对路径规划算法有更深入的了解。运行效果就像我们看到的图示那样不同算法在自定义地图上各展神通为机器人规划出一条条或全局最优、或适应动态环境的路径。