CN108000520A - 机器人的运动控制方法及系统 - Google Patents

Abstract

本发明涉及机器人的运动控制方法及系统，所述方法包括以下步骤：输入预设参数，并构造出位移矢量；将位移矢量投射到三个坐标轴上，根据预设参数分别计算出三个轴上所对应的脉冲的脉宽；三轴同步输出计算所得的脉冲，同时计算出下一个脉冲的脉宽；重复上一步骤直至完成预设的运动轨迹。本发明在一个脉冲时间内计算完成下一个脉冲的宽度并输出，只存在机械和电机的误差，而没有计算误差，大大提高桌面机器人的运动和定位精度。

Description

机器人的运动控制方法及系统

技术领域

本发明属机器人控制技术领域，具体涉及一种机器人的运动控制方法及系统。

背景技术

随着计算机集成制造技术的迅猛发展，工业、农业、航空等行业对机器人的需求越来越大，随之而来的是对机器人控制器的需求也越旺盛。目前我国桌面机器人运动控制系统中的控制器存在定位不准确、轨迹契合度不高的问题。现有控制器采用了插补的逼近算法，在实际应用中轨迹契合误差和累积误差较大，产生较大的运动轨迹和停止定位偏差。

目前应用的最广泛的是“示教再现型”机器人，其工作原理如下：操作人员利用“示教控制器”发出指令，让机器人的机械手臂运动，一步步完成它应当完成的各个动作，这个过程叫“示教”；机器人上的传感器能够把机器人所得的信息依次送给机器人的记忆装置，该记忆装置可以把机器人各部分运动顺序、位置、速度等记录并存储起来，这一过程叫编程；机器人工作时，记忆装置把信号放出来控制机器人动作，这就是“再现”的过程，它可以按照操作人员教给它的动作自动地、不断地、反复地进行工作。

目前的示教器采用按键和文字屏方式，示教学习难度大、操作不方便、示教效率低。示教器用按键和文字屏完成设备工序动作的规划，学习和操作难度大。

发明内容

有鉴于此，本发明的目的在于克服现有技术的不足，提供一种机器人的运动控制方法及系统。

为实现以上目的，本发明采用如下技术方案：

机器人的运动控制方法，包括以下步骤：

S1：输入预设参数，并构造出位移矢量；

S2：将位移矢量投射到三个坐标轴上，根据预设参数分别计算出三个轴上所对应的脉冲的脉宽；

S3：三轴同步输出计算所得的脉冲，同时计算出下一个脉冲的脉宽；

S4：重复上一步骤直至完成预设的运动轨迹。

进一步地，步骤S2和步骤S3中，计算脉宽的具体方法为：

其中，T_n+1为任意一个坐标轴所对应的第n+1个脉冲的脉宽；V_n为同一坐标轴上的第n个脉冲所对应的速度；ΔS为脉冲当量，n为自然数。

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

S201：将位移矢量投射到三个坐标轴上，然后分别计算出该位移矢量与三个轴的夹角α、β和γ；

S202：根据输入的预设参数计算出加速距离、减速距离和恒速距离；

S203：分别用sinα、sinβ和sinγ乘以加速距离，得出加速距离在三个轴上的对应分量；同样方式计算出减速距离和恒速距离在三个轴上的对应分量；

S204：根据任意一个坐标轴上的三个距离分量，推算出该轴所对应的速度分量V_n，再计算出该轴所对应的脉冲的脉宽；

S205：重复上一步骤，计算出另外两个坐标轴所对应的脉冲的脉宽。

更进一步地，所述步骤S202的计算方法是：

加速距离

减速距离

恒速距离

其中，a是加速度，d是减速度，v_star是起始速度、v_stable是恒定速度、v_end是结束速度，(x₁,y₁,z₁)是起点坐标，(x₂,y₂,z₂)是终点坐标，以上均为预设参数。

更进一步地，所述步骤S204具体包括以下步骤：

定义S_current＝ΔS×n；

若S_current＜S_acc，则处于加速段，推算方法为V_n+1＝V_n+a×T_n+1；

若S_acc≤S_current＜S_acc+S_stable，则处于恒速段，推算方法为V_n+1＝v_stable；

否则处于减速段，推算方法为V_n+1＝V_n-d×T_n+1；

其中，n为自然数，V₀＝v_star。

本发明还提供了机器人的运动控制系统，包括：

启动模块，用于显示欢迎词条，新建或选择文件，以及完成参数初始化；

示教模块，用于绘出运行轨迹的图形，并对图形进行编辑；

加工模块，用于进行加工指令的处理；以及

数据处理模块，用于根据编辑完成的运行轨迹计算出所需要的控制信号，并输出控制信号。

所述系统还包括测试模块，用于进行输入IO状态、输出IO状态和轴动作的测试。

所述系统还包括系统模块，用于显示版本号和日期，以及进行文件删除操作。

进一步地，所述示教模块对图形进行编辑，包括删除段、阵列复制段和移动所选段。

进一步地，所述加工指令包括描点、启动/停止、暂停/继续、复位和数据显示；

执行描点指令，即在所述示教模块生成的轨迹图形上实时显示目标当前位置；

执行数据显示指令，即在屏幕上显示单次运行时间、当前加工件数、预置加工件数。

本发明采用以上技术方案，控制方法采用了单脉冲预置实时算法，根据运动轨迹和速度，在一个脉冲时间内计算完成下一个脉冲的宽度并输出，具有在机械设计的当量脉冲情况下完成指定速度，并在轨迹契合以及停止定位时达到或小于一个脉冲的精度。该方法只存在机械和电机的误差，而没有计算误差，大大提高桌面机器人的运动和定位精度。

本发明的控制系统，示教操作方便，示教简单便捷，直接在电容屏上触控轨迹图形和利用少量屏上的控件即可完成示教工序。本发明定位精确、轨迹稳合度高，示教学习容易，操作方便，可以大大提高示教效率。

附图说明

为了更清楚地说明本发明实施例或现有技术中的技术方案，下面将对实施例或现有技术描述中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图仅仅是本发明的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图获得其他的附图。

图1是本发明机器人的运动控制方法流程图；

图2是本发明机器人的运动控制系统结构示意图；

图3是本发明运动控制系统的工作流程示意图。

具体实施方式

为使本发明的目的、技术方案和优点更加清楚，下面将对本发明的技术方案进行详细的描述。显然，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动的前提下所得到的所有其它实施方式，都属于本发明所保护的范围。

如图1所示，本发明提供了机器人的运动控制方法，包括以下步骤：

S1：输入预设参数，并构造出位移矢量；

S2：将位移矢量投射到三个坐标轴上，根据预设参数分别计算出三个轴上所对应的脉冲的脉宽；

S3：三轴同步输出计算所得的脉冲，同时计算出下一个脉冲的脉宽；

S4：重复上一步骤直至完成预设的运动轨迹。

进一步地，步骤S2和步骤S3中，计算脉宽的具体方法为：

其中，T_n+1为任意一个坐标轴所对应的第n+1个脉冲的脉宽；V_n为同一坐标轴上的第n个脉冲所对应的速度；ΔS为脉冲当量，n为自然数。

脉冲当量ΔS是当控制器输出一个定位控制脉冲时，所产生的定位控制移动的位移。对直线运动来说，是指移动的距离；对圆周运动来说，是指其转动的角度。

一个脉冲对应移动距离ΔS，而脉冲的宽度决定了多长时间来走完这个距离ΔS。因此，根据当前的速度和脉冲当量，就可以决定一个脉冲需要多少脉宽，这是对单个脉冲距离的一个均分算法。这种算法的好处是，不但可以使控制精度达到一个脉冲当量的距离，还可以精确控制速度。

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

S201：将位移矢量投射到三个坐标轴(X、Y、Z轴)上，然后分别计算出该位移矢量与三个轴的夹角α、β和γ；

S202：根据输入的预设参数计算出加速距离、减速距离和恒速距离；

S203：分别用sinα、sinβ和sinγ乘以加速距离，得出加速距离在三个轴上的对应分量；同样方式计算出减速距离和恒速距离在三个轴上的对应分量；

S204：根据任意一个坐标轴上的三个距离分量，推算出该轴所对应的速度分量V_n，再计算出该轴所对应的脉冲的脉宽；

S205：重复上一步骤，计算出另外两个坐标轴所对应的脉冲的脉宽。

更进一步地，所述步骤S202的计算方法是：

加速距离

减速距离

恒速距离

其中，a是加速度，d是减速度，v_star是起始速度、v_stable是恒定速度、v_end是结束速度，(x₁,y₁,z₁)是起点坐标，(x₂,y₂,z₂)是终点坐标，以上均为预设参数。

更进一步地，所述步骤S204具体包括以下步骤：

定义S_current＝ΔS×n；

若S_current＜S_acc，则处于加速段，推算方法为V_n+1＝V_n+a×T_n+1；

若S_acc≤S_current＜S_acc+S_stable，则处于恒速段，推算方法为V_n+1＝v_stable；

否则处于减速段，推算方法为V_n+1＝V_n-d×T_n+1；

其中，n为自然数，V₀＝v_star。

需要说明的是，在进行判断时，S_acc、S_stable和S_dec均为投射到某个坐标轴之后的值；并且a、d、v_star、V_n均为同一个坐标轴所对应的数值。

如图2所示，本发明还提供了机器人的运动控制系统，包括：

启动模块，用于显示欢迎词条，新建或选择文件，以及完成参数初始化；

示教模块，用于绘出运行轨迹的图形，并对图形进行编辑；

加工模块，用于进行加工指令的处理；以及

数据处理模块，用于根据编辑完成的运行轨迹计算出所需要的控制信号，并输出控制信号。

所述系统还包括测试模块，用于进行输入IO状态、输出IO状态和轴动作的测试。

所述系统还包括系统模块，用于显示版本号和日期，以及进行文件删除操作。

进一步地，所述示教模块对图形进行编辑，包括删除段、阵列复制段和移动所选段。

进一步地，所述加工指令包括描点、启动/停止、暂停/继续、复位和数据显示；

执行描点指令，即在所述示教模块生成的轨迹图形上实时显示目标当前位置；

执行数据显示指令，即在屏幕上显示单次运行时间、当前加工件数、预置加工件数。

为进一步详述本专利，下面结合具体实施例进行拓展说明。

本发明的机器人运动控制方法应用于机器人的控制器中，本方法采用了独创的单脉冲预置实时矢量插补算法，该算法分两部分：输入预设参数；当前脉宽时间内计算出下一个脉冲的脉宽。

其中，预设参数包括：起点坐标(x₁,y₁,z₁)、中间点坐标(对于高阶方程有多个中间点坐标)、终点坐标(x₂,y₂,z₂)、曲线类型(空间上的直线、圆弧、椭圆、双曲线及任意曲线)、起始速度v_star、恒定速度v_stable、结束速度v_end、加速度a、减速度d、脉冲当量ΔS。中间点坐标和曲线类型结合起来就是运动轨迹。

当前脉宽为当前实时输出的脉冲宽度(即一个脉冲时间)，在这个时间段内根据上述的预设参数，计算出下一个脉冲的宽度(即下一个脉冲的时间)；然后在输出下一个脉冲的时间内计算出再下一个脉冲的宽度，直至完成规划的曲线路径。

在优选的实施方案中，本发明利用摇杆来输入预设参数。输入装置利用成熟的游戏手柄相关技术，采用两个摇杆，每个摇杆可以输入两个方向上的数据，则两个摇杆可以对应X、Y、Z、R四个轴。其中X、Y、Z是空间坐标系的三个轴，R代表旋转轴。

需要进一步说明的是，摇杆是对控制对象的一个定位控制，只在示教操作时有效，移动摇杆的时候，控制对象在空间上是实时移动(以方便操作人员对实际对象的位置进行定位)，同时在会在屏上画出轨迹(根据曲线类型和中间点)、起点和终点。轨迹和动作指令完成后，会下载到控制器的存储器存储。在加工操作时，摇杆取消控制；点击启动后，控制器将实时控制对象根据示教好的轨迹和动作指令进行移动和动作。

现以直线为例对具体计算过程加以说明，圆弧与其它图形轨迹与直线的情形类似。输入起点(x₁,y₁,z₁)，终点(x₂,y₂,z₂)，及运动轨迹(直线、圆弧或其它)，可以构造位移矢量。将位移矢量投射到三个轴上，并计算出轴夹角α、β、γ。根据脉冲当量、轴夹角和速度要求(全部是矢量)可以计算出相应的投射到三个轴上的分量。

比如计算投射到X轴上的分量，只需将加速距离S_acc、减速距离S_dec、恒速距离S_stable分别乘以sinα即得。

计算出三个距离后，进入当前脉宽中断，并在一个脉宽时间内计算出下一个脉冲的脉宽：

其中，V_n是计算出来的当前的速度投射到X轴后的一个的分量。

需要说明的是，这是以直线为例的计算，复杂曲线的计算中，V_n和T_n需要用到微积分和矢量公式(高数和线性代数)。

轴向下一速度：它是当前速度V_n的下一个脉冲速度。

其中，PPU是当量脉冲，即一个单位(mm)需要多少个脉冲，其倒数就是脉冲当量ΔS，即一个脉冲走多少距离(mm)。

这样下一个脉宽根据要求输出，并在下一个脉宽时间内计算再下一次的脉宽，三轴同步输出后即完成桌面机器人的运动轨迹控制。

这个计算方法，是在当前脉宽时间内来计算下一个脉宽，保证了在一个脉冲当量内，控制精度即为小于一个脉冲当量。较之目前的逼近算法，减少了一个计时器来计算速度要求和距离，大几幅度减少因两个计时器计算(一个计算目标值的速度和距离，一个计算脉宽)带来的较大误差，以及运行时的累计误差。这个算法只存在机械和电机的误差，而没有计算误差，大大提高桌面机器人的运动和定位精度。

本发明的人机交互采用了电容触摸屏，并针对控制器和示教的要求开发出基于Android操作系统的APP应用，这套应用程序使操作人员可以通过屏幕上的轨迹图形的触控直接完成选线、选点、复制、粘贴、删除点/线的图形操作，通过控件的操作可以完成参数设置、设备的启/停等一系列动作操作。后续这套APP应用可以直接使用到手机、平板上，通过人们常用的消费类设备即可完成机器人的运动控制。

本发明的控制系统包括以下部分：

启动模块：欢迎词条、文件新建或选择、根据新建或选择分别进入示教或加工模块、完成参数初始化。

示教模块：进行绘图和图形操作，包括以下模块：轨迹绘图、各段动作指令参数设置窗口、图形处理(删除段、阵列复制段、移动所选段等图形处理)、各段运动指令参数设置、示教动作(单步、点/线切换等)。

各段动作指令参数设置窗口，指的是控制对象除移动外需要控制的指令参数，例如焊锡机上：抖枪次数(焊枪焊完后提起前的抖动动作，以匀合锡)、抖枪方向(即在哪个方向进行抖枪，X、Y或者Z轴)、斜枪进入坐标(为避开移动路径上的阻碍，从目标点的斜上方以倾斜的方式到达目标点)等等。

单步：指的是在示教时，轨迹图和动作规划一些或者完全规划后，需要一步一步来检查精度和坐标，单步就是点击一下，控制器控制对象完成一段的轨迹和动作指令。

加工模块：进行加工指令的处理，包括以下模块：描点与窗口(在示教生成的轨迹图形上实时显示目标当前位置)、加工指令模块(启动/停止、暂停/继续、复位等)、加工数据显示模块(单次运行时间、当前加工件数、预置加工件数)。

如图3所示，本发明的系统中，加工模块对轨迹图进行加工后，将相关参数发送到数据处理模块；由数据处理模块计算输出脉冲的脉宽。

测试模块：分三个模块：输入IO状态、输出IO状态、轴动作测试模块。

系统模块：显示系统参数，包括：版本号、日期、文件删除等。

以上所述，仅为本发明的具体实施方式，但本发明的保护范围并不局限于此，任何熟悉本技术领域的技术人员在本发明揭露的技术范围内，可轻易想到变化或替换，都应涵盖在本发明的保护范围之内。因此，本发明的保护范围应以所述权利要求的保护范围为准。

Claims (10)

1.机器人的运动控制方法，其特征在于，包括以下步骤：

S1：输入预设参数，并构造出位移矢量；

S2：将位移矢量投射到三个坐标轴上，根据预设参数分别计算出三个轴上所对应的脉冲的脉宽；

S3：三轴同步输出计算所得的脉冲，同时计算出下一个脉冲的脉宽；

S4：重复上一步骤直至完成预设的运动轨迹。

2.根据权利要求1所述的机器人的运动控制方法，其特征在于，计算脉宽的具体方法为：

<mrow> <msub> <mi>T</mi> <mrow> <mi>n</mi> <mo>+</mo> <mn>1</mn> </mrow> </msub> <mo>=</mo> <mfrac> <mrow> <mi>&amp;Delta;</mi> <mi>S</mi> </mrow> <msub> <mi>V</mi> <mi>n</mi> </msub> </mfrac> <mo>;</mo> </mrow>

其中，T_n+1为任意一个坐标轴所对应的第n+1个脉冲的脉宽；V_n为同一坐标轴上的第n个脉冲所对应的速度；ΔS为脉冲当量，n为自然数。

3.根据权利要求2所述的机器人的运动控制方法，其特征在于，所述步骤S2具体包括：

S201：将位移矢量投射到三个坐标轴上，然后分别计算出该位移矢量与三个轴的夹角α、β和γ；

S202：根据输入的预设参数计算出加速距离、减速距离和恒速距离；

S203：分别用sinα、sinβ和sinγ乘以加速距离，得出加速距离在三个轴上的对应分量；同样方式计算出减速距离和恒速距离在三个轴上的对应分量；

S204：根据任意一个坐标轴上的三个距离分量，推算出该轴所对应的速度分量V_n，再计算出该轴所对应的脉冲的脉宽；

S205：重复上一步骤，计算出另外两个坐标轴所对应的脉冲的脉宽。

4.根据权利要求3所述的机器人的运动控制方法，其特征在于，所述步骤S202的计算方法是：

加速距离

减速距离

恒速距离

其中，a是加速度，d是减速度，v_star是起始速度、v_stable是恒定速度、v_end是结束速度，(x₁,y₁,z₁)是起点坐标，(x₂,y₂,z₂)是终点坐标，以上均为预设参数。

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

定义S_current＝ΔS×n；

若S_current＜S_acc，则处于加速段，推算方法为V_n+1＝V_n+a×T_n+1；

若S_acc≤S_current＜S_acc+S_stable，则处于恒速段，推算方法为V_n+1＝v_stable；

否则处于减速段，推算方法为V_n+1＝V_n-d×T_n+1；

其中，n为自然数，V₀＝v_star。

6.机器人的运动控制系统，其特征在于，包括：

启动模块，用于显示欢迎词条，新建或选择文件，以及完成参数初始化；

示教模块，用于绘出运行轨迹的图形，并对图形进行编辑；

加工模块，用于进行加工指令的处理；以及

数据处理模块，用于根据编辑完成的运行轨迹计算出所需要的控制信号，并输出控制信号。

7.根据权利要求6所述的机器人的运动控制系统，其特征在于：还包括测试模块，用于进行输入IO状态、输出IO状态和轴动作的测试。

8.根据权利要求6所述的机器人的运动控制系统，其特征在于：还包括系统模块，用于显示版本号和日期，以及进行文件删除操作。

9.根据权利要求6所述的机器人的运动控制系统，其特征在于：所述示教模块对图形进行编辑，包括删除段、阵列复制段和移动所选段。

10.根据权利要求6所述的机器人的运动控制系统，其特征在于：所述加工指令包括描点、启动/停止、暂停/继续、复位和数据显示；

执行描点指令，即在所述示教模块生成的轨迹图形上实时显示目标当前位置；

执行数据显示指令，即在屏幕上显示单次运行时间、当前加工件数、预置加工件数。