grbl控制3轴机械臂 原理 实现 （二） 之3D机械臂模拟及实现

参考文章:https://www.alanzucconi.com/2020/09/14/inverse-kinematics-in-3d/

上一篇:grbl控制3轴机械臂 原理 实现 （一） 之2D机械臂模拟及实现

3轴机械臂几何分析

    到目前为止，我们知道了如何在XY平面约束的情况下解决正逆运动学问题。从2D到3D的诀窍是重新使用2D解决方案。我们所介绍的这种3轴机械臂，一共3个轴，控制L1臂一个轴，控制L2臂一个轴，底座将L1和L2臂整体旋转为一个轴。很明显我们就多了一个整体旋转的维度。并且底座的旋转不会改变L1和L2臂的角度。

我们可以通过三个步骤解决问题：

1. 绕Y轴旋转目标点，直到它位于XY平面上
2. 移动机械臂L1和L2臂，使其到达目标该点，就像在2D模式下一样
3. 通过沿Y轴的相反方向旋转整个机械臂来“撤消”旋转。

以上三个步骤逆解举例：

假设我们最终要得到的目标点是下图红色点，那么我们就要求得大臂旋转角度，小臂旋转角度，两臂在Y轴旋转角度

根据步骤一，绕Y轴旋转到X，Y平面上如下图，这样我们就能像处理2D机械臂一样

步骤二，在此平面上处理得到的L1，L2臂角度

步骤三，得到L1，L2臂的角度后，恢复Y轴的旋转，并求得Y轴的旋转角度

根据的值，求得紫色的线与X轴的夹角，这个夹角就是Y轴的旋转角度

至此，3个角度都求出来了。

实际上，旋转Y轴是为了方便理解，实际应用中，我们可以认为目标点所处于平面(W,Y)上

在这个(W,Y)平面上，直接按2D机械臂方式来算出L1，L2臂的角度即可，Y轴旋转角度计算照旧。

3D运动学逆解（Inverse kinematics）

我们把2d机械臂的图画到3d里面，可见原来我们的X边，变成了W边

根据坐标轴得知W边在（X，Z）平面，根据X边，W边，Z边是直角三角形，通过公式     求出W的长度

代入值得

     得     

再根据W边和Y边求出原点到（X,Y,Z）的距离为A边，W边，Y边，A边是直角三角形，依旧通过公式 

代入值得

     得        代入上面W得