CN109648563B

CN109648563B - 串联机器人运动控制方法及计算机存储介质

Info

Publication number: CN109648563B
Application number: CN201910001011.4A
Authority: CN
Inventors: 丁胜利; 陈盛; 梁志伟; 高翔; 谭彩明
Original assignee: Nanjing University of Posts and Telecommunications
Current assignee: Nanjing University of Posts and Telecommunications
Priority date: 2019-01-02
Filing date: 2019-01-02
Publication date: 2021-10-29
Anticipated expiration: 2039-01-02
Also published as: CN109648563A

Abstract

本发明公开了一种串联机器人运动控制方法及计算机存储介质，所述的控制方法是根据机器人的末端点和关节点位置信息去控制机器人达到目标点位置，包括步骤：(1)建立全局坐标系并确定目标点位置信息；(2)获取末端点和关节点的位置信息；(3)计算各关节点所需角度信息；(4)采用PID方式分别控制各关节电机往目标方向转动；(5)检测末端和目标点之间的距离是否到达允许误差范围内，如果达到则停止转动，如果没有则回到步骤(2)执行。本方法克服了目前机器人控制方法中求解繁琐、控制效率低等问题，能够有效的降低运算和控制的复杂度，减轻控制器的负担。

Description

串联机器人运动控制方法及计算机存储介质

技术领域

本发明涉及一种串联机器人运动控制方法及计算机存储介质，特别是涉及一种基于关节位置信息感知的串联机器人运动控制方法及计算机存储介质。

背景技术

当前机器人被广泛应用到人类生产生活的各个方面，随着机器人的普及，对机器人的性能要求也越来越高。性能的提高主要体现在灵活度和快速性。现有的机器人控制方法常基于D-H运动学模型，建立运动学的矩阵方程，然后把关节变量分离出来，从而求解。分离变量的求解过程随着自由度的增加而迅速变得极其复杂，计算量极大，无法克服多解问题，严重阻碍了机器人的实时性能和执行任务的效率。另一种常用方法是迭代求解，该方法的速度取决于迭代速度，精确度取决于迭代步长，实际使用中效果常常不理想，求解速度慢。

因此，目前亟需一种简单易操作的机器人控制方法，用与解决现有技术中的机器人控制方法存在的控制效率较低，求解过程过于繁琐的问题。

发明内容

发明目的：本发明要解决的技术问题是提供一种串联机器人运动控制方法及计算机存储介质，克服了目前机器人控制方法中求解繁琐、控制效率低等问题，能够有效的降低运算和控制的复杂度，减轻控制器的负担。

技术方案：本发明所述的串联机器人运动控制方法，根据机器人的末端点和关节点位置信息去控制机器人达到目标点位置，包括步骤：

(1)建立全局坐标系并确定目标点位置信息；

(2)获取末端点和关节点的位置信息；

(3)计算各关节点所需角度信息；

(4)采用PID方式分别控制各关节电机往目标方向转动；

(5)检测末端和目标点之间的距离是否到达允许误差范围内，如果达到则停止转动，如果没有则回到步骤(2)执行。

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

(1.1)以机器人的第一个关节点为坐标原点建立三维全局坐标系，Z轴垂直于机器人底座向上，X轴和Y轴根据右手法则确定，正视图为Z轴和Y轴形成的平面图；

(1.2)根据目标点的位置信息确定其在全局坐标系中的坐标(x_o，y_o，z_o)。

进一步的，所述步骤(2)的位置信息为末端点和关节点在全局坐标系中的坐标。

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

(3.1)计算关节点转动轴延长线与XY平面的夹角A，与ZY平面的夹角B，与XZ平面夹角C，选择A、B、C中角度最大夹角所对应的平面为参照平面，如果A＝B＝C则选择XY平面为参照平面；

(3.2)计算关节点和目标点连线在参照平面上的投影形成的向量[x_n，y_n]，计算关节点和末端点连线在参照平面上的投影形成的向量[x′_n，y′_n]；

(3.3)计算上述两向量之间的夹角θ_n(t)。

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

(4.1)确定关节点电机的转动方向，从第一个关节到最后一个关节根据步骤(3)所求得的夹角θ_n(t)转动，转动方法为：若满足y_n×x′_n-y′_n×x_n＜0，转动方向为在所述参照平面上的投影顺时针转动，若不满足则转动方向为在所述参照平面上的投影逆时针转动；

(4.2)确定关节点电机的转动角速度，角速度大小由下述公式确定：

其中K_p、K_i、K_d分别为比例、积分、微分系数；

(4.3)关节点电机按照上述的转动方向和角速度进行转动，转动的控制周期为Δt。

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

(5.1)获取末端点的坐标并计算末端点到目标点的平方距离：

L＝(X-x_o)²+(Y-y_o)²+(Z-z_o)²，

其中(X,Y,Z)为末端点的坐标，(x_o，y_o，z_o)为目标点的坐标；

(5.2)设定误差允许大小ε，ε根据实际工作中要求的位置精度进行设定；

(5.3)若L小于等于ε则停止转动，否则回到步骤(2)继续执行。

本发明所述的计算机存储介质，其上存储有计算机程序，所述程序被计算机处理器执行时实现上述的方法。

有益效果：本方法能够有效地降低机器人运动控制方法中的复杂度，彻底避免多解问题，提高处理的实时性，减轻了控制器的负担。

附图说明

图1是本方法的整体流程图；

图2是三自由度串联机械臂初始位姿及全局坐标系图；

图3是关节1的投影向量和角度图；

图4是关节2的投影向量和角度图；

图5是关节1、2、3的运动方向和距离检测图。

具体实施方式

本实施例以三自由度串联机械臂为例，通过关节位置信息感知来实现对该机器人点位运动的控制，本方法具体实施步骤如如图1所示。

(1)建立全局坐标系：

(1.1)以第一个关节点为坐标原点开始建立串联机器人的三维全局坐标系，Z轴垂直于机器人底座向上，X轴和Y轴根据右手法则确定，正视图为Z轴和Y轴形成的平面图；

(1.2)根据目标点的位置信息确定其在全局坐标系中的坐标(x_o，y_o，z_o)；

如图2所示，为建立的三自由度串联机械臂初始位姿图及全局坐标系，坐标系的原点为关节1的关节点，关节1的旋转轴垂直于地面，关节点2、3的旋转轴平行于XY平面，但不与YZ平面垂直，小球为目标点，坐标即为(x_o，y_o，z_o)。

(2)获取关节位置信息；

(2.1)获取末端点的位置，各关节点的高度和转动角度；

(2.2)确定末端点在全局坐标系中的坐标(x_w，y_w，z_w)，图2中已经指明了末端点；

(2.3)确定各关节在全局坐标系中的坐标，关节点1的坐标为(0,0,0),关节点2为(x₂，y₂，z₂)，关节点3为(x₃，y₃，z₃)，关节坐标矩阵为:

(3)计算各关节所需角度信息；

(3.1)如图3和图4所示，本实施方式中的机器人关节较少，默认目标点坐标中的y₀不等于0，选择XY和YZ这两个投影面即可满足工作要求，如果y₀等于0，把YZ平面换成XZ平面即可。计算关节1和目标点连线在XY平面上的投影形成的向量，计算关节2和目标点连线在ZY平面上的投影形成的向量，关节3和目标点在ZY平面的投影形成的向量，得：

(3.2)如图3、4所示，计算关节1和末端点连线在XY平面上的投影形成的向量，计算关节2和末端点连线在ZY平面上的投影形成的向量，关节3和目标点在ZY平面的投影形成的向量，得：

(3.3)计算两向量之间的夹角θ_n(t)，所求角度的位置已在图3、4中标出，关节1、2、3的所求角度为：

(4)如图5所示，采用PID方式控制电机同时往目标方向转动；

(4.1)图5中关节1、2、3的转动方向为

式中对应的d小于0则顺时针转动，不满足则逆时针转动，如图5中箭头所指方向；

(4.2)关节1、2、3电机开始按所求得的方向转动；

关节速度大小采用PID控制，速度大小由下述公式确定：

式中K_p、K_i、K_d分别为比例、积分、微分系数，具体数值由操作人员根据实际控制***设置；

转动的控制周期为Δt，Δt根据机器人实际作业时的情况设置，在实际作业时可以安排每隔10ms或者更长时间检测一次，只要这个控制周期Δt大于机器人的控制指令周期即可。

(5)检测末端位置和目标位置的距离，在图5中已标出，判断是否到达允许误差范围内，如果没有则回到步骤(2)；

(5.1)设定误差允许大小ε，误差范围即为±ε，ε等于实际工作中要求的位置精度；

(5.2)在机器人的运动过程中，每个控制周期测量末端点位置并计算一次末端点到目标点的平方距离L＝(X-x_o)²+(Y-y_o)²+(Z-z_o)²，如果小于等于n则停止转动；否则，回到上述步骤(2)。

本发明的实施例还提供了一种计算机存储介质，其上存储有计算机程序。当所述计算机程序由处理器执行时，可以实现前述控制的方法。例如，该计算机存储介质为计算机可读存储介质。

本领域内的技术人员应明白，本申请的实施例可提供为方法、***、或计算机程序产品。因此，本申请可采用完全硬件实施例、完全软件实施例、或结合软件和硬件方面的实施例的形式。而且，本申请可采用在一个或多个其中包含有计算机可用程序代码的计算机可用存储介质(包括但不限于磁盘存储器、CD-ROM、光学存储器等)上实施的计算机程序产品的形式。

本申请是参照根据本申请实施例的方法、设备(***)、和计算机程序产品的流程图和/或方框图来描述的。应理解可由计算机程序指令实现流程图和/或方框图中的每一流程和/或方框、以及流程图和/或方框图中的流程和/或方框的结合。可提供这些计算机程序指令到通用计算机、专用计算机、嵌入式处理机或其他可编程数据处理设备的处理器以产生一个机器，使得通过计算机或其他可编程数据处理设备的处理器执行的指令产生用于实现在流程图一个流程或多个流程和/或方框图一个方框或多个方框中指定的功能的装置。

这些计算机程序指令也可存储在能引导计算机或其他可编程数据处理设备以特定方式工作的计算机可读存储器中，使得存储在该计算机可读存储器中的指令产生包括指令装置的制造品，该指令装置实现在流程图一个流程或多个流程和/或方框图一个方框或多个方框中指定的功能。

这些计算机程序指令也可装载到计算机或其他可编程数据处理设备上，使得在计算机或其他可编程设备上执行一系列操作步骤以产生计算机实现的处理，从而在计算机或其他可编程设备上执行的指令提供用于实现在流程图一个流程或多个流程和/或方框图一个方框或多个方框中指定的功能的步骤。

Claims

1.一种串联机器人运动控制方法，其特征在于：所述的控制方法是根据机器人的末端点和关节点位置信息去控制机器人达到目标点位置，包括步骤：

(1)建立全局坐标系并确定目标点位置信息；

(2)获取末端点和关节点的位置信息；

(3)计算各关节点分别与末端和目标点所形成的向量夹角，具体包括以下步骤：

(3.3)计算上述两向量之间的夹角θ_n(t)；

(4)基于该向量夹角，确定关节点电机的转动方向，从第一个关节到最后一个关节根据步骤(3)所求得的夹角θ_n(t)转动，转动方法为：若满足y_n×x′_n-y′_n×x_n<0，转动方向为在所述参照平面上的投影顺时针转动，若不满足则转动方向为在所述参照平面上的投影逆时针转动；

采用PID方式分别控制各关节电机往目标方向转动；

2.根据权利要求1所述的串联机器人运动控制方法，其特征在于所述步骤(1)具体包括以下步骤：

(2.1)以机器人的第一个关节点为坐标原点建立三维全局坐标系，Z轴垂直于机器人底座向上，X轴和Y轴根据右手法则确定，正视图为Z轴和Y轴形成的平面图；

(2.2)根据目标点的位置信息确定其在全局坐标系中的坐标(x_o，y_o，z_o)。

3.根据权利要求1所述的串联机器人运动控制方法，其特征在于所述步骤(2)的位置信息为末端点和关节点在全局坐标系中的坐标。

4.根据权利要求1所述的串联机器人运动控制方法，其特征在于所述步骤(4)具体包括以下步骤：

(5.1)确定关节点电机的转动角速度，角速度大小由下述公式确定：

其中K_p、K_i、K_d分别为比例、积分、微分系数；

(5.2)关节点电机按照上述的转动方向和角速度进行转动，转动的控制周期为Δt。

5.根据权利要求4所述的串联机器人运动控制方法，其特征在于所述步骤(5)具体包括以下步骤：

(6.1)获取末端点的坐标并计算末端点到目标点的平方距离：

L＝(X-x_o)²+(Y-y_o)²+(Z-z_o)²，

其中(X,Y,Z)为末端点的坐标，(x_o，y_o，z_o)为目标点的坐标；

(6.2)设定误差允许大小ε，ε根据实际工作中要求的位置精度进行设定；

(6.3)若L小于等于ε则停止转动，否则回到步骤(2)继续执行。

6.一种计算机存储介质，其上存储有计算机程序，其特征在于：所述程序被计算机处理器执行时实现权利要求1至5任一项所述的方法。