一种机器人轨迹同步控制方法、计算机可读存储介质与流程

本发明涉及机器人同步控制技术领域，特别涉及一种机器人轨迹同步控制方法、计算机可读存储介质。

背景技术：

是否具有较高的轨迹精度是衡量机器人性能的重要指标之一。机器人的运动有四种类型：关节插补运动、直线插补运动、圆弧插补运动、样条插补运动，以从a点运动到b点为例，除了关节插补运动只需要控制机器人精确运动到终点b，其他三种插补运动还需要精确控制从起点a至终点b之间的运动轨迹(轨迹精度)，机器人的轨迹精度受机器人运动学参数的准确性、机器人速度、机器人关节的同步性等因素的影响，而机器人关节的同步性对机器人的轨迹精度的影响最大。

现有机器人各个关节多采用传统三环pid控制方法，调节pid参数使机器人各个关节刚度处于近似水平，从而使机器人各个关节对指令的相应的近似相同，保证机器人的同步性。而当机器人位置发生改变时，机器人各轴的转动惯量改变、摩擦力改变，上述pid参数并不能使得各个关节对指令的响应近似相同，从而无法保证机器人位置改变时的同步性。

即上述三环pid控制方法本质上是独立地对机器人各个关节闭环控制，存在机器人各个关节对指令的跟随不完全相同，有些轴快，有些轴慢的缺陷，导致关节同步性较差，机器人轨迹精度无法保证。

技术实现要素：

本发明的目的是克服上述背景技术中不足，提供一种机器人轨迹同步控制方法、计算机可读存储介质，可在传统的三环pid控制的基础上，根据机器人末端反馈位姿和理论位姿之间的偏差，实时调整机器人各个关节的运动，以保证机器人关节的同步性，从而提高机器人的轨迹精度。

为了达到上述的技术效果，本发明采取以下技术方案：

一种机器人轨迹同步控制方法，包括以下步骤：

步骤1.采用质量弹簧阻尼模型的二阶系统对机器人末端的位置和姿态即位姿进行闭环控制，得到笛卡尔空间下的线加速度和角加速度

步骤2.将笛卡尔空间下的线加速度和角加速度转换成机器人的关节速度和关节加速度

步骤3.若通过步骤1和步骤2得到的机器人末端的位姿及关节速度与设定值存在偏差时，引入内环控制回路，控制机器人各个关节按照关节加速度

进行加减速，以补偿机器人各个关节运动时对其他关节产生的耦合力矩；

步骤4.在机器人每个控制插补周期内，根据机器人末端的设定位姿与实际位姿之间的偏差，计算每个关节的关节加速度并作用于机器人的内环控制回路的控制器上，使设定位姿与实际位姿之间的偏差趋于0以实现机器人轨迹同步。

进一步地，所述步骤1具体包括：

步骤1.1.根据机器人各个关节的反馈角度q，采用机器人正运动公式kin(q)计算机器人末端的反馈位姿x；

步骤1.2.根据用户指定的期望位姿矩阵计算得到期望位姿xd；

步骤1.3.计算期望位姿xd与反馈位姿x之间的位姿误差e；

步骤1.4.采用二阶系统对末端位姿进行闭环控制，计算得到笛卡尔空间下的线加速度和角加速度

进一步地，所述步骤1.1具体包括：

步骤1.1.1.采用标准d-h法建立机器人的运动学模型，得到机器人的d-h参数：d、a、α；

步骤1.1.2.根据坐标变换方法得到机器人末端坐标系{n}在机器人极坐标系下的齐次变换矩阵：

其中，n表示机器人关节的数目，表示从第i-1个关节的坐标系{i-1}到第i个关节的坐标系{i}的齐次变换矩阵，i＝1,2,3...n，且的计算公式如下：

则x可表示为：

其中，表示矩阵的第i行的第j列的元素。

进一步地，所述步骤1.2中期望位姿xd具体计算如下：

其中进一步地，所述步骤1.3中位姿误差e的计算方法如下：

其中，

进一步地，所述步骤1.4中采用的二阶系统可等效为质量弹簧阻尼模型。

进一步地，笛卡尔空间下的线加速度和角加速度的计算方法如下：

位姿误差e满足如下方程：

其中，kd和kp均为6×6的正定矩阵，

其中vd、表示设定的线速度和设定的线加速度，wd、表示设定的角速度和设定的角加速度，v、表示线速度和线加速度，其中w、表示角速度和角加速度，代入式(8)可得：

进一步地，所述步骤2具体为：

根据机器人正向速度关系知：

其中，j(q)表示机器人的雅克比矩阵，表示机器人的关节速度；

将式(10)左右两端同时求导数得：

其中，表示雅克比矩阵的导数，则对于机器人不经过奇异点时有：

将步骤1.4中已经求得的代入式(12)得到：

进一步地，所述步骤3中具体为：

根据拉格朗日动力学方程有：

其中，τ表示机器人的关节驱动力矩，表示机器人的惯性力矩，

表示机器人的离心力矩和科氏力矩，g(q)表示机器人的重力矩；

在引入的内环控制回路中：

联立式(14)、(15)两个等式得：

同时将式(16)左右两端的非线性项和g(q)消去，得到：

由于m(q)可逆，故(17)式等价于：

其中，u为变量具体作为机器人的内环控制回路的控制器上的闭环输入；

经过上述计算可知内环控制回路的闭环输入u的值与关节加速度的值相等，最后在每个插补周期都计算关节加速度并赋值给u，实现控制器的实时控制。

同时，本发明还公开了一种计算机可读存储介质，所述计算机可读存储介质存储有计算机程序，所述计算机程序被处理器执行时实现上述机器人轨迹同步控制方法的步骤。

本发明与现有技术相比，具有以下的有益效果：

本发明的机器人轨迹同步控制方法在传统的三环pid控制的基础上，可实现根据机器人末端反馈位姿和理论位姿之间的偏差，实时调整机器人各个关节的运动，以保证机器人关节的同步性，从而提高机器人的轨迹精度。

附图说明

图1是本发明的机器人轨迹同步控制原理示意图。

具体实施方式

下面结合本发明的实施例对本发明作进一步的阐述和说明。

实施例：

实施例一：

一种机器人轨迹同步控制方法，本实施例中，具体由内控制回路和外控制环两部分组成，具体的，包括以下步骤：

步骤1.采用质量弹簧阻尼模型的二阶系统对机器人末端的位姿即位置和姿态进行闭环控制，得到笛卡尔空间下的线加速度和角加速度

具体包括：

步骤1.1：如图1所示，根据机器人各个关节的反馈角度q，采用机器人正运动公式kin(q)计算机器人末端的反馈位姿x，方法如下：

采用标准d-h法建立机器人的运动学模型，得到机器人的d-h参数：d、a、α。根据坐标变换方法得到机器人末端坐标系{n}在机器人极坐标系下的齐次变换矩阵：

其中，n表示机器人关节的数目，表示从第i-1个关节的坐标系{i-1}到第i个关节的坐标系{i}的齐次变换矩阵，i＝1,2,3...n，的计算公式如下：

则x可表示为：

其中表示矩阵的第i行的第j列的元素。

步骤1.2：同理根据用户指定的期望位姿矩阵计算得到期望的位姿如下：

其中，

步骤1.3：计算位姿误差e。

其中，

步骤1.4：采用二阶系统(可等效为质量弹簧阻尼模型)对末端位姿进行闭环控制，计算得到笛卡尔空间的线加速度和角加速度

具体的，位姿误差e满足如下方程：

其中，kd和kp均为6×6的正定矩阵。

而其中vd、表示设定的线速度和设定的线加速度，其中wd、表示设定的角速度和设定的角加速度，其中v、表示线速度和线加速度，其中w、表示角速度和角加速度，代入方程中可得：

步骤2.将笛卡尔空间下的线加速度和角加速度转换成关节的加速度

具体的，根据机器人正向速度关系知：

其中，j(q)表示机器人的雅克比矩阵，表示机器人的关节速度。

左右两端同时求导数得：

其中表示雅克比矩阵的导数，则对于机器人不经过奇异点时有：

将步骤1.4中已经求得的代入，得到：

步骤3.若通过步骤1和步骤2得到的机器人末端的位姿及关节速度与设定值存在偏差时，引入内环控制回路，控制机器人各个关节按照关节加速度进行加减速，以补偿机器人各个关节运动时对其他关节产生的耦合力矩。

具体为，根据拉格朗日动力学方程有：

其中τ表示机器人的关节驱动力矩，表示机器人的惯性力矩，表示机器人的离心力矩和科氏力矩，g(q)表示机器人的重力矩。

在引入的内环控制回路中：

联立上述两个等式有：

同时将左右两端的非线性项和g(q)消去，得到：

由于m(q)可逆，故上式等价于：

即本实施例中，内环控制回路的控制器上的闭环输入的值与关节加速度的值相等。

步骤4.在机器人每个控制插补周期内，根据机器人末端的设定位姿与实际位姿之间的偏差，计算每个关节的加速度并赋值给u，作用于机器人上，实现机器人轨迹的实时控制，使设定位姿与实际位姿之间的偏差逼近0。

在实际应用中，通过试验得出，使用传统三环pid控制方法，机器人高速运动时的轨迹精度，其最大值为1.7mm，用本控制方法机器人高速运动时的轨迹精度，其最大值为0.7mm，通过对比可知，本方法将轨迹精度提高了约2.5倍左右。

实施例二

本实施例中公开了一种计算机设备，该计算机设备可以是服务器，该计算机设备包括通过系统总线连接的处理器、存储器、网络接口和数据库。其中，该计算机设备的处理器用于提供计算和控制能力。该计算机设备的存储器包括非易失性存储介质、内存储器。该非易失性存储介质存储有操作系统、计算机程序和数据库。该内存储器为非易失性存储介质中的操作系统和计算机程序的运行提供环境。该计算机设备的数据库用于存储机器人轨迹同步控制方法中涉及到的数据。该计算机设备的网络接口用于与外部的终端通过网络连接通信。该计算机程序被处理器执行时以实现机器人轨迹同步控制方法。

在另一个实施例中，提供了一种计算机设备，包括存储器、处理器及存储在存储器上并可在处理器上运行的计算机程序，处理器执行计算机程序时实现上述实施例一中机器人轨迹同步控制方法的步骤。为避免重复，这里不再赘述。

在另一个实施例中，提供了一种计算机可读存储介质，其上存储有计算机程序，计算机程序被处理器执行时实现上述实施例一中机器人轨迹同步控制方法的步骤。为避免重复，这里不再赘述。

可以理解的是，以上实施方式仅仅是为了说明本发明的原理而采用的示例性实施方式，然而本发明并不局限于此。对于本领域内的普通技术人员而言，在不脱离本发明的精神和实质的情况下，可以做出各种变型和改进，这些变型和改进也视为本发明的保护范围。