CN105082135A - 一种机器人点动操作的速度控制方法 - Google Patents

Abstract

本发明公开了一种机器人点动操作的速度控制方法，该方法将点动操作的速度控制过程划分为两个阶段：加速&匀速段和停止段。其中，加速&匀速段包括周期变加速段、加速-匀速过渡段以及匀速段三个子阶段，加速-匀速过渡段和匀速段是可缺省的；每个子阶段以固定长度的时间片为单位，对轨迹的插补增量进行规划。本发明能够实现微小距离点动，并有效避免手动示教过程中点动操作引起的过大机械冲击，以及高速点动无法及时平滑停止等问题；可应用于机器人各坐标系空间下手动示教点动操作的速度控制。

Description

一种机器人点动操作的速度控制方法

技术领域

本发明涉及机器人示教模式下点动操作的一种速度控制方法，属于工业机器人技术。

背景技术

手动示教模式下点动(JOG)操作是机器人示教人员在各类关节坐标系空间或笛卡尔坐标系、工具坐标系和用户坐标系等空间下驱动机器人运动到达示教点的操作。点动运行时，控制器需要配合示教过程的按键状态，实时和同步地进行插补计算和执行。传统控制器采用一种直观的解决方案：控制器在每个实时控制周期计算下一个插补点位置，并在同一周期内完成执行任务。然而，这种方案存在以下缺点：①点动速度并不平滑，会产生机械冲击；②开始运动加速度一般很大，无法实现微小位移点动；③只考虑到单个周期内的运动范围，若当前运动速度很大时，可能因无法及时停止而超出运动限位。

发明内容

发明目的：为了克服现有技术中存在的不足，本发明针对伺服电机处于位置控制模式的情况，提供一种机器人手动示教模式下点动操作的速度控制方法，为实现机器人关节坐标系空间下的单轴平滑点动操作提供一种轨迹规划步骤和程序实现方法，同时为实现笛卡尔坐标系、工具坐标系和用户坐标系等空间下的移动点动和转动点动操作提供一种轨迹规划步骤和程序实现方法。本发明方法能使点动过程速度平滑，能有效降低对机械结构的冲击、磨损以及减少运动能耗，实现微小距离点动，并解决高速点动无法及时平滑停止的问题。

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

一种机器人点动操作的速度控制方法，将点动操作的速度控制过程划分为加速&匀速段和停止段，其中加速&匀速段包括周期变加速段、加速-匀速过渡段和匀速段三个子阶段，加速-匀速过渡段和匀速段为两个可或缺的子阶段，每个子阶段以固定长度的时间片为单位进行轨迹插补规划，且各个时间片内插补增量变化的加速度连续，且每个时间片的起始增量加速度和终止增量加速度均为零；每个时间片由连续的若干个控制周期组成，每个时间片完成后，根据判断条件确定下一时刻采用加速&匀速段插补序列还是停止段插补序列，其中判断条件包括：①当前控制过程处于哪个子阶段，②点动操作是否结束，③机器人运动是否在可达范围内，④机器人是否产生碰撞。

周期变加速段、加速-匀速过渡段和匀速段的状态跳转关系如图1所示：当点动操作时间较长时，点动操作包含上述三个子阶段和停止段；当点动操作时间较短时，加速-匀速过渡段和匀速段可能或缺。各阶段插补增量的动态指标如图2所示，点动的插补序列点由初始位置域插补增量相加确定。该方法的具体包括如下部分：

Part1：记录初始位置或位姿信息

控制器在接收到点动请求命令后，记录初始位置或位姿信息。若是在关节空间下执行单轴点动操作，点动的初始位置为点动轴的初始角度；若是在笛卡尔坐标系、工具坐标系和用户坐标系等空间下执行单轴点动操作，点动的初始位姿为机器人工具末端点(ToolCenterPoint，TCP)在参考坐标系下位姿变换矩阵。

Part2：参数的定义

以i＝1,2,…,X表示加速&匀速段的所有时间片，时间片的时间长度为T₂₀：第i个时间片的起始增量为θ_i0，对应的起始增量速度和加速度分别为和θ；第i个时间片的中点增量为θ_ih，对应的中点增量速度和加速度分别为和，同时为第i个时间片的加速度峰值；第i个时间片的终止增量为θ_if，对应的终止增量速度和加速度分别为和匀速段的给定速度为；插补时间为t；初始时i＝1；

停止段的起始增量为θ₀₀，对应的起始增量速度和加速度分别为和停止段的终止增量为θ_0f，对应的终止增量速度和加速度分别为和

Part3：插补规则设计

在执行第i个时间片的插补序列时，同时计算第(i+1)个时间片的插补序列，目的是为了能够在第i个时间片插补完成时能立即执行第(i+1)个时间片的插补序列，保证运动的连贯性；所述第(i+1)个时间片的插补序列分为两类：①加速&匀速段的插补序列Normal和停止段的插补序列Ending；

所述插补序列Normal分为三种情况：①若第(i+1)个时间片的加速度峰值没有达到最大加速度a_max，第(i+1)个时间片的速度也没有达到速度峰值则第(i+1)个时间片为周期变加速段，采用周期变加速段插补方法计算第(i+1)个时间片的插补序列；②若第(i+1)个时间片的加速度峰值达到最大加速度a_max，但是第(i+1)个时间片的速度没有达到速度峰值则第(i+1)个时间片为加速-匀速过渡段，采用加速-匀速过渡段插补方法计算第(i+1)个时间片的插补序列；③若第(i+1)个时间片的速度达到速度峰值则第(i+1)个时间片为匀速段，采用匀速段插补方法计算第(i+1)个时间片的插补序列；

由于不知道点动操作何时停止，因此在执行每一个时间片的插补序列的同时都需要计算插补序列Ending，插补序列Ending的起点为第i个插补序列Normal的延续。

Part4：周期变加速段的时间片的插补序列的计算和执行

控制程序开启插补序列的计算过程，首先计算周期变加速段的第i个时间片的插补序列，周期变加速段的插补增量曲线函数确定方法如下：

周期变加速段可以包括若干个时间片；在周期变加速段内，相邻两个时间片的加速度峰值依次递增固定值△a，即要求第(i+1)个时间片规划后所得的加速度峰值不超过最大加速度a_max。

由于周期变加速段的每个时间片的θ_i0、和可以看作是确定量，T₂₀为固定值，因此可以考虑使用四阶多项式对周期变加速段内的第i个时间片的插补量θ_i(t)进行插值，即：

θ_i(t)＝ω_i0+ω_i1t+ω_i2t²+ω_i3t³+ω_i4t⁴

其中ω_i0、ω_i1、ω_i2、ω_i3和ω_i4为多项式系数，其约束条件为：

$\left\{\begin{array}{c}{\mathrm{\θ}}_{i0}={\mathrm{\ω}}_{i0}\\ {\stackrel{\·}{\mathrm{\θ}}}_{i0}={\mathrm{\ω}}_{i1}\\ {\stackrel{\·\·}{\mathrm{\θ}}}_{i0}=2{\mathrm{\ω}}_{i2}\\ {\stackrel{\·\·}{\mathrm{\θ}}}_{ih}=2{\mathrm{\ω}}_{i2}+6{\mathrm{\ω}}_{i3}\left(\frac{T_{20}}{2}\right)+12{\mathrm{\ω}}_{i4}{\left(\frac{T_{20}}{2}\right)}^2\\ {\stackrel{\·\·}{\mathrm{\θ}}}_{if}=2{\mathrm{\ω}}_{i2}+6{\mathrm{\ω}}_{i3}{T}_{20}+12{\mathrm{\ω}}_{i4}{T}_{20}^2\end{array}\right.$

求解得到对周期变加速段内的第i个时间片的插补量θ_i(t)进行插值的函数为：

${\mathrm{\θ}}_i\left(t\right)={\mathrm{\θ}}_{i0}+{\stackrel{\·}{\mathrm{\θ}}}_{i0}t+\frac{2{\stackrel{\·\·}{\mathrm{\θ}}}_{ih}}{3T_{20}}{t}^3-\frac{{\stackrel{\·\·}{\mathrm{\θ}}}_{ih}}{3T_{20}^2}{t}^4$

Part5：加速-匀速过渡段时间片的插补序列的计算和执行

由于加速-匀速过渡段的时间片的θ_i0、和可以看作是确定量，因此同样可以考虑使用四阶多项式对加速-匀速过渡段的第i个时间片的插补量θ_i(t)进行插值，即：

θ_i(t)＝ω_i0+ω_i1t+ω_i2t²+ω_i3t³+ω_i4t⁴

其中ω_i0、ω_i1、ω_i2、ω_i3和ω_i4为多项式系数，其约束条件为：

$\left\{\begin{array}{c}{\mathrm{\θ}}_{i0}={\mathrm{\ω}}_{i0}\\ {\stackrel{\·}{\mathrm{\θ}}}_{i0}={\mathrm{\ω}}_{i1}\\ {\stackrel{\·\·}{\mathrm{\θ}}}_{i0}=2{\mathrm{\ω}}_{i2}\\ {\stackrel{\·\·}{\mathrm{\θ}}}_{if}={\mathrm{\ω}}_{i0}+2{\mathrm{\ω}}_{i1}{T}_{20}+3{\mathrm{\ω}}_{i2}{T}_{20}^2+4{\mathrm{\ω}}_{i3}{T}_{20}^3\\ {\stackrel{\·\·}{\mathrm{\θ}}}_{if}=2{\mathrm{\ω}}_{i2}+6{\mathrm{\ω}}_{i3}{T}_{20}+12{\mathrm{\ω}}_{i4}{T}_{20}^2\end{array}\right.$

求解得到对加速-匀速过渡段的第i个时间片的插补量θ_i(t)进行插值的函数为：

${\mathrm{\θ}}_i\left(t\right)={\mathrm{\θ}}_{i0}+{\stackrel{\·}{\mathrm{\θ}}}_{i0}t+\frac{{\stackrel{\·}{\mathrm{\θ}}}_{if}-{\stackrel{\·}{\mathrm{\θ}}}_{i0}}{T_{20}^2}{t}^3-\frac{{\stackrel{\·}{\mathrm{\θ}}}_{if}-{\stackrel{\·}{\mathrm{\θ}}}_{i0}}{2T_{20}^3}{t}^4$

Part6：匀速段时间片的插补序列的计算和执行

由于匀速段的时间片的θ_i0和可以看作是确定量，因此同样可以考虑使用一次线性多项式对匀速段内的第i个时间片的插补量θ_i(t)进行插值，即：

θ_i(t)＝ω_i0+ω_i1t

其中ω_i0和ω_i1为多项式系数，其约束条件为：

$\left\{\begin{array}{c}{\mathrm{\θ}}_{i0}={\mathrm{\ω}}_{i0}\\ {\stackrel{\·}{\mathrm{\θ}}}_{i0}={\mathrm{\ω}}_{i1}\end{array}\right.$

求解得到对匀速段内的第i个时间片的插补量θ_i(t)进行插值的函数为：

${\mathrm{\θ}}_i\left(t\right)={\mathrm{\θ}}_{i0}+{\stackrel{\·}{\mathrm{\θ}}}_{i0}t$

Part7：停止段的插补序列的计算和执行

若点动操作结束，则注销计算进程；若开启了执行线程，等待停止段序列执行完毕后，注销执行线程，等待下一次点动请求。

一旦发现点动操作结束、超出机器人可达范围或发生碰撞，则直接跳转到停止段；在停止段，点动操作以运行时间最短而且平滑的方式将速度降为0，在停止段不再使用时间片进行分割，而使用运行允许的最大减速度-a_Decel,max(-a_Decel,max为正值)作为减速度峰值进行规划。

由于停止段的θ₀₀、 ${\stackrel{\·}{\mathrm{\θ}}}_{00},{\stackrel{\·\·}{\mathrm{\θ}}}_{00}(=0),{\stackrel{\·\·}{\mathrm{\θ}}}_{0h}(=-a_{Decel,max}),{\stackrel{\·}{\mathrm{\θ}}}_{0f}(=0)$ 和可以看做是确定量，但是运行时间△t是一个未知变量，同样可以考虑使用四阶多项式对停止段的插补量θ₀(t)进行插值，即：

θ₀(t)＝ω₀₀+ω₀₁t+ω₀₂t²+ω₀₃t³+ω₀₄t⁴

其中ω₀₀、ω₀₁、ω₀₂、ω₀₃和ω₀₄为多项式系数，其约束条件为：

$\left\{\begin{array}{c}{\mathrm{\θ}}_{00}={\mathrm{\ω}}_{00}\\ {\stackrel{\·}{\mathrm{\θ}}}_{00}={\mathrm{\ω}}_{01}\\ {\stackrel{\·\·}{\mathrm{\θ}}}_{00}=2{\mathrm{\ω}}_{02}\\ {\stackrel{\·\·}{\mathrm{\θ}}}_{0h}=2{\mathrm{\ω}}_{02}+6{\mathrm{\ω}}_{03}\left(\frac{\mathrm{\Δ}t}{2}\right)+12{\mathrm{\ω}}_{04}{\left(\frac{\mathrm{\Δ}t}{2}\right)}^2\\ {\stackrel{\·}{\mathrm{\θ}}}_{0f}={\mathrm{\ω}}_{00}+2{\mathrm{\ω}}_{01}\mathrm{\Δ}t+3{\mathrm{\ω}}_{02}{\mathrm{\Δt}}^2+4{\mathrm{\ω}}_{03}{\mathrm{\Δt}}^3\\ {\stackrel{\·\·}{\mathrm{\θ}}}_f=2{\mathrm{\ω}}_2+6{\mathrm{\ω}}_3\mathrm{\Δ}t+12{\mathrm{\ω}}_4{\mathrm{\Δt}}^2\end{array}\right.$

求解得到对停止段的插补量θ₀(t)进行插值的函数为：

${\mathrm{\θ}}_0\left(t\right)={\mathrm{\θ}}_{00}+{\stackrel{\·}{\mathrm{\θ}}}_{00}t-\frac{4a_{Decel,max}^2}{9{\stackrel{\·}{\mathrm{\θ}}}_{00}}{t}^3+\frac{4a_{Decel,max}^3}{27{\stackrel{\·}{\mathrm{\θ}}}_{00}^2}{t}^4;$

停止段的运行时间为：

有益效果：本发明提供的机器人点动操作的速度控制方法，相对于现有技术，具有如下优势：1、改善点动操作的运动性能。相比于传统控制器的点动解决方案，本发明点动的规划结果加速度曲线连续，速度平滑，能够有效降低点动过程中的机械冲击与磨损，延长设备寿命；点动初始阶段加速度较小并逐渐加大，能够实现微小位移点动，更便于用户对机器人进行示教操作；以时间片为单位，规划过程充分考虑点动能否在可达范围内停止，在算法层面避免了机器人高速点动容易越出运动软限位的问题。2、具有通用性。本发明并没有涉及到机器人的具体类型，对机器人示教的点动操作实现方法具有通用性。

附图说明

图1为点动轨迹规划的状态跳转示意图。程序最初处于等待点动请求状态，当检测到按键信号，跳转到周期变加速段状态；若按键时间较长，将通过加速-匀速过渡段进入匀速段状态。过程中，一旦检测到按键松开或有不可达插补点，将进入停止段状态。图中：①检测到点动请求信号；②下一时间片加速度递增(但不得超过最大加速度)，规划所得的终止速度仍小于给定匀速段速度；③下一时间片加速度若递增(但不得超过最大加速度)，规划所得的终止速度将超过给定匀速段速度；④未收到点动结束请求，且下一时间片运动可达；⑤收到点动结束请求，或下一时间片有不可达点。

图2为本发明的四阶段插补增量及其变化速度、变化加速度和变化加加速度的规划示意图。

图3为本发明的用户主动停止情形下，计算线程与执行线程的同步举例。

图4为本发明的有不可达点情形下，计算线程与执行线程的同步举例。

图5为本发明点动过程中各个时间片插补序列的计算与执行情况。图中：①初始化：接收到点动请求，开始计算第一个时间片的两段插补点序列；若计算成功，开始执行序列。②时间片间过渡：“继续运行标志位”有效，即检测到下一时间片的两段插补点序列计算完成，说明这些路径点均可达；于是丢弃上一时间片计算得到的停止段序列，并执行新的加速(匀速)段。③停止信号：“继续运动标志位”被清零。④停止：“继续运动标志位”失效，不再计算新的两段序列，下阶段执行停止段序列。

图6为本发明两种不同的点动停止(用户主动停止/有不可达点)条件下插补序列的计算与执行结果。

图7以垂直六轴关节型工业机器人为例，展示关节坐标系空间下单轴点动操作的示意图。

图8为笛卡尔坐标系空间下，机器人工具末端点TCP沿着笛卡尔坐标系的X、Y或Z轴移动，或绕着笛卡尔坐标系的X、Y或Z轴转动，进行点动操作的示意图。

图9为工具坐标系空间下，机器人工具末端点TCP沿着工具坐标系的X、Y或Z轴移动，或绕着工具坐标系的X、Y或Z轴转动，进行点动操作的示意图。

具体实施方式

下面结合附图对本发明作更进一步的说明。

本发明实施的关键在于控制程序对机器人点动插补序列的计算线程与执行线程的同步管理。当控制器接收到点动请求信号时，程序相继开启插补序列的计算线程与执行行程；循环计算与执行若干个加速&匀速段时间片的插补序列后，当检测到用户主动停止点动(松开按键)或机器人运动有不可达点(超出机器人可达范围或将发生碰撞)，执行停止段序列，依次注销计算进程与执行线程，等待下一次点动请求。时间片的长度对程序而言是固定的，需要程序开发人员根据控制器实际情况决定，举例：某控制器实时周期性控制时钟为8毫秒，可考虑以20个周期为一个时间片，即每个时间片T₂₀为160毫秒。

图3和图4分别是用户主动停止情形下、有不可达点情形下，采用本发明技术方案的实现步骤，计算行程与执行线程同步的典型举例。在运行时间片内，当执行线程接收到按键松开消息或计算失败消息，会将“继续运动标志位”清零，下个时间片决策为执行停止段插补序列。下一时间片到底是执行新的加速(匀速)段序列，还是执行当前时间片紧接的停止段序列，是控制程序在每个时间片插补序列执行结束时通过读取“继续运动标志位”的状态决定的。点动过程中各个时间片插补序列的计算与执行结果如图5和图6所示。

下面以关节坐标系空间下的单轴点动(图7)、笛卡尔坐标系空间下的移动和转动点动(图8)、以及工具坐标系空间下的移动和转动点动(图9)来进一步介绍本发明的实现步骤。

使用N表示每个时间片包含的周期数，举例：N＝20；使用m表示停止段占用的周期数(△t为停止段的运行时间；T为控制周期时间，举例：T＝8ms)。

(1)关节坐标系空间下的单轴点动(图7)

Step1：记录点动轴的初始位置或关节角度P₀；

Step2：计算插补序列

对周期变加速段内的第i个时间片的插补量θ_i(t)进行插值的函数为对加速-匀速过渡段的第i个时间片的插补量θ_i(t)进行插值的函数为对匀速段内的第i个时间片的插补量θ_i(t)进行插值的函数为对停止段的插补量θ(t)进行插值的函数为

${\mathrm{\θ}}_0\left(t\right)={\mathrm{\θ}}_{00}+{\stackrel{\·}{\mathrm{\θ}}}_{00}t-\frac{4a_{Decel,max}^2}{9{\stackrel{\·}{\mathrm{\θ}}}_{00}}{t}^3+\frac{4a_{Decel,max}^3}{27{\stackrel{\·}{\mathrm{\θ}}}_{00}^2}{t}^4.$

若在第i个时间片两端插补序列存在不可达点(超出点动轴限位或将导致机器人碰撞)，或检测到此时按键已松开，直接跳转至Step4，进入停止段，点动轴不动作；否则，控制程序将开启插补序列的执行程序，并继续进行插补序列的计算。

对加速&匀速段增量函数取N个插补时刻t＝0,T,2T,…(N-1)T，得到加速&匀速段的N个插补增量△_k(k＝1,2,…,N)；记录最后插补点的增量大小△_N和增量速度对停止段增量函数取m+1个插补点时刻t＝0,T,2T,…(m-1)T,△t，得到停止段的m+1个插补增量△_k(k＝N+1,N+2,…,N+m,N+m+1)，第一个时间片的起始增量和起始增量速度均为0，对于周期变加速段：时间片的中点增量加速度不能超过运动允许的最大加速度a_max；第一个时间片的中点增量加速度可以取一个较小值，举例：0.03×a_max；相邻两个时间片加速度递增量可以取一个合适值，举例：0.05×a_max。

对于关节坐标系空间下的单轴点动，周期变加速段时间片的中点增量加速度不能超过点动轴运动允许的最大角加速度或线加速度，加速-匀速过渡段时间片的终止增量速度对应为指令给定的点动轴期望运动速度，停止段的中点增量减速度对应点动轴运动允许的最大角减速度或线减速度。由P₀+△_k可以计算得到第一个时间片的N个加速&匀速段插补序列点的轴关节给定值，以及与该时间片紧接的m+1个停止段插补序列点的轴关节给定值。

Step3：执行已经计算好的时间片插补序列，同步返回Step2计算下一个时间片插补序列。

执行线程读取并执行已经计算好的当前时间片加速(匀速)段序列；与此同时，计算线程开始计算下一时间片的两段插补序列，记录两段中的加速(匀速)段最后插补点的增量大小和增量速度，并更新点动过程的运动阶段状态。

更新点动过程的运动阶段状态需依据图1的状态流程图导向，插补序列计算过程中，下一时间片的起始增量大小和角速度/线速度分别为当前时间片加速(匀速)段最后插补点的增量大小和角速度/线速度。

Step4：点动结束

注销计算进程；若开启了执行线程，等待停止段序列执行完毕后，注销执行线程。等待下一次点动请求。

(2)笛卡尔坐标系空间下的移动和转动点动(图8)

笛卡尔坐标系空间下的点动以笛卡尔坐标系为参考坐标系，机器人工具末端点TCP可以是沿坐标轴X/Y/Z进行+X/-X/+Y/-Y/+Z/-Z方向的移动点动，也可以是绕着坐标轴X/Y/Z进行+Rx/-Rx/+Ry/-Ry/+Rz/-Rz方向的转动点动。具体步骤如下：

Step1：记录机器人TCP的初始位姿

在接收到用户点动请求后，首先记录各关节位置，通过求解机器人的运动学正解，得到机器人当前工具末端点TCP的4×4变换矩阵为T₀，R₀为表示姿态的3×3坐标旋转矩阵，p₀为表示位置的3×1位置偏移向量。

Step2：计算第一个时间片的插补序列

首先需要根据是移动点动还是转动点动，明确点动的规划对象。

若是移动点动，规划对象为TCP在坐标轴X/Y/Z方向上的位置。以沿着X轴正方向移动为例，若沿+X方向上的位置增量为△_j(j＝1,2,…,N)，则移动维度上产生的路径位置偏移增量为转动维度上没有变化，对应的旋转矩阵增量为单位矩阵：△R_j＝I_3×3。因此，增量变换矩阵为运动的参考坐标系为笛卡尔坐标系，插补点的对应变换矩阵为原变换矩阵T₀左乘增量变换矩阵△T_j，即：

$T_j={\mathrm{\ΔT}}_j\·T_0=\left[\begin{array}{cc}{I}_{3\×3}& {\mathrm{\Δp}}_j\\ 0& 1\end{array}\right]\left[\begin{array}{cc}{R}_0& p_0\\ 0& 1\end{array}\right]=\left[\begin{array}{cc}{R}_0& p_0+{\mathrm{\Δp}}_j\\ 0& 1\end{array}\right]$

若是转动点动，规划对象为TCP绕着坐标轴X/Y/Z的正或反方向旋转的角度大小。以绕着X轴正方向转动为例，若绕+X方向的旋转角度增量为则转动维度上产生的旋转矩阵增量移动维度上没有变化，对应的位置偏移增量为零向量：因此，增量变换矩阵为运动的参考坐标系为笛卡尔坐标系，对应插补点的对应变换矩阵为原变换矩阵T₀左乘增量变换矩阵即：

${\tilde{T}}_j=\mathrm{\Δ}{\tilde{T}}_j\·T_0=\left[\begin{array}{cc}\mathrm{\Δ}{\tilde{R}}_j& {\mathrm{\Θ}}_{3\×1}\\ 0& 1\end{array}\right]\left[\begin{array}{cc}{R}_0& p_0\\ 0& 1\end{array}\right]=\left[\begin{array}{cc}\mathrm{\Δ}{\tilde{R}}_j{R}_0& \mathrm{\Δ}{\tilde{R}}_j{p}_0\\ 0& 1\end{array}\right]$

明确规划对象后，本步骤的具体内容可以描述为：开启计算线程，计算第一个时间片的两段插补序列，并记录两段序列中加速&匀速段(即周期变加速段)的最后插补点的增量大小以及增量速度。若两段插补序列中存在不可达点(超出机器人可达范围或将发生碰撞)，或检测到此时按键已松开，跳转到Step5，机器人不动作。否则控制程序将开启插补序列的执行线程，进入Step3。

两段插补序列插补增量的计算方法与第一种情况计算两段插补序列插补增量的方法是相同的。利用移动点动计算式或转动点动计算式可以计算各个插补点的变换矩阵，通过求解机器人的运动学逆解，可以得到机器人的各轴关节给定值，作为某个插补点的一组插补序列；因而，可以计算得到第一个时间片的N组加速(匀速)段插补序列点的轴关节给定值，以及与该时间片紧接的m+1组停止段插补序列点的轴关节给定值。

Step3：执行已经计算好的时间片插补序列，同步计算下一个时间片插补序列。

执行线程读取并执行已经计算好的当前时间片加速&匀速段序列；与此同时，计算线程开始计算下一时间片的两段插补序列，记录两段中的加速&匀速段最后插补点的增量大小和增量速度，并更新点动过程的运动阶段状态。

更新点动过程的运动阶段状态需依据图1的状态流程图导向，插补序列计算过程中，下一时间片的增量大小以及增量速度分别为当前时间片加速&匀速段最后插补点的增量大小以及增量速度。

Step4：重复Step3，循环执行与计算时间片插补序列，直到满足停止条件。

下一个时间片两段插补序列计算完毕后，若各组序列点下机器人均处于可达范围内，计算线程等待执行线程将当前时间片的加速&匀速段序列插补点执行完毕；当执行完毕后，若按键仍处于按下状态，返回并重复Step3进行后续时间片插补序列的同步计算与执行。若下一个时间片的两段插补序列存在不可达点，或执行完毕后检测到按键松开，注销计算线程，下一时间片开始执行当前时间片紧接的停止段序列。

Step5：点动结束。

注销计算进程；若开启了执行线程，等待停止段序列执行完毕后，注销执行线程。等待下一次点动请求。

(3)工具坐标系空间下的移动和转动点动(图9)

工具系空间下的点动以工具坐标系为参考坐标系，机器人工具末端点TCP可以是沿工具坐标系坐标轴X/Y/Z进行+X/-X/+Y/-Y/+Z/-Z方向的移动点动，也可以是绕着工具坐标轴X/Y/Z进行+Rx/-Rx/+Ry/-Ry/+Rz/-Rz方向的转动点动。

具体步骤与第(2)种情况笛卡尔坐标系空间下移动和转动点动的实现步骤Step1至Step5是非常类似的，唯一区别在于Step2和Step3中对插补点变换矩阵T_j或的计算方法。

若是移动点动，规划对象为TCP在工具坐标系坐标轴X/Y/Z方向上的位置。以沿着工具坐标系X轴正方向移动为例，若沿工具坐标系+X方向上的位置增量为△_j(j＝1,2,…,N)，则移动维度上产生的路径位置偏移增量为转动维度上没有变化，对应的旋转矩阵增量为单位矩阵：△R_j＝I_3×3。因此，增量变换矩阵为运动的参考坐标系为工具坐标系，转换到笛卡尔坐标空间下描述，插补点的对应变换矩阵为原变换矩阵T₀右乘增量变换矩阵△T_j，即：

$T_j=T_0\·{\mathrm{\ΔT}}_j=\left[\begin{array}{cc}{R}_0& p_0\\ 0& 1\end{array}\right]\left[\begin{array}{cc}{I}_{3\×3}& {\mathrm{\Δp}}_j\\ 0& 1\end{array}\right]=\left[\begin{array}{cc}{R}_0& R_0{\mathrm{\Δp}}_j+p_0\\ 0& 1\end{array}\right]$

若是转动点动，规划对象为TCP绕着工具坐标系坐标轴X/Y/Z的正或反方向旋转的角度大小。以绕着关节坐标系X轴正方向转动为例，若绕工具坐标系+X方向的旋转角度增量为则转动维度上产生的旋转矩阵增量移动维度上没有变化，对应的位置偏移增量为零向量：因此，增量变换矩阵为运动的参考坐标系为工具坐标系，转换到笛卡尔坐标空间下描述，插补点的对应变换矩阵为原变换矩阵T₀右乘增量变换矩阵即：

${\tilde{T}}_j=T_0\·\mathrm{\Δ}{\tilde{T}}_j=\left[\begin{array}{cc}{R}_0& p_0\\ 0& 1\end{array}\right]\left[\begin{array}{cc}\mathrm{\Δ}{\tilde{R}}_j& {\mathrm{\Θ}}_{3\×1}\\ 0& 1\end{array}\right]=\left[\begin{array}{cc}{R}_0\mathrm{\Δ}{\tilde{R}}_j& p_0\\ 0& 1\end{array}\right]$

以上述两式分别代替第(2)种情况笛卡尔坐标系空间下移动和转动点动的实现步骤Step2和Step3中的计算式，即可采用类似实现步骤，对应实现工具坐标系空间下的移动和转动点动。

以上所述仅是本发明的优选实施方式，应当指出：对于本技术领域的普通技术人员来说，在不脱离本发明原理的前提下，还可以做出若干改进和润饰，这些改进和润饰也应视为本发明的保护范围。

Claims (6)

1.一种机器人点动操作的速度控制方法，其特征在于：将点动操作的速度控制过程划分为加速&匀速段和停止段，其中加速&匀速段包括周期变加速段、加速-匀速过渡段和匀速段三个子阶段，加速-匀速过渡段和匀速段为两个可或缺的子阶段，每个子阶段以固定长度的时间片为单位进行轨迹插补规划，且各个时间片内插补增量变化的加速度连续，且每个时间片的起始增量加速度和终止增量加速度均为零；每个时间片由连续的若干个控制周期组成，每个时间片完成后，根据判断条件确定下一时刻采用加速&匀速段插补序列还是停止段插补序列，其中判断条件包括：①当前控制过程处于哪个子阶段，②点动操作是否结束，③机器人运动是否在可达范围内，④机器人是否产生碰撞；

以i＝1,2,…,X表示加速&匀速段的所有时间片，时间片的时间长度为T₂₀：第i个时间片的起始增量为θ_i0，对应的起始增量速度和加速度分别为和第i个时间片的中点增量为θ_ih，对应的中点增量速度和加速度分别为和 同时为第i个时间片的加速度峰值；第i个时间片的终止增量为θ_if，对应的终止增量速度和加速度分别为和匀速段的给定速度为 同时为速度峰值；插补时间为t，θ_(i+1)0＝θ_if，停止段的起始增量为θ₀₀，对应的起始增量速度和加速度分别为和停止段的终止增量为θ_0f，对应的终止增量速度和加速度分别为和

2.根据权利要求1所述的机器人点动操作的速度控制方法，其特征在于：在执行第i个时间片的插补序列时，同时计算第(i+1)个时间片的插补序列，目的是为了能够在第i个时间片插补完成时能立即执行第(i+1)个时间片的插补序列，保证运动的连贯性；所述第(i+1)个时间片的插补序列分为两类：①加速&匀速段的插补序列Normal和停止段的插补序列Ending；

所述插补序列Normal分为三种情况：①若第(i+1)个时间片的加速度峰值没有达到最大加速度a_max，第(i+1)个时间片的速度也没有达到速度峰值则第(i+1)个时间片为周期变加速段，采用周期变加速段插补方法计算第(i+1)个时间片的插补序列；②若第(i+1)个时间片的加速度峰值达到最大加速度a_max，但是第(i+1)个时间片的速度没有达到速度峰值则第(i+1)个时间片为加速-匀速过渡段，采用加速-匀速过渡段插补方法计算第(i+1)个时间片的插补序列；③若第(i+1)个时间片的速度达到速度峰值则第(i+1)个时间片为匀速段，采用匀速段插补方法计算第(i+1)个时间片的插补序列；

由于不知道点动操作何时停止，因此在执行每一个时间片的插补序列的同时都需要计算插补序列Ending，插补序列Ending的起点为第i个插补序列Normal的延续。

3.根据权利要求1所述的机器人点动操作的速度控制方法，其特征在于：所述周期变加速段包括若干个时间片；在周期变加速段内，相邻两个时间片的加速度峰值依次递增固定值△a，即要求第(i+1)个时间片规划后所得的加速度峰值不超过最大加速度a_max；设第i个时间片在周期变加速段，若加速度峰值继续增大△a，第(i+1)个时间片规划后所得的加速度峰值将会超过最大加速度a_max，而第(i+1)个时间片规划后所得的速度没有达到速度峰值则第(i+1)个时间片跳转到加速-匀速过渡段；使用以下四阶多项式对周期变加速段内的第i个时间片的插补量θ_i(t)进行插值：

${\mathrm{\θ}}_i\left(t\right)={\mathrm{\θ}}_{i0}+{\stackrel{\·}{\mathrm{\θ}}}_{i0}t+\frac{2{\stackrel{\·\·}{\mathrm{\θ}}}_{ih}}{3T_{20}}{t}^3-\frac{{\stackrel{\·\·}{\mathrm{\θ}}}_{ih}}{3T_{20}^2}{t}^4.$

4.根据权利要求1所述的机器人点动操作的速度控制方法，其特征在于：所述若存在加速-匀速过渡段，则加速-匀速过渡段只占用一个时间片；使用以下四阶多项式对加速-匀速过渡段内的第i个时间片的插补量θ_i(t)进行插值：

${\mathrm{\θ}}_i\left(t\right)={\mathrm{\θ}}_{i0}+{\stackrel{\·}{\mathrm{\θ}}}_{i0}t+\frac{{\stackrel{\·}{\mathrm{\θ}}}_{if}-{\stackrel{\·}{\mathrm{\θ}}}_{i0}}{T_{20}^2}{t}^3-\frac{{\stackrel{\·}{\mathrm{\θ}}}_{if}-{\stackrel{\·}{\mathrm{\θ}}}_{i0}}{2T_{20}^3}{t}^4.$

5.根据权利要求1所述的机器人点动操作的速度控制方法，其特征在于：所述匀速段包括若干个时间片；使用以下线性多项式对匀速段内的第i个时间片的插补量θ_i(t)进行插值：

${\mathrm{\θ}}_i\left(t\right)={\mathrm{\θ}}_{i0}+{\stackrel{\·}{\mathrm{\θ}}}_{i0}t.$

6.根据权利要求1所述的机器人点动操作的速度控制方法，其特征在于：一旦发现点动操作结束、机器人运动不可达或机器人产生碰撞，则由加速&匀速段直接跳转到停止段，在停止段，点动操作以运行时间最短而且平滑的方式将速度降为0，在停止段不再使用时间片进行分割，二使用运行允许的最大减速度-a_Decel,max作为减速度峰值进行规划，使用以下四阶多项式对停止段的插补量θ₀(t)进行插值：

${\mathrm{\θ}}_0\left(t\right)={\mathrm{\θ}}_{00}+{\stackrel{\·}{\mathrm{\θ}}}_{00}t-\frac{4a_{Decel,max}^2}{9{\stackrel{\·}{\mathrm{\θ}}}_{00}}{t}^3+\frac{4a_{Decel,max}^3}{27{\stackrel{\·}{\mathrm{\θ}}}_{00}^2}{t}^4;$

停止段的运行时间为：