1

我有一个机器人，我想让这个机器人按照一个预先定义的路径，在外面的一个场地上以八字形的形式出现。机器人不易操控，风等外部因素使机器人很可能不会遵循确切的路径，因此它通常会靠近路径。

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

在任何特定时刻我都希望能够

我总是希望能够为机器人提供一个在三次样条线上的转向点。为了能够做到这一点，我认为最简单的方法是：

  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()
4

1 回答 1

1

现在我有三个问题：

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

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

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

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

如果您已经启动，已经接近，并且知道您的最后一个索引位置（或估计的目标点的索引），那么您可以在您的内部状态附近（无论是当前“前进”方向）的切片中搜索“最近”索引，在您步进时更新您的内部输出索引...

在涉及状态的情况下，我会考虑一个具有 XY 位置、航向、移动幅度、外样条索引估计、索引方向的 OOP 机器人类

和一些实际编程的乐趣

[编辑：]
“最近”的第一次传球 

# 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)

只是检查出索引是否严格单调/循环 - 确实如此，但是 linspace 给出了不止一个精确的循环，我目测了整个循环大约 75 个点：# 按索引对样条点进行动画绘制

#import numpy as np
#import matplotlib.pyplot as plt
from matplotlib.animation import FuncAnimation

fig1, ax = plt.subplots()
xdata, ydata = [], []
ln, = plt.plot([], [], 'ro', animated=True)

def update(frame):
    xdata.append(aout[0][frame])
    ydata.append(aout[1][frame])
    ln.set_data(xdata, ydata)
    return ln,


def init():
    ax.set_xlim(-1, 1)
    ax.set_ylim(-1, 1)
    return ln,

data = np.array(out) #np.random.rand(2, 25)
l, = plt.plot([], [], 'r-')
plt.xlim(0, 1)
plt.ylim(0, 1)
ani = FuncAnimation(fig, update,
                              frames=np.array(range(74)),
                               init_func=init, blit=True)
plt.show()
于 2017-05-22T03:39:19.157 回答