无网格A*路径查找算法。成本函数问题

Question

我一直试图解决的问题是，对于一辆有障碍物的dubins汽车（无向后运动，匀速），从给定位置到给定目标的路径查找。我尝试用一些简单的避障方法实现一个无网格a*算法。我希望生成的路径直接朝着目标前进，并做一些小的调整以绕过它发现的障碍。然而，一旦在地图上引入障碍物，路径似乎就会卡在算法代价函数的局部最小点上

我实施的成本函数如下所示：

f（x）=c（x）+g（x）

其中c（x）是总旅行成本，即从节点i-1移动到i的累积成本。 此外，g（x）是从当前节点到最终目标的最佳路径的成本，由于忽略了障碍物，该路径成为一条直线

成本在最小堆中用作优先级值，其中每次迭代都会弹出最小节点并生成子节点。在生成子对象时，可以控制这些子对象是否在边界之外、是否尚未访问过以及是否在障碍物内。如果这些控件返回false，则将子控件添加到堆中

我尝试在路径成本中引入加权因子k*g（x），希望这能“激励”算法朝着目标前进，而不是停留在某一点上。然而，这只是将最小点转移到另一个位置，但仍然会导致卡住

我将在下面介绍A*算法的代码实现：

# Description: Pathfinding algorithm, iteratively generates new neighbouring
# nodes and selects the cheapest of these through utilizing a min heap. 
# In: Car class object, a node as starting point.
# Out: The finishing node, with attached parent pointers.
def Astar(car, current):
    minHeap = [] #initialize heap as list
    h.heappush(minHeap, current) #push initial node onto heap
    heapCount = 1 #add upon pushes to heap, subtract upon pop
    # Iterate through nodes in priority queue
    while not ((goal(car, current)) or heapCount == 0):
        current = h.heappop(minHeap)
        heapCount -= 1
        for phi in [-m.pi/4, 0, m.pi/4]: #Full turns or straight are optimal, according to Pontryagins maximum principle
            #calculate new values for each phi (steering angle)
            xn, yn, thetan = step(car, current.x, current.y, current.theta, phi)
            #control feasibility of position
            if validCheck(car, xn, yn, current):
                #calculate costs for these directives
                costC = current.travelled + m.hypot(current.x - xn, current.y - yn) #cost of travel from start position
                costG = m.hypot(car.xt - xn, car.yt - yn) #current optimal distance to goal
                totalCost = costC + costG
                #renew time stamp
                newTime = current.time + 0.01
                #create child from new data
                child = Node(xn, yn, thetan, phi, totalCost, costC, newTime, current)
                #push child onto heap
                h.heappush(minHeap, child)
                heapCount += 1
        
    return current

请注意，car是一个包含某些属性的类：

• x0：浮动：初始x位置[m]
• y0：浮动：初始y位置[m]
• xt：浮动：目标x位置[m]
• yt：浮动：目标y位置[m]
• xlb：浮动：最小x位置[m]
• xub：浮动：最大x位置[m]
• ylb：浮动：最小y位置[m]
• yub：浮动：最大y位置[m]
• obs:list：每个障碍obs的元组列表[i]

它还包括一种方法步骤，当给定转向角和先前的航向和位置时，该方法可以生成新的航向角和位置

任何关于这个问题的建议或帮助，为什么会发生，以及我能做些什么来改进路径查找，都将不胜感激

Answer 1

我还没有一个解决方案，但有一个解释发生了什么，也许还有一个提示，你可以做什么

分析

A*算法是用于图搜索的，如果给定一个合适的代价函数，则与BFS等未提供信息的策略相比，可以大大减少搜索空间。但问题图的大小仍然很重要

在您的代码中，我看到时间增量为0.01，我认为这暗示您正在从父节点到子节点执行非常小的步骤。这当然是有道理的，最接近于一个平滑的，非量化的运动。但同时，它也导致了问题图的巨大增长

没有障碍，A*仍然可以优雅地处理这个巨大的图形。它将推迟与直线的所有偏差，因为它们的成本将高于直线上的节点。堆将增长（有一些调试输出显示它的大小…），但绝大多数节点将永远不会被进一步探索

有了障碍，游戏发生了翻天覆地的变化。比如说，有一个障碍物，因此得到的最佳路径比直线长1.00个单位。然后A*将探索所有无意义路径，从起点到障碍物的线路上的某个位置开始，任意向左或向右转弯，直到这些路径达到额外的1.00长度。将会有很多这些无用的路径，而A*会被困在探索胡说八道中

暗示

我会让A*在更高层次上运作

我猜你的障碍是多边形。因此，生成的总路径要么忽略障碍物，要么在其中一个拐角处触及障碍物。接触点之间的元素将从具有某个航向的接触点开始，由初始全转向部分、直线部分和最终全转向部分组成，然后到达具有某个（不同）航向的下一个接触点（老实说，我不能绝对肯定这种转弯-直转弯模式是否真的能涵盖所有可能的情况）。给定此类元素的起点和终点以及所需的终点，您可以使用一些几何图形计算零件，顺便检查碰撞

当通过某个接触点时，你无法预先知道最佳航向，因此你必须检查所有可能的航向

我将把这些元素作为A*探索的步骤

这就是我将A*应用于您的任务的方式：

• 要计算节点的子节点，对于所有其他多边形角点和该角点处的所有标题，计算从父角点到另一角点的元素，从而得到给定的标题。检查此类元素在几何上是否可行，并且是否与某个障碍物碰撞

• 作为成本函数，累积到目前为止行驶的长度，然后添加到目标的最短障碍物忽略路径。这可以是笔直的毕达哥拉斯距离，也可以是更精细的距离，考虑到从当前航向到面对目标的必要初始转弯