0
3 RC Servos와 Arduino를 사용하여 간단한 로봇 팔을 만들었습니다. 방금 놀고 로봇에 대해 배우고 싶습니다.전방 운동학 데이터 모델링
현재 서보의 세 각도 위치를 사용하여 로봇 팔의 끝 부분의 위치를 계산하려고합니다. "Forward Kinematics"나는 이것을위한 기술적 용어라고 생각합니다. 팔 끝은 펜이지만, 나중에 그 위에 무언가를 그려 볼 수도 있다고 생각했습니다.
팔의 동작 범위에서 나는 직교 좌표계를 설정하고 24 (각도 => 위치) 샘플을 기록했습니다. pastebin.com/ESqWzJJB
이제이 데이터를 모델링하려고합니다.하지만 여기서는 약간 깊이 있습니다. 여기 내 접근 방식은 다음과 같습니다.
나는 Denikit-Hartenberg 방정식을 Wikipedia en.wikipedia.org/wiki/Denavit-Hartenberg_parameters에 있습니다. 그런 다음 최소 제곱 최적화를 사용하여 매개 변수를 결정하려고합니다.
y = f(ax+b)*c+d
내 파이썬 코드 :
minimize(sum(norm(f(x,P)-y)^2))
는 또한 가능한 왜곡 (서보 각도 즉 위상 시프트)을 보상하기 위해 입력 및 모델의 출력과 선형 조건을 추가 pastebin.com을/gQF72mQn
from numpy import *
from scipy.optimize import minimize
# Denavit-Hartenberg Matrix as found on Wikipedia "Denavit-Hartenberg parameters"
def DenHarMat(theta, alpha, a, d):
cos_theta = cos(theta)
sin_theta = sin(theta)
cos_alpha = cos(alpha)
sin_alpha = sin(alpha)
return array([
[cos_theta, -sin_theta*cos_alpha, sin_theta*sin_alpha, a*cos_theta],
[sin_theta, cos_theta*cos_alpha, -cos_theta*sin_alpha, a*sin_theta],
[0, sin_alpha, cos_alpha, d],
[0, 0, 0, 1],
])
def model_function(parameters, x):
# split parameter vector
scale_input, parameters = split(parameters,[3])
translate_input, parameters = split(parameters,[3])
scale_output, parameters = split(parameters,[3])
translate_output, parameters = split(parameters,[3])
p_T1, parameters = split(parameters,[3])
p_T2, parameters = split(parameters,[3])
p_T3, parameters = split(parameters,[3])
# compute linear input distortions
theta = x * scale_input + translate_input
# load Denavit-Hartenberg Matricies
T1 = DenHarMat(theta[0], p_T1[0], p_T1[1], p_T1[2])
T2 = DenHarMat(theta[1], p_T2[0], p_T2[1], p_T2[2])
T3 = DenHarMat(theta[2], p_T3[0], p_T3[1], p_T3[2])
# compute joint transformations
# y = T1 * T2 * T3 * [0 0 0 1]
y = dot(T1,dot(T2,dot(T3,array([0,0,0,1]))))
# compute linear output distortions
return y[0:3] * scale_output + translate_output
# least squares cost function
def cost_function(parameters, X, Y):
return sum(sum(square(model_function(parameters, X[i]) - Y[i])) for i in range(X.shape[0]))/X.shape[0]
# ========== main script start ===========
# load data
data = genfromtxt('data.txt', delimiter=',', dtype='float32')
X = data[:,0:3]
Y = data[:,3:6]
cost = 9999999
#try:
# parameters = genfromtxt('parameters.txt', delimiter=',', dtype='float32')
# cost = cost_function(parameters, X, Y)
#except IOError:
# pass
# random init
for i in range(100):
tmpParams = (random.rand(7*3)*2-1)*8
tmpCost = cost_function(tmpParams, X, Y)
if tmpCost < cost:
cost = tmpCost
parameters = tmpParams
print('Random Cost: ' + str(cost))
savetxt('parameters.txt', parameters, delimiter=',')
# optimization
continueOptimization = True
while continueOptimization:
res = minimize(cost_function, parameters, args=(X,Y), method='nelder-mead', options={'maxiter':100,'xtol': 1e-5})
parameters = res.x
print(res.fun)
savetxt('parameters.txt', parameters, delimiter=',')
continueOptimization = not res.success
print(res)
하지만 작동하지 않을 것입니다. 또한 간단한 3x4 행렬 곱셈을 시도했습니다.이 곱셈은 모델로서별로 의미가 없지만 이상하게도 위에 나온보다 정교한 모델보다 좋지 않습니다.
도움이 될만한 사람이 있기를 바랍니다.
당신이 당신의 질문에 코드를 게시 아닌 사람들은 그것을보고 다른 웹 사이트로 이동하시기 바랍니다 수 :
여기에 도움이 될 수 있습니다 일부 동영상은? –