rrt算法栅格地图python

### 基于RRT算法在栅格地图上的路径规划Python实现 #### RRT算法简介 RRT（Rapidly-exploring Random Tree）算法是一种高效的随机化路径规划方法，尤其适合应用于高维空间和复杂环境中的路径规划问题。该算法通过构建一棵不断扩展的随机树来探索环境，最终找到从起始点到目标点的可行路径[^2]。 #### 栅格地图表示 为了适应栅格地图，在实现过程中需要考虑如何有效地表示障碍物以及自由空间。通常情况下，可以使用二维数组来表示栅格地图，其中0代表无障碍区域，1或其他数值则表示存在障碍物的位置。 #### Python代码示例 下面是一个简单的基于RRT算法在栅格地图上进行路径规划的Python代码示例： ```python import numpy as np import random import matplotlib.pyplot as plt class Node: def __init__(self, x, y): self.x = x self.y = y self.parent = None def get_random_point(map_size): return (random.uniform(0, map_size[0]), random.uniform(0, map_size[1])) def nearest_node(nodes, point): distances = [(node.x - point[0])**2 + (node.y - point[1])**2 for node in nodes] min_index = np.argmin(distances) return nodes[min_index] def steer(from_node, to_point, extend_length=0.5): new_x, new_y = from_node.x, from_node.y direction = np.array([to_point[0]-from_node.x, to_point[1]-from_node.y]) length = np.linalg.norm(direction) if length > extend_length: direction = direction / length * extend_length new_x += direction[0] new_y += direction[1] return Node(new_x, new_y), length <= extend_length def check_collision(node, obstacle_map, resolution=0.1): grid_x = int(node.x/resolution) grid_y = int(node.y/resolution) try: return not bool(obstacle_map[grid_y][grid_x]) # True means no collision. except IndexError: return False def rrt_planning(start, goal, obstacle_map, max_iter=1000, step_len=0.5): start_node = Node(*start) end_node = Node(*goal) tree_nodes = [start_node] for _ in range(max_iter): rand_pt = get_random_point((len(obstacle_map)*0.1, len(obstacle_map[0])*0.1)) near_node = nearest_node(tree_nodes, rand_pt) new_node, reached_goal = steer(near_node, rand_pt, extend_length=step_len) if check_collision(new_node, obstacle_map): new_node.parent = near_node tree_nodes.append(new_node) if reached_goal and ((new_node.x-goal[0])**2+(new_node.y-goal[1])**2)**0.5 < step_len*2: path = [] while new_node is not None: path.append((new_node.x, new_node.y)) new_node = new_node.parent return list(reversed(path)) return [] if __name__ == "__main__": # Define a simple binary occupancy grid where '0' represents free space, # and any non-zero value indicates an occupied cell or obstacle. obstacle_map = [ [0, 0, 0, 0, 0], [0, 1, 1, 1, 0], [0, 0, 0, 1, 0], [0, 1, 0, 0, 0], [0, 0, 0, 0, 0] ] start_pos = (0.5, 0.5) # Start position within the continuous coordinate system of the environment. target_pos = (4.5, 4.5) # Target position also given by coordinates. result_path = rrt_planning(start=start_pos, goal=target_pos, obstacle_map=obstacle_map) fig, ax = plt.subplots() ax.imshow(np.transpose(obstacle_map), origin='lower', cmap="gray_r") if result_path: px, py = zip(*result_path) ax.plot(px, py, '-r') print("Path found!") else: print("No viable path could be determined.") plt.show() ``` 此段代码实现了基本版的RRT算法，并将其应用到了一个简化版本的二值占用网格图中。需要注意的是实际应用场景下还需要进一步优化参数设置并加入更多功能特性以提高鲁棒性和效率[^1]。

### RRT算法在二维栅格地图上的实现 #### 构建环境模型 为了使RRT算法能够有效工作于二维栅格地图，首先要定义好环境边界以及障碍物的位置。这可以通过创建一个矩阵来表示，其中每个元素对应着地图上的一个小方格；对于无障碍的地方设置为0，而对于存在障碍物之处则标记成1或其他非零数值[^2]。 #### 初始化参数与节点类设计 接着要设定一些必要的初始化参数，比如最大迭代次数、步长等。同时还需要编写`Node`类用于存储各个节点的信息（坐标位置），并能方便地计算两节点间的距离或角度差值[^3]。 ```python class Node: def __init__(self, x, y): self.x = x self.y = y self.parent = None # 记录父节点以便回溯路径 def distance(node1, node2): return ((node1.x - node2.x)**2 + (node1.y - node2.y)**2)**0.5 ``` #### 随机采样策略 采用均匀分布的方式在整个可行区域内选取新的样本点作为潜在的目标方向。考虑到目标导向的重要性，在某些情况下可以增加一定概率直接取终点为目标，从而加速收敛过程。 ```python import numpy as np def sample_free_space(map_size, goal_pos, bias=0.1): if np.random.rand() < bias: # 给定bias比例偏向选择终点 return goal_pos.copy() while True: pos = [np.random.randint(0, map_size[0]), np.random.randint(0, map_size[1])] if not is_obstacle(pos): # 判断是否位于障碍区内 break return pos ``` #### 寻找最近邻居及生长新枝条 每当获得一个新的随机样本之后，就需要在其周围查找已存在的最邻近的树形结构成员，并尝试向其延伸一小段距离形成新的分支。如果遇到碰撞，则放弃此次操作继续下一轮循环直到满足终止条件为止[^4]。 ```python from scipy.spatial import KDTree def nearest_neighbor(nodes_positions, query_point): tree = KDTree(nodes_positions) _, idx = tree.query(query_point) return nodes_positions[idx] def extend_tree(current_node, sampled_point, step_length): direction_vector = normalize(np.array(sampled_point)-np.array([current_node.x,current_node.y])) next_position = current_node.position + direction_vector * min(step_length,distance(current_node,sampled_point)) if check_collision(next_position): return None new_node = Node(*next_position.tolist()) new_node.parent=current_node return new_node ``` 以上就是利用Python语言描述的一个简化版适用于二维平面内静态场景下的快速探索随机树(Rapidly-exploring Random Tree,RRT)路径规划算法框架。实际应用时还需考虑更多细节优化如平滑处理最终得到的轨迹线段使之更加贴近实际情况的要求等等。