二维平面机器人逆向运动学

0 投票

1 回答

51 浏览

提问于 2025-04-14 15:28

我有一个简单的2自由度平面机器人，配置如下：

L1和L2是我两个手臂的长度。L1是245.5毫米，L2是160毫米。Alpha是第一个角度（底座的角度），beta是连接到第一个手臂的第二个角度。

当我编写Arduino程序并尝试我的数学计算时，结果却不一致。计算出来的点完全不对。甚至在一个轴上进行线性运动也不行。于是我写了一个简单的Python脚本，使用完全相同的数学方法，在这个脚本中，我保持x或y不变，逐步增加另一个值。然后我绘制了计算出的alpha和beta角度的图表。我本以为它们应该是线性增加或减少（大致像虚线那样）。但是结果却是这样：

我使用的是网上到处都能找到的标准公式（原始来源），但显然它们并不管用。公式如下：

$\beta = cos^{-1}(\frac{x^2+y^2-L_{1}^2-L_{2}^2}{2*L_{1}*L_2})$

$\alpha = tan^{-1}(\frac{y}{x})-tan^{-1}(\frac{L_2sin(\beta)}{L_1+L_2cos(\beta)})$

这里是我用来进行计算的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编程机械臂线性运动

1 个回答

你的第一个公式（关于beta的）是正确的……但是要注意，有两个角度beta都能满足这个公式。我不确定你的第二个公式（关于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  
----------

回答于 2025-04-14 由 Python大师

分享举报

二维平面机器人逆向运动学

1 个回答

撰写回答