2DOF 平面机器人的逆运动学

Question

我有一个简单的 2DOF 平面机器人，具有以下配置： L1和L2是我两条手臂的长度。 L1为245.5mm，L2为160mm。 Alpha 是第一个角度（底座的角度），beta 是连接到第一个臂的第二个角度。

当我对 Arduino 进行编程并尝试数学时，我得到的结果不一致。分数完全偏离了。即使沿一个轴的线性运动也不起作用。所以我用完全相同的数学写了一个简单的 python 脚本，其中我保持 x 或 y 不变，并以小增量增加另一个。然后我绘制了计算出的 α 和 β 角的图表。我希望它们线性增加或减少（大约像虚线）。但我得到了这些结果： 我正在使用可以在互联网上找到的标准公式（来源到原始），但显然它们不起作用。他们是：

$ eta = cos^{-1}( rac{x^2+y^2-L_{1}^2-L_{2}^2}{2*L_{1}*L_2})$

$ lpha = tan^{-1}( rac{y}{x})-tan^{-1}( rac{L_2sin(eta)}{L_1+L_2cos(eta)}) $

这也是我用来进行计算的 Python 代码：

import math

L1 = 245.5
L2 = 160

x = 100
y = 70

def calc_IK_beta(x, y):
    beta = (x * x + y * y - L1 * L1 - L2 * L2) / (2 * L1 * L2)
    beta = math.acos(beta)

    beta_d = beta * 180 / math.pi

    if x < 0 and y > 0:
        beta_d = -beta_d
    elif x < 0 and y < 0:
        beta_d = -180 - beta_d
    elif x > 0 and y < 0:
        beta_d = -beta_d

    return beta #in radians

def calc_IK_alpha(x, y):
    beta = calc_IK_beta(x, y)

    alpha = math.atan2(y, x) - math.atan2(L2 * math.sin(beta), L1 + L2 * math.cos(beta))

    alpha_d = alpha * 180 / math.pi

    if x < 0 and y > 0:
        alpha_d = -alpha_d
    elif x < 0 and y < 0:
        alpha_d = -180 - alpha_d
    elif x > 0 and y < 0:
        alpha_d = 180 - alpha_d

    return alpha #in radians

当我对 Arduino 进行编程并尝试数学时，我得到的结果不一致。分数完全偏离了。即使沿一个轴的线性运动也不起作用。所以我用完全相同的数学写了一个简单的 python 脚本，其中我保持 x 或 y 不变，并以小增量增加另一个。然后我绘制了计算出的 α 和 β 角的图表。我希望它们线性增加或减少（大约像虚线）。

Answer 1

你的第一个公式（β）是正确的......但请注意，有两个角度β满足它。我不知道你的第二个公式（对于 alpha）是否正确，但请注意，反转 tan 函数可能会因在 (-180,180] （或任何其他 360 度角度范围）中选择错误的解决方案而出错。我想出了使用不同的 alpha 表达式（在下面的代码中给出）。

在下面的代码中：

每个 (x,y) 都有两对 (alpha, beta) 的解，因为必须有（画出完成“风筝”形状的边）；

我愿意接受有人指出错误，但我已经尝试过 通过从两个角度对中的每一个重新计算 (x,y) 来“回溯检查”。

from math import pi, cos, sin, acos, atan2


RAD_TO_DEG = 180.0 / pi


def angles_to_xy( L1, L2, alpha, beta ):
    x = L1 * cos( alpha ) + L2 * cos( alpha - beta )
    y = L1 * sin( alpha ) + L2 * sin( alpha - beta )
    return x, y


def xy_to_angles( L1, L2, x, y ):      # WARNING: there are TWO pairs of angles that satisfy this
    cosbeta = ( x ** 2 + y ** 2 - L1 ** 2 - L2 ** 2 ) / ( 2.0 * L1 * L2 )
    beta1 = acos( cosbeta )
    beta2 = -beta1
    A, B = L1 + L2 * cosbeta, L2 * sin( beta1 )
    alpha1 = atan2( y * A + x * B, x * A - y * B )
    alpha2 = atan2( y * A - x * B, x * A + y * B )
    return alpha1, beta1, alpha2, beta2


def checker( L1, L2, x, y ):
    alpha1, beta1, alpha2, beta2 = xy_to_angles( L1, L2, x, y )
    alpha1_deg, beta1_deg = alpha1 * RAD_TO_DEG, beta1 * RAD_TO_DEG
    alpha2_deg, beta2_deg = alpha2 * RAD_TO_DEG, beta2 * RAD_TO_DEG
    print( "alpha1, beta1, alpha2, beta2 (degs)= ", ( 4 * "{:7.2f}  " ).format( alpha1_deg, beta1_deg, alpha2_deg, beta2_deg ) )
    X1, Y1 = angles_to_xy( L1, L2, alpha1, beta1 )
    X2, Y2 = angles_to_xy( L1, L2, alpha2, beta2 )
    print( "Back-check X, Y (twice) = ", ( 4 * "{:7.2f}  " ).format( X1, Y1, X2, Y2 ) )
    print( "----------" )


L1, L2 = 245.5, 160.0

x, y = 100.0,  70.0;   checker( L1, L2, x, y )
x, y = 113.0, 359.0;   checker( L1, L2, x, y )

输出：

alpha1, beta1, alpha2, beta2 (degs)=    69.19   154.61     0.79  -154.61  
Back-check X, Y (twice) =   100.00    70.00   100.00    70.00  
----------
alpha1, beta1, alpha2, beta2 (degs)=    89.95    44.76    55.11   -44.76  
Back-check X, Y (twice) =   113.00   359.00   113.00   359.00  
----------