RRT路径规划算法在⼆维仿真环境中的应⽤--Python代码实现
在上⼀节中，介绍了 RRT 算法的原理，这⼀节将⼀步步实现 RRT 路径规划算法在⼆维环境中的路径规划，来进⼀步加深对 RRT 算法的理解。
⼆维环境的搭建
我们将搭建下图所⽰的⼆维环境，绿⾊点为起点（0，0），红⾊点为⽬标点（15, 12），⿊⾊的圆表⽰障碍物。
实现上述环境的代码如下：
start =[0,0]# 起点
goal =[15,12]# 终点
# 障碍物（x, y, radiu）
obstacle_list =[
(3,3,1.5),
(12,2,3),
(3,9,2),
(9,11,2)
]
plt.axis([-2,18,-2,15])
for(ox, oy, size)in obstacle_list:
plt.plot(ox, oy,"ok", ms=30* size)
plt.plot(start[0], start[1],"og")
plt.plot(goal[0], goal[1],"or")
plt.show()
plt.pause(0.01)
RRT 路径规划算法
1. 算法初始化
class RRT:
# 初始化
def__init__(self,
obstacle_list,# 障碍物
rand_area,# 采样的区域
expand_dis=2.0,# 步长
goal_sample_rate=10,# ⽬标采样率
max_iter=200):# 最⼤迭代次数
self.start =None
self.min_rand = rand_area[0]
self.max_rand = rand_area[1]
self.max_iter = max_iter
self.obstacle_list = obstacle_list
2. 路径规划
1. 将起点和终点结点化，⽅便计算该结点到起点的路径距离以及后⾯的路径回溯。
def rrt_planning(self, start, goal, animation=True):
self.start = Node(start[0], start[1])
path =None
结点化的代码如下：
class Node:
def__init__(self, x, y):
self.x = x
self.y = y
self.parent =None
2. 开始在环境中循环采样点，在此有⼀个⼩的采样技巧，为了使 RRT 树能朝着⽬标点的⽅向⽣长，在采样时，以⼀定的概率采样
⽬标点。
rnd = self.sample()# 在环境中随机采样点
采样函数如下：
def sample(self):
if random.randint(0,100)> al_sample_rate:
rnd =[random.uniform(self.min_rand, self.max_rand),
random.uniform(self.min_rand, self.max_rand)]
else:
rnd =[al.x, al.y]
return rnd
3. 从结点树中到距离采样点最近的结点。
n_ind = _nearest_list_de_list, rnd)
nearest_node = de_list[n_ind]
def get_nearest_list_index(nodes, rnd):
""" 计算树中距离采样点距离最近的结点 """
d_list =[(node.x - rnd[0])**2+(node.y - rnd[1])**2
for node in nodes]
min_index = d_list.index(min(d_list))
return min_index
4. ⽣成新的下⼀个结点。在到树中距离采样点最近的结点后，可以计算两个结点的连线和⽔平的⽅向的⾓度，再根据步长的⼤
⼩，即可计算出下⼀个树结点的位置。
theta = math.atan2(rnd[1]- nearest_node.y, rnd[0]- nearest_node.x)
new_node = _new_node(theta, n_ind, nearest_node)
def get_new_node(self, theta, n_ind, nearest_node):
""" 计算新结点 """
new_node = copy.deepcopy(nearest_node)
new_node.x += pand_dis * s(theta)
new_node.y += pand_dis * math.sin(theta)
st += pand_dis
new_node.parent = n_ind
return new_node
5. 检测碰撞。检测新⽣成的结点的路径是否会与障碍物碰撞
no_collision = self.check_segment_collision(new_node.x, new_node.y, nearest_node.x, nearest_node.y)
其中检测碰撞的函数如下：
def check_segment_collision(self, x1, y1, x2, y2):
for(ox, oy, radius)in self.obstacle_list:
dd = self.distance_squared_point_to_segment(
np.array([x1, y1]),
np.array([x2, y2]),
np.array([ox, oy])
)
if dd <= radius **2:
return False
return True
其中 distance_squared_point_to_segment()函数的功能为：求点到线段的最短距离，代码如下：
def distance_squared_point_to_segment(v, w, p):
""" 计算线段 vw 和点 p 之间的最短距离"""
if np.array_equal(v, w):# 点 v 和点 w 重合的情况
return(p - v).dot(p - v)
l2 =(w - v).dot(w - v)# 线段 vw 长度的平⽅
t =max(0,min(1,(p - v).dot(w - v)/ l2))
projection = v + t *(w - v)
return(p - projection).dot(p - projection)
6. 如果没有与障碍物发⽣碰撞，则将新结点加⼊到树中，并绘制新结点以及⽣长的新枝⼲。再判断新结点是否是⽬标点的邻接结
点。
if no_collision:
# ⼀步⼀绘制
if animation:
self.draw_graph(new_node, path)
# 判断新结点是否临近⽬标点
if self.is_near_goal(new_node):
if self.check_segment_collision(new_node.x, new_node.y,
last_index =de_list)-1
path = _final_course(last_index)# 回溯路径
path_length = _path_len(path)# 计算路径的长度
print("当前的路径长度为：{}".format(path_length))
if animation:
self.draw_graph(new_node, path)
return path
其中，is_near_goal()是判断新结点是否邻近⽬标点的函数，其代码如下：
def is_near_goal(self, node):
d = self.line_cost(node, al)
if d < pand_dis:
return True
return False
line_cost() 函数是如果新⽣成的结点邻近⽬标结点的情况下，该结点到⽬标结点之间的距离。其代码如下：
def line_cost(node1, node2):
return math.sqrt((node1.x - node2.x)**2+(node1.y - node2.y)**2)
get_final_course()是获取最终从起点到终点的路径的函数。其代码如下：
def get_final_course(self, last_index):
""" 回溯路径 """
path =[[al.x, al.y]]
de_list[last_index].parent is not None:
node = de_list[last_index]
path.append([node.x, node.y])
last_index = node.parent
path.append([self.start.x, self.start.y])
return path
get_path_len()是求取路径的总长度的函数，其代码如下：
def get_path_len(path):
""" 计算路径的长度 """
path_length =0
for i in range(1,len(path)):
node1_x = path[i][0]
node1_y = path[i][1]
node2_x = path[i -1][0]
node2_y = path[i -1][1]
path_length += math.sqrt((node1_x - node2_x)**2+(node1_y - node2_y)**2)
return path_length
draw_graph() 为绘制地图以及结点路径等函数，使之可视化。其代码如下：
def draw_graph(self, rnd=None, path=None):
plt.clf()
# 绘制新的结点
if rnd is not None:
plt.plot(rnd.x, rnd.y,'^k')
# 绘制路径
for node de_list:
if node.parent is not None:
if node.x or node.y is not None:
plt.plot([node.x, de_list[node.parent].x],
[node.y, de_list[node.parent].y],
'-g')
# 绘制障碍物
for(ox, oy, size)in self.obstacle_list:
plt.plot(ox, oy,"ok", ms=30* size)
# 绘制起点、终点
plt.plot(self.start.x, self.start.y,"og")
plt.plot(self.start.x, self.start.y,"og")
plt.al.x, al.y,"or")
# 绘制路径
if path is not None:
plt.plot([x for(x, y)in path],[y for(x, y)in path],'-r')
# 绘制图的设置
plt.axis([-2,18,-2,15])
plt.pause(0.01)
最终运⾏结果如下：
完整代码如下：
import random
import math
import copy
import time
import matplotlib.pyplot as plt
import numpy as np
class RRT:
# 初始化
def__init__(self,
obstacle_list,# 障碍物
rand_area,# 采样的区域
python新手代码你好expand_dis=2.0,# 步长
goal_sample_rate=10,# ⽬标采样率
max_iter=200):# 最⼤迭代次数
self.start =None
self.min_rand = rand_area[0]
self.max_rand = rand_area[1]
self.max_iter = max_iter
self.obstacle_list = obstacle_list

版权声明：本站内容均来自互联网，仅供演示用，请勿用于商业和其他非法用途。如果侵犯了您的权益请与我们联系QQ:729038198，我们将在24小时内删除。