机械臂逆运动学--python版

前言：在树莓派上，若想对机械臂进行逆运动学求解，相较于代数法，几何法求解机械臂逆运动学具有明显优势：计算速度快且资源消耗低，适合树莓派的有限计算能力；算法简单易实现，适合快速开发和实时控制；能够确保机械臂快速响应指令，提升系统的整体性能。

一些概念：水平误差和垂直误差的引入通常是为了模拟实际机械系统中的偏差。这些误差可能来源于机械连接件的制造误差、装配误差或者在实际运动过程中由于各类力的作用导致的偏差。具体来说：

1. 水平误差 (el)：这是在水平方向上存在的系统误差。它可能是因为机械臂的关节在设计或制造过程中存在一些未考虑到的偏差，导致目标位置在水平方向上产生了误差。

2. 垂直误差 (eh)：这是在垂直方向上存在的系统误差。类似地，这种误差可能是由于机械臂的垂直方向定位存在一些不可避免的偏差，导致机械臂的末端位置在垂直方向上产生了误差。

在实际的机器人控制中，这些误差需要被考虑进去，以提高运动学计算的精度。模拟这些误差可以帮助设计更鲁棒的控制算法，并在仿真和实际应用中更接近真实的工作环境。

from math import cos, sin, sqrt, atan2, degrees, isclose

def inverse_kinematics(x, y, z):
    l1 = 150  # 连杆1长度
    l2 = 160  # 连杆2长度
    el = 60   # 水平误差
    eh = 35   # 垂直误差

    # 计算目标点与原点之间的距离
    distance = sqrt(x**2 + y**2 + z**2)
    
    # 检查目标点是否在机械臂的可达范围内
    if distance > (l1 + l2 + el):
        print("够不着")
        return False, None, None, None

    # 计算第一个关节的角度
    if isclose(x, 0) and isclose(y, 0):
        j1 = 0
    else:
        j1 = atan2(y, x)

    # 计算水平距离
    if not isclose(cos(j1), 0):
        len = x / cos(j1)
    else:
        len = abs(y)

    # 考虑水平误差后的长度
    a = len - el
    b = z - eh

    # 计算第三个关节的角度
    cos_j3 = ((a**2 + b**2 - l1**2 - l2**2) / (2 * l2 * l1))
    if not -1 <= cos_j3 <= 1:
        print("无解")
        return False, None, None, None

    sin_j3 = sqrt(1 - cos_j3**2)
    j3 = atan2(sin_j3, cos_j3)

    # 计算第二个关节的角度
    k1 = l1 + l2 * cos_j3
    k2 = l2 * sin_j3
    w = atan2(k2, k1)
    j2 = atan2(b, a) - w

    # 计算末端执行器的实际位置
    x1 = (l1 * sin(j2) + l2 * sin(j2 + j3) + el) * cos(j1)
    y1 = (l1 * sin(j2) + l2 * sin(j2 + j3) + el) * sin(j1)
    z1 = l1 * cos(j2) + l2 * cos(j2 + j3) + eh

    # 将角度转换为度数
    deg_j1 = degrees(j1)
    deg_j2 = degrees(j2)
    deg_j3 = degrees(j3) - 90 + deg_j2

    print("结果：j1: {} ,j2: {} ,j3: {} ".format(deg_j1, deg_j2, deg_j3))

    # 检查角度是否在允许范围内
    if deg_j1 > 135 or deg_j1 < -135 or deg_j2 > 100 or deg_j2 < -60 or deg_j3 > 135 or deg_j3 < 20:
        print("超出约束")
        return False, None, None, None

    print("运动学正解结果：x: {}, y: {}, z: {}\r\n".format(x1, y1, z1))

    return True, deg_j1, deg_j2, deg_j3

if __name__ == '__main__':
    has_sol, j1, j2, j3 = inverse_kinematics(500, 50, 10)