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

一种机械臂的运动轨迹规划方法、装置及机器人的制作方法
【专利摘要】本发明公开了一种机械臂的运动轨迹规划方法，包括：根据预置的起点坐标、终点坐标以及速度曲线算法控制机械臂以预定的运动轨迹进行运动；当接收到用户发出的停止指令后，获取机械臂的当前运动时间，并根据所述当前运动时间及为每个运动阶段配置的持续时间确定所述机械臂当前所处的运动阶段；根据运动臂当前所处的运动阶段更改预定的运动阶段的持续时间及初始参数集合中的预定的初始参数，生成更改后的运动轨迹，并控制机械臂以更改后的运动轨迹进行运动。本发明还公开了一种机械臂的运动轨迹规划装置及机器人，在接收到用户的停止指令后，通过对运动轨迹进行重新规划，使得所述运动臂在保持平稳运动及速度连续的前提下，尽快停止。
【专利说明】
一种机械臂的运动轨迹规划方法、装置及机器人
技术领域
[0001] 本发明涉及机器人控制领域，具体是一种机械臂的运动轨迹规划方法、装置及机 器人。
【背景技术】
[0002] 在机械臂的工作过程中，需要对机械臂的轨迹进行规划，一般来说，这种轨迹规划 都是通过预定的速度规划算法来实现的，例如，可使用S型(Double S)速度曲线算法来完成 机械臂的轨迹规划。
[0003] 有些速度规划算法在进行轨迹规划时，需要事先接收用户提供的起点和终点的位 置，然后程序会在两点之间生成一系列的插补点来描述机械臂的运行轨迹。比如在示教功 能中，用户通过手持设备发送指令，使得机械臂从起点出发，一直沿某个方向直线运动，直 至到达终点。
[0004] 但是很多时间，终点的位置终点位置是由用户根据实际的情况实时决定的，因而 用户无法在一开始运动的时候就给出一个具体的终点坐标，这导致了基于目前的速度规划 算法的机械臂在工作过程中无法对运动轨迹进行提前规划，影响了用户的使用体验。

【发明内容】

[0005] 针对上述问题，本发明的目的在于提供一种机械臂的运动轨迹规划方法、装置及 机器人，能根据用户的操作进行运动轨迹的重新规划，满足了用户的使用需求。
[0006] 本发明提供了一种机械臂的运动轨迹规划方法，包括如下步骤：
[0007] 根据预置的起点坐标、终点坐标以及速度曲线算法，控制机械臂以预定的运动轨 迹进行运动；其中，所述速度曲线算法根据预先配置的初始参数集合将所述机械臂的运动 轨迹规划为至少两个运动阶段，并为每个运动阶段配置一段持续时间；
[0008] 当接收到用户发出的停止指令后，获取所述机械臂的当前运动时间，并根据所述 当前运动时间及为每个运动阶段配置的持续时间确定所述机械臂当前所处的运动阶段；
[0009] 根据所述运动臂当前所处的运动阶段更改预定的运动阶段的持续时间及所述初 始参数集合中的预定的初始参数，生成更改后的运动轨迹，并控制所述机械臂以更改后的 运动轨迹进行运动。
[0010] 优选地，所述速度曲线算法为S型速度曲线算法，则所述初始参数集合至少包括路 程、加速运动阶段的最大加速度、最大速度、减速运动阶段的最小减速度、最大加加速度以 及最小加加速度这些初始参数;所述运动阶段包括加速运动阶段、匀速运动阶段及减速运 动阶段，所述加速运动阶段包括第一加速运动阶段、第二加速运动阶段及第三加速运动阶 段;所述减速运动阶段包括第一减速运动阶段、第二减速运动阶段及第三减速运动阶段。
[0011] 优选地，所述根据所述运动臂当前所处的运动阶段更改预定的运动阶段的持续时 间及所述初始参数集合中的预定的初始参数，生成更改后的运动轨迹，并控制所述机械臂 以更改后的运动轨迹进行运动，具体为：
[0012] 当判断所述机械臂当前处于第一加速运动阶段时，将所述第一加速运动阶段、第 三加速运动阶段、所述第一减速运动阶段及所述第三减速运动阶段的持续时间更改为当前 运动时间，并将所述第二加速运动阶段、匀速运动阶段及第二减速运动阶段的持续时间更 改为零；
[0013] 根据所述当前运动时间对路程、最大速度、加速运动阶段的最大加速度及减速运 动阶段的最小减速度这些初始参数进行更改，根据更改后的初始参数生成更改后的运动轨 迹，并控制所述机械臂以更改后的运动轨迹进行运动。
[0014] 优选地，所述根据所述运动臂当前所处的运动阶段更改预定的运动阶段的持续时 间及所述初始参数集合中的预定的初始参数，生成更改后的运动轨迹，并控制所述机械臂 以更改后的运动轨迹进行运动，具体为：
[0015] 当判断所述机械臂当前处于第二加速运动阶段时，将所述第二加速运动阶段及第 二减速运动阶段的持续时间更改为t-t，并将所述匀速运动阶段的持续时间更改为零;其 中，t为当前运动时间，ti-to为预先配置的从开始计时的时刻to到所述第一加速运动阶段结 束的持续时间；
[0016] 根据所述当前运动时间t对所述路程及所述最大速度这两个初始参数进行更改， 根据更改后的初始参数生成更改后的运动轨迹，并控制所述机械臂以更改后的运动轨迹进 行运动。
[0017] 优选地，所述根据所述运动臂当前所处的运动阶段更改预定的运动阶段的持续时 间及所述初始参数集合中的预定的初始参数，生成更改后的运动轨迹，并控制所述机械臂 以更改后的运动轨迹进行运动，具体为：
[0018] 当判断所述机械臂当前处于第三加速运动阶段时，将所述匀速运动阶段的持续时 间更改为零；
[0019] 根据所述当前运动时间对路程这个初始参数进行更改，根据更改后的初始参数生 成更改后的运动轨迹，并控制所述机械臂以更改后的运动轨迹进行运动。
[0020] 优选地，所述根据所述运动臂当前所处的运动阶段更改预定的运动阶段的持续时 间及所述初始参数集合中的预定的初始参数，生成更改后的运动轨迹，并控制所述机械臂 以更改后的运动轨迹进行运动，具体为：
[0021] 当判断所述机械臂当前处于匀速运动阶段时，将所述匀速运动阶段的持续时间更 改为t_t3;其中，t为当前运动时间，t 3-to为预先配置的从开始计时的时刻to到所述第三加 速运动阶段结束的持续时间；
[0022] 根据所述当前运动时间t对所述路程这个初始参数进行更改，根据更改后的初始 参数生成更改后的运动轨迹，并控制所述机械臂以更改后的运动轨迹进行运动。
[0023] 优选地，在所述根据预置的起点坐标、终点坐标以及速度曲线算法，控制机械臂以 预定的运动轨迹进行运动之前，还包括：
[0024] 基于机械臂的关节参数，生成所述机械臂的工作区域；
[0025] 根据预先输入的起点的坐标及移动方向生成一条射线，并计算所述射线与所述工 作区域的边界的交点，生成交点集合；
[0026]计算所述交点集合内的每个交点与所述起点的距离，获取与最小距离对应的交点 的坐标，并将所述交点标记为终点。
[0027] 本发明还提供了一种机械臂的运动轨迹规划装置，包括：
[0028] 运动控制单元，用于根据预置的起点坐标、终点坐标以及速度曲线算法，控制机械 臂以预定的运动轨迹进行运动;其中，所述速度曲线算法根据预先配置的初始参数集合将 所述机械臂的运动轨迹规划为至少两个运动阶段，并为每个运动阶段配置一段持续时间；
[0029] 运动时间获取单元，用于当接收到用户发出的停止指令后，获取所述机械臂的当 前运动时间，并根据所述当前运动时间t及为每个运动阶段配置的持续时间确定所述机械 臂当前所处的运动阶段；
[0030] 运动轨迹更改单元，用于根据所述运动臂当前所处的运动阶段更改预定的运动阶 段的持续时间及所述初始参数集合中的预定的初始参数，生成更改后的运动轨迹，并控制 所述机械臂以更改后的运动轨迹进行运动。
[0031] 优选地，还包括：
[0032] 工作区域生成单元，用于基于待工作的机械臂的关节参数，生成所述机械臂的工 作区域；
[0033] 交点集合生成单元，用于根据预先输入的起点的坐标及移动方向生成一条射线， 并计算所述射线与所述工作区域的边界的交点，生成交点集合；
[0034] 终点标记单元，用于计算所述交点集合内的每个交点与所述起点的距离，获取与 最小距离对应的交点的坐标，并将所述交点标记为终点。
[0035] 本发明还提供了一种机器人，包括上述的机械臂的运动轨迹规划装置。
[0036] 本发明实施例提供的机械臂的运动轨迹规划方法、装置及机器人，当检测到进入 减速运动阶段之前接收到用户的停止指令后，则根据当前所处的运动阶段对当前及后续的 运动轨迹进行重新规划，使得所述运动臂在保持平稳运动及速度连续的前提下，尽快停止， 从而满足用户的使用需求。
【附图说明】
[0037] 为了更清楚地说明本发明的技术方案，下面将对实施方式中所需要使用的附图作 简单地介绍，显而易见地，下面描述中的附图仅仅是本发明的一些实施方式，对于本领域普 通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图获得其他的附图。
[0038] 图1是本发明实施例提供的机械臂的运动轨迹规划方法的流程示意图。
[0039] 图2是S形速度曲线算法的运动过程示意图。
[0040] 图3是S形速度曲线算法的计算原理流程图。
[0041] 图4是SCARA型机械臂的结构示意图。
[0042] 图5是图4所示的SCARA型机械臂在DH坐标系的示意图。
[0043] 图6是图4所示的SCARA型机械臂的工作区域示意图。
[0044] 图7是图6所示的工作区域的俯视图。
[0045]图8是射线与工作区域的连接示意图。
[0046]图9是本发明实施例提供的通过射线与圆相交测试的优化算法来计算射线与圆的 交点的流程示意图。
[0047]图10(a)至10(c)是图9所示的优化算法的原理图。
[0048]图11是本发明实施例提供的机械臂的运动轨迹规划装置的结构示意图。
[0049] 图12是本发明实施例提供的机械臂的运动轨迹规划装置的另一种结构示意图。
【具体实施方式】
[0050] 下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完 整地描述，显然，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例。基于 本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他 实施例，都属于本发明保护的范围。
[0051] 本发明实施例提供一种机械臂的运动轨迹规划方法，可根据用户发出的停止指令 实时确定出终点坐标，并提前规划新的运动轨迹。
[0052] 请参阅图1，本发明实施例提供一种机械臂的运动轨迹规划方法流程示意图，其可 由机械臂的运动轨迹规划装置（以下简称为运动轨迹规划装置)来执行，并至少包括如下步 骤：
[0053] SlOl，根据预置的起点坐标、终点坐标以及速度曲线算法，控制机械臂以预定的运 动轨迹进行运动;其中，所述速度曲线算法根据预先配置的初始参数集合将所述机械臂的 运动轨迹规划为至少两个运动阶段，并为每个运动阶段配置一段持续时间。
[0054] 在本发明实施例中，机械臂是机器人技术领域中最广泛应用的自动化机械装置， 其广泛应用于工业制造、医学治疗、娱乐服务、军事、半导体制造以及太空探索等领域。尽管 它们的形态各有不同，但它们都有一个共同的特点，就是能够接受指令，并根据指令精确地 定位到三维(或二维)空间上的某一点进行作业。
[0055] 在本发明实施例中，机械臂的运动一般都是根据速度曲线算法来控制的，例如，常 见的有梯形速度曲线算法、正弦形速度曲线算法以及S形速度曲线算法，其中，S形速度曲线 算法由于能够对加速度的变化率进行控制，并且在速度连接处光滑过渡，所以在运行时十 分平稳，不会出现震动的现象，因此得到广泛的应用。
[0056]其中，S型速度曲线算法在运行时，需用户输入初始参数集合(包括路程qi、起点速 度vo、起点加速度ao终点速度^、终点加速度ai，加速运动阶段的最大加速度amax、最大速度 Vmax、减速运动阶段的最小减速度amin、最大加加速度jmax以及最小加加速度jmin)，然后再根 据这些初始参数进行运动轨迹的轨迹。如图2所示，一般来说，典型的S型速度曲线算法会将 运动轨迹规划为三个运动阶段:加速运动阶段(持续时间T a)、匀速运动阶段(持续时间为Tv) 及减速运动阶段(持续时间Td)，更具体地:所述加速运动阶段包括第一加速运动阶段(对应 于时间区间[?ο，ω，持续时间为T jl，此时加速度从0逐渐增加到amax)、第二加速运动阶段 (对应于时间区间Uht 2),此时加速度保持不变)及第三加速运动阶段(对应于时间区间 [t2，t3)，持续时间为T jl，加速度从amax逐渐减小为0);所述减速运动阶段包括第一减速运动 阶段(对应于时间区间[t 4，t5)，持续时间为Tj2,此时加速度从0逐渐减小到a_)、第二减速 运动阶段(对应于时间区间[t 5，t6)，此时加速度保持不变)及第三减速运动阶段(对应于时 间区间[t6，t7)，持续时间为T j2，此时加速度从amin逐渐增大为0)，其中，在加速度加速阶段 和加速度减速阶段，相应的加加速度分别为jmax和jmin，且一般地，jmax = _ jmin，Tjl = Tj2，amax --Bmin ο
[0057]下面将详细介绍S型速度曲线算法进行轨迹规划的过程，其中，为了便于计算，这 里取VQ = vi = 〇，ao = ai = 0。如图3所示，首先，S型速度曲线算法需先接受用户给出的初始参 数，所述初始参数包括:路程qi、起点速度VQ、起点加速度ao终点速度V1、终点加速度ai，加速 运动阶段的最大加速度amax、最大速度Vmax、减速运动阶段的最小减速度amin、最大加加速度 Jmax以及最小加加速度jmin。接着，S型速度曲线算法根据这些初始参数，判断(Vmax-VO) jmax〈 <2；^是否成立，若成立，则使用公式（1)来计算具体的时间参数TjlJA TjTd。若不成立， 则使用公式⑵来计算具体的时间参数1^1、1'」2工及1 (1。此后，在获得1'」1、1'」2工及1(1后，根据 公式(3)计算得到的T a来计算得到IV。接着，判断Τν>0是否成立，若成立，则使用公式(6)、公 式（7)来计算得到最终的t〇、tl、t2、13、1435、16、〖7。若不成立，则进一步判断是否满足
;若满足，则根据公式(4)得到Tv以及新的T a&Td，并根据公式(6)、公式 ⑴来计算得到最终的1：()、1：1、1：2、〖3、〖4、〖5、〖6、〖7。若不满足，则使用公式(5)计算得到1\以及 新的Tjl、Tj2、Ta及Td，并根据公式(6)、公式(7)来计算得到最终的t〇、tl、t2、t3、t4、t5、t6、t7<^ 中公式(1)~(7)的具体形式如下：
[0065]在本发明实施例中，在获得如公式(6)、（7)所述的公式或等式后，即可以获得所述 机械臂在任意时刻的位置，具体可参考如下的公式(8):
[0067] S102,当接收到用户发出的停止指令后，获取所述机械臂的当前运动时间，并根据 所述当前运动时间及为每个运动阶段配置的持续时间确定所述机械臂当前所处的运动阶 段。
[0068] 在本发明实施例中，用户在使用所述机械臂的时候，可能随时根据需要发出停止 指令(例如在示教过程中，用户可通过示教盒向机械臂发出停止指令），则当所述运动轨迹 规划装置接收到用户发出的停止指令后，获取所述机械臂的当前运动时间t，并根据所述当 前运动时间t及所述持续时间(或时间区间）确定所述机械臂当前所处的运动阶段。
[0069] 例如，假设所述运动轨迹规划装置检测到当前运行时间为5S，而t3 = 4s，t4 = 6S，则 可确定当前所处的运动阶段为匀速运动阶段。又例如，假设所述运动轨迹规划装置检测到 当前运行时间为25，而〖1 = 1.5844 = 35，则可确定当前所处的运动阶段为第二加速运动阶 段。
[0070] S103,根据所述运动臂当前所处的运动阶段更改预定的运动阶段的持续时间及所 述初始参数集合中的预定的初始参数，生成更改后的运动轨迹，并控制所述机械臂以更改 后的运动轨迹进行运动。
[0071] 在本发明实施例中，在用户发出停止指令后，所述运动轨迹规划装置需要尽快且 保持稳定的情况下尽快停下所述机械臂，为此，所述运动轨迹规划装置需要根据所述运动 臂当前所处的运动阶段更改预定的运动阶段的持续时间及所述初始参数集合中的预定的 初始参数。
[0072] 具体地：
[0073] S1031，当判断所述机械臂当前处于第一加速运动阶段时，将所述第一加速运动阶 段、第三加速运动阶段、所述第一减速运动阶段及所述第三减速运动阶段的持续时间更改 为t，并将所述第二加速运动阶段、匀速运动阶段及第二减速运动阶段的持续时间更改为 零。
[0074] 在本发明实施例中，当所述运动轨迹规划装置判断用户在第一加速运动阶段就发 出停止指令，即当前运行时间〖〈。时，所述运动轨迹规划装置立即终止第一加速运动阶段， 同时为了尽快停止所述机械臂，还将所述第二加速运动阶段、匀速运动阶段及第二减速运 动阶段的持续时间更改为零。
[0075] 则此时，各个运动阶段的时间区间变为:所述第一加速运动阶段对应的时间区间 为[to，tlnl)，所述第二加速运动阶段对应的时间区间为[t lnl，t2nl)，所述第三加速运动阶段 对应的时间区间为[t2nl，t3nl)，所述匀速运动阶段对应的时间区间为[t3nl，t 4nl)，所述第一 减速运动阶段对应的时间区间为[t4nl，t5nl)，所述第二减速运动阶段对应的时间区间为 [t5nl，t6nl )，所述第三减速运动阶段对应的时间区间更改为[t6nl，t7nl )，其中，t0 = O，tlnl = ?2η1 - t j ?3η1 - ?4η1 - 2t，t5nl - ?6η1 - 3t，t7nl - 4t 0
[0076] S1032,根据所述当前运动时间对路程、最大速度、加速运动阶段的最大加速度及 减速运动阶段的最小减速度这些初始参数进行更改，根据更改后的初始参数生成更改后的 运动轨迹，并控制所述机械臂以更改后的运动轨迹进行运动。
[0077] 其中，将加速运动阶段的最大加速度amax更改为jmax*t，将减速运动阶段的最小减 速度iimin更改为 _ jmin*t，将取大速度Vmax更改为t*amax，将路程Ql更改为t*amax*2t，此后，根据 公式(8)即可获得更改后的运动轨迹，并控制所述机械臂以更改后的运动轨迹进行运动。
[0078] 具体地：
[0079] S1033,当判断所述机械臂当前处于第二加速运动阶段时，将所述第二加速运动阶 段及第二减速运动阶段的持续时间更改为t-t，并将所述匀速运动阶段的持续时间更改为 零。
[0080]在本发明实施例中，当所述运动轨迹规划装置判断用户在第二加速运动阶段就发 出停止指令，此时当前运行时间t满足:tl〈t〈t2时，所述运动轨迹规划装置立即终止第二加 速运动阶段，即将所述第二加速运动阶段，同时为了尽快停止所述机械臂，还将所述第二减 速运动阶段的持续时间更改为t-ti，将所述匀速运动阶段更改为零。
[0081 ]则此时，各个运动阶段的时间区间变为:所述第一加速运动阶段对应的时间区间 为[thtd，所述第二加速运动阶段对应的时间区间为所述第三加速运动阶段对应 的时间区间为[t，t3n2)，所述匀速运动阶段对应的时间区间为[t 3n2，t4n2)，所述第一减速运 动阶段对应的时间区间为[t4n2，t 5n2)，所述第二减速运动阶段对应的时间区间为[t5n2， t6n2 )，所述第三减速运动阶段对应的时间区间更改为[t6n2，t7n2 )，其中，t0 = 0，t3n2 = t4n2 = t +Tjl，?5η2 - t4n2+Tj2，?6η2 - ?5η2+?~tl，?7η2 - t6n2+Tj2 0
[0082] S1034,根据所述当前运动时间t对所述路程如及所述最大速度Vmax这两个初始参 数进行更改，根据更改后的初始参数生成更改后的运动轨迹，并控制所述机械臂以更改后 的运动轨迹进行运动。
[0083] 其中，更改后的Vmax=(T-Tjl)jmax Tjl，更改后的Q1=(T-Tjl)amax T，T = Ta-(t2_t)。
[0084] 此后，根据公式(8)即可获得更改后的运动轨迹，并控制所述机械臂以更改后的运 动轨迹进行运动。
[0085] 具体地：
[0086] S1035,当判断所述机械臂当前处于第三加速运动阶段时，将所述匀速运动阶段的 持续时间更改为零。
[0087] 在本发明实施例中，当所述运动轨迹规划装置判断用户在第三加速运动阶段就发 出停止指令，此时当前运行时间t满足:t2〈t〈t 3时，为了尽快停止所述机械臂，将所述匀速运 动阶段更改为零。
[0088] 则此时，各个运动阶段的时间区间变为:所述第一加速运动阶段对应的时间区间 为[thtd，所述第二加速运动阶段对应的时间区间为所述第三加速运动阶段对应 的时间区间为[t 2，t3)，所述匀速运动阶段对应的时间区间为[t3，t4n3)，所述第一减速运动 阶段对应的时间区间为[t4n3，t5n3)，所述第二减速运动阶段对应的时间区间为[t 5n3，t6n3)， 所述第三减速运动阶段对应的时间区间更改为[t6n3，t7n3 )，其中，t0 = O，t4n3 = t3，t5n3 = t4n3+ Tj2 ? ?6η3 - t5n3+Td_2Tj2，?7η2 - t6n3+Tj2 0
[0089] S1036,根据所述当前运动时间t对初始参数qi进行更改，根据更改后的初始参数 生成更改后的运动轨迹，并控制所述机械臂以更改后的运动轨迹进行运动。
[0090] 其中，qi = qi-Vmax IV。此后，根据公式(8)即可获得更改后的运动轨迹，并控制所述 机械臂以更改后的运动轨迹进行运动。
[0091] 具体地：
[0092] S1037,当判断所述机械臂当前处于匀速运动阶段时，将所述匀速运动阶段的持续 时间更改为t_t3。
[0093] 此时，各个运动阶段的时间区间变为:所述第一加速运动阶段对应的时间区间为 [thtd，所述第二加速运动阶段对应的时间区间为所述第三加速运动阶段对应的 时间区间为[t 2，t3)，所述匀速运动阶段对应的时间区间为[t3，t)，所述第一减速运动阶段 对应的时间区间为[t，t 5n4)，所述第二减速运动阶段对应的时间区间为[t5n4，t6n4)，所述第 三减速运动阶段对应的时间区间更改为[t6n4, t7n4)，其中，t5n4 = t+Tj2，t6n4 = t5n4+Td_2Tj2， ?7η4 - t6n4+Tj2 0
[0094] S1038,根据所述当前运动时间t对路程如这个初始参数进行更改，根据更改后的 初始参数生成更改后的运动轨迹，并控制所述机械臂以更改后的运动轨迹进行运动。
[0095] 其中，更改后的路程(11 = (111_[1\-(卜七3)]。此后，根据公式(8)即可获得更改后 的运动轨迹，并控制所述机械臂以更改后的运动轨迹进行运动。
[0096] 在本发明实施例中，若是在减速运动阶段接收到用户的停止指令，则不对运动轨 迹进行更改。
[0097] 综上所述，本发明实施例提供的机械臂的运动轨迹规划方法，当检测到进入减速 运动阶段之前接收到用户的停止指令后，则根据当前所处的运动阶段对当前及后续的运动 轨迹进行重新规划，使得所述运动臂在保持平稳运动及速度连续的前提下，尽快停止，从而 满足用户的使用需求。
[0098]需要说明的是，上述实施例中，所述速度曲线算法为S型速度曲线算法，但本发明 的思想同样可应用于其他的速度曲线算法，特别的，适用于具有多个运动阶段的速度曲线 算法，这些方案均在本发明的保护范围之内，在此不做赘述。
[0099] 需要说明的是，在上述实施例中，在运动前，需要获得终点坐标，再根据起点坐标、 运动方向和终点坐标获得路程9:，下面将介绍本发明的一个优选实施例提供的终点坐标的 获取方法。
[0100] 具体地，可基于如下步骤来获得终点坐标：
[0101] SO1，基于待工作的机械臂的关节参数，生成所述机械臂的工作区域。
[0102] -般来说，一个机械臂包括若干个关节，每个关节都具有关节参数，所述关节参数 可包括关节类型、臂长(连接两个关节的连接臂的长度）、活动范围等。其中，所述关节类型 可包括转动关节及滑动关节，转动关节可控制连接臂的转动，而滑动关节可实现连接臂的 垂直滑动。通过不同关节的组合，即可控制所述机械臂的工作区域。具体地，可通过如下步 骤来确定所述机械臂的工作区域：
[0103] soil，根据机械臂的关节类型及关节之间的相对位置关系，基于DH坐标系建立生 成每个关节的坐标系。
[0104] 如图4所不，以SCARA(Selective Compliant Articulated Robot for Assembly) 型机械臂为例，其具有4个关节11』2，13』4，其中11』2，14为转动关节，且11与12，12与了4 通过相应的连接臂进行连接，而J3为滑动关节。当然，应当理解的是，本发明还可应用于其 他类型的机械臂，在此不做赘述。
[0105] 在确定上述关节后，需要针对每个关节建立坐标系，如图5所示，可基于DH (Denavit-Hartenberg)坐标系来建立每个关节的坐标系，其中转动关节Ji的转动轴与各自 的zi轴(转动关节Ji的连接臂绕zi轴转动)互相平行，连接臂的臂长为ai，U|ie {1，2,4}}; 滑动关节J3的轴(z3轴)跟J4的轴(z4轴)平行。在确定出zi轴后，可用第一个关节指向第二 个关节的方向作为xi方向，而yi的方向可基于右手定则来确定，如此，即生成了每个关节的 坐标系。
[0106] S012,根据每个关节的坐标系、每个关节的活动范围及关节之间的臂长，生成所述 机械臂的工作区域。
[0107] 在本发明实施例中，在生成每个关节的坐标系后，根据每个关节的活动范围（对于 转动关节，其活动范围为其连接臂转动的转动角范围，对于滑动关节，其活动范围为其连接 臂的上下运动范围），即可生成所述机械臂的工作区域(如图6所示），此时，只需在平面进行 投影即可生成如图7所示的俯视图。
[0108] S02,根据预先输入的起点的坐标及移动方向生成一条射线，并计算所述射线与所 述工作区域的边界的交点，生成交点集合。
[0109]具体的，可包括：
[0110] S021，将所述工作区域分解为至少两段弧线，获取每段弧线的圆心、半径及圆心角 范围，并生成与每段弧线对应的圆的方程。
[0111] 从图7中可以看出，所述机械臂的工作区域是由片H哀^,(5?四段弧线所 围成。而这四段弧线分别位于θ〇1，θ〇2, θ〇3, 0〇4四个圆上。圆心(^，(^与心轴重合山:^和出 分别为J1轴转动到正负极限时J 2轴的位置。Jiim分别是轴上的正/负方向的最大活动
[0112] 这四段弧线的详细参数可参照表1所示：
[0118] S022,根据预先输入的起点的坐标及移动方向生成一条射线，计算所述射线与每 个圆的交点。
[0119] 在本发明实施例中，假设起点为I，方向向量为n，I |n| I为方向向量η的单位向量。 则此时射线可表示为R(u) = I+u · I |n| I。
[0120] 如果起点I位于工作区域内，则射线R(u)必与某圆〇 Oi有交点
[0121] 其中，在计算交点时，可通过将射线R(U)的方程依次与每个圆的方程进行联立，在 利用参数方程法求解获得。
[0122] S023,计算所述交点在所述交点所在的圆上的弧度角，并在所述弧度角位于所述 圆心角范围内时，确定所述交点位于所述弧线上，将所述交点存入预设的集合，生成交点集 合。
[0123] 在本发明实施例中，上述求得的交点中，有些虽然与圆相交，但是并没有位于弧线上， 因而需要去除。具体地，如图8所示，射线IP分别与θ (^相交于点N;与θ O2相交于点K和点M;与θ 〇4相交于点J和点L;与θ O3没有交点。其中，点K，N虽然在圆上，但是并不在围成所述工作区域 的弧线上，所以在得到交点后，还要查看所述交点的弧度角Θ1是否满足6^,." < # <
[0124] 具体地，假设点s(x，y)是θ0ι上的一个点，则点s相对于0〇1的弧度角Θ 1为：
[0126] 通过上述公式就可以确定哪些交点是在弧线上，哪些不在。此时，将那些位于弧线 上的交点存入预设的集合ξ = Ικ1}中，生成交点集合。
[0127] 需要说明的是，在本发明的其他实施例中，所述工作区域并不一定是由圆的弧线 组成。例如，所述工作区域可能由椭圆的弧线组成或者由不同类型的几何形状的弧线混合 组成，此时，同理，可获得这些弧线对应的几何形状的方程及角度范围，再通过上述的方法 即可计算生成交点集合，这些技术方案均在本发明的保护范围之内，在此不做赘述。
[0128] S03,计算所述交点集合内的每个交点与所述起点的距离，获取与最小距离对应的 交点的坐标，并将所述交点标记为终点。
[0129] 在本发明实施例中，可通过欧拉公式计算得到每个交点与所述起点I的距离，然后 对计算得到的每个距离进行比较，获取与最小距离对应的交点的坐标，并将所述交点标记 为终点，此时，即得到所需的工作区域的边界。
[0130] 综上所述，在本优选实施例中，通过生成所述机械臂的工作区域，再计算由预先输 入的起点的坐标及移动方向生成的射线与工作区域的边界的交点，并获取与起点具有最小 距离的交点的坐标，获得与所述起点及移动方向相应的终点。本发明基于几何的方法来确 定机械臂工作区域的边界因此机器人在运动前就可以提前知道终点的位置，方便机器人进 行运动轨迹的规划。
[0131] 优选地，针对步骤S022,在生成所述射线后，可以使用传统的参数方程法求交点， 也可以使用Akenine-Mdller等提出的基于射线与圆相交测试的优化算法来计算所述射线 与每个圆的交点。
[0132] 具体地，如图9所示，设射线的参数方程为R(u) = I+u· I |n| I，其中I |n| I是单位长 度。如图10(a)所示，首先计算出从起点I出发到圆心⑴的向量这=/_ 6^向量这的长度 a2 = U以及向量α沿着I |n| I方向的投影i = a:若则说明起点位于0 〇汐卜，且射线的方向沿远离^(^方向延伸，因此所述射线与QO1不相交(如图10(b)所示），此 时完成第一次排除测试。否则，利用勾股定理计算出圆心⑴与投影之间距离的平方:m2 = a2- I2,若M2 > rf，则可以判定射线与圆O1-定不相交，完成第二次排除测试。如果射线和0 O1 经过两次排除测试，则就可以判定它们一定相交。接下来，计算射线与0 Oi的交点：首先，计 算出距离
然后判断起点I是否位于Θ O1R，若α2 > η2,则说明起点I位于 〇〇,外，此时，所述射线与圆O1有两个交点，分别为?4 = / + 〇 + (?) I |n| I以及 4 = ? + (!-幻 Ilnll(如图10(a)所示）。若a2 < if，则说明起点I位于圆〇,内，此时，所 述射线与圆O1有一个交点，为Ι+α+q) · I |n| I (如图10(C)所示）。
[0133] 本优选方案中，在计算射线与圆的交点之前，先通过两次测试判断射线与圆是否 有交点，在计算时，仅计算与射线有交点的圆，而不需要对那些与射线没有交点的圆进行计 算，如此，减小了计算量，提升了计算效率。
[0134] 请一并参阅图11，图11是本发明实施例提供的一种机械臂的运动轨迹规划装置 100,其包括：
[0135] 运动控制单元10,用于根据预置的起点坐标、终点坐标以及速度曲线算法，控制机 械臂以预定的运动轨迹进行运动;其中，所述速度曲线算法根据预先配置的初始参数集合 将所述机械臂的运动轨迹规划为至少两个运动阶段，并为每个运动阶段配置一段持续时 间。
[0136] 运动时间获取单元20,用于当接收到用户发出的停止指令后，获取所述机械臂的 当前运动时间t，并根据所述当前运动时间t及为每个运动阶段配置的持续时间确定所述机 械臂当前所处的运动阶段。
[0137] 运动轨迹更改单元30,用于根据所述运动臂当前所处的运动阶段更改预定的运动 阶段的持续时间及所述初始参数集合中的预定的初始参数，生成更改后的运动轨迹，并控 制所述机械臂以更改后的运动轨迹进行运动。
[0138] 具体为：
[0139] 当所述运动轨迹更改单元30判断所述机械臂当前处于第一加速运动阶段时，将所 述第一加速运动阶段、第三加速运动阶段、所述第一减速运动阶段及所述第三减速运动阶 段的持续时间更改为t，并将所述第二加速运动阶段、匀速运动阶段及第二减速运动阶段的 持续时间更改为零;此后，根据所述当前运动时间t对初始参数am ax、amin、Vmax及qi进行更改， 根据更改后的初始参数生成更改后的运动轨迹，并控制所述机械臂以更改后的运动轨迹进 行运动。
[0140] 具体地：
[0141] 当所述运动轨迹更改单元30判断所述机械臂当前处于第二加速运动阶段时，将所 述第二加速运动阶段及第二减速运动阶段的持续时间更改为t_t 2,并将所述匀速运动阶段 的持续时间更改为零，此后，根据所述当前运动时间t对初始参数vmax& qi进行更改，根据更 改后的初始参数生成更改后的运动轨迹，并控制所述机械臂以更改后的运动轨迹进行运 动。
[0142] 具体地：
[0143] 当所述运动轨迹更改单元30判断所述机械臂当前处于第三加速运动阶段时，将所 述匀速运动阶段的持续时间更改为零；
[0144] 根据所述当前运动时间t对初始参数qi进行更改，根据更改后的初始参数生成更 改后的运动轨迹，并控制所述机械臂以更改后的运动轨迹进行运动。
[0145] 具体地：
[0146] 当所述运动轨迹更改单元30判断所述机械臂当前处于匀速运动阶段时，将所述匀 速运动阶段的持续时间更改为t_t3;
[0147] 根据所述当前运动时间t对初始参数qi进行更改，根据更改后的初始参数生成更 改后的运动轨迹，并控制所述机械臂以更改后的运动轨迹进行运动。
[0148] 本发明实施例提供的机械臂的运动轨迹规划装置100,当检测到进入减速运动阶 段之前接收到用户的停止指令后，则根据当前所处的运动阶段对当前及后续的运动轨迹进 行重新规划，使得所述运动臂在保持平稳运动及速度连续的前提下，尽快停止，从而满足用 户的使用需求。
[0149] 请一并参阅图12,需要说明的是，在本发明实施例中，为了获得所述机械臂的终点 坐标，所述机械臂的运动轨迹规划装置还包括：
[0150] 工作区域生成单元40,用于基于待工作的机械臂的关节参数，生成所述机械臂的 工作区域。
[0151]交点集合生成单元50,用于根据预先输入的起点的坐标及移动方向生成一条射 线，并计算所述射线与所述工作区域的边界的交点，生成交点集合。
[0152]终点标记单元60,用于计算所述交点集合内的每个交点与所述起点的距离，获取 与最小距离对应的交点的坐标，并将所述交点标记为终点。
[0153]本发明实施例提供中，通过工作区域生成单元40生成所述机械臂的工作区域，交 点集合生成单元50再计算由预先输入的起点的坐标及移动方向生成的射线与工作区域的 边界的交点，所述终点标记单元60获取与起点具有最小距离的交点的坐标，获得与所述起 点及移动方向相应的终点，再根据所述起点、终点及预设的轨迹规划算法进行运动轨迹的 规划。本发明基于几何的方法来确定机械臂工作区域的边界因此机器人在运动前就可以提 前知道终点的位置，方便机器人进行运动轨迹的规划。
[0154] 本发明还提供一种机器人，包括上述的机械臂的运动轨迹规划装置100。
[0155] 本发明实施例提供的机器人，当检测到进入减速运动阶段之前接收到用户的停止 指令后，则根据当前所处的运动阶段对当前及后续的运动轨迹进行重新规划，使得所述运 动臂在保持平稳运动及速度连续的前提下，尽快停止，从而满足用户的使用需求。
[0156] 本发明实施例提供的机器人，若是在进入减速运动阶段之前接收到用户的停止指 令后，则对整个运动轨迹进行重新规划，使得所述运动臂在保持平稳运动及速度连续的前 提下，尽快停止，从而满足用户的使用需求。
【主权项】
1. 一种机械臂的运动轨迹规划方法，其特征在于，包括如下步骤： 根据预置的起点坐标、终点坐标以及速度曲线算法，控制机械臂以预定的运动轨迹进 行运动；其中，所述速度曲线算法根据预先配置的初始参数集合将所述机械臂的运动轨迹 规划为至少两个运动阶段，并为每个运动阶段配置一段持续时间； 当接收到用户发出的停止指令后，获取所述机械臂的当前运动时间，并根据所述当前 运动时间及为每个运动阶段配置的持续时间确定所述机械臂当前所处的运动阶段； 根据所述运动臂当前所处的运动阶段更改预定的运动阶段的持续时间及所述初始参 数集合中的预定的初始参数，生成更改后的运动轨迹，并控制所述机械臂以更改后的运动 轨迹进行运动。2. 根据权利要求1所述的机械臂的运动轨迹规划方法，其特征在于，所述速度曲线算法 为S型速度曲线算法，则所述初始参数集合至少包括路程、加速运动阶段的最大加速度、最 大速度、减速运动阶段的最小减速度、最大加加速度以及最小加加速度这些初始参数;所述 运动阶段包括加速运动阶段、匀速运动阶段及减速运动阶段，所述加速运动阶段包括第一 加速运动阶段、第二加速运动阶段及第三加速运动阶段;所述减速运动阶段包括第一减速 运动阶段、第二减速运动阶段及第三减速运动阶段。3. 根据权利要求2所述的机械臂的运动轨迹规划方法，其特征在于，所述根据所述运动 臂当前所处的运动阶段更改预定的运动阶段的持续时间及所述初始参数集合中的预定的 初始参数，生成更改后的运动轨迹，并控制所述机械臂以更改后的运动轨迹进行运动，具体 为： 当判断所述机械臂当前处于第一加速运动阶段时，将所述第一加速运动阶段、第三加 速运动阶段、所述第一减速运动阶段及所述第三减速运动阶段的持续时间更改为当前运动 时间，并将所述第二加速运动阶段、匀速运动阶段及第二减速运动阶段的持续时间更改为 零； 根据所述当前运动时间对所述路程、所述最大速度、所述加速运动阶段的最大加速度 及所述减速运动阶段的最小减速度这些初始参数进行更改，根据更改后的初始参数生成更 改后的运动轨迹，并控制所述机械臂以更改后的运动轨迹进行运动。4. 根据权利要求2所述的机械臂的运动轨迹规划方法，其特征在于，所述根据所述运动 臂当前所处的运动阶段更改预定的运动阶段的持续时间及所述初始参数集合中的预定的 初始参数，生成更改后的运动轨迹，并控制所述机械臂以更改后的运动轨迹进行运动，具体 为： 当判断所述机械臂当前处于第二加速运动阶段时，将所述第二加速运动阶段及第二减 速运动阶段的持续时间更改为t-t，并将所述匀速运动阶段的持续时间更改为零;其中，t 为当前运动时间，ti-to为预先配置的从开始计时的时刻to到所述第一加速运动阶段结束的 持续时间； 根据所述当前运动时间t对所述路程及所述最大速度这两个初始参数进行更改，根据 更改后的初始参数生成更改后的运动轨迹，并控制所述机械臂以更改后的运动轨迹进行运 动。5. 根据权利要求2所述的机械臂的运动轨迹规划方法，其特征在于，所述根据所述运动 臂当前所处的运动阶段更改预定的运动阶段的持续时间及所述初始参数集合中的预定的 初始参数，生成更改后的运动轨迹，并控制所述机械臂以更改后的运动轨迹进行运动，具体 为： 当判断所述机械臂当前处于第三加速运动阶段时，将所述匀速运动阶段的持续时间更 改为零； 根据所述当前运动时间对所述路程这个初始参数进行更改，根据更改后的初始参数生 成更改后的运动轨迹，并控制所述机械臂以更改后的运动轨迹进行运动。6. 根据权利要求2所述的机械臂的运动轨迹规划方法，其特征在于，所述根据所述运动 臂当前所处的运动阶段更改预定的运动阶段的持续时间及所述初始参数集合中的预定的 初始参数，生成更改后的运动轨迹，并控制所述机械臂以更改后的运动轨迹进行运动，具体 为： 当判断所述机械臂当前处于匀速运动阶段时，将所述匀速运动阶段的持续时间更改为 t_t3;其中，t为当前运动时间，t3-to为预先配置的从开始计时的时刻to到所述第三加速运 动阶段结束的持续时间； 根据所述当前运动时间t对所述路程这个初始参数进行更改，根据更改后的初始参数 生成更改后的运动轨迹，并控制所述机械臂以更改后的运动轨迹进行运动。7. 根据权利要求1至6任意一项所述的机械臂的运动轨迹规划方法，其特征在于，在所 述根据预置的起点坐标、终点坐标以及速度曲线算法，控制机械臂以预定的运动轨迹进行 运动之前，还包括： 基于机械臂的关节参数，生成所述机械臂的工作区域； 根据预先输入的起点的坐标及移动方向生成一条射线，并计算所述射线与所述工作区 域的边界的交点，生成交点集合； 计算所述交点集合内的每个交点与所述起点的距离，获取与最小距离对应的交点的坐 标，并将所述交点标记为终点。8. -种机械臂的运动轨迹规划装置，其特征在于，包括： 运动控制单元，用于根据预置的起点坐标、终点坐标以及速度曲线算法，控制机械臂以 预定的运动轨迹进行运动;其中，所述速度曲线算法根据预先配置的初始参数集合将所述 机械臂的运动轨迹规划为至少两个运动阶段，并为每个运动阶段配置一段持续时间； 运动时间获取单元，用于当接收到用户发出的停止指令后，获取所述机械臂的当前运 动时间，并根据所述当前运动时间及为每个运动阶段配置的持续时间确定所述机械臂当前 所处的运动阶段； 运动轨迹更改单元，用于根据所述运动臂当前所处的运动阶段更改预定的运动阶段的 持续时间及所述初始参数集合中的预定的初始参数，生成更改后的运动轨迹，并控制所述 机械臂以更改后的运动轨迹进行运动。9. 根据权利要求8所述的机械臂的运动轨迹规划装置，其特征在于，还包括： 工作区域生成单元，用于基于待工作的机械臂的关节参数，生成所述机械臂的工作区 域； 交点集合生成单元，用于根据预先输入的起点的坐标及移动方向生成一条射线，并计 算所述射线与所述工作区域的边界的交点，生成交点集合； 终点标记单元，用于计算所述交点集合内的每个交点与所述起点的距离，获取与最小 距离对应的交点的坐标，并将所述交点标记为终点。10.-种机器人，其特征在于，包括如权利要求8或9所述的机械臂的运动轨迹规划装 置。
【文档编号】B25J9/16GK105922265SQ201610453116
【公开日】2016年9月7日
【申请日】2016年6月20日
【发明人】罗汉杰
【申请人】广州视源电子科技股份有限公司