CN109571478A - 一种串联多自由度机械臂末端循迹控制方法 - Google Patents

Abstract

本发明公开了一种串联多自由度机械臂末端循迹控制方法，包括：获取机械臂末端的当前位置以及机械臂末端的目标运动轨迹；将所述目标运动轨迹分解简化成若干个首尾相连的直线段，并基于前述直线段的端点构中间点集；基于机械臂末端的当前位置作为矢量起点和以距离该当前位置最近的中间点得到虚拟方向驱动力；获得机械臂上各个关节的当前位置数据，将虚拟方向驱动力自机械臂末端开始顺次分解到各关节上，并作用于对应的杆件；重复前面相关步骤直至机械臂末端遍历中间点集中的所有中间点，到达目标点，完成目标运动轨迹的循迹。本发明求解的机械臂控制策略，能够满足结果唯一，轨迹可控确定以及鲁棒性强等特点。

Description

一种串联多自由度机械臂末端循迹控制方法

技术领域

本发明属于机器人循迹控制技术领域，具体涉及一种串联多自由度机械臂末端循迹控制方法。

背景技术

串联多自由度机械臂在制造业、农业、娱乐业等行业得到越来越广泛的应用。机械臂末端的运动最终依靠关节运动实现，串联多自由度机械臂一般采用两种位置控制方式：点位控制-PTP(Point to Point)方式，连续路径控制-CP(Continuous Path)方式，PTP方式只关心起始点和目标点的位置，而不考虑两点间的位移路径；CP方式下，不仅要求机械臂的末端的以一定的精度到达目标点，而且对机器人的末端轨迹有一定的精度范围要求。因此，机械臂的末端的高精度高柔顺性的循迹控制具有重要意义。

由于多关节机械臂是一个高度复杂、高度耦合的非线性系统，机器臂动力学方程的复杂性、系统的不确定性导致传统的工业机器人控制器方法，如(计算力矩法)，未能有较好的适用性，而现有的多关节机械臂末端循迹先进控制策略方法，主要有变结构控制、自适应控制、迭代学习控制、鲁棒控制等，存在着诸如控制过程中，需要测量关节加速度、存在抖振，影响精度、收敛速度较慢等若干问题。

发明内容

针对上述问题，本发明提出一种多自由度机器人机械臂末端循迹控制方法，求解的机械臂控制策略，能够满足结果唯一，轨迹可控确定以及鲁棒性强等要求。具有快速、平稳、准确、简便等特点。

实现上述技术目的，达到上述技术效果，本发明通过以下技术方案实现：

一种串联多自由度机械臂末端循迹控制方法，所述多自由度机械臂包括若干个顺次耦合连接的杆件，相邻杆件之间设有关节；包括：

(1)获取机械臂末端的当前位置以及机械臂末端的目标运动轨迹；

(2)将所述目标运动轨迹分解成若干个首尾相连的直线段；

(3)顺次获取各直线段的端点作为所述目标运动轨迹的近似离散中间点，形成中间点集并保存；

(4)以机械臂末端的当前位置作为矢量起点，以距离该当前位置最近的中间点的位置作为矢量终点，并进行归一化得到单位矢量，进而乘以一定比例增益后得到虚拟方向驱动力；

(5)获得机械臂上各个关节的当前位置数据，并基于所述机械臂上各个关节的当前位置数据，将所述虚拟方向驱动力自机械臂末端开始顺次分解到各关节上，并作用对应的杆件；

(6)重复步骤(4)-(5)直至机械臂末端运动至步骤(4)中所述的中间点位置；

(7)重复步骤(6)直至机械臂末端遍历中间点集中的所有中间点，到达目标点，完成目标运动轨迹的循迹。

进一步地，所述步骤(2)还包括：

目标轨迹上与各直线段对应的圆弧上的所有点距离直线段的最大距离均在设定的容错误差范围内。

进一步地，所述步骤(5)具体包括以下步骤：

(5.1)获得机械臂上各个关节的当前位置数据；

(5.2)设定所述虚拟方向驱动力用于作用于机械臂末端所处的第一杆件上，并基于所述机械臂上各个关节的当前位置数据，计算用于作用于所述第一杆件上的分力f1，以及剩余分力f2；并基于分力f1得到与所述第一杆件相连的第一关节的控制参数；

(5.3)设定所述剩余分力f2用于作用至与所述第一关节相连的第二杆件上，并基于所述机械臂上各个关节的当前位置数据，计算得到用于作用于所述第二杆件上的分力f3，以及剩余分力f4；并基于分力f3得到与所述第二杆件相连的第二关节的控制参数；

(5.4)重复步骤(5.3)直至计算出剩余分力为0或者剩余分力传递至机械臂上最后的一个杆件上，获得各个关节的控制参数；

(5.5)机械臂根据所述各个关节的控制参数控制各个关节驱动对应的杆件运动。

进一步地，当机械臂中的关节为转动关节时，所述步骤(5.2)具体为：

设定所述虚拟方向驱动力作用于机械臂末端所处的第一杆件上；

基于所述机械臂上各个关节的当前位置数据，将所述虚拟方向驱动力分解为处于与所述第一杆件相连的第一关节转动平面内且垂直于作用点至第一关节旋转中心的连线方向的分力f1，以及处于虚拟方向作用力和该分力f1同一平面内的且垂直于分力f1的剩余分力f2，并计算出作用于所述第一关节的扭矩M1＝为分力f1×第一杆件的有效长度L1，M1即为转动关节电机控制参数，目标扭矩；

以此类推步骤(5.3)的具体过程。

进一步地，当机械臂中的关节为拉伸关节时，所述步骤(5.2)具体为：

设定所述虚拟方向驱动力用于作用于机械臂末端所处的第一杆件上；

基于所述机械臂上各个关节的当前位置数据，将所述虚拟方向驱动力分解为沿着关节移动的自由度方向的分力f1，以及处于虚拟方向驱动力和该分力f1同一平面内的且垂直于分力f1的剩余分力f2；并计算出用于作用于第一关节的拉力＝为分力f1，f1即为拉伸关节电机控制参数，通过乘以相应的力臂，得到目标转矩；

以此类推步骤(5.3)的具体过程。

进一步地，所述机械臂末端的当前位置以及机械臂上各个关节的当前位置数据均通过读取机械臂各关节的编码器相对值以及初始状态的编码器值计算得到。

进一步地，所述步骤(3)还包括：对中间点集中各个中间点均设置标志位，所述标志位初始值为1；

所述步骤(6)中还包括：当机械臂末端运动至中间点位置后，将该中间点的标志位置为0。

进一步地，所述步骤(6)重复步骤(4)-(5)直至机械臂末端运动至步骤(4)中所述的中间点位置，具体为：

(6.1)不断重复步骤(4)-(5)，并实时采集机械臂末端的实时位置；

(6.2)将所述机械臂末端的实时位置与所述中间点的位置进行比对，当二者之间的差值小于设定阈值范围，则判定机械臂末端运动至所述中间点，并将该中间点的标志位置为0。

进一步地，所述步骤(7)具体为：

重复步骤(6)直至中间点集中的所有中间点的标志位均被置为0，则表示机械臂末端运动至目标点，完成目标运动轨迹的循迹。

本发明的有益效果：

本发明将机械臂由作业空间内的起始点至目标点，同时具有一定的轨迹以及轨迹精度要求的运动过程的求解问题，分解为不同时间段，通过连接机械臂末端的当前位置与机械臂末端的瞬时目标位置的矢量线段，经过一定的合适的比例增益后，模拟施加于机械臂末端，(在不考虑各关节间耦合力有限的情况下)获得瞬时串联多关节机械臂各关节的诸如扭矩、力等控制参数。以此作为该瞬时机械臂各关节瞬时扭矩输出控制的依据。本发明求解的机械臂控制策略，能够满足结果唯一，轨迹可控确定以及鲁棒性强等要求。具有快速、平稳、准确、简便等特点。

附图说明

图1为本发明一种实施例的机械臂的结构示意图。

图2为本发明一种实施例中当关节为转动关节时的力分解图；

图3为本发明一种实施例中当关节为拉伸关节时的力分解图。

具体实施方式

为了使本发明的目的、技术方案及优点更加清楚明白，以下结合实施例，对本发明进行进一步详细说明。应当理解，此处所描述的具体实施例仅仅用以解释本发明，并不用于限定本发明。

下面结合附图对本发明的应用原理作详细的描述。

如图1-3所示，本发明提供了一种串联多自由度机械臂末端循迹控制方法，所述串联多自由度机械臂包括若干个顺次耦合连接的杆件1，相邻杆件1之间设有关节2，所述关节2为转动关节或者拉伸关节；位于首端处的杆件的一端连接有基座3，位于末端的杆件的一端连接有末端执行器4，具体包括以下步骤：

(1)获取机械臂末端的当前位置以及机械臂末端的目标运动轨迹；

在本发明实施例的一种具体实施例中，所述机械臂末端的当前位置可以通过读取机械臂末端或各关节的编码器相对值以及初始状态的编码器值计算得到；或者依靠其它视觉测量、三坐标测量仪等辅助测量装置推算得到；由于如何获取机械臂末端的当前位置不是本发明的发明点所在，现有技术中的任意能够实现位置获取的手段都可以用到本发明中，因此不做过多赘述；所述目标运动轨迹指的是为机械臂末端预设的运动轨迹；

(2)将所述目标运动轨迹分解成若干个首尾相连的直线段(离散点集或者函数表达式，函数表达式的要在轨迹上进行离散采样成离散点集)；在本发明的一种具体实施例中，为了提高分解的精度，目标轨迹上与各直线段对应的圆弧上的所有点距离直线段的最大距离均在设定的容错误差范围内；具体实施过程为：

(3)顺次获取各直线段的端点作为所述目标运动轨迹的近似离散中间点，形成中间点集并保存；在本发明的优选实施例中，所述步骤(3)还包括：对中间点集中各个中间点均设置标志位，所述标志位初始值为1；当机械臂末端经过某中间点，则将该中间点的标志位置为0，其余未经过的中间点的标志位依旧为1，作为机械臂末端的阶段性目标中间位置；

(4)以机械臂末端的当前位置作为矢量起点，阶段性目标中间点的位置作为矢量终点，获得初始矢量，并进行归一化得到单位矢量，进而得到虚拟方向驱动力；所述虚拟方向驱动力是通过对所述单位矢量进行乘以一定的比例增益得到；

(5)获得机械臂上各个关节的当前位置数据，并基于所述机械臂上各个关节的当前位置数据，将所述虚拟方向驱动力自机械臂末端开始顺次分解到各关节上，并作用对应的杆件；在本发明的一种具体实施例中，所述机械臂上各个关节的当前位置数据通过各关节的编码器相对值以及初始状态的编码器值计算得到；或者依靠其它视觉测量、三坐标测量仪等辅助测量装置推算得到；现有技术中的任意能够实现位置获取的手段都可以用到本发明中，因此不做过多赘述，所述步骤(5)具体包括以下步骤：

(5.1)获得机械臂上各个关节的当前位置数据；

(5.2)设定所述虚拟方向驱动力用于作用于机械臂末端所处的第一杆件(循迹点)上，并基于所述机械臂上各个关节的当前位置数据，计算用于作用于所述第一杆件上的分力f1，以及剩余分力f2；并基于分力f1得到与所述第一杆件相连的第一关节的控制参数；

(5.3)设定所述剩余分力f2用于作用至与所述第一关节相连的第二杆件上，并基于所述机械臂上各个关节的当前位置数据，计算用于作用于所述第二杆件上的分力f3，以及剩余分力f4；并基于分力f3得到与所述第二杆件相连的第二关节的控制参数；

(5.4)重复步骤(5.3)直至计算出剩余分力为0或者剩余分力传递至机械臂上最后的一个杆件上，获得各个关节的控制参数；

(5.5)位于机械臂末端的末端执行器根据所述各个关节的控制参数控制各个关节驱动对应的杆件运动。

更具体地：

当机械臂中的关节为转动关节时，所述步骤(5.2)具体为：

设定所述虚拟方向驱动力用于作用于机械臂末端所处的第一杆件(循迹点，所述循迹点指的是一个需要始终保持在目标运动轨迹上的点)上；

基于所述机械臂上各个关节的当前位置数据，将所述虚拟方向驱动力分解为处于与所述第一杆件相连的第一关节转动平面内且垂直于作用点至第一关节旋转中心的连线方向的分力f1，以及处于虚拟方向作用力和该分力f1同一平面内的且垂直于分力f1的剩余分力f2，并计算出作用于所述第一关节的扭矩M1＝为分力f1×第一杆件的有效长度L1；

以此类推步骤(5.3)的具体过程。

当机械臂中的关节为拉伸关节时，所述步骤(5.2)具体为：

设定所述虚拟方向驱动力用于作用于机械臂末端所处的第一杆件(循迹点，所述循迹点指的是一个需要始终保持在目标运动轨迹上的点)上；

基于所述机械臂上各个关节的当前位置数据，将所述虚拟方向驱动力分解为沿着拉伸关节移动的自由度方向(即轴线)的分力f1，以及处于虚拟方向驱动力和该分力f1同一平面内的且垂直于分力f1的剩余分力f2；并计算出用于作用于第一关节的拉力＝分力f1；

以此类推步骤(5.3)的具体过程。

(6)重复步骤(4)-(5)直至机械臂末端运动至步骤(4)中所述的中间点位置；在本发明的一种具体实施例中，所述步骤(6)具体为：

(6.1)不断重复步骤(4)-(5)，并实时采集机械臂末端的实时位置；

(6.2)将所述机械臂末端的实时位置与所述中间点的位置进行比对，当二者之间的差值小于设定阈值范围，则判定机械臂末端运动至所述中间点，并将该中间点的标志位置为0；

(7)重复步骤(6)直至机械臂末端遍历中间点集中的所有中间点，到达目标点，完成目标运动轨迹的循迹；在本发明的一种具体实施例中，所述步骤(7)具体为：

重复步骤(6)直至中间点集中的所有中间点的标志位均被置为0，则表示机械臂末端运动至目标点，完成目标运动轨迹的循迹。

实施例2

本实施例与实施例1的区别在于：

本实施例中的步骤(3)具体为：

顺次获取各直线段的端点作为所述目标运动轨迹的近似离散中间点，形成中间点集并保存；

本实施例中的步骤(6)具体为：

(6.1)不断重复步骤(4)-(5)，并实时采集机械臂末端的实时位置；

(6.2)将所述机械臂末端的实时位置与所述中间点的位置进行比对，当二者之间的差值小于设定阈值范围，则判定机械臂末端运动至所述中间点，并删除该中间点；

(7)重复步骤(6)直至机械臂末端遍历中间点集中的所有中间点，到达目标点，完成目标运动轨迹的循迹；在本发明的一种具体实施例中，所述步骤(7)具体为：

重复步骤(6)直至中间点集中的中间点数目为0，则表示机械臂末端运动至目标点，完成目标运动轨迹的循迹。

其余部分均与实施例1相同。

以上显示和描述了本发明的基本原理和主要特征和本发明的优点。本行业的技术人员应该了解，本发明不受上述实施例的限制，上述实施例和说明书中描述的只是说明本发明的原理，在不脱离本发明精神和范围的前提下，本发明还会有各种变化和改进，这些变化和改进都落入要求保护的本发明范围内。本发明要求保护范围由所附的权利要求书及其等效物界定。

Claims (9)

1.一种串联多自由度机械臂末端循迹控制方法，所述多自由度机械臂包括若干个顺次耦合连接的杆件，相邻杆件之间设有关节；其特征在于，包括：

(1)获取机械臂末端的当前位置以及机械臂末端的目标运动轨迹；

(2)将所述目标运动轨迹分解成若干个首尾相连的直线段；

(3)顺次获取各直线段的端点作为所述目标运动轨迹的近似离散中间点，形成中间点集并保存；

(4)以机械臂末端的当前位置作为矢量起点，以距离该当前位置最近的中间点的位置作为矢量终点，并进行归一化得到单位矢量，进而乘以一定比例增益后得到虚拟方向驱动力；

(5)获得机械臂上各个关节的当前位置数据，并基于所述机械臂上各个关节的当前位置数据，将所述虚拟方向驱动力自机械臂末端开始顺次分解到各关节上，并作用对应的杆件；

(6)重复步骤(4)-(5)直至机械臂末端运动至步骤(4)中所述的中间点位置；

(7)重复步骤(6)直至机械臂末端遍历中间点集中的所有中间点，到达目标点，完成目标运动轨迹的循迹。

2.根据权利要求1所述的一种串联多自由度机械臂末端循迹控制方法，其特征在于：所述步骤(2)还包括：

目标轨迹上与各直线段对应的圆弧上的所有点距离直线段的最大距离均在设定的容错误差范围内。

3.根据权利要求1所述的一种串联多自由度机械臂末端循迹控制方法，其特征在于：所述步骤(5)具体包括以下步骤：

(5.1)获得机械臂上各个关节的当前位置数据；

(5.2)设定所述虚拟方向驱动力用于作用于机械臂末端所处的第一杆件上，并基于所述机械臂上各个关节的当前位置数据，计算用于作用于所述第一杆件上的分力f1，以及剩余分力f2；并基于分力f1得到与所述第一杆件相连的第一关节的控制参数；

(5.3)设定所述剩余分力f2用于作用至与所述第一关节相连的第二杆件上，并基于所述机械臂上各个关节的当前位置数据，计算得到用于作用于所述第二杆件上的分力f3，以及剩余分力f4；并基于分力f3得到与所述第二杆件相连的第二关节的控制参数；

(5.4)重复步骤(5.3)直至计算出剩余分力为0或者剩余分力传递至机械臂上最后的一个杆件上，获得各个关节的控制参数；

(5.5)机械臂根据所述各个关节的控制参数控制各个关节驱动对应的杆件运动。

4.根据权利要求3所述的一种串联多自由度机械臂末端循迹控制方法，其特征在于：

当机械臂中的关节为转动关节时，所述步骤(5.2)具体为：

设定所述虚拟方向驱动力作用于机械臂末端所处的第一杆件上；

基于所述机械臂上各个关节的当前位置数据，将所述虚拟方向驱动力分解为处于与所述第一杆件相连的第一关节转动平面内且垂直于作用点至第一关节旋转中心的连线方向的分力f1，以及处于虚拟方向作用力和该分力f1同一平面内的且垂直于分力f1的剩余分力f2，并计算出作用于所述第一关节的扭矩M1＝为分力f1×第一杆件的有效长度L1，M1即为转动关节电机控制参数，目标扭矩；

以此类推步骤(5.3)的具体过程。

5.根据权利要求3所述的一种串联多自由度机械臂末端循迹控制方法，其特征在于：

当机械臂中的关节为拉伸关节时，所述步骤(5.2)具体为：

设定所述虚拟方向驱动力用于作用于机械臂末端所处的第一杆件上；

基于所述机械臂上各个关节的当前位置数据，将所述虚拟方向驱动力分解为沿着关节移动的自由度方向的分力f1，以及处于虚拟方向驱动力和该分力f1同一平面内的且垂直于分力f1的剩余分力f2；并计算出用于作用于第一关节的拉力＝为分力f1，f1即为拉伸关节电机控制参数，通过乘以相应的力臂，得到目标转矩；

以此类推步骤(5.3)的具体过程。

6.根据权利要求3所述的一种串联多自由度机械臂末端循迹控制方法，其特征在于：所述机械臂末端的当前位置以及机械臂上各个关节的当前位置数据均通过读取机械臂各关节的编码器相对值以及初始状态的编码器值计算得到。

7.根据权利要求1所述的一种串联多自由度机械臂末端循迹控制方法，其特征在于：所述步骤(3)还包括：对中间点集中各个中间点均设置标志位，所述标志位初始值为1；

所述步骤(6)中还包括：当机械臂末端运动至中间点位置后，将该中间点的标志位置为0。

8.根据权利要求7所述的一种串联多自由度机械臂末端循迹控制方法，其特征在于：所述步骤(6)重复步骤(4)-(5)直至机械臂末端运动至步骤(4)中所述的中间点位置，具体为：

(6.1)不断重复步骤(4)-(5)，并实时采集机械臂末端的实时位置；

(6.2)将所述机械臂末端的实时位置与所述中间点的位置进行比对，当二者之间的差值小于设定阈值范围，则判定机械臂末端运动至所述中间点，并将该中间点的标志位置为0。

9.根据权利要求8所述的一种串联多自由度机械臂末端循迹控制方法，其特征在于：所述步骤(7)具体为：

重复步骤(6)直至中间点集中的所有中间点的标志位均被置为0，则表示机械臂末端运动至目标点，完成目标运动轨迹的循迹。