本发明涉及一种感知与校准方法，尤其是一种近距离机械臂感知与校准方法，属于机械臂感知定位的技术领域。
背景技术：
在工业化的流水线生产中，智能化机器人担任了非常重要的角色，机器人与机器人之间的交互及协同作业也越来越广泛。如工业化生产中两个机器人进行零件的传递，多机器人同时进行零件的装配或加工，机器人辅助人工进行作业等等。在大多数工业场景下，生产线上的机器人都是进行一些固定的动作，机器人的运动位置及关节角度都进行过精确校准，这样的机器人虽然能满足大规模的流水线作业需求，但缺乏智能性，一旦工作台位置发生偏移，或需要完成一项新的任务，则需要重新进行手动校准，无法满足未来工业中智能生产的要求。
目前，机械臂的校准都是利用传统的视觉传感器和深度传感器得到目标物体的点云信息，但是机器人是个复杂的机械结构体，对机器人的关节进行识别和定位存在较大难度。传统的识别和标定方法是在目标物体上放置一个特定的标签，在图像中检测特定的标签来确定目标物体的位置，这种方法在视觉图像清晰，目标物体无遮挡的时候有较好的效果，但机器人动作灵活，位姿复杂，很容易造成目标点遮挡或偏移，影响识别的准确性。而采用通信设备和定位传感器对机器人进行定位也存在精度较差的问题，目前市场上精度最高的室内定位产品误差在10cm以内，这个精度无法满足机器人精细作业的要求。
技术实现要素：
本发明的目的是克服现有技术中存在的不足，提供一种近距离机械臂感知与校准方法，其能在近距离范围内感知机械臂，并获取机械臂的三维位置以及位姿信息，满足机器人精细作业的要求。
按照本发明提供的技术方案，一种近距离机械臂感知与校准方法，所述感知与校准方法包括如下步骤：
步骤1、进行数据采集，采集的数据包括机器人的rgb图像和深度图像，并根据所采集的rgb图像、深度图像获取机械臂当前状态下每个轴关节的三维坐标；
步骤2、利用步骤1中获取机械臂每个轴关节的三维坐标，构建多目标的感知校准神经网络，所述感知校准神经网络包括机器人检测网络以及关键点检测网络，机器人检测网络以rfbnet作为基础网络框架，关键点检测网络以resnet34作为基础网络框架；对机器人检测网络，输入机器人的rgb图像能得到机器人的边界框，对关键点检测网络，输入机器人的深度图像以及机器人边界框能得到机器人上关键点的三维空间坐标；
在训练感知校准神经网络时，采用边界框匹配作为机器人检测网络的损失函数，采用均方误差作为关键点检测网络的损失函数；
步骤3、利用上述的感知校准神经网络得到机器人位姿检测模型，利用机器人位姿检测模型，在输入机器人rgb图像时，能得到机器人关键点在图像中对应的像素点坐标以及关键点的深度值。
步骤1中，采集的数据包括真实世界数据以及仿真数据；
在采集真实世界数据时，使用kinect相机采集机器人的rgb图像以及深度图像，kinect相机架设在机械臂前方，通过手眼校准确定机械臂座在相机坐标系下的三维坐标，同时获得机械臂在当前状态下的每个轴关节的角度，利用正向运动学方式确定每个轴关节在相机坐标系下的三维坐标；
采集仿真数据时，利用仿真平台webots按照真实环境搭建背景，并利用仿真环节中的gps传感器获取世界坐标系下的三维坐标。
关键点检测网络的检测的关键点为机器人基座、肩关节、肘关节、腕关节以及机器人手臂末端。
本发明的优点：能在近距离范围内感知机械臂，并获取机械臂的三维位置以及位姿信息，满足机器人精细作业的要求。
具体实施方式
下面结合具体附图和实施例对本发明作进一步说明。
为了能在近距离范围内感知机械臂，并获取机械臂的三维位置以及位姿信息，下面以珞石xb4机器人为例，对本发明的具体过程进行详细说明。具体地，本发明包含了数据采集和标定、构建深度学习神经网络、构建损失函数并进行训练三个部分。
步骤1、进行数据采集，采集的数据包括机器人的rgb图像和深度图像，并根据所采集的rgb图像、深度图像获取机械臂当前状态下每个轴关节的三维坐标；
本发明实施例中，采集的数据包括真实世界数据以及仿真数据；对于深度神经网络来说，数据集的质量和大小直接影响着训练结果的准确性。为了得到大量的数据集和准确的基本事实，本发明采用真实数据和仿真数据混合的方法，在真实世界和仿真环境中同时采集机械臂的图像，得到一个总共9000个样本的数据集，真实数据和仿真数据之比为1:2。如果只使用真实数据进行训练，数据集采集工作就会变得非常繁重和冗长，而只使用仿真数据的话，训练的结果只适用于仿真环境，输入真实世界的图像，结果就会变得不可预知。其中，采集真实世界数据有以下几个步骤：
1、系统搭建：数据采集需要利用一台kinect相机以及一台珞石xb4机器人，将kinect相机架设在珞石xb4机器人前方1.2m-1.5m处（大于两个机械臂的工作范围），将机器人控制器和kinect相机连接到一台pc（计算机），由pc统一控制机器人和kinect相机。
2、手眼校准：对机器人关键点进行标定的时候，所有的坐标信息都是在相机坐标系下描述的，手眼校准的研究内容就是确定机器人坐标系和相机坐标系之间的相对关系。手眼校准是通过在机器人末端放置标定板进行，kinect相机会通过识别标定板进而计算出世界坐标系与相机坐标系之间的转换矩阵。具体方法在github上有详细介绍。在kinect相机与机器人位置都固定的情况下进行校准，校准结束之后即可进行数据采集。
3、数据采集：在kinect相机和机器人位置固定的情况下进行校准和数据采集。采集过程中对机器人进行编程，使机器人每个关节按照设定的步长运动，在机器人的每个姿态下，使用kinect相机采集机器人的rgb图像和深度图像。并记录当前姿态下，机器人六个关节的转动角度。在采集3000张数据之后，移动kinect相机，重新开始校准和采集，共采集五次，样本总计15000个。
具体地，从机器人基座、腰部、大臂、小臂、腕部、末端依次分布的6个转动轴分别为轴1-轴6，珞石xb4机器人关节的运动步长与数据集大小有关，对机器人的五个关节设置相应的步长，来确保在规定的数据集大小内覆盖继机器人的大部分姿态。轴6在运动时机器人姿态不会发生变化，所以轴6不运动。同时我们缩小了每个关节的运动范围，排除了在实际的工作环境中机器人几乎不会出现一些极限动作。确定机器人五个轴的运动范围分别为轴1（-100°~ 100°）、轴2（-120°~ 80°）、轴3（-100°~ 50°）、轴4（-120°~ 120°）、轴5（-90°~ 90°）。设置五个关节的步长分别为：25°、25°、25°、40°、30°，轴1产生8个动作，轴2产生8个动作，以此类推共产生13824个动作。真实数据集样本容量共15000，可以覆盖机器人的全部动作姿态。如果继续扩充数据集，可以适当减小关节步长，但需要保证数据集可以覆盖机器人的全部动作姿态。
4、数据的标定。在获得机器人的图像之后，需要获取机器人关键点在相机下的坐标。因为在采集数据之前已经对相机进行校准，所以得到关键点的像素坐标之后，左乘相机内参矩阵的逆可以直接获得相机坐标系下的三维坐标。为了减轻数据标定的工作量，只需对机器人底座进行标定。因为每组图像中机器人的基座位置保持不变，因为只需标注五次。获得机器人基座坐标后，已知每个轴关节的角度，通过正向运动学公式即可获得每个关节的三维坐标。机器人正运动学是计算机器人关节位置姿态的常规方法。
具体实施时，共采集到了五组真实数据集，不同数据集下机器人基座位置不同，样本总量15000，每个样本包括机器人的rgb图像、深度图像、关键点的空间坐标和像素坐标。每张图片以分辨率为640*480的大小保存。
关于仿真数据的采集，具体地，在webots仿真环境中采集数据包括以下几个步骤：
1、建立xb4型机器人的仿真模型。xb4机器人的step格式模型文件可以在珞石官网进行下载，使用solidwork软件将step格式模型文件转为vrml97格式文件即可导入到webots中进行建模。使用webots中的伺服电机插件作为机器人结构体之间的连接轴，共六轴，对每个轴设置真实的运动范围（上文已给出），在机器人的每个关节处放置gps传感器以获取关节的空间三维坐标，使用视觉传感器获取机器人的rgb图像，使用距离传感器获取机器人的深度图像。所有传感器和伺服电机都可以在仿真环境中直接获得。同时在视觉传感器的位置也放置一个gps传感器，来获得相机的空间位置坐标。
2、搭建采集数据环境。在仿真环境中采集数据与真实环境中配置相同。将机器人放置在空间坐标系原点，视觉传感器和距离传感器放置在机器人前方1.2-1.5m区域。抬高机器人基座位置，保证机器人运动过程中末端不会接触地面，并使机器人出现在相机视角的中心位置。对机器人的轴1至轴5设定步长分别为：25°、20°、15°、20°、30°，轴1至轴5分别产生8、10、10、12、6个动作，机器人共产生57600个不同的姿态。我们在仿真环境中共采集50,000个样本，可以覆盖机器人大部分的动作姿态。我们编程控制机器人每个关节按照步长进行运动，便可获得机器人在空间中的所有姿态。机器人每改变一次姿态，传感器分别记录下机器人的rgb图像、深度图像、相机的空间三维坐标、关节点的空间三维坐标。
3、对采集系统编程控制。分别对机器人和传感器进行脚本编程，使采集过程自动化。webots环境中提供了相关api，可以轻松编写机器人控制脚本。机器人的关节控制主要使用voidwb_motor_set_position(wbdevicetagtag,doubleposition)函数和voidwb_motor_set_velocity(wbdevicetagtag,doublevelocity)函数，分别控制机器人关节的目标位置和运动速度；对视觉传感器的控制使用char*wb_camera_get_image(wbdevicetagtag)函数来获得机器人图像；获取gps传感器的数据主要使用constdouble*wb_gps_get_values(wbdevicetagtag)函数。
最终共采集50000个仿真数据样本，仿真数据会先用于网络的训练，得到一个初步模型之后，在真实数据集上继续训练得到最终的网络模型。
步骤2、利用步骤1中获取机械臂每个轴关节的三维坐标，构建多目标的感知校准神经网络，所述感知校准神经网络包括机器人检测网络以及关键点检测网络，机器人检测网络以rfbnet作为基础网络框架，关键点检测网络以resnet34作为基础网络框架；对机器人检测网络，输入机器人的rgb图像能得到机器人的边界框，对关键点检测网络，输入机器人的深度图像以及机器人边界框能得到机器人上关键点的三维空间坐标；
在训练感知校准神经网络时，采用边界框匹配作为机器人检测网络的损失函数，采用均方误差作为关键点检测网络的损失函数；
本发明实施例中，构建一个基于多目标的深度神经网络结构，因此可以输出多个结果。具体地，神经网络主要训练如下目标：1）、机器人基座的三维坐标；2）、机器人的四个关节三维坐标：肩关节、肘关节、腕关节、机械臂末端关节，珞石xb4型机器人共有6轴关节，其中腰部转动轴与基座位置较近，腕部旋转关节相对位置不发生变化，不考虑检测；3）、机器人边界框。机器人是边界框是机器人在图像中最有可能出现的位置，在训练数据集中就已经人工标注好边界框的位置。
具体地，构建得到的神经网络主要有两个分支，其中一个分支是机器人检测网络，输入机器人的rgb图像，输出机器人边界框；另一个分支是关键点检测网络，输入机器人深度图像以及机器人边界框，输出机器人本体上五个关键点的三维空间坐标。机器人的五个关键点包括：机器人基座、肩关节、肘关节、腕关节和机器人手臂末端。
在训练神经网络的过程中，将机器人rgb图像、深度图像、机器人边界框位置输入到目标检测分支网络，训练完成后得到机器人的边界框检测模型，将边界框、rgb图像、深度图像以及关键点坐标信息输入到关键点预测网络进行训练，训练完成后得到关键点预测模型。
训练过程中，机器人图像分别经过神经网络的卷积层、池化层、全连接层生成机器人关键点热图，热图为机器人关键点的分布概率，某一区域的概率值越大，则越接近目标关键点的位置。对热图使用积分回归的方法得到关键点的预测坐标，实现端到端的训练。积分方法是对规范化后的热图在三维坐标系
根据网络架构，设计损失函数来使网络收敛。对于机器人五个关键点的坐标预测，使用均方误差作为损失函数，同时计算每两个关节之间的距离，即机器人连杆的长度，并计算与真实连杆长度之间的差值，作为另一个损失函数。
针对机器人检测网络分支，对边界框设计损失函数。机器人边界框的表示方法为矩形框左上和右下两个像素点坐标。对于网络输出——机器人边界框，不能简单地用边界框面积的大小和顶点位置来评估预测的准确性，因为边界框是否准确的标准是机器人本体是否全部在边界框内。理想机器人边界框是图像中机器人全部在边界框内并且框内所包含的背景最少。
机器人检测网络以rfbnet作为基础网络框架。关键点检测网络以resnet34网络模型作为基本架构，resnet34网络有34个卷积层，卷积核大小为3×3，使用relu函数作为激活函数，表达式为：
在图像经过神经网络的卷积处理后，得到有关机器人关键点的热图
其中，
其中，
在训练的每一次迭代中，图像输入到网络中进行卷积处理，输出特征图和预测关键点坐标，预测值与真实值通过损失函数进行误差计算，将损失值反馈到网络中进行调整，然后进行下一次迭代，网络共进行500次迭代。
构建了多个损失函数来使关键点的预测值与真实值拟合，具体拟合的方法为本技术领域常用的方法。拟合过程体现在神经网络的训练过程中，随着迭代次数的增加，预测值就越接近真实值。。损失函数是神经网络训练中的性能度量，用来计算真实值与预测值之间的误差。神经网络进行前向传播阶段，依次调用每一层得到逐层输出，最后一层为计算损失函数，更新预测值与真实值之间的误差。损失函数层是神经网络前向传播的终点，也是反向传播的起点，反向传播是一个不断更新权重
神经网络有三个目标输出，因为同时针对所有目标对网络进行训练，所以将多个目标的损失函数组合为一个总体的损失函数用于实际训练，具体为下述的公式（9）。对于机器人边界框的定义是机器人本体在图像中最有可能出现的位置，在保证机器人本体出现在框内的情况下使得边界框最小，最小化背景信息。在训练过程中，将机器人本体特征及关键点坐标信息融合进网络，使得网络在预测边界框的时候会额外考虑关键点的位置以纠正预测结果。机器人边界框的表示方法为矩形框左上角和右下角两个点的坐标，如果关键点位置不在边界框内，则损失函数为最大；关键点位置在边界框内则正常计算顶点之间的误差。机器人边界框损失函数如下：
其中
机器人基座坐标和关节坐标的损失函数都表述为回归任务，它们都使用估计值与基本事实数据之间的欧氏距离。公式（5）为机器人关节三维坐标的损失函数
同样，公式（6）为机器人基座坐标的损失函数
公式（7）为机器人连杆损失函数。
其中，
最终的损失函数
神经网络同时对三个目标进行训练，为了得到理想的训练效果，对损失函数的权重进行了配比以达到最优的效果。对每个损失函数权重取值为：
在训练过程中每迭代一次，网络都会得到一组关键点的预测值，此时计算预测值与真实值之间的损失值，将损失值回馈到网络中进行下一轮的迭代，直到网络不再收敛，即损失值不会继续变小，结束训练。
步骤3、利用上述的感知校准神经网络得到机器人位姿检测模型，利用机器人位姿检测模型，在输入机器人rgb图像时，能得到机器人关键点在图像中对应的像素点坐标以及关键点的深度值。
本网络训练所使用的数据集包括15000个真实样本和50000个仿真样本，，测试集全部为真实图像。将图像的输入大小缩小为原始尺寸的一半，降至256×256像素，并将输入图像的像素值归一化为介于0和1之间的范围。此外为了避免任何训练偏差，将数据随机打包并分成64个图像进行小批处理以充分利用gpu内存。在训练开始时将学习率设置为0.001，然后随着训练的进行逐渐降低至0.000001，卷积神经网络共迭代500次。使用常规的nvidiageforce2080ti显卡进行训练。
输入的机器人图像会经过缩放层、卷积层、池化层等一系列计算神经网络在训练过程中会逐渐收敛，即损失函数的值越来越小，每一轮迭代之后都会改变神经网络中的参数，然后开始下一轮的迭代。在迭代近400次时，网络不在收敛，关键点误差在2.0-4.0cm范围内浮动。此时的神经网络即为机器人姿态检测模型。
输入为大小640*480像素的机器人rgb图像，先通过卷积神经网络模块得到图像的机器人本体信息和heatmap信息，机器人本体信息将输入机器人检测模块，经过这一分支输出机器人边界框。heatmap信息则经过训练好的回归模型计算出机器人的基座坐标和关节坐标。使用3000张正式图像作为测试集，将模型输出的机器人边界框、基座坐标和关节坐标与真实数据值作对比，在机器人边界框方面，预测值与估计值重合率为96%，关节平均误差为2.6cm，基座坐标平均误差为2.1cm。
1.一种近距离机械臂感知与校准方法，其特征是，所述感知与校准方法包括如下步骤：
步骤1、进行数据采集，采集的数据包括机器人的rgb图像和深度图像，并根据所采集的rgb图像、深度图像获取机械臂当前状态下每个轴关节的三维坐标；
步骤2、利用步骤1中获取机械臂每个轴关节的三维坐标，构建多目标的感知校准神经网络，所述感知校准神经网络包括机器人检测网络以及关键点检测网络，机器人检测网络以rfbnet作为基础网络框架，关键点检测网络以resnet34作为基础网络框架；对机器人检测网络，输入机器人的rgb图像能得到机器人的边界框，对关键点检测网络，输入机器人的深度图像以及机器人边界框能得到机器人上关键点的三维空间坐标；
在训练感知校准神经网络时，采用边界框匹配作为机器人检测网络的损失函数，采用均方误差作为关键点检测网络的损失函数；
步骤3、利用上述的感知校准神经网络得到机器人位姿检测模型，利用机器人位姿检测模型，在输入机器人rgb图像时，能得到机器人关键点在图像中对应的像素点坐标以及关键点的深度值。
2.根据权利要求1所述的近距离机械臂感知与校准方法，其特征是，步骤1中，采集的数据包括真实世界数据以及仿真数据；
在采集真实世界数据时，使用kinect相机采集机器人的rgb图像以及深度图像，kinect相机架设在机械臂前方，通过手眼校准确定机械臂座在相机坐标系下的三维坐标，同时获得机械臂在当前状态下的每个轴关节的角度，利用正向运动学方式确定每个轴关节在相机坐标系下的三维坐标；
采集仿真数据时，利用仿真平台webots按照真实环境搭建背景，并利用仿真环节中的gps传感器获取世界坐标系下的三维坐标。
3.根据权利要求1所述的近距离机械臂感知与校准方法，其特征是，关键点检测网络的检测的关键点为机器人基座、肩关节、肘关节、腕关节以及机器人手臂末端。
技术总结