通过有序循环的最短路径 waypoints
Shortest path through ordered circular waypoints
我正在尝试实现一种算法，该算法通过 waypoints 的 有序列表计算最短路径及其从当前位置到目标的相关距离一个二维平面。航路点由其中心坐标 (x, y) 及其半径 r 定义。最短路径必须与每个航路点圆周相交 至少一次 。这与其他路径优化问题不同，因为我已经知道 order 其中 waypoints 必须被交叉。
在 simple case 中，连续的 waypoints 是不同的且未对齐，这可以使用连续的角二等分来解决。棘手的情况是：
- 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 实现的精简版，它不处理对齐 waypoints，并且处理 很糟糕 同心连续 waypoints.我对其进行了调整，因为它通常使用纬度和经度，而不是欧几里德 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
我知道如何测试不同的极端情况，但我认为这种方法不好，因为可能还有其他我没有想到的极端情况。另一种方法是离散化 waypoints 圆周并应用最短路径算法（例如 A*），但效率非常低。
所以这是我的问题：是否有更简洁的方法来解决这个问题？
我会这样做：
- 对于每个顺序的圆，选择圆周上的任意点，并通过这些点布置路径。
- 对于每个圆，沿圆周向使总路径长度变小的方向移动点。
- 重复2.直到无法进一步改进。
郑重声明，我使用拟牛顿方法实现了一个解决方案，并对其进行了描述 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)
我正在尝试实现一种算法，该算法通过 waypoints 的 有序列表计算最短路径及其从当前位置到目标的相关距离一个二维平面。航路点由其中心坐标 (x, y) 及其半径 r 定义。最短路径必须与每个航路点圆周相交 至少一次 。这与其他路径优化问题不同，因为我已经知道 order 其中 waypoints 必须被交叉。
在 simple case 中，连续的 waypoints 是不同的且未对齐，这可以使用连续的角二等分来解决。棘手的情况是：
- 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 实现的精简版，它不处理对齐 waypoints，并且处理 很糟糕 同心连续 waypoints.我对其进行了调整，因为它通常使用纬度和经度，而不是欧几里德 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
我知道如何测试不同的极端情况，但我认为这种方法不好，因为可能还有其他我没有想到的极端情况。另一种方法是离散化 waypoints 圆周并应用最短路径算法（例如 A*），但效率非常低。
所以这是我的问题：是否有更简洁的方法来解决这个问题？
我会这样做：
- 对于每个顺序的圆，选择圆周上的任意点，并通过这些点布置路径。
- 对于每个圆，沿圆周向使总路径长度变小的方向移动点。
- 重复2.直到无法进一步改进。
郑重声明，我使用拟牛顿方法实现了一个解决方案，并对其进行了描述 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)