我正在尝试实现一种算法，该算法通过二维平面中的有序航点列表计算最短路径及其从当前位置到目标的相关距离。航点由其中心坐标 (x， y( 和半径 r 定义。最短路径必须至少与每个航点圆周相交一次。这与其他路径优化问题不同，因为我已经知道航点必须交叉的顺序。
在简单的情况下，连续的航点是不同的并且没有对齐，这可以使用连续的角度平分来解决。棘手的情况是：
- 当三个或更多连续航点具有相同的中心但半径不同时
- 当连续的航点对齐时，直线穿过所有航点
这是我的 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*，但效率非常低。
所以我的问题来了：有没有更简洁的方法来解决这个问题？
作为记录，我使用准牛顿方法实现了一个解决方案，并在这篇简短的文章中对其进行了描述。主要工作概述如下。
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.，直到无法进行进一步的改进。