我使用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矩阵乘法，作为一个模型没有多大意义，但奇怪的是它并没有比上面那个更复杂的模型更糟糕。
我希望有人可以提供帮助。
答案 0 :(得分：1)
如果我理解正确，您正在尝试解决机器人手臂的反向运动学（IK）。正向运动学（FK）是关于在给定关节角度的情况下确定末端执行器的位置。您想要找到使末端效应器到达所需位置的角度。
为了解决IK问题，您必须弄清楚手臂的前向运动。 如果您不确定当前的FK，可以使用以下脚本为每个关节（包括末端效应器）获取符号FK矩阵。它也会产生雅可比行列式。
import numpy as np
from sympy import *
def pos(matrix):
list = [0,0,0]
list[0] = matrix[0,3]
list[1] = matrix[1,3]
list[2] = matrix[2,3]
return np.array(list).astype(float).tolist()
class KinematicChain:
def __init__(self):
self.i = 1
self.syms = []
self.types = []
self.matrices = []
self.fk = []
def add(self, type, relPos):
"""
Parameters:
type - the type of joint
relpos - the position of the joint relative to the previos one
"""
mat = self.transMatrix(type, relPos);
self.matrices.append(mat)
self.types.append(type)
if len(self.fk) == 0:
self.fk.append(eye(4)*mat)
else:
self.fk.append(simplify(self.fk[-1]*mat))
def jacobian(self):
fk = self.fk[-1]
px = fk[0,3]
py = fk[1,3]
pz = fk[2,3]
f = Matrix([px, py, pz])
if (len(self.syms) < 1):
return eye(4)
else:
x = Matrix(self.syms)
ans = f.jacobian(x)
return ans
def transMatrix(self, type, p):
if (type != "FIXED"):
s1 = "a" + str(self.i)
self.i += 1
a = symbols(s1)
self.syms.append(a)
if (type == "FIXED"):
return Matrix([
[1, 0, 0, p[0]],
[0, 1, 0, p[1]],
[0, 0, 1, p[2]],
[0, 0, 0, 1]])
elif (type == "RX"):
return Matrix([
[1, 0, 0, p[0]],
[0, cos(a), -sin(a), p[1]],
[0, sin(a), cos(a), p[2]],
[0, 0, 0, 1]])
elif (type == "RY"):
return Matrix([
[cos(a), 0, sin(a), p[0]],
[0, 1, 0, p[1]],
[-sin(a), 0, cos(a), p[2]],
[0, 0, 0, 1]])
elif (type == "RZ"):
return Matrix([
[cos(a), -sin(a), 0, p[0]],
[sin(a), cos(a), 0, p[1]],
[0, 0, 1, p[2]],
[0, 0, 0, 1]])
elif (type == "PX"):
return Matrix([
[1, 0, 0, p[0] + a],
[0, 1, 0, p[1]],
[0, 0, 1, p[2]],
[0, 0, 0, 1]])
elif (type == "PY"):
return Matrix([
[1, 0, 0, p[0]],
[0, 1, 0, p[1] + a],
[0, 0, 1, p[2]],
[0, 0, 0, 1]])
elif (type == "PZ"):
return Matrix([
[1, 0, 0, p[0]],
[0, 1, 0, p[1]],
[0, 0, 1, p[2] + a],
[0, 0, 0, 1]])
else:
return eye(4)
解决IK的方法有很多种。一个好的是Damped Least Squared方法。 请参阅：http://math.ucsd.edu/~sbuss/ResearchWeb/ikmethods/iksurvey.pdf
一种更简单的方法是Cyclic Coordinate Decent，它非常易于处理具有有限矩阵支持的arduino。请参阅：http://www.cs.cmu.edu/~15464-s13/assignments/assignment2/jlander_gamedev_nov98.pdf
答案 1 :(得分：0)
我认为你要做的是某种＆＃34;运动学校准＆＃34;：从一组测量数据中识别机器人参数。如果您真的想深入挖掘，有许多经典教科书讨论这个主题，例如[Mooring et al.] "Fundamentals of manipulator calibration"。
回到你的问题，很多事情都会导致参数识别失败，所以请注意，这不是食谱的答案。 ;）
一种可能的情况是你有两个（或更多）平行轴的关节。在更简单的机器人中进行这种配置是很常见的，例如在SCARA或PUMA之类的机制中。在这种情况下，使用DH约定有很多种方法可以选择轴线。
有不同的方法可以解决这个问题，但YMMV。您可以尝试的一件事是使用 Hayati-modified DH模型。这个模型增加了一个参数＆＃34; beta＆＃34;对于基本的DH，在平行轴情况下应对奇点。
或者您可以尝试创建自己的＆＃34; custom＆＃34;转换矩阵来模拟你的机制。例如，您可以使用roll-pitch-yaw（或欧拉角）来表示关节轴之间的旋转，然后添加一个长度参数以到达下一个关节等。
引起我注意的另一件事是_scale_output_。我认为这意味着你可以拥有多个＆＃34;臂长＆＃34;给定数据集的解决方案。作为说明，[scale_output=1, arm_length=100]和[scale_output=100, arm_length=1]都将使用相同的关节角度给出相同的位置。尝试从模型中删除scale_output，看看是否有帮助。
您也可以尝试其他优化/最小化例程。我过去成功地使用scipy.optimize.leastsq()进行了运动学校准。
希望这有帮助！
答案 2 :(得分：0)
看到你的目标是更多地了解机器人技术，从长远来看，建立强大的基础知识将极大地帮助你。您最有可能希望首先深入了解转换矩阵的世界，这样当您遇到更复杂的主题（如DH表和反向运动学）时，您将有一些东西可以建立起来。
以下是一些可能有用的视频：
https://www.youtube.com/watch?v=xYQpeKYCfGs&list=PLJtm2YNbaY4_rQApwht0ia5r_sx3vaSxv