CN115919472B - 一种机械臂定位方法及相关系统、装置、设备及介质 - Google Patents

Abstract

本申请提供了一种机械臂定位方法，应用于软式内镜操作机器人系统，该系统包括机械臂、双目相机、单目相机、激光测距传感器组和控制装置，该方法包括：控制装置基于所述双目相机，引导机械臂到达手术床体的标志识别位置，使手术床体的标志位于所述单目相机的视野中，然后接收所激光测距传感器组反馈的、激光测距传感器组与手术床体的距离信息，根据距离信息调整机械臂的位姿。该方法能够实现自动将机械臂定位至合适位置，避免医护人员人工拖动机械臂寻找合适手术位置，缩短了每台手术或每次检查时间，降低了成本。

Description

一种机械臂定位方法及相关系统、装置、设备及介质

技术领域

本申请涉及机械臂控制技术领域，尤其涉及一种机械臂定位方法、系统、计算机可读存储介质以及计算机程序产品。

背景技术

随着机器人辅助技术的不断发展，产生了各种功能的机械臂，例如是辅助进行医疗检查的机械臂、实现自动化生产流水线的机械臂。其中，一些机械臂还需要人工辅助进行定位。下面以用于对处于人体自然腔道（如消化道、呼吸道）内部的病灶进行检查或手术治疗的软式内镜示例说明。

传统的软式内镜检查或手术，需要人双手配合持镜体、操作旋钮、手工输送镜体等动作完成操作过程。有的检查或手术需要在放射线的图像引导下进行，医务人员需要穿戴沉重的铅制防护服等长时间手工操作软式内镜，对医务人员诊疗操作质量、体力和健康影响较大，尤其对经验丰富的年长或女性医务人员制约更大，甚至被迫放弃内镜工作。

随着机器人辅助技术的发展，医生可以通过调节手柄开关和按钮就可以调节软式内镜的输送长度和姿态，可以大大降低医生的体力和手工操作疲劳，降低手术的操作要求，减少对医务人员的辐射，同时可以提高医务人员与影像之间的交互。

然而，现有的软式内镜操作机器人系统在备台（术前准备）花费大量时间。例如，术前需要医护人员人工拖动机械臂，寻找合适手术位置，根据不同手术类型，机械臂需要人为拖动至不同空间位置。如此导致每台手术或每次检查时间增长，为了满足需求，需要配置较多的软式内镜，增加了成本。

发明内容

本申请提供了一种机械臂定位方法，该方法能够实现自动将机械臂定位至合适位置，避免医护人员人工拖动机械臂寻找合适手术位置，缩短了每台手术或每次检查时间，降低了成本。本申请还提供了上述方法对应的软式内镜操作机器人系统、控制装置、计算设备、计算机可读存储介质以及计算机程序产品。

第一方面，本申请提供了一种机械臂定位方法。该方法应用于软式内镜操作机器人系统，所述系统包括机械臂、双目相机、单目相机、激光测距传感器组和控制装置。

具体地，所述控制装置基于所述双目相机，引导所述机械臂到达手术床体标志的识别位置，使手术床体的标志位于所述单目相机的视野中，然后控制装置接收所述激光测距传感器组反馈的、所述激光测距传感器组与所述手术床体的距离信息，根据所述距离信息调整所述机械臂的位姿。

在一些可能的实现方式中，所述控制装置基于所述双目相机，引导所述机械臂到达手术床体的标志识别位置，包括：

所述控制装置控制所述机械臂由安全位置运动至备台位置；

所述控制装置接收所述双目相机识别到的所述机械臂的标志和所述手术床体的标志的空间位置；

所述控制装置根据所述空间位置，对所述机械臂粗定位。

在一些可能的实现方式中，所述系统还包括输送装置，所述激光测距传感器组中的多个激光测距传感器均匀分布在所述输送装置上，所述激光测距传感器组与所述手术床体的距离信息包括每个激光测距传感器与所述手术床体的距离信息；

所述控制装置根据所述距离信息调整所述机械臂的位姿，包括：

所述控制装置根据所述多个激光测距传感器与所述手术床体的距离信息，确定距离变化量；

所述控制装置根据所述距离变化量，确定所述手术床体的旋转量和偏移量；

所述控制装置根据所述旋转量和所述偏移量调整所述机械臂的位姿。

在一些可能的实现方式中，所述方法还包括：

所述控制装置基于所述单目相机采集的目标腔道的特征标识数据建立所述目标腔道的三维模型；

所述控制装置根据所述目标腔道的三维模型以及所述目标腔道的模型模板，获得所述目标腔道的特征标识的位姿偏移量；

所述控制装置根据所述目标腔道的特征标识的位姿偏移量，调整所述机械臂的位姿。

在一些可能的实现方式中，所述控制装置根据所述目标腔道的三维模型以及所述目标腔道的模型模板，获得所述目标腔道的特征标识的位姿偏移量，包括：

所述控制装置比对所述目标腔道的三维模型以及所述目标腔道的模型模板，获得模型特征之间的位姿信息；

所述控制装置根据模型特征之间的位姿信息以及所述模型模板中单位像素距离与实际距离的比例系数，获得所述目标腔道的特征标识的位姿偏移量。

在一些可能的实现方式中，所述系统还包括输送装置，所述方法还包括：

所述控制装置接收所述双目相机反馈的所述输送装置和所述目标腔道的距离；

所述控制装置根据所述输送装置和所述目标腔道的距离，调节所述机械臂的位置。

在一些可能的实现方式中，所述控制装置根据所述输送装置和所述目标腔道的距离，调节所述机械臂的位置，包括：

所述控制装置根据所述机械臂运动后的空间位置和图像信息确定所述输送装置和所述目标腔道之间的空间关系；

所述控制装置根据所述空间关系以及所述输送装置和所述目标腔道的距离进行数据校验；

所述控制装置在校验结果为不一致时，调整所述机械臂的位置，直至所述校验结果为一致。

第二方面，本申请提供了一种软式内镜操作机器人系统。所述系统包括机械臂、双目相机、单目相机、激光测距传感器组和控制装置；

所述控制装置，用于基于所述双目相机，引导所述机械臂到达手术床体的标志识别位置，使手术床体的标志位于所述单目相机的视野中；

所述激光测距传感器组，用于向所述控制装置反馈所述激光测距传感器组与所述手术床体的距离信息；

所述控制装置，还用于根据所述距离信息调整所述机械臂的位姿。

第三方面，本申请提供了一种控制装置。所述装置包括：

机械臂运动控制模块，用于基于所述双目相机，引导所述机械臂到达手术床体的标志识别位置，使手术床体的标志位于所述单目相机的视野中；

信息采集处理控制模块，用于接收所述激光测距传感器组反馈的、所述激光测距传感器组与所述手术床体的距离信息；

所述机械臂运动控制模块，还用于根据所述距离信息调整所述机械臂的位姿。

在一些可能的实现方式中，所述机械臂运动控制模块具体用于：

控制所述机械臂由安全位置运动至备台位置；

接收所述双目相机识别到的所述机械臂的标志和所述手术床体的标志的空间位置；

根据所述空间位置，对所述机械臂粗定位。

在一些可能的实现方式中，所述系统还包括输送装置，所述激光测距传感器组中的多个激光测距传感器均匀分布在所述输送装置上，所述激光测距传感器组与所述手术床体的距离信息包括每个激光测距传感器与所述手术床体的距离信息；

所述信息采集处理控制模块还用于：

根据所述多个激光测距传感器与所述手术床体的距离信息，确定距离变化量；

所述控制装置根据所述距离变化量，确定所述手术床体的旋转量和偏移量；

所述机械臂运动控制模块具体用于：

根据所述旋转量和所述偏移量调整所述机械臂的位姿。

在一些可能的实现方式中，所述信息采集处理控制模块包括传感器信息采集处理单元和数字模型采集处理单元；

所述传感器信息采集处理单元具体用于：

接收所述激光测距传感器组反馈的、所述激光测距传感器组与所述手术床体的距离信息

所述数字模型采集处理单元用于：

基于所述单目相机采集的目标腔道的特征标识数据建立所述目标腔道的三维模型；

根据所述目标腔道的三维模型以及所述目标腔道的模型模板，获得所述目标腔道的特征标识的位姿偏移量；

所述机械臂运动控制模块具体用于：

根据所述目标腔道的特征标识的位姿偏移量，调整所述机械臂的位姿。

在一些可能的实现方式中，所述数字模型采集处理单元具体用于：

比对所述目标腔道的三维模型以及所述目标腔道的模型模板，获得模型特征之间的位姿信息；

根据模型特征之间的位姿信息以及所述模型模板中单位像素距离与实际距离的比例系数，获得所述目标腔道的特征标识的位姿偏移量。

在一些可能的实现方式中，所述系统还包括输送装置，所述信息采集处理控制模块还用于：

接收所述双目相机反馈的所述输送装置和所述目标腔道的距离；

所述机械臂运动控制模块还用于：

根据所述输送装置和所述目标腔道的距离，调节所述机械臂的位置。

在一些可能的实现方式中，所述控制装置还包括：

空间关系数据生成模块，用于：根据所述机械臂运动后的空间位置和图像信息确定所述输送装置和所述目标腔道之间的空间关系；

所述空间关系数据生成模块还用于：

根据所述空间关系以及所述输送装置和所述目标腔道的距离进行数据校验；

所述机械臂运动控制模块具体用于：

在校验结果为不一致时，调整所述机械臂的位置，直至所述校验结果为一致。

第四方面，本申请提供了一种计算设备。所述计算设备包括处理器和存储器，所述存储器中存储有指令，所述处理器执行所述指令，以使所述计算设备执行如本申请第一方面或第一方面的任一种实现方式所述的方法中由控制装置执行的步骤。

第五方面，本申请提供了一种计算机可读存储介质。所述计算机可读存储介质中存储有指令，当其在计算设备上运行时，使得计算设备执行上述第一方面或第一方面的任一种实现方式所述的方法中由控制装置执行的步骤。

第六方面，本申请提供了一种计算机程序产品。所述计算机程序产品包括计算机可读指令，当其在计算设备上运行时，使得所述计算设备执行上述第一方面或第一方面的任一种实现方式所述的方法中由控制装置执行的步骤。

本申请在上述各方面提供的实现方式的基础上，还可以进行进一步组合以提供更多实现方式。

基于上述内容描述，可知本申请的技术方案具有如下有益效果：

具体地，软式内镜操作机器人系统中引入控制装置、双目相机、单目相机和激光测距传感器组，控制装置基于双目相机，引导机械臂到达手术床体标志的识别位置，使手术床体的标志位于单目相机的视野中，然后控制装置接收所述激光测距传感器组反馈的、所述激光测距传感器组与所述手术床体的距离信息，根据所述距离信息调整所述机械臂的位姿。如此，医护人员可以无需人工拖动机械臂寻找合适手术位置，缩短了每台手术或每次检查时间，降低了成本。

附图说明

结合附图并参考以下具体实施方式，本申请各实施例的上述和其他特征、优点及方面将变得更加明显。贯穿附图中，相同或相似的附图标记表示相同或相似的元素。应当理解附图是示意性的，原件和元素不一定按照比例绘制。

图1为本申请实施例提供的一种软式内镜操作机器人系统的结构示意图；

图2为本申请实施例提供的一种单目结构光相机模型模板的示意图；

图3为本申请实施例提供的一种双目相机引导视野的示意图；

图4为本申请实施例提供的一种被测标识的示意图；

图5为本申请实施例提供的一种激光测距传感器的示意图；

图6为本申请实施例提供的一种单目相机的示意图；

图7为本申请实施例提供的一种控制装置的结构示意图；

图8为本申请实施例提供的一种确定旋转量和偏移量的示意图；

图9为本申请实施例提供的一种确定位姿偏移量的示意图；

图10为本申请实施例提供的一种信息采集处理控制模块的工作原理图；

图11为本申请实施例提供的一种机械臂运动控制模块的工作原理图；

图12为本申请实施例提供的一种空间关系数据生成模块的工作原理图；

图13为本申请实施例提供的一种机械臂定位方法的流程图。

具体实施方式

下面将参照附图更详细地描述本申请的实施例。虽然附图中显示了本申请的某些实施例，然而应当理解的是，本申请可以通过各种形式来实现，而且不应该被解释为限于这里阐述的实施例，相反提供这些实施例是为了更加透彻和完整地理解本申请。应当理解的是，本申请的附图及实施例仅用于示例性作用，并非用于限制本申请的保护范围。

本文使用的术语“包括”及其变形是开放性包括，即“包括但不限于”。术语“基于”是“至少部分地基于”。术语“一个实施例”表示“至少一个实施例”；术语“另一实施例”表示“至少一个另外的实施例”；术语“一些实施例”表示“至少一些实施例”。其他术语的相关定义将在下文描述中给出。

需要注意，本申请中提及的“第一”、“第二”等概念仅用于对不同的装置、模块或单元进行区分，并非用于限定这些装置、模块或单元所执行的功能的顺序或者相互依存关系。

需要注意，本申请中提及的“一个”、“多个”的修饰是示意性而非限制性的，本领域技术人员应当理解，除非在上下文另有明确指出，否则应该理解为“一个或多个”。

针对现有的软式内镜操控机器人系统需要医护人员在术前人工拖动机械臂，寻找合适手术位置，导致每台手术或每次检查时间增长，进而增加成本的问题，本申请提供了一种新型的软式内镜操作机器人系统。

在传统的软式内镜操控机器人基础上，本申请的软式内镜操作机器人系统引入了双目相机、单目相机、激光测距传感器组和控制装置。其中，控制装置，用于基于所述双目相机，引导所述机械臂到达手术床体标志的识别位置，使所述手术床体的标志位于所述单目相机的视野中；激光测距传感器组，于向所述控制装置反馈所述激光测距传感器组与所述手术床体的距离信息；所述控制装置，还用于根据所述距离信息调整所述机械臂的位姿。

在本申请的软式内镜操作机器人系统中，控制装置通过与单目相机、双目相机、激光测距传感器组协作，实现了机械臂的自动定位，无需医护人员人工拖动机械臂寻找合适手术位置，缩短了每台手术或每次检查时间，降低了成本。

为了便于理解，下面结合附图对本申请的软式内镜操作机器人系统进行介绍。

参见图1所示的软式内镜操作机器人系统的结构示意图，软式内镜操作机器人系统10包括机械臂101、双目相机102、单目相机103、激光测距传感器组104和控制装置105。其中，机械臂101可以是多自由度机械臂，在图1的示例中，软式内镜操作机器人系统100包括多个多自由度机械臂，例如，软式内镜操作机器人系统100可以包括两个多自由度机械臂。控制装置105可以是软件装置，该软件装置可以部署在计算设备，如台式机等终端上，计算设备执行软件装置的程序代码，从而实现相应的功能。在一些示例中，控制装置105也可以是硬件装置，例如是实现相应功能的计算设备。进一步地，软式内镜操作机器人系统10还包括输送装置106和操作装置107。为了方便使用，软式内镜操作机器人系统10还可以包括机器人台车108。进一步地，软式内镜操作机器人系统10还可以包括医生端控制台109以及配套的工作站110。

在主从控制模式下，医生的操作命令可以被转换为机械臂101、输送装置106或操作装置107的运动。其中，操作装置107用于操作软式内镜弯曲度弯曲，输送装置106用于控制软式内镜进入人体的长度，二者同时控制软式内镜旋转，亦可单独控制，另一个跟随。同时操作装置107能驱动软式内镜配套的器械。

在本实施例中，首先需要进行软式内镜操作机器人系统10标定并建立以单目相机103为机械臂工具中心点的三维工具坐标系，标定双目相机102与机械臂101的手眼关系。其中，机械臂101相当于软式内镜操作机器人系统10的手，相机（如双目相机102）相当于软式内镜操作机器人系统10的眼。

为了使得相机（亦即机器人的眼）与机械臂（亦即机器人的手）坐标系之间建立关系就必须要对机器人与相机坐标系进行标定，该标定过程也就叫做手眼标定。通常机器人的手眼关系分为eye-in-hand以及eye-to-hand两种。 其中，eye-in-hand也就是眼在手上，机器人的视觉系统随着机械臂末端运动，eye -to-hand是眼睛能看到手，但不在手上。单目相机是eye-in-hand，双目是eye-to-hand。

在满足软式内镜操作机器人系统10条件下，单目相机103的结构光模组发射正弦条纹打在目标腔道（如口腔）的特征标志体（简称为标志）上，单目相机103接收由目标腔道（如口腔）的特征标志（简称为标志）体反射回来的正弦条纹，根据相位差建立单目结构光相机模型模板，简称为模型模板，如图2所示。在该实施例中，可以获取到咬口外圆特征与单目相机之间的距离矩阵，并获取标记点距离信息，利用实际特征距离测定模型模板中单位像素距离与实际距离比例系数。

下面对软式内镜操作机器人系统10的组成部分的功能进行介绍。

控制装置105，用于基于所述双目相机102，引导所述机械臂101到达手术床体的标志识别位置，使手术床体的标志位于所述单目相机103的视野中；

激光测距传感器组104，用于向所述控制装置105反馈所述激光测距传感器组104与所述手术床体的距离信息；

控制装置105，还用于根据所述距离信息调整所述机械臂101的位姿。

按术式需求，患者可以采用侧躺或者平躺等方式，备台时（也就是手术前准备），将患者的手术床体推放至指定区域，比如在做上消化道检查时，软式内镜操作机器人系统10中的控制装置105控制机械臂101由安全位置运动至备台位置，然后参见图3，通过双目相机102识别机械臂101的标志体与目标腔道（如口腔）的特征标志体的空间位置，使患者的目标腔道的特征表征体（如咬口）处于机械臂101上附着的单目相机103视野区域内，在调整过程中通过激光测距传感器组104检测到的手术床体距离反馈输送装置106与患者的目标腔道（如口腔）之间距离，调整所述机械臂101的位姿，避免发生位置干涉。

本实施例中，参见图4，手术床体的标志、口腔的标志（特征标识）及激光反射材质校准平面部分，手术床体事务标志为双目相机102识别床体位置标识，需要处于双目相机102的视野中，该标识与口腔具有相对位置关系。口腔的标志（特征标识）为胃镜咬口（简称为咬口），咬口高度及色差明显，易于通过结构光创建模型；激光反射材质校准平面应于手术床体平行，用以激光反馈床体位置。

接着参见图5，激光测距传感器组104包括多个激光测距传感器（也可以简称为激光传感器），例如是两个激光测距传感器。激光测距传感器可以均匀分布在输送装置106上。如图5所示，激光测距传感器通过发出对人体无害的红外激光测量手术床体到输送装置106的距离，使用232串口通讯，用以接收控制装置105的输入指令以及输出激光测距传感器检测到的距离信息。

然后参见图6，单目相机103可以是单目结构光相机，具体包括单目相机和结构光模块。控制装置105可以控制结构光模块发射散斑打在口腔的特征标志体如咬口上，单目相机通过以太网接收控制指令，并获取由口腔的特征标志体如咬口反射回来的正弦条纹图像。该图像可以用于构建口腔的三维模型。

进一步地，控制装置105还用于基于所述单目相机103采集的目标腔道的特征标识数据建立所述目标腔道的三维模型，根据所述目标腔道的三维模型以及所述目标腔道的模型模板，获得所述目标腔道的特征标识的位姿偏移量，根据所述目标腔道的特征标识的位姿偏移量，调整所述机械臂101的位姿。

仍以上消化道示例说明，控制装置105通过单目相机103（例如是单目结构光相机）采集口腔特征标识数据（如咬口反射回的正弦条纹图像）并建立数字模型，该数字模型为口腔的三维模型，然后将该三维模型与预存的单目结构光相机口腔特征标识模型模板进行对比，获取口腔的特征标识的位置姿态信息，该位置姿态信息包括位姿偏移量，控制装置105可以基于单目相机工具坐标系控制机械臂101按照上述位姿偏移量进行位姿调整。

此外，控制装置105还用于接收所述双目相机102反馈的所述输送装置106和所述目标腔道的距离；根据所述输送装置106和所述目标腔道的距离，调节所述机械臂101的位置。其中，控制装置105可以根据所述机械臂101运动后的空间位置和图像信息确定所述输送装置106和所述目标腔道之间的空间关系，根据所述空间关系以及所述输送装置106和所述目标腔道的距离进行数据校验；在校验结果为不一致时，调整所述机械臂101的位置，直至所述校验结果为一致。

继续以上消化道示例说明，控制装置105还用于通过双目相机102获取输送装置106和口腔之间距离，并基于该距离和空间关系进行校验，以完成术前输送机械臂定位工作。

本申请实施例实现自动定位的关键在于控制装置105，下面对控制装置105进行重点说明。

参见图7所示的控制装置105的结构示意图，该控制装置105包括如下模块：

机械臂运动控制模块1052，用于基于所述双目相机102，引导所述机械臂101到达手术床体的标志识别位置，使手术床体的标志位于所述单目相机103的视野中；

信息采集处理控制模块1054，用于接收所述激光测距传感器组104反馈的、所述激光测距传感器组104与所述手术床体的距离信息；

所述机械臂运动控制模块1052，还用于根据所述距离信息调整所述机械臂101的位姿。

其中，机械臂运动控制模块1052、信息采集处理控制模块1054可以是软件模块，也可以是硬件模块，例如可以是部署有相应软件的计算设备。

具体实现时，所述机械臂运动控制模块1052具体用于控制所述机械臂由安全位置运动至备台位置，接收所述双目相机识别到的所述机械臂的标志和所述手术床体的标志的空间位置，根据所述空间位置，对所述机械臂粗定位。

其中，所述激光测距传感器组10中的多个激光测距传感器均匀分布在所述输送装置106上，所述激光测距传感器组104与所述手术床体的距离信息包括每个激光测距传感器与所述手术床体的距离信息。

相应地，所述信息采集处理控制模块1054在接收到激光测距传感器组104反馈，还用于根据所述多个激光测距传感器与所述手术床体的距离信息，确定距离变化量，然后根据所述距离变化量，确定所述手术床体的旋转量和偏移量。如此，机械臂运动控制模块1052可以根据所述旋转量和所述偏移量调整所述机械臂的位姿。

其中，信息采集处理控制模块1054可以包括块包括传感器信息采集处理单元1054-2和数字模型采集处理单元1054-4。下面结合附图对确定旋转量和偏移量过程进行详细说明。参见图8所示的确定旋转量和偏移量的示意图，传感器信息采集处理单元1054-2用以接收通过232通讯模块输入的两个均匀分布在输送装置的激光测距传感器测量到手术床体的距离信息。

其中,第一个激光测距传感器点位信息为Q1( x ₁ ,y ₁ )，位于机械臂选定工具坐标系末端点,接受到的距离信息为 D ₁ ，第二个激光测距传感器点位信息为Q2( x ₂ ,y ₂ ),接受到的距离信息为 D ₂ ，传感器信息采集处理单元1054-2可以根据 D ₁ 和 D ₂ 得到距离变化量，具体为：

（1）

两个激光测距传感器的均匀位置可以表示为：

（2）

相应地，手术床体基于机械臂工具坐标系末端点绕Z轴的旋转量为：

（3）

机械臂101调整旋转后手术床体与输送装置106距离为：

（4）

其中， L ₁ 为输送装置到激光测距传感器X方向长度，将该信息实时反馈监控，可以避免发生机械臂101与手术床体空间干涉。

数字模型采集处理单元1054-4用于基于所述单目相机103采集的目标腔道的特征标识数据建立所述目标腔道的三维模型（单目结构光相机模型），根据所述目标腔道的三维模型以及所述目标腔道的模型模板（单目结构光相机模型模板），获得所述目标腔道的特征标识的位姿偏移量。

具体地，数字模型采集处理单元1054-4可以比对所述目标腔道的三维模型以及所述目标腔道的模型模板，获得模型特征之间的位姿信息，根据模型特征之间的位姿信息以及所述模型模板中单位像素距离与实际距离的比例系数，获得所述目标腔道的特征标识的位姿偏移量。

相应地，机械臂运动控制模块1052具体用于：根据所述目标腔道的特征标识的位姿偏移量，调整所述机械臂的位姿。

下面结合附图对确定位姿偏移量过程进行详细说明。参见图9所示的确定位姿偏移量的示意图，结构光模块发射正弦条纹打在口腔特征标志体上，单目相机103接收由口腔特征标志体反射回来的正弦条纹，根据相位差建立单目结构光相机模型，数字模型采集处理单元1054-4然后数字模型采集处理单元1054-4将对比预存单目结构光相机模型模板，抽取条纹扭曲边界，根据拟合口腔咬口圆心，计算圆心位置偏差,利用单位像素距离与实际距离比例系数修正偏差模型特征，得到与位置重合的数学模型，根据拟合口腔圆的圆度及特征相位差，如下图，按照ZYX旋序首先计算将、、、绕Z轴旋转到模板XY坐标轴的角度值，根据、对比、之间的距离差确认旋转方向以及旋转角度，绕Y轴旋转，根据、对比、之间的距离差确认旋转方向以及旋转角度，绕X轴旋转，得到口腔与输送装置之间基于单目相机工具坐标系的位姿变化关系。

如图10所示，信息采集处理控制模块1054，用以保证得到正确数据以及数字模型信息，在双目相机引导机械臂101运动过程中，控制激光测距传感器进行信息数据采集，反馈实时距离信息，当前检测距离小于设定值，对机械臂进行急停处理，以确保机械臂与手术床不发生干涉；双目相机102引导机械臂101运动结束后，进行单目结构光数字模型采集，经数字模型采集处理单元1054-4得到位姿变化关系，发送至机械臂运动控制模块1052执行相应运动，经过多次校准，使得到的数字模型信息与预存数字模型信息模板匹配以达到机械臂手术预备姿态位置。

需要说明的是，如图11所示，本申请实施例的机械臂运动控制模块1052用以接收信息采集处理控制模块1054输入的位置信息，通过机器人逆运动学使机械臂101移动或旋转到相应位姿，并返回到达目标位置信息。信息采集处理控制模块1054向机械臂运动控制模块1052发送运动控制指令，机械臂运动控制模块1052返回执行结果，直至信息采集处理控制模块1054停止发送运动控制指令，位置调节完毕。

其中，信息采集处理控制模块1054可以接收所述双目相机反馈的所述输送装置和所述目标腔道的距离；所述机械臂运动控制模块1052可以根据所述输送装置和所述目标腔道的距离，调节所述机械臂的位置。

在本实施例中，控制装置105还可以包括空间关系数据生成模块1056。空间关系数据生成模块1056用于根据所述机械臂运动后的空间位置和图像信息确定所述输送装置和所述目标腔道之间的空间关系。空间关系数据生成模块1056还用于根据所述空间关系以及所述输送装置和所述目标腔道的距离进行数据校验。所述机械臂运动控制模块1052在校验结果为不一致时，调整所述机械臂的位置，直至所述校验结果为一致。

以上消化道示例说明。如图12所示，空间关系数据生成模块1056，用以根据所述机械臂运动后空间位置和图像信息计算输送装置输送口与口腔之间的空间关系，通过双目相机特征对比获取输送装置输送口与口腔之间的距离，进行数据校核。

基于上述内容描述，本申请实施例的软式内镜操作机器人系统10通过控制装置105可以实现自动定位机械臂101，并且定位精确，操作精简有效，减轻医护人员的劳动强度，解决双目相机102定位视野内靶点被遮挡问题。

基于本申请实施例提供的软式内镜操作机器人系统10，本申请实施例还提供相应的机械臂定位方法。

参见图13所示的机械臂定位方法的流程图，该方法包括如下步骤：

S1302：控制装置105基于所述双目相机102，引导所述机械臂101到达手术床体的标志识别位置，使所述手术床体的标志位于所述单目相机的视野中。

具体地，控制装置105控制所述机械臂由安全位置运动至备台位置，接收所述双目相机102识别到的所述机械臂101的标志和所述手术床体的标志的空间位置，然后根据所述空间位置，对所述机械臂101粗定位。

S1304：控制装置105接收所述激光测距传感器组104反馈的、所述激光测距传感器组104与所述手术床体的距离信息，根据所述距离信息调整所述机械臂101的位姿。

激光测距传感器组104中的多个激光测距传感器均匀分布在所述输送装置106上，激光测距传感器组104与所述手术床体的距离信息包括每个激光测距传感器与所述手术床体的距离信息。

相依地，控制装置105可以根据所述多个激光测距传感器与所述手术床体的距离信息，确定距离变化量，然后根据所述距离变化量，确定所述手术床体的旋转量和偏移量，接着根据所述旋转量和所述偏移量调整所述机械臂101的位姿。

S1306：控制装置105基于所述单目相机采集的目标腔道的特征标识数据建立所述目标腔道的三维模型。

具体建模过程可以参见上文相关内容描述，在此不再赘述。

S1308：控制装置105根据所述目标腔道的三维模型以及所述目标腔道的模型模板，获得所述目标腔道的特征标识的位姿偏移量。

控制装置105可以比对所述目标腔道的三维模型以及所述目标腔道的模型模板，获得模型特征之间的位姿信息，然后根据模型特征之间的位姿信息以及所述模型模板中单位像素距离与实际距离的比例系数，获得所述目标腔道的特征标识的位姿偏移量。

S1310：控制装置105根据所述目标腔道的特征标识的位姿偏移量，调整所述机械臂的位姿。

S1312：控制装置105接收所述双目相机102反馈的所述输送装置106和所述目标腔道的距离。

S1314：控制装置105根据所述输送装置106和所述目标腔道的距离，调节所述机械臂101的位置。

具体地，控制装置105可以根据所述机械臂运动后的空间位置和图像信息确定所述输送装置和所述目标腔道之间的空间关系，然后根据所述空间关系以及所述输送装置和所述目标腔道的距离进行数据校验；在校验结果为不一致时，控制装置105调整所述机械臂101的位置，直至所述校验结果为一致。

上述S1306至S1314为本申请实施例的可选步骤，执行本申请实施例的方法也可以不执行上述步骤。

在该方法中，软式内镜操作机器人系统10中引入控制装置105、双目相机102、单目相机103和激光测距传感器组104，控制装置105基于双目相机，引导机械臂101到达手术床体标志的识别位置，使手术床体的标志位于单目相机103的视野中，然后控制装置105接收所述激光测距传感器组104反馈的、所述激光测距传感器组104与所述手术床体的距离信息，根据所述距离信息调整所述机械臂101的位姿。如此，医护人员可以无需人工拖动机械臂寻找合适手术位置，缩短了每台手术或每次检查时间，降低了成本。

本申请实施例还提供了一种计算设备，该计算设备可以是云计算设备，或者是边缘计算设备，包括：处理器、存储器、系统总线；

处理器以及存储器通过系统总线相连；

存储器用于存储一个或多个程序，其中，一个或多个程序包括指令，该指令当被处理器执行时使处理器执行上述机械臂定位方法中由控制装置105执行的步骤。

本申请还提供一种计算机可读存储介质，也称作机器可读介质。在本申请的上下文中，机器可读介质可以是有形的介质，其可以包含或存储以供指令执行系统、装置或设备使用或与指令执行系统、装置或设备结合地使用的程序。机器可读介质可以是机器可读信号介质或机器可读储存介质。机器可读介质可以包括但不限于电子的、磁性的、光学的、电磁的、红外的、或半导体系统、装置或设备，或者上述内容的任何合适组合。机器可读存储介质的更具体示例会包括基于一个或多个线的电气连接、便携式计算机盘、硬盘、随机存取存储器（RAM）、只读存储器（ROM）、可擦除可编程只读存储器（EPROM或快闪存储器）、光纤、便捷式紧凑盘只读存储器（CD-ROM）、光学储存设备、磁储存设备、或上述内容的任何合适组合。

需要说明的是，本申请上述的计算机可读介质可以是计算机可读信号介质或者计算机可读存储介质或者是上述两者的任意组合。计算机可读存储介质例如可以是——但不限于——电、磁、光、电磁、红外线、或半导体的系统、装置或器件，或者任意以上的组合。计算机可读存储介质的更具体的例子可以包括但不限于：具有一个或多个导线的电连接、便携式计算机磁盘、硬盘、随机访问存储器（RAM）、只读存储器（ROM）、可擦式可编程只读存储器（EPROM或闪存）、光纤、便携式紧凑磁盘只读存储器（CD-ROM）、光存储器件、磁存储器件、或者上述的任意合适的组合。在本申请中，计算机可读存储介质可以是任何包含或存储程序的有形介质，该程序可以被指令执行系统、装置或者器件使用或者与其结合使用。而在本申请中，计算机可读信号介质可以包括在基带中或者作为载波一部分传播的数据信号，其中承载了计算机可读的程序代码。这种传播的数据信号可以采用多种形式，包括但不限于电磁信号、光信号或上述的任意合适的组合。计算机可读信号介质还可以是计算机可读存储介质以外的任何计算机可读介质，该计算机可读信号介质可以发送、传播或者传输用于由指令执行系统、装置或者器件使用或者与其结合使用的程序。计算机可读介质上包含的程序代码可以用任何适当的介质传输，包括但不限于：电线、光缆、RF（射频）等等，或者上述的任意合适的组合。

上述计算机可读介质承载有一个或者多个程序，当上述一个或者多个程序被该服务器执行时，使得服务器：按照当前调度模式调起任务，获取任务需要处理的第一数据，并处理所述第一数据；根据所述第一数据的数据量以及所述任务对应的预设阈值，设置所述任务在下一阶段的目标调度模式；按照所述目标调度模式，获取任务需要处理的第二数据，并处理所述第二数据。

特别地，根据本申请的实施例，上文参考流程图描述的过程可以被实现为计算机软件程序。例如，本申请的实施例包括一种计算机程序产品，其包括承载在非暂态计算机可读介质上的计算机程序，该计算机程序包含用于执行流程图所示的方法的程序代码。在这样的实施例中，该计算机程序可以通过通信装置从网络上被下载和安装，或者从存储装置被安装。在该计算机程序被处理装置执行时，执行本申请实施例的方法中限定的上述功能。

尽管已经采用特定于结构特征和/或方法逻辑动作的语言描述了本主题，但是应当理解所附权利要求书中所限定的主题未必局限于上面描述的特定特征或动作。相反，上面所描述的特定特征和动作仅仅是实现权利要求书的示例形式。

虽然在上面论述中包含了若干具体实现细节，但是这些不应当被解释为对本申请的范围的限制。在单独的实施例的上下文中描述的某些特征还可以组合地实现在单个实施例中。相反地，在单个实施例的上下文中描述的各种特征也可以单独地或以任何合适的子组合的方式实现在多个实施例中。

以上描述仅为本申请的较佳实施例以及对所运用技术原理的说明。本领域技术人员应当理解，本申请中所涉及的公开范围，并不限于上述技术特征的特定组合而成的技术方案，同时也应涵盖在不脱离上述公开构思的情况下，由上述技术特征或其等同特征进行任意组合而形成的其它技术方案。例如上述特征与本申请中公开的（但不限于）具有类似功能的技术特征进行互相替换而形成的技术方案。

Claims (13)

1.一种机械臂定位方法，其特征在于，应用于软式内镜操作机器人系统，所述系统包括机械臂、双目相机、单目相机、激光测距传感器组和控制装置，所述方法包括：

所述控制装置基于所述双目相机，引导所述机械臂到达手术床体的标志识别位置，使所述手术床体的标志位于所述单目相机的视野中；

所述控制装置接收所述激光测距传感器组反馈的、所述激光测距传感器组与所述手术床体的距离信息，根据所述距离信息调整所述机械臂的位姿。

2.根据权利要求1所述的方法，其特征在于，所述控制装置基于所述双目相机，引导所述机械臂到达手术床体的标志识别位置，包括：

所述控制装置控制所述机械臂由安全位置运动至备台位置；

所述控制装置接收所述双目相机识别到的所述机械臂的标志和所述手术床体的标志的空间位置；

所述控制装置根据所述空间位置，对所述机械臂粗定位。

3.根据权利要求1所述的方法，其特征在于，所述系统还包括输送装置，所述激光测距传感器组中的多个激光测距传感器均匀分布在所述输送装置上，所述激光测距传感器组与所述手术床体的距离信息包括每个激光测距传感器与所述手术床体的距离信息；

所述控制装置根据所述距离信息调整所述机械臂的位姿，包括：

所述控制装置根据所述多个激光测距传感器与所述手术床体的距离信息，确定距离变化量；

所述控制装置根据所述距离变化量，确定所述手术床体的旋转量和偏移量；

所述控制装置根据所述旋转量和所述偏移量调整所述机械臂的位姿。

4.根据权利要求1至3任一项所述的方法，其特征在于，所述方法还包括：

所述控制装置基于所述单目相机采集的目标腔道的特征标识数据建立所述目标腔道的三维模型；

所述控制装置根据所述目标腔道的三维模型以及所述目标腔道的模型模板，获得所述目标腔道的特征标识的位姿偏移量；

所述控制装置根据所述目标腔道的特征标识的位姿偏移量，调整所述机械臂的位姿。

5.根据权利要求4所述的方法，其特征在于，所述控制装置根据所述目标腔道的三维模型以及所述目标腔道的模型模板，获得所述目标腔道的特征标识的位姿偏移量，包括：

所述控制装置比对所述目标腔道的三维模型以及所述目标腔道的模型模板，获得模型特征之间的位姿信息；

所述控制装置根据模型特征之间的位姿信息以及所述模型模板中单位像素距离与实际距离的比例系数，获得所述目标腔道的特征标识的位姿偏移量。

6.根据权利要求1至3任一项所述的方法，其特征在于，所述系统还包括输送装置，所述方法还包括：

所述控制装置接收所述双目相机反馈的所述输送装置和目标腔道的距离；

所述控制装置根据所述输送装置和所述目标腔道的距离，调节所述机械臂的位置。

7.根据权利要求6所述的方法，其特征在于，所述控制装置根据所述输送装置和所述目标腔道的距离，调节所述机械臂的位置，包括：

所述控制装置根据所述机械臂运动后的空间位置和图像信息确定所述输送装置和所述目标腔道之间的空间关系；

所述控制装置根据所述空间关系以及所述输送装置和所述目标腔道的距离进行数据校验；

所述控制装置在校验结果为不一致时，调整所述机械臂的位置，直至所述校验结果为一致。

8.一种软式内镜操作机器人系统，其特征在于，所述系统包括机械臂、双目相机、单目相机、激光测距传感器组和控制装置；

所述控制装置，用于基于所述双目相机，引导所述机械臂到达手术床体标志的识别位置，使所述手术床体的标志位于所述单目相机的视野中；

所述激光测距传感器组，用于向所述控制装置反馈所述激光测距传感器组与所述手术床体的距离信息；

所述控制装置，还用于根据所述距离信息调整所述机械臂的位姿。

9.一种控制装置，其特征在于，所述装置包括：

机械臂运动控制模块，用于基于双目相机，引导所述机械臂到达手术床体标志的识别位置，使所述手术床体的标志位于单目相机的视野中；

信息采集处理控制模块，用于接收激光测距传感器组反馈的、所述激光测距传感器组与所述手术床体的距离信息；

所述机械臂运动控制模块，还用于根据所述距离信息调整所述机械臂的位姿。

10.根据权利要求9所述的装置，其特征在于，所述信息采集处理控制模块包括传感器信息采集处理单元和数字模型采集处理单元；

所述传感器信息采集处理单元具体用于：

接收所述激光测距传感器组反馈的、所述激光测距传感器组与所述手术床体的距离信息

所述数字模型采集处理单元用于：

基于所述单目相机采集的目标腔道的特征标识数据建立所述目标腔道的三维模型；

根据所述目标腔道的三维模型以及所述目标腔道的模型模板，获得所述目标腔道的特征标识的位姿偏移量；

所述机械臂运动控制模块具体用于：

根据所述目标腔道的特征标识的位姿偏移量，调整所述机械臂的位姿。

11.一种计算设备，其特征在于，所述计算设备包括处理器和存储器，所述存储器中存储有指令，所述处理器执行所述指令，以使所述计算设备执行如权利要求1至7任一项所述的方法中由控制装置执行的步骤。

12.一种计算机可读存储介质，其特征在于，包括计算机可读指令，当所述计算机可读指令在计算设备上运行时，使得服务器执行如权利要求1至7中任一项所述的方法中由控制装置执行的步骤。

13.一种计算机程序产品，其特征在于，包括计算机可读指令，当所述计算机程序产品在计算设备上运行时，使得所述计算执行如权利要求1至7中任一项所述的方法中由控制装置执行的步骤。

Images