Я создал простую роботизированную руку, используя 3 RC Servos и Arduino. Я просто хочу поиграть с ним и узнать кое-что о робототехнике.Моделирование данных прямой кинематики
В настоящее время я пытаюсь вычислить положение наконечника роботизированной руки, используя три угловых положения сервоприводов. «Передовая кинематика» Я думаю, это технический термин для этого. Кстати, кончик руки - это ручка, мне показалось, что я попытаюсь что-то сделать с ней позже.
В диапазоне движений руки я установил декартову систему координат и записал образцы 24 (угол => положение). pastebin.com/ESqWzJJB
Теперь, я пытаюсь смоделировать эти данные, но я немного из глубины. Вот мой подход:
Я использую уравнения Денавита-Хартенберга, найденные в Википедии en.wikipedia.org/wiki/Denavit-Hartenberg_parameters. Затем я пытаюсь определить параметры, используя оптимизацию наименьших квадратов.
minimize(sum(norm(f(x,P)-y)^2))
Я также добавил линейных членов к входу и выходу модели для компенсации возможных искажений (например, фазовый сдвиг в угле серво):
y = f(ax+b)*c+d
Мой код Python: 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, что не имеет большого смысла в качестве модели, но, как ни странно, это не делало хуже, чем более сложная модель выше.
Я надеюсь, что есть кто-то, кто может помочь.
Можете ли вы опубликовать свой код в ваш вопрос, а не заставить людей перейти на другой сайт, чтобы увидеть его? –