机器人工程中如何在三次样条曲线上求最近点？

Question

我有一个机器人，我想让这个机器人沿着一条预先定义好的路径，在外面的田野上以8字形的形式出现。机器人不容易转向，而且有一些外部因素，如风，使得机器人很可能不会沿着准确的路径行驶，因此它通常会紧靠路径。在

我想让它遵循的路径是通过定义五个点，然后使用三次样条曲线在这些点上创建一条直线（我用来定义此路径的代码位于此消息下方）：

我总是想给机器人提供一个三次样条线上的转向点。要做到这一点，我认为最简单的方法是：

1. 从机器人当前位置计算三次样条曲线上的最近点
2. 沿三次样条线前进0.2个单元，以确定机器人要瞄准的新航路点。在

例如，如果机器人在上面的网格中的位置是x=0.4, y=-0.5，那么最近的点大约是x=0.4, y=-0.28，而新的航路点大约是x=0.22, y=-0.18：

现在我有三个问题：

1. 如何找到三次样条曲线上最近的点？在
2. 如何从三次样条曲线上的发现点“前进”0.2个单位？在
3. 即使路径在中间交叉，我如何保持在给定的路径上？在

欢迎所有提示！在

import numpy as np
import matplotlib.pyplot as plt
from scipy import interpolate

x = [-0.5, 0, 0.5, 0.5,  0, -0.5,  -0.5, 0, 0.5]
y = [0.25, 0, -0.25, 0.25, 0, -0.25, 0.25, 0, -0.25]

tck, u = interpolate.splprep([x, y], s=0)
unew = np.arange(0, 1.01, 0.01)
out = interpolate.splev(unew, tck)

plt.figure()
plt.plot(x, y, 'o', out[0], out[1])
plt.legend(['Predefined points', 'Cubic Spline'])
plt.axis([-0.75, 0.75, -0.75, 0.75])
plt.show()

Answer 1

Now I've got three questions:

How do I find the nearest point on the cubic spline? How do I "advance" 0.2 units from the found point on the cubic spline? How do I stay on the given path, even when the path crosses itself in the middle?

我相信利用样条线数组索引作为一个导航维度是有帮助的

在没有初始信息的情况下，一个粗略但不是那么慢的启动步骤就是简单地找到到整个样条线x，y轨迹的最小距离，out-许多方法都是用来寻找最近的

然后使用该点的索引作为状态变量，并与方向（增加或减少out索引）一起找到下一个要指向的索引

如果你已经开始，很接近，并且知道你最后一个索引位置（或者估计的目标点的索引），那么你可以在靠近你的内部状态索引（无论当前的“前进”方向是什么）的切片中搜索“最近的”，在你一步一步地更新你的内部输出索引。。。在

在涉及状态的情况下，我会考虑一个OOP Robot类，包含XY位置、方向、移动幅度、外样条索引估计、索引方向

和一些有趣的实际编程

[编辑：
在“最近”处第一次通过

# out is a list of arrays, covert to 2d array
aout=np.array(out)

tpoint = np.array([[0.5],[-0.7]])

diff = aout-tpoint

sd = diff[0]*diff[0] + diff[1]*diff[1]  # squared distance

np.min(sd)
cpi=np.where(sd<=np.min(sd)+0.00001)[0]
plt.plot((aout[0, cpi],tpoint[0]),( aout[1, cpi], tpoint[1]), linewidth=2)

只需检查out index是否为srictly montonic/cyclic-确实如此，但是linspace给出了不止一个精确的循环，我就对一个完整周期观察了大约75个点： #用索引动画绘制样条曲线点

^{pr2}$