正向运动学数据建模

Question

我已经建立了一个简单的机械臂使用3个RC伺服和一个Arduino。 我只想玩玩它，学点机器人技术。在

目前，我正在尝试使用伺服系统的三个角度位置来计算机械臂末端的位置。 “正向运动学”我认为是这个的技术术语。 顺便说一句，我的胳膊尖是一支笔，我想我可以试着用它来画些东西。在

在手臂的运动范围内，我建立了笛卡尔坐标系，并记录了24个（角度=>位置）样本。 pastebin.com/ESqWzJJB在

现在，我正试图对这些数据建模，但我有点力不从心。 到目前为止，我的方法是：

我用的是维基百科上的Denavit-Hartenberg方程en.wikipedia.org/wiki/Denavit网站–Hartenberg_参数。 然后我尝试用最小二乘法优化来确定参数。在

minimize(sum(norm(f(x,P)-y)^2))

我还为模型的输入和输出添加了线性项，以补偿可能的失真（例如伺服角度的相移）：

^{pr2}$

我的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矩阵乘法，它作为一个模型没有多大意义，但奇怪的是，它没有比上面更复杂的模型做得差。在

我希望有人能帮上忙。在

Answer 1

我想你想做的是某种“运动学校准”：从一组测量数据中识别机器人参数。如果你真的想深入研究，有很多经典的教科书讨论这个主题，例如[Mooring et al.] "Fundamentals of manipulator calibration"。在

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

一种可能的情况是有两个（或多个）具有平行轴的关节。在更简单的机器人中有这种配置是很常见的，例如在SCARA，或{}类机构中。在这种情况下，使用DH约定有无限多的方法来选择轴线。在

有不同的方法来解决这个问题，但是YMMV。你可以尝试使用Hayati修正的DH模型。该模型在基本DH的基础上增加了一个参数beta来处理平行轴情况下的奇异性。在

或者，您可以尝试创建自己的“自定义”转换矩阵来为您的机制建模。例如，可以使用“侧滚-俯仰偏航”（或“Euler角度”）来表示关节轴之间的旋转，然后添加一个长度参数以到达下一个关节，等等

另一件引起我注意的事情是“缩放”输出。我认为这意味着对于一个给定的数据集，你可以有多个“公平”的解决方案。举例来说，[scale_output=1, arm_length=100]和{}将以相同的关节角度给出相同的位置。尝试从模型中删除scale_output，看看这是否有帮助。在

此外，您可能还想尝试其他优化/最小化例程。我曾成功地使用scipy.optimize.leastsq()进行运动学校准。在

希望这有帮助！在

Answer 2

如果我没听错的话，你是在试图解机器人手臂的反向运动学（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

一个简单的方法是循环坐标下降，这是相当容易管理的工作在一个有限的矩阵支持的arduino。请参见：http://www.cs.cmu.edu/~15464-s13/assignments/assignment2/jlander_gamedev_nov98.pdf

Answer 3

鉴于你的目标是学习更多的机器人技术，首先建立坚实的基础知识将大大有助于你的长远发展。你很可能想先深入研究变换矩阵的世界，这样当你涉及到更复杂的主题，比如DH表和逆运动学时，你会有一些东西可以借鉴。在

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

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