CN105922265B - 一种机械臂的运动轨迹规划方法、装置及机器人 - Google Patents

Abstract

本发明公开了一种机械臂的运动轨迹规划方法，包括：根据预置的起点坐标、终点坐标以及速度曲线算法控制机械臂以预定的运动轨迹进行运动；当接收到用户发出的停止指令后，获取机械臂的当前运动时间，并根据所述当前运动时间及为每个运动阶段配置的持续时间确定所述机械臂当前所处的运动阶段；根据运动臂当前所处的运动阶段更改预定的运动阶段的持续时间及初始参数集合中的预定的初始参数，生成更改后的运动轨迹，并控制机械臂以更改后的运动轨迹进行运动。本发明还公开了一种机械臂的运动轨迹规划装置及机器人，在接收到用户的停止指令后，通过对运动轨迹进行重新规划，使得所述运动臂在保持平稳运动及速度连续的前提下，尽快停止。

Description

一种机械臂的运动轨迹规划方法、装置及机器人

技术领域

本发明涉及机器人控制领域，具体是一种机械臂的运动轨迹规划方法、装置及机器人。

背景技术

在机械臂的工作过程中，需要对机械臂的轨迹进行规划，一般来说，这种轨迹规划都是通过预定的速度规划算法来实现的，例如，可使用S型(Double S)速度曲线算法来完成机械臂的轨迹规划。

有些速度规划算法在进行轨迹规划时，需要事先接收用户提供的起点和终点的位置，然后程序会在两点之间生成一系列的插补点来描述机械臂的运行轨迹。比如在示教功能中，用户通过手持设备发送指令，使得机械臂从起点出发，一直沿某个方向直线运动，直至到达终点。

但是很多时间，终点的位置终点位置是由用户根据实际的情况实时决定的，因而用户无法在一开始运动的时候就给出一个具体的终点坐标，这导致了基于目前的速度规划算法的机械臂在工作过程中无法对运动轨迹进行提前规划，影响了用户的使用体验。

发明内容

针对上述问题，本发明的目的在于提供一种机械臂的运动轨迹规划方法、装置及机器人，能根据用户的操作进行运动轨迹的重新规划，满足了用户的使用需求。

本发明提供了一种机械臂的运动轨迹规划方法，包括如下步骤：

根据预置的起点坐标、终点坐标以及速度曲线算法，控制机械臂以预定的运动轨迹进行运动；其中，所述速度曲线算法根据预先配置的初始参数集合将所述机械臂的运动轨迹规划为至少两个运动阶段，并为每个运动阶段配置一段持续时间；

当接收到用户发出的停止指令后，获取所述机械臂的当前运动时间，并根据所述当前运动时间及为每个运动阶段配置的持续时间确定所述机械臂当前所处的运动阶段；

根据所述运动臂当前所处的运动阶段更改预定的运动阶段的持续时间及所述初始参数集合中的预定的初始参数，生成更改后的运动轨迹，并控制所述机械臂以更改后的运动轨迹进行运动。

优选地，所述速度曲线算法为S型速度曲线算法，则所述初始参数集合至少包括路程、加速运动阶段的最大加速度、最大速度、减速运动阶段的最小减速度、最大加加速度以及最小加加速度这些初始参数；所述运动阶段包括加速运动阶段、匀速运动阶段及减速运动阶段，所述加速运动阶段包括第一加速运动阶段、第二加速运动阶段及第三加速运动阶段；所述减速运动阶段包括第一减速运动阶段、第二减速运动阶段及第三减速运动阶段。

优选地，所述根据所述运动臂当前所处的运动阶段更改预定的运动阶段的持续时间及所述初始参数集合中的预定的初始参数，生成更改后的运动轨迹，并控制所述机械臂以更改后的运动轨迹进行运动，具体为：

当判断所述机械臂当前处于第一加速运动阶段时，将所述第一加速运动阶段、第三加速运动阶段、所述第一减速运动阶段及所述第三减速运动阶段的持续时间更改为当前运动时间，并将所述第二加速运动阶段、匀速运动阶段及第二减速运动阶段的持续时间更改为零；

根据所述当前运动时间对路程、最大速度、加速运动阶段的最大加速度及减速运动阶段的最小减速度这些初始参数进行更改，根据更改后的初始参数生成更改后的运动轨迹，并控制所述机械臂以更改后的运动轨迹进行运动。

优选地，所述根据所述运动臂当前所处的运动阶段更改预定的运动阶段的持续时间及所述初始参数集合中的预定的初始参数，生成更改后的运动轨迹，并控制所述机械臂以更改后的运动轨迹进行运动，具体为：

当判断所述机械臂当前处于第二加速运动阶段时，将所述第二加速运动阶段及第二减速运动阶段的持续时间更改为t-t₁，并将所述匀速运动阶段的持续时间更改为零；其中，t为当前运动时间，t₁-t₀为预先配置的从开始计时的时刻t₀到所述第一加速运动阶段结束的持续时间；

根据所述当前运动时间t对所述路程及所述最大速度这两个初始参数进行更改，根据更改后的初始参数生成更改后的运动轨迹，并控制所述机械臂以更改后的运动轨迹进行运动。

优选地，所述根据所述运动臂当前所处的运动阶段更改预定的运动阶段的持续时间及所述初始参数集合中的预定的初始参数，生成更改后的运动轨迹，并控制所述机械臂以更改后的运动轨迹进行运动，具体为：

当判断所述机械臂当前处于第三加速运动阶段时，将所述匀速运动阶段的持续时间更改为零；

根据所述当前运动时间对路程这个初始参数进行更改，根据更改后的初始参数生成更改后的运动轨迹，并控制所述机械臂以更改后的运动轨迹进行运动。

优选地，所述根据所述运动臂当前所处的运动阶段更改预定的运动阶段的持续时间及所述初始参数集合中的预定的初始参数，生成更改后的运动轨迹，并控制所述机械臂以更改后的运动轨迹进行运动，具体为：

当判断所述机械臂当前处于匀速运动阶段时，将所述匀速运动阶段的持续时间更改为t-t₃；其中，t为当前运动时间，t₃-t₀为预先配置的从开始计时的时刻t₀到所述第三加速运动阶段结束的持续时间；

根据所述当前运动时间t对所述路程这个初始参数进行更改，根据更改后的初始参数生成更改后的运动轨迹，并控制所述机械臂以更改后的运动轨迹进行运动。

优选地，在所述根据预置的起点坐标、终点坐标以及速度曲线算法，控制机械臂以预定的运动轨迹进行运动之前，还包括：

基于机械臂的关节参数，生成所述机械臂的工作区域；

根据预先输入的起点的坐标及移动方向生成一条射线，并计算所述射线与所述工作区域的边界的交点，生成交点集合；

计算所述交点集合内的每个交点与所述起点的距离，获取与最小距离对应的交点的坐标，并将所述交点标记为终点。

本发明还提供了一种机械臂的运动轨迹规划装置，包括：

运动控制单元，用于根据预置的起点坐标、终点坐标以及速度曲线算法，控制机械臂以预定的运动轨迹进行运动；其中，所述速度曲线算法根据预先配置的初始参数集合将所述机械臂的运动轨迹规划为至少两个运动阶段，并为每个运动阶段配置一段持续时间；

运动时间获取单元，用于当接收到用户发出的停止指令后，获取所述机械臂的当前运动时间，并根据所述当前运动时间t及为每个运动阶段配置的持续时间确定所述机械臂当前所处的运动阶段；

运动轨迹更改单元，用于根据所述运动臂当前所处的运动阶段更改预定的运动阶段的持续时间及所述初始参数集合中的预定的初始参数，生成更改后的运动轨迹，并控制所述机械臂以更改后的运动轨迹进行运动。

优选地，还包括：

工作区域生成单元，用于基于待工作的机械臂的关节参数，生成所述机械臂的工作区域；

交点集合生成单元，用于根据预先输入的起点的坐标及移动方向生成一条射线，并计算所述射线与所述工作区域的边界的交点，生成交点集合；

终点标记单元，用于计算所述交点集合内的每个交点与所述起点的距离，获取与最小距离对应的交点的坐标，并将所述交点标记为终点。

本发明还提供了一种机器人，包括上述的机械臂的运动轨迹规划装置。

本发明实施例提供的机械臂的运动轨迹规划方法、装置及机器人，当检测到进入减速运动阶段之前接收到用户的停止指令后，则根据当前所处的运动阶段对当前及后续的运动轨迹进行重新规划，使得所述运动臂在保持平稳运动及速度连续的前提下，尽快停止，从而满足用户的使用需求。

附图说明

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

图1是本发明实施例提供的机械臂的运动轨迹规划方法的流程示意图。

图2是S形速度曲线算法的运动过程示意图。

图3是S形速度曲线算法的计算原理流程图。

图4是SCARA型机械臂的结构示意图。

图5是图4所示的SCARA型机械臂在DH坐标系的示意图。

图6是图4所示的SCARA型机械臂的工作区域示意图。

图7是图6所示的工作区域的俯视图。

图8是射线与工作区域的连接示意图。

图9是本发明实施例提供的通过射线与圆相交测试的优化算法来计算射线与圆的交点的流程示意图。

图10(a)至10(c)是图9所示的优化算法的原理图。

图11是本发明实施例提供的机械臂的运动轨迹规划装置的结构示意图。

图12是本发明实施例提供的机械臂的运动轨迹规划装置的另一种结构示意图。

具体实施方式

下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。

本发明实施例提供一种机械臂的运动轨迹规划方法，可根据用户发出的停止指令实时确定出终点坐标，并提前规划新的运动轨迹。

请参阅图1，本发明实施例提供一种机械臂的运动轨迹规划方法流程示意图，其可由机械臂的运动轨迹规划装置(以下简称为运动轨迹规划装置)来执行，并至少包括如下步骤：

S101，根据预置的起点坐标、终点坐标以及速度曲线算法，控制机械臂以预定的运动轨迹进行运动；其中，所述速度曲线算法根据预先配置的初始参数集合将所述机械臂的运动轨迹规划为至少两个运动阶段，并为每个运动阶段配置一段持续时间。

在本发明实施例中，机械臂是机器人技术领域中最广泛应用的自动化机械装置，其广泛应用于工业制造、医学治疗、娱乐服务、军事、半导体制造以及太空探索等领域。尽管它们的形态各有不同，但它们都有一个共同的特点，就是能够接受指令，并根据指令精确地定位到三维(或二维)空间上的某一点进行作业。

在本发明实施例中，机械臂的运动一般都是根据速度曲线算法来控制的，例如，常见的有梯形速度曲线算法、正弦形速度曲线算法以及S形速度曲线算法，其中，S形速度曲线算法由于能够对加速度的变化率进行控制，并且在速度连接处光滑过渡，所以在运行时十分平稳，不会出现震动的现象，因此得到广泛的应用。

其中，S型速度曲线算法在运行时，需用户输入初始参数集合(包括路程q₁、起点速度v₀、起点加速度a₀终点速度v₁、终点加速度a₁，加速运动阶段的最大加速度a_max、最大速度v_max、减速运动阶段的最小减速度a_min、最大加加速度j_max以及最小加加速度j_min)，然后再根据这些初始参数进行运动轨迹的轨迹。如图2所示，一般来说，典型的S型速度曲线算法会将运动轨迹规划为三个运动阶段：加速运动阶段(持续时间T_a)、匀速运动阶段(持续时间为T_v)及减速运动阶段(持续时间T_d)，更具体地：所述加速运动阶段包括第一加速运动阶段(对应于时间区间[t₀，t₁)，持续时间为T_j1，此时加速度从0逐渐增加到a_max)、第二加速运动阶段(对应于时间区间[t₁，t₂)，此时加速度保持不变)及第三加速运动阶段(对应于时间区间[t₂，t₃)，持续时间为T_j1，加速度从a_max逐渐减小为0)；所述减速运动阶段包括第一减速运动阶段(对应于时间区间[t₄，t₅)，持续时间为T_j2，此时加速度从0逐渐减小到a_min)、第二减速运动阶段(对应于时间区间[t₅，t₆)，此时加速度保持不变)及第三减速运动阶段(对应于时间区间[t₆，t₇)，持续时间为T_j2，此时加速度从a_min逐渐增大为0)，其中，在加速度加速阶段和加速度减速阶段，相应的加加速度分别为j_max和j_min，且一般地，j_max＝-j_min，T_j1＝T_j2，a_max＝a_min。

下面将详细介绍S型速度曲线算法进行轨迹规划的过程，其中，为了便于计算，这里取v₀＝v₁＝0，a₀＝a₁＝0。如图3所示，首先，S型速度曲线算法需先接受用户给出的初始参数，所述初始参数包括：路程q₁、起点速度v₀、起点加速度a₀终点速度v₁、终点加速度a₁，加速运动阶段的最大加速度a_max、最大速度v_max、减速运动阶段的最小减速度a_min、最大加加速度j_max以及最小加加速度j_min。接着，S型速度曲线算法根据这些初始参数，判断(v_max-v₀)j_max<是否成立，若成立，则使用公式(1)来计算具体的时间参数T_j1、T_j2、T_a及T_d。若不成立，则使用公式(2)来计算具体的时间参数T_j1、T_j2、T_a及T_d。此后，在获得T_j1、T_j2、T_a及T_d后，根据公式(3)计算得到的T_a来计算得到T_v。接着，判断T_v>0是否成立，若成立，则使用公式(6)、公式(7)来计算得到最终的t₀、t₁、t₂、t₃、t₄、t₅、t₆、t₇。若不成立，则进一步判断是否满足若满足，则根据公式(4)得到T_v以及新的T_a及T_d，并根据公式(6)、公式(7)来计算得到最终的t₀、t₁、t₂、t₃、t₄、t₅、t₆、t₇。若不满足，则使用公式(5)计算得到T_v以及新的T_j1、T_j2、T_a及T_d，并根据公式(6)、公式(7)来计算得到最终的t₀、t₁、t₂、t₃、t₄、t₅、t₆、t₇。其中公式(1)～(7)的具体形式如下：

在本发明实施例中，在获得如公式(6)、(7)所述的公式或等式后，即可以获得所述机械臂在任意时刻的位置，具体可参考如下的公式(8)：

S102，当接收到用户发出的停止指令后，获取所述机械臂的当前运动时间，并根据所述当前运动时间及为每个运动阶段配置的持续时间确定所述机械臂当前所处的运动阶段。

在本发明实施例中，用户在使用所述机械臂的时候，可能随时根据需要发出停止指令(例如在示教过程中，用户可通过示教盒向机械臂发出停止指令)，则当所述运动轨迹规划装置接收到用户发出的停止指令后，获取所述机械臂的当前运动时间t，并根据所述当前运动时间t及所述持续时间(或时间区间)确定所述机械臂当前所处的运动阶段。

例如，假设所述运动轨迹规划装置检测到当前运行时间为5S，而t₃＝4s，t₄＝6S，则可确定当前所处的运动阶段为匀速运动阶段。又例如，假设所述运动轨迹规划装置检测到当前运行时间为2S，而t₁＝1.5s，t₄＝3S，则可确定当前所处的运动阶段为第二加速运动阶段。

S103，根据所述运动臂当前所处的运动阶段更改预定的运动阶段的持续时间及所述初始参数集合中的预定的初始参数，生成更改后的运动轨迹，并控制所述机械臂以更改后的运动轨迹进行运动。

在本发明实施例中，在用户发出停止指令后，所述运动轨迹规划装置需要尽快且保持稳定的情况下尽快停下所述机械臂，为此，所述运动轨迹规划装置需要根据所述运动臂当前所处的运动阶段更改预定的运动阶段的持续时间及所述初始参数集合中的预定的初始参数。

具体地：

S1031，当判断所述机械臂当前处于第一加速运动阶段时，将所述第一加速运动阶段、第三加速运动阶段、所述第一减速运动阶段及所述第三减速运动阶段的持续时间更改为t，并将所述第二加速运动阶段、匀速运动阶段及第二减速运动阶段的持续时间更改为零。

在本发明实施例中，当所述运动轨迹规划装置判断用户在第一加速运动阶段就发出停止指令，即当前运行时间t<t₁时，所述运动轨迹规划装置立即终止第一加速运动阶段，同时为了尽快停止所述机械臂，还将所述第二加速运动阶段、匀速运动阶段及第二减速运动阶段的持续时间更改为零。

则此时，各个运动阶段的时间区间变为：所述第一加速运动阶段对应的时间区间为[t₀，t_1n1)，所述第二加速运动阶段对应的时间区间为[t_1n1，t_2n1)，所述第三加速运动阶段对应的时间区间为[t_2n1，t_3n1)，所述匀速运动阶段对应的时间区间为[t_3n1，t_4n1)，所述第一减速运动阶段对应的时间区间为[t_4n1，t_5n1)，所述第二减速运动阶段对应的时间区间为[t_5n1，t_6n1)，所述第三减速运动阶段对应的时间区间更改为[t_6n1，t_7n1)，其中，t₀＝0,t_1n1＝t_2n1＝t，t_3n1＝t_4n1＝2t，t_5n1＝t_6n1＝3t，t_7n1＝4t。

S1032，根据所述当前运动时间对路程、最大速度、加速运动阶段的最大加速度及减速运动阶段的最小减速度这些初始参数进行更改，根据更改后的初始参数生成更改后的运动轨迹，并控制所述机械臂以更改后的运动轨迹进行运动。

其中，将加速运动阶段的最大加速度a_max更改为j_max*t，将减速运动阶段的最小减速度a_min更改为-j_min*t，将最大速度v_max更改为t*a_max，将路程q₁更改为t*a_max*2t，此后，根据公式(8)即可获得更改后的运动轨迹，并控制所述机械臂以更改后的运动轨迹进行运动。

具体地：

S1033，当判断所述机械臂当前处于第二加速运动阶段时，将所述第二加速运动阶段及第二减速运动阶段的持续时间更改为t-t₁，并将所述匀速运动阶段的持续时间更改为零。

在本发明实施例中，当所述运动轨迹规划装置判断用户在第二加速运动阶段就发出停止指令，此时当前运行时间t满足：t₁<t<t₂时，所述运动轨迹规划装置立即终止第二加速运动阶段，即将所述第二加速运动阶段，同时为了尽快停止所述机械臂，还将所述第二减速运动阶段的持续时间更改为t-t₁，将所述匀速运动阶段更改为零。

则此时，各个运动阶段的时间区间变为：所述第一加速运动阶段对应的时间区间为[t₀，t₁)，所述第二加速运动阶段对应的时间区间为[t₁，t)，所述第三加速运动阶段对应的时间区间为[t，t_3n2)，所述匀速运动阶段对应的时间区间为[t_3n2，t_4n2)，所述第一减速运动阶段对应的时间区间为[t_4n2，t_5n2)，所述第二减速运动阶段对应的时间区间为[t_5n2，t_6n2)，所述第三减速运动阶段对应的时间区间更改为[t_6n2，t_7n2)，其中，t₀＝0,t_3n2＝t_4n2＝t+T_j1，t_5n2＝t_4n2+T_j2，t_6n2＝t_5n2+t-t₁，t_7n2＝t_6n2+T_j2。

S1034，根据所述当前运动时间t对所述路程q₁及所述最大速度v_max这两个初始参数进行更改，根据更改后的初始参数生成更改后的运动轨迹，并控制所述机械臂以更改后的运动轨迹进行运动。

其中，更改后的v_max＝(T-T_j1)j_max T_j1，更改后的q₁＝(T-T_j1)a_max T，T＝T_a-(t₂-t)。

此后，根据公式(8)即可获得更改后的运动轨迹，并控制所述机械臂以更改后的运动轨迹进行运动。

具体地：

S1035，当判断所述机械臂当前处于第三加速运动阶段时，将所述匀速运动阶段的持续时间更改为零。

在本发明实施例中，当所述运动轨迹规划装置判断用户在第三加速运动阶段就发出停止指令，此时当前运行时间t满足：t₂<t<t₃时，为了尽快停止所述机械臂，将所述匀速运动阶段更改为零。

则此时，各个运动阶段的时间区间变为：所述第一加速运动阶段对应的时间区间为[t₀，t₁)，所述第二加速运动阶段对应的时间区间为[t₁，t₂)，所述第三加速运动阶段对应的时间区间为[t₂，t₃)，所述匀速运动阶段对应的时间区间为[t₃，t_4n3)，所述第一减速运动阶段对应的时间区间为[t_4n3，t_5n3)，所述第二减速运动阶段对应的时间区间为[t_5n3，t_6n3)，所述第三减速运动阶段对应的时间区间更改为[t_6n3，t_7n3)，其中，t₀＝0,t_4n3＝t₃，t_5n3＝t_4n3+T_j2，t_6n3＝t_5n3+T_d-2T_j2，t_7n2＝t_6n3+T_j2。

S1036，根据所述当前运动时间t对初始参数q₁进行更改，根据更改后的初始参数生成更改后的运动轨迹，并控制所述机械臂以更改后的运动轨迹进行运动。

其中，q₁＝q₁-v_max T_v。此后，根据公式(8)即可获得更改后的运动轨迹，并控制所述机械臂以更改后的运动轨迹进行运动。

具体地：

S1037，当判断所述机械臂当前处于匀速运动阶段时，将所述匀速运动阶段的持续时间更改为t-t₃。

此时，各个运动阶段的时间区间变为：所述第一加速运动阶段对应的时间区间为[t₀，t₁)，所述第二加速运动阶段对应的时间区间为[t₁，t₂)，所述第三加速运动阶段对应的时间区间为[t₂，t₃)，所述匀速运动阶段对应的时间区间为[t₃，t)，所述第一减速运动阶段对应的时间区间为[t，t_5n4)，所述第二减速运动阶段对应的时间区间为[t_5n4，t_6n4)，所述第三减速运动阶段对应的时间区间更改为[t_6n4，t_7n4)，其中，t_5n4＝t+T_j2，t_6n4＝t_5n4+T_d-2T_j2，t_7n4＝t_6n4+T_j2。

S1038，根据所述当前运动时间t对路程q₁这个初始参数进行更改，根据更改后的初始参数生成更改后的运动轨迹，并控制所述机械臂以更改后的运动轨迹进行运动。

其中，更改后的路程q₁＝q₁-v_max[T_v-(t-t₃)]。此后，根据公式(8)即可获得更改后的运动轨迹，并控制所述机械臂以更改后的运动轨迹进行运动。

在本发明实施例中，若是在减速运动阶段接收到用户的停止指令，则不对运动轨迹进行更改。

综上所述，本发明实施例提供的机械臂的运动轨迹规划方法，当检测到进入减速运动阶段之前接收到用户的停止指令后，则根据当前所处的运动阶段对当前及后续的运动轨迹进行重新规划，使得所述运动臂在保持平稳运动及速度连续的前提下，尽快停止，从而满足用户的使用需求。

需要说明的是，上述实施例中，所述速度曲线算法为S型速度曲线算法，但本发明的思想同样可应用于其他的速度曲线算法，特别的，适用于具有多个运动阶段的速度曲线算法，这些方案均在本发明的保护范围之内，在此不做赘述。

需要说明的是，在上述实施例中，在运动前，需要获得终点坐标，再根据起点坐标、运动方向和终点坐标获得路程q₁，下面将介绍本发明的一个优选实施例提供的终点坐标的获取方法。

具体地，可基于如下步骤来获得终点坐标：

S01，基于待工作的机械臂的关节参数，生成所述机械臂的工作区域。

一般来说，一个机械臂包括若干个关节，每个关节都具有关节参数，所述关节参数可包括关节类型、臂长(连接两个关节的连接臂的长度)、活动范围等。其中，所述关节类型可包括转动关节及滑动关节，转动关节可控制连接臂的转动，而滑动关节可实现连接臂的垂直滑动。通过不同关节的组合，即可控制所述机械臂的工作区域。具体地，可通过如下步骤来确定所述机械臂的工作区域：

S011，根据机械臂的关节类型及关节之间的相对位置关系，基于DH坐标系建立生成每个关节的坐标系。

如图4所示，以SCARA(Selective Compliant Articulated Robot for Assembly)型机械臂为例，其具有4个关节J1，J2，J3，J4，其中J1，J2，J4为转动关节，且J1与J2，J2与J4通过相应的连接臂进行连接，而J3为滑动关节。当然，应当理解的是，本发明还可应用于其他类型的机械臂，在此不做赘述。

在确定上述关节后，需要针对每个关节建立坐标系，如图5所示，可基于DH(Denavit-Hartenberg)坐标系来建立每个关节的坐标系，其中转动关节Ji的转动轴与各自的zi轴(转动关节Ji的连接臂绕zi轴转动)互相平行，连接臂的臂长为αi，{i│i∈{1,2,4}}；滑动关节J3的轴(z3轴)跟J4的轴(z4轴)平行。在确定出zi轴后，可用第一个关节指向第二个关节的方向作为xi方向，而yi的方向可基于右手定则来确定，如此，即生成了每个关节的坐标系。

S012，根据每个关节的坐标系、每个关节的活动范围及关节之间的臂长，生成所述机械臂的工作区域。

在本发明实施例中，在生成每个关节的坐标系后，根据每个关节的活动范围(对于转动关节，其活动范围为其连接臂转动的转动角范围，对于滑动关节，其活动范围为其连接臂的上下运动范围)，即可生成所述机械臂的工作区域(如图6所示)，此时，只需在平面进行投影即可生成如图7所示的俯视图。

S02，根据预先输入的起点的坐标及移动方向生成一条射线，并计算所述射线与所述工作区域的边界的交点，生成交点集合。

具体的，可包括：

S021，将所述工作区域分解为至少两段弧线，获取每段弧线的圆心、半径及圆心角范围，并生成与每段弧线对应的圆的方程。

从图7中可以看出，所述机械臂的工作区域是由 四段弧线所围成。而这四段弧线分别位于⊙O₁，⊙O₂，⊙O₃，⊙O₄四个圆上。圆心O₁，O₄与J₁轴重合；O₂和O₃分别为J₁轴转动到正负极限时J₂轴的位置。和分别是轴J_i的正/负方向的最大活动范围，

这四段弧线的详细参数可参照表1所示：

表1

其中：

S022，根据预先输入的起点的坐标及移动方向生成一条射线，计算所述射线与每个圆的交点。

在本发明实施例中，假设起点为I，方向向量为n，||n||为方向向量n的单位向量。则此时射线可表示为R(u)＝I+u·||n||。

如果起点I位于工作区域内，则射线R(u)必与某圆⊙O_i有交点

其中，在计算交点时，可通过将射线R(u)的方程依次与每个圆的方程进行联立，在利用参数方程法求解获得。

S023，计算所述交点在所述交点所在的圆上的弧度角，并在所述弧度角位于所述圆心角范围内时，确定所述交点位于所述弧线上，将所述交点存入预设的集合，生成交点集合。

在本发明实施例中，上述求得的交点中，有些虽然与圆相交，但是并没有位于弧线上，因而需要去除。具体地，如图8所示，射线IP分别与⊙O₁相交于点N；与⊙O₂相交于点K和点M；与⊙O₄相交于点J和点L；与⊙O₃没有交点。其中，点K，N虽然在圆上，但是并不在围成所述工作区域的弧线上，所以在得到交点后，还要查看所述交点的弧度角θⁱ是否满足

具体地，假设点s(x,y)是⊙O_i上的一个点，则点s相对于⊙O_i的弧度角θⁱ为：

通过上述公式就可以确定哪些交点是在弧线上，哪些不在。此时，将那些位于弧线上的交点存入预设的集合ξ＝{κⁱ}中，生成交点集合。

需要说明的是，在本发明的其他实施例中，所述工作区域并不一定是由圆的弧线组成。例如，所述工作区域可能由椭圆的弧线组成或者由不同类型的几何形状的弧线混合组成，此时，同理，可获得这些弧线对应的几何形状的方程及角度范围，再通过上述的方法即可计算生成交点集合，这些技术方案均在本发明的保护范围之内，在此不做赘述。

S03，计算所述交点集合内的每个交点与所述起点的距离，获取与最小距离对应的交点的坐标，并将所述交点标记为终点。

在本发明实施例中，可通过欧拉公式计算得到每个交点与所述起点I的距离，然后对计算得到的每个距离进行比较，获取与最小距离对应的交点的坐标，并将所述交点标记为终点，此时，即得到所需的工作区域的边界。

综上所述，在本优选实施例中，通过生成所述机械臂的工作区域，再计算由预先输入的起点的坐标及移动方向生成的射线与工作区域的边界的交点，并获取与起点具有最小距离的交点的坐标，获得与所述起点及移动方向相应的终点。本发明基于几何的方法来确定机械臂工作区域的边界因此机器人在运动前就可以提前知道终点的位置，方便机器人进行运动轨迹的规划。

优选地，针对步骤S022，在生成所述射线后，可以使用传统的参数方程法求交点，也可以使用等提出的基于射线与圆相交测试的优化算法来计算所述射线与每个圆的交点。

具体地，如图9所示，设射线的参数方程为R(u)＝I+u·||n||，其中||n||是单位长度。如图10(a)所示，首先计算出从起点I出发到圆心O_i的向量向量的长度以及向量沿着||n||方向的投影若且l<0，则说明起点位于⊙O_i外，且射线的方向沿远离⊙O_i方向延伸，因此所述射线与⊙O_i不相交(如图10(b)所示)，此时完成第一次排除测试。否则，利用勾股定理计算出圆心O_i与投影之间距离的平方：m²＝a²-l²，若则可以判定射线与圆O_i一定不相交，完成第二次排除测试。如果射线和⊙O_i经过两次排除测试，则就可以判定它们一定相交。接下来，计算射线与⊙O_i的交点：首先，计算出距离然后判断起点I是否位于⊙O_i内，若则说明起点I位于⊙O_i外，此时，所述射线与圆O_i有两个交点，分别为||n||以及(如图10(a)所示)。若则说明起点I位于圆O_i内，此时，所述射线与圆O_i有一个交点，为I+(l+q)·||n||(如图10(c)所示)。

本优选方案中，在计算射线与圆的交点之前，先通过两次测试判断射线与圆是否有交点，在计算时，仅计算与射线有交点的圆，而不需要对那些与射线没有交点的圆进行计算，如此，减小了计算量，提升了计算效率。

请一并参阅图11，图11是本发明实施例提供的一种机械臂的运动轨迹规划装置100，其包括：

运动控制单元10，用于根据预置的起点坐标、终点坐标以及速度曲线算法，控制机械臂以预定的运动轨迹进行运动；其中，所述速度曲线算法根据预先配置的初始参数集合将所述机械臂的运动轨迹规划为至少两个运动阶段，并为每个运动阶段配置一段持续时间。

运动时间获取单元20，用于当接收到用户发出的停止指令后，获取所述机械臂的当前运动时间t，并根据所述当前运动时间t及为每个运动阶段配置的持续时间确定所述机械臂当前所处的运动阶段。

运动轨迹更改单元30，用于根据所述运动臂当前所处的运动阶段更改预定的运动阶段的持续时间及所述初始参数集合中的预定的初始参数，生成更改后的运动轨迹，并控制所述机械臂以更改后的运动轨迹进行运动。

具体为：

当所述运动轨迹更改单元30判断所述机械臂当前处于第一加速运动阶段时，将所述第一加速运动阶段、第三加速运动阶段、所述第一减速运动阶段及所述第三减速运动阶段的持续时间更改为t，并将所述第二加速运动阶段、匀速运动阶段及第二减速运动阶段的持续时间更改为零；此后，根据所述当前运动时间t对初始参数a_max、a_min、v_max及q₁进行更改，根据更改后的初始参数生成更改后的运动轨迹，并控制所述机械臂以更改后的运动轨迹进行运动。

具体地：

当所述运动轨迹更改单元30判断所述机械臂当前处于第二加速运动阶段时，将所述第二加速运动阶段及第二减速运动阶段的持续时间更改为t-t₂，并将所述匀速运动阶段的持续时间更改为零，此后，根据所述当前运动时间t对初始参数v_max及q₁进行更改，根据更改后的初始参数生成更改后的运动轨迹，并控制所述机械臂以更改后的运动轨迹进行运动。

具体地：

当所述运动轨迹更改单元30判断所述机械臂当前处于第三加速运动阶段时，将所述匀速运动阶段的持续时间更改为零；

根据所述当前运动时间t对初始参数q₁进行更改，根据更改后的初始参数生成更改后的运动轨迹，并控制所述机械臂以更改后的运动轨迹进行运动。

具体地：

当所述运动轨迹更改单元30判断所述机械臂当前处于匀速运动阶段时，将所述匀速运动阶段的持续时间更改为t-t₃；

根据所述当前运动时间t对初始参数q₁进行更改，根据更改后的初始参数生成更改后的运动轨迹，并控制所述机械臂以更改后的运动轨迹进行运动。

本发明实施例提供的机械臂的运动轨迹规划装置100，当检测到进入减速运动阶段之前接收到用户的停止指令后，则根据当前所处的运动阶段对当前及后续的运动轨迹进行重新规划，使得所述运动臂在保持平稳运动及速度连续的前提下，尽快停止，从而满足用户的使用需求。

请一并参阅图12，需要说明的是，在本发明实施例中，为了获得所述机械臂的终点坐标，所述机械臂的运动轨迹规划装置还包括：

工作区域生成单元40，用于基于待工作的机械臂的关节参数，生成所述机械臂的工作区域。

交点集合生成单元50，用于根据预先输入的起点的坐标及移动方向生成一条射线，并计算所述射线与所述工作区域的边界的交点，生成交点集合。

终点标记单元60，用于计算所述交点集合内的每个交点与所述起点的距离，获取与最小距离对应的交点的坐标，并将所述交点标记为终点。

本发明实施例提供中，通过工作区域生成单元40生成所述机械臂的工作区域，交点集合生成单元50再计算由预先输入的起点的坐标及移动方向生成的射线与工作区域的边界的交点，所述终点标记单元60获取与起点具有最小距离的交点的坐标，获得与所述起点及移动方向相应的终点，再根据所述起点、终点及预设的轨迹规划算法进行运动轨迹的规划。本发明基于几何的方法来确定机械臂工作区域的边界因此机器人在运动前就可以提前知道终点的位置，方便机器人进行运动轨迹的规划。

本发明还提供一种机器人，包括上述的机械臂的运动轨迹规划装置100。

本发明实施例提供的机器人，当检测到进入减速运动阶段之前接收到用户的停止指令后，则根据当前所处的运动阶段对当前及后续的运动轨迹进行重新规划，使得所述运动臂在保持平稳运动及速度连续的前提下，尽快停止，从而满足用户的使用需求。

本发明实施例提供的机器人，若是在进入减速运动阶段之前接收到用户的停止指令后，则对整个运动轨迹进行重新规划，使得所述运动臂在保持平稳运动及速度连续的前提下，尽快停止，从而满足用户的使用需求。

Claims (10)

1.一种机械臂的运动轨迹规划方法，其特征在于，包括如下步骤：

根据预置的起点坐标、终点坐标以及速度曲线算法，控制机械臂以预定的运动轨迹进行运动；其中，所述速度曲线算法根据预先配置的初始参数集合将所述机械臂的运动轨迹规划为至少两个运动阶段，并为每个运动阶段配置一段持续时间；

当接收到用户发出的停止指令后，获取所述机械臂的当前运动时间，并根据所述当前运动时间及为每个运动阶段配置的持续时间确定所述机械臂当前所处的运动阶段；

根据所述机械臂当前所处的运动阶段更改预定的运动阶段的持续时间及所述初始参数集合中的预定的初始参数，生成更改后的运动轨迹，并控制所述机械臂以更改后的运动轨迹进行运动。

2.根据权利要求1所述的机械臂的运动轨迹规划方法，其特征在于，所述速度曲线算法为S型速度曲线算法，则所述初始参数集合至少包括路程、加速运动阶段的最大加速度、最大速度、减速运动阶段的最小减速度、最大加加速度以及最小加加速度这些初始参数；所述运动阶段包括加速运动阶段、匀速运动阶段及减速运动阶段，所述加速运动阶段包括第一加速运动阶段、第二加速运动阶段及第三加速运动阶段；所述减速运动阶段包括第一减速运动阶段、第二减速运动阶段及第三减速运动阶段。

3.根据权利要求2所述的机械臂的运动轨迹规划方法，其特征在于，所述根据所述机械臂当前所处的运动阶段更改预定的运动阶段的持续时间及所述初始参数集合中的预定的初始参数，生成更改后的运动轨迹，并控制所述机械臂以更改后的运动轨迹进行运动，具体为：

当判断所述机械臂当前处于第一加速运动阶段时，将所述第一加速运动阶段、第三加速运动阶段、所述第一减速运动阶段及所述第三减速运动阶段的持续时间更改为当前运动时间，并将所述第二加速运动阶段、匀速运动阶段及第二减速运动阶段的持续时间更改为零；

根据所述当前运动时间对所述路程、所述最大速度、所述加速运动阶段的最大加速度及所述减速运动阶段的最小减速度这些初始参数进行更改，根据更改后的初始参数生成更改后的运动轨迹，并控制所述机械臂以更改后的运动轨迹进行运动。

4.根据权利要求2所述的机械臂的运动轨迹规划方法，其特征在于，所述根据所述机械臂当前所处的运动阶段更改预定的运动阶段的持续时间及所述初始参数集合中的预定的初始参数，生成更改后的运动轨迹，并控制所述机械臂以更改后的运动轨迹进行运动，具体为：

当判断所述机械臂当前处于第二加速运动阶段时，将所述第二加速运动阶段及第二减速运动阶段的持续时间更改为t-t1，并将所述匀速运动阶段的持续时间更改为零；其中，t为当前运动时间，t1-t0为预先配置的从开始计时的时刻t0到所述第一加速运动阶段结束的持续时间；

根据所述当前运动时间t对所述路程及所述最大速度这两个初始参数进行更改，根据更改后的初始参数生成更改后的运动轨迹，并控制所述机械臂以更改后的运动轨迹进行运动。

5.根据权利要求2所述的机械臂的运动轨迹规划方法，其特征在于，所述根据所述机械臂当前所处的运动阶段更改预定的运动阶段的持续时间及所述初始参数集合中的预定的初始参数，生成更改后的运动轨迹，并控制所述机械臂以更改后的运动轨迹进行运动，具体为：

当判断所述机械臂当前处于第三加速运动阶段时，将所述匀速运动阶段的持续时间更改为零；

根据所述当前运动时间对所述路程这个初始参数进行更改，根据更改后的初始参数生成更改后的运动轨迹，并控制所述机械臂以更改后的运动轨迹进行运动。

6.根据权利要求2所述的机械臂的运动轨迹规划方法，其特征在于，所述根据所述机械臂当前所处的运动阶段更改预定的运动阶段的持续时间及所述初始参数集合中的预定的初始参数，生成更改后的运动轨迹，并控制所述机械臂以更改后的运动轨迹进行运动，具体为：

当判断所述机械臂当前处于匀速运动阶段时，将所述匀速运动阶段的持续时间更改为t-t₃；其中，t为当前运动时间，t₃-t₀为预先配置的从开始计时的时刻t₀到所述第三加速运动阶段结束的持续时间；

根据所述当前运动时间t对所述路程这个初始参数进行更改，根据更改后的初始参数生成更改后的运动轨迹，并控制所述机械臂以更改后的运动轨迹进行运动。

7.根据权利要求1至6任意一项所述的机械臂的运动轨迹规划方法，其特征在于，在所述根据预置的起点坐标、终点坐标以及速度曲线算法，控制机械臂以预定的运动轨迹进行运动之前，还包括：

基于机械臂的关节参数，生成所述机械臂的工作区域；

根据预先输入的起点的坐标及移动方向生成一条射线，并计算所述射线与所述工作区域的边界的交点，生成交点集合；

计算所述交点集合内的每个交点与所述起点的距离，获取与最小距离对应的交点的坐标，并将所述交点标记为终点。

8.一种机械臂的运动轨迹规划装置，其特征在于，包括：

运动控制单元，用于根据预置的起点坐标、终点坐标以及速度曲线算法，控制机械臂以预定的运动轨迹进行运动；其中，所述速度曲线算法根据预先配置的初始参数集合将所述机械臂的运动轨迹规划为至少两个运动阶段，并为每个运动阶段配置一段持续时间；

运动时间获取单元，用于当接收到用户发出的停止指令后，获取所述机械臂的当前运动时间，并根据所述当前运动时间及为每个运动阶段配置的持续时间确定所述机械臂当前所处的运动阶段；

运动轨迹更改单元，用于根据所述机械臂当前所处的运动阶段更改预定的运动阶段的持续时间及所述初始参数集合中的预定的初始参数，生成更改后的运动轨迹，并控制所述机械臂以更改后的运动轨迹进行运动。

9.根据权利要求8所述的机械臂的运动轨迹规划装置，其特征在于，还包括：

工作区域生成单元，用于基于待工作的机械臂的关节参数，生成所述机械臂的工作区域；

交点集合生成单元，用于根据预先输入的起点的坐标及移动方向生成一条射线，并计算所述射线与所述工作区域的边界的交点，生成交点集合；

终点标记单元，用于计算所述交点集合内的每个交点与所述起点的距离，获取与最小距离对应的交点的坐标，并将所述交点标记为终点。

10.一种机器人，其特征在于，包括如权利要求8或9所述的机械臂的运动轨迹规划装置。