0

我使用 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 矩阵乘法,它作为模型没有多大意义,但奇怪的是,它并没有比上面更复杂的模型差。

我希望那里有人可以提供帮助。

4

3 回答 3

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 的方法有很多。一个好的方法是阻尼最小二乘法。见: 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

于 2014-06-25T20:16:24.163 回答
0

我认为您正在尝试做的是某种“运动学校准”:从一组测量数据中识别机器人参数。如果您真的想深入挖掘,有许多经典教科书讨论这个主题,例如[Mooring et al.] "Fundamentals of manipulator calibration"

回到您的问题,很多事情都会导致您的参数识别无法收敛,所以请注意,这不是一个食谱答案。;)

一种可能的情况是您有两个(或更多)具有平行轴的关节。在更简单的机器人中具有这种配置是很常见的,例如在SCARA或- 类似的PUMA机制中。在这种情况下,根据 DH 约定,选择轴线的方法有无数种。

有不同的方法可以解决这个问题,但是 YMMV。您可以尝试的一件事是使用Hayati-modified DH model。该模型在基本 DH 中增加了一个参数“beta”,以应对平行轴情况下的奇异性。

或者您可以尝试创建自己的“自定义”转换矩阵来为您的机制建模。例如,您可以使用roll-pitch-yaw(或欧拉角)来表示关节轴之间的旋转,然后添加一个长度参数以到达下一个关节等。

引起我注意的另一件事是_scale_output_。我认为这意味着您可以为给定的数据集提供多个“臂长”解决方案。作为说明,两者[scale_output=1, arm_length=100][scale_output=100, arm_length=1]都将给出具有相同关节角度的相同位置。尝试scale_output从模型中删除,看看是否有帮助。

此外,您可能想尝试其他优化/最小化例程。我scipy.optimize.leastsq()过去成功地用于运动学校准。

希望这可以帮助!

于 2016-10-06T17:28:41.120 回答
0

鉴于您的目标是更多地了解机器人技术,从长远来看,首先建立强大的基础将对您有很大帮助。您很可能希望首先深入了解变换矩阵的世界,这样当您遇到更复杂的主题(如 DH 表和逆运动学)时,您就有了一些基础。

以下是一些可能有帮助的视频:

https://www.youtube.com/watch?v=xYQpeKYCfGs&list=PLJtm2YNbaY4_rQApwht0ia5r_sx3vaSxv

于 2016-11-28T21:37:01.317 回答