机器人的组合路径的速度前瞻方法、装置与流程

1.本发明涉数据处理技术领域，具体涉及一种机器人的组合路径的速度前瞻方法、机器人的组合路径的速度前瞻装置。


背景技术：

2.在机器人运动过程中，为了保证轨迹精度，可采用速度前瞻控制。速度前瞻控制能够提前对运动轨迹进行分析和处理，发现高曲率点和尖锐拐角，然后对路径上的速度进行规划，找出减速点。
3.相关技术中，一般采用连续微线段进行速度前瞻，其实现思路是：依次读取每段路径的始末点，然后根据路径长度和始末速度计算允许规划的最大速度vlim，再计算衔接点pi处受衔接夹角约束的速度vc，以vc和前后两段路径最大规划速度vlim的最小值vmin作为pi点速度。
4.上述方式中，计算衔接点速度时需要前后进行两次对比并迭代计算，计算效率低，且并没有考虑多个维度的速度前瞻策略，例如无法处理位置和姿态同步的问题，无法保证机器人的运动效率和平稳度。


技术实现要素：

5.本发明为解决上述技术问题，提供了一种机器人的组合路径的速度前瞻方法，支持多维度组合路径的速度前瞻，能有效改善机器人的机械臂的运动效率和平稳度，拓宽了应用场景，且采用递归算法构建二叉树结构以进行规划数据存储，即采用二分法的分治思想，计算效率高。
6.本发明还提供了一种机器人的组合路径的速度前瞻装置。
7.本发明采用的技术方案如下：本发明的第一方面实施例提出了一种机器人的组合路径的速度前瞻方法，包括以下步骤：获取所述机器人的组合路径在每个维度的速度敏感点的速度边界；获取所述机器人的组合路径在每个维度下的最大规划速度、最大规划加速度、最大规划加加速度和始末状态，并对所有维度进行同步速度规划；根据每个维度的速度敏感点的速度边界和规划速度进行路径分割，并采用递归算法构建二叉树结构，以进行规划数据存储，所述规划数据包括：几何路径和速度轮廓，所述速度轮廓包括路径长度、速度和加速度随时间变化的曲线；对所述二叉树结构进行遍历，以将所述二叉树结构的各个叶子节点的规划数据压入机器人的轨迹执行队列本发明上述提出的机器人的组合路径的速度前瞻方法还具有如下附加技术特征：根据本发明的一个实施例，根据每个维度的速度敏感点的速度边界和规划速度进行路径分割，并采用递归算法构建二叉树结构，具体包括：将所述组合路径作为根节点；获取所述根节点在每个维度下的每个速度敏感点处的速度边界和对应的规划速度，并采用递归算法构建二叉树结构，其中；所述二叉树结构的左节点和右节点构建的条件是：当前节点
在任一个维度存在规划速度大于所述速度边界的速度敏感点时，将当前节点中所述规划速度与所述速度边界的差值绝对值的最大值对应的速度敏感点作为前瞻分割点将所述当前节点一分为二，以形成对应的左节点和右节点，将所述前瞻分割点的速度边界作为所述左节点的末速度、所述右节点的始速度，且将所述前瞻分割点的加速度设置为零；所述二叉树的叶子节点的构建条件：当前节点在每个维度不存在规划速度大于所述速度边界的速度敏感点，将当前节点作为叶子节点。
8.根据本发明一个实施例，上述的速度前瞻方法还包括：依次获取所述叶子节点在每个维度下的末速度、始速度和运动约束，所述运动约束包括最大规划速度、最大规划加速度和最大规划加加速度规划数据；判断所述叶子节点在对应的运动约束下是否能够由始速度达到末速度；如果所述叶子节点在对应的规划数据下不能够由始速度达到末速度，则对所述叶子节点的末速度或始速度进行调整，并对所述叶子节点重新进行速度规划。
9.根据本发明的一个实施例，对所述叶子节点的末速度或始速度进行调整，具体包括：将所述叶子节点的所述末速度和所述始速度中较大值向下调整，调整下限是所述末速度和所述始速度相等，且加速度保持不变。
10.根据本发明的一个实施例，上述的速度前瞻方法还包括：如果对所述叶子节点的末速度进行速度调整，则将调整后的速度同步至后一个叶子节点的始速度；如果对所述叶子节点的始速度进行速度调整，则将调整后的速度同步至前一个叶子节点的末速度。
11.根据本发明的一个实施例，采用先序遍历的方式对所述二叉树结构进行遍历。
12.本发明的第二方面实施例提出了一种机器人的组合路径的速度前瞻装置，包括：获取模块，所述获取模块用于获取所述机器人的组合路径在每个维度的速度敏感点的速度边界；规划模块，所述规划模块用于获取所述机器人的组合路径在每个维度下的最大规划速度、最大规划加速度、最大规划加加速度和始末状态，并对所有维度进行同步速度规划；构建模块，所述构建模块用于根据每个维度的速度敏感点的速度边界和规划速度进行路径分割，并采用递归算法构建二叉树结构，以进行规划数据存储，所述规划数据包括：几何路径和速度轮廓，所述速度轮廓包括路径长度、速度和加速度随时间变化的曲线；遍历模块，所述遍历模块用于对所述二叉树结构进行遍历，以将所述二叉树结构的各个叶子节点的规划数据压入机器人的轨迹执行队列。
13.本发明上述提出的机器人的组合路径的速度前瞻装置还具有如下附加技术特征：根据本发明的一个实施例，所述构建模块具体用于：将所述组合路径作为根节点；获取所述根节点在每个维度下的每个速度敏感点处的速度边界和对应的规划速度，并采用递归算法构建二叉树结构，其中；所述二叉树结构的左节点和右节点构建的条件是：当前节点在任一个维度存在规划速度大于所述速度边界的速度敏感点时，将当前节点中所述规划速度与所述速度边界的差值绝对值的最大值对应的速度敏感点作为前瞻分割点将所述当前节点一分为二，以形成对应的左节点和右节点，将所述前瞻分割点的速度边界作为所述左节点的末速度、所述右节点的始速度，且将所述前瞻分割点的加速度设置为零；所述二叉树的叶子节点的构建条件：当前节点在每个维度不存在规划速度大于所述速度边界的速度敏感点，将当前节点作为叶子节点。
14.根据本发明的一个实施例，所述构建模块还用于：依次获取所述叶子节点在每个维度下的末速度、始速度和运动约束，所述运动约束包括最大规划速度、最大规划加速度和
最大规划加加速度规划数据；判断所述叶子节点在对应的运动约束下是否能够由始速度达到末速度；如果所述叶子节点在对应的规划数据下不能够由始速度达到末速度，则对所述叶子节点的末速度或始速度进行调整，并对所述叶子节点重新进行速度规划。
15.根据本发明的一个实施例，所述构建模块还用于：如果对所述叶子节点的末速度进行速度调整，则将调整后的速度同步至后一个叶子节点的始速度；如果对所述叶子节点的始速度进行速度调整，则将调整后的速度同步至前一个叶子节点的末速度。
16.根据本发明的一个实施例，所述构建模块具体用于：将所述叶子节点的所述末速度和所述始速度中较大值向下调整，调整下限是所述末速度和所述始速度相等，且加速度保持不变。
17.根据本发明的一个实施例，所述构建模块还用于：如果对所述叶子节点的末速度进行速度调整，则将调整后的速度同步至后一个叶子节点的始速度；如果对所述叶子节点的始速度进行速度调整，则将调整后的速度同步至前一个叶子节点的末速度。
18.根据本发明的一个实施例，所述遍历模块4采用先序遍历的方式对所述二叉树结构进行遍历。
19.本发明的有益效果：1、支持多维度组合路径的速度前瞻，能有效改善机器人的机械臂的运动效率和平稳度，拓宽了应用场景；2、采用递归算法构建二叉树结构以进行规划数据存储，即采用二分法的分治思想，计算效率高；3、前瞻分割点速度可不为零，能进一步提升相关机器人的运动效率。
附图说明
20.图1是根据本发明一个实施例的机器人的组合路径的速度前瞻方法的流程图；图2为本发明一个具体实施例的二叉树结构示意图；图3是根据本发明一个实施例的机器人的组合路径的速度前瞻装置的方框示意图。
具体实施方式
21.下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。
22.图1是根据本发明一个实施例的机器人的组合路径的速度前瞻方法的流程图，如图1所示，该方法包括以下步骤：s1，获取机器人的组合路径在每个维度的速度敏感点的速度边界。
23.具体地，机器人的组合路径即为机器人需要运行的连续多段路径（运动轨迹），速度敏感点为路径中需要对速度进行约束的点，速度边界为速度到达的最大值。速度敏感点和对应的速度边界可以根据组合路径的具体情况进行提前计算。对于不同的维度，组合路径的速度敏感点、速度边界不同，例如，在位置维度下，对于曲线路径考虑曲率、弓高误差对
速度的约束，对于连续微线段考虑衔接夹角对速度的约束；在姿态维度下，考虑实际应用场景对角速度的约束。综合比较速度约束和给定最大规划速度，取最小值作为该点的速度边界。具体地维度可以根据实际应用场景进行设定，包括不限于上述的位置维度和姿态维度。
24.s2，获取机器人的组合路径在每个维度下的最大规划速度、最大规划加速度、最大规划加加速度和始末状态，并对所有维度进行同步速度规划。
25.在本发明的实施例中，可以采用在线规划算法对机器人进行速度规划，在线规划算法能够在线生成机器人的运动轨迹，该算法适用于多自由度机器人，支持多个维度的同步速度规划，且运动始末的速度和加速度都可以设成任意值，生成的轨迹相位同步、时间最优，并且对于所有维度都是同步的，所有维度都能在同一时刻到达目标。
26.s3，根据每个维度的速度敏感点的速度边界和规划速度进行路径分割，并采用递归算法构建二叉树结构，以进行规划数据存储。其中，规划数据可以包括：几何路径和速度轮廓，速度轮廓包括路径长度、速度和加速度随时间变化的曲线。
27.在本发明的一个实施例中，根据每个维度的速度敏感点的速度边界和规划速度进行路径分割，并采用递归算法构建二叉树结构，具体包括：将组合路径作为根节点；获取根节点在每个维度下的每个速度敏感点处的速度边界和对应的规划速度，并采用递归算法构建二叉树结构，其中；二叉树结构的左节点和右节点构建的条件是：当前节点在任一个维度存在规划速度大于速度边界的速度敏感点时，将当前节点中规划速度与速度边界的差值绝对值的最大值对应的速度敏感点作为前瞻分割点将当前节点一分为二，以形成对应的左节点和右节点，将前瞻分割点的速度边界作为左节点的末速度、右节点的始速度，且将前瞻分割点的加速度设置为零；二叉树的叶子节点的构建条件：当前节点在每个维度不存在规划速度大于速度边界的速度敏感点，将当前节点作为叶子节点。
28.具体地，分割后的路径统称为节点，速度前瞻成功后的节点统称为叶子，按照从整体到局部，从左到右的顺序对组合路径进行速度规划和分割。如图2所示，将组合路径作为根节点1，采用递归算法对根节点1进行路径分割，以形成左节点和右节点，具体分割步骤为：逐一比较当前节点在任意一个维度（例如位置维度）下速度敏感点处的速度边界和对应的规划速度，判断是否存在规划速度大于速度边界的速度敏感点，如果存在规划速度大于速度边界的速度敏感点则将当前节点中规划速度与速度边界的差值绝对值的最大值对应的速度敏感点作为前瞻分割点，将当前节点一分为二，形成左节点2和右节点3，并将将前瞻分割点的速度边界作为左节点的末速度、右节点的始速度，且将前瞻分割点的加速度设置为零。然后再采用上述分割步骤对左节点2进行路径分割，如此循环。
29.如果当前节点不存在规划速度大于速度边界的速度敏感点，则认为当前节点在该维度规划成功，进一步判断当前节点在下一个维度是否存在规划速度大于速度边界的速度敏感点，如果存在则采用上述分割步骤进行分割即可，如果不存在，则继续判断剩下维度，直至所有维度判断完毕。如果该节点在所有维度判断完毕，该节点在所有维度均不存在规划速度大于速度边界的速度敏感点，则将该节点作为叶子节点，该节点对应的路径完成速度前瞻。图2中节点8、9、10、11、12、13、14和15均为叶子节点。
30.可以理解，本发明采用的是递归算法构建二叉树结构，因此先对左节点进行路径分割，再对对应的右节点进行分割，直至所有节点分割完毕。
31.举例而言，维度包括位置维度和姿态维度，如果图2所示，先判断节点1在位置维度是否满足分割条件（存在规划速度大于速度边界的速度敏感点），节点1在位置维度满足分割条件，节点1路径分割为节点2和3后，再判断节点2位置维度是否满足分割条件，如果节点2在位置维度满足分割条件，节点2被分割成节点4和节点5，再判断节点4是否在位置维度满足分割条件，如果节点4在位置维度不满足分割条件，则进一步判断节点4在姿态维度是否满足分割条件，如果节点4在姿态维度满足分割条件，则节点4分割为节点8和节点9。然后判断节点8是否在位置维度满足分割条件，如果节点8在位置维度不满足分割条件，则进一步判断节点8在姿态维度是否满足分割条件，如果节点8在姿态维度不满足分割条件，则节点8在所有维度完成分割，节点8作为叶子节点。然后依次判断节点8对应的右节点（即节点9）是否在位置维度、姿态维度满足分割条件，如果节点9在位置维度和姿态维度均不满足分割条件，则节点9在所有维度完成分割，节点9作为叶子节点。如果自身是右节点，则会直接返回到其父节点进行判断。
32.由于节点9自身是右节点，则返回节点9的父节点即节点4。由于节点4已进行分割，则对节点4对应的右节点（节点5）是否在位置维度满足分割条件，如果节点5在位置维度不满足分割条件，则进一步判断节点5在姿态维度是否满足分割条件，如果节点5在姿态维度满足分割条件，则节点5分割为节点10和节点11。然后依次判断节点10是否在位置维度、姿态维度满足分割条件，如果节点10在位置维度和姿态维度均不满足分割条件，则节点10在所有维度完成分割，节点10作为叶子节点。然后判断节点10对应的右节点（即节点11）是否在位置维度、姿态维度满足分割条件，如果节点11在位置维度和姿态维度均不满足分割条件，则节点11在所有维度完成分割，节点11作为叶子节点。
33.由于节点11自身为右节点，则返回节点11的父节点即节点5。节点5已完成分割且自身为右节点，则再返回节点5的父节点即节点2，节点2存在对应的右节点即节点3，再对节点3进行分割，分割逻辑与上述的节点2相同，本发明中不再进行赘述。从而形成图2所示的二叉树结构，其中二叉树结构中每个节点中均存储有对应路径的几何路径和速度轮廓，速度轮廓包括路径长度、速度和加速度随时间变化的曲线。
34.总体而言，按照从整体到局部，从左到右的顺序对组合路径进行速度规划和分割。从整体的角度看待速度前瞻问题，采用二分法的分治思想，和一般的局部迭代优化相比，计算效率更高。
35.s4，对二叉树结构进行遍历，以将二叉树结构的各个叶子节点的规划数据压入机器人的轨迹执行队列。
36.在本发明的一个实施例中，可以采用先序遍历的方式对二叉树结构进行遍历，也可以采用中序遍历、后序遍历或层序遍历的方式，能遍历出二叉树结构的叶子节点即可。
37.由此，上述方法支持多维度组合路径的速度前瞻，能有效改善机器人的机械臂的运动效率和平稳度，拓宽了应用场景，且采用递归算法构建二叉树结构以进行规划数据存储，即采用二分法的分治思想，计算效率高。
38.根据本发明的一个实施例，上述的组合路径的速度前瞻方法还包括：依次获取叶子节点在每个维度下的末速度、始速度和运动约束，运动约束包括最大规划速度、最大规划加速度和最大规划加加速度规划数据；判断叶子节点在对应的运动约束下是否能够由始速度达到末速度；如果叶子节点在对应的规划数据下不能够由始速度达到末速度，则对叶子
节点的末速度或始速度进行调整，并对叶子节点重新进行速度规划。
39.进一步的而言，对叶子节点的末速度或始速度进行调整，具体包括：将叶子节点的末速度和始速度中较大值向下调整，调整下限是末速度和始速度相等，且加速度保持不变。
40.根据本发明的一个实施例，如果对叶子节点的末速度进行速度调整，则将调整后的速度同步至后一个叶子节点的始速度；如果对叶子节点的始速度进行速度调整，则将调整后的速度同步至前一个叶子节点的末速度。
41.具体地，速度前瞻过程中，采用前瞻分割点处的速度边界作为左右节点的末/始速度，速度规划时可能存在因路径长度短始末速度无法达到的情况，此时在满足速度边界要求后，逐一对每个维度的始/末速度进行调整，重新速度规划。其中速度调整是将速度轮廓中始末速度较大值向下调整，调整下限是始末速度相等，加速度保持不变。若调整当前叶子节点的起始速度，则上溯到上一叶子调整其末速度；若调整当前叶子的末速度，则调整下一叶子的起始速度与之相等，从而保证速度连续。
42.至此，机器人的多维度自适应速度前瞻成功，得到速度、加速度连续的多维速度轮廓。
43.综上所述，根据本发明实施例的组合路径的速度前瞻方法，支持多维度组合路径的速度前瞻，能有效改善机器人的机械臂的运动效率和平稳度，拓宽了应用场景；采用递归算法构建二叉树结构以进行规划数据存储，即采用二分法的分治思想，计算效率高；前瞻分割点速度可不为零，能进一步提升相关机器人的运动效率。
44.与上述的机器人的组合路径的速度前瞻方法相对应，本发明还提出一种机器人的组合路径的速度前瞻装置。由于本发明的装置实施例与上述的方法实施例，对装置实施例中未披露的细节请参照上述的方法实施例，本发明中不再进行赘述。
45.图3是根据本发明一个实施例的机器人的组合路径的速度前瞻装置的方框示意图，如图3所示，该装置包括：获取模块10、规划模块20、构建模块30和遍历模块40。
46.其中，获取模块10用于获取机器人的组合路径在每个维度的速度敏感点的速度边界；规划模块20用于获取机器人的组合路径在每个维度下的最大规划速度、最大规划加速度、最大规划加加速度和始末状态，并对所有维度进行同步速度规划；构建模块30用于根据每个维度的速度敏感点的速度边界和规划速度进行路径分割，并采用递归算法构建二叉树结构，以进行规划数据存储，规划数据包括：几何路径和速度轮廓，速度轮廓包括路径长度、速度和加速度随时间变化的曲线；遍历模块40用于对二叉树结构进行遍历，以将二叉树结构的各个叶子节点的规划数据压入机器人的轨迹执行队列。
47.根据本发明的一个实施例，构建模块30具体用于：将组合路径作为根节点；获取根节点在每个维度下的每个速度敏感点处的速度边界和对应的规划速度，并采用递归算法构建二叉树结构，其中；二叉树结构的左节点和右节点构建的条件是：当前节点在任一个维度存在规划速度大于速度边界的速度敏感点时，将当前节点中规划速度与速度边界的差值绝对值的最大值对应的速度敏感点作为前瞻分割点将当前节点一分为二，以形成对应的左节点和右节点，将前瞻分割点的速度边界作为左节点的末速度、右节点的始速度，且将前瞻分割点的加速度设置为零；二叉树的叶子节点的构建条件：当前节点在每个维度不存在规划速度大于速度边界的速度敏感点，将当前节点作为叶子节点。
48.根据本发明的一个实施例，构建模块30还用于：依次获取叶子节点在每个维度下
的末速度、始速度和运动约束，运动约束包括最大规划速度、最大规划加速度和最大规划加加速度规划数据；判断叶子节点在对应的运动约束下是否能够由始速度达到末速度；如果叶子节点在对应的规划数据下不能够由始速度达到末速度，则对叶子节点的末速度或始速度进行调整，并对叶子节点重新进行速度规划。
49.根据本发明的一个实施例，构建模块30还用于：如果对叶子节点的末速度进行速度调整，则将调整后的速度同步至后一个叶子节点的始速度；如果对叶子节点的始速度进行速度调整，则将调整后的速度同步至前一个叶子节点的末速度。
50.根据本发明的一个实施例，构建模块30具体用于：将叶子节点的末速度和始速度中较大值向下调整，调整下限是末速度和始速度相等，且加速度保持不变。
51.根据本发明的一个实施例，构建模块30还用于：如果对叶子节点的末速度进行速度调整，则将调整后的速度同步至后一个叶子节点的始速度；如果对叶子节点的始速度进行速度调整，则将调整后的速度同步至前一个叶子节点的末速度。
52.根据本发明的一个实施例，遍历模块40采用先序遍历的方式对二叉树结构进行遍历。
53.综上所述，根据本发明实施例的机器人的组合路径的速度前瞻装置，支持多维度组合路径的速度前瞻，能有效改善机器人的机械臂的运动效率和平稳度，拓宽了应用场景；采用递归算法构建二叉树结构以进行规划数据存储，即采用二分法的分治思想，计算效率高；前瞻分割点速度可不为零，能进一步提升相关机器人的运动效率。
54.在本发明的描述中，术语“第一”、“第二”仅用于描述目的，而不能理解为指示或暗示相对重要性或者隐含指明所指示的技术特征的数量。由此，限定有“第一”、“第二”的特征可以明示或者隐含地包括一个或者更多个该特征。“多个”的含义是两个或两个以上，除非另有明确具体地限定。
55.在本说明书的描述中，参考术语“一个实施例”、“一些实施例”、“示例”、“具体示例”、或“一些示例”等的描述意指结合该实施例或示例描述的具体特征、结构、材料或者特点包含于本发明的至少一个实施例或示例中。在本说明书中，对上述术语的示意性表述不必针对相同的实施例或示例。而且，描述的具体特征、结构、材料或者特点可以在任一个或多个实施例或示例中以合适的方式结合。此外，在不相互矛盾的情况下，本领域的技术人员可以将本说明书中描述的不同实施例或示例以及不同实施例或示例的特征进行结合和组合。
56.尽管已经示出和描述了本发明的实施例，对于本领域的普通技术人员而言，可以理解在不脱离本发明的原理和精神的情况下可以对这些实施例进行多种变化、修改、替换和变型，本发明的范围由所附权利要求及其等同物限定。