路径规划算法的定义是:移动机器人在具有障碍物的环境中按照一定的评价标准,寻找一条从起始状态到目标状态的无碰路径。路径规划算法的其中一种分类方法是分为全局路径规划和局部路径规划。
全局路径规划是根据环境全局的信息,这包括机器人在当前状态下探测不到的信息。全局规划将环境信息存储在一张图中,利用这张图找到可行的路径。全局算法往往需要耗费大量的计算时间,不适于快速变化的动态环境,同时由于全局路径规划需要事先获得全局环境信息,也不适于未知环境下的规划任务。
import numpy as np
from random import random
import matplotlib.pyplot as plt
from matplotlib import collections as mc
from collections import deque
import math
import time
class Line():
''' Define line '''
def __init__(self, p0, p1):
self.p = np.array(p0)
self.dirn = np.array(p1) - np.array(p0)
self.dist = np.linalg.norm(self.dirn)
self.dirn /= self.dist # normalize
def path(self, t):
return self.p + t * self.dirn
def Intersection(line, center, radius):
''' Check line-sphere (circle) intersection '''
radius=radius+0.1
a = np.dot(line.dirn, line.dirn)
b = 2 * np.dot(line.dirn, line.p - center)
c = np.dot(line.p - center, line.p - center) - radius * radius
discriminant = b * b - 4 * a * c
if discriminant < 0:
return False
t1 = (-b + np.sqrt(discriminant)) / (2 * a)
t2 = (-b - np.sqrt(discriminant)) / (2 * a)
if (t1 < 0 and t2 < 0) or (t1 > line.dist and t2 > line.dist):
return False
return True
def distance(x, y):
return np.linalg.norm(np.array(x) - np.array(y))
def isInObstacle(vex, obstacles, radius):
for obs in obstacles:
if distance(obs, vex) < radius+0.01:
return True
return False
def isThruObstacle(line, obstacles, radius):
for obs in obstacles:
if Intersection(line, obs, radius):
return True
return False
def nearest(G, vex, obstacles, radius):
Nvex = None
Nidx = None
minDist = float("inf")
for idx, v in enumerate(G.vertices):
line = Line(v, vex)
if isThruObstacle(line, obstacles, radius):
continue
dist = distance(v, vex)
if dist < minDist:
minDist = dist
Nidx = idx
Nvex = v
return Nvex, Nidx
def newVertex(randvex, nearvex, stepSize):
dirn = np.array(randvex) - np.array(nearvex)
length = np.linalg.norm(dirn)
dirn = (dirn / length) * min (stepSize, length)
newvex = (nearvex[0]+dirn[0], nearvex[1]+dirn[1])
return newvex
def window(startpos, endpos):
''' Define seach window - 2 times of start to end rectangle'''
width = endpos[0] - startpos[0]
height = endpos[1] - startpos[1]
winx = startpos[0] - (width / 2.)
winy = startpos[1] - (height / 2.)
return winx, winy, width, height
def isInWindow(pos, winx, winy, width, height):
''' Restrict new vertex insides search window'''
if winx < pos[0] < winx+width and \
winy < pos[1] < winy+height:
return True
else:
return False
class Graph:
''' Define graph '''
def __init__(self, startpos, endpos):
self.startpos = startpos
self.endpos = endpos
self.vertices = [startpos]
self.edges = []
self.success = False
self.vex2idx = {startpos:0}
self.neighbors = {0:[]}
self.distances = {0:0.}
self.parent={0:-1}
self.sx = endpos[0] - startpos[0]
self.sy = endpos[1] - startpos[1]
def add_vex(self, pos):
try:
idx = self.vex2idx[pos]
self.parent[idx]=-1
except:
idx = len(self.vertices)
self.vertices.append(pos)
self.vex2idx[pos] = idx
self.neighbors[idx] = []
self.parent[idx]=-1
return idx
def add_edge(self, idx1, idx2, cost):
self.edges.append((idx1, idx2))
self.neighbors[idx1].append((idx2, cost))
self.neighbors[idx2].append((idx1, cost))
self.parent[idx1]=idx2
def randomPosition(self):
rx = random()
ry = random()
posx = self.startpos[0] - (self.sx / 2.) + rx * self.sx * 2
posy = self.startpos[1] - (self.sy / 2.) + ry * self.sy * 2
return posx, posy
def Parent(G,vex):
vexidx=G.vex2idx[vex]
parentidx=G.parent[vexidx]
if parentidx==-1:
return startpos
else:
return G.vertices[parentidx]
def FindReachest(G,nearvex,newvex):
reachest=nearvex
while reachest!=startpos:
parent=Parent(G,reachest)
line=Line(newvex,parent)
if not isThruObstacle(line,obstacles,radius):
reachest=parent
else:
return reachest
return reachest
def CreateNode(G,reachestvex,newvex,dichotomy):
allow=reachestvex
if reachestvex!=startpos:
forbid=Parent(G,reachestvex)
while distance(allow,forbid)>dichotomy:
mid=((allow[0]+forbid[0])/2,(allow[1]+forbid[1])/2) # 일단 이 부분은 다시 수정
line=Line(newvex,mid)
if not isInObstacle(mid,obstacles,radius) and not isThruObstacle(line,obstacles,radius):
allow=mid
else:
forbid=mid
forbid=newvex
while distance(allow,forbid)>dichotomy:
mid=((allow[0]+forbid[0])/2,(allow[1]+forbid[1])/2) #일단 이 부분은 다시 수정
parent=Parent(G,reachestvex)
line=Line(parent,mid)
if not isInObstacle(mid,obstacles,radius) and not isThruObstacle(line,obstacles,radius):
allow=mid
else:
forbid=mid
if allow!=reachestvex:
create=allow
else:
create=None
return create
def farfrom(vex1,vex2):
return math.sqrt(((vex1[0]-vex2[0])**2+(vex1[1]-vex2[1])**2))
def RRT(startpos, endpos, obstacles, n_iter, radius, stepSize):
''' RRT algorithm '''
G = Graph(startpos, endpos)
for _ in range(n_iter):
randvex = G.randomPosition()
if isInObstacle(randvex, obstacles, radius):
continue
nearvex, nearidx = nearest(G, randvex, obstacles, radius)
if nearvex is None:
continue
newvex = newVertex(randvex, nearvex, stepSize)
newidx = G.add_vex(newvex)
dist = distance(newvex, nearvex)
G.add_edge(newidx, nearidx, dist)
dist = distance(newvex, G.endpos)
if dist < 2 * radius:
endidx = G.add_vex(G.endpos)
G.add_edge(newidx, endidx, dist)
G.success = True
#print('success')
# break
return G
def RRT_star(startpos, endpos, obstacles, n_iter, radius, stepSize,dichotomy):
''' RRT star algorithm '''
G = Graph(startpos, endpos)
for i in range(n_iter):
# plot(G, obstacles, radius)
randvex = G.randomPosition()
if isInObstacle(randvex, obstacles, radius):
continue
nearvex, nearidx = nearest(G, randvex, obstacles, radius)
if nearvex is None:
continue
newvex = newVertex(randvex, nearvex, stepSize)
if isInObstacle(newvex,obstacles,radius):
continue
reachestvex=FindReachest(G,nearvex,newvex)
# print('reachestvex:',reachestvex)
createvex=CreateNode(G,reachestvex,newvex,dichotomy)
# print('createvex',createvex)
if createvex is not None:
newidx = G.add_vex(newvex)
createvexidx=G.add_vex(createvex)
reachestParent=Parent(G,reachestvex)
reachestParentidx=G.vex2idx[reachestParent]
dist = distance(reachestParent,createvex)
G.add_edge(createvexidx,reachestParentidx,dist)
G.distances[createvexidx]=G.distances[reachestParentidx]+dist
dist = distance(newvex,createvex)
G.add_edge(newidx,createvexidx,dist)
G.distances[newidx]=G.distances[createvexidx]+dist
else:
line = Line(nearvex,newvex)
if isThruObstacle(line, obstacles, radius):
continue
newidx = G.add_vex(newvex)
dist = distance(newvex, reachestvex)
reachestidx=G.vex2idx[reachestvex]
G.add_edge(newidx, reachestidx, dist)
G.distances[newidx] = G.distances[reachestidx] + dist
# update nearby vertices distance (if shorter)
for vex in G.vertices:
if vex == newvex:
continue
dist = distance(vex, newvex)
if dist > radius:
continue
line = Line(vex, newvex)
if isThruObstacle(line, obstacles, radius):
continue
idx = G.vex2idx[vex]
if G.distances[newidx] + dist < G.distances[idx]:
G.add_edge(idx, newidx, dist)
G.distances[idx] = G.distances[newidx] + dist
dist = distance(newvex, G.endpos)
if dist < 2 * radius:
endidx = G.add_vex(G.endpos)
G.add_edge(newidx, endidx, dist)
try:
G.distances[endidx] = min(G.distances[endidx], G.distances[newidx]+dist)
except:
G.distances[endidx] = G.distances[newidx]+dist
G.success = True
# print('success')
break
return G
def dijkstra(G):
'''
Dijkstra algorithm for finding shortest path from start position to end.
'''
srcIdx = G.vex2idx[G.startpos]
dstIdx = G.vex2idx[G.endpos]
# build dijkstra
nodes = list(G.neighbors.keys())
dist = {node: float('inf') for node in nodes}
prev = {node: None for node in nodes}
dist[srcIdx] = 0
while nodes:
curNode = min(nodes, key=lambda node: dist[node])
nodes.remove(curNode)
if dist[curNode] == float('inf'):
break
for neighbor, cost in G.neighbors[curNode]:
newCost = dist[curNode] + cost
if newCost < dist[neighbor]:
dist[neighbor] = newCost
prev[neighbor] = curNode
# retrieve path
path = deque()
curNode = dstIdx
while prev[curNode] is not None:
path.appendleft(G.vertices[curNode])
curNode = prev[curNode]
path.appendleft(G.vertices[curNode])
return list(path)
def plot(G, obstacles, radius, path=None):
'''
Plot RRT, obstacles and shortest path
'''
px = [x for x, y in G.vertices]
py = [y for x, y in G.vertices]
fig, ax = plt.subplots()
for obs in obstacles:
circle = plt.Circle(obs, radius, color='red')
ax.add_artist(circle)
ax.scatter(px, py, c='cyan')
ax.scatter(G.startpos[0], G.startpos[1], c='black')
ax.scatter(G.endpos[0], G.endpos[1], c='black')
lines = [(G.vertices[edge[0]], G.vertices[edge[1]]) for edge in G.edges]
lc = mc.LineCollection(lines, colors='green', linewidths=2)
ax.add_collection(lc)
if path is not None:
paths = [(path[i], path[i+1]) for i in range(len(path)-1)]
lc2 = mc.LineCollection(paths, colors='blue', linewidths=3)
ax.add_collection(lc2)
ax.autoscale()
ax.margins(0.1)
plt.show()
def pathSearch(startpos, endpos, obstacles, n_iter, radius, stepSize,dichotomy):
G = RRT_star(startpos, endpos, obstacles, n_iter, radius, stepSize,dichotomy)
if G.success:
path = dijkstra(G)
# plot(G, obstacles, radius, path)
return path
if __name__ == '__main__':
startpos = (0., 0.)
endpos = (48., 28.)
obstacles = [(1., 1.), (2., 2.)]
n_iter = 200
radius = 0.5
stepSize = 0.7
dichotomy= 0.1
S_time=[]
S_dist=[]
for _ in range(100):
start = time.time()
G = RRT_star(startpos, endpos, obstacles, n_iter, radius, stepSize,dichotomy)
# G = RRT(startpos, endpos, obstacles, n_iter, radius, stepSize)
if G.success:
path = dijkstra(G)
print(path)
plot(G, obstacles, radius, path)
end = time.time()
S_time.append(end-start)
# print(f"{end - start:.5f} sec")
dist=0
prevx=startpos[0]
prevy=startpos[1]
for x,y in path:
x_diff=prevx-x
y_diff=prevy-y
dist+= math.sqrt(x_diff**2+y_diff**2)
S_dist.append(dist)
else:
pass
print('yo')
plot(G, obstacles, radius)
print('Cost Mean=',np.mean(S_dist))
print('Cost std=',np.std(S_dist))
print('Cost Min=', np.min(S_dist))
print('Cost Max=', np.max(S_dist))
print('Time Mean=',np.mean(S_time))
print('Time std=',np.std(S_time))
print('Time Min=', np.min(S_time))
print('Time Max=', np.max(S_time))
# Cost Mean= 19.95158009760057
# Cost std= 2.2203056859587984
# Cost Min= 16.286848376528287
# Cost Max= 26.856049490040363
# Time Mean= 0.12551307439804077
# Time std= 0.1737030710498345
# Time Min= 0.019977331161499023
# Time Max= 0.965425968170166