机械臂逆运动学求解常用的方法有几何法、解析法、数值法
从求解的方式和计算的效率上来看，几何法和解析法会考虑机械臂结构不同而造成的差异，因此对于不同结构的机械臂会有特定的求解方式。
通常来说，这两种方法具有速度快、精度高的优点和通用性差、普适性低的缺点。而数值法则通常有相对统一的求解方式，具有适用性好但速度慢、数值稳定性差的特点。
对于工业机器人而言，通常处于特定的工作环境中，为了满足一定的工作性能要求而常采用解析法进行求解。
本文将使用解析法对该型机械臂进行逆运动学的求解。
下面通过Python代码的形式将这些推导的公式表示出来：
# 输入位姿矩阵T
# 例如：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]]))
# 输出解的列表q_list,里面的一个列表就代表一组解
# 例如：q_list=[[q1_1,q21,……],[],[],……]
def ikine_6DOF(T):
# 提取元素
nx = T[0, 0]
ny = T[1, 0]
nz = T[2, 0]
ox = T[0, 1]
oy = T[1, 1]
oz = T[2, 1]
ax = T[0, 2]
ay = T[1, 2]
az = T[2, 2]
px = T[0, 3]
py = T[1, 3]
pz = T[2, 3]
# 求解q1
# 见论文公式(2.23)
q1_1 = 2 * np.arctan2(px - ax * d6 + np.sqrt((px - ax * d6) ** 2 + (-py + ay * d6) ** 2 - d4 ** 2),
d4 - py + ay * d6)
print(q1_1)
if q1_1 < q1_range[0] or q1_1 > q1_range[1]:
q1_1 = False
q1_2 = 2 * np.arctan2(px - ax * d6 - np.sqrt((px - ax * d6) ** 2 + (-py + ay * d6) ** 2 - d4 ** 2),
d4 - py + ay * d6)
if q1_2 < q1_range[0] or q1_2 > q1_range[1]:
q1_2 = False
# 求解q6
# 见论文公式(2.29)
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
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
# 求解q5
# 见论文公式(2.24)
if ay * np.cos(q1_1) - ax * np.sin(q1_1) > 1:
q5_1 = False
q5_2 = False
else:
q5_1 = np.arccos(ay * np.cos(q1_1) - ax * np.sin(q1_1))
q5_2 = -q5_1
if ay * np.cos(q1_2) - ax * np.sin(q1_2) > 1:
q5_3 = False
q5_4 = False
else:
q5_3 = np.arccos(ay * np.cos(q1_2) - ax * np.sin(q1_2))
q5_4 = -q5_3
# 求解q3
# 见论文公式(2.40)
# 如果arccos()中的值大于1，会出现nan的结果，报错：RuntimeWarning: invalid value encountered in arccos
if q5_1 == 0:
m1 = 10000
else:
m1 = px * np.cos(q1_1) + py * np.sin(q1_1) - d5 * az / np.sin(q5_1) - d6 * (
ax * np.cos(q1_1) + ay * np.sin(q1_1))
if q5_2 == 0:
m2 = 10000
else:
m2 = px * np.cos(q1_1) + py * np.sin(q1_1) - d5 * az / np.sin(q5_2) - d6 * (
ax * np.cos(q1_1) + ay * np.sin(q1_1))
if q5_3 == 0:
m3 = 10000
else:
m3 = px * np.cos(q1_2) + py * np.sin(q1_2) - d5 * az / np.sin(q5_3) - d6 * (
ax * np.cos(q1_2) + ay * np.sin(q1_2))
if q5_4 == 0:
m4 = 10000
else:
m4 = px * np.cos(q1_2) + py * np.sin(q1_2) - d5 * az / np.sin(q5_4) - d6 * (
ax * np.cos(q1_2) + ay * np.sin(q1_2))
if q5_1 == 0:
n1 = 10000
else:
n1 = pz - d1 - d6 * az + d5 / np.sin(q5_1) * (ax * np.cos(q1_1) + ay * np.sin(q1_1))
if q5_2 == 0:
n2 = 10000
else:
n2 = pz - d1 - d6 * az + d5 / np.sin(q5_2) * (ax * np.cos(q1_1) + ay * np.sin(q1_1))
if q5_3 == 0:
n3 = 10000
else:
n3 = pz - d1 - d6 * az + d5 / np.sin(q5_3) * (ax * np.cos(q1_2) + ay * np.sin(q1_2))
if q5_4 == 0:
n4 = 10000
else:
n4 = pz - d1 - d6 * az + d5 / np.sin(q5_4) * (ax * np.cos(q1_2) + ay * np.sin(q1_2))
if (m1 ** 2 + n1 ** 2 - a2 ** 2 - a3 ** 2) / (2 * a2 * a3) > 1 or (m1 ** 2 + n1 ** 2 - a2 ** 2 - a3 ** 2) / (
2 * a2 * a3) < -1:
q3_1 = False
q3_2 = False
else:
q3_1 = np.arccos((m1 ** 2 + n1 ** 2 - a2 ** 2 - a3 ** 2) / (2 * a2 * a3))
q3_2 = -q3_1
if (m2 ** 2 + n2 ** 2 - a2 ** 2 - a3 ** 2) / (2 * a2 * a3) > 1 or (m2 ** 2 + n2 ** 2 - a2 ** 2 - a3 ** 2) / (
2 * a2 * a3) < -1:
q3_3 = False
q3_4 = False
else:
q3_3 = np.arccos((m2 ** 2 + n2 ** 2 - a2 ** 2 - a3 ** 2) / (2 * a2 * a3))
q3_4 = -q3_3
if (m3 ** 2 + n3 ** 2 - a2 ** 2 - a3 ** 2) / (2 * a2 * a3) > 1 or (m3 ** 2 + n3 ** 2 - a2 ** 2 - a3 ** 2) / (
2 * a2 * a3) < -1:
q3_5 = False
q3_6 = False
else:
q3_5 = np.arccos((m3 ** 2 + n3 ** 2 - a2 ** 2 - a3 ** 2) / (2 * a2 * a3))
q3_6 = -q3_5
if (m4 ** 2 + n4 ** 2 - a2 ** 2 - a3 ** 2) / (2 * a2 * a3) > 1 or (m4 ** 2 + n4 ** 2 - a2 ** 2 - a3 ** 2) / (
2 * a2 * a3) < -1:
q3_7 = False
q3_8 = False
else:
q3_7 = np.arccos((m4 ** 2 + n4 ** 2 - a2 ** 2 - a3 ** 2) / (2 * a2 * a3))
q3_8 = -q3_7
# 求解q2
# 见论文公式(2.43)
if not q3_1:
q2_1 = False
q2_2 = False
else:
q2_1 = 2 * np.arctan2(
-a3 * np.sin(q3_1) + np.sqrt((-a3 * np.sin(q3_1)) ** 2 + (a2 + a3 * np.cos(q3_1)) ** 2 - m1 ** 2),
a2 + a3 * np.cos(q3_1) + m1)
q2_2 = 2 * np.arctan2(
-a3 * np.sin(q3_1) - np.sqrt((-a3 * np.sin(q3_1)) ** 2 + (a2 + a3 * np.cos(q3_1)) ** 2 - m1 ** 2),
a2 + a3 * np.cos(q3_1) + m1)
if not q3_2:
q2_3 = False
q2_4 = False
else:
q2_3 = 2 * np.arctan2(
-a3 * np.sin(q3_2) + np.sqrt((-a3 * np.sin(q3_2)) ** 2 + (a2 + a3 * np.cos(q3_2)) ** 2 - m1 ** 2),
a2 + a3 * np.cos(q3_2) + m1)
q2_4 = 2 * np.arctan2(
-a3 * np.sin(q3_2) - np.sqrt((-a3 * np.sin(q3_2)) ** 2 + (a2 + a3 * np.cos(q3_2)) ** 2 - m1 ** 2),
a2 + a3 * np.cos(q3_2) + m1)
if not q3_3:
q2_5 = False
q2_6 = False
else:
q2_5 = 2 * np.arctan2(
-a3 * np.sin(q3_3) + np.sqrt((-a3 * np.sin(q3_3)) ** 2 + (a2 + a3 * np.cos(q3_3)) ** 2 - m2 ** 2),
a2 + a3 * np.cos(q3_3) + m2)
q2_6 = 2 * np.arctan2(
-a3 * np.sin(q3_3) - np.sqrt((-a3 * np.sin(q3_3)) ** 2 + (a2 + a3 * np.cos(q3_3)) ** 2 - m2 ** 2),
a2 + a3 * np.cos(q3_3) + m2)
if not q3_4:
q2_7 = False
q2_8 = False
else:
q2_7 = 2 * np.arctan2(
-a3 * np.sin(q3_4) + np.sqrt((-a3 * np.sin(q3_4)) ** 2 + (a2 + a3 * np.cos(q3_4)) ** 2 - m2 ** 2),
a2 + a3 * np.cos(q3_4) + m2)
q2_8 = 2 * np.arctan2(
-a3 * np.sin(q3_4) - np.sqrt((-a3 * np.sin(q3_4)) ** 2 + (a2 + a3 * np.cos(q3_4)) ** 2 - m2 ** 2),
a2 + a3 * np.cos(q3_4) + m2)
if not q3_5:
q2_9 = False
q2_10 = False
else:
q2_9 = 2 * np.arctan2(
-a3 * np.sin(q3_5) + np.sqrt((-a3 * np.sin(q3_5)) ** 2 + (a2 + a3 * np.cos(q3_5)) ** 2 - m3 ** 2),
a2 + a3 * np.cos(q3_5) + m3)
q2_10 = 2 * np.arctan2(
-a3 * np.sin(q3_5) - np.sqrt((-a3 * np.sin(q3_5)) ** 2 + (a2 + a3 * np.cos(q3_5)) ** 2 - m3 ** 2),
a2 + a3 * np.cos(q3_5) + m3)
if not q3_6:
q2_11 = False
q2_12 = False
else:
q2_11 = 2 * np.arctan2(
-a3 * np.sin(q3_6) + np.sqrt((-a3 * np.sin(q3_6)) ** 2 + (a2 + a3 * np.cos(q3_6)) ** 2 - m3 ** 2),
a2 + a3 * np.cos(q3_6) + m3)
q2_12 = 2 * np.arctan2(
-a3 * np.sin(q3_6) - np.sqrt((-a3 * np.sin(q3_6)) ** 2 + (a2 + a3 * np.cos(q3_6)) ** 2 - m3 ** 2),
a2 + a3 * np.cos(q3_6) + m3)
if not q3_7:
q2_13 = False
q2_14 = False
else:
q2_13 = 2 * np.arctan2(
-a3 * np.sin(q3_7) + np.sqrt((-a3 * np.sin(q3_7)) ** 2 + (a2 + a3 * np.cos(q3_7)) ** 2 - m4 ** 2),
a2 + a3 * np.cos(q3_7) + m4)
q2_14 = 2 * np.arctan2(
-a3 * np.sin(q3_7) - np.sqrt((-a3 * np.sin(q3_7)) ** 2 + (a2 + a3 * np.cos(q3_7)) ** 2 - m4 ** 2),
a2 + a3 * np.cos(q3_7) + m4)
if not q3_8:
q2_15 = False
q2_16 = False
else:
q2_15 = 2 * np.arctan2(
-a3 * np.sin(q3_8) + np.sqrt((-a3 * np.sin(q3_8)) ** 2 + (a2 + a3 * np.cos(q3_8)) ** 2 - m4 ** 2),
a2 + a3 * np.cos(q3_8) + m4)
q2_16 = 2 * np.arctan2(
-a3 * np.sin(q3_8) - np.sqrt((-a3 * np.sin(q3_8)) ** 2 + (a2 + a3 * np.cos(q3_8)) ** 2 - m4 ** 2),
a2 + a3 * np.cos(q3_8) + m4)
# 求解q4
# 见论文公式(2.46)
if not q3_1:
q4_1 = False
q4_2 = False
else:
q4_1 = np.arctan2(az, ax * np.cos(q1_1) + ay * np.sin(q1_1)) - q3_1 - q2_1
q4_2 = np.arctan2(az, ax * np.cos(q1_1) + ay * np.sin(q1_1)) - q3_1 - q2_2
if q4_1 > np.pi:
q4_1 = q4_1 - np.pi
elif q4_1 < -np.pi:
q4_1 = q4_1 + np.pi
if q4_2 > np.pi:
q4_2 = q4_2 - np.pi
elif q4_2 < -np.pi:
q4_2 = q4_2 + np.pi
if not q3_2:
q4_3 = False
q4_4 = False
else:
q4_3 = np.arctan2(az, ax * np.cos(q1_1) + ay * np.sin(q1_1)) - q3_2 - q2_3
q4_4 = np.arctan2(az, ax * np.cos(q1_1) + ay * np.sin(q1_1)) - q3_2 - q2_4
if q4_3 > np.pi:
q4_3 = q4_3 - np.pi
elif q4_3 < -np.pi:
q4_3 = q4_3 + np.pi
if q4_4 > np.pi:
q4_4 = q4_4 - np.pi
elif q4_4 < -np.pi:
q4_4 = q4_4 + np.pi
if not q3_3:
q4_5 = False
q4_6 = False
else:
q4_5 = np.arctan2(az, ax * np.cos(q1_1) + ay * np.sin(q1_1)) - q3_3 - q2_5
q4_6 = np.arctan2(az, ax * np.cos(q1_1) + ay * np.sin(q1_1)) - q3_3 - q2_6
if q4_5 > np.pi:
q4_5 = q4_5 - np.pi
elif q4_5 < -np.pi:
q4_5 = q4_5 + np.pi
if q4_6 > np.pi:
q4_6 = q4_6 - np.pi
elif q4_6 < -np.pi:
q4_6 = q4_6 + np.pi
if not q3_4:
q4_7 = False
q4_8 = False
else:
q4_7 = np.arctan2(az, ax * np.cos(q1_1) + ay * np.sin(q1_1)) - q3_4 - q2_7
q4_8 = np.arctan2(az, ax * np.cos(q1_1) + ay * np.sin(q1_1)) - q3_4 - q2_8
if q4_7 > np.pi:
q4_7 = q4_7 - np.pi
elif q4_7 < -np.pi:
q4_7 = q4_7 + np.pi
if q4_8 > np.pi:
q4_8 = q4_8 - np.pi
elif q4_8 < -np.pi:
q4_8 = q4_8 + np.pi
if not q3_5:
q4_9 = False
q4_10 = False
else:
q4_9 = np.arctan2(az, ax * np.cos(q1_2) + ay * np.sin(q1_2)) - q3_5 - q2_9
q4_10 = np.arctan2(az, ax * np.cos(q1_2) + ay * np.sin(q1_2)) - q3_5 - q2_10
if q4_9 > np.pi:
q4_9 = q4_9 - np.pi
elif q4_9 < -np.pi:
q4_9 = q4_9 + np.pi
if q4_10 > np.pi:
q4_10 = q4_10 - np.pi
elif q4_10 < -np.pi:
q4_10 = q4_10 + np.pi
if not q3_6:
q4_11 = False
q4_12 = False
else:
q4_11 = np.arctan2(az, ax * np.cos(q1_2) + ay * np.sin(q1_2)) - q3_6 - q2_11
q4_12 = np.arctan2(az, ax * np.cos(q1_2) + ay * np.sin(q1_2)) - q3_6 - q2_12
if q4_11 > np.pi:
q4_11 = q4_11 - np.pi
elif q4_11 < -np.pi:
q4_11 = q4_11 + np.pi
if q4_12 > np.pi:
q4_12 = q4_12 - np.pi
elif q4_12 < -np.pi:
q4_12 = q4_12 + np.pi
if not q3_7:
q4_13 = False
q4_14 = False
else:
q4_13 = np.arctan2(az, ax * np.cos(q1_2) + ay * np.sin(q1_2)) - q3_7 - q2_13
q4_14 = np.arctan2(az, ax * np.cos(q1_2) + ay * np.sin(q1_2)) - q3_7 - q2_14
if q4_13 > np.pi:
q4_13 = q4_13 - np.pi
elif q4_13 < -np.pi:
q4_13 = q4_13 + np.pi
if q4_14 > np.pi:
q4_14 = q4_14 - np.pi
elif q4_14 < -np.pi:
q4_14 = q4_14 + np.pi
if not q3_8:
q4_15 = False
q4_16 = False
else:
q4_15 = np.arctan2(az, ax * np.cos(q1_2) + ay * np.sin(q1_2)) - q3_8 - q2_15
q4_16 = np.arctan2(az, ax * np.cos(q1_2) + ay * np.sin(q1_2)) - q3_8 - q2_16
if q4_15 > np.pi:
q4_15 = q4_15 - np.pi
elif q4_15 < -np.pi:
q4_15 = q4_15 + np.pi
if q4_16 > np.pi:
q4_16 = q4_16 - np.pi
elif q4_16 < -np.pi:
q4_16 = q4_16 + np.pi
ikine_1 = [q1_1, q2_1, q3_1, q4_1, q5_1, q6_1]
ikine_2 = [q1_1, q2_2, q3_1, q4_2, q5_1, q6_1]
ikine_3 = [q1_1, q2_3, q3_2, q4_3, q5_1, q6_1]
ikine_4 = [q1_1, q2_4, q3_2, q4_4, q5_1, q6_1]
ikine_5 = [q1_1, q2_5, q3_3, q4_5, q5_2, q6_1]
ikine_6 = [q1_1, q2_6, q3_3, q4_6, q5_2, q6_1]
ikine_7 = [q1_1, q2_7, q3_4, q4_7, q5_2, q6_1]
ikine_8 = [q1_1, q2_8, q3_4, q4_8, q5_2, q6_1]
ikine_9 = [q1_2, q2_9, q3_5, q4_9, q5_3, q6_2]
ikine_10 = [q1_2, q2_10, q3_5, q4_10, q5_3, q6_2]
ikine_11 = [q1_2, q2_11, q3_6, q4_11, q5_3, q6_2]
ikine_12 = [q1_2, q2_12, q3_6, q4_12, q5_3, q6_2]
ikine_13 = [q1_2, q2_13, q3_7, q4_13, q5_4, q6_2]
ikine_14 = [q1_2, q2_14, q3_7, q4_14, q5_4, q6_2]
ikine_15 = [q1_2, q2_15, q3_8, q4_15, q5_4, q6_2]
ikine_16 = [q1_2, q2_16, q3_8, q4_16, q5_4, q6_2]
q_ikine_list = []
q_ikine_list_all = [ikine_1,
ikine_2,
ikine_3,
ikine_4,
ikine_5,
ikine_6,
ikine_7,
ikine_8,
ikine_9,
ikine_10,
ikine_11,
ikine_12,
ikine_13,
ikine_14,
ikine_15,
ikine_16]
for one_q_ikinelist in q_ikine_list_all:
for count, q in enumerate(one_q_ikinelist):
if not q:
one_q_ikinelist = None
break
if q < q_range[count][0]/np.pi*180 or q > q_range[count][1]/np.pi*180:
one_q_ikinelist = None
print(one_q_ikinelist)
break
if one_q_ikinelist is not None:
q_ikine_list.append(one_q_ikinelist)
return q_ikine_list
……具体的解一共有十六个，但是不是每一个值都是有效的，则需要我们去鉴别！
但是我也不是很会取舍其中的解析解，我认为从以下几方面考虑：
奇异解位置、关节角约束、性能约束……
时间最优、能量最优、路径最优……
3029
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
