CN117754566A - 机械臂碰撞检测方法、装置、计算机设备和存储介质 - Google Patents

Abstract

本申请涉及一种机械臂碰撞检测方法、装置、计算机设备和存储介质。该方法包括：处理器获取机器人各机械臂的初始摆位以及属性信息，根据各机械臂的初始摆位与属性信息，确定各机械臂的初始摆位对应的构型代价函数，据初始摆位的构型代价函数，求得满足预设条件的构型代价函数所对应的各机械臂初始摆位为各机械臂的目标初始摆位。其中，各机械臂的初始摆位均满足各机械臂的创口约束条件。在本方案中，处理器得到符合优化目标以及约束条件的目标初始摆位，基于目标初始摆位，后续机械臂运动规划时，各机械臂拥有更大的运动规划空间，减少了各机械臂之间相互碰撞的概率，有效地避免各机械臂在手术过程中发生碰撞的情况。

Description

机械臂碰撞检测方法、装置、计算机设备和存储介质

本申请的申请日为“2021年02月04日”，申请号为“202110166245.1”，发明名称为“机械臂摆位优化方法、装置、计算机设备和存储介质”的案件的分案申请。

技术领域

本申请涉及医疗设备技术领域，特别是涉及一种机械臂碰撞检测方法、装置、计算机设备和存储介质。

背景技术

随着科学技术的发展，智能机器人、多机械臂型医疗手术机器人被广泛应用至医疗技术领域。例如，将多机械臂型医疗手术机器人应用在多种类型的手术场景下。在手术过程中，为保证手术正常进行，应尽量规避多机械臂碰撞的问题。

现有技术中，一般通过干涉检测实现多机械臂的碰撞规避，即，在机械臂发生干涉的情况下，及时对机械臂的位置进行修正，避免进一步的干涉碰撞。

显然，上述现有技术仍难以很好的规避多机械臂碰撞的问题。

发明内容

基于此，有必要针对上述技术问题，提供一种能够规避多机械臂碰撞的机械臂碰撞检测方法、装置、计算机设备和存储介质。

第一方面，提供一种机械臂摆位优化方法，该方法包括：

获取机器人各机械臂的初始摆位以及属性信息；

其中，各机械臂的初始摆位均满足各机械臂的创口约束条件；

根据各机械臂的初始摆位与属性信息，确定各机械臂的初始摆位对应的构型代价函数；

根据初始摆位的构型代价函数，求得满足预设条件的构型代价函数所对应的各机械臂初始摆位为各机械臂的目标初始摆位。

在其中一个实施例中，上述根据各机械臂的初始摆位与属性信息，确定各机械臂的初始摆位对应的构型代价函数包括：

根据机械臂的初始摆位与属性信息确定机械臂的至少一个运动关节特征点，确定相邻机械臂各运动关节特征点的距离；

根据各机械臂的初始摆位下，相邻机械臂各运动关节特征点的距离、预设的各机械臂运动关节权重，确定初始摆位的构型代价函数。

在其中一个实施例中，上述运动关节特征点的选取为连杆模型中心、连杆重心、连杆质心、连杆包围盒中心、连杆末端最远点或关节旋转中心。

在其中一个实施例中，上述运动关节权重可以根据运动关节所对应的连杆级数、连杆距离病人距离、连杆体积大小和、连杆累计自由度中的一种或组合确定。

在其中一个实施例中，上述根据初始摆位的构型代价函数，求得满足预设条件的构型代价函数所对应的各机械臂初始摆位为各机械臂的目标初始摆位，包括：

计算初始摆位的构型代价函数的第一值；

根据第一值执行迭代操作，直到第一值满足迭代条件，则确定初始摆位为目标初始摆位。

在其中一个实施例中，上述根据第一值执行迭代操作，直到第一值满足迭代条件，则确定初始摆位为目标初始摆位，包括：

根据第一值和预设的差值矩阵，确定下一个摆位及其构型代价函数；差值矩阵根据创口约束条件确定；

若下一个摆位的构型代价函数的第一值不满足迭代条件，则将下一个摆位的构型代价函数的第一值作为输入，返回执行迭代操作；

若下一个摆位的构型代价函数的第一值满足迭代条件，则将下一个摆位确定为目标初始摆位。

在其中一个实施例中，上述迭代条件包括下一个摆位的构型代价函数的第一值与初始摆位的构型代价函数的第一值之间的差值处于预设的差值范围内，和/或，下一个摆位的构型代价函数的第一值与预设标准值之间的差值处于差值范围内。

在其中一个实施例中，上述初始摆位包括各机械臂在所有情况下的多个初始摆位；根据初始摆位的构型代价函数，求得满足预设条件的构型代价函数所对应的各所述机械臂初始摆位为各机械臂的目标初始摆位，还包括：

获取多个初始摆位对应的构型代价函数；

遍历计算各初始摆位对应的构型代价函数的第一值；

将第一值最大的构型代价函数所对应的初始摆位，确定为目标初始摆位。

第二方面，提供一种机械臂摆位优化装置，该装置包括：

获取模块，用于获取机器人各机械臂的初始摆位以及属性信息；其中，各机械臂的初始摆位均满足各机械臂的创口约束条件；

第一确定模块，用于根据各机械臂的初始摆位与属性信息，确定各机械臂的初始摆位对应的构型代价函数；

第二确定模块，用于根据初始摆位的构型代价函数，求得满足预设条件的构型代价函数所对应的各机械臂初始摆位为各机械臂的目标初始摆位。

第三方面，提供一种计算机设备，包括存储器和处理器，该存储器存储有计算机程序，该处理器执行该计算机程序时实现上述第一方面任一所述的机械臂摆位优化方法。

第四方面，提供一种计算机可读存储介质，其上存储有计算机程序，该计算机程序被处理器执行时实现上述第一方面任一所述的机械臂摆位优化方法。

上述机械臂碰撞检测方法、装置、计算机设备和存储介质，处理器获取机器人各机械臂的初始摆位以及属性信息，根据各机械臂的初始摆位与属性信息，确定各机械臂的初始摆位对应的构型代价函数，据初始摆位的构型代价函数，求得满足预设条件的构型代价函数所对应的各机械臂初始摆位为各机械臂的目标初始摆位。其中，各机械臂的初始摆位均满足各机械臂的创口约束条件。在本方案中，处理器可以根据各机械臂的初始摆位以及属性信息，确定初始摆位的构型代价函数，并基于构型代价函数进行初始摆位的优化计算，得到符合优化目标以及约束条件的目标初始摆位，基于目标初始摆位，后续机械臂运动规划时，各机械臂拥有更大的运动规划空间，减少了各机械臂之间相互碰撞的概率，有效地避免各机械臂在手术过程中发生碰撞的情况，并且，初始摆位与目标初始摆位均满足预设的创口约束条件，确保碰撞检测过程中的各机械臂的位姿状态满足手术器械处于同一个创口点不改变，避免了病人手术时创口扩大的问题。

附图说明

图1为一个实施例中机械臂摆位优化方法的应用环境图；

图2为一个实施例中机械臂摆位优化方法的流程示意图；

图3为一个实施例中多机械臂对应的创口点的位置示意图；

图4为一个实施例中单个机械臂的多种摆位的示意图；

图5为一个实施例中多个机械臂多种初始摆位的示意图；

图6为一个实施例中机械臂摆位优化方法的流程示意图；

图7为一个实施例中机械臂摆位优化方法的流程示意图；

图8为一个实施例中机械臂摆位优化方法的流程示意图；

图9为一个实施例中机械臂摆位优化方法的流程示意图；

图10为一个实施例中机械臂摆位优化方法的流程示意图；

图11为一个实施例中机械臂摆位优化装置的结构框图；

图12为一个实施例中计算机设备的内部结构图。

具体实施方式

为了使本申请的目的、技术方案及优点更加清楚明白，以下结合附图及实施例，对本申请进行进一步详细说明。应当理解，此处描述的具体实施例仅仅用以解释本申请，并不用于限定本申请。

本申请提供的机械臂摆位优化方法，可以应用于如图1所示的应用环境中。其中，本方案涉及到的多机械臂机器人可以但不限于是各种型号、各种厂家的多机械臂型医疗手术机器人，其中，多机械臂机器人中包括可以控制机械臂的处理器101，多机械臂机器人通过处理器101控制机械臂102进行机械臂摆位优化操作。

下面将通过实施例并结合附图具体地对本申请的技术方案以及本申请的技术方案如何解决上述技术问题进行详细说明。下面这几个具体的实施例可以相互结合，对于相同或相似的概念或过程可能在某些实施例中不再赘述。需要说明的是，本申请图2-图10实施例提供的机械臂摆位优化方法，其执行主体为多机械臂机器人的处理器101，也可以是机械臂摆位优化装置，该机械臂摆位优化装置可以通过软件、硬件或者软硬件结合的方式成为多机械臂机器人的部分或全部。下述方法实施例中，均以执行主体是处理器101为例来进行说明。

在一个实施例中，如图2所示，提供了一种机械臂摆位优化方法，涉及的是计算机设备获取机器人各机械臂的初始摆位以及属性信息，根据各机械臂的初始摆位与属性信息，确定各机械臂的初始摆位对应的构型代价函数，根据初始摆位的构型代价函数，求得满足预设条件的构型代价函数所对应的各机械臂初始摆位为各机械臂的目标初始摆位。过程，包括以下步骤：

S201、获取机器人各机械臂的初始摆位以及属性信息；其中，各机械臂的初始摆位均满足各机械臂的创口约束条件。

其中，处理器获取机器人各机械臂的初始摆位，包括获取各机械臂的在初始摆位下的位姿信息，其中，包括各机械臂的运动关节的位姿、以及各运动关节特征点的空间坐标。属性信息指的是各机械臂的运动关节数量、运动关节体积、运动关节的权重值等参数。

在本实施例中，各机械臂的初始摆位满足预设的创口约束条件。其中，创口约束条件指的是根据创口点位置所构建的约束条件，创口点位置可以包括一个或多个，创口点位置为固定位置，在机械臂的运动过程中，为了确保不拉伤创口，需要根据创口点的位置对机械臂的运动空间或运动路径进行约束。根据创口点位置构建创口约束条件，将创口点位置的关系作为创口约束条件。示例地，如图3所示，每个机械臂都需要穿过创口点位置P1和P2从而接触至病灶点，其中P1和P2的空间相对位置固定，将创口点位置的关系作为创口约束条件，本实施例中不限定创口点位置的数量。

在本实施例中，根据创建的创口点约束条件，获取满足创口点约束条件的机器人各机械臂的初始摆位。其中，如图4所示，单个机械臂在同一个初始摆位下，有两种可能的摆位状态，不同的摆位状态所对应的机械臂运动空间也不同。在具有多个机械臂的情况下，以机器人具有2个机械臂来说明，如图5所示，机器人有2个机械臂，每个机械臂在初始摆位下都有2组关节值的解，那么机器人就存在4种不同的初始摆位的状态。机器人在不同的初始摆位下，形成的后续多机械臂的路径规划有不同的影响。

可选地，机械臂在初始摆位较为聚拢的情况下，例如图4中的多臂摆位1，机械臂关节之间距离过近，在后续规划出的运动路径中机械臂更容易发生干涉碰撞，因此，多臂摆位1可以认为是较差的摆位构型。在初始摆位较为扩展的情况下，例如图4中的多臂摆位4，各机械臂连杆较为分散，相比之下各机械臂不容易发生干涉碰撞，因此，多臂摆位4可以认为是较好的摆位构型。

在确定初始摆位的过程中，可在较好的摆位构型中确定符合创口点约束条件的初始摆位，并获取初始摆位对应的各机械臂的在初始摆位下的运动关节的位姿、以及各运动关节特征点的空间坐标。

S202、根据各机械臂的初始摆位与属性信息，确定各机械臂的初始摆位对应的构型代价函数。

其中，构型代价函数指的是将初始摆位以可量化的形式所构建的函数表达式，该构型代价函数包括各机械臂的属性信息之间的关系。

在本实施例中，构型代价函数包括当前初始摆位下相邻机械臂对应的关系，可选地，处理器可以根据两两机械臂中各运动关节之间的相对距离、机械臂各运动关节所对应的权重，构建当前初始摆位下的构型代价函数。在当前机器人包括两个机械臂的情况下，构型代价函数指的就是两两机械臂的构型代价函数。在当前机器人包括多于两个机械臂的情况下啊，构型代价函数可以为相邻机械臂的构型代价函数，本实施例对此不做限定。

S203、根据初始摆位的构型代价函数，求得满足预设条件的构型代价函数所对应的各机械臂初始摆位为各机械臂的目标初始摆位。

在本实施例中，预设条件可以为预先设定的优化目标，例如，预设条件为满足运动空间最大；那么处理器在获取到多个构型代价函数的情况下，可以通过构型代价函数的函数值，确定满足运动空间最大的构型代价函数，并将其所对应的初始摆位可以确定为目标初始摆位。可选地，预设条件还可以为设定的迭代算法中的迭代条件，其中，根据预设的迭代条件，将初始摆位的构型代价函数待入迭代算法过程中，直到确定满足迭代条件的构型代价函数，将该构型代价函数对应的摆位确定为目标初始摆位，本实施例对此不做限定。

上述机械臂摆位优化方法中，处理器获取机器人各机械臂的初始摆位以及属性信息，根据各机械臂的初始摆位与属性信息，确定各机械臂的初始摆位对应的构型代价函数，据初始摆位的构型代价函数，求得满足预设条件的构型代价函数所对应的各机械臂初始摆位为各机械臂的目标初始摆位。其中，各机械臂的初始摆位均满足各机械臂的创口约束条件。在本方案中，处理器可以根据各机械臂的初始摆位以及属性信息，确定初始摆位的构型代价函数，并基于构型代价函数进行初始摆位的优化计算，得到符合优化目标以及约束条件的目标初始摆位，基于目标初始摆位，后续机械臂运动规划时，各机械臂拥有更大的运动规划空间，减少了各机械臂之间相互碰撞的概率，有效地避免各机械臂在手术过程中发生碰撞的情况，并且，初始摆位与目标初始摆位均满足预设的创口约束条件，确保碰撞检测过程中的各机械臂的位姿状态满足手术器械处于同一个创口点不改变，避免了病人手术时创口扩大的问题。

处理器针对初始摆位构建起对应的构型代价函数，在其中一个实施例中，如图6所示，上述根据各机械臂的初始摆位与属性信息，确定各机械臂的初始摆位对应的构型代价函数包括：

S301、根据机械臂的初始摆位与属性信息确定机械臂的至少一个运动关节特征点，确定相邻机械臂各运动关节特征点的距离。

可选地，上述运动关节特征点的选取可以为各机械臂的连杆模型中心、连杆重心、连杆质心、连杆包围盒中心、连杆末端最远点或关节旋转中心。

在本实施例中，根据初始摆位的机械臂的位姿，确定机械臂中各运动关节的空间坐标，从而确定机械臂的至少一个运动关节特征点。根据相邻机械臂中各运动关节特征点，确定相邻机械臂各运动关节点之间的相对距离。

S302、根据各机械臂的初始摆位下，相邻机械臂各运动关节特征点的距离、预设的各机械臂运动关节权重，确定初始摆位的构型代价函数。

其中，各机械臂的运动关节权重与各机械臂的属性信息相关，其中，运动关节权重可以根据机械臂中运动关节所对应的连杆级数、连杆距离病人距离、连杆体积大小、连杆累计自由度等参数确定。示例地，以连杆级数为例说明，机械臂的基坐标系为0级，与基坐标系相连的连杆为1级，连杆往上级别增加，连杆级别越高；以连杆距离病人距离为例说明，计算机械臂所距离病人的最近距离，如果距离越近，那么摆位权重值越大；以连杆体积大小为例说明，机械臂中连杆体积越大，占用的活动空间也越多，需要越大的摆位空间，因此需要赋予更大的权重值；以连杆累计自由度为例说明，例如机械臂的基坐标系没有自由度，则机械臂每往上一级会增加一个自由度，连杆自由度越大，活动越灵活，则赋予更大的权重值，本实施例对权重值的设置不做限定。

在本实施例中，示例地，根据各机械臂的初始摆位下，相邻机械臂各运动关节特征点的距离、预设的各机械臂运动关节权重确定初始摆位的构型代价函数，其中，构型代价函数可表示为：

其中，代表A机械臂的i个运动关节特征点到B机械臂的j个运动关节特征点之间的距离，/>代表对应的权重；其中，i，j为自然数，n，m为自然数。

在本实施例中，根据各机械臂的初始摆位下，相邻机械臂各运动关节特征点的距离、预设的各机械臂运动关节权重，确定初始摆位的构型代价函数，根据构型代价函数可以确定当前初始摆位的构型值，从而可以实现对初始摆位的定性量化。

处理器可以通过迭代优化的方式，寻找符合迭代条件的目标初始摆位，在其中一个实施例中，如图7所示，上述根据所述初始摆位的构型代价函数，求得满足预设条件的构型代价函数所对应的各所述机械臂初始摆位为各所述机械臂的目标初始摆位，包括：

S401、计算初始摆位的构型代价函数的第一值。

在本实施例中，处理器根据初始摆位的构型代价函数，将初始摆位下，计算得到的相邻机械臂的运动关节特征点之间的距离、权重带入该构型代价函数中，计算得到初始摆位对应的构型代价函数值，即为第一值。

S402、根据第一值执行迭代操作，直到第一值满足迭代条件，则确定初始摆位为目标初始摆位。

可选地，迭代条件包括下一个摆位的构型代价函数的第一值与初始摆位的构型代价函数的第一值之间的差值处于预设的差值范围内，和/或，下一个摆位的构型代价函数的第一值与预设标准值之间的差值处于差值范围内。

在本实施例中，根据对初始摆位的构型代价函数计算得到的第一值，以及预设的迭代系数进行迭代操作，直到下一个摆位的构型代价函数的第一值与初始摆位的构型代价函数的第一值之间的差值处于预设的差值范围内，和/或，下一个摆位的构型代价函数的第一值与预设标准值之间的差值处于差值范围内，输出目标初始摆位。

在本实施例中，基于迭代条件，根据初始摆位的第一值进行迭代操作，直到符合迭代条件，该方法可以有效地确定迭代优化后的目标初始摆位。

处理器在执行迭代操作的过程中，具体包括，在其中一个实施例中，如图8所示，上述根据第一值执行迭代操作，直到第一值满足迭代条件，则确定初始摆位为目标初始摆位，包括：

S501、根据第一值和预设的差值矩阵，确定下一个摆位及其构型代价函数；差值矩阵根据创口约束条件确定。

其中，预设的差值矩阵为根据创口约束条件所构建的迭代差值矩阵，在本实施例中，处理器在迭代过程中，根据计算得到的第i个初始摆位的第一值和预设的差值矩阵，确定第i+1个摆位和其对应的构型代价函数，并计算第i+1个摆位的构型代价函数的第一值。

S502、若下一个摆位的构型代价函数的第一值不满足迭代条件，则将下一个摆位的构型代价函数的第一值作为输入，返回执行迭代操作。

在本实施例中，根据第i+1个摆位的构型代价函数的第一值，判断该第一值是否满足迭代条件，若第i+1个摆位的构型代价函数的第一值与初始摆位的构型代价函数的第一值之间的差值处于预设的差值范围之外，和/或，第i+1个摆位的构型代价函数的第一值与预设标准值之间的差值处于差值范围之外，则将第i+1个摆位的构型代价函数第一值作为输入，返回执行迭代操作。

S503、若下一个摆位的构型代价函数的第一值满足迭代条件，则将下一个摆位确定为目标初始摆位。

在本实施例中，若第i+1个摆位的构型代价函数的第一值与初始摆位的构型代价函数的第一值之间的差值处于预设的差值范围内，和/或，第i+1个摆位的构型代价函数的第一值与预设标准值之间的差值处于差值范围内，则将第i+1个摆位确定为目标初始摆位，本实施例对此不做限定。

在本实施例中，根据初始摆位的构型代价函数的第一值和预设的迭代条件进行迭代操作，直到摆位的构型代价函数的第一值符合迭代条件，输出目标输出摆位，该方法可以有效地确定迭代优化后的目标初始摆位。

处理器还可以通过遍历所有摆位的构型代价函数而确定目标初始摆位，在其中一个实施例中，如图9所示，上述初始摆位包括各机械臂在所有情况下的多个初始摆位；根据初始摆位的构型代价函数，求得满足预设条件的构型代价函数所对应的各所述机械臂初始摆位为各机械臂的目标初始摆位，还包括：

S601、获取多个初始摆位对应的构型代价函数。

在本实施例中，处理器可以根据机器人的机械臂的数量、机械臂各运动关节的数量，确定机器人所有可能的候选初始摆位，并根据所有候选初始摆位下的各机械臂的位姿信息、属性信息，确定各个候选初始摆位对应的构型代价函数。

S602、遍历计算各初始摆位对应的构型代价函数的第一值。

在本实施例中，遍历计算各个候选初始摆位对应的构型代价函数的函数值，可选地，可以将计算所得到的所有候选初始摆位对应的构型代价函数的函数值存储至指定的存储空间中。

S603、将第一值最大的构型代价函数所对应的初始摆位，确定为目标初始摆位。

在本实施例中，可以通过任意一种比较的方式，确定所有候选初始摆位对应的构型代价函数的函数值中，确定函数值最大的构型代价函数，并将该构型代价函数对应的摆位确定为目标初始摆位。

在本实施例中，遍历计算的方法适用于规模较小，机械臂数量较少的场景下，在该场景下，基于遍历计算的方法，可以简单、快速的确定目标初始摆位。

在确定目标初始摆位之后，可选地，本实施例还提供了一种多机械臂碰撞检测方法，包括：根据目标初始摆位中各机械臂的空间位姿和预设的检测模式，对各机械臂进行碰撞检测，输出碰撞检测结果。

其中，预设的检测模式包括机械臂自身的碰撞检测、机械臂与环境障碍物模型的碰撞检测、机械臂与机械臂之间的碰撞检测、以及机械臂的其他碰撞检测。

在其中一种检测模式下，上述检测模式包括机械臂自碰撞检测；根据各机械臂的空间位姿和预设的检测模式，对各机械臂进行碰撞检测，输出碰撞检测结果，包括：

遍历各机械臂的每一个空间位姿，对各机械臂进行自碰撞检测；若存在机械臂自身发生碰撞，则确定碰撞检测结果为碰撞。

其中，机械臂自碰撞检测是为了保证机械臂运动到目标位置时不与自身发生碰撞。

在本实施例中，采取遍历的方式，对每个机械臂的各连杆进行自碰撞检测，示例地，机械臂A有4个连杆A1、A2、A3、A4，遍历每一个空间位姿下，遍历A1、A2、A3、A4之间是否存在碰撞或干涉。若A1、A2、A3、A4之间存在任意两个连杆发生碰撞或干涉，则确定碰撞检测结果为碰撞，退出碰撞检测程序。

在另一种碰撞检测模式下，上述检测模式包括机械臂与环境碰撞检测；根据各机械臂的空间位姿和预设的检测模式，对各机械臂进行碰撞检测，输出碰撞检测结果，包括：

遍历各机械臂的空间位姿，对各机械臂进行环境碰撞检测；若存在机械臂与环境障碍物发生碰撞，则确定碰撞检测结果为碰撞。

其中，环境碰撞检测指的是将各机械臂与环境障碍物模型进行遍历检测，环境障碍物模型为预先设定的当前环境中可能存在的障碍模型。例如，手术台、CT机、聚光灯等，和机械臂模型同理，该模型也只是用于表征环境中障碍物的三维几何特征，不局限于表现形式或文件格式。

在本实施例中，在环境障碍物模型确定后，需要确定环境障碍物模型与机械臂之间的相对位置关系，其中，可以通过外部测量或是标定的方式，获取机械臂和环境障碍物模型在世界坐标系下的位姿表示，将环境障碍物模型的坐标和各机械臂的坐标统一至同一个世界坐标系下，对各机械臂进行环境碰撞检测。各机械臂与环境碰撞检测同样也是采取遍历的方式，针对每一个机械臂，将机械臂与所有环境障碍物模型分别进行碰撞检测。若机械臂与任意一个环境障碍物模型发生干涉或者碰撞，则确定碰撞检测结果为碰撞，退出碰撞检测程序。

在又一种碰撞检测模式下，上述检测模式包括机械臂与机械臂摆位优化；根据各机械臂的空间位姿和预设的检测模式，对各机械臂进行碰撞检测，输出碰撞检测结果，包括：

遍历各机械臂的空间位姿，对各机械臂进行两两碰撞检测；若存在两两机械臂之间发生碰撞或干涉，则确定碰撞检测结果为碰撞。

本实施例提供的检测模式指的是对不同机械臂之间的碰撞检测。示例地，多机械臂机器人中有A、B、C、D四个机械臂，要进行机械臂与机械臂的碰撞检测，可以确定两个机械臂一组进行排列组合，排列组合包括[A，B]、[A，C]、[A，D]、[B，C]、[B，D]、[C，D]共6种组合方式。针对每种情况进行碰撞检测遍历，具体地，机械臂A有4个连杆A1、A2、A3、A4，机械臂B有5个连杆B1、B2、B3、B4、B5，那么[A，B]的碰撞检测遍历流程就是，将A1与B1~B5进行碰撞检测；将A2与B1~B5进行碰撞检测，遍历重复执行碰撞检测。若存在A1与B1~B5中任意一个机械臂之间发生碰撞或干涉，则结束程序，确定碰撞就检测结果为碰撞。可选地，如果A1与B1~B5中每一个都没发生碰撞，则将A1换成A2，将A2与B1~B5进行碰撞检测，得到检测结果。

在本实施例中，在每一种检测模式下，机器人可以通过遍历检测的方式，对各机械臂进行相应模式下的碰撞检测。示例地，机器人针对每一个机械臂进行自碰撞检测、针对每一个机械臂与环境障碍物模型进行碰撞检测、针对每一个机械臂与其他机械臂进行两两碰撞检测等，本实施例对此不做限定。

为了更好的说明上述方法，如图10所示，本实施例提供一种机械臂摆位优化方法，具体包括：

S101、获取机器人各机械臂的初始摆位以及属性信息；

S102、根据机械臂的初始摆位与属性信息确定机械臂的至少一个运动关节特征点，确定相邻机械臂各运动关节特征点的距离；

S103、根据各机械臂的初始摆位下，相邻机械臂各运动关节特征点的距离、预设的各机械臂运动关节权重，确定初始摆位的构型代价函数；

S104、计算初始摆位的构型代价函数的第一值；

S105、根据第一值和预设的差值矩阵，确定下一个摆位及其构型代价函数；

S106、若下一个摆位的构型代价函数的第一值不满足迭代条件，则将下一个摆位的构型代价函数的第一值作为输入，返回执行步骤S104；

S107、若下一个摆位的构型代价函数的第一值满足迭代条件，则将下一个摆位确定为目标初始摆位；

S108、基于目标初始摆位，遍历各机械臂的每一个空间位姿，对各机械臂进行自碰撞检测；

S109、若存在机械臂自身发生碰撞，则确定碰撞检测结果为碰撞；

S110、若不存在碰撞，则遍历各机械臂的空间位姿，对各机械臂进行环境碰撞检测；

S111、若存在机械臂与环境障碍物发生碰撞，则确定碰撞检测结果为碰撞；

S112、若不存在碰撞，则遍历各机械臂的空间位姿，对各机械臂进行两两碰撞检测；

S113、若存在两两机械臂之间发生碰撞或干涉，则确定碰撞检测结果为碰撞；

S114、输出无碰撞结果。

在本实施例中，处理器得到符合优化目标以及约束条件的目标初始摆位，基于目标初始摆位，后续机械臂运动规划时，各机械臂拥有更大的运动规划空间，减少了各机械臂之间相互碰撞的概率，有效地避免各机械臂在手术过程中发生碰撞的情况，并且，初始摆位与目标初始摆位均满足预设的创口约束条件，确保碰撞检测过程中的各机械臂的位姿状态满足手术器械处于同一个创口点不改变，避免了病人手术时创口扩大的问题。

上述实施例提供的机械臂摆位优化方法，其实现原理和技术效果与上述方法实施例类似，在此不再赘述。

应该理解的是，虽然图2-10的流程图中的各个步骤按照箭头的指示依次显示，但是这些步骤并不是必然按照箭头指示的顺序依次执行。除非本文中有明确的说明，这些步骤的执行并没有严格的顺序限制，这些步骤可以以其它的顺序执行。而且，图2-10中的至少一部分步骤可以包括多个步骤或者多个阶段，这些步骤或者阶段并不必然是在同一时刻执行完成，而是可以在不同的时刻执行，这些步骤或者阶段的执行顺序也不必然是依次进行，而是可以与其它步骤或者其它步骤中的步骤或者阶段的至少一部分轮流或者交替地执行。

在一个实施例中，如图11所示，提供了一种机械臂摆位优化装置，包括：获取模块01、第一确定模块02和第二确定模块03，其中：

获取模块01，用于获取机器人各机械臂的初始摆位以及属性信息；其中，各机械臂的初始摆位均满足各机械臂的创口约束条件；

第一确定模块02，用于根据各机械臂的初始摆位与属性信息，确定各机械臂的初始摆位对应的构型代价函数；

第二确定模块03，用于根据初始摆位的构型代价函数，求得满足预设条件的构型代价函数所对应的各机械臂初始摆位为各机械臂的目标初始摆位。

在其中一个实施例中，第一确定模块02，用于根据机械臂的初始摆位与属性信息确定机械臂的至少一个运动关节特征点，确定相邻机械臂各运动关节特征点的距离；根据各机械臂的初始摆位下，相邻机械臂各运动关节特征点的距离、预设的各机械臂运动关节权重，确定初始摆位的构型代价函数。

在其中一个实施例中，上述运动关节特征点的选取为连杆模型中心、连杆重心、连杆质心、连杆包围盒中心、连杆末端最远点或关节旋转中心。

在其中一个实施例中，上述运动关节权重可以根据运动关节所对应的连杆级数、连杆距离病人距离、连杆体积大小和、连杆累计自由度中的一种或组合确定。

在其中一个实施例中，第二确定模块03，用于计算初始摆位的构型代价函数的第一值；根据第一值执行迭代操作，直到第一值满足迭代条件，则确定初始摆位为目标初始摆位。

在其中一个实施例中，第二确定模块03，用于根据第一值和预设的差值矩阵，确定下一个摆位及其构型代价函数；差值矩阵根据创口约束条件确定；若下一个摆位的构型代价函数的第一值不满足迭代条件，则将下一个摆位的构型代价函数的第一值作为输入，返回执行迭代操作；若下一个摆位的构型代价函数的第一值满足迭代条件，则将下一个摆位确定为目标初始摆位。

在其中一个实施例中，上述迭代条件包括下一个摆位的构型代价函数的第一值与初始摆位的构型代价函数的第一值之间的差值处于预设的差值范围内，和/或，下一个摆位的构型代价函数的第一值与预设标准值之间的差值处于差值范围内。

在其中一个实施例中，上述初始摆位包括各机械臂在所有情况下的多个初始摆位；第二确定模块03，还用于获取多个初始摆位对应的构型代价函数；遍历计算各初始摆位对应的构型代价函数的第一值；将第一值最大的构型代价函数所对应的初始摆位，确定为目标初始摆位。

关于机械臂摆位优化装置的具体限定可以参见上文中对于机械臂摆位优化方法的限定，在此不再赘述。上述机械臂摆位优化装置中的各个模块可全部或部分通过软件、硬件及其组合来实现。上述各模块可以硬件形式内嵌于或独立于计算机设备中的处理器中，也可以以软件形式存储于计算机设备中的存储器中，以便于处理器调用执行以上各个模块对应的操作。

在一个实施例中，提供了一种计算机设备，该计算机设备可以是服务器，其内部结构图可以如图12所示。该计算机设备包括通过***总线连接的处理器、存储器、通信接口、显示屏和输入装置。其中，该计算机设备的处理器用于提供计算和控制能力。该计算机设备的存储器包括非易失性存储介质、内存储器。该非易失性存储介质存储有操作***和计算机程序。该内存储器为非易失性存储介质中的操作***和计算机程序的运行提供环境。该计算机设备的通信接口用于与外部的终端进行有线或无线方式的通信，无线方式可通过WIFI、运营商网络、NFC（近场通信）或其他技术实现。该计算机程序被处理器执行时以实现一种机械臂摆位优化方法。该计算机设备的显示屏可以是液晶显示屏或者电子墨水显示屏，该计算机设备的输入装置可以是显示屏上覆盖的触摸层，也可以是计算机设备外壳上设置的按键、轨迹球或触控板，还可以是外接的键盘、触控板或鼠标等。

本领域技术人员可以理解，图12中示出的结构，仅仅是与本申请方案相关的部分结构的框图，并不构成对本申请方案所应用于其上的计算机设备的限定，具体的计算机设备可以包括比图中所示更多或更少的部件，或者组合某些部件，或者具有不同的部件布置。

在一个实施例中，提供了一种计算机设备，包括存储器和处理器，存储器中存储有计算机程序，该处理器执行计算机程序时实现以下步骤：

获取机器人各机械臂的初始摆位以及属性信息；

其中，各机械臂的初始摆位均满足各机械臂的创口约束条件；

根据各机械臂的初始摆位与属性信息，确定各机械臂的初始摆位对应的构型代价函数；

根据初始摆位的构型代价函数，求得满足预设条件的构型代价函数所对应的各机械臂初始摆位为各机械臂的目标初始摆位。

上述实施例提供的计算机设备，其实现原理和技术效果与上述方法实施例类似，在此不再赘述。

在一个实施例中，提供了一种计算机可读存储介质，其上存储有计算机程序，计算机程序被处理器执行时实现以下步骤：

获取机器人各机械臂的初始摆位以及属性信息；

其中，各机械臂的初始摆位均满足各机械臂的创口约束条件；

根据各机械臂的初始摆位与属性信息，确定各机械臂的初始摆位对应的构型代价函数；

根据初始摆位的构型代价函数，求得满足预设条件的构型代价函数所对应的各机械臂初始摆位为各机械臂的目标初始摆位。

上述实施例提供的计算机可读存储介质，其实现原理和技术效果与上述方法实施例类似，在此不再赘述。

本领域普通技术人员可以理解实现上述实施例方法中的全部或部分流程，是可以通过计算机程序来指令相关的硬件来完成，所述的计算机程序可存储于一非易失性计算机可读取存储介质中，该计算机程序在执行时，可包括如上述各方法的实施例的流程。其中，本申请所提供的各实施例中所使用的对存储器、存储、数据库或其它介质的任何引用，均可包括非易失性和易失性存储器中的至少一种。非易失性存储器可包括只读存储器（Read-Only Memory，ROM）、磁带、软盘、闪存或光存储器等。易失性存储器可包括随机存取存储器（Random Access Memory，RAM）或外部高速缓冲存储器。作为说明而非局限，RAM可以是多种形式，比如静态随机存取存储器（Static Random Access Memory，SRAM）或动态随机存取存储器（Dynamic Random Access Memory，DRAM）等。

以上实施例的各技术特征可以进行任意的组合，为使描述简洁，未对上述实施例中的各个技术特征所有可能的组合都进行描述，然而，只要这些技术特征的组合不存在矛盾，都应当认为是本说明书记载的范围。

以上所述实施例仅表达了本申请的几种实施方式，其描述较为具体和详细，但并不能因此而理解为对发明专利范围的限制。应当指出的是，对于本领域的普通技术人员来说，在不脱离本申请构思的前提下，还可以做出若干变形和改进，这些都属于本申请的保护范围。因此，本申请专利的保护范围应以所附权利要求为准。

Claims (22)

1.一种机械臂碰撞检测方法，其特征在于，所述方法包括：

获取机器人各机械臂的初始摆位对应的构型代价函数；

根据所述初始摆位对应的构型代价函数，确定各所述机械臂的目标初始摆位；

根据所述目标初始摆位中各所述机械臂的空间位姿和预设的检测模式，对各所述机械臂进行碰撞检测，输出碰撞检测结果。

2.根据权利要求1所述的方法，其特征在于，所述获取机器人各机械臂的初始摆位对应的构型代价函数，包括：

根据各所述机械臂的初始摆位，确定各所述机械臂的至少一个运动关节特征点；

根据各所述机械臂的至少一个运动关节特征点，确定相邻机械臂各运动关节特征点的距离；

根据各所述机械臂的初始摆位下，所述相邻机械臂各运动关节特征点的距离和各所述机械臂的运动关节权重，确定所述初始摆位的构型代价函数。

3.根据权利要求2所述的方法，其特征在于，所述根据各所述机械臂的初始摆位，确定各所述机械臂的至少一个运动关节特征点，包括：

根据所述初始摆位对应的各所述机械臂的位姿，确定各所述机械臂中运动关节的空间坐标；

根据各所述机械臂中运动关节的空间坐标，确定各所述机械臂的至少一个运动关节特征点。

4.根据权利要求2或3所述的方法，其特征在于，所述运动关节特征点包括所述机械臂的连杆模型中心、连杆重心、连杆质心、连杆包围盒中心、连杆末端最远点或关节旋转中心。

5.根据权利要求2或3所述的方法，其特征在于，所述方法还包括：

针对任一机械臂，根据所述机械臂中运动关节所对应的连杆级数、连杆距离病人距离、连杆体积、连杆累计自由度中的至少一种，确定所述机械臂的运动关节权重。

6.根据权利要求1-3任一项所述的方法，其特征在于，所述构型代价函数包括所述初始摆位下两两相邻机械臂的构型代价函数。

7.根据权利要求1-3任一项所述的方法，其特征在于，所述初始摆位包括各机械臂在所有情况下的多个初始摆位；所述根据所述初始摆位对应的构型代价函数，确定各所述机械臂的目标初始摆位，包括：

获取所述多个初始摆位对应的构型代价函数；

遍历计算各所述初始摆位对应的构型代价函数的第一值；

将所述第一值最大的构型代价函数所对应的初始摆位，确定为所述目标初始摆位。

8.根据权利要求1-3任一项所述的方法，其特征在于，所述检测模式包括机械臂自碰撞检测，所述根据所述目标初始摆位中各所述机械臂的空间位姿和预设的检测模式，对各所述机械臂进行碰撞检测，输出碰撞检测结果，包括：

遍历各所述机械臂的空间位姿，对各所述机械臂进行自碰撞检测；

若存在机械臂自身发生碰撞，则确定所述碰撞检测结果为碰撞。

9.根据权利要求1-3任一项所述的方法，其特征在于，所述检测模式包括机械臂与环境碰撞检测，所述根据所述目标初始摆位中各所述机械臂的空间位姿和预设的检测模式，对各所述机械臂进行碰撞检测，输出碰撞检测结果，包括：

遍历各所述机械臂的空间位姿，对各所述机械臂进行环境碰撞检测；

若存在机械臂与环境障碍物发生碰撞，则确定所述碰撞检测结果为碰撞。

10.根据权利要求1-3任一项所述的方法，其特征在于，所述检测模式包括机械臂与机械臂之间的碰撞检测，所述根据所述目标初始摆位中各所述机械臂的空间位姿和预设的检测模式，对各所述机械臂进行碰撞检测，输出碰撞检测结果，包括：

遍历各所述机械臂的空间位姿，对各所述机械臂进行两两碰撞检测；

若存在两两机械臂之间发生碰撞或干涉，则确定所述碰撞检测结果为碰撞。

11.一种机械臂碰撞检测装置，其特征在于，所述装置包括：

获取模块，用于获取机器人各机械臂的初始摆位对应的构型代价函数；

确定模块，用于根据所述初始摆位对应的构型代价函数，确定各所述机械臂的目标初始摆位；

检测模块，用于根据所述目标初始摆位中各所述机械臂的空间位姿和预设的检测模式，对各所述机械臂进行碰撞检测，输出碰撞检测结果。

12.根据权利要求11所述的装置，其特征在于，获取模块包括：

第一确定单元，用于根据各所述机械臂的初始摆位，确定各所述机械臂的至少一个运动关节特征点；

第二确定单元，用于根据各所述机械臂的至少一个运动关节特征点，确定相邻机械臂各运动关节特征点的距离；

第三确定单元，用于根据各所述机械臂的初始摆位下，所述相邻机械臂各运动关节特征点的距离和各所述机械臂的运动关节权重，确定所述初始摆位的构型代价函数。

13.根据权利要求12所述的装置，其特征在于，所述第一确定单元，包括：

确定子单元，用于根据所述初始摆位对应的各所述机械臂的位姿，确定各所述机械臂中运动关节的空间坐标；根据各所述机械臂中运动关节的空间坐标，确定各所述机械臂的至少一个运动关节特征点。

14.根据权利要求12或13所述的装置，其特征在于，所述运动关节特征点包括所述机械臂的连杆模型中心、连杆重心、连杆质心、连杆包围盒中心、连杆末端最远点或关节旋转中心。

15.根据权利要求12或13所述的装置，其特征在于，所述装置还用于针对任一机械臂，根据所述机械臂中运动关节所对应的连杆级数、连杆距离病人距离、连杆体积、连杆累计自由度中的至少一种，确定所述机械臂的运动关节权重。

16.根据权利要求11-13任一项所述的装置，其特征在于，所述构型代价函数包括所述初始摆位下两两相邻机械臂的构型代价函数。

17.根据权利要求11-13任一项所述的装置，其特征在于，所述初始摆位包括各机械臂在所有情况下的多个初始摆位；所述确定模块包括：

第四确定单元，用于获取所述多个初始摆位对应的构型代价函数；遍历计算各所述初始摆位对应的构型代价函数的第一值；将所述第一值最大的构型代价函数所对应的初始摆位，确定为所述目标初始摆位。

18.根据权利要求11-13任一项所述的装置，其特征在于，所述检测模式包括机械臂自碰撞检测，所述检测模块包括：

第一遍历单元，用于遍历各所述机械臂的空间位姿，对各所述机械臂进行自碰撞检测；

第五确定单元，用于若存在机械臂自身发生碰撞，则确定所述碰撞检测结果为碰撞。

19.根据权利要求11-13任一项所述的装置，其特征在于，所述检测模式包括机械臂与环境碰撞检测，所述检测模块包括：

第二遍历单元，用于遍历各所述机械臂的空间位姿，对各所述机械臂进行环境碰撞检测；

第六确定单元，用于若存在机械臂与环境障碍物发生碰撞，则确定所述碰撞检测结果为碰撞。

20.根据权利要求11-13任一项所述的装置，其特征在于，所述检测模式包括机械臂与机械臂之间的碰撞检测，所述检测模块包括：

第三遍历单元，用于遍历各所述机械臂的空间位姿，对各所述机械臂进行两两碰撞检测；

第七确定单元，用于若存在两两机械臂之间发生碰撞或干涉，则确定所述碰撞检测结果为碰撞。

21.一种计算机设备，包括存储器和处理器，所述存储器存储有计算机程序，其特征在于，所述处理器执行所述计算机程序时实现权利要求1至10中任一项所述的方法的步骤。

22.一种计算机可读存储介质，其上存储有计算机程序，其特征在于，所述计算机程序被处理器执行时实现权利要求1至10中任一项所述的方法的步骤。