UR3机械臂控制——几何法

为使用几何法求机械臂的关节角度，需要先知道机械臂的各个臂的长度和初始坐标。下面为机械臂的3D图，和注明的点的坐标。

由于使用几何法求6个关节角度计算相对复杂，现在固定关节5的角度为90度，即这个末端一直是朝前的。下面是一个侧面图：

关节计算

计算I点和F点的对应关系：

实际应用时的目标坐标应以机械臂末端坐标为准，但以末端坐标来计算复杂，改为使用F点的坐标来进行关节计算，即将F点作为目标位置，下面计算I点和F点间的对应关系, 将坐标系分为4部分，分别计算，计算过程如下图：

求上图中的 $\theta$ 角：

下图是 $\theta$ 角的求取过程

下图是上面第4行到第5行的运算过程，你会发现下面的结果求出来会有两个结果 ，实际只有一个可用，用的是 $arctan2(B,A)-arctan2(C,sqrt(A\ast A+B\ast B-C\ast 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

UR3控制TPC的python3.8代码、因时灵巧手（Insprie-robots）串口通信