python - 当两点之间存在视线时，如何计算父节点和邻居节点之间的距离？

Question

我已经用 Python 完成了我的 Astar 算法，现在我需要将它转换为 Theta star 算法，我在下面构建了我的视线算法，但是当我使用 Theta star 算法时，我遇到了一些问题通过计算距离的视线，我怎样才能让它跳过有视线的点。运行我的代码后，我发现没有任何效果，我发现它正在作为 Astar 算法运行。有什么帮助吗？

我有问题的片段:

sight = lineOfsight(grid, y, x, y2, x2)
if sight == True:

        g2 = g + delta[i][2] + math.sqrt((x2 - x)**2 + (y2 - y)**2)

         h2 = math.sqrt((x2 - goal[0])**2 + (y2 - goal[1])**2)

         f2 = g2 + h2
  else:

         g2 = g + delta[i][2]             
         h2 = math.sqrt((x2 - goal[0])**2 + (y2 - goal[1])**2)

         f2 = g2 + h2

 open.append([f2,g2,h2,x2,y2])

我的视线代码:

def lineOfsight(grid, y1, x1, y2, x2):
    y_size = len(grid)
    x_size = len(grid)

    #Distance
    dy=y2-y1
    dx=x2-x1

    if dy < 0:
        dy = -dy
        sy = -1
    else:
        sy = 1

    if dx < 0:
        dx = -dx
        sx = -1
    else:
        sx = 1

    f = 0

    if dx >= dy:
        while x1 != x2:
            f = f + dy
            if f >= dx and 0 < y1+(sy-1)/2 and y1+(sy-1)/2 < y_size and 0 < x1+(sx-1)/2 and x1+(sx-1)/2 < x_size:
                if grid[x1+int((sx-1)/2)][y1+int((sy-1)/2)]:

                    return False
                y1 = y1 + sy
                f  = f  - dx

            elif 0 < y1+(sy-1)/2 and y1+(sy-1)/2 < y_size and 0 < x1+(sx-1)/2 and x1+(sx-1)/2 < x_size:
                if f != 0 and grid[x1+(sx-1)/2][y1+(sy-1)/2]:

                    return False

            elif 1<y1 and y1<y_size and 0 < x1+(sx-1)/2 and x1+(sx-1)/2 < x_size:
                if dy==0 and grid[x1+int((sx-1)/2)][y1] and grid[x1+int((sx-1)/2)][y1-1] :

                    return False
            x1 = x1 + sx

    else:

        while y1 != y2:
            f = f + dx
            if f >= dy and 0 < y1+(sy-1)/2 and y1+(sy-1)/2 < y_size and 0< x1+(sx-1)/2 and x1+(sx-1)/2 < x_size:
                if grid[x1+int((sx-1)/2)][y1+int((sy-1)/2)]:

                    return False
                x1 = x1 + sx
                f = f - dy
            elif 0 < y1+(sy-1)/2 and y1+(sy-1)/2 < y_size and 0 < x1+(sx-1)/2 and x1+(sx-1)/2 < x_size:
                if f !=0 and grid[x1+int((sx-1)/2)][y1+int((sy-1)/2)]:

                    return False

            elif 0 < y1+(sy-1)/2 and y1+(sy-1)/2 < y_size and 1 < x1 and x1 < x_size:       
                if dx == 0 and grid[x1][y1+ int((sy-1)/2)] and grid[x1-1][y1+int((sy-1)/2)]:

                    return False

            y1=y1+sy

    return True

我的西塔星码:

import matplotlib.pyplot as plt
from lineofsightss import *
#grid format
# 0 = navigable space
# 1 = occupied space

import random
import math

grid = [[0, 1, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0],
        [0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0],
        [0, 1, 1, 0, 1, 0, 0, 0, 0, 0, 0, 0],
        [0, 1, 0, 1, 0, 0, 0, 1, 0, 0, 0, 0],
        [0, 0, 0, 1, 1, 0, 0, 1, 0, 0, 0, 0],
        [0, 0, 0, 1, 1, 0, 0, 1, 0, 0, 0, 0],
        [0, 0, 0, 1, 1, 0, 0, 1, 0, 0, 0, 0],
        [0, 0, 0, 1, 1, 0, 0, 1, 0, 0, 0, 0],
        [0, 0, 0, 1, 1, 0, 0, 0, 0, 0, 0, 0],
        [0, 0, 0, 1, 1, 0, 0, 0, 0, 0, 0, 0]]



init = [0,0]                            #Start location is (5,5) which we put it in open list.
goal = [len(grid)-1,len(grid[0])-1]     

heuristic = [[0 for row in range(len(grid[0]))] for col in range(len(grid))]
for i in range(len(grid)):    
    for j in range(len(grid[0])):            
        heuristic[i][j] = abs(i - goal[0]) + abs(j - goal[1])

plt.plot(0,10)
plt.plot(0,-len(grid)-10)
plt.grid(True)
plt.axis("equal")

plt.plot([-1, len(grid[0])],[[-x/2 for x in range(-1,len(grid)*2+1)], [-y/2 for y in range(-1,len(grid)*2+1)]], ".k")
plt.plot([[x/2 for x in range(-2,len(grid[0])*2+1)],[x/2 for x in range(-2,len(grid[-1])*2+1)]],[1, -len(grid)],".k")

plt.plot(init[1],-init[0],"og")
plt.plot(goal[1],-goal[0],"ob")


#Below the four potential actions to the single field


delta =      [[1, 0, 1],
              [0, 1, 1],
              [-1, 0, 1],
              [0, -1, 1],
              [-1, -1, math.sqrt(2)],
              [-1, 1, math.sqrt(2)],
              [1, -1, math.sqrt(2)],
              [1, 1, math.sqrt(2)]]



delta_name = ['V','>','<','^','//','\\','\\','//']


def search():

    pltx,plty=[],[]
    #open list elements are of the type [g,x,y]

    closed = [[0 for row in range(len(grid[0]))] for col in range(len(grid))]
    action = [[-1 for row in range(len(grid[0]))] for col in range(len(grid))]
    #We initialize the starting location as checked
    closed[init[0]][init[1]] = 1
    expand=[[-1 for row in range(len(grid[0]))] for col in range(len(grid))]

    # we assigned the cordinates and g value
    x = init[0]
    y = init[1]
    g = 0

    h = math.sqrt((x - goal[0])**2 + (y - goal[1])**2)

    f = g + h

    #our open list will contain our initial value
    open = [[f, g, h, x, y]]


    found  = False   #flag that is set when search complete
    resign = False   #Flag set if we can't find expand
    count = 0

    #print('initial open list:')
    #for i in range(len(open)):
            #print('  ', open[i])
    #print('----')

    while found is False and resign is False:

        #Check if we still have elements in the open list
        if len(open) == 0:    #If our open list is empty, there is nothing to expand.
            resign = True
            print('Fail')
            print('############# Search terminated without success')
            print()
        else:
            #if there is still elements on our list
            #remove node from list
            open.sort()             #sort elements in an increasing order from the smallest g value up
            open.reverse()          #reverse the list
            next = open.pop()       #remove the element with the smallest g value from the list
            #print('list item')
            #print('next')

            #Then we assign the three values to x,y and g. Which is our expantion.
            x = next[3]
            y = next[4]
            g = next[1]
            #elvation[x][y] = np.random.randint(100, size=(5,6))
            expand[x][y] = count
            count+=1

            #Check if we are done
            if x == goal[0] and y == goal[1]:
                found = True
                print(next) #The three elements above this "if".
                print('############## Search is success')
                print()

            else:
                #expand winning element and add to new open list
                for i in range(len(delta)):       #going through all our actions the four actions
                    #We apply the actions to x and y with additional delta to construct x2 and y2
                    x2 = x + delta[i][0]
                    y2 = y + delta[i][1]


                    #if x2 and y2 falls into the grid
                    if x2 >= 0 and x2 < len(grid) and y2 >=0 and y2 <= len(grid[0])-1:

                        #if x2 and y2 not checked yet and there is not obstacles
                        if closed[x2][y2] == 0 and grid[x2][y2]==0:

                            sight = lineOfsight(grid, y, x, y2, x2)
                            if sight == True:

                                g2 = g + delta[i][2] + math.sqrt((x2 - x)**2 + (y2 - y)**2)

                                h2 = math.sqrt((x2 - goal[0])**2 + (y2 - goal[1])**2)

                                f2 = g2 + h2
                            else:

                                g2 = g + delta[i][2]             
                                h2 = math.sqrt((x2 - goal[0])**2 + (y2 - goal[1])**2)

                                f2 = g2 + h2

                            open.append([f2,g2,h2,x2,y2])
                            #we add them to our open list
                            pltx.append(y2)
                            plty.append(-x2)
                            #print('append list item')
                            #print([g2,x2,y2])
                            #Then we check them to never expand again
                            closed[x2][y2] = 1
                            action[x2][y2] = i

    for i in range(len(expand)):
        print(expand[i])
    print()
    policy=[[' ' for row in range(len(grid[0]))] for col in range(len(grid))]
    x=goal[0]
    y=goal[1]
    policy[x][y]='*'
    visx = [y]
    visy = [-x]
    while x !=init[0] or y !=init[1]:
        x2=x-delta[action[x][y]][0]
        y2=y-delta[action[x][y]][1]
        policy[x2][y2]= delta_name[action[x][y]]
        x=x2
        y=y2
        visx.append(y)
        visy.append(-x)
    for i in range(len(policy)):
        print(policy[i])
    print()





    plt.plot(visx,visy, "-r")

    plt.show()

search()

下面是我的输出:

Answer 1

在Theta*中，当相邻节点看到父节点时，您应该尝试将该相邻节点直接连接到当前节点的父节点。这是导致任意角度、非网格对齐路径的过程。

解决方案实际上缺少节点与任意父节点(不一定是网格中的邻居)的这种关联(因此在完成搜索时不会正确重建路径)。

---

这涉及对问题代码的一些更改:

• 路径重构应该以不同的方式实现，包含八个运动方向之一的“action”数组是不够的。一种可行的替代方案是它包含父节点的 (x, y) 坐标，即 action[x][y] = (parent_x, parent_y)。

• 在 if sight == True 的代码中，你应该计算邻居的 g-score 作为使用从 parent 到邻居的直线的路径(图中的虚线多于)。此时，g-score 的计算考虑了当前节点，这是没有必要的。

---

下面是对已发布代码的修改，其中包含其中一些更改。其他问题可能仍然存在，但这是朝着正确方向迈出的一步。

def search():
    pltx,plty=[],[]

    closed = [[0 for row in range(len(grid[0]))] for col in range(len(grid))]
    action = [[(-1, -1) for row in range(len(grid[0]))] for col in range(len(grid))]
    closed[init[0]][init[1]] = 1
    expand = [[-1 for row in range(len(grid[0]))] for col in range(len(grid))]

    # we assigned the coordinates and g value
    x = init[0]
    y = init[1]
    g = 0

    h = math.sqrt((x - goal[0])**2 + (y - goal[1])**2)

    f = g + h

    open = [[f, g, h, x, y]]

    found = False    # flag that is set when search complete
    resign = False   # flag set if we can't find expand
    count = 0

    while found is False and resign is False:
        # check if we still have elements in the open list
        if len(open) == 0:    # if our open list is empty, there is nothing to expand.
            resign = True
            print('Fail')
            print('############# Search terminated without success')
            print()
        else:
            # if there is still elements on our list
            # remove node from list
            open.sort()             # sort elements in an increasing order from the smallest g value up
            open.reverse()          # reverse the list
            next = open.pop()       # remove the element with the smallest g value from the list

            # then we assign the three values to x,y and g. Which is our expantion.
            x = next[3]
            y = next[4]
            g = next[1]
            # elvation[x][y] = np.random.randint(100, size=(5,6))
            expand[x][y] = count
            count += 1

            # check if we are done
            if x == goal[0] and y == goal[1]:
                found = True
                print(next)     # the three elements above this "if".
                print('############## Search is success')
                print()

            else:
                # expand winning element and add to new open list
                for i in range(len(delta)):       # going through all our actions the four actions
                    # we apply the actions to x and y with additional delta to construct x2 and y2
                    x2 = x + delta[i][0]
                    y2 = y + delta[i][1]

                    # if x2 and y2 falls into the grid
                    if 0 <= x2 < len(grid) and 0 <= y2 <= len(grid[0]) - 1:
                        #if x2 and y2 not checked yet and there is not obstacles
                        if closed[x2][y2] == 0 and grid[x2][y2] == 0:
                            sight = lineOfsight(grid, y, x, y2, x2)

                            parent_x, parent_y = action[x][y]
                            if sight and parent_x >= 0:
                                g2 = g + math.sqrt((x2 - parent_x)**2 + (y2 - parent_y)**2)
                                h2 = math.sqrt((x2 - goal[0])**2 + (y2 - goal[1])**2)
                                f2 = g2 + h2
                                action[x2][y2] = (parent_x, parent_y)
                            else:
                                g2 = g + delta[i][2]
                                h2 = math.sqrt((x2 - goal[0])**2 + (y2 - goal[1])**2)
                                f2 = g2 + h2
                                action[x2][y2] = (x, y)

                            open.append([f2,g2,h2,x2,y2])
                            # we add them to our open list
                            pltx.append(y2)
                            plty.append(-x2)
                            closed[x2][y2] = 1

    for i in range(len(expand)):
        print(expand[i])
    print()
    policy=[[' ' for row in range(len(grid[0]))] for col in range(len(grid))]
    x=goal[0]
    y=goal[1]
    visx = [y]
    visy = [-x]
    while x !=init[0] or y !=init[1]:
        x2=action[x][y][0]
        y2=action[x][y][1]
        x=x2
        y=y2
        visx.append(y)
        visy.append(-x)
    print()

    plt.plot(visx,visy, "-r")

    plt.show()

这会产生以下路径: