我怎样才能把星型算法的公式修改成我下面需要的？

Question

我正在研究星型算法。我在试着建立一个无人机的轨道依靠一颗恒星。我已经实现了下面的代码。我需要考虑障碍物的高度并修改公式：

F= G+H到{}

E：表示障碍物的高度。我们看到无人机在地图上以特定的高度飞行，如果障碍物很高（意味着它的风险非常高），障碍物和无人机之间的距离太近，所以无人机会选择飞越较短的障碍物。如果无人机绕着障碍物转的高度比它高。在

我添加了一个带有随机高度生成和无人机高度的海拔地图，但它不适用于我。我能得到一些帮助吗？。在

A-star Python代码：

import numpy

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

heuristic = [[9, 8, 7, 6, 5, 4],
             [8, 7, 6, 5, 4, 3],
             [7, 6, 5, 4, 3, 2],
             [6, 5, 4, 3, 2, 1],
             [5, 4, 3, 2, 1, 0]]

init = [0,0]                           
goal = [len(grid)-1,len(grid[0])-1]     

delta = [[-1 , 0],   #up 
         [ 0 ,-1],   #left
         [ 1 , 0],   #down
         [ 0 , 1]]   #right

delta_name = ['^','<','V','>']  #The name of above actions

cost = 1   #Each step costs you one

drone_height = 60

def search():
    #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))]


    elvation = numpy.random.randint(0, 100+1, size=(5, 6))
    print(elvation)
    # we assigned the cordinates and g value
    x = init[0]
    y = init[1]
    g = 0
    h = heuristic[x][y]
    e = elvation[x][y]
    f = g + h + e

    #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()             
            open.reverse()          #reverse the list
            next = open.pop()       
            #print('list item')
            #print('next')


            x = next[3]
            y = next[4]
            g = next[1]

            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)):      
                    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 and e < drone_height:
                            g2 = g + cost             #we increment the cose
                            h2 = heuristic[x2][y2]
                            e2 = elvation[x2][y2]
                            f2 = g2 + h2 + e2

                            open.append([f2,g2,h2,x2,y2])   #we add them to our open list
                            #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]='*'
    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
    for i in range(len(policy)):
        print(policy[i])




search()