[大创]一步一步写路径规划并绘图可视化 II

引言

继上一篇大创-路径规划博客的撰写快一年了，这快一年的时间里，发生了许多事，其中最值得一提的就是，我又回归单身了（没想到吧，不是疫情！），好吧，不值得一提，我不配拥有爱情呢。都怪当初把感情写进了博客导致我现在每次看到博客内容就，就 哎懂得人都懂。
言归正传。这次是真的要写一写路径规划算法了，这篇博客是对我一直久久未更新的大创-路径规划篇进行一个整理总结，本来计划之前更新的，结果私下里改了一些，却没精力更新博客了。这次续更还有一个任务就是要解决现有程序的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进行了规划。
因此需要将弧线表示可视范围优化，或者直接将弧线改为直线，就不会出现这类问题了。