CN113650011A - 一种机械臂拼接路径规划方法及装置 - Google Patents

Abstract

本申请公开了一种机械臂路径规划的方法及计算机设备，该方法包括：确定机械臂起始点参数和目标点参数，根据所述起始点参数和所述目标点参数基于A*算法从预设轨迹库中搜索出机械臂从起始点运动到目标点的多个第一路径，其中，第一路径是指从起始点运动到目标点过程中任意相邻两节点之间的路径；根据预设的拼接算法依次将所述第一路径进行有序拼接得到机械臂从起始点运动到目标点所对应的初始路径，验证所述初始路径是否满足碰撞检测；若不满足，则调整所述初始路径得到调整后的路径，将调整后的路径作为机械臂的路径，其中，调整后的路径满足碰撞检测。本申请解决了现有技术所规划的路径使得机械臂的运动效率较低的技术问题。

Description

一种机械臂拼接路径规划方法及装置

技术领域

本申请涉及机械臂路径规划技术领域，尤其涉及一种机械臂路径规划的方法及计算机设备。

背景技术

空间站的机械臂为七自由度冗余机械臂，操作灵活度高，可以通过灵活度优化实现各类复杂操作的同时，达到高效快速、减少能量损耗和减小基座扰动的目标。为了实现控制机械臂进行各种复杂的操作，需要对机械臂的路径进行规划。

对于机械臂的路径规划冗余特性的存在，也使得机械臂的规划与控制更为复杂。目前，对机械臂的路径规划方案，并未涉及末端在笛卡尔空间的运动映射到关节空间的方案，故末端在笛卡尔空间的运动无法映射到关节空间解析求解，而是需要在无穷空间中进行搜索求优，使得机械臂操作路径规划不仅需要考虑运动学层面的问题，还要结合动力学模型建立运动规划体系，分析不同类型任务和操作下机械臂的动力学特性，建立机械臂运动路径规划准则，提出运动学模型和动力学模型相结合的运动规划方法。此外，空间站机械臂执行复杂操作的运动规划一般无法通过一条路径到达目标点，需要从起始点经过多个中间点到达目标点。现有方法一般采取先减速到中间点，再从中间点加速的方式进行中间点前后两段路径拼接，降低了空间站机械臂执行复杂操作的效率。需要进一步考虑对中间点前后路径的拼接方式进行优化，以提高空间站机械臂的运动效率。

发明内容

本申请解决的技术问题是：针对现有技术所规划的路径使得机械臂的运动效率较低。本申请提供了一种机械臂路径规划的方法及计算机设备，本申请实施例所提供的方案中，根据A^＊算法从预设轨迹库中搜索出机械臂从起始点运动到目标点的多个第一路径,根据预设的拼接算法依次将第一路径进行有序拼接得到机械臂从起始点运动到目标点所对应的初始路径，验证所述初始路径是否满足碰撞检测，若不满足，则调整初始路径得到满足碰撞检测的调整后的路径，提供了一种对中间点前后路径的拼接方式进行优化，以提高机械臂的运动效率。

第一方面，本申请实施例提供一种机械臂路径规划的方法，该方法包括：

确定机械臂起始点参数和目标点参数，根据所述起始点参数和所述目标点参数基于A^＊算法从预设轨迹库中搜索出机械臂从起始点运动到目标点的多个第一路径，其中，第一路径是指从起始点运动到目标点过程中任意相邻两节点之间的路径；

根据预设的拼接算法依次将所述第一路径进行有序拼接得到机械臂从起始点运动到目标点所对应的初始路径，验证所述初始路径是否满足碰撞检测；

若不满足，则调整所述初始路径得到调整后的路径，将调整后的路径作为机械臂的路径，其中，调整后的路径满足碰撞检测。

本申请实施例所提供的方案中，根据A^＊算法从预设轨迹库中搜索出机械臂从起始点运动到目标点的多个第一路径,根据预设的拼接算法依次将第一路径进行有序拼接得到机械臂从起始点运动到目标点所对应的初始路径，验证所述初始路径是否满足碰撞检测，若不满足，则调整初始路径得到满足碰撞检测的调整后的路径，。

可选地，根据预设的拼接算法依次将所述第一路径进行有序拼接得到机械臂的初始路径，包括：

确定每个所述第一路径的起点和终点，根据所述起点和终点从多个第一路径中确定出任意相邻的第二路径和第三路径，其中，第二路径的终点和第三路径的起点相同；

分别确定规划所述第二路径和所述第三路径所对应的加速度和规划时长，根据所述预设的拼接算法、所述加速度和所述规划时长确定出拼接加速度和拼接时长；

根据所述拼接加速度和所述拼接时长将所述第二路径和所述第三路径进行拼接得到拼接后的路径，其中，规划后的路径的起点为第二路径的起点，终点为第三路径的终点；

通过预设的拼接算法将所述拼接后的路径与下一相邻的第一路径进行拼接，直到将多个第一路径拼接到一起得到所述初始路径，其中，下一相邻的第一路径的起点与所述拼接后的路径的终点相同。

可选地，根据所述预设的拼接算法、所述加速度和所述规划时长确定出拼接加速度和拼接时长，包括：

计算所述第二路径和所述第三路径所对应的加速度的最大值，将所述加速度的最大值作为所述拼接加速度；

根据所述第二路径所对应的加速度和规划时间计算所述第二路径所对应的第一速度，根据所述第三路径所对应的加速度和规划时间计算所述第三路径所对应的第二速度；

计算第一速度和所述第二速度之差的绝对值，根据所述绝对值和所述拼接加速度得到所述拼接时长。

可选地，所述第二路径和所述第三路径为空间关节路径规划方法、笛卡尔路径规划方法或曲线路径规划方法所规划得到的路径。

可选地，若所述第二路径和所述第三路径为空间关节路径规划方法所规划得到的路径，根据所述拼接加速度和所述拼接时长对所述第二路径和所述第三路径进行拼接得到拼接后的路径，包括：

通过如下公式得到所述规划后的路径：

其中，θ(t)表示规划后的路径；θ₀表示第二路径的初始状态；

表示第二路径的加速度；t_c1表示第二路径的加速段的时长；t_c2表示第二路径的匀速段的时长；t_c3表示第二路径的减速段的时长；t_f1表示第二路径的时长，t_f1＝t_c1+t_c2+t_c3；

表示所述拼接加速度；t_b表示所述拼接时长；

表示第三路径的加速度；t_c4表示第三路径的加速段的时长；t_c5表示第三路径的匀速段的时长；t_c6表示第三路径的减速段的时长；t_f2表示第三路径的时长，t_f2＝t_c4+t_c5+t_c6；θ_f2表示第三路径的目标状态；t_Δ表示第二路径和第三路径的总时长与所述规划后的路径的总时长的差值。

可选地，验证所述初始路径是否满足碰撞检测，包括：

构建障碍物模型以及机械臂沿所述初始路径运动的外包络模型，判断所述外包络模型与所述障碍物模型是否满足预设条件；

若满足，则确定所述初始路径满足碰撞检测；否则，确定所述初始路径不满足碰撞检测。

可选地，预设条件，包括：

所述外包络空间和所述障碍物模型之间不存在交集；和/或

所述外包络空间和所述障碍物模型之间的最短距离大于预设阈值。

可选地，调整所述初始路径得到调整后的路径，包括：

确定所述外包络空间和所述障碍物模型之间存在交集或最短距离小于预设阈值的至少一个点，确定每个点所属的第一路径，以及该第一路径在所述初始路径上前后两段所属的第一路径；

分别将该第一路径的起点与其前一段所属的第一路径的终端连接，以及将该第一路径的终点与其后一段所属的第一路径的起点连接，得到调整后的路径。

第二方面，本申请提供一种计算机设备，该计算机设备，包括：

存储器，用于存储至少一个处理器所执行的指令；

处理器，用于执行存储器中存储的指令执行第一方面所述的方法。

第三方面，本申请提供一种计算机可读存储介质，所述计算机可读存储介质存储有计算机指令，当所述计算机指令在计算机上运行时，使得计算机执行第一方面所述的方法。

附图说明

图1为本申请实施例所提供的一种机械臂路径规划的方法的流程示意图；

图2A为本申请实施例提供的一种梯形速度插值得到的关节路径示意图；

图2B为本申请实施例提供的又一种梯形速度插值得到的关节路径示意图；

图3A为本申请实施例提供的一种的关节路径的速度示意图；

图3B为本申请实施例提供的又一种关节路径的速度示意图；

图4A为本申请实施例提供的一种的关节路径的加速度示意图；

图4B为本申请实施例提供的又一种关节路径的加速度示意图；

图5为本申请实施例所提供的一种机械臂的关节空间规划的流程示意图；

图6为本申请实施例所提供的一种机械臂直线路径规划运动的示意图；

图7为本申请实施例所提供的一种机械臂的笛卡尔空间直线路径规划的流程示意图；

图8为本申请实施例提供的一种最小范数解示意图；

图9A表示本申请实施例所提供的一种根据预设拼接算法将第一条轨迹和第二条轨迹进行规划得到规划后的路径的示意图；

图9B表示本申请实施例所提供的一种规划后的路径的速度示意图；

图9C表示本申请实施例所提供的一种规划后的路径的加速度示意图；

图10A表示本申请实施例所提供的一种直接将第一条轨迹和第二条轨迹有序连接所得到的拼接后的路径示意图；

图10B表示本申请实施例所提供的一种预设算法将第一条轨迹和第二条轨迹有序连接所得到的拼接后的路径示意图；

图10C表示本申请实施例所提供的一种两种拼接路径的对比示意图；

图10D表示本申请实施例所提供的一种两种拼接路径的对比示意图；

图11为本申请实施例所提供的一种计算机设备的结构示意图。

具体实施方式

本申请实施例提供的方案中，所描述的实施例仅是本申请一部分实施例，而不是全部的实施例。基于本申请中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其它实施例，都属于本申请保护的范围。

为了更好的理解上述技术方案，下面通过附图以及具体实施例对本申请技术方案做详细的说明，应当理解本申请实施例以及实施例中的具体特征是对本申请技术方案的详细的说明，而不是对本申请技术方案的限定，在不冲突的情况下，本申请实施例以及实施例中的技术特征可以相互组合。

以下结合说明书附图对本申请实施例所提供的一种机械臂路径规划的方法做进一步详细的说明，该方法具体实现方式可以包括以下步骤(方法流程如图1所示)：

步骤101，确定机械臂起始点参数和目标点参数，根据所述起始点参数和所述目标点参数基于A^＊算法从预设轨迹库中搜索出机械臂从起始点运动到目标点的多个第一路径，其中，第一路径是指从起始点运动到目标点过程中任意相邻两节点之间的路径。

具体的，在本申请实施例所提供的方案中，在对机械臂进行路径规划时，若输入的规划任务为复杂任务，其中，复杂任务是机械臂不能直接执行完成的任务，将复杂任务的描述为：起始节点，中间节点1、中间节点2、中间节点3、…、中间节点n以及目标节点，其中，n为不小于1的正整数。

进一步，根据上述复杂任务的规划时，需要对任意两个点之间的路径进行规划，即规划的路径包括n+1个路径，分别为：路径1是起始节点到中间节点1；路径2是中间节点1到中间节点2；路径3是中间节点2到中间节点3；…；路径n是中间节点n-1到中间节点n；路径n+1是中间节点n到目标节点。

具体的，在本申请实施例所提供的方案中，路径规划是指机械臂基本运动的规划，使机械臂关节或末端沿预定路径运动，运动过程中不发生与周围环境的碰撞和自身碰撞的问题。主要包括关节空间路径规划(规划机械臂关节运动)和笛卡尔空间路径规划(规划机械臂末端运动，逆解得到关节运动)等，其中，路径终点以机械臂关节角约定的任务使用关节空间路径规划实现；路径终点以末端位姿约定的任务使用笛卡尔空间直线路径规划实现；对于工具操作及末端适配器对接等小范围精细操作任务使用笛卡尔空间直线路径规划实现，进而通过上述路径规划方式得到路径1、路径2、路径3…路径n+1。为了便于理解下面分别对上述路径规划几种方式进行简要介绍。

一、关节空间规划

关节空间轨迹规划方法是以关节角度的函数来描述空间机械臂的路径，进行路径规划。关节空间规划不必在直角坐标系中描述两个路径点之间的路径形状，计算简单容易。关节空间路径规划实现方法是，给定空间机械臂的起始节点位形和终止点位形，对关节进行插值，从而得到关节路径。

具体的，在关节空间中进行轨迹规划，需要给定空间机械臂的起始节点位形和终止点位形，对关节进行插值，得到关节路径。常用的插值方法有梯形速度插值、和多项式插值等。梯形速度插值函数将关节运动分为加速、匀速和减速三个阶段进行描述，该类方法原理较为直观，应用较为广泛；多项式插值函数可根据机械臂关节的约束条件来确定其运行轨迹。

例如，若规划路径起始节点的关节角θ₀，规划路径目标节点的关节角θ_f以及轨迹所对应的时间t_f，可采用如下梯形速度插值：

其中，θ(t)表示规划的路径；

表示路径加速段和减速段的加速度；t_c1表示路径的加速段的时长；t_c2表示路径的匀速段的时长；t_f表示路径的时长。

参见图2A为本申请实施例提供的一种梯形速度插值得到的关节路径示意图；参见图2B为本申请实施例提供的又一种梯形速度插值得到的关节路径示意图；参见图3A为本申请实施例提供的一种的关节路径的速度示意图；参见图3B为本申请实施例提供的又一种关节路径的速度示意图；参见图4A为本申请实施例提供的一种的关节路径的加速度示意图；参见图4B为本申请实施例提供的又一种关节路径的加速度示意图；其中，图3A和图4A为图2A所示的关节路径所对应的速度和加速度；图3B和图4B为图2B所示的关节路径所对应的速度和加速度。

参见图5，为本申请实施例提供的一种机械臂的关节空间规划的流程示意图。具体的，空间关节规划的具体步骤如下所示：

1、首先根据当前机械臂的构型，得到机械臂的初始关节角q_ini＝[θ₁ θ₂ … θ_n]_ini，并根据任务需要，确定期望关节角q_des＝[θ₁ θ₂ … θ_n]_des；

2.计算总的运动角度dist＝||q_des-q_int||，选取速度梯形规划方式，根据任务中对关节最大速度和最大加速度的限制要求，计算轨迹规划总时间t_f和加速段时间t_c1，同时设定时间步长t₀，计算总规划步数n＝t_f/t₀；

3.计算每个插补步的步长：以k(k＝0～n)表示插补次数，以t_k＝k·t₀表示第k次插补的时间，则t_k时刻的角位移θ_k可按梯形速度插值表示的角位移函数计算给出；

4.判断当前规划步数是否达到总数，若达到，则规划结束；否则，返回步骤3循环进行求解，直至规划任务完成。

二、笛卡尔空间路径规划

笛卡尔空间路径规划是指在给定机械臂初始构型和目标末端位姿的基础上，通过规划机械臂末端的运动实现机械臂从起始节点运动至到目标节点的过程。完成笛卡尔空间的路径规划并没有完成整个路径规划的工作，最终还要转换到关节空间的路径规划。笛卡尔空间路径规划实现方法是：首先用数学方法在笛卡尔空间中描述期望路径，计算出末端各插值点的位姿信息，然后通过逆运动学求解得到关节的相应参数，根据这些点的坐标由关节空间的路径规划建立相应的运动方程，然后进行插值，从而得到目标轨迹。

具体的，假设规划路径起始节点的位姿X_e0＝[P_e0，ψ_e0]及规划路径目标节点的位姿X_ef＝[P_ef，ψ_ef]。在路径规划过程中，需要机械臂的末端执行器从起始节点X_e0到终点X_ef的直线运动，其运动轨迹参见图6所示。

参见图7，为本申请实施例提供的一种直线路径的流程示意图。具体的，直线路径规划化步骤如下所示：

1.已知机械臂末端相对于惯性坐标系的初始位姿X_e0和期望位姿X_ef，由此获得起点和终点的位置向量P_e0和P_ef与姿态矩阵R_e0和R_ef；

2.由P_e0和P_ef计算运动位移d_f＝||P_e0-P_ef||，利用

得出由起点指向终点的直线的方向余弦；

3.计算终点相对起点的姿态变化矩阵Rot，由姿态变化矩阵Rot反算对应的ZYX欧拉角的变化α_n、β_n和γ_n，即为末端在运动过程中姿态欧拉角的变化量；

4.根据任务中对末端速度的限制要求，计算轨迹规划总时间t_f和加速段时间t_s，同时设定时间步长t₀，计算总规划步数n＝t_f/t₀；

5.计算每个插补步的步长：以k(k＝0～n)表示插补次数，以t_k＝k·t₀表示第k次插补的时间，则t_k时刻的位移s_k可按速度梯形法中的位移函数计算给出，同时可得到插补步长Δs_k；

6.求出每步的位置微分变量d_x,d_y,d_z；

7.求出第k步的姿态矩阵；

8.由dR_k＝R_k+1-R_k得出第k步的姿态微分算子

又算子

即可得到姿态微分变量δ_x,δ_y,δ_z；

9.D＝[d_x,d_y,d_z,δ_x,δ_y,δ_z]^T即为每个插补步内的微分运动矢量，由

即可得该插补步内物体的运动速度矢量；

10.在完成末端第k步速度和角速度的规划基础上，基于速度级的逆运动学方程进行求解得到各关节第k步的转动角速度；

11.求得各关节的转动角速度后，再通过对各关节转动角速度求导和积分运算分别求得各关节第k步的转动角加速度和转动角位移，完成第k步的轨迹规划；

12.判定当前规划步数是否达到总数，是，则规划结束；否，则返回第5步循环进行求解，直至规划任务完成。

进一步，为了便于理解上述第10步所涉及的基于速度级的逆运动学方程进行求解得到各关节第k步的转动角速度的过程，下面对机械臂中逆运动学过程进行简要介绍。

具体的，机械臂逆运动学指是机械臂末端执行器操作映射到关节的过程，其目的是将工作内机械臂末端位姿转化成相对应的机械臂关节变量。通过运动学逆解可以实现对机械臂在末端操作的位姿控制，在机械臂的运动分析、离线编程、轨迹控制中都有重要的应用。

对机械臂而言，运动学逆解模型需根据末端速度与关节角速度之间的映射关系而建立。以下针对机械臂在完全受控模式下的运动学逆解进行分析，具体过程如下：

由运动学方程可知，机械臂末端的运动可通过机械臂基座的运动和关节的运动进行描述，即：

采用矩阵变换方式描述可得机械臂末端与基座、关节角的映射关系为：

对于七自由度大型机械臂***，由于航天器基座和机器人本体之间存在运动耦合，在机器人运动时，航天器基座的位姿会随七自由度大型机械臂的运动而运动。根据七自由度大型机械臂基座是否受控，可将七自由度大型机械臂的控制模式分为固定基座、自由漂浮(基座不受控)和自由飞行(基座受控)三种工作模式。对于不同工作模式的七自由度大型机械臂来说，上面的约束关系是一样的，即对于七自由度大型机械臂来说，均存在末端速度、关节角与基座速度的映射关系。

一般情况下，由于其位置、姿态均受控，即v₀、ω₀均为0，则此模式下的运动微分方程可表示为：

其中J_m是关节运动映射到末端笛卡尔运动的变换矩阵。

和

分别表示笛卡尔和关节的运动，采用位姿和角度的一阶导数描述。对于七自由度大型机械臂，

表示末端速度

表示关节速度

则可建立以下系：

式中J_m为地面机械臂的普通雅克比矩阵，由上式即可得出固定基座模式下，七自由度大型机械臂速度级的运动学逆解：

对于冗余度机械臂而言，上式实际为一种特解，也称为最小二范数解，其所解出的关节角速度组成的向量，范数最小，代表了整体运动量最小的情况。当需要优化其他性能指标时，才采用梯度投影法进行逆运动学求解。

最小二范数法需要计算Jacobian矩阵的广义逆，采用下面的方法可以回避广义逆的计算，能够极大地提高解算的实时性。

上式的解由特解和齐次解组成，可表示为

式中，r为任意常数，

为任意特解，

为齐次解。满足：

根据上式，可得关节角速度的范数的平方为

为求

的最小值，可令上式的右边对r的偏微分为0，即

根据上式，可解出最小范数解对应的常数r为

将上式代入最小二范数解得

令与向量

同方向的向量为u_k，即

则，

根据点乘的几何意义，u_k表示向量

投影到向量

上的长度，因此，上式表明最小范数解即为特解

减去其投影到

向量方向上的部分所得到的向量。参见图8，为本申请实施例提供的一种最小范数解示意图。

进一步，在本申请实施例所提供的方案中，可以将任一两节点之间的路径在规划完成后，将其存储到预设轨迹库中，当对机械臂的路径进行规划时，可从预设轨迹库中选择出起始节点到目标节点的多段路径，并将多段路径进行有序连接得到起始节点到目标节点的路径。

作为举例，可以通过A^＊算法从预设轨迹库中选择出从起始节点到目标节点的多段路径。

为了便于理解，下面对A^*算法原理进行简要介绍。

A^＊算法是典型的启发式搜索方法，被广泛应用于最优路径的求解，其核心在于估价函数的设计。在选择当前节点的下一个评估节点时引入估价函数f(x)

f(x)＝g(x)+h(x)

式中，g(x)是从起始点到节点x最小代价路径的实际代价；h(x)是从节点x到目标点路径的估计代价。

为便于利用A^＊算法，定义空间机械臂的7个关节角度为一个七维数组，起始关节角度为

当前关节角度为

第i步最佳关节角度为

关节角度搜索步长为

第i步的后继节点关节角度为

(由关节角度搜索步长确定)，期望关节角度为

定义估价函数

其中，q_im为第i步机械臂关节角度中的第m个关节的关节角度，q_{des m}为机械臂期望关节角度中的第m个关节的关节角度。由此可以求出第i步后继节点关节角度中f_i(q)值最小的节点作为此步最优关节角度，根据步长生成第i+1步的后继节点关节角度，重复此过程可保证每一步都可以得到最优关节角度，因此通过该估价函数可以实现空间机械臂从起始关节角度到目标关节角度的构型空间最优路径规划。

为了便于分析，设定3个表，即OPEN表、CLOSE表和后继节点表。把起始点q_ini放入OPEN表，初始化f₀(q)＝h₀(q)，置CLOSE表为空表。

重复下列过程，直到找到目标点为止。若OPEN表为空表，表明空间机械臂路径规划失败；否则，开始以下循环。

Step1选取OPEN表中未设置过的具有最小f值的节点为最佳节点q_{best_i}，并将其放入CLOSE表，同时将其从OPEN表中删除；

Step2判断q_{best_i}是否为目标点q_des，若是，则求解成功，循环结束；否则进行下一步；

Step3根据设定的步长l_step值，计算后继节点q_{suc_i}；

Step4按下述流程对每个后继节点q_{suc_i}进行处理：

a.判断q_{suc_i}是否在空间机械臂***空间内，若是，建立从q_{suc_i}返回q_{best_i}的指针；否则转至Step3；

b.计算

c.判断q_{suc_i}是否在OPEN表中，若是转至d；否则转至e；

d.定义数组q_{old_i}＝q_{suc_i}，将q_{old_i}填到q_{best_i}的后继节点表中，进而比较新旧轨迹代价：若g(q_{old_i})>g(q_{suc_i})，更新q_{old_i}的父辈节点为q_{best_i}，并更新g(q_{old_i})＝g(q_{suc_i})，进一步修正f(q_{old_i})值，同时对OPEN表重新排序；否则转至g；

e.判断q_{suc_i}是否在CLOSE表中，若是，转至d；否则转至f；

f.将q_{suc_i}放入OPEN表，并添入到q_{best_i}的后继表中，转至g；

g.计算f(q_{suc_i})，并转至a。

步骤102，根据预设的拼接算法依次将所述第一路径进行有序拼接得到机械臂从起始点运动到目标点所对应的初始路径，验证所述初始路径是否满足碰撞检测。

在一种可能实现的方式中，根据预设的拼接算法依次将所述第一路径进行有序拼接得到机械臂的初始路径，包括：确定每个所述第一路径的起点和终点，根据所述起点和终点从多个第一路径中确定出任意相邻的第二路径和第三路径，其中，第二路径的终点和第三路径的起点相同；分别确定规划所述第二路径和所述第三路径所对应的加速度和规划时长，根据所述预设的拼接算法、所述加速度和所述规划时长确定出拼接加速度和拼接时长；根据所述拼接加速度和所述拼接时长将所述第二路径和所述第三路径进行拼接得到拼接后的路径，其中，规划后的路径的起点为第二路径的起点，终点为第三路径的终点；通过预设的拼接算法将所述拼接后的路径与下一相邻的第一路径进行拼接，直到将多个第一路径拼接到一起得到所述初始路径，其中，下一相邻的第一路径的起点与所述拼接后的路径的终点相同。

进一步，在一种可能实现的方式中，根据所述预设的拼接算法、所述加速度和所述规划时长确定出拼接加速度和拼接时长，包括：计算所述第二路径和所述第三路径所对应的加速度的最大值，将所述加速度的最大值作为所述拼接加速度；根据所述第二路径所对应的加速度和规划时间计算所述第二路径所对应的第一速度，根据所述第三路径所对应的加速度和规划时间计算所述第三路径所对应的第二速度；计算第一速度和所述第二速度之差的绝对值，根据所述绝对值和所述拼接加速度得到所述拼接时长。

进一步，在一种可能实现的方式中，所述第二路径和所述第三路径为空间关节路径规划方法、笛卡尔路径规划方法或曲线路径规划方法所规划得到的路径。

为了便于理解，下面以关节空间规划的路径为例简要介绍其相邻路径之间拼接过程。

作为举例，在采用预设拼接算法对关节空间规划的路径进行拼接时，以拼接加速度

拼接第一条轨迹和第二条轨迹，其中，第一条轨迹和第二条轨迹均是由关节空间规划所得到的，

分别表示第一条轨迹加速段和减速段所对应的加速度和第二条轨迹加速段和减速段所对应的加速度。具体的，第一条轨迹和第二条轨迹分别通过下式表示：

其中，θ₁(t)和θ₂(t)分别表示第一条轨迹和第二条轨迹；t_c1、t_c4分别表示第一条轨迹和第二条轨迹加速段所对应的时长；t_c2、t_c5分别表示第一条轨迹和第二条轨迹匀速段所对应的时长；t_f1、t_f2分别表示第一条轨迹和第二条轨迹所对应的时长。

进一步，将第一轨迹和第二轨迹进行拼接得到如下公式得到所示的规划后的路径：

其中，θ(t)表示规划后的路径；θ₀表示第二路径的初始状态；

表示第二路径的加速度；t_c1表示第二路径的加速段的时长；t_c2表示第二路径的匀速段的时长；t_c3表示第二路径的减速段的时长；t_f1表示第二路径的时长，t_f1＝t_c1+t_c2+t_c3；

表示所述拼接加速度；t_b表示所述拼接时长；

表示第三路径的加速度；t_c4表示第三路径的加速段的时长；t_c5表示第三路径的匀速段的时长；t_c6表示第三路径的减速段的时长；t_f2表示第三路径的时长，t_f2＝t_c4+t_c5+t_c6；θ_f2表示第三路径的目标状态；t_Δ表示第二路径和第三路径的总时长与所述规划后的路径的总时长的差值。

具体的，t_b表示拼接时间，

表示拼接加速度

t_Δ表示拼接轨迹于原始轨迹的时间差。由于原始路径和拼接路径到达的终点相同，则可以得到拼接轨迹优于原始轨迹，且t_Δ的时间范围为

根据预设拼接算法将第一条轨迹和第二条轨迹进行规划得到规划后的路径相关信息参见图9A、图9B和图9C所示，其中，图9A表示本申请实施例所提供的一种根据预设拼接算法将第一条轨迹和第二条轨迹进行规划得到规划后的路径的示意图；图9B表示本申请实施例所提供的一种规划后的路径的速度示意图；图9C表示本申请实施例所提供的一种规划后的路径的加速度示意图。

进一步，在从起始点到目标点的所有段路径拼接到一起得到初始路径后，还需要验证初始路径是否会与预设的障碍物发生碰撞。在本申请实施例所提供的方案中，验证所述初始路径是否满足碰撞检测的方式有多种，下面以其中一种为例进行说明。

在一种可能实现的方式中，验证所述初始路径是否满足碰撞检测，包括：

构建障碍物模型以及机械臂沿所述初始路径运动的外包络模型，判断所述外包络模型与所述障碍物模型是否满足预设条件；

若满足，则确定所述初始路径满足碰撞检测；否则，确定所述初始路径不满足碰撞检测。

进一步，在一种可能实现的方式中，预设条件，包括：所述外包络空间和所述障碍物模型之间不存在交集；和/或所述外包络空间和所述障碍物模型之间的最短距离大于预设阈值。

具体的，定义χ_obs(t)为t时刻的障碍空间；χ_arm(θ(t))为沿拼接轨迹运动过程中t时刻的机械臂外包络空间。机械臂沿拼接轨迹运动过程中，需满足下面两个等价条件之一：

(1)机械臂与障碍空间不发生碰撞，即

(2)障碍空间χ_obs(t)和机械臂外包络空间χ_arm(θ(t))的最短距离大于0，即dis(χ_arm(θ(t))，χ_obs(t))>0。

步骤103，若初始路径是不满足碰撞检测，则调整所述初始路径得到调整后的路径，将调整后的路径作为机械臂的路径，其中，调整后的路径满足碰撞检测。

在一种可能实现的方式中，调整所述初始路径得到调整后的路径，包括：确定所述外包络空间和所述障碍物模型之间存在交集或最短距离小于预设阈值的至少一个点，确定每个点所属的第一路径，以及该第一路径在所述初始路径上前后两段所属的第一路径；分别将该第一路径的起点与其前一段所属的第一路径的终端连接，以及将该第一路径的终点与其后一段所属的第一路径的起点连接，得到调整后的路径。

进一步，若初始路径满足碰撞检测，则过程结束。

为了便于理解本申请实施例所提供的方案的技术效果，下面以举例的形式进行说明。参见图10A和图10B所示，图10A表示直接将第一条轨迹和第二条轨迹有序连接所得到的拼接后的路径，其中，S₁₁表示机械臂沿第一条轨迹运动的距离；S₁₂表示机械臂沿第二条轨迹运动的距离；机械臂沿将第一条轨迹和第二条轨迹有序连接所得到的拼接后的路径运动的距离(图10A中阴影部分的面积)为S₁＝S₁₁+S₁₂。

图10B表示根据本申请实施例所提供的预设算法将第一条轨迹和第二条轨迹有序连接所得到的拼接后的路径，其中，机械臂沿拼接后的路径运动的距离(图10B中阴影部分的面积)为S₂＝S₂₁+S₂₂+S₂₃。

由于拼接前后，机械臂运动的距离相同，必须有S1＝S2。由于已知S11＝S21，则只需满足S12＝S22+S23(1)。此时，如下图10C所示，对比拼接前后的面积，有S12＝S23+S14(2)。对比上述公式(1)和公式(2)，则如果满足条件S22＝S14，则拼接前后的机械臂运动距离相同。

进一步，如图10D所示，S₂₂＝1/2*h(t_c1-t_b+t_c2)，S₁₄＝h*t_Δ，则有，t_Δ表示拼接轨迹于原始轨迹的时间差，t_Δ＝1/2*(t_c1-t_b+t_c2)。

当

有

则t_Δ取最大值，即t_Δ＝1/2(t_c1-t_b+t_c2)＝1/2(t_c1+t_c2)。

当

有

则t_Δ取最小值，即t_Δ＝1/2(t_c1-t_b+t_c2)＝1/2[t_c1-(t_c1+t_c2)+t_c2]＝0。

综上，由于原始路径和拼接路径到达的终点相同，则可以得到拼接轨迹优于原始轨迹，且t_Δ的时间范围为

本申请实施例所提供的方案中，根据A^＊算法从预设轨迹库中搜索出机械臂从起始点运动到目标点的多个第一路径,根据预设的拼接算法依次将第一路径进行有序拼接得到机械臂从起始点运动到目标点所对应的初始路径，验证所述初始路径是否满足碰撞检测，若不满足，则调整初始路径得到满足碰撞检测的调整后的路径，提供了一种对中间点前后路径的拼接方式进行优化，以提高机械臂的运动效率。

参见图11，本申请提供一种计算机设备，该计算机设备，包括：

存储器1101，用于存储至少一个处理器所执行的指令；

处理器1102，用于执行存储器中存储的指令执行图1所述的方法。

本申请提供一种计算机可读存储介质，所述计算机可读存储介质存储有计算机指令，当所述计算机指令在计算机上运行时，使得计算机执行图1所述的方法。

本领域内的技术人员应明白，本申请的实施例可提供为方法、***、或计算机程序产品。因此，本申请可采用完全硬件实施例、完全软件实施例、或结合软件和硬件方面的实施例的形式。而且，本申请可采用在一个或多个其中包含有计算机可用程序代码的计算机可用存储介质(包括但不限于磁盘存储器和光学存储器等)上实施的计算机程序产品的形式。

本申请是参照根据本申请实施例的方法、设备(***)、和计算机程序产品的流程图和/或方框图来描述的。应理解可由计算机程序指令实现流程图和/或方框图中的每一流程和/或方框、以及流程图和/或方框图中的流程和/或方框的结合。可提供这些计算机程序指令到通用计算机、专用计算机、嵌入式处理机或其他可编程数据处理设备的处理器以产生一个机器，使得通过计算机或其他可编程数据处理设备的处理器执行的指令产生用于实现在流程图一个流程或多个流程和/或方框图一个方框或多个方框中指定的功能的装置。

这些计算机程序指令也可存储在能引导计算机或其他可编程数据处理设备以特定方式工作的计算机可读存储器中，使得存储在该计算机可读存储器中的指令产生包括指令装置的制造品，该指令装置实现在流程图一个流程或多个流程和/或方框图一个方框或多个方框中指定的功能。

这些计算机程序指令也可装载到计算机或其他可编程数据处理设备上，使得在计算机或其他可编程设备上执行一系列操作步骤以产生计算机实现的处理，从而在计算机或其他可编程设备上执行的指令提供用于实现在流程图一个流程或多个流程和/或方框图一个方框或多个方框中指定的功能的步骤。

显然，本领域的技术人员可以对本申请进行各种改动和变型而不脱离本申请的精神和范围。这样，倘若本申请的这些修改和变型属于本申请权利要求及其等同技术的范围之内，则本申请也意图包含这些改动和变型在内。

Claims (10)

1.一种机械臂路径规划的方法，其特征在于，包括：

确定机械臂起始点参数和目标点参数，根据所述起始点参数和所述目标点参数基于A*算法从预设轨迹库中搜索出机械臂从起始点运动到目标点的多个第一路径，其中，第一路径是指从起始点运动到目标点过程中任意相邻两节点之间的路径；

根据预设的拼接算法依次将所述第一路径进行有序拼接得到机械臂从起始点运动到目标点所对应的初始路径，验证所述初始路径是否满足碰撞检测；

若不满足，则调整所述初始路径得到调整后的路径，将调整后的路径作为机械臂的路径，其中，调整后的路径满足碰撞检测。

2.如权利要求1所述的方法，其特征在于，根据预设的拼接算法依次将所述第一路径进行有序拼接得到机械臂的初始路径，包括：

确定每个所述第一路径的起点和终点，根据所述起点和终点从多个第一路径中确定出任意相邻的第二路径和第三路径，其中，第二路径的终点和第三路径的起点相同；

分别确定规划所述第二路径和所述第三路径所对应的加速度和规划时长，根据所述预设的拼接算法、所述加速度和所述规划时长确定出拼接加速度和拼接时长；

根据所述拼接加速度和所述拼接时长将所述第二路径和所述第三路径进行拼接得到拼接后的路径，其中，规划后的路径的起点为第二路径的起点，终点为第三路径的终点；

通过预设的拼接算法将所述拼接后的路径与下一相邻的第一路径进行拼接，直到将多个第一路径拼接到一起得到所述初始路径，其中，下一相邻的第一路径的起点与所述拼接后的路径的终点相同。

3.如权利要求2所述的方法，其特征在于，根据所述预设的拼接算法、所述加速度和所述规划时长确定出拼接加速度和拼接时长，包括：

计算所述第二路径和所述第三路径所对应的加速度的最大值，将所述加速度的最大值作为所述拼接加速度；

根据所述第二路径所对应的加速度和规划时间计算所述第二路径所对应的第一速度，根据所述第三路径所对应的加速度和规划时间计算所述第三路径所对应的第二速度；

计算第一速度和所述第二速度之差的绝对值，根据所述绝对值和所述拼接加速度得到所述拼接时长。

4.如权利要求3所述的方法，其特征在于，所述第二路径和所述第三路径为空间关节路径规划方法、笛卡尔路径规划方法或曲线路径规划方法所规划得到的路径。

5.如权利要求4所述的方法，其特征在于，若所述第二路径和所述第三路径为空间关节路径规划方法所规划得到的路径，根据所述拼接加速度和所述拼接时长对所述第二路径和所述第三路径进行拼接得到拼接后的路径，包括：

通过如下公式得到所述规划后的路径：

其中，θ(t)表示规划后的路径；θ₀表示第二路径的初始状态；

表示第二路径的加速度；t_c1表示第二路径的加速段的时长；t_c2表示第二路径的匀速段的时长；t_c3表示第二路径的减速段的时长；t_f1表示第二路径的时长，t_f1＝t_c1+t_c2+t_c3；

表示所述拼接加速度；t_b表示所述拼接时长；

表示第三路径的加速度；t_c4表示第三路径的加速段的时长；t_c5表示第三路径的匀速段的时长；t_c6表示第三路径的减速段的时长；t_f2表示第三路径的时长，t_f2＝t_c4+t_c5+t_c6；θ_f2表示第三路径的目标状态；t_Δ表示第二路径和第三路径的总时长与所述规划后的路径的总时长的差值。

6.如权利要求1-5任一项所述的方法，其特征在于，验证所述初始路径是否满足碰撞检测，包括：

构建障碍物模型以及机械臂沿所述初始路径运动的外包络模型，判断所述外包络模型与所述障碍物模型是否满足预设条件；

若满足，则确定所述初始路径满足碰撞检测；否则，确定所述初始路径不满足碰撞检测。

7.如权利要求6所述的方法，其特征在于，预设条件，包括：

所述外包络空间和所述障碍物模型之间不存在交集；和/或

所述外包络空间和所述障碍物模型之间的最短距离大于预设阈值。

8.如权利要求7所述的方法，其特征在于，调整所述初始路径得到调整后的路径，包括：

确定所述外包络空间和所述障碍物模型之间存在交集或最短距离小于预设阈值的至少一个点，确定每个点所属的第一路径，以及该第一路径在所述初始路径上前后两段所属的第一路径；

分别将该第一路径的起点与其前一段所属的第一路径的终端连接，以及将该第一路径的终点与其后一段所属的第一路径的起点连接，得到调整后的路径。

9.一种计算机设备，该计算机设备，包括：

存储器，用于存储至少一个处理器所执行的指令；

处理器，用于执行存储器中存储的指令执行如权利要求1-8任一项所述的方法。

10.一种计算机可读存储介质，其特征在于，所述计算机可读存储介质存储有计算机指令，当所述计算机指令在计算机上运行时，使得计算机执行如权利要求1-8任一项所述的方法。

Images