CN111906784B - 一种基于机器视觉引导的咽拭子双臂采样机器人及采样方法 - Google Patents

Abstract

本发明公开了一种基于机器视觉引导的咽拭子双臂采样机器人及采样方法，本发明提供了一种基于机器视觉引导的咽拭子双臂采样机器人及采样方法，该机器人系统利用工业深度相机获取RGB‑D深度点云信息，对口腔空间环境进行识别处理，采用深度学习算法，确定舌尖与咽喉深部的三维坐标作为双臂的期望坐标，基于moveit功能包，完成移动双臂的协同运动规划，机器人控制系统则是基于ROS系统开发，通过人机交互控制与ROS无线通信，实现远程控制及实时监控功能。本发明能够避免医护人员与患者体内传染物直接接触，监控双臂机器人完成繁杂的咽拭子采样过程，并确保较高的执行精度与成功率，避免患者出现采样过程受伤等意外风险。

Description

一种基于机器视觉引导的咽拭子双臂采样机器人及采样方法

技术领域

本发明属于视觉控制领域，涉及一种基于机器视觉引导的咽拭子双臂采样机器人及采样方法。

背景技术

鉴于新型冠状病毒具有人际传播能力，而经呼吸道飞沫和密切接触为其主要的传播途径。目前核酸检查是新型冠状病毒的主要检测手段，必须对患者进行咽拭子采样，在其过程中，需要与病患近距离接触，由于患者呼吸，咳嗽所产生得大量病毒传染物，不利于医务人员的身体健康，且咽拭子采样复杂，医务人员的心情也会影响到拭子的采样质量。

介于目前情况，急需有一款针对咽拭子采样的智能机器人，能够避免医务人员与患者直接接触，降低医护人员感染率。

发明内容

本发明提供了一种基于机器视觉引导的咽拭子双臂采样机器人及采样方法，其目的在于，通过该机器人自主完成复杂的咽拭子采样和归纳任务，避免医务人员直接与患者接触，并实时远端监控。

本发明提供的技术方案如下：

一方面，一种基于机器视觉引导的双臂机器人咽拭子采样方法，包括以下步骤：

步骤1：咽拭子采样准备，双臂机器人夹取压舌板和纯净棉签；

步骤2：患者口腔场景建模，利用深度相机扫描患者口腔环境，获取口腔点云数据，构建口腔三维场景；

步骤3：基于端到端的深度PointNet++网络进行口腔场景分割与定位；

对构建的口腔场景采用PointNet++网络进行分割，获取舌头、牙齿、扁桃体、上颚器官的分割图，并对分割图进行定位，获得舌头与扁桃体的空间位置坐标，并作为双臂机器人的双臂采样期望坐标；

步骤4：基于双臂采样期望坐标，调用moveit功能包对双臂机器人的双臂进行运动规划，获得双臂运动轨迹；

步骤5：利用获得的双臂运动轨迹，控制双臂机器人的双臂到达采样期望坐标进行采样。

进一步地，所述对构建的口腔场景采用的端到端的PointNet++网络进行分割，获取舌头、牙齿、扁桃体、上颚器官的分割图的具体过程如下：

步骤3.1：首先依据PointNet++网络构建pointnet_sem_seg网络代码，执行python训练代码，并安装pytorch深度学习框架，载入三维口腔点云数据集，训练PointNet++网络获取权重；

其中，训练集点数为4096，batchsize＝16，epoch＝200,learning_rate＝0.01，学习率设置为每经过20步衰减0.5倍，训练数据为深度相机读取到的口腔点云数据，输出的点云分割结果使用MeshLab软件可视化；

步骤3.2：利用训练好的PointNet++网络，输入采集的口腔点云，获取到点云分割后提取出舌头部位点云和扁桃体部位点云，以点云的表面中心坐标为最佳的目标点作为机械臂期望坐标位置，同时使用深度相机对该目标点计算三维距离(Xc,Yc,Zc)；

所述PointNet++网络包括级联的多尺度PointnetSAModuleMSG和上采样网络PointnetFPModule；

其中，所述PointnetSAModuleMSG包括级联的采样层Sample layer、组合层Grouping layer以及特征提取层PointNet layer；

采样层Sample layer用于对输入的点云数据进行farthest point sampling采样，从采样点中选取若干中心点，输入三维张量为(B,N,3)，输出张量(B,npoint)，B为批次大小，N为点云数据集中的点云总数，3代表点的坐标特征xyz，npoint表示采样输出的中心点数量；

组合层Grouping layer用于通过采样层Sample layer获得的中心点与其局部点构成若干区域，设置区域球半径大小radius，区域采样点的上限nsample，控制球规模及点个数，通过Ball query算法生成若干局部区域，返回表示局部区域特征的索引张量(B,npoint,nsample)；

nsample是每个区域的采样点数量的上限，其大小为16或者32，对应两种radius，总共有4层PointnetSAModuleMSG，在每层PointnetSAModuleMSG中，nsample的取值不同；

先设定局部区域再进行特征提取，局部区域是通过Ball query算法，通过设置的radius即球半径大小确定区域大小，每个区域的点是通过farthest point sampling采样获得的，直至采样的数量达到上限nsample。

而因为口腔的点云的分布是密度不均匀的，故此选择多尺度的方式，因此每一层PointnetSAModuleMSG都有两种尺度的区域，即两个半径值和两个采样上限值；

特征提取层PointNet layer用于对每个局部区域的特征使用PointNet提取特征向量；

为实现不同对象咽拭子采样的泛化能力及采样过程的时效性，传统的2D视觉抓取技术无法满足，该视觉伺服控制系统考虑采用新型3D视觉抓取技术，使用了一种端到端的深度学习方法PointNet++网络，可直接处理视觉采集装置捕获口腔点云数据。

对于采样方式，使用farthest point sampling算法，从点云数据中采样出npoint个点数，相比于随机采样算法，能够更好的覆盖到整个点云数据集；

Pointnet主要由三部分模块组成，解决旋转和平移不变性的t-net：训练t-net网络得到转换矩阵与输入点云数据相乘实现对齐操作，多层感知机MLP：对点云数据做低维度到高纬度的空间映射，以及最大池化函数max pool的对称性质解决点云无序的问题。

通过对上述的三个模块级联组合实现类似于CNN从浅层特征到深层的语义特征的提取；输出至上采样网络实现实例分割；

所述上采样网络PointnetFPModul对PointnetSAModuleMSG的输出的全局特征进行上采样，生成新的点，当点的个数只有一个的时候，直接将其重复复制成N个点；当点的个数大于一个的时候，采用基于K最近邻的线性差值的方式进行上采样，再对上采样后的每一个点都做一个MLP，同时拼接上下采样前相同点个数的SA层的特征；

线性插值公式如下：

k表示选取的最相邻的样本数量，p表示最相邻距离的幂，设置p＝2，k＝3，x表示内插值中的任意一点，x_i表示内插值中的已知点，d(x,x_i)表示任意一点x和已知点x_i之间距离，w_i(x)表示任意一点x权重值，取距离倒数使得距离越远的点权重越小；

p的值越大距离影响的效果就越明显，最后对每一个点的权重实现全局的归一化。

主要通过线性差值与MLP堆叠完成。

进一步地，所述对双臂机器人的双臂进行运动规划时，采用A*启发式搜索算法进行双臂无碰撞路径规划，具体过程如下：

给定双臂机器人的初始位姿、目标位姿以及双臂机器人和环境的几何描述，通过取当前路径点周围邻位中启发式函数值最小的邻位来确定下一个路径点，最终得到一条初始点到目标点的路径。

运动学插件使用IKFAST，进行碰撞检测后，采用action通信机制输出机械臂的末端到期望坐标的关节轨迹数据，并通过RVIZ插件接口，可视化界面实时地同步机械臂地运动过程，机械臂端通过配置插口接收到轨迹后使用五次样条插补运算，实现位置、速度、加速的连续，并发布到每一个电机位置控制接口，实现每一个电机的运转，同时以topic通信机制50hz频率反馈给moveit各个关节状态信息。

一种基于机器视觉引导的咽拭子采样双臂机器人，包括设置在移动机器人本体上的视觉采集模块、移动平台控制模块、机械双臂控制模块、数据处理模块、远端控制模块、电源模块及咽拭子采样模块；

其中，视觉采集模块、机械双臂控制模块与数据处理模块均以网线连接，所述移动平台控制模块、机械双臂控制模块受控于远端控制模块，所述移动机器人本体设置有双臂，所述咽拭子采样模块设置在移动机器人本体的双臂上，且受控于机械双臂控制模块，所述电源模块为移动机器人本体供电；

所述数据处理模块采用上述的一种基于机器视觉引导的双臂机器人咽拭子采样方法进行数据处理。

进一步地，所述移动机器人本体的移动底盘采用麦克纳姆轮结构。

可实现360度无死角全向运动，灵活性较高；其负载大于50KG，人体测温捕捉距离在30米内，机器人测温机构可实现负40度到正60度的俯仰区间。

进一步地，所述移动机器人的双臂采用六轴12自由度，臂长200mm-400mm，负载能力大于3KG，定位精度在0.2mm误差内的机械臂，且机械臂末端采用柔性采样头。

实现柔性抓取，采样精度控制在0.1mm误差内。

进一步地，所述视觉采集模块，为设置在移动机器人本体的移动平台前端的kinect深度相机，通过扫描口腔环境，采集点云数据，对口腔场景建模，并将点云pcd文件发送给数据处理模块。

所述数据处理模块，指咽拭子机器人操作系统，与生产者电脑连接，利用深度神经网络PointNet++，对口腔点云数据分析处理，获得口腔点云分割图，以舌头与扁桃体的点云表面中心坐标作为舌尖与咽喉位置坐标，但此时的位置坐标是相对于相机坐标系下，将其转换至机械臂坐标系并将此位置坐标发送给ROS机器人控制系统，通过系统中节点管理器Master与所有节点进行数据交互，主导机械臂的运动；

所述移动平台模块，指机器人全向移动底盘结构，包括底层控制器、驱动器、外部通信接口和全向移动平台，全向移动平台采用麦克纳姆轮采用麦克纳姆轮结构，可实现360度无死角全向运动，所述底层控制器用于控制电机速度及移动平台的转动角度，通过CANopen总线与驱动器进行通信，从而控制机器人的运动，所述驱动器，用于驱动移动平台运动，同时，还从控制器中引出了USB和网口链接外部设备与远端控制模块连接，实现实时远端监控；

机械手臂模块包括机械手臂控制器和移动机械双臂和机械手臂末端相机，不同于深度相机，所述末端相机将实时拍摄机械臂末端于口腔中的采样过程，与显示屏相连远端实时观测，所述移动机械臂选用双臂6轴12自由度机械双臂机器人，臂长200mm-400mm其负载能力大于3KG，机械臂控制器与数据模块连接，借助ROS系统moveit功能包控制机械臂发布控制指令指定机械臂移动至期望位置；

进一步地，移动机器人的移动平台和双臂由直流无刷伺服电机带减速机驱动，所述电源模块为24V，40AH的锂电池组。

进一步地，所述咽拭子采样模块，包括固定于移动机器人本体的移动底盘上的压舌板、棉签、试管，以及固定患者口腔的固定装置。

进一步地，所述远端控制模块，包括显示屏及工控机，通过5GWIFI信号连接到本地路由器。

即与远程控制器在同一个路由器下，通过远程API可直接控制移动地盘和机械臂，借助相机拍摄咽拭子采集过程，实现远端监控调节，出现意外情况可及时中断，同时通过人机交互界面操作数据处理模块主机直接控制移动地盘及机械臂运动。

有益效果

本发明提供了一种基于机器视觉引导的咽拭子双臂采样机器人及采样方法，该机器人系统利用工业深度相机获取RGB-D深度点云信息，对口腔空间环境进行识别处理，采用深度学习算法，确定舌尖与咽喉深部的三维坐标作为双臂的期望坐标，基于moveit功能包，完成移动双臂的协同运动规划，机器人控制系统则是基于ROS系统开发，通过人机交互控制与ROS无线通信，实现远程控制及实时监控功能。

本发明相较于现有技术而言，具有以下优点：

1.本发明借由最新3D分割技术，直接使用原始点云数据，经由PointNet++网络处理，获取期望坐标传递给机械臂实行运动规划，相比于过往的点云数据处理方法，大多为获取原始点云鸟瞰图，利用2D的图像方法处理，如FasterRNN，MaskRNN，这样将损失3D结构信息，且过分依赖于2D的检测结果，误差较大，而将原始点云数据转化为体素型，转而使用3D神经网络的方式，会造成计算量的大幅度增加，无法适用于多物体和复杂背景，且分辨率较低，无法满足基本要求。比较而言，PointNet++是一种端到端的处理方法，利用数学对称函数的思想有效的解决了点云数据的无序性问题，且计算量更小，在网络中引入t-net得到转换矩阵与输入点云数据相乘实现对齐操作,解决点云数据的旋转和平移不变性.在局部区域中反复使用PointNet提取点云特征，改善了局部关系的缺失，实现了多级特征学习，以达到精准的3D分割。

2.本发明使用双臂协同机器人，相比于单臂而言，双臂机器人自由度更大，作业空间更广，能够实现精准协作完成更为复杂的任务需求，但协作算法更为复杂。介于咽拭子采样过程，存在压舌取样，样本归纳等协同作业需求，需要同时作业多个物体，使用单臂实现，时效性过低且过程冗杂，难以解决口腔小空间下作业的困难挑战，双臂具有更强大的灵活性和操作能力。

3.针对人数激增的疫情形势及人传人的危险状况，本发明能够避免医护人员与患者体内传染物直接接触，监控双臂机器人完成繁杂的咽拭子采样过程，并确保较高的执行精度与成功率，通过有效的力反馈技术，避免患者出现采样过程受伤等意外风险。可有效地投入到疫情工作中，解决医疗人员紧缺及被感染的危险可能，缓解全球疫情压力。

附图说明

图1为本发明实例所述的一种基于机器视觉引导的咽拭子采样双臂机器人结构框图；

图2为机器人作业流程图；

图3为处理口腔点云数据网络结构图；

图4为机械双臂协同控制策略；

图5为机器人多视角机械机构图，其中，(a)为左视图，(b)为主视图，(c)为俯视图。

具体实施方式

下面将结合附图和实施例对本发明做进一步的说明。

如图1和图5所示，一种基于机器视觉引导的咽拭子采样双臂机器人，包括设置在移动机器人本体上的视觉采集模块、移动平台控制模块、机械双臂控制模块、数据处理模块、远端控制模块、电源模块及咽拭子采样模块；

其中，视觉采集模块、机械双臂控制模块与数据处理模块均以网线连接，所述移动平台控制模块、机械双臂控制模块受控于远端控制模块，所述移动机器人本体设置有双臂，所述咽拭子采样模块设置在移动机器人本体的双臂上，且受控于机械双臂控制模块，所述电源模块为移动机器人本体供电；

所述数据处理模块采用上述的一种基于机器视觉引导的双臂机器人咽拭子采样方法进行数据处理。

所述移动机器人本体的移动底盘采用麦克纳姆轮结构，可实现360度无死角全向运动，灵活性较高；其负载大于50KG，人体测温捕捉距离在30米内，机器人测温机构可实现负40度到正60度的俯仰区间。

所述移动机器人的双臂采用六轴12自由度，臂长200mm-400mm，负载能力大于3KG，定位精度在0.2mm误差内的机械臂，且机械臂末端采用柔性采样头，实现柔性抓取，采样精度控制在0.1mm误差内。

所述视觉采集模块，为设置在移动机器人本体的移动平台前端的kinect深度相机，通过扫描口腔环境，采集点云数据，对口腔场景建模，并将点云pcd文件发送给数据处理模块。

所述数据处理模块，指咽拭子机器人操作系统，与生产者电脑连接，利用深度神经网络PointNet++，对口腔点云数据分析处理，获得口腔点云分割图，以舌头与扁桃体的点云表面中心坐标作为舌尖与咽喉位置坐标，但此时的位置坐标是相对于相机坐标系下，将其转换至机械臂坐标系并将此位置坐标发送给ROS机器人控制系统，通过系统中节点管理器Master与所有节点进行数据交互，主导机械臂的运动；

所述移动平台模块，指机器人全向移动底盘结构，包括底层控制器、驱动器、外部通信接口和全向移动平台，全向移动平台采用麦克纳姆轮采用麦克纳姆轮结构，可实现360度无死角全向运动，所述底层控制器用于控制电机速度及移动平台的转动角度，通过CANopen总线与驱动器进行通信，从而控制机器人的运动，所述驱动器，用于驱动移动平台运动，同时，还从控制器中引出了USB和网口链接外部设备与远端控制模块连接，实现实时远端监控；

机械手臂模块包括机械手臂控制器和移动机械双臂和机械手臂末端相机，不同于深度相机，所述末端相机将实时拍摄机械臂末端于口腔中的采样过程，与显示屏相连远端实时观测，所述移动机械臂选用双臂6轴12自由度机械双臂机器人，臂长200mm-400mm其负载能力大于3KG，机械臂控制器与数据模块连接，借ROS控制系统moveit工具控制机械臂发布控制指令指定机械臂移动至期望位置；

移动机器人的移动平台和双臂由直流无刷伺服电机带减速机驱动，所述电源模块为24V，40AH的锂电池组。

所述咽拭子采样模块，包括固定于移动机器人本体的移动底盘上的压舌板、棉签、试管，以及固定患者口腔的固定装置。

所述远端控制模块，包括显示屏及工控机，通过5GWIFI信号连接到本地路由器。

即与远程控制器在同一个路由器下，通过远程API可直接控制移动地盘和机械臂，借助相机拍摄咽拭子采集过程，实现远端监控调节，出现意外情况可及时中断，同时通过人机交互界面操作数据处理模块主机直接控制移动地盘及机械臂运动。

在本实例中，通过视觉采集模块获取到了患者口腔内舌尖位置坐标与咽喉深部位置坐标，分别作为移动左臂与右臂的期望坐标，介于此时机械臂上已经夹有压舌板与棉签，除了需要导入相同比例大小的机械臂模型外，还需要加入压舌板与棉签的模型参数，通过拼接的方式将两者视为机械臂的本体部分加入到运动规划之中，完成了模型的导入后，基于moveit功能包包含开源运动规划库(OMPL)许多常用运动规划算法以基于采样的算法，如RRT、PRM，A*等，选用A*启发式搜索算法实现了空间机械臂的无碰撞路径规划，给定机器人的初始位姿、目标位姿以及关于机器人和环境的几何描述，通过取当前路径点周围邻位中启发式函数值最小的邻位来确定下一个路径点，最终得到一条初始点到目标点的路径.运动学插件使用IKFAST，一种基于解析算法的运动学插件，快速的前提下可以保证每次求解的一致性，完成将机械臂的末端到期望坐标的规划过程，并通过RVIZ可视化界面实时地同步机械臂地运动过程。

参见图2，图2为一种基于机器视觉引导的咽拭子采样双臂机器人作业的流程图，一种基于机器视觉引导的双臂机器人咽拭子采样方法，包括以下步骤：

步骤1：咽拭子采样准备，双臂机器人夹取压舌板和纯净棉签；

步骤2：患者口腔场景建模，利用深度相机扫描患者口腔环境，获取口腔点云数据，构建口腔三维场景；

步骤3：基于端到端的深度PointNet++网络进行口腔场景分割与定位；

对构建的口腔场景采用PointNet++网络进行分割，获取舌头、牙齿、扁桃体、上颚器官的分割图，并对分割图进行定位，获得舌头与扁桃体的空间位置坐标，并作为双臂机器人的双臂采样期望坐标；

步骤4：基于双臂采样期望坐标，调用moveit功能包对双臂机器人的双臂进行运动规划，获得双臂运动轨迹；

步骤5：利用获得的双臂运动轨迹，控制双臂机器人的双臂达到采样期望坐标进行采样。

在ROS操作系统中对机器人建模编写URDF文件，主要涉及移动底盘和机械臂中运动连杆与关节间组合关系及外观参数，并将其导入到MoveIt Setup Assistant Tool，由于压舌板与采样棉签固定在移动地盘确定位置，向机械臂传递压舌板及棉签坐标位置，调用moveit功能包对期望坐标进行路径规划后发布规划指令，实现对已知位置物体无碰撞抓取。

选择深度相机固定于移动底盘，并于上位机相连，在ROS系统下配置中USB_CAM功能包驱动深度相机，完成eye-to-hand相机标定。

移动机械双臂使用RealSenseD435深度相机、Robotip2F_85夹具。在ROS系统下配置中USB_CAM功能包驱动深度相机，同时将所述相机采集的点云数据输出到数据处理模块，为避免相机数据源引起的误差，首先使用camera_calibration功能包，借用一张棋盘格标定靶完成相机内参标定，相机外参标定分为eye to hand,eye in hand两种模式，由于我们的深度相机位置，我们选用前者完成外参的配置，使用easy_handeye功能包，其算法内容是对于机器人夹着标定板运动任意两个位姿，有以下公式

等式两边是对同一位置标定板下的机器人移动过程中任意两个位姿，由于标定靶不动使得等式成立，以等式左边为例，

表示第一个机器人位姿下，机器人在末端坐标系下位置，

表示相机在机器人坐标系下的位置，

表示物体在相机坐标系下的位置，等式右边同理。

经过转换后

即可得到机器人基坐标与相机坐标的位置关系。其中A是机械臂基坐标系与末端坐标系的转换关系，B是相机坐标系与标定板的转换关系，以此通过多次标定点采集可得所求的X，即机械臂末端坐标系与相机坐标系的转换关系。

本实例中移动机器人中的视觉伺服控制系统采用3D视觉抓取技术，使用了一种端到端的深度学习方法PointNet++网络直接处理捕获的口腔点云数据，实现口腔3D分割，确定舌尖与扁桃体空间位置。

机械臂末端抓取的压舌板与棉签当集成为一体执行采样过程的路径规划，将压舌板与棉签模型一比一设置为长方体块与机械臂拼接在一起，继而在避障的路径规划中，保证夹取物同样不与口腔发生碰撞。

机械臂在笛卡尔空间完成直线路径规划，完成采样过程；

采用实时交互式人机控制器控制，在执行规划动作的同时，由机械臂末端相机捕获采样画面，通过5G信号传送至远端显示屏界面，出现意外情况可及时中断程序地运行终止机械臂地运动，无线通讯机制包括数据传输，数据监控等，实现了远程操控。

读取moveit模块发布的运动规划信息队列，移动机械左臂移动至已知空间位置即垃圾桶上方，末端夹爪失去力控制，压舌板相继脱落，移动至已知方位2即试管位置，夹取试管，与右臂夹取的棉签实现交互完成样本收纳。

将本发明实例所述方法通过编程实现时，按照以下方式进行设置：

在ROS命令行启动MoveIt功能包，配置相应设置，具体为：加载所创建的机器人URDF模型文件，生成自碰撞矩阵，添加规划组仅指定机器人机械臂关节部分，添加左右两臂规划组，设置其运动学求解器为IkFAST，添加机械臂末端为手爪规划组，添加机器人初始位姿(可任意设置)，标记末端执行器，配置与3D传感器相关参数，生成配置文件完成机械臂加载工作。

当机器人准备采样时，启动刚刚保存完成的demo文件，即可在rivz可视化平台中显示所编写的URDF机器人模型，而界面左半区域为moveit功能按键，此刻发布采样话题，即外部调用Python编程接口向机械臂传递压舌板及棉签坐标的预定坐标，实施逆向运动方程求解，计算机械臂各个关机抓取时的姿态，完成路径规划后发布规划指令。

为获取口腔内原始点云数据，构建数据采集系统，对PCL源代码进行编译及配置，制作程序库，并利用OpenNI开源接口读取原始的深度图像和彩色图像并驱动编程，并最终保存为pcd二进制数据格式。

口腔环境的三维建模，采用相对坐标ICP的三维重建方式，首先通过SIFT特征点提取及匹配，得到相邻帧的变换矩阵，作为ICP迭代初值，再进行帧间配准，建立相对坐标关系，将各帧点云数据配准到统一坐标系下，最后基于上下层点云进行层间配准，得到可视化的三维场景模型。

在Ros系统下完成图像接口kinect相机驱动功能包配置，命令终端启动相机驱动文件后，在rivz可视化界面中添加pointcloud2插件，可视化点云3维模型，同时显示深度相机实时获取的RGB图像与深度图像。

图3是本实例中所采用的视觉数据处理的网络结构图所述PointNet++网络包括级联的多尺度PointnetSAModuleMSG和上采样网络PointnetFPModule；

其中，所述PointnetSAModuleMSG包括级联的采样层Sample layer、组合层Grouping layer以及特征提取层PointNet layer；

采样层Sample layer用于对输入的点云数据进行farthest point sampling采样，从采样点中选取若干中心点，输入三维张量为(B,N,3)，输出张量(B,npoint)，B为批次大小，N为点云数据集中的点云总数，3代表点的坐标特征xyz，npoint表示采样输出的中心点数量；

组合层Grouping layer用于通过采样层Sample layer获得的中心点与其局部点构成若干区域，设置区域球半径大小radius，区域采样点的上限nsample，控制球规模及点个数，通过Ball query算法生成若干局部区域，返回表示局部区域特征的索引张量(B,npoint,nsample)；

nsample是每个区域的采样点数量的上限，其大小为16或者32，对应两种radius，总共有4层PointnetSAModuleMSG，在每层PointnetSAModuleMSG中，nsample的取值不同；

先设定局部区域再进行特征提取，局部区域是通过Ball query算法，通过设置的radius即球半径大小确定区域大小，每个区域的点是通过farthest point sampling采样获得的，直至采样的数量达到上限nsample。

而因为口腔的点云的分布是密度不均匀的，故此选择多尺度的方式，因此每一层PointnetSAModuleMSG都有两种尺度的区域，即两个半径值和两个采样上限值；

特征提取层PointNet layer用于对每个局部区域的特征使用PointNet提取特征向量；

为实现不同对象咽拭子采样的泛化能力及采样过程的时效性，传统的2D视觉抓取技术无法满足，该视觉伺服控制系统考虑采用新型3D视觉抓取技术，使用了一种端到端的深度学习方法PointNet++网络，可直接处理视觉采集装置捕获到的口腔点云数据。

对于采样方式，使用farthest point sampling算法，从点云数据中采样出npoint个点数，相比于随机采样算法，能够更好的覆盖到整个点云数据集；

Pointnet主要由三部分模块组成，解决旋转和平移不变性的t-net：训练t-net网络得到转换矩阵与输入点云数据相乘实现对齐操作，多层感知机MLP：对点云数据做低维度到高纬度的空间映射，以及最大池化函数max pool的对称性质解决点云无序的问题。

通过对上述的三个模块级联组合实现类似于CNN从浅层特征到深层的语义特征的提取；输出至上采样网络实现实例分割；

所述上采样网络PointnetFPModul对PointnetSAModuleMSG的输出的全局特征进行上采样，生成新的点，当点的个数只有一个的时候，直接将其重复复制成N个点；当点的个数大于一个的时候，采用基于K最近邻的线性差值的方式进行上采样，再对上采样后的每一个点都做一个MLP，同时拼接上下采样前相同点个数的SA层的特征；

线性插值公式如下：

k表示选取的最相邻的样本数量，p表示最相邻距离的幂，设置p＝2，k＝3，x表示内插值中的任意一点，x_i表示内插值中的已知点，d(x,x_i)表示任意一点x和已知点x_i之间距离，w_i(x)表示任意一点x权重值，取距离倒数使得距离越远的点权重越小；

p的值越大距离影响的效果就越明显，最后对每一个点的权重实现全局的归一化。

主要通过线性差值与MLP堆叠完成。

网络构成如下：

前四层网络均为为PointnetSAModuleMSG，第一层网络输入通道数为9，采样点数npoint＝1024即所有点,组合层包括三种半径radii＝[0.05,0.1]，对应区域点上限分别为nsamples＝[16,32]，以及三种升维操作mlps＝[[9,16,16,32],[9,32,32,64]]即9维升至32维、64维度，最后拼接在一起，32+64＝96作为下一层的输入通道数，新生成的点重新组合成新的区域，第二层中采样点数npoint＝256，组合层半径参数为[0.1,0.2]，区域点数上限nsamples＝[16,32],mlps＝[[96,64,64,128],[96,64,96,128]]，输出通道数为128+128＝256，第三层中采样点数npoint＝64,组合层半径参数为[0.2,0.4]，区域点数上限nsamples＝[16,32],mlps＝[[256,128,196,256],[256,128,196,256]]，输出通道数为256+256＝512,第四层中采样点数npoint＝16,组合层半径参数为[0.4,0.8]，区域点数上限nsamples＝[16,32],mlps＝[[512,256,256,512],[512,256,256,512]]，输出通道数为512+512＝1024。

后四层为PointnetFPModule，输入通道数为上一层输出的1024维特征向量，mlp参数分别为[256，256]，[256,256]，[256，128]，[128，128，128]得到上采样点数据，经过第一层conv-batchnorm-rule完成,第二层conv后通过softmax函数分类。

如图4所示，所述对双臂机器人的双臂进行运动规划时，采用A*启发式搜索算法进行双臂无碰撞路径规划，具体过程如下：

给定双臂机器人的初始位姿、目标位姿以及双臂机器人和环境的几何描述，通过取当前路径点周围邻位中启发式函数值最小的邻位来确定下一个路径点，最终得到一条初始点到目标点的路径。

运动学插件使用IKFAST，进行碰撞检测后，采用action通信机制输出机械臂的末端到期望坐标的关节轨迹数据，并通过RVIZ插件接口，可视化界面实时地同步机械臂地运动过程，机械臂端通过配置插口接收到轨迹后使用五次样条插补运算，实现位置、速度、加速的连续，并发布到每一个电机位置控制接口，实现每一个电机的运转，同时以topic通信机制50hz频率反馈给moveit各个关节状态信息。

双臂协同作业对空间、协作姿态、协作方式提出了更高的柔性要求，能实现基于最佳协作、最好运动性能，为实现采样时短距离地直线运动，在笛卡尔空间下，利用逆向运动学求解地方法，在咽喉深度的期望坐标，规定距离为2cm，获取直线两端坐标，规划过程加入碰撞检测环节，再借由moveit完成机械臂直线轨迹规划，通过话题消息的形式将机械臂各个关节的运动信息序列按照队列的方式发送给机械臂的运动控制模块。

其处理过程为：首先初始化ROS节点信息及机械臂关节组，获取机械臂初始位置，设置运动过程中最大速度与最大加速度，通过两点构成一条直线的原理，我们获取到两个期望点坐标后，即可描绘出整条直线中的点组成，进行路径规划，当覆盖率实现100％时即此直线中所通过点机械臂均能抵达，即可对机械臂控制模块发布通信参数，最后按照机械臂控制模块支持的通信方式取回机械臂运动的实际位姿并发布给moveit初始化程序模块。

两个单臂机器人由于采用了两个独立的控制器进行控制，样本收纳过程处于不协调、不连贯的状态；而双臂运动控制是作为一个完整的机器人控制系统，由同一个控制器同步向双臂发送协调操作指令，大大提高了运动的协调性。为实现双臂的柔顺控制，采用主从动作的阻抗控制方法，引入了双臂间相对误差并将其定义为双臂间的虚拟恢复力以引入到双臂的阻抗控制模型中，根据双臂间的物理约束关系，建立双臂间的主从约束关系，另外，为实现双臂机器人夹持物体与环境交互过程中力的可控性，采用外环阻抗控制，为消除机械臂末端与夹取物间的饱和挤压力又能保持较大范围的接触力，在移动机械双臂末端和接触物体之间构造一个内部阻抗控制器，简称内部阻抗，以控制机械臂末端与接触物的力大小，这样的双环控制策略有效的解决了双臂协调操作物体与环境交换的问题。

采用实时交互式人机控制器控制，在执行规划动作的同时，由机械臂末端相机捕获采样画面，通过5G信号传送至远端显示屏界面，出现意外情况可及时中断程序地运行终止机械臂地运动，无线通讯机制包括数据传输，数据监控等，实现了远程操控。

最后应当说明的是：以上实施例仅用以说明本发明的技术方案而非对其限制，尽管参照上述实施例对本发明进行了详尽的说明，所属领域的普通技术人员应当理解，上述实施例仅仅是对本发明的示意性实现方式的解释，实施例中的细节并不构成对本发明范围的限制，在不背离本发明的精神和范围的情况下，任何基于本发明技术方案的等效变换、简单替换等显而易见的改变，均落在本发明保护范围之内。

Claims (9)

1.一种基于机器视觉引导的双臂机器人咽拭子采样方法，其特征在于，包括以下步骤：

步骤1：咽拭子采样准备，双臂机器人夹取压舌板和纯净棉签；

步骤2：患者口腔场景建模，利用深度相机扫描患者口腔环境，获取口腔点云数据，构建口腔三维场景；

步骤3：基于端到端的深度PointNet++网络进行口腔场景分割与定位；

对构建的口腔场景采用PointNet++网络进行分割，获取舌头、牙齿、扁桃体、上颚器官的分割图，并对分割图进行定位，获得舌头与扁桃体的空间位置坐标，并作为双臂机器人的双臂采样期望坐标；

步骤4：基于双臂采样期望坐标，调用moveit功能包对双臂机器人的双臂进行运动规划，获得双臂运动轨迹；

步骤5：利用获得的双臂运动轨迹，控制双臂机器人的双臂达到采样期望坐标进行采样；

所述对构建的口腔场景采用的端到端的PointNet++网络进行分割，获取舌头、牙齿、扁桃体、上颚器官的分割图的具体过程如下：

步骤3.1：首先依据PointNet++网络构建pointnet_sem_seg网络代码，执行python训练程序，并安装pytorch深度学习框架，载入三维口腔点云数据集，训练PointNet++网络获取权重；

其中，训练集点数为4096，batchsize＝16，epoch＝200，learning_rate＝0.01，学习率设置为每经过20步衰减0.5倍，训练数据为深度相机读取到的口腔点云数据，输出的点云分割结果使用MeshLab软件可视化；

步骤3.2：利用训练好的PointNet++网络，输入采集的口腔点云，获取到点云分割后提取出舌头部位和扁桃体部位点云，以点云的表面中心坐标为最佳的目标点作为机械臂期望坐标位置，同时使用深度相机对该目标点计算三维距离(Xc,Yc,Zc)；

所述PointNet++网络包括级联的多尺度PointnetSAModuleMSG和上采样网络PointnetFPModule；

其中，所述PointnetSAModuleMSG包括级联的采样层Sample layer、组合层Groupinglayer以及特征提取层PointNet layer；

采样层Sample layer用于对输入的点云数据进行farthest point sampling采样，从采样点中选取若干中心点，输入三维张量为(B,N,3)，输出张量(B,npoint)，B为批次大小，N为点云数据集中的点云总数，3代表点的坐标特征xyz，npoint表示采样输出的中心点数量；

组合层Grouping layer用于通过采样层Sample layer获得的中心点与其局部点构成若干区域，设置区域球半径大小radius，区域采样点的上限nsample，控制球规模及点个数，通过Ball query算法生成若干局部区域，返回表示局部区域特征的索引张量(B,npoint,nsample)；

所述上采样网络PointnetFPModul对PointnetSAModuleMSG的输出的全局特征进行上采样，生成新的点，当点的个数只有一个的时候，直接将其重复复制成N个点；当点的个数大于一个的时候，采用基于K最近邻的线性差值的方式进行上采样，再对上采样后的每一个点都做一个MLP，同时拼接上下采样前相同点个数的SA层的特征；

线性插值公式如下：

k表示选取的最相邻的样本数量，p表示最相邻距离的幂，设置p＝2，k＝3，x表示内插值中的任意一点，x_i表示内插值中的已知点，d(x,x_i)表示任意一点x和已知点x_i之间距离，w_i(x)表示任意一点x的第i个相邻的已知点x_i的权重值，取距离倒数使得距离越远的点权重越小。

2.根据权利要求1所述的方法，其特征在于，所述对双臂机器人的双臂进行运动规划时，采用A*启发式搜索算法进行双臂无碰撞路径规划，具体过程如下：

给定双臂机器人的初始位姿、目标位姿以及双臂机器人和环境的几何描述，通过取当前路径点周围邻位中启发式函数值最小的邻位来确定下一个路径点，最终得到一条初始点到目标点的路径。

3.一种基于机器视觉引导的咽拭子采样双臂机器人，其特征在于，包括设置在移动机器人本体上的视觉采集模块、移动平台控制模块、机械双臂控制模块、数据处理模块、远端控制模块、电源模块及咽拭子采样模块；

其中，视觉采集模块、机械双臂控制模块与数据处理模块均以网线连接，所述移动平台控制模块、机械双臂控制模块受控于远端控制模块，所述移动机器人本体设置有双臂，所述咽拭子采样模块设置在移动机器人本体的双臂上，且受控于机械双臂控制模块，所述电源模块为移动机器人本体供电；

所述数据处理模块采用权利要求1-2任一项所述的一种基于机器视觉引导的双臂机器人咽拭子采样方法进行数据处理。

4.根据权利要求3所述的基于机器视觉引导的咽拭子采样双臂机器人，其特征在于，所述移动机器人本体的移动底盘采用麦克纳姆轮结构。

5.根据权利要求3所述的基于机器视觉引导的咽拭子采样双臂机器人，其特征在于，所述移动机器人的双臂采用六轴12自由度，臂长200mm-400mm，负载能力大于3KG，定位精度在0.2mm误差内的机械臂，且机械臂末端采用柔性采样头。

6.根据权利要求3所述的基于机器视觉引导的咽拭子采样双臂机器人，其特征在于，所述视觉采集模块，为设置在移动机器人本体的移动平台前端的kinect深度相机，通过扫描口腔环境，采集点云数据，对口腔场景建模，并将点云pcd文件发送给数据处理模块。

7.根据权利要求3所述的基于机器视觉引导的咽拭子采样双臂机器人，其特征在于，移动机器人的移动平台和双臂由直流无刷伺服电机带减速机驱动，所述电源模块为24V，40AH的锂电池组。

8.根据权利要求3所述的基于机器视觉引导的咽拭子采样双臂机器人，其特征在于，所述咽拭子采样模块，包括固定于移动机器人本体的移动底盘上的压舌板、棉签、试管，以及固定患者口腔的固定装置。

9.根据权利要求3所述的基于机器视觉引导的咽拭子采样双臂机器人，其特征在于，所述远端控制模块，包括显示屏及工控机，通过5GWIFI信号连接到本地路由器。

Images