CN109531574B - 一种基于舵机驱动的四轴机械臂的运动求解算法 - Google Patents

Abstract

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

Description

一种基于舵机驱动的四轴机械臂的运动求解算法

技术领域

本发明涉及机器人舵机驱动机械臂运动求解技术领域，具体地说是一种基于舵机驱动的四轴机械臂的运动求解算法。

背景技术

机械臂的运动求解一般采用传统的矩阵变换计算，但是传统的矩阵变换计算过于复杂，计算量较大，对机械臂的控制系统的计算内存占用较大，耗电较高。

现需要一种利用了机械臂本身的结构几何信息，高效，能快速搭建模型验证机械臂结构的问题。

发明内容

本发明就是为了解决现有技术存在的上述不足，提供一种基于舵机驱动的四轴机械臂的运动求解算法。本发明提出的运动求解方法利用了四轴机械臂本身的结构几何信息。相比传统的矩阵变换计算，本文提出的方法更加简单，求解更加高效，能快速搭建模型验证机械臂结构的问题。本发明主要由三部分组成：1.映射模块、2.求解模块、3.执行模块。其中，映射模块主要完成关节空间与舵机刻度空间的映射；求解模块主要解决了空间坐标到关节角度的映射；执行模块主要解决了机械臂如何准确运动到设定的坐标位置。

本发明解决其技术问题所采取的技术方案是：

一种基于舵机驱动的四轴机械臂的运动求解算法，包括正向求解和反向求解：

所述的正向求解是已知机械臂工具端要运动到目标位置L_xyz＝[L_x，L_y，L_z]，驱动舵机转动，使得机械臂工具端运动到目标位置，要使机械臂运动到目标位置[L_x，L_y，L_z]，首先需要调用求解模块计算出机械的关节角度[θ₁，θ₂，θ₃]，再调用映射模块算出舵机的位置刻度[k₁、k₂、k₃]，最后调用执行模块让舵机转到指定的刻度即可，具体步骤为：

步骤一、调用求解模块计算出机械的关节角度[θ₁，θ₂，θ₃]；

其中角度θ₁，θ₂，θ₃与β，γ的关系为θ₂＝β-π/2，θ₃＝π-γ-θ₂；θ₁为附图3 中的∠BAD

杆件间距D₀、L_A、L_B、L_C、D₂、D₁都在同一平面上，因此可求出角度θ₁：

θ₁＝arctan(tan(θ₁))

求得节点B的坐标为B_xyz：

求得节点G的坐标为G_xyz：

节点I与节点L存在如下关系：

则获得节点I的坐标：

有节点坐标B_xyz、G_xyz、I_xyz可以求得L_GI、L_BI，如下所示：

由三角形△GHI可以求得角度：γ和∠HGI，结果如下：

由三角形△GBI可以求得角度：∠BGI，结果如下：

最终可得β为：β＝∠HGI+∠BGI；

步骤二、根据关节角[θ₁，θ₂，θ₃]和舵机刻度的映射关系求解出相应舵机的刻度k₁、k₂、k₃；

步骤三、将相应舵机的刻度k₁、k₂、k₃通过执行模块分别赋值给相应的舵机，机械臂工具端运动到目标位置L_xyz＝[L_x，L_y，L_z]。

总之已知机械臂工具端要运动到目标位置L_xyz＝[L_x，L_y，L_z]，驱动舵机转动，使得机械臂工具端运动到目标位置的流程是：

已知工具端坐标L,求出旋转轴角度θ₁；

已知中轴旋转角度θ₁,求出节点B，G，I坐标；

已知节点G,I，求出长度L_GI；

已知三角形△GHI的各边长，求出角度∠HGI，即γ；

已知三角形△GHI和△GBI的各边长，求出角度∠HGI，∠BGI,即

β＝∠HGI+∠BGI；

由γ和β以及映射模块，可以求得角度θ₂，θ₃；

有映射模块，可以求得舵机刻度为k₁、k₂、k₃；

所述的反向求解是求解机械臂工具端所在位置，具体步骤为：

步骤一、调用执行模块获取相应舵机的当前刻度k₁、k₂、k₃；

步骤二、根据关节角[θ₁，θ₂，θ₃]和舵机刻度的映射关系求解出相应舵机的 [θ₁，θ₂，θ₃]；

步骤三、根据反向求解步骤二所得的关节角[θ₁，θ₂，θ₃]求解机械臂工具端所在位置L_xyz＝[L_x，L_y，L_z]，其中角度θ₁，θ₂，θ₃与β，γ的关系为θ₂＝β-π/2，θ₃＝π-γ-θ₂；

在四边形GBIH中，我们令节点B的坐标为[B_xy＝D₀，B_z＝0]，由此可得节点G_xy的坐标：

由几何关系可知，节点H在平面四边形GBIH中的坐标为：

节点I在平面四边形GBIH中的坐标为：

最终，节点L在平面四边形GBIH的坐标为：

由于平面四边形GBIH是绕旋转中轴旋转了θ₁角度，所以，可以用如下公式把节点L扩充为：

可得机械臂工具端所在位置L_xyz＝[L_x，L_y，L_z]。

总之，求解机械臂工具端所在位置的流程是：

获取相应舵机的当前刻度k₁、k₂、k₃，通过映射模块转换为角度值[θ₁，θ₂，θ₃]，把角度[θ₁，θ₂，θ₃]转化为角度θ₁，β，γ；

已知旋转轴角度θ₁,求节点坐标B；

已知节点B坐标和杆长L_A,求出节点G坐标；

已知节点G坐标、杆长L_B和角度β,求出节点H坐标；

已知节点H坐标、杆长L_C和角度γ,求出节点I坐标；

已知节点I坐标，杆长D₂，杆长D₁，求出工具端坐标L；

所述关节角[θ₁，θ₂，θ₃]和舵机刻度[k₁、k₂、k₃]的映射关系为：

θ₁＝(k₁-KBase₁)·KUnit·Ratio₁-QBase₁

θ₂＝(k₂-KBase₂)·KUnit·Ratio₂+QBase₂

θ₃＝(k₃-KBase₃)·KUnit·Ratio₃-QBase₃

其中KBase₁、KBase₂、KBase₃是相应关节的基准刻度，KUnit是舵机一个刻度对应的角度范围，Ratio₁、Ratio₂、Ratio₃是相应关节同步轮比例，QBase₁、 QBase₂、QBase₃是相应关节的基准角度。

映射模块求解过程：

固定机械臂到指定姿态(指定姿态是可以改变的，这样计算基础会改变，在运动中计算相应的机械臂末端计也随之改变)，如附图1所示：

调用执行模块读取舵机的当前刻度[k₁、k₂、k₃]，并设置为基准刻度KBase₁、KBase₂、KBase₃；

读取机械臂此时的关节角度[θ₁，θ₂，θ₃]，并设置为基准角度QBase₁、QBase₂、QBase₃；

记录同步带轮的减速比Ratio₁、Ratio₂、Ratio₃；

记录舵机一个刻度对应的角度范围KUnit(舵机的性能参数，表示舵机一个刻度对应的角度范围)；

建立关节角度[θ₁，θ₂，θ₃]与舵机刻度[k₁、k₂、k₃]之间的映射关系；

建立角度θ₂，θ₃与β，γ的关系。

上面所说的“指定姿态”是指校准状态，即机械臂末端运动计算的坐标零点，机械臂末端运动时的计算基准点。

本发明的有益效果是：

1、本发明提出的运动求解方法利用了四轴机械臂本身的结构几何信息。相比传统的矩阵变换计算，本文提出的方法更加简单，求解更加高效，能快速搭建模型验证机械臂结构的问题。本发明主要由三部分组成：1.映射模块、2.求解模块、3.执行模块。其中，映射模块主要完成关节空间与舵机刻度空间的映射；求解模块主要解决了空间坐标到关节角度的映射；执行模块主要解决了机械臂如何准确运动到设定的坐标位置。

附图说明

图1为本发明映射求解过程中机械臂指定姿态视图；

图2为本发明机械臂任意角度视图；

图3为本发明求解简图；

图4为本发明正向求解和反向求解流程图。

本发明所使用的长度(D₀、L_A、L_B等)、点(B、G等)即为图中所标注的参数和位置。

具体实施方式

为了能清楚说明本方案的技术特点，下面通过具体实施方式，并结合其附图，对本发明进行详细阐述。下文的公开提供了许多不同的实施例或例子用来实现本发明的不同结构。为了简化本发明的公开，下文中对特定例子的部件和设置进行描述。此外，本发明可以在不同例子中重复参考数字和/或字母。这种重复是为了简化和清楚的目的，其本身不指示所讨论各种实施例和/或设置之间的关系。应当注意，在附图中所图示的部件不一定按比例绘制。本发明省略了对公知组件和处理技术及工艺的描述以避免不必要地限制本发明。术语“中心”、“上”、“下”、“左”、“右”、“竖直”、“水平”、“内”、“外”等指示的方位或位置关系为基于附图所示的方位或位置关系，仅是为了便于描述本发明和简化描述，而不是指示或暗示所指的装置或元件必须具有特定的方位、以特定的方位构造和操作，因此不能理解为对本发明的限制。此外，术语“第一”、“第二”、“第三”仅用于描述目的，而不能理解为指示或暗示相对重要性。在本发明的描述中，需要说明的是，除非另有明确的规定和限定，术语“安装”、“相连”、“连接”应做广义理解，例如，可以是固定连接，也可以是可拆卸连接，或一体地连接；可以是机械连接，也可以是电连接；可以是直接相连，也可以通过中间媒介间接相连，可以是两个元件内部的连通。对于本领域的普通技术人员而言，可以具体情况理解上述术语在本发明中的具体含义。

一种基于舵机驱动的四轴机械臂的运动求解算法，包括正向求解和反向求解：

所述的正向求解是已知机械臂工具端要运动到目标位置L_xyz＝[L_x，L_y，L_z]，驱动舵机转动，使得机械臂工具端运动到目标位置，要使机械臂运动到目标位置[L_x，L_y，L_z]，首先需要调用求解模块计算出机械的关节角度[θ₁，θ₂，θ₃]，再调用映射模块算出舵机的位置刻度[k₁、k₂、k₃]，最后调用执行模块让舵机转到指定的刻度即可，具体步骤为：

步骤一、调用求解模块计算出机械的关节角度[θ₁，θ₂，θ₃]；

其中角度θ₁，θ₂，θ₃与β，γ的关系为θ₂＝β-π/2，θ₃＝π-γ-θ₂；θ₁为附图3 中的∠BAD

杆件间距D₀、L_A、L_B、L_C、D₂、D₁都在同一平面上，因此可求出角度θ₁：

θ₁＝arctan(tan(θ₁))

求得节点B的坐标为B_xyz：

求得节点G的坐标为G_xyz：

节点I与节点L存在如下关系：

则获得节点I的坐标：

有节点坐标B_xyz、G_xyz、I_xyz可以求得L_GI、L_BI，如下所示：

由三角形△GHI可以求得角度：γ和∠HGI，结果如下：

由三角形△GBI可以求得角度：∠BGI，结果如下：

最终可得β为：β＝∠HGI+∠BGI；

步骤二、根据关节角[θ₁，θ₂，θ₃]和舵机刻度的映射关系求解出相应舵机的刻度k₁、k₂、k₃；

步骤三、将相应舵机的刻度k₁、k₂、k₃通过执行模块分别赋值给相应的舵机，机械臂工具端运动到目标位置L_xyz＝[L_x，L_y，L_z]。

总之已知机械臂工具端要运动到目标位置L_xyz＝[L_x，L_y，L_z]，驱动舵机转动，使得机械臂工具端运动到目标位置的流程是：

已知工具端坐标L,求出旋转轴角度θ₁；

已知中轴旋转角度θ₁,求出节点B，G，I坐标；

已知节点G,I，求出长度L_GI；

已知三角形△GHI的各边长，求出角度∠HGI，即γ；

已知三角形△GHI和△GBI的各边长，求出角度∠HGI，∠BGI,即

β＝∠HGI+∠BGI；

由γ和β以及映射模块，可以求得角度θ₂，θ₃；

有映射模块，可以求得舵机刻度为k₁、k₂、k₃；

所述的反向求解是求解机械臂工具端所在位置，具体步骤为：

步骤一、调用执行模块获取相应舵机的当前刻度k₁、k₂、k₃；

步骤二、根据关节角[θ₁，θ₂，θ₃]和舵机刻度的映射关系求解出相应舵机的 [θ₁，θ₂，θ₃]；

步骤三、根据反向求解步骤二所得的关节角[θ₁，θ₂，θ₃]求解机械臂工具端所在位置L_xyz＝[L_x，L_y，L_z]，其中角度θ₁，θ₂，θ₃与β，γ的关系为θ₂＝β-π/2，θ₃＝π-γ-θ₂；

在四边形GBIH中，我们令节点B的坐标为[B_xy＝D₀，B_z＝0]，由此可得节点G_xy的坐标：

由几何关系可知，节点H在平面四边形GBIH中的坐标为：

节点I在平面四边形GBIH中的坐标为：

最终，节点L在平面四边形GBIH的坐标为：

由于平面四边形GBIH是绕旋转中轴旋转了θ₁角度，所以，可以用如下公式把节点L扩充为：

可得机械臂工具端所在位置L_xyz＝[L_x，L_y，L_z]。

总之，求解机械臂工具端所在位置的流程是：

获取相应舵机的当前刻度k₁、k₂、k₃，通过映射模块转换为角度值[θ₁，θ₂，θ₃]，把角度[θ₁，θ₂，θ₃]转化为角度θ₁，β，γ；

已知旋转轴角度θ₁,求节点坐标B；

已知节点B坐标和杆长L_A，求出节点G坐标；

已知节点G坐标、杆长L_B和角度β，求出节点H坐标；

已知节点H坐标、杆长L_C和角度γ，求出节点I坐标；

已知节点I坐标，杆长D₂，杆长D₁，求出工具端坐标L；

所述关节角[θ₁，θ₂，θ₃]和舵机刻度[k₁、k₂、k₃]的映射关系为：

θ₁＝(k₁-KBase₁)·KUnit·Ratio₁-QBase₁

θ₂＝(k₂-KBase₂)·KUnit·Ratio₂+QBase₂

θ₃＝(k₃-KBase₃)·KUnit·Ratio₃-QBase₃

其中KBase₁、KBase₂、KBase₃是相应关节的基准刻度，KUnit是舵机一个刻度对应的角度范围，Ratio₁、Ratio₂、Ratio₃是相应关节同步轮比例，QBase₁、 QBase₂、QBase₃是相应关节的基准角度。

映射模块求解过程：

固定机械臂到指定姿态，如附图1所示：

调用执行模块读取舵机的当前刻度[k₁、k₂、k₃]，并设置为基准刻度KBase₁、KBase₂、KBase₃；

读取机械臂此时的关节角度[θ₁，θ₂，θ₃]，并设置为基准角度QBase₁、QBase₂、QBase₃；

记录同步带轮的减速比Ratio₁、Ratio₂、Ratio₃；

记录舵机一个刻度对应的角度范围KUnit(舵机的性能参数，表示舵机一个刻度对应的角度范围)；

建立关节角度[θ₁，θ₂，θ₃]与舵机刻度[k₁、k₂、k₃]之间的映射关系；

建立角度θ₂，θ₃与β，γ的关系。

上面所说的“指定姿态”是指校准状态，即机械臂末端运动计算的坐标零点，机械臂末端运动时的计算基准点。

上述实施例为本发明较佳的实施方式，但本发明的实施方式并不受上述实施例的限制，其他的任何未背离本发明的精神实质与原理下所作的改变、修饰、替代、组合、简化，均应为等效的置换方式，都包含在本发明的保护范围之内。

Claims (2)

1.一种基于舵机驱动的四轴机械臂的运动求解算法，其特征是，包括正向求解和反向求解：

所述的正向求解是已知机械臂工具端要运动到目标位置L_xyz＝[L_x，L_y，L_z]，驱动舵机转动，使得机械臂工具端运动到目标位置，具体步骤为：

步骤一、调用求解模块计算出机械的关节角度[θ₁，θ₂，θ₃]；

θ₁＝arctan(tan(θ₁))

其中角度θ₁，θ₂，θ₃与β，γ的关系为θ₂＝β-π/2，θ₃＝π-γ-θ₂；

杆件间距D₀、L_A、L_B、L_C、D₂、D₁都在同一平面上，因此可求出角度θ₁：

求得节点B的坐标为B_xyz：

求得节点G的坐标为G_xyz：

节点I与节点L存在如下关系：

则获得节点I的坐标：

有节点坐标B_xyz、G_xyz、I_xyz可以求得L_GI、L_BI，如下所示：

由三角形△GHI可以求得角度：γ和∠HGI，结果如下：

由三角形△GBI可以求得角度：∠BGI，结果如下：

最终可得β为：β＝∠HGI+∠BGI；

步骤二、根据关节角[θ₁，θ₂，θ₃]和舵机刻度的映射关系求解出相应舵机的刻度k₁、k₂、k₃；

步骤三、将相应舵机的刻度k₁、k₂、k₃通过执行模块分别赋值给相应的舵机，机械臂工具端运动到目标位置L_xyz＝[L_x，L_y，L_z]；

所述的反向求解是求解机械臂工具端所在位置，具体步骤为：

步骤一、调用执行模块获取相应舵机的当前刻度k₁、k₂、k₃；

步骤二、根据关节角[θ₁，θ₂，θ₃]和舵机刻度的映射关系求解出相应舵机的[θ₁，θ₂，θ₃]；

步骤三、根据反向求解步骤二所得的关节角[θ₁，θ₂，θ₃]求解机械臂工具端所在位置L_xyz＝[L_x，L_y，L_z]，其中角度θ₁，θ₂，θ₃与β，γ的关系为θ₂＝β-π/2，θ₃＝π-γ-θ₂；

在四边形GBIH中，我们令节点B的坐标为[B_xy＝D₀，B_z＝0]，由此可得节点G_xy的坐标：

由几何关系可知，节点H在平面四边形GBIH中的坐标为：

节点I在平面四边形GBIH中的坐标为：

最终，节点L在平面四边形GBIH的坐标为：

由于平面四边形GBIH是绕旋转中轴旋转了θ₁角度，所以，可以用如下公式把节点L扩充为：

可得机械臂工具端所在位置L_xyz＝[L_x，L_y，L_z]。

2.根据权利要求1所述的一种基于舵机驱动的四轴机械臂的运动求解算法，其特征是，所述关节角[θ₁，θ₂，θ₃]和舵机刻度[k₁、k₂、k₃]的映射关系为：

θ₁＝(k₁-KBase₁)·KUnit·Ratio₁-QBase₁

θ₂＝(k₂-KBase₂)·KUnit·Ratio₂+QBase₂

θ₃＝(k₃-KBase₃)·KUnit·Ratio₃-QBase₃

其中KBase1、KBase2、KBase3是相应关节的基准刻度，KUnit是舵机一个刻度对应的角度范围，Ratiol、Ratio2、Ratio3是相应关节同步轮比例，QBase1、QBase2、QBase3是相应关节的基准角度。

Images