通过有序圆形航路点的最短路径答案

【问题标题】：Shortest path through ordered circular waypoints通过有序圆形航路点的最短路径
【发布时间】：2019-10-09 08:38:57
【问题描述】：

我正在尝试实现一种算法，该算法通过二维平面中的路点有序列表计算最短路径及其从当前位置到目标的相关距离。航路点由其中心坐标 (x, y) 和半径 r 定义。最短路径必须与每个航路点圆周相交至少一次。这与其他路径优化问题不同，因为我已经知道必须通过路径点的顺序。

在simple case 中，连续的航路点是不同的且未对齐，这可以使用连续的角平分来解决。棘手的情况是：

这是我的 Python 实现的精简版本，它不处理对齐的路点，并且处理糟糕同心的连续路点。我对其进行了调整，因为它通常使用纬度和经度，而不是欧几里得空间中的点。

def optimize(position, waypoints):
    # current position is on the shortest path, cumulative distance starts at zero
    shortest_path = [position.center]
    optimized_distance = 0

    # if only one waypoint left, go in a straight line
    if len(waypoints) == 1:
        shortest_path.append(waypoints[-1].center)
        optimized_distance += distance(position.center, waypoints[-1].center)

    else:
        # consider the last optimized point (one) and the next two waypoints (two, three)
        for two, three in zip(waypoints[:], waypoints[1:]):
            one = fast_waypoints[-1]

            in_heading = get_heading(two.center, one.center)
            in_distance = distance(one.center, two.center)
            out_distance = distance(two.center, three.center)

            # two next waypoints are concentric
            if out_distance == 0:
                next_target, nb_concentric = find_next_not_concentric(two, waypoints)
                out_heading = get_heading(two.center, next_target.center)
                angle = out_heading - in_heading
                leg_distance = two.radius
                leg_heading = in_heading + (0.5/nb_concentric) * angle
            else:
                out_heading = get_heading(two.center, three.center)
                angle = out_heading - in_heading
                leg_heading = in_heading + 0.5 * angle
                leg_distance = (2 * in_distance * out_distance * math.cos(math.radians(angle * 0.5))) / (in_distance + out_distance)


            best_leg_distance = min(leg_distance, two.radius)
            next_best = get_offset(two.center, leg_heading, min_leg_distance)
            shortest_path.append(next_best.center)
            optimized_distance += distance(one.center, next_best.center)

    return optimized_distance, shortest_path

我可以看到如何测试不同的极端情况，但我认为这种方法不好，因为可能还有其他我没有想到的极端情况。另一种方法是离散化航点圆周并应用最短路径算法，例如 A*，但这将非常低效。

所以这是我的问题：有没有更简洁的方法来解决这个问题？

【问题讨论】：

标签： python optimization geometry shortest-path

【解决方案1】：

为了记录，我使用准牛顿方法实现了一个解决方案，并描述了它in this short article。主要工作总结如下。

import numpy as np
from scipy.optimize import minimize

# objective function definition
def tasklen(θ, x, y, r):
    x_proj = x + r*np.sin(θ)
    y_proj = y + r*np.cos(θ)

    dists = np.sqrt(np.power(np.diff(x_proj), 2) + np.power(np.diff(y_proj), 2))

    return dists.sum()

# center coordinates and radii of turnpoints
X = np.array([0, 5, 0, 7, 12, 12]).astype(float)
Y = np.array([0, 0, 4, 7, 0, 5]).astype(float)
R = np.array([0, 2, 1, 2, 1, 0]).astype(float)

# first initialization vector is an array of zeros
init_vector = np.zeros(R.shape).astype(float)

# using scipy's solvers to minimize the objective function
result = minimize(tasklen, init_vector, args=(X, Y, R), tol=10e-5)

【讨论】：

【解决方案2】：

我会这样做：

对于每个圆，依次选择圆周上的任意一点，并通过这些点设置路径。
对于每个圆，沿圆周向使总路径长度更小的方向移动点。
重复 2. 直到无法进一步改进为止。

【讨论】：