引言
继上一篇大创-路径规划博客的撰写快一年了，这快一年的时间里，发生了许多事，其中最值得一提的就是，我又回归单身了（没想到吧，不是疫情！），好吧，不值得一提，我不配拥有爱情呢。都怪当初把感情写进了博客导致我现在每次看到博客内容就，就 哎懂得人都懂。
言归正传。这次是真的要写一写路径规划算法了，这篇博客是对我一直久久未更新的大创-路径规划篇进行一个整理总结，本来计划之前更新的，结果私下里改了一些，却没精力更新博客了。这次续更还有一个任务就是要解决现有程序的bug，以及完善它，并把它发表成论文。
本篇就不对代码进行讲解了，后面会分几篇博客分模块讲解代码
project
整个工程目前写成了四个程序，1个主函数加3个模块（可视化算法模块、路径规划算法模块、绘制模块），流程图如下吧：
t_main.py
import t_visual as visual
import t_solutions as solution
import t_paint as paint
start = (310,200)
end = (-300,-360)
class_allpoints = [[(80, 380), (80, 270), (0, 270), (0, 380)],
[(270, 200), (270, -70), (180, -70), (180, 200)],
[(-100, -100), (-100, -350), (-350, -350), (-350, -100)],
[(-20,50),(100,50),(100,-200),(-20,-200)],
[(-150,300),(-10,300),(-10,150),(-150,150)],
[(140,-90),(200,-90),(200,-160),(140,-160)],
[(-200,0),(-90,0),(-90,60),(-200,60)]
]
paint.drawPolygons(class_allpoints)
graph = visual.main(start, end ,class_allpoints)
road = solution.solution_one(start, end, graph)[:-1]
paint.drawRoad(road,width=3)
paint.turtle.mainloop()
t_visual.py
'''
用弧线有问题，应该用纯数学计算 用直线 用直线的两点式求交点坐标来求距离 再比较，这样才不会出bug
'''
import math
import copy
def main(start, end, class_allpoints):
#生成所需数据
allpoints = [i for each in class_allpoints for i in each]
graph = dict()
if start not in allpoints:
allpoints.append(start)
if end not in allpoints:
allpoints.append(end)
for this_point in allpoints:
pop_allpoints = None
#深度拷贝
for each in class_allpoints:
for i in each:
if i == this_point:
pop_allpoints = copy.deepcopy(class_allpoints)
pop_allpoints[class_allpoints.index(each)].remove(i)
break
if pop_allpoints:
break
pop_allpoints = pop_allpoints if pop_allpoints else copy.deepcopy(class_allpoints)
data = generateInterval(this_point, pop_allpoints)
graph[this_point] = judgeVisualPoints(this_point, pop_allpoints, data[0], end)
return graph
#生成点区间
def generateInterval(this_point, pop_allpoints):
angleANDdistance = list()
angleANDpoint = list()
for each in pop_allpoints:
min_y = None
maxTmp = [None,None,None]
minTmp = [None,None,None]
for point in each:
tmp = (math.degrees(math.atan((point[1]-this_point[1])/(point[0]-this_point[0]))) if (point[0]-this_point[0]!=0) else -90 if (point[1]-this_point[1]>0) else 90)
tmp = tmp + 180 if point[0]-this_point[0]>0 else tmp #定义角度
#比较
maxTmp = [point,tmp,math.sqrt((point[1]-this_point[1])**2+(point[0]-this_point[0])**2)] if (maxTmp[1]==None or tmp > maxTmp[1]) else maxTmp
minTmp = [point,tmp,math.sqrt((point[1]-this_point[1])**2+(point[0]-this_point[0])**2)] if (minTmp[1]==None or tmp < minTmp[1]) else minTmp
if maxTmp[1]-minTmp[1] > 180 and (maxTmp[1]!=270 and minTmp!=0 or minTmp!=-90 and maxTmp!=180): #特殊情况处理
maxPoint = (min(point[0] for point in each),min(point[1] for point in each))
minPoint = (max(point[0] for point in each),min(point[1] for point in each))
maxTmp = [maxPoint,
(math.degrees(math.atan((maxPoint[1]-this_point[1])/(maxPoint[0]-this_point[0]))) if (maxPoint[0]-this_point[0]!=0) else -90),
math.sqrt((maxPoint[1]-this_point[1])**2+(maxPoint[0]-this_point[0])**2)]
minTmp = [minPoint,
(math.degrees(math.atan((minPoint[1]-this_point[1])/(minPoint[0]-this_point[0])))+180 if (minPoint[0]-this_point[0]!=0) else 270),
math.sqrt((minPoint[1]-this_point[1])**2+(minPoint[0]-this_point[0])**2)]
min_y = min(point[1] for point in each)
break
if min_y:
angleANDdistance.append([minTmp[1:],[270,min_y-this_point[1]]])
angleANDdistance.append([[-90,min_y-this_point[1]],maxTmp[1:]])
else:
angleANDdistance.append([minTmp[1:],maxTmp[1:]])
angleANDpoint.append([minTmp[:-1],maxTmp[:-1]])
return [angleANDdistance,angleANDpoint]
def judgeVisualPoints(this_point, pop_allpoints, angleANDdistance, end):
visualPoints = list()
sequentialPoints = sorted([i for each in pop_allpoints for i in each]+[end], key=lambda x:(x[0]-this_point[0])**2+(x[1]-this_point[1])**2)
for point in sequentialPoints:
#数据处理
angle = (math.degrees(math.atan((point[1]-this_point[1])/(point[0]-this_point[0]))) if (point[0]-this_point[0]!=0) else -90 if (point[1]-this_point[1]>0) else 90)
angle = angle + 180 if point[0]-this_point[0]>0 else angle
tmp = __IsInIntervals(angle, angleANDdistance)
if tmp:
distance = (angle-tmp[0][0])/(tmp[1][0]-tmp[0][0])*(tmp[1][1]-tmp[0][1])+tmp[0][1]
if point == end and this_point == (270,-70):
print(point,angle,tmp,distance,math.sqrt((point[1]-this_point[1])**2+(point[0]-this_point[0])**2),sep="
")
if math.sqrt((point[1]-this_point[1])**2+(point[0]-this_point[0])**2) < distance:
visualPoints.append(point)
else:
visualPoints.append(point)
return visualPoints
def __IsInIntervals(angle,Raw_intervalsList):
#排序
Raw_intervalsList.sort(key=lambda x: x[0][1] if x[0][1]<x[1][1] else x[1][1])
for each in Raw_intervalsList:
intervalsList = (each[0][0],each[1][0])
if angle>intervalsList[0] and angle<intervalsList[1]:
return each
else:
continue
return False
t_solutions.py
'''
算法二和算法三本身应该也是存在问题的，必须设计一个机制避免其无限递归的发生
'''
import math
def solution_one(start, end, graph):
def __searchPath(start, end, graph):
results = []
__generatePath(graph, [start,0], end, results)
results.sort(key=lambda x:x[-1])
return results
def __generatePath(graph, path, end, results):
current = path[-2]
if current == end:
results.append(path)
#print(path)
else:
for n in graph[current]:
if n not in path and path[-1]<(minDistance*1.5): #递归深度为 3/2起始点终止点间距离
distance = path[-1] + math.sqrt((n[0]-path[-2][0])**2 + (n[1]-path[-2][1])**2) #增量计算不同路径的距离
__generatePath(graph, path[:-1]+[n,distance], end, results)
def showPath(results):
print('The path from ', results[0][0], 'to' , results[0][-1], 'is :')
for path in results:
print(path)
minDistance = math.sqrt((end[1]-start[1])**2 + (end[0]-start[0])**2)
results = __searchPath(start, end, graph)
if __name__ == "__main__":
showPath(results)
return results[0]
def solution_two(start, end, graph):
print(graph)
#待再优化
def __findNext(start,end,graph, path):
minDistance = [None,None]
for each in graph[start]:
if each == end:
path.append(end)
return
else:
distance = math.sqrt((each[0]-start[0])**2+(each[1]-start[1])**2)+math.sqrt((end[0]-each[0])**2+(end[1]-each[1])**2)
if minDistance[1]==None or minDistance[1]>distance:
minDistance = [each,distance]
print(minDistance[0])
path.append(minDistance[0])
__findNext(minDistance[0],end, graph, path)
path = [start]
__findNext(start, end, graph, path)
print(path)
return path
def solution_three(start, end, graph):
def __findNext(start,end,graph, path):
minDistance = [None,None]
for each in graph[start]:
if each in path:
continue
elif each == end:
path.append(end)
return
next_min_distance = None
tmp_list = graph[each][:]
try:
tmp_list.remove(start)
print(start)
print(tmp_list)
except:
pass
for i in tmp_list:
if i == end:
path.extend([each,i])
return
next_distance = math.sqrt((end[0]-i[0])**2+(end[1]-i[1])**2) + math.sqrt((i[0]-each[0])**2+(i[1]-each[1])**2)
if next_min_distance==None or next_min_distance>next_distance:
next_min_distance = next_distance
distance = next_min_distance + math.sqrt((each[0]-start[0])**2+(each[1]-start[1])**2)
if minDistance[1]==None or minDistance[1]>distance:
minDistance = [each,distance]
path.append(minDistance[0])
__findNext(minDistance[0],end, graph, path)
path = [start]
__findNext(start, end, graph, path)
print(path)
return path
def solution_four(start, end, graph):
def __searchPath(start, end, graph):
results = []
__generatePath(graph, [start,0], end, results)
results.sort(key=lambda x:x[-1])
return results
def __generatePath(graph, path, end, results):
current = path[-2]
if current == end:
results.append(path)
#print(path)
else:
for n in graph[current]:
if n not in path and path[-1]<(minDistance*1.5):
distance = path[-1] + math.sqrt((n[0]-path[-2][0])**2 + (n[1]-path[-2][1])**2) #增量计算不同路径的距离
__generatePath(graph, path[:-1]+[n,distance], end, results)
def showPath(results):
print('The path from ', results[0][0], 'to' , results[0][-1], 'is :')
for path in results:
print(path)
prePath = solution_three(start,end,graph)
minDistance = 0.0
for i in range(len(prePath)-1):
minDistance += math.sqrt((prePath[i+1][0]-prePath[i][0])**2+(prePath[i+1][1]-prePath[i][1])**2)
results = __searchPath(start, end, graph)
return results[0]
t_paint.py
import turtle
__turtle = turtle.Turtle()
__turtle.hideturtle()
__turtle.speed(0)
#pointsList: [[point,point,point,...],[point,point,..],...]
def drawPolygons(pointsList,color='black',width=2):
if isinstance(pointsList,list) and isinstance(pointsList[0],list):
__turtle.width(width)
__turtle.color(color)
for each in pointsList:
__draw(each,style="polygon")
else:
print("date error!")
#pointsList: [point,point,point,...]
def drawRoad(pointsList,color="blue",width=1,speed=1):
__turtle.width(width)
__turtle.color(color)
__turtle.speed(speed)
__draw(pointsList,style=None)
#pointsList: [point,point,point,...]
def __draw(pointsList,style):
__turtle.penup()
__turtle.goto(pointsList[-1] if style=="polygon" else pointsList[0])
__turtle.pendown()
for point in pointsList:
__turtle.goto(point)
__turtle.penup()
运行结果
上述障碍物代码执行后的运行截图：
问题
整个程序存在一个很大的，也是最致命的问题，它存在于 visual.py 里面，在注释第一行就已经指出了：“用弧线有问题，应该用纯数学计算 用直线 用直线的两点式求交点坐标来求距离 再比较，这样才不会出bug”，举个例子，当某个障碍物是长方形，要越过该障碍物时：（更改 main.py 中 end 为 (150,150)，start 不变之后）
我们发现，程序出错了，原因就在于我的可视算法出了bug，它直接将end终点计算在了右上角这个障碍物右上点的可视范围内（可直线抵达的范围内）。
红色阴影部分都被视为右上点的可视范围内，而恰好只有 end 终点落在了这个区域内，直接当做可视点（可直线抵达点）传给了solution.py进行了规划。
因此需要将弧线表示可视范围优化，或者直接将弧线改为直线，就不会出现这类问题了。