로봇 공학 프로젝트에서 입방 스플라인에 가장 가까운 점을 얻는 방법은 무엇입니까?

Question

나는 로봇을 가지고 있는데,이 로봇을 외부 필드의 8 자 형태로 미리 정의 된 경로를 따르고 싶습니다. 로봇은 조종하기 쉽지 않으며 바람과 같은 외부 요인에 의해 로봇이 정확한 경로를 따르지 않아 경로 옆에 놓일 가능성이 높습니다.

나는 5 점을 정의하여 형성 수행하고 큐빅 스플라인 (I가이 경로가이 메시지 아래에 정의하는 데 사용되는 코드)를 사용하는 점 이상 라인 만들려면 경로 :

을

저는 항상 큐빅 스플라인 선상에있는 지점으로 로봇을 조정할 수 있기를 원합니다.

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

지금은있어 세 가지 질문 :

어떻게 큐빅 스플라인에 가장 가까운 지점을 찾을 수 있습니까? 어떻게 큐빅 스플라인의 발견 된 점에서 0.2 단위 "전진"합니까? 중간에 경로가 교차하는 경우에도 은 지정된 경로에 머물러야합니까?

나는 도움없이 초기 정보, 원유와 navigaion 차원

으로 스플라인 배열 인덱스를 활용할 수 있지만 그 느린 시작 단계는 단순히로 분 거리를 찾을 것 것 생각 out - 가장 가까운 검색을위한 많은 SE ans

그런 다음 해당 포인트의 인덱스를 상태 변수로 사용하고 방향과 함께 (증가 또는 감소하는 out 인덱스) 다음으로 이동할 인덱스를 찾습니다.

마지막 색인 위치 (또는 예상 목표 지점의 색인)를 시작하고 닫은 후 알면, (현재 "앞으로"방향이 무엇이든간에) 근처의 조각에서 "가장 가까운"을 검색 할 수 있습니다. 상태 지수는 단계로 밖으로 내부 인덱스를 ... 업데이트 상태

내가, 제목, XY 위치와 OOP 로봇 클래스를 고려 크기, 밖으로 스플라인 지수 추정, 지수 방향

일부를 움직일 것입니다 참여
1 패스에서 : 재미는 실제 프로그래밍

[편집]을 가지고있다 "가장 가까운"지수 순환/srictly montonic 밖으로 경우

# 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 하나 이상의 정확한주기를 제공, 나는 eyeballed ~ 전체 사이클 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()