devbox
从前慢现在也慢
这个屌丝很懒，什么也没留下！
关注作者
热门标签
热门文章
当前位置:   article > 正文

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

作者：从前慢现在也慢 | 2024-05-04 16:46:14

赞

踩

机械臂逆运动学

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

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

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

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

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

 

下面通过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

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

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

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

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

声明：本文内容由网友自发贡献，不代表【wpsshop博客】立场，版权归原作者所有，本站不承担相应法律责任。如您发现有侵权的内容，请联系我们。转载请注明出处：https://www.wpsshop.cn/w/从前慢现在也慢/article/detail/535324
推荐阅读
相关标签

Copyright © 2003-2013 www.wpsshop.cn 版权所有，并保留所有权利。

  

闽ICP备14008679号