Algorithm——逆解算三轴机械臂运动公式得到运动坐标

三轴机械臂

博主是在淘宝上买的这个机械臂，这个机械臂应该是6轴的，但是实际上只有3个轴是最核心的，是建立坐标系所需的轴，其余的顶端三个轴都是一些附加的机械爪控制轴，就不是核心机械臂运动的轴。所以只用3个轴便可以完成机械臂在三维坐标系中的运动，三个轴如图所示。

三轴机械臂的控制原理及运动方程求解

因为直接上三轴立体坐标系会稍微有些复杂，所以先从垂直坐标系看，再加上一个轴就会简单很多。

垂直坐标系

在垂直坐标系中，机械臂实际只用到了轴1和轴2，可以保证机械臂到达垂直平面内的点，所以我们先建立锤子坐标系，进行列写运动公式，进行求解。

建立坐标系

机械臂的顶部所到达的点（x2,y2）为我们所求的点，θ1和θ2分别为机械臂轴2和轴1对应舵机转动的角度。（x1,y1）为轴2对应的点的坐标，（△x2,△y2）为以（x1,y1）为原点的当前坐标。

列写运动方程

根据坐标系列写运动方程：

求解θ1和θ2

真就列方程2分钟，解方程2小时。最终手算出表达式如图所示：

所求的θ1和θ2即为我们需要的舵机转动的角度。

三轴立体空间

只需直接加上Z轴即可实现到达空间中任意一点了~Z轴不需要进行解算，不影响机械臂垂直平面内的运动，只需控制舵机到达运动对应角度即可。

单片机控制

使用PYB的小伙伴可以参考我的博客，实现机械臂的运动~

Micropython——基于单片机的三轴机械臂的控制

如果觉得好，就给苦逼解方程的博主留个赞吧~