一种操作仪工作端移动轨迹的实时位姿估计与评估方法与流程

1.本发明属于物体空间实时位姿跟踪领域，主要涉及基于特征点检测的位姿自动跟踪，具体涉及一种操作仪工作端移动轨迹的实时位姿估计与评估方法。


背景技术：

2.在操作员操作仪实施作业的过程中，操作仪工作端的轨迹位姿要求通常会影响作业的质量，对作业过程中工作端位姿的实时监控和量化评估就为重要。
3.在实际应用中，操作仪的工作端经常会被遮挡，如操作仪工作端和作业目标接触时工作端被遮挡，因此无法通过视觉直接跟踪工作端的目标点。
4.另外，作业过程中工作端位姿的实时估计对关键点检测算法的效率要求很高，在主流的特征点检测算法中，输出是关于特征点的高斯热图，大量的输出参数降低了其算法的效率，如赵子昂提出的《基于卷积神经网络的精确和实时人体关键点检测算法研究及部署》论文，输出的参数为高斯热图，降低了特征点识别的效率。再如申请公布号为cn202111661938的中国发明专利，将深度信息输入网络进行卷积处理，减慢了位姿估计的运算速度，降低了特征点识别的效率。
5.因此，为了满足对操作仪工作端的实时位姿评估的需求，需要克服工作端被遮挡的问题与特征点识别算法的计算效率问题。
6.本发明因此提出了一种操作仪工作端移动轨迹的实时位姿估计与评估方法，该成果可用于评估操作员是否符合预期操作轨迹的位姿要求，以及对操作员的岗前培训和业务考核。


技术实现要素：

7.本发明的目的在于克服现有技术的缺点与不足，提供操作仪工作端移动轨迹的实时位姿估计与评估方法，解决了工作端被遮挡无法定位的问题，特征点算法计算速度较慢的问题以及操作员操作质量量化问题。
8.本发明至少通过如下技术方案之一实现。
9.一种操作仪工作端移动轨迹的实时位姿估计与评估方法，该方法基于的操作仪工作端移动轨迹位姿估计与评估系统包括立体视觉相机、相机外连接件、工作台、指导操作轨迹、计算机、操作仪及其特征区域与工作端、操作员，其特征在于：该方法包括步骤：s1、为操作仪选取与设计多个特征区域，选取特征区域的中心点为操作仪的特征点，通过拍照与标记不同姿态下的操作仪与标记特征区域为s3的特征点检测卷积神经网络训练提供训练集；s2、在合适位置搭建平台，连接设备，立体视觉相机采集操作仪的彩色rgb与深度图像并发送到计算机；s3、构建特征点检测的卷积神经网络，将预处理后的彩色rgb图像输入特征点检测卷积神经网络，通过解码网络输出获得图像中的操作仪特征点所在的像素二维坐标值；通
过正样本匹配获得可供训练使用的正样本，构造损失函数用于回归调整卷积核系数；s4、处理特征点附近的点云数据，降低点云噪声干扰，获取在空间中操作仪各特征点的三维坐标值，计算得出操作仪工作端的三维坐标值以及法向向量；s5、根据工作端的三维坐标值与规划的指导操作轨迹对操作仪工作端移动轨迹进行量化评估。
10.进一步地，所述步骤s1具体包括：s11、为操作仪选取特征区域，选取特征区域的中心点为操作仪的特征点；s12、根据s11在无明显特征的情况下设计特征图案，将该图案贴至操作仪上作为特征区域，选取图案中心点为操作仪的特征点；s11、s12选择的特征区域记为，对应的特征点记为，指的是选择的特征区域的类别个数；s13、用彩色相机拍摄操作仪在不同姿态下的图片，使用中心标记法标记特征区域，并且确保在不同姿态下看见至少三个特征区域；所述中心标记法指的是用两个点作为特征区域的标记，是操作仪特征点在图像中的像素二维坐标，是操作仪特征区域的最小外接四边形的一个角在图像中的像素二维坐标，通过拍照不同姿态下的操作仪与标记特征点为s2的卷积神经网络训练提供训练集。
11.所述中心标记法指的是用两个点作为特征区域的标记，是操作仪特征点在图像中的像素二维坐标，是操作仪特征区域的最小外接四边形的一个角在图像中的像素二维坐标，通过拍照不同姿态下的操作仪与标记特征点为s2的卷积神经网络训练提供训练集。
12.进一步地，所述步骤s2具体包括：s21、采集数据开始前、将相机通过相机的外接件固定在工作台上，调整相机外接件的位置，使得相机处于最佳工作位置，即在移动操作仪的过程中既能捕捉到清晰的操作仪的图像，又使得相机获取清晰的操作仪的点云数据；s22、将rgb彩色图像与深度图像输入到计算机中。
13.进一步地，所述步骤s3具体包括：s31、采集的rgb彩色图像宽高为，根据图像裁剪方法将的图像裁剪为；所述图像裁剪方法，是根据上一帧识别的到的操作仪的范围对该帧的图像进行裁剪，图像裁剪的方法提高了网络计算的速度；s32、特征点检测卷积神经网络的输入大小为，选取中的最大值作为放缩的最大边界，将裁剪后的图像不失真的放缩为的宽高大小，对比网络输入大小，用rgb值为的黑色填充图像空余的部分，再将图像的rgb值以256为1进行归一化处理；s33、将放缩图像输入主干特征提取网络获取图像深层信息；
先通过focus网络拓宽通道数，再经过四个残差网络，取后三个残差网络的输出作为加强特征提取网络的输入，以输入为为例，的大小分别为、、；s34、为了将深层信息与中层信息融合，在加强特征提取网络中，进行相互上下采样与卷积得到加强特征提取网络的输出；s35、在分类器与回归器中，使用卷积的方式将s34的输出分别卷积成大小为的，与输入的的宽高大小相同；s36、将网络输出结果进行解码，以网络输出为例，输出的定义为将原图分为个网络输出区域，网络输出区域的中心点记为；所述有四个参数：代表s1中所述特征点与的偏移系数；代表s1中所述特征区域的宽高系数；所述代表是否含有的概率；所述的层数与s1中所述特征区域的类别个数一致，其值代表中含有对应类别的概率；通过解码获得特征区域，选取特征区域的中心点，获得操作仪的特征点在彩色rgb图像中的二维坐标；s37、在网络训练过程中，通过正样本匹配方法从网络输出的大量负样本中选出可用来训练的正样本数据，再构造损失函数用于回归调整卷积核系数；所述的损失函数包括四个部分、、、：特征区域损失函数：正样本输出区域与步骤s13中标记的区域的iou损失；特征点存在损失函数：正负样本与步骤s13中标记区域是否含有特征区域计算交叉熵损失；特征点类别损失函数：正负样本与步骤s13中标记的区域的类别计算交叉熵损失；特征点偏移损失函数：正负样本与步骤s13中标记区域中心点的欧氏距离；总损失函数计算公式如下：其中为重点损失函数系数；训练时，对构建好的特征点检测卷积神经网络设置训练的超参数和训练条件进行
网络参数训练。
14.进一步地，所述步骤s4具体包括：s41、基于s36获取的操作仪上目标特征点的二维坐标，通过对同一时刻获取的点云数据进行直通滤波、条件滤波、半径滤波操作处理无效点云数据，清除噪声的干扰；s42、通过深度优先搜索算法与三维映射获取操作仪上目标特征点的深度信息；s43、通过特征点的三维坐标建立坐标系；以三个特征点为一组，将靠近工作端的特征点作为空间坐标系的原点，以向量作为的轴的单位向量，通过叉乘向量与向量获得垂直于所在平面的向量，再除以模长，作为的轴的单位向量，通过叉乘向量与向量再除以模长获得的轴的单位向量；计算公式如下：；计算公式如下：；计算公式如下：；计算公式如下：；计算公式如下：；计算公式如下：；计算公式如下：；计算公式如下：；计算公式如下：；计算公式如下：s44、基于操作仪的模型，根据建立的坐标系与工作端的位置关系系数（，，）与工作端法向向量在坐标系下的坐标；通过的原点计算获得工作端在相机坐标系下的坐标与法向向量，计算公式如下：，计算公式如下：，计算公式如下：，计算公式如下：，计算公式如下：在特征点的坐标值中加入误差，根据计算可得工作端的误差关系如下：
该公式表明，与的相互距离呈负相关，与呈正相关，其中指的是工作端与由组成的平面的距离。
15.进一步地，所述步骤s5具体包括：s51、根据s44中实时获取的操作仪的工作端的空间坐标，将其与指导操作轨迹点的坐标的差值与重合轨迹阈值与偏离轨迹阈值做得分估计；s52、根据s4中实时获取的操作仪的工作端的法向量与指导操作轨迹点的法向量的夹角的余弦值与角度偏离阈值做得分估计；s53、对操作员使用操作仪在规划轨迹上移动的点进行量化评分。
16.与现有的关键点检测网络相比，该方法的输出参数较少，网络计算量减少，能够满足实时检测的需求；在工作端被遮挡的情况下，也能通过操作仪上特征点的坐标计算获得工作端的位姿信息；实现操作仪工作端移动轨迹的实时位姿估计与评估。
附图说明
17.图1是本发明方法的操作仪工作端移动轨迹的实时位姿估计与评估系统总体结构示意图；图2是本发明方法的特征点检测卷积神经网络结构示意图；图3是本发明方法的对操作仪移动轨迹跟踪的实时位姿估计与评估方法流程示意图；图4是本发明具体实施例的操作仪工作端移动轨迹的实时位姿估计与评估系统总体结构示意图；图5是本发明具体实施例的操作仪（按摩仪）上的特征区域图；图6是本发明具体实施例的特征区域的特征点标记图；图7是本发明具体实施例的计算工作端轨迹与指导操作轨迹图；图4中：1-操作仪（按摩仪）、2-立体视觉相机（kinect深度相机）、3-相机外连接件、4-指导操作轨迹（指导按摩轨迹）、5-硅胶人体面部模型、6-计算机（硬件gpu设备为3050ti）、7-工作台；图5中：8-操作仪上的特征点
①
，9-操作仪上的特征点
②
，10-操作仪上的特征点
③
；图7中：11-指导操作轨迹法向量、12-面部特征点、13-工作端实际位置与法向量、14-工作端实际移动的轨迹、15-工作端实际移动轨迹的法向量。
具体实施方式
18.下面结合实施例及附图对本发明作进一步的详细描述，但本发明的实施方式不限于此。
19.请参阅图3、图4，本发明提供的操作仪工作端移动轨迹的实时位姿估计与评估方法，该方法基于的操作仪工作端移动轨迹的实时位姿估计与评估系统包括：1-操作仪（按摩仪）、2-立体视觉相机（kinect深度相机）、3-相机外连接件、4-指导操作轨迹（指导按摩轨迹）、5-硅胶人体面部模型、6-计算机（硬件gpu设备为3050ti）、7-工作台。
20.在本实施例中，按摩仪1将在硅胶人体面部模型5上沿着指导操作轨迹4的方向以及法向进行按摩操作，要求对按摩仪工作端的移动轨迹进行实时的位姿估计与评估。
21.立体视觉相机2通过相机外接件3固定在工作台7上，防止相机移动导致规划轨迹的偏移；指导操作轨迹4与面部特征点12是在硅胶人体面部模型5上生成的，其生成方法不在本专利描述中。
22.如图3所示，本发明所提供的一种操作仪工作端移动轨迹的实时位姿估计与评估方法，包括以下步骤：步骤s1：选择与设计特征区域：1）请参阅图5，在按摩的过程中，在按摩仪的侧面选取与设计共3个特征区域；2）选取特征点
①
，
②
，
③
作为计算工作端位姿的一组；3）请参阅图6，使用中心标记法标记特征区域作为s3的网络训练集。
23.步骤s2：搭建平台，采集图像并发送到计算机：1）采集数据开始前、将相机通过相机的外接件固定在工作台上，调整相机外接件的位置，使得相机处于最佳工作位置，即在移动操作仪的过程中既能捕捉到清晰的操作仪的图像，又使得相机能获取清晰的操作仪的点云数据；2）将rgb彩色图像与深度图像输入到计算机中。
24.步骤s3：获取按摩仪上特征点的坐标：1）根据上一帧按摩仪上的特征点区域对rgb图像进行裁剪，然后放缩成特征点识别网络输入大小；2）经过主干特征提取网络、加强特征提取网络、分类器与回归器，得到大小分别为、、的输出、、，输出参数共16800个；3）对该网络输出进行解码获取特征点在图像中的二维坐标；4）相比于特征点检测网络输出高斯热图的参数数量，有着更少的计算，在rtx3050ti的gpu上可达到40fps的运算速度，满足实时位姿估计的要求。
25.步骤s4：计算按摩仪工作端的位姿：1）根据是否获得特征点，对以特征点为中心的一定半径内的点云数据实施直通滤波、条件滤波、半径滤波获得较少噪声点的点云数据；2）通过对该点云数据进行深度优先搜索算法，将特征点的像素二维坐标转化为操作仪特征点的三维坐标；3）根据特征点组合建立坐标系；4）基于操作仪的模型，根据建立的坐标系与工作端的位置关系系数
与工作端法向向量在坐标系下的坐标；计算得工作端实际的位置与法向向量13，由此组成工作端实际移动的轨迹14和法向量15。
26.步骤s5：请参阅图7，根据指导操作轨迹4和法向量11，对工作端实际移动的轨迹14和法向量15进行评估：1）根据s4中实时获取的按摩仪的工作端的空间坐标的坐标值与指导操作轨迹点的坐标的差值与重合轨迹阈值与偏离轨迹阈值做得分估计：若小于，判断工作端已到达该轨迹点；若大于但小于，判断工作端仍在轨迹附近；若大于，判断工作端已经偏离轨迹；2）根据s4中实时获取的操作仪的工作端的法向量与指导操作轨迹点的法向量的夹角的余弦值与角度偏离阈值做得分估计；若大于，判断工作端在该轨迹点上与规划轨迹法向量保持一致；若小于，判断工作端在该轨迹点上无法满足规划轨迹法向量；3）对操作员使用按摩仪在指导操作轨迹上移动的点进行量化评分；在本发明的实施例中，量化评分计算公式如下：计算公式如下：其中为该指导轨迹上点的数目，为指导轨迹中一个点的量化评分。
27.在评估系统开始后，将重复s2-s5直至操作员完成指导操作轨迹，在此过程中，本发明克服特征点识别算法的计算效率问题，能够对按摩仪的工作端实现精确的位置与姿态估计，也克服了工作端被遮挡无法识别位姿的问题，满足了对操作员的岗前培训和业务考核要求。
28.本发明的上述实施例仅仅是为清楚地说明本发明所作的举例，而并非是对本发明的实施方式的限定，对于所属领域的普通技术人员来说，在上述说明的基础上还可以做出其它不同形式的变化或变动。这里无需也无法对所有的实施方式予以穷举。凡在本发明的精神和原则之内所作的任何修改、等同替换和改进等，均应包含在本发明权利要求的保护范围之内。