[易懂]两连杆关节机械臂机器人给定位置求解各关节转动角度教程模拟Python实现

两连杆关节机械臂机器人给定位置求解各关节转动角度教程模拟Python实现

github代码地址：https://github.com/varyshare/easy_slam_tutorial/tree/master/joint_robot_simulation

我们要解决的问题是已知一个目标点坐标(x,y)，已知两个连杆的长度a1,a2，我们的目标是求q1,q2这两个关节角.如下图所示：

因为已知坐标(x,y)即我们已知下图中的三角形的两个直角边。根据勾股定理可以得到斜边的长度为$r=\sqrt{x^2+y^2}$.

因此下面这个三角形所有的边都是已知的了。

高中的几何学告诉我们三条边已知的话那就可以根据余弦定理求出一个角。因此我们是计划把那个大角$\alpha$求出来。为什么？因为求出$\alpha$那么我们就可以求出关节角q2，因为它们是互为补角。$q2=180-\alpha$。现在我们已经求出一个关节角了。

现在我们知道了角度q2.而且知道第2个杆的长度a2. 因此我们可以解出下面这个三角形的两条边.

于是乎，我们现在已知下面这个直角三角形的两条直角边。根据反切公式可以求出$\beta$这个锐角。为什么要求$\beta$这个锐角？请看后面的分析。

下面这个图的三角形的两个直角边就是目标点的横坐标和纵坐标x,y。那么我们是可以求出边y对着的那个角，并且 β \beta