我使用 3 个 RC Servos 和一个 Arduino 构建了一个简单的机械臂。我只是想玩弄它并学习一些有关机器人的知识。
目前，我正在尝试使用伺服系统的三个角位置来计算机械臂尖端的位置。我认为“正向运动学”是这个的技术术语。顺便说一句，手臂的尖端是一支笔，我想我以后可能会尝试用它来画点什么。
在手臂的运动范围内，我建立了笛卡尔坐标系并记录了 24 个（角度 => 位置）样本。pastebin.com/ESqWzJJB
现在，我正在尝试对这些数据进行建模，但我在这里有点超出我的深度。到目前为止，这是我的方法：
我使用维基百科 en.wikipedia.org/wiki/Denavit–Hartenberg_parameters 上的 Denavit–Hartenberg 方程。然后我尝试使用最小二乘优化来确定参数。
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 矩阵乘法，它作为模型没有多大意义，但奇怪的是，它并没有比上面更复杂的模型差。
我希望那里有人可以提供帮助。