通过有序环形航路点的最短路径 [英] Shortest path through ordered circular waypoints

问题描述

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

I am trying to implement an algorithm which computes the shortest path and and its associated distance from a current position to a goal through an ordered list of waypoints in a 2d plane. A waypoint is defined by its center coordinates (x, y) and its radius r. The shortest path have to intersect each waypoint circumference at least once. This is different from other path optimization problems because I already know the order in which the waypoints have to be crossed.

在简单情况中，连续的航路点是不同的且未对齐，这可以可以使用连续的角度平分线求解.棘手的情况是:

In the simple case, consecutive waypoints are distinct and not aligned and this can be solved using consecutive angle bisections. The tricky cases are :

• 当三个或多个连续的航路点具有相同的中心但半径不同时
• 对齐连续的航路点以使一条直线穿过所有航路点

• when three or more consecutive waypoints have the same center but different radii
• when consecutive waypoints are aligned such that a straight line passes through all of them

这是我的Python实现的精简版本，它不处理对齐的航路点，并且处理严重同心的连续航路点.我改编它是因为它通常使用纬度和经度，而不是欧氏空间中的点.

Here is a stripped down version of my Python implementation, which does not handle aligned waypoints, and handles badly concentric consecutive waypoints. I adapted it because it normally uses latitudes and longitudes, not points in the euclidean space.

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 *)，但效率极低.

I can see how to test for the different corner cases but I think this approach is bad, because there may be other corner cases I haven't thought of. Another approach would be to discretize the waypoints circumferences and apply a shortest path algorithm such as A*, but that would be highly inefficient.

这是我的问题:是否有更简洁的方法来解决此问题?

推荐答案

为了记录，我使用拟牛顿方法实现了一个解决方案，并对其进行了描述

For the record, I implemented a solution using Quasi-Newton methods, and described it in this short article. The main work is summarized below.

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)

这篇关于通过有序环形航路点的最短路径的文章就介绍到这了，希望我们推荐的答案对大家有所帮助，也希望大家多多支持IT屋！