[发明专利]一种基于舵机驱动的四轴机械臂的运动求解算法

摘要

本发明公开了一种基于舵机驱动的四轴机械臂的运动求解算法，包括正向求解和反向求解：所述的正向求解是已知机械臂工具端要运动到目标位置Lxyz＝[Lx，Ly，Lz]，驱动舵机转动，使得机械臂工具端运动到目标位置，要使机械臂运动到目标位置[Lx，Ly，Lz]，首先需要调用求解模块计算出机械的关节角度[θ1，θ2，θ3]，再调用映射模块算出舵机的位置刻度[k1、k2、k3]，最后调用执行模块让舵机转到指定的刻度即可。所述的反向求解是求解机械臂工具端所在位置。运动求解方法利用了四轴机械臂本身的结构几何信息。相比传统的矩阵变换计算，提出的方法更加简单，求解更加高效，能快速搭建模型验证机械臂结构的问题。

主权项

1.一种基于舵机驱动的四轴机械臂的运动求解算法，其特征是，包括正向求解和反向求解：所述的正向求解是已知机械臂工具端要运动到目标位置Lxyz＝[Lx，Ly，Lz]，驱动舵机转动，使得机械臂工具端运动到目标位置，具体步骤为：步骤一、调用求解模块计算出机械的关节角度[θ1，θ2，θ3]；θ1＝arctan(tan(θ1))其中角度θ1，θ2，θ3与β，γ的关系为θ2＝β‑π/2，θ3＝π‑γ‑θ2；杆件D0、LA、LB、LC、D2、D1都在同一平面上，因此可求出角度θ1：求得节点B的坐标为Bxyz：求得节点G的坐标为Gxyz：节点I与节点L存在如下关系：则获得节点I的坐标：有节点坐标Bxyz、Gxyz、Ixyz可以求得LGI、LBI，如下所示：由三角形△GHI可以求得角度：γ和∠HGI，结果如下：由三角形△GHI可以求得角度：∠BGI，结果如下：最终可得β为：β＝∠HGI+∠BGI；步骤二、根据关节角[θ1，θ2，θ3]和舵机刻度的映射关系求解出相应舵机的刻度k1、k2、k3；步骤三、将相应舵机的刻度k1、k2、k3通过执行模块分别赋值给相应的舵机，机械臂工具端运动到目标位置Lxyz＝[Lx，Ly，Lz]；所述的反向求解是求解机械臂工具端所在位置，具体步骤为：步骤一、调用执行模块获取相应舵机的当前刻度k1、k2、k3；步骤二、根据关节角[θ1，θ2，θ3]和舵机刻度的映射关系求解出相应舵机的[θ1，θ2，θ3]；步骤三、根据反向求解步骤二所得的关节角[θ1，θ2，θ3]求解机械臂工具端所在位置Lxyz＝[Lx，Ly，Lz]，其中角度θ1，θ2，θ3与β，γ的关系为θ2＝β‑π/2，θ3＝π‑γ‑θ2；在□GBIH中，我们令节点B的坐标为[Bxy＝D0，Bz＝0]，由此可得节点Gxy的坐标：由几何关系可知，节点H在平面□GBIH中的坐标为：节点I在平面□GBIH中的坐标为：最终，节点L在平面□GBIH的坐标为：由于平面□GBIH是绕旋转中轴旋转了θ1角度，所以，可以用如下公式把节点L扩充为：可得机械臂工具端所在位置Lxyz＝[Lx，Ly，Lz]。