混合空间过渡轨迹规划方法及装置与流程

1.本发明涉及工业机器人运动控制技术领域，具体涉及一种混合空间过渡轨迹规划方法及装置。


背景技术：

2.工业机械臂已经广泛运用于各个领域，极大地提高了生产效率。笛卡尔空间轨迹一般用于作业任务，例如焊接、涂胶等，关节空间轨迹一般用于不同作业任务之间点到点的位姿转换，两者独立规划已经成熟且较为常见。在实际应用场景中，机械臂的运动轨迹往往需要在两个空间中不停转换来完成一项完整的加工任务，如果还采用每段轨迹单独规划的方式则会造成机械臂在轨迹收尾阶段时频繁启停，不仅降低了作业效率，且容易对机械臂造成冲击，增加时间成本和能耗成本。
3.针对上述需求，目前已有的研究是在多段轨迹之间采用过渡段的方式平滑运动，即从上一条轨迹平滑过渡到下一条轨迹，中间不停顿。工业界常用的是圆弧过渡，可保证轨迹平滑且可实现匀速过渡，但一般只用在直线与直线轨迹之间，其他轨迹之间的拼接由于加速度的跃变导致并不适用圆弧过渡。
4.针对混合空间过渡，中国发明专利“一种工业机器人应用的过渡轨迹规划方法”(申请号201610075346.7)对位姿6个自由度分别采用基于五次多项式的两条抛物线融合合成过渡曲线的算法，保证了轨迹、速度、加速度的平滑性，对于无位置变化仅有姿态变化的轨迹也能实现过渡。但该方案存在的缺陷也较为明显：一是轨迹过渡的平滑时间选取策略较难，选取不当容易造成过渡段速度过冲或降速明显；二是在过渡点速度选取上只是简单使两者相等，无法兼顾平衡前后两段轨迹的动态特性，可能对机械臂造成抖动，影响作业效率；三是该方案研究只在规划层进行研究，没有考虑插补时精度问题及在线调速问题，而在实际工程应用中，确保轨迹精度以及示教模式下过渡算法支持在线调速是基础，因此同时兼顾考虑混合空间的规划层算法与插补层精度指标和在线调速特性是很有必要的。
5.中国发明专利“一种基于混合空间的工业机器人过渡轨迹规划方法”(申请号202010978202.9)使用笛卡尔空间加关节空间的混合空间矢量描述方法来规划机器人的过渡轨迹，可实现奇异段运动路径与常规段运动路径之间的过渡轨迹规划。通过混合空间规划方法对机器人存在奇异区域的过渡轨迹进行规划，雅可比矩阵连接轨迹过渡点处的混合空间和笛卡尔空间的速度、加速度矢量，使得机器人末端轨迹平滑过渡，避免了常规的笛卡尔空间过渡轨迹规划方法会对机器人本体产生冲击的情况。但该方案存在的缺陷也较为明显：一是该方案未对衔接的下一段轨迹进行前瞻预规划，因此过渡终点pe的速度信息会选取不当导致下一段规划失败，并且容易造成过渡段速度过冲或降速明显；二是该主要针对奇异段运动路径进行规划，不适用于所有混合空间场景；同时，方案研究只在规划层进行研究，没有考虑插补时精度问题及在线调速问题，而在实际工程应用中，确保轨迹精度以及示教模式下过渡算法支持在线调速是基础，因此同时兼顾考虑混合空间的规划层算法与插补层精度指标和在线调速特性是很有必要的。


技术实现要素：

6.本发明所要解决的技术问题在于如何实现平滑时间及过渡点速度的可靠稳定选取，避免出现过渡段速度过冲导致超速或降速明显。
7.本发明通过以下技术手段实现解决上述技术问题的：
8.一方面，本发明提出了一种混合空间过渡轨迹规划方法，所述方法包括以下步骤：
9.获取过渡轨迹规划的运动参数，所述运动参数包括第一段轨迹的起点位姿和终点位姿以及第二段轨迹的终点位姿和过渡参数百分比，所述第一段轨迹和所述第二段轨迹为笛卡尔空间的直线轨迹或关节空间的轨迹；
10.对所述第一段轨迹和所述第二段轨迹进行独立预规划，计算所述关节空间的轨迹的规划总时间和/或所述笛卡尔空间的直线轨迹的减速段时间；
11.根据所述关节空间的轨迹的规划总时间和/或所述笛卡尔空间的直线轨迹的减速段时间以及所述过渡参数百分比，计算平滑过渡时间以及过渡点的边界条件；
12.基于所述过渡点的边界条件和所述平滑过渡时间，在关节空间内构造样条曲线并进行求解，得到过渡曲线的轨迹方程。
13.本发明从过渡段前后两段轨迹本身的运动特性出发，对两段轨迹进行独立预规划，计算关节空间的轨迹的规划总时间和/或笛卡尔空间的直线轨迹的减速段时间，从而计算平滑过渡时间及过渡点的边界条件，避免出现过渡段速度过冲导致超速或降速明显，将混合空间过渡段映射到关节空间，构造一条位置、速度及加速度均连续的过渡轨迹，可实现关节空间与关节空间、关节空间与笛卡尔空间之间的轨迹平滑过渡，而且不存在姿态过渡的问题。
14.进一步地，所述对所述第一段轨迹和所述第二段轨迹进行独立预规划，计算所述关节空间的轨迹的规划总时间和/或所述笛卡尔空间的直线轨迹的减速段时间，包括：
15.对于所述关节空间的轨迹，根据逆运动学计算出该轨迹起点位姿对应的起点关节角以及终点位姿对应的终点关节角，并根据s型曲线加减速算法对该轨迹进行起点关节速度为0、终点关节速度为0的预规划，得到所述关节空间的轨迹的规划总时间；
16.对于所述笛卡尔空间的直线轨迹，根据s型曲线加减速算法对该轨迹进行起点速度为0、终点速度为0的预规划，得到所述笛卡尔空间的直线轨迹的减速段时间。
17.进一步地，所述根据所述关节空间的轨迹的规划总时间和/或所述笛卡尔空间的直线轨迹的减速段时间以及所述过渡参数百分比，计算平滑过渡时间以及过渡点的边界条件，包括：
18.在所述第一段轨迹和所述第二段轨迹为不同空间的轨迹时，将属于所述关节空间的轨迹的规划总时间的一半与属于所述笛卡尔空间的直线轨迹的减速段时间进行比较，取两者中较小值与所述过渡参数百分比相乘，得到所述平滑过渡时间；
19.在所述第一段轨迹和所述第二段轨迹均为所述关节空间的轨迹时，将两段轨迹的规划总时间中较小值的一半与所述过渡参数百分比相乘，得到所述平滑过渡时间；
20.基于所述平滑过渡时间，计算所述过渡点的边界条件，所述过渡点包括过渡起点和过渡终点。
21.进一步地，所述基于所述平滑过渡时间，计算所述过渡点的边界条件，包括：
22.对于为所述关节空间的轨迹，将所述平滑过渡时间代入该轨迹预规划后的时间插
值函数，得到第一过渡点的关节角和关节速度；
23.对于为所述笛卡尔空间的直线轨迹，将所述平滑过渡时间代入该直线轨迹预规划后的时间插值函数，得到与所述平滑过渡时间对应的位移，并根据所述位移和该轨迹的长度，更新所述过渡参数百分比；
24.基于更新后的所述过渡参数百分比，计算该直线轨迹上的过渡点的位姿和对应的笛卡尔速度六分量；
25.根据逆向运动学将所述直线轨迹上的过渡点的位姿进行反接，得到第二过渡点的关节角；
26.根据雅克比矩阵将所述笛卡尔速度六分量映射为第二过渡点的关节速度，其中所述第一过渡点和所述第二过渡点中的一个作为所述过渡起点、另一个作为所述过渡终点。
27.进一步地，在所述基于所述过渡点的边界条件和所述平滑过渡时间，在关节空间内构造样条曲线并进行求解，得到过渡曲线的轨迹方程之后，还包括：
28.设定插补时间t1＝interp_time，其中，interp_time是系统给定的插补周期；
29.实时检测速度倍率值rate，并基于当前检测的所述速度倍率值更新上一所述插补周期，计算当前的所述插补时间，实现在线调速。
30.进一步地，所述方法还包括：
31.在计算当前的所述插补时间之前，根据上一所述插补周期，计算剩余的插补次数；
32.根据所述平滑过渡时间和所述剩余的插补次数，计算误差时间；
33.根据所述误差时间和上一所述插补周期，计算目标插补周期；
34.基于当前检测的所述速度倍率值，更新所述目标插补周期。
35.另一方面，本发明提出了一种混合空间过渡轨迹规划装置，所述装置包括：
36.获取模块，用于获取过渡轨迹规划的运动参数，所述运动参数包括第一段轨迹的起点位姿和终点位姿以及第二段轨迹的终点位姿和过渡参数百分比，所述第一段轨迹和所述第二段轨迹为笛卡尔空间的直线轨迹或关节空间的轨迹；
37.预规划模块，用于对所述第一段轨迹和所述第二段轨迹进行独立预规划，计算所述关节空间的轨迹的规划总时间和/或所述笛卡尔空间的直线轨迹的减速段时间；
38.计算模块，用于根据所述关节空间的轨迹的规划总时间和/或所述笛卡尔空间的直线轨迹的减速段时间以及所述过渡参数百分比，计算平滑过渡时间以及过渡点的边界条件；
39.轨迹规划模块，用于基于所述过渡点的边界条件和所述平滑过渡时间，在关节空间内构造样条曲线并进行求解，得到过渡曲线的轨迹方程。
40.进一步地，所述预规划模块包括：
41.第一预规划单元，用于对于所述关节空间的轨迹，根据逆运动学计算出该轨迹起点位姿对应的起点关节角以及终点位姿对应的终点关节角，并根据s型曲线加减速算法对该轨迹进行起点关节速度为0、终点关节速度为0的预规划，得到所述关节空间的轨迹的规划总时间；
42.第二预规划单元，用于对于所述笛卡尔空间的直线轨迹，根据s型曲线加减速算法对该轨迹进行起点速度为0、终点速度为0的预规划，得到所述笛卡尔空间的直线轨迹的减速段时间。
43.进一步地，所述计算模块，包括：
44.平滑过渡时间计算单元，用于在所述第一段轨迹和所述第二段轨迹为不同空间的轨迹时，将属于所述关节空间的轨迹的规划总时间的一半与属于所述笛卡尔空间的直线轨迹的减速段时间进行比较，取两者中较小值与所述过渡参数百分比相乘，得到所述平滑过渡时间；
45.以及在所述第一段轨迹和所述第二段轨迹均为所述关节空间的轨迹时，将两段轨迹的规划总时间中较小值的一半与所述过渡参数百分比相乘，得到所述平滑过渡时间；
46.边界条件确定单元，用于执行如下步骤：
47.对于为所述关节空间的轨迹，将所述平滑过渡时间代入该轨迹预规划后的时间插值函数，得到第一过渡点的关节角和关节速度；
48.对于为所述笛卡尔空间的直线轨迹，将所述平滑过渡时间代入该直线轨迹预规划后的时间插值函数，得到与所述平滑过渡时间对应的位移，并根据所述位移和该轨迹的长度，更新所述过渡参数百分比；
49.基于更新后的所述过渡参数百分比，计算该直线轨迹上的过渡点的位姿和对应的笛卡尔速度六分量；
50.根据逆向运动学将所述直线轨迹上的过渡点的位姿进行反接，得到第二过渡点的关节角；
51.根据雅克比矩阵将所述笛卡尔速度六分量映射为第二过渡点的关节速度，其中所述第一过渡点和所述第二过渡点中的一个作为过渡起点、另一个作为过渡终点。
52.进一步地，所述装置还包括：
53.插补模块，用于设定插补时间t1＝interp_time，其中，interp_time是系统给定的插补周期，并实时检测速度倍率值rate，并基于当前检测的所述速度倍率值更新上一所述插补周期，计算当前的所述插补时间，实现在线调速。
54.本发明的优点在于：
55.(1)本发明从过渡段前后两段轨迹本身的运动特性出发，对两段轨迹进行独立预规划，计算关节空间的轨迹的规划总时间和/或笛卡尔空间的直线轨迹的减速段时间，从而计算平滑过渡时间及过渡点的边界条件，避免出现过渡段速度过冲导致超速或降速明显，将混合空间过渡段映射到关节空间，构造一条位置、速度及加速度均连续的过渡轨迹，可实现关节空间与关节空间、关节空间与笛卡尔空间之间的轨迹平滑过渡，而且不存在姿态过渡的问题。
56.(2)通过实时调整速度倍率值，更新插补周期，利用插补时间的减小来实现插补次数的增多，实现在线调速。
57.(3)在计算当前的插补时间之前，根据上一插补周期，计算误差时间，根据误差时间和上一插补周期，计算目标插补周期，根据实时检测的速度倍率值更新目标插补周期，实现过渡段的高精度插补。
58.本发明附加的方面和优点将在下面的描述中部分给出，部分将从下面的描述中变得明显，或通过本发明的实践了解到。
附图说明
59.图1是本发明第一实施例中混合空间过渡轨迹规划方法的流程图；
60.图2是本发明第一实施例中混合空间过渡轨迹规划方法的整体流程图；
61.图3是本发明中应用场景下机器人运行位移三维效果图；
62.图4是本发明中速度倍率下各关节位移随时间变化曲线图；
63.图5是本发明中应用场景下机器人各关节位移随时间变化曲线图；
64.图6是本发明第二实施例中混合空间过渡轨迹规划装置的结构图。
具体实施方式
65.为使本发明实施例的目的、技术方案和优点更加清楚，下面将结合本发明实施例，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有作出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。
66.如图1所述，本发明第一实施例提出了一种混合空间过渡轨迹规划方法，所述方法包括以下步骤：
67.s10、获取过渡轨迹规划的运动参数，所述运动参数包括第一段轨迹的起点位姿和终点位姿、第二段轨迹的终点位姿和过渡参数百分比，所述第一段轨迹和所述第二段轨迹为笛卡尔空间的直线轨迹或关节空间的轨迹；
68.需要说明的是，本实施例根据示教器段输入的运动指令解析得到两条轨迹的点位信息及过渡可选参数。机械臂末端点位姿由位置矢量(x,y,z)和姿态四元数(w,x,y,z)共同描述，过渡轨迹涉及的点位信息包括：第一段轨迹起点位姿pstart，第一段轨迹终点位姿pmid，第二段轨迹终点位姿pend，过渡参数百分比cp。
69.s20、对所述第一段轨迹和所述第二段轨迹进行独立预规划，计算所述关节空间的轨迹的规划总时间和/或所述笛卡尔空间的直线轨迹的减速段时间；
70.s30、根据所述关节空间的轨迹的规划总时间和/或所述笛卡尔空间的直线轨迹的减速段时间以及所述过渡参数百分比，计算平滑过渡时间以及过渡点的边界条件；
71.s40、基于所述过渡点的边界条件和所述平滑过渡时间，在关节空间内构造样条曲线并进行求解，得到过渡曲线的轨迹方程。
72.需要说明的是，本发明从过渡段前后两段轨迹本身的运动特性出发，对两段轨迹进行独立预规划，计算关节空间的轨迹的规划总时间和/或笛卡尔空间的直线轨迹的减速段时间，从而计算平滑过渡时间及过渡点的边界条件，避免出现过渡段速度过冲导致超速或降速明显；将混合空间过渡段映射到关节空间，构造一条位置、速度及加速度均连续的过渡轨迹，可实现关节空间与关节空间、关节空间与笛卡尔空间之间的轨迹平滑过渡，而且不存在姿态过渡的问题。
73.在一实施例中，所述步骤s20，包括以下步骤：
74.s21、对于所述关节空间的轨迹，根据逆运动学计算出该轨迹起点位姿对应的起点关节角以及终点位姿对应的终点关节角，并根据s型曲线加减速算法对该轨迹进行起点关节速度为0、终点关节速度为0的预规划，得到所述关节空间的轨迹的规划总时间；
75.s22、对于所述笛卡尔空间的直线轨迹，根据s型曲线加减速算法对该轨迹进行起
点速度为0、终点速度为0的预规划，得到所述笛卡尔空间的直线轨迹的减速段时间。
76.需要说明的是，当第一段轨迹是笛卡尔空间的直线轨迹时，根据s型曲线加减速算法对第一段轨迹进行起点速度为0、终点速度为0的预规划，计算得到第一段直线轨迹规划总时间line1_total_time、减速段时间dec1_time。当第一段轨迹是关节空间轨迹时，先根据逆运动学计算出第一段轨迹起点位姿对应的起点关节角、第一段轨迹终点位姿对应的终点关节角，然后根据s型曲线加减速算法对第一段轨迹进行起点关节速度为0、终点关节速度为0的预规划，计算得到第一段关节空间轨迹规划总时间joint1_total_time。
77.当第二段轨迹是笛卡尔空间的直线轨迹时，根据s型曲线加减速算法对第二段轨迹进行起点速度为0、终点速度为0的预规划，计算得到第二段直线轨迹规划总时间line2_total_time、减速段时间dec2_time。当第二段轨迹是关节空间轨迹时，先根据逆运动学计算出第二段轨迹起点位姿对应的起点关节角、第二段轨迹终点位姿对应的终点关节角，然后根据s型曲线加减速算法对第二段轨迹进行起点关节速度为0、终点关节速度为0的预规划，计算得到第二段关节空间轨迹规划总时间joint2_total_time。
78.在一实施例中，所述步骤s30，包括以下步骤：
79.s31、在所述第一段轨迹和所述第二段轨迹为不同空间的轨迹时，将属于所述关节空间的轨迹的规划总时间的一半与属于所述笛卡尔空间的直线轨迹的减速段时间进行比较，取两者中较小值与所述过渡参数百分比相乘，得到所述平滑过渡时间；
80.需要说明的是，若两段轨迹分别属于不同的空间，则平滑过渡时间smooth_time的计算公式如下：
[0081][0082]
s32、在所述第一段轨迹和所述第二段轨迹均为所述关节空间的轨迹时，将两段轨迹的规划总时间中较小值的一半与所述过渡参数百分比相乘，得到所述平滑过渡时间；
[0083]
需要说明的是，若两段轨迹均为关节空间的轨迹，则平滑过渡时间smooth_time的计算公式如下：
[0084][0085]
s33、基于所述平滑过渡时间，计算所述过渡点的边界条件，所述过渡点包括过渡起点和过渡终点。
[0086]
在一实施例中，所述步骤s33，具体包括以下步骤：
[0087]
s331、对于为所述关节空间的轨迹，将所述平滑过渡时间代入该轨迹预规划后的时间插值函数，得到第一过渡点的关节角和关节速度；
[0088]
s332、对于为所述笛卡尔空间的直线轨迹，将所述平滑过渡时间代入该直线轨迹预规划后的时间插值函数，得到与所述平滑过渡时间对应的位移，并根据所述位移和该轨迹的长度，更新所述过渡参数百分比；
[0089]
需要说明的是，过渡参数百分比的更新公式为：
[0090][0091]
其中，pos_ret为平滑过渡时间smooth_time对应的位移，dist_line为计算直线段
长度。
[0092]
s333、基于更新后的所述过渡参数百分比，计算该直线轨迹上的过渡点的位姿和对应的笛卡尔速度六分量；
[0093]
s334、根据逆向运动学将所述直线轨迹上的过渡点的位姿进行反接，得到第二过渡点的关节角；
[0094]
s335、根据雅克比矩阵将所述笛卡尔速度六分量映射为第二过渡点的关节速度，其中所述第一过渡点和所述第二过渡点中的一个作为所述过渡起点、另一个作为所述过渡终点。
[0095]
需要说明的是，若第一段轨迹为笛卡尔直线段轨迹，过渡点位姿和对应的笛卡尔速度六分量都计算完毕后，根据逆向运动学将过渡点位姿反解得到过渡点关节角j_pt1，根据雅克比矩阵将过渡点对应的笛卡尔速度六分量映射为关节速度jv_pt1。若第一段轨迹为关节空间轨迹，则直接可得到过渡点关节角j_pt1和对应的关节速度jv_pt1。
[0096]
若第二段轨迹为笛卡尔直线段轨迹，过渡点位姿和对应的笛卡尔速度六分量都计算完毕后，根据逆向运动学将过渡点位姿反解得到过渡点关节角j_pt2，根据雅克比矩阵将过渡点对应的笛卡尔速度六分量映射为关节速度jv_pt2。若第一段轨迹为关节空间轨迹，则直接可得到过渡点关节角j_pt2和对应的关节速度jv_pt2。
[0097]
需要说明的是，为简化运算，两个过渡点的关节加速度ja_pt1和ja_pt2均设为0。
[0098]
上述平滑时间及过渡点速度选取策略稳定可靠，平衡了两段曲线的动态特性，避免出现过渡段速度过冲导致超速或降速明显。
[0099]
在一实施例中，在所述步骤s40中，根据过渡点的边界条件，即过渡关节角、过渡关节速度及加速度，结合平滑过渡时间，在关节空间内构造五次样条曲线如下：
[0100]
p(t)＝at5+bt4+ct3+dt2+et+f
[0101]
其中，a、b、c、d、e、f为构造五次样条曲线的六个系数。
[0102]
对p(t)求一阶导数可以得到速度，对p(t)求二阶导数可以得到加速度，并且根据给定的六个边界条件，可构造出六个方程，结合p(t)、p(t)的一阶导数和p(t)的二阶导数，可准确求出六个未知系数，由此可完全确定过渡曲线的轨迹方程：
[0103][0104]
其中，t＝t
1-t0。
[0105]
在一实施例中，如图2所示，在所述步骤s40之后，还包括：
[0106]
设定插补时间t1＝interp_time，其中，interp_time是系统给定的插补周期；
[0107]
实时检测速度倍率值rate，并基于当前检测的所述速度倍率值更新上一所述插补周期，计算当前的所述插补时间，实现在线调速。
[0108]
需要说明的是，在过渡曲线轨迹规划完成后，进行到过渡曲线的插补过程，为实现在线调速，本实施例首先固定第一个插补时间t1＝interp_time_1，interp_time_1是系统
给定的插补周期，表示为速度倍率rate＝100％对应的插补周期，然后将t1带入到p(t)中计算当前插补时间t1对应的关节角jstates；实时检测速度倍率值rate，当用户改变速度倍率rate时，实时线程对倍率改变进行检测，若发生改变则过渡曲线的插补需要对倍率改变进行响应，采取策略为基于当前检测的速度倍率值更新上一插补周期interp_time_2＝rate
×
interp_time_1，计算第二个插补时间t2＝t1+interp_time_2并带入p(t)中计算当前插补时间对应的关节角jstates，以此类推，利用插补时间的减小来实现插补次数的增多，进而实现在线调速。
[0109]
在一实施例中，所述方法还包括：
[0110]
在计算当前的所述插补时间之前，根据上一所述插补周期，计算剩余的插补次数；
[0111]
根据所述平滑过渡时间和所述剩余的插补次数，计算误差时间；
[0112]
根据所述误差时间和上一所述插补周期，计算目标插补周期；
[0113]
基于当前检测的所述速度倍率值，更新所述目标插补周期。
[0114]
具体地，上述在线调速方法容易造成周期误差，根本原因是最终计算得到的插补时间tn不等于规划得到的过渡平滑时间smooth_time。为实现过渡段的高精度插补，在计算插补时间ti之前，根据更新前插补周期interp_time_i-1计算剩余的插补次数：
[0115][0116]
然后计算此时的误差时间：
[0117]
δt＝smooth_time-(t
i-1
+cnt
×
interp_time_i-1)
[0118]
然后将δt平均分配到每个插补周期中，更新之前计算插补周期interp_time_i-1：
[0119][0120]
最后将ti＝ti-1+interp_time，并带入到p(t)中计算当前插补时间ti对应的关节角jstates。以此类推，最终迭代计算得出的插补时间等于规划得到的过渡平滑时间smooth_time，且误差小于1e-5，实现过渡段的高精度插补。
[0121]
图3所示为在一种应用场景下机器人运行位移三维效果图，第一条轨迹为笛卡尔直线轨迹，第二条轨迹为关节空间轨迹，整体是按照100％速度倍率运行；图4为该速度倍率下各关节位移随时间变化曲线图；在相同运行指令下，图5为在一种应用场景下机器人各关节位移随时间变化曲线图，在运行开始时刻机器人接收到20％速度倍率指令，达到目标速度后并插补运行一段时间后检测得到50％速度倍率指令，插补运行一段时间后再次收到70％速度倍率指令，直至机器人运行停止。
[0122]
如图6所示，本发明第二实施例提出了一种混合空间过渡轨迹规划装置，所述装置包括：
[0123]
获取模块10，用于获取过渡轨迹规划的运动参数，所述运动参数包括第一段轨迹的起点位姿和终点位姿以及第二段轨迹的终点位姿和过渡参数百分比，所述第一段轨迹和所述第二段轨迹为笛卡尔空间的直线轨迹或关节空间的轨迹；
[0124]
预规划模块20，用于对所述第一段轨迹和所述第二段轨迹进行独立预规划，计算所述关节空间的轨迹的规划总时间和/或所述笛卡尔空间的直线轨迹的减速段时间；
[0125]
计算模块30，用于根据所述关节空间的轨迹的规划总时间和/或所述笛卡尔空间的直线轨迹的减速段时间以及所述过渡参数百分比，计算平滑过渡时间以及过渡点的边界条件；
[0126]
轨迹规划模块40，用于基于所述过渡点的边界条件和所述平滑过渡时间，在关节空间内构造样条曲线并进行求解，得到过渡曲线的轨迹方程。
[0127]
在一实施例中，所述预规划模块包括：
[0128]
第一预规划单元，用于对于所述关节空间的轨迹，根据逆运动学计算出该轨迹起点位姿对应的起点关节角以及终点位姿对应的终点关节角，并根据s型曲线加减速算法对该轨迹进行起点关节速度为0、终点关节速度为0的预规划，得到所述关节空间的轨迹的规划总时间；
[0129]
第二预规划单元，用于对于所述笛卡尔空间的直线轨迹，根据s型曲线加减速算法对该轨迹进行起点速度为0、终点速度为0的预规划，得到所述笛卡尔空间的直线轨迹的减速段时间。
[0130]
在一实施例中，所述计算模块，包括：
[0131]
平滑过渡时间计算单元，用于在所述第一段轨迹和所述第二段轨迹为不同空间的轨迹时，将属于所述关节空间的轨迹的规划总时间的一半与属于所述笛卡尔空间的直线轨迹的减速段时间进行比较，取两者中较小值与所述过渡参数百分比相乘，得到所述平滑过渡时间；
[0132]
以及在所述第一段轨迹和所述第二段轨迹均为所述关节空间的轨迹时，将两段轨迹的规划总时间中较小值的一半与所述过渡参数百分比相乘，得到所述平滑过渡时间；
[0133]
边界条件确定单元，用于执行如下步骤：
[0134]
对于为所述关节空间的轨迹，将所述平滑过渡时间代入该轨迹预规划后的时间插值函数，得到第一过渡点的关节角和关节速度；
[0135]
对于为所述笛卡尔空间的直线轨迹，将所述平滑过渡时间代入该直线轨迹预规划后的时间插值函数，得到与所述平滑过渡时间对应的位移，并根据所述位移和该轨迹的长度，更新所述过渡参数百分比；
[0136]
基于更新后的所述过渡参数百分比，计算该直线轨迹上的过渡点的位姿和对应的笛卡尔速度六分量；
[0137]
根据逆向运动学将所述直线轨迹上的过渡点的位姿进行反接，得到第二过渡点的关节角；
[0138]
根据雅克比矩阵将所述笛卡尔速度六分量映射为第二过渡点的关节速度，其中所述第一过渡点和所述第二过渡点中的一个作为过渡起点、另一个作为过渡终点。
[0139]
在一实施例中，所述装置还包括：
[0140]
插补模块，用于设定插补时间t1＝interp_time，其中，interp_time是系统给定的插补周期，并实时检测速度倍率值rate，并基于当前检测的所述速度倍率值更新上一所述插补周期，计算当前的所述插补时间，实现在线调速。
[0141]
在一实施例中，所述装置还包括更新模块，具体用于：
[0142]
在计算当前的所述插补时间之前，根据上一所述插补周期，计算剩余的插补次数；
[0143]
根据所述平滑过渡时间和所述剩余的插补次数，计算误差时间；
[0144]
根据所述误差时间和上一所述插补周期，计算目标插补周期；
[0145]
基于当前检测的所述速度倍率值，更新所述目标插补周期
[0146]
需要说明的是，本发明所述混合空间过渡轨迹规划装置的其他实施例或具有实现方法可参照上述各方法实施例，此处不再赘余。
[0147]
需要说明的是，在流程图中表示或在此以其他方式描述的逻辑和/或步骤，例如，可以被认为是用于实现逻辑功能的可执行指令的定序列表，可以具体实现在任何计算机可读介质中，以供指令执行系统、装置或设备(如基于计算机的系统、包括处理器的系统或其他可以从指令执行系统、装置或设备取指令并执行指令的系统)使用，或结合这些指令执行系统、装置或设备而使用。就本说明书而言，“计算机可读介质”可以是任何可以包含、存储、通信、传播或传输程序以供指令执行系统、装置或设备或结合这些指令执行系统、装置或设备而使用的装置。计算机可读介质的更具体的示例(非穷尽性列表)包括以下：具有一个或多个布线的电连接部(电子装置)，便携式计算机盘盒(磁装置)，随机存取存储器(ram)，只读存储器(rom)，可擦除可编辑只读存储器(eprom或闪速存储器)，光纤装置，以及便携式光盘只读存储器(cdrom)。另外，计算机可读介质甚至可以是可在其上打印所述程序的纸或其他合适的介质，因为可以例如通过对纸或其他介质进行光学扫描，接着进行编辑、解译或必要时以其他合适方式进行处理来以电子方式获得所述程序，然后将其存储在计算机存储器中。
[0148]
应当理解，本发明的各部分可以用硬件、软件、固件或它们的组合来实现。在上述实施方式中，多个步骤或方法可以用存储在存储器中且由合适的指令执行系统执行的软件或固件来实现。例如，如果用硬件来实现，和在另一实施方式中一样，可用本领域公知的下列技术中的任一项或他们的组合来实现：具有用于对数据信号实现逻辑功能的逻辑门电路的离散逻辑电路，具有合适的组合逻辑门电路的专用集成电路，可编程门阵列(pga)，现场可编程门阵列(fpga)等。
[0149]
在本说明书的描述中，参考术语“一个实施例”、“一些实施例”、“示例”、“具体示例”、或“一些示例”等的描述意指结合该实施例或示例描述的具体特征、结构、材料或者特点包含于本发明的至少一个实施例或示例中。在本说明书中，对上述术语的示意性表述不一定指的是相同的实施例或示例。而且，描述的具体特征、结构、材料或者特点可以在任何的一个或多个实施例或示例中以合适的方式结合。
[0150]
此外，术语“第一”、“第二”仅用于描述目的，而不能理解为指示或暗示相对重要性或者隐含指明所指示的技术特征的数量。由此，限定有“第一”、“第二”的特征可以明示或者隐含地包括至少一个该特征。在本发明的描述中，“多个”的含义是至少两个，例如两个，三个等，除非另有明确具体的限定。
[0151]
尽管上面已经示出和描述了本发明的实施例，可以理解的是，上述实施例是示例性的，不能理解为对本发明的限制，本领域的普通技术人员在本发明的范围内可以对上述实施例进行变化、修改、替换和变型。