UR3机械臂控制——几何法
为使用几何法求机械臂的关节角度,需要先知道机械臂的各个臂的长度和初始坐标。下面为机械臂的3D图,和注明的点的坐标。
由于使用几何法求6个关节角度计算相对复杂,现在固定关节5的角度为90度,即这个末端一直是朝前的。下面是一个侧面图:
关节计算
计算I点和F点的对应关系:
实际应用时的目标坐标应以机械臂末端坐标为准,但以末端坐标来计算复杂,改为使用F点的坐标来进行关节计算,即将F点作为目标位置,下面计算I点和F点间的对应关系, 将坐标系分为4部分,分别计算,计算过程如下图:
求上图中的 θ \theta θ 角:
下图是 θ \theta θ 角的求取过程
下图是上面第4行到第5行的运算过程,你会发现下面的结果求出来会有两个结果 ,实际只有一个可用,用的是 a r c t a n 2 ( B , A ) − a r c t a n 2 ( C , s q r t ( A ∗ A + B ∗ B − C ∗ C ) ) arctan2(B,A) - arctan2(C, sqrt(A*A+B*B-C*C)) arctan2(B,A)−arctan2(C,sqrt(A∗A+B∗B−C∗C))
得到F点坐标后根据几何关系计算:
在BEF三角形中使用余弦定理,可以解出BEF,ABE角度。这里的角Joint1也可以用上面求出来的
θ
\theta
θ ,结果是一样的。
考虑EFH角:
EFH的角度在下面的计算中会进行微调,使得杠HI始终和地面保持水平。
关节角度计算
Joint2 应该是 -90° -(theta_ABE - 90°);Joint3 应该是 -(180° - theta_BEF);Joint1 和 Joint4 在上面已经给出。
取了目标点,根据目标点计算其他点坐标,根据求的坐标画的3D图:
根据结果给的关节在机械臂上的验证:
阴影部分的图像为结果图。
末端位置的验证:
输入坐标为 x = 300mm, y = -200mm, z = 126.22mm;刚开始得到关节角度后代到正运动得到的坐标和输入坐标存在偏差,通过修正后的最后的坐标和初始坐标的差距得到了很好的弥补
代码
https://download.csdn.net/download/qq_42236622/55382600
我用于修正坐标的方法是正运动输入得到的关节后得到坐标,和原来的输入坐标对比修正,修正后结果很明显。
正运动代码:https://blog.csdn.net/qq_42236622/article/details/121711775