• myCobot pro 机械臂（6）逆向运动学

    机械臂逆运动学求解常用的方法有几何法、解析法、数值法

    从求解的方式和计算的效率上来看，几何法和解析法会考虑机械臂结构不同而造成的差异，因此对于不同结构的机械臂会有特定的求解方式。

    通常来说，这两种方法具有速度快、精度高的优点和通用性差、普适性低的缺点。而数值法则通常有相对统一的求解方式，具有适用性好但速度慢、数值稳定性差的特点。

    对于工业机器人而言，通常处于特定的工作环境中，为了满足一定的工作性能要求而常采用解析法进行求解。

    本文将使用解析法对该型机械臂进行逆运动学的求解。

     

    下面通过Python代码的形式将这些推导的公式表示出来：

    1. # 输入位姿矩阵T
    2. # 例如：T=np.matrix(np.array([[0.5,-0.7,-0.3,36],[-0.3,-0.6,0.6,-28.3],[-0.7,-0.2,-0.6,355.7],[0,0,0,1]]))
    3.  
    4. # 输出解的列表q_list,里面的一个列表就代表一组解
    5. # 例如：q_list=[[q1_1,q21,……],[],[],……]
    6.  
    7. def ikine_6DOF(T):
    8.     # 提取元素
    9.     nx = T[0, 0]
    10.     ny = T[1, 0]
    11.     nz = T[2, 0]
    12.     ox = T[0, 1]
    13.     oy = T[1, 1]
    14.     oz = T[2, 1]
    15.     ax = T[0, 2]
    16.     ay = T[1, 2]
    17.     az = T[2, 2]
    18.     px = T[0, 3]
    19.     py = T[1, 3]
    20.     pz = T[2, 3]
    21.  
    22.     # 求解q1
    23.     # 见论文公式(2.23)
    24.     q1_1 = 2 * np.arctan2(px - ax * d6 + np.sqrt((px - ax * d6) ** 2 + (-py + ay * d6) ** 2 - d4 ** 2),
    25.                           d4 - py + ay * d6)
    26.     print(q1_1)
    27.     if q1_1 < q1_range[0] or q1_1 > q1_range[1]:
    28.         q1_1 = False
    29.     q1_2 = 2 * np.arctan2(px - ax * d6 - np.sqrt((px - ax * d6) ** 2 + (-py + ay * d6) ** 2 - d4 ** 2),
    30.                           d4 - py + ay * d6)
    31.     if q1_2 < q1_range[0] or q1_2 > q1_range[1]:
    32.         q1_2 = False
    33.  
    34.     # 求解q6
    35.     # 见论文公式(2.29)
    36.     q6_1 = np.arctan2(ox * np.sin(q1_1) - oy * np.cos(q1_1), ny * np.cos(q1_1) - nx * np.sin(q1_1)) + np.pi
    37.     q6_2 = np.arctan2(ox * np.sin(q1_2) - oy * np.cos(q1_2), ny * np.cos(q1_2) - nx * np.sin(q1_2)) + np.pi
    38.  
    39.     # 求解q5
    40.     # 见论文公式(2.24)
    41.     if ay * np.cos(q1_1) - ax * np.sin(q1_1) > 1:
    42.         q5_1 = False
    43.         q5_2 = False
    44.     else:
    45.         q5_1 = np.arccos(ay * np.cos(q1_1) - ax * np.sin(q1_1))
    46.         q5_2 = -q5_1
    47.     if ay * np.cos(q1_2) - ax * np.sin(q1_2) > 1:
    48.         q5_3 = False
    49.         q5_4 = False
    50.     else:
    51.         q5_3 = np.arccos(ay * np.cos(q1_2) - ax * np.sin(q1_2))
    52.         q5_4 = -q5_3
    53.  
    54.     # 求解q3
    55.     # 见论文公式(2.40)
    56.     # 如果arccos()中的值大于1，会出现nan的结果，报错：RuntimeWarning: invalid value encountered in arccos
    57.     if q5_1 == 0:
    58.         m1 = 10000
    59.     else:
    60.         m1 = px * np.cos(q1_1) + py * np.sin(q1_1) - d5 * az / np.sin(q5_1) - d6 * (
    61.                     ax * np.cos(q1_1) + ay * np.sin(q1_1))
    62.     if q5_2 == 0:
    63.         m2 = 10000
    64.     else:
    65.         m2 = px * np.cos(q1_1) + py * np.sin(q1_1) - d5 * az / np.sin(q5_2) - d6 * (
    66.                     ax * np.cos(q1_1) + ay * np.sin(q1_1))
    67.     if q5_3 == 0:
    68.         m3 = 10000
    69.     else:
    70.         m3 = px * np.cos(q1_2) + py * np.sin(q1_2) - d5 * az / np.sin(q5_3) - d6 * (
    71.                     ax * np.cos(q1_2) + ay * np.sin(q1_2))
    72.     if q5_4 == 0:
    73.         m4 = 10000
    74.     else:
    75.         m4 = px * np.cos(q1_2) + py * np.sin(q1_2) - d5 * az / np.sin(q5_4) - d6 * (
    76.                     ax * np.cos(q1_2) + ay * np.sin(q1_2))
    77.     if q5_1 == 0:
    78.         n1 = 10000
    79.     else:
    80.         n1 = pz - d1 - d6 * az + d5 / np.sin(q5_1) * (ax * np.cos(q1_1) + ay * np.sin(q1_1))
    81.     if q5_2 == 0:
    82.         n2 = 10000
    83.     else:
    84.         n2 = pz - d1 - d6 * az + d5 / np.sin(q5_2) * (ax * np.cos(q1_1) + ay * np.sin(q1_1))
    85.     if q5_3 == 0:
    86.         n3 = 10000
    87.     else:
    88.         n3 = pz - d1 - d6 * az + d5 / np.sin(q5_3) * (ax * np.cos(q1_2) + ay * np.sin(q1_2))
    89.     if q5_4 == 0:
    90.         n4 = 10000
    91.     else:
    92.         n4 = pz - d1 - d6 * az + d5 / np.sin(q5_4) * (ax * np.cos(q1_2) + ay * np.sin(q1_2))
    93.  
    94.     if (m1 ** 2 + n1 ** 2 - a2 ** 2 - a3 ** 2) / (2 * a2 * a3) > 1 or (m1 ** 2 + n1 ** 2 - a2 ** 2 - a3 ** 2) / (
    95.             2 * a2 * a3) < -1:
    96.         q3_1 = False
    97.         q3_2 = False
    98.     else:
    99.         q3_1 = np.arccos((m1 ** 2 + n1 ** 2 - a2 ** 2 - a3 ** 2) / (2 * a2 * a3))
    100.         q3_2 = -q3_1
    101.     if (m2 ** 2 + n2 ** 2 - a2 ** 2 - a3 ** 2) / (2 * a2 * a3) > 1 or (m2 ** 2 + n2 ** 2 - a2 ** 2 - a3 ** 2) / (
    102.             2 * a2 * a3) < -1:
    103.         q3_3 = False
    104.         q3_4 = False
    105.     else:
    106.         q3_3 = np.arccos((m2 ** 2 + n2 ** 2 - a2 ** 2 - a3 ** 2) / (2 * a2 * a3))
    107.         q3_4 = -q3_3
    108.     if (m3 ** 2 + n3 ** 2 - a2 ** 2 - a3 ** 2) / (2 * a2 * a3) > 1 or (m3 ** 2 + n3 ** 2 - a2 ** 2 - a3 ** 2) / (
    109.             2 * a2 * a3) < -1:
    110.         q3_5 = False
    111.         q3_6 = False
    112.     else:
    113.         q3_5 = np.arccos((m3 ** 2 + n3 ** 2 - a2 ** 2 - a3 ** 2) / (2 * a2 * a3))
    114.         q3_6 = -q3_5
    115.     if (m4 ** 2 + n4 ** 2 - a2 ** 2 - a3 ** 2) / (2 * a2 * a3) > 1 or (m4 ** 2 + n4 ** 2 - a2 ** 2 - a3 ** 2) / (
    116.             2 * a2 * a3) < -1:
    117.         q3_7 = False
    118.         q3_8 = False
    119.     else:
    120.         q3_7 = np.arccos((m4 ** 2 + n4 ** 2 - a2 ** 2 - a3 ** 2) / (2 * a2 * a3))
    121.         q3_8 = -q3_7
    122.  
    123.     # 求解q2
    124.     # 见论文公式(2.43)
    125.     if not q3_1:
    126.         q2_1 = False
    127.         q2_2 = False
    128.     else:
    129.         q2_1 = 2 * np.arctan2(
    130.             -a3 * np.sin(q3_1) + np.sqrt((-a3 * np.sin(q3_1)) ** 2 + (a2 + a3 * np.cos(q3_1)) ** 2 - m1 ** 2),
    131.             a2 + a3 * np.cos(q3_1) + m1)
    132.         q2_2 = 2 * np.arctan2(
    133.             -a3 * np.sin(q3_1) - np.sqrt((-a3 * np.sin(q3_1)) ** 2 + (a2 + a3 * np.cos(q3_1)) ** 2 - m1 ** 2),
    134.             a2 + a3 * np.cos(q3_1) + m1)
    135.     if not q3_2:
    136.         q2_3 = False
    137.         q2_4 = False
    138.     else:
    139.         q2_3 = 2 * np.arctan2(
    140.             -a3 * np.sin(q3_2) + np.sqrt((-a3 * np.sin(q3_2)) ** 2 + (a2 + a3 * np.cos(q3_2)) ** 2 - m1 ** 2),
    141.             a2 + a3 * np.cos(q3_2) + m1)
    142.         q2_4 = 2 * np.arctan2(
    143.             -a3 * np.sin(q3_2) - np.sqrt((-a3 * np.sin(q3_2)) ** 2 + (a2 + a3 * np.cos(q3_2)) ** 2 - m1 ** 2),
    144.             a2 + a3 * np.cos(q3_2) + m1)
    145.     if not q3_3:
    146.         q2_5 = False
    147.         q2_6 = False
    148.     else:
    149.         q2_5 = 2 * np.arctan2(
    150.             -a3 * np.sin(q3_3) + np.sqrt((-a3 * np.sin(q3_3)) ** 2 + (a2 + a3 * np.cos(q3_3)) ** 2 - m2 ** 2),
    151.             a2 + a3 * np.cos(q3_3) + m2)
    152.         q2_6 = 2 * np.arctan2(
    153.             -a3 * np.sin(q3_3) - np.sqrt((-a3 * np.sin(q3_3)) ** 2 + (a2 + a3 * np.cos(q3_3)) ** 2 - m2 ** 2),
    154.             a2 + a3 * np.cos(q3_3) + m2)
    155.     if not q3_4:
    156.         q2_7 = False
    157.         q2_8 = False
    158.     else:
    159.         q2_7 = 2 * np.arctan2(
    160.             -a3 * np.sin(q3_4) + np.sqrt((-a3 * np.sin(q3_4)) ** 2 + (a2 + a3 * np.cos(q3_4)) ** 2 - m2 ** 2),
    161.             a2 + a3 * np.cos(q3_4) + m2)
    162.         q2_8 = 2 * np.arctan2(
    163.             -a3 * np.sin(q3_4) - np.sqrt((-a3 * np.sin(q3_4)) ** 2 + (a2 + a3 * np.cos(q3_4)) ** 2 - m2 ** 2),
    164.             a2 + a3 * np.cos(q3_4) + m2)
    165.     if not q3_5:
    166.         q2_9 = False
    167.         q2_10 = False
    168.     else:
    169.         q2_9 = 2 * np.arctan2(
    170.             -a3 * np.sin(q3_5) + np.sqrt((-a3 * np.sin(q3_5)) ** 2 + (a2 + a3 * np.cos(q3_5)) ** 2 - m3 ** 2),
    171.             a2 + a3 * np.cos(q3_5) + m3)
    172.         q2_10 = 2 * np.arctan2(
    173.             -a3 * np.sin(q3_5) - np.sqrt((-a3 * np.sin(q3_5)) ** 2 + (a2 + a3 * np.cos(q3_5)) ** 2 - m3 ** 2),
    174.             a2 + a3 * np.cos(q3_5) + m3)
    175.     if not q3_6:
    176.         q2_11 = False
    177.         q2_12 = False
    178.     else:
    179.         q2_11 = 2 * np.arctan2(
    180.             -a3 * np.sin(q3_6) + np.sqrt((-a3 * np.sin(q3_6)) ** 2 + (a2 + a3 * np.cos(q3_6)) ** 2 - m3 ** 2),
    181.             a2 + a3 * np.cos(q3_6) + m3)
    182.         q2_12 = 2 * np.arctan2(
    183.             -a3 * np.sin(q3_6) - np.sqrt((-a3 * np.sin(q3_6)) ** 2 + (a2 + a3 * np.cos(q3_6)) ** 2 - m3 ** 2),
    184.             a2 + a3 * np.cos(q3_6) + m3)
    185.     if not q3_7:
    186.         q2_13 = False
    187.         q2_14 = False
    188.     else:
    189.         q2_13 = 2 * np.arctan2(
    190.             -a3 * np.sin(q3_7) + np.sqrt((-a3 * np.sin(q3_7)) ** 2 + (a2 + a3 * np.cos(q3_7)) ** 2 - m4 ** 2),
    191.             a2 + a3 * np.cos(q3_7) + m4)
    192.         q2_14 = 2 * np.arctan2(
    193.             -a3 * np.sin(q3_7) - np.sqrt((-a3 * np.sin(q3_7)) ** 2 + (a2 + a3 * np.cos(q3_7)) ** 2 - m4 ** 2),
    194.             a2 + a3 * np.cos(q3_7) + m4)
    195.     if not q3_8:
    196.         q2_15 = False
    197.         q2_16 = False
    198.     else:
    199.         q2_15 = 2 * np.arctan2(
    200.             -a3 * np.sin(q3_8) + np.sqrt((-a3 * np.sin(q3_8)) ** 2 + (a2 + a3 * np.cos(q3_8)) ** 2 - m4 ** 2),
    201.             a2 + a3 * np.cos(q3_8) + m4)
    202.         q2_16 = 2 * np.arctan2(
    203.             -a3 * np.sin(q3_8) - np.sqrt((-a3 * np.sin(q3_8)) ** 2 + (a2 + a3 * np.cos(q3_8)) ** 2 - m4 ** 2),
    204.             a2 + a3 * np.cos(q3_8) + m4)
    205.  
    206.     # 求解q4
    207.     # 见论文公式(2.46)
    208.     if not q3_1:
    209.         q4_1 = False
    210.         q4_2 = False
    211.     else:
    212.         q4_1 = np.arctan2(az, ax * np.cos(q1_1) + ay * np.sin(q1_1)) - q3_1 - q2_1
    213.         q4_2 = np.arctan2(az, ax * np.cos(q1_1) + ay * np.sin(q1_1)) - q3_1 - q2_2
    214.         if q4_1 > np.pi:
    215.             q4_1 = q4_1 - np.pi
    216.         elif q4_1 < -np.pi:
    217.             q4_1 = q4_1 + np.pi
    218.         if q4_2 > np.pi:
    219.             q4_2 = q4_2 - np.pi
    220.         elif q4_2 < -np.pi:
    221.             q4_2 = q4_2 + np.pi
    222.     if not q3_2:
    223.         q4_3 = False
    224.         q4_4 = False
    225.     else:
    226.         q4_3 = np.arctan2(az, ax * np.cos(q1_1) + ay * np.sin(q1_1)) - q3_2 - q2_3
    227.         q4_4 = np.arctan2(az, ax * np.cos(q1_1) + ay * np.sin(q1_1)) - q3_2 - q2_4
    228.         if q4_3 > np.pi:
    229.             q4_3 = q4_3 - np.pi
    230.         elif q4_3 < -np.pi:
    231.             q4_3 = q4_3 + np.pi
    232.         if q4_4 > np.pi:
    233.             q4_4 = q4_4 - np.pi
    234.         elif q4_4 < -np.pi:
    235.             q4_4 = q4_4 + np.pi
    236.     if not q3_3:
    237.         q4_5 = False
    238.         q4_6 = False
    239.     else:
    240.         q4_5 = np.arctan2(az, ax * np.cos(q1_1) + ay * np.sin(q1_1)) - q3_3 - q2_5
    241.         q4_6 = np.arctan2(az, ax * np.cos(q1_1) + ay * np.sin(q1_1)) - q3_3 - q2_6
    242.         if q4_5 > np.pi:
    243.             q4_5 = q4_5 - np.pi
    244.         elif q4_5 < -np.pi:
    245.             q4_5 = q4_5 + np.pi
    246.         if q4_6 > np.pi:
    247.             q4_6 = q4_6 - np.pi
    248.         elif q4_6 < -np.pi:
    249.             q4_6 = q4_6 + np.pi
    250.     if not q3_4:
    251.         q4_7 = False
    252.         q4_8 = False
    253.     else:
    254.         q4_7 = np.arctan2(az, ax * np.cos(q1_1) + ay * np.sin(q1_1)) - q3_4 - q2_7
    255.         q4_8 = np.arctan2(az, ax * np.cos(q1_1) + ay * np.sin(q1_1)) - q3_4 - q2_8
    256.         if q4_7 > np.pi:
    257.             q4_7 = q4_7 - np.pi
    258.         elif q4_7 < -np.pi:
    259.             q4_7 = q4_7 + np.pi
    260.         if q4_8 > np.pi:
    261.             q4_8 = q4_8 - np.pi
    262.         elif q4_8 < -np.pi:
    263.             q4_8 = q4_8 + np.pi
    264.     if not q3_5:
    265.         q4_9 = False
    266.         q4_10 = False
    267.     else:
    268.         q4_9 = np.arctan2(az, ax * np.cos(q1_2) + ay * np.sin(q1_2)) - q3_5 - q2_9
    269.         q4_10 = np.arctan2(az, ax * np.cos(q1_2) + ay * np.sin(q1_2)) - q3_5 - q2_10
    270.         if q4_9 > np.pi:
    271.             q4_9 = q4_9 - np.pi
    272.         elif q4_9 < -np.pi:
    273.             q4_9 = q4_9 + np.pi
    274.         if q4_10 > np.pi:
    275.             q4_10 = q4_10 - np.pi
    276.         elif q4_10 < -np.pi:
    277.             q4_10 = q4_10 + np.pi
    278.     if not q3_6:
    279.         q4_11 = False
    280.         q4_12 = False
    281.     else:
    282.         q4_11 = np.arctan2(az, ax * np.cos(q1_2) + ay * np.sin(q1_2)) - q3_6 - q2_11
    283.         q4_12 = np.arctan2(az, ax * np.cos(q1_2) + ay * np.sin(q1_2)) - q3_6 - q2_12
    284.         if q4_11 > np.pi:
    285.             q4_11 = q4_11 - np.pi
    286.         elif q4_11 < -np.pi:
    287.             q4_11 = q4_11 + np.pi
    288.         if q4_12 > np.pi:
    289.             q4_12 = q4_12 - np.pi
    290.         elif q4_12 < -np.pi:
    291.             q4_12 = q4_12 + np.pi
    292.     if not q3_7:
    293.         q4_13 = False
    294.         q4_14 = False
    295.     else:
    296.         q4_13 = np.arctan2(az, ax * np.cos(q1_2) + ay * np.sin(q1_2)) - q3_7 - q2_13
    297.         q4_14 = np.arctan2(az, ax * np.cos(q1_2) + ay * np.sin(q1_2)) - q3_7 - q2_14
    298.         if q4_13 > np.pi:
    299.             q4_13 = q4_13 - np.pi
    300.         elif q4_13 < -np.pi:
    301.             q4_13 = q4_13 + np.pi
    302.         if q4_14 > np.pi:
    303.             q4_14 = q4_14 - np.pi
    304.         elif q4_14 < -np.pi:
    305.             q4_14 = q4_14 + np.pi
    306.     if not q3_8:
    307.         q4_15 = False
    308.         q4_16 = False
    309.     else:
    310.         q4_15 = np.arctan2(az, ax * np.cos(q1_2) + ay * np.sin(q1_2)) - q3_8 - q2_15
    311.         q4_16 = np.arctan2(az, ax * np.cos(q1_2) + ay * np.sin(q1_2)) - q3_8 - q2_16
    312.         if q4_15 > np.pi:
    313.             q4_15 = q4_15 - np.pi
    314.         elif q4_15 < -np.pi:
    315.             q4_15 = q4_15 + np.pi
    316.         if q4_16 > np.pi:
    317.             q4_16 = q4_16 - np.pi
    318.         elif q4_16 < -np.pi:
    319.             q4_16 = q4_16 + np.pi
    320.  
    321.     ikine_1 = [q1_1, q2_1, q3_1, q4_1, q5_1, q6_1]
    322.     ikine_2 = [q1_1, q2_2, q3_1, q4_2, q5_1, q6_1]
    323.     ikine_3 = [q1_1, q2_3, q3_2, q4_3, q5_1, q6_1]
    324.     ikine_4 = [q1_1, q2_4, q3_2, q4_4, q5_1, q6_1]
    325.     ikine_5 = [q1_1, q2_5, q3_3, q4_5, q5_2, q6_1]
    326.     ikine_6 = [q1_1, q2_6, q3_3, q4_6, q5_2, q6_1]
    327.     ikine_7 = [q1_1, q2_7, q3_4, q4_7, q5_2, q6_1]
    328.     ikine_8 = [q1_1, q2_8, q3_4, q4_8, q5_2, q6_1]
    329.     ikine_9 = [q1_2, q2_9, q3_5, q4_9, q5_3, q6_2]
    330.     ikine_10 = [q1_2, q2_10, q3_5, q4_10, q5_3, q6_2]
    331.     ikine_11 = [q1_2, q2_11, q3_6, q4_11, q5_3, q6_2]
    332.     ikine_12 = [q1_2, q2_12, q3_6, q4_12, q5_3, q6_2]
    333.     ikine_13 = [q1_2, q2_13, q3_7, q4_13, q5_4, q6_2]
    334.     ikine_14 = [q1_2, q2_14, q3_7, q4_14, q5_4, q6_2]
    335.     ikine_15 = [q1_2, q2_15, q3_8, q4_15, q5_4, q6_2]
    336.     ikine_16 = [q1_2, q2_16, q3_8, q4_16, q5_4, q6_2]
    337.  
    338.     q_ikine_list = []
    339.     q_ikine_list_all = [ikine_1,
    340.                         ikine_2,
    341.                         ikine_3,
    342.                         ikine_4,
    343.                         ikine_5,
    344.                         ikine_6,
    345.                         ikine_7,
    346.                         ikine_8,
    347.                         ikine_9,
    348.                         ikine_10,
    349.                         ikine_11,
    350.                         ikine_12,
    351.                         ikine_13,
    352.                         ikine_14,
    353.                         ikine_15,
    354.                         ikine_16]
    355.  
    356.     for one_q_ikinelist in q_ikine_list_all:
    357.         for count, q in enumerate(one_q_ikinelist):
    358.             if not q:
    359.                 one_q_ikinelist = None
    360.                 break
    361.             if q < q_range[count][0]/np.pi*180 or q > q_range[count][1]/np.pi*180:
    362.                 one_q_ikinelist = None
    363.                 print(one_q_ikinelist)
    364.                 break
    365.         if one_q_ikinelist is not None:
    366.             q_ikine_list.append(one_q_ikinelist)
    367.  
    368.     return q_ikine_list

    ……具体的解一共有十六个，但是不是每一个值都是有效的，则需要我们去鉴别！

    但是我也不是很会取舍其中的解析解，我认为从以下几方面考虑：

    奇异解位置、关节角约束、性能约束……

    时间最优、能量最优、路径最优……

  • 相关阅读:
    【数据结构----KMP算法】校招笔试题总结
    【数字信号去噪】小波软阈值+硬阈值+改进阈值数字信号去噪【含Matlab源码 1025期】
    【web-渗透测试方法】(15.4)测试会话管理机制
    为什么不建议库导出c++接口
    数据可视化之大数据平台可视化
    Linux内核互斥技术1
    跨境电商在电商行业中是否还有立足之地？
    理论辐照度计算
    anaconda环境更改gcc版本并编译Pytorch3D 0.4.0
    C++编程题目2
  • 原文地址：https://blog.csdn.net/weixin_44917390/article/details/125438674