四轴机械臂算法详解

我终终终终于来了，真的好久没更新了
首先给大家看看我们最近做的一个四轴机械臂项目吧
这是它已经拼装在基础底盘上的实物图了
可以看到一共有四个马达来驱动这个机械臂

其中最上端与最底部的马达可以独立控制
因此算法的核心在于中间的两节机械臂

如图为最核心的算法模型
其中a,b分别代表两个机械臂
ε与δ由马达控制器自动设置
因此我们的目标就是求出ε和δ的值
X和Y由遥控手柄实时传入(可以为增量或者绝对位置)
c=sqrt(X²+Y²)
γ=atan(Y/X)
在三角形ABC中有两个直角三角形
a²-AD²=k²=b²-BD²
并且AD+BD=c得到
AD=(a²-b²+c²)/(2*c)
BD=c-AD
α=acos(AD/a)
β=acos(BD/b)

最终得到
ε=90°-α-γ
δ=180°-α-β

然后将角度换算成马达控制器的position
传入pid控制器中即可