Python实现改进后的Bi-RRT算法实例
1.背景说明
以下代码是参照 上海交通大学海洋工程国家重点实验室《基于改进双向RRT的无人艇局部路径规划算法研究》的算法思想实现的。
2.算法流程
- 产生随机节点pi
- 寻找T1中距离p1最近的节点pn
- 以pn为父节点按原始步长向pi延伸得到虚新节点pa
- 确定距离pi最近的障碍物
- 使用动态步长策略计算实际步长sf
- 按照实际sf延伸得到实际节点新pw
- 障碍物检测 通过则进入步骤8 否则重回步骤1
- 转角约束检测 通过则进入步骤9 否则重回步骤1
- 将pw加入T1
- 在T2中寻找距离pw最近的节点pj
- 以pj为父节点按原始步长向pw延伸得到虚新节点pb
- 确定距离pb最近的障碍物
- 使用动态步长策略计算实际步长sf
- 按照实际sf延伸得到实际新节点px
- 障碍物检测 通过则进入步骤16 否则重回步骤1
- 转角约束检测 通过则进入步骤17 否则重回步骤1
- 将pw加入T2
- 检测是否满足相遇条件 是则进入步骤20 否则继续步骤1
- 检测是否满足平滑连接 是则结束搜索 否则继续步骤1
- 路径回溯
3.实际代码
"""
基于改进双向RRT算法的路径规划
"""
import matplotlib.pyplot as plt
from matplotlib.pyplot import MultipleLocator
import numpy as np
import math
import random
import copy
class Point(object):
"""
路径节点
"""
def __init__(self, x, y):
self.x = x
self.y = y
self.parent = None
class BiRRT(object):
"""
双向RRT算法实现
"""
def __init__(self, start, goal, angle, step, distance, obstacle_list, rand_area, safe, recover):
"""
初始化
:param start: 起点 [x,y]
:param goal: 终点 [x,y]
:param angle: 转向角 60
:param step: 基础步长 10
:param obstacle_list: 障碍物列表 [[x,y,radius]...]
:param rand_area: 区域大小
:param safe: 安全航迹结束点
:param recover: 安全航迹恢复点
"""
self.start = Point(start[0], start[1])
self.goal = Point(goal[0], goal[1])
self.angle = angle
self.step = step
self.distance = distance
self.obstacle_list = obstacle_list
self.min_rand = rand_area[0]
self.max_rand = rand_area[1]
self.goalSampleRate = 0.05
self.safe = Point(safe[0], safe[1])
self.recover = Point(recover[0], recover[1])
# 从起点开始搜索
self.point_list_from_start = [self.start]
begin = copy.deepcopy(self.safe)
begin.parent = 0
self.point_list_from_start.append(begin)
# 从终点开始搜索
self.point_list_from_goal = [self.goal]
begin = copy.deepcopy(self.recover)
begin.parent = 0
self.point_list_from_goal.append(begin)
def random_point(self):
"""
产生随机节点
:return:
"""
point_x = random.uniform(self.safe.x, self.recover.y)
point_y = random.uniform(self.safe.x, self.recover.y)
point = [point_x, point_y]
return point
@staticmethod
def get_nearest_list_index(point_list, rnd):
"""
在节点列表中找到距离目标节点中最近的点
:param point_list: 节点列表 T1 or obstacle_list
:param rnd: 目标节点
:return: 最近节点的位置
"""
d_list = [(point.x - rnd[0]) ** 2 + (point.y - rnd[1]) ** 2 for point in point_list]
min_index = d_list.index(min(d_list))
return min_index
def get_nearest_obstacle_index(self, point):
"""
找到距离point最近的障碍物
:param point: 节点
:return: 最近的障碍物
"""
d_list = [(math.sqrt((point.x - x) ** 2 + (point.y - y) ** 2)) - r for (x, y, r) in self.obstacle_list]
min_index = d_list.index(min(d_list))
return min_index
def collision_check(self, t, new_point, obstacle_list):
"""
检查新的节点是否会碰撞或穿越到障碍物
:param t: 树
:param new_point: 实际新节点
:param obstacle_list: 障碍物列表
:return:
"""
flag = 1
for (ox, oy, radius) in obstacle_list:
# 点到障碍物中心的距离
dx = ox - new_point.x
dy = oy - new_point.y
d = math.sqrt(dx * dx + dy * dy)
# 判断是否穿过障碍物
if t == 1:
parent_point = self.point_list_from_start[new_point.parent]
else:
parent_point = self.point_list_from_goal[new_point.parent]
vector_o = np.array([ox, oy])
vector_p = np.array([parent_point.x, parent_point.y])
vector_n = np.array([new_point.x, new_point.y])
d_p_n = np.abs(np.cross(vector_p - vector_o, vector_n - vector_o)) / np.linalg.norm(vector_p - vector_n)
# 如果点落在或穿过障碍物
if d < radius or d_p_n < radius:
flag = 0
return flag
return flag
def angle_check(self, new_point, parent_point, ancestor_point):
"""
转角约束
:param new_point: 新节点 w
:param parent_point: n节点(新节点的父级节点)
:param ancestor_point: f祖先节点
:return:
"""
vector_p_n = np.array([new_point.x - parent_point.x, new_point.y - parent_point.y])
vector_a_p = np.array([parent_point.x - ancestor_point.x, parent_point.y - ancestor_point.y])
angle = get_angle(vector_a_p, vector_p_n)
if angle self.goalSampleRate:
rnd = self.random_point()
else:
rnd = [self.goal.x, self.goal.y]
# 计算后的实际新节点和动态步长
new_point, actual_step = self.coordinate(1, rnd)
# 限制条件检测
if not self.condition_check(1, new_point):
continue
# 实际新节点通过检测 加入T1
self.point_list_from_start.append(new_point)
"""
搜索树2的实现
"""
# 实际新节点
new_point_two, actual_step = self.coordinate(2, [new_point.x, new_point.y])
# 限制条件检测
if not self.condition_check(2, new_point_two):
continue
# 实际新节点加入 T2
self.point_list_from_goal.append(new_point_two)
"""
判断是否达到目标点
"""
# 判断是否满足相遇条件
dx = new_point.x - new_point_two.x
dy = new_point.y - new_point_two.y
d = math.sqrt(dx * dx + dy * dy)
if self.distance < d < self.step:
if self.perfect_connect(new_point, new_point_two):
break
else:
continue
else:
continue
return self.generate_path_list()
def draw_statistic(self, path):
"""
绘制静态图像
:param path: 规划完成的路径
:return:
"""
plt.clf()
# 绘制区域
# x轴刻度区间
x_major_location = MultipleLocator(10)
# y轴刻度区间
y_major_location = MultipleLocator(10)
# 坐标轴实例
ax = plt.gca()
ax.xaxis.set_major_locator(x_major_location)
ax.yaxis.set_major_locator(y_major_location)
plt.axis([self.min_rand, self.max_rand, self.min_rand, self.max_rand])
# 绘制障碍物
for (ox, oy, radius) in self.obstacle_list:
circle = plt.Circle(xy=(ox, oy), radius=radius, color="r")
ax.add_patch(circle)
# 绘制起点
plt.plot(self.start.x, self.start.y, "^g")
# 绘制终点
plt.plot(self.goal.x, self.goal.y, "^b")
# 绘制规划的路径
plt.plot([data[0] for data in path], [data[1] for data in path], "-y")
for (x, y) in path:
plt.scatter(x, y, marker='o', c='b', edgecolors='b')
ax.set_aspect('equal', adjustable='datalim')
ax.plot()
plt.grid(True)
plt.show()
def get_angle(a, b):
"""
向量夹角计算
:param a:
:param b:
:return:
"""
a_norm = np.sqrt(np.sum(a * a))
b_norm = np.sqrt(np.sum(b * b))
cos_value = float(np.dot(a, b) / (a_norm * b_norm))
eps = 1e-6
if 1.0 < cos_value < 1.0 + eps:
cos_value = 1.0
elif -1.0 - eps < cos_value < -1.0:
cos_value = -1.0
arc_value = np.arccos(cos_value)
angle_value = arc_value * (180 / np.pi)
return angle_value
def get_total_distance(path):
"""
计算路径总长度
:param path:
:return:
"""
total_distance = 0
for index in range(2, len(path)-1):
one = path[index-1]
two = path[index]
total_distance += np.sqrt(np.square(two[0] - one[0]) + np.square(two[1] - one[0]))
print(total_distance)
def main():
print("============================Start planning your path============================")
rand_area = [0, 100] # 区域大小
step = 10 # 基础步长
angle = 60 # 最大转向角
distance = 5 # 最小航行距离
start = [0, 0] # 起点
goal = [100, 100] # 终点
safe = [20, 20]
recover = [90, 90]
# 障碍物
obstacle_list = [
(50, 50, 15),
(62, 13, 12),
(50, 87, 11)
]
bi_rrt = BiRRT(start=start, goal=goal, angle=angle, step=step, distance=distance, obstacle_list=obstacle_list,
rand_area=rand_area, safe=safe, recover=recover)
path, path_1, path_2 = bi_rrt.planning()
bi_rrt.draw_statistic(path)
get_total_distance(path)
print("==========================End of planned path==========================")
if __name__ == '__main__':
main()
4. 效果图
Original: https://www.cnblogs.com/cnpolaris/p/16738756.html
Author: CNPolaris
Title: Python实现改进后的Bi-RRT算法实例
原创文章受到原创版权保护。转载请注明出处:https://www.johngo689.com/712076/
转载文章受原作者版权保护。转载请注明原作者出处!