本发明提供一种针对机械臂运动控制的手臂引导运动控制方法，包括：基于捕获的彩色图像和深度图像，实现三维人体姿态识别，提取出人体骨架模型，得到手臂关节的三维坐标；建立手臂模型和机械臂的空间模型，并建立手臂与机械臂之间的映射关系；将机械臂与手臂转换到同一坐标系，根据手臂与机械臂之间的映射关系，通过手臂关节的三维坐标得到对应的机械臂关节的三维坐标；将机械臂关节的三维坐标转换为空间向量，利用空间向量法获得机械臂的关节值，基于获得的机械臂的关节值完成机械臂的运动控制。本发明涉及机械臂运动控制领域，可实现机械臂的半自主运动控制并提高机械臂控制的灵活性。
1.一种针对机械臂运动控制的手臂引导运动控制方法，其特征在于，所述手臂引导运动控制方法包括：通过视觉传感器捕获视觉范围内的彩色图像和深度图像，实现三维人体姿态识别，并提取出人体骨架模型，得到手臂关节的三维坐标；
建立手臂模型和机械臂的空间模型，并建立手臂与机械臂之间的映射关系；
将机械臂与手臂转换到同一坐标系，根据手臂与机械臂之间的映射关系，通过手臂关节的三维坐标得到对应的机械臂关节的三维坐标；
将机械臂关节的三维坐标转换为空间向量，利用空间向量法获得机械臂的关节值，基于获得的机械臂的关节值完成机械臂的运动控制；
其中，所述通过视觉传感器捕获视觉范围内的彩色图像和深度图像，实现三维人体姿态识别，并提取出人体骨架模型，包括：通过RGB-D深度相机捕获视觉范围内的彩色图像和深度图像，并对捕获的彩色图像和深度图像进行配准；
将配准后的彩色图像输入OpenPose框架，得到二维人体姿态识别图像，并结合配准后的深度图像，实现三维人体姿态识别，并提取出人体骨架模型；
设Pir为在深度摄像头坐标下某点的空间坐标，pir为该点在像平面上的投影坐标，Hir为深度摄像头内参矩阵，由小孔成像模型可知，其满足以下关系：pir＝HirPir
设Prgb为在RGB摄像头坐标下同一点的空间坐标，prgb为该点在RGB像平面上的投影坐标，Hrgb为RGB摄像头的内参矩阵；由于深度摄像头的坐标和RGB摄像头的坐标不同，它们之间可通过一个旋转平移变换联系起来，即：Prgb＝RPir+T
最后再用Hrgb对Prgb投影，即可得到该点对应的RGB坐标：prgb＝HrgbPrgb
外参矩阵实际上也是由一个旋转矩阵Rir或Rrgb和平移向量Tir或Trgb构成的，它表示将一个全局坐标系下的点P变换到摄像头坐标系下，分别对深度摄像头和RGB摄像头进行变换，有以下关系：Pir＝RirP+Tir
通过最后一个公式即可实现彩色图像和深度图像的配准。
2.如权利要求1所述的针对机械臂运动控制的手臂引导运动控制方法，其特征在于，所述将配准后的彩色图像输入OpenPose框架，得到二维人体姿态识别图像，包括：将配准后的彩色图像输入二分支的卷积神经网络，并预测身体监测的二维置信图S(J)和亲和域L(c)，在L(c)中的每个图像位置都编码一个2D向量后，通过二分配匹配解析置信图和亲和域来确定关节点所属对象，并将相邻关节点连接为肢体，输出图像中所有人的2D识别图，并提取关节点的二维坐标数据，其中，所提取数据包括15个关节点。
3.如权利要求2所述的针对机械臂运动控制的手臂引导运动控制方法，其特征在于，所述结合配准后的深度图像，实现三维人体姿态识别，并提取出人体骨架模型，包括：在彩色图像中得到的关节点坐标为(u，v)，映射到深度图像中的坐标为(u，v，d)，通过针孔相机原理可得：d＝z*s
其中，fx，fy是相机在x轴、y轴的焦距，cx,cy是相机的光圈中心，s为深度图的缩放因子；
通过转换，得到(u，v，d)对应的空间坐标(x，y，z)，从而实现从2D关节点到3D关节点的转换；
转换后根据三维关节点数据，提取人体骨架模型；其中，人体骨架模型包含15个预定义的点和14条连接线，这些点定义为He：头部，Ne：颈部，Ls：左肩部，Rs：右肩部，Le：左肘，Re：右肘，Lw：左手腕，Rw：右手腕，Hb：半身，Lt：左大腿，Rt：右大腿，Lk：左膝，Rk：右膝，La：左脚踝，Ra：右脚踝；这些点之间有14条线，包括He-Ne，Ne-Ls，Ne-Rs，Ls-Le，Le-Lw，Rs-Re，Re-Rw，Nk-Hb，Hb-Lt，Lt-Lk，Lk-La，Hb-Rt，Rt-Rk和Rk-Ra。
4.如权利要求1所述的针对机械臂运动控制的手臂引导运动控制方法，其特征在于，所述建立手臂模型和机械臂的空间模型，包括：基于手臂自由度建立相应手臂模型；其中，手臂自由度包括：肩关节的水平自由度和垂直自由度、大臂的旋转自由度、肘关节的旋转自由度、小臂关节的垂直自由度、手腕关节的旋转自由度以及手的自由度；
根据关节坐标系建立规则和右手定则建立机械臂每个关节的坐标系，并在建立的关节坐标系的基础上描述机械臂的四个参数，其中连杆描述的连杆参数有两个，一个是连杆的长度a，另一个是连杆的转角α，其次就是描述连杆之间关系的连杆偏矩和关节角；利用上面四个参数建立D-H参数矩阵，从而构建一个机械臂的空间模型。
5.如权利要求4所述的针对机械臂运动控制的手臂引导运动控制方法，其特征在于，所述建立手臂与机械臂之间的映射关系，包括：根据机械臂的关节自由度，选取大臂的旋转自由度、肩部的水平自由度、肩部的垂直自由度和肘部的垂直自由度与机械臂自由度建立映射关系，从而实现操作者的手臂动作和机械臂之间实时和连续地一对一的映射。
6.如权利要求5所述的针对机械臂运动控制的手臂引导运动控制方法，其特征在于，所述将机械臂与手臂转换到同一坐标系具体为通过手眼标定方法将机械臂与手臂转换到同一坐标系；其中，手眼标定包括相机标定和机械臂标定；机械臂标定是将机械臂坐标系转换为世界坐标系，相机标定是将像素坐标系转换到图像坐标系转换到相机坐标系再到世界坐标系，由此可以确定像素坐标系与机械臂坐标系之间的转化关系；具体标定方法如下：通过在机械臂末端粘贴标签并在捕获的图像中识别出标签，并计算标签的中心点，来获得机械臂末端在像素坐标系中的坐标；之后通过深度相机得到的深度值，得到相机坐标系下机械臂末端的三维坐标；最后通过一个旋转矩阵R和平移矩阵T，即可获得两个坐标系之间的转换关系。
7.如权利要求1所述的针对机械臂运动控制的手臂引导运动控制方法，其特征在于，所述利用空间向量法获得机械臂的关节值，包括：将肩部(S)、肘部(E)和腕部(W)的三维坐标信息转化为空间向量ES，即肘部到肩部连接为向量，方向指向肩部；和空间向量EW，即肘部和腕部连接为向量，方向指向腕部；
对于肘部垂直关节值的计算，使用空间向量ES和EH的夹角得出，计算过程如下：ES＝(SX-EX,SY-EY,SZ-EZ)
其中，SX、SY、SZ为肩部(S)的三维坐标值；EX、EY、EZ为肘部(E)的三维坐标值；WX、WY、WZ为腕部(W)的三维坐标值；
肩部的垂直旋转角度，是将向量ES投影到xoy平面，通过求解其与y坐标轴的夹角得到，计算过程如下：ES＝(SX-EX,SY-EY,0)
肩部的水平旋转角度，是将向量ES投影到xoz平面，求解其与x轴夹角：ES＝(SX-EX,0,SZ-EZ)
对于大臂的旋转角度，采用一种渐进算法，求取空间平面xoz与肩部、肘部、腕部所组成的平面的夹角作为大臂的旋转角度，计算公式如下：ES＝(SX-EX,SY-EY,SZ-EZ)
通过空间向量法，计算出肘部、肩部和大臂的四个自由度的旋转角度θ1、θ2、θ3、θ4，即可完成机械臂对人手臂的动作跟随。
8.如权利要求1所述的针对机械臂运动控制的手臂引导运动控制方法，其特征在于，所述基于获得的机械臂的关节值完成机械臂的运动控制，包括：上位机将获得的机械臂的关节值通过ROS控制系统传递给Arbotix-M控制板，所述Arbotix-M控制板根据机械臂的关节值产生控制信号，并将所述控制信号发送给机械臂的舵机，所述舵机收到所述控制信号后，驱动电机改变舵角，同时机械臂将自身关节信息通过所述Arbotix-M控制板反馈回所述上位机，随时监控机械臂的关节状态，从而实现对机械臂的运动闭环控制。
一种针对机械臂运动控制的手臂引导运动控制方法
技术领域
[0001] 本发明涉及机械臂的运动控制领域，特别是指一种针对机械臂运动控制的手臂引导运动控制方法。
背景技术
[0002] OpenPose是卡内基梅隆大学(Carnegie Mellon University)的研究人员开发的一个身体跟踪系统。该系统能实时对人的手、肢体、脸部(总共130个关键点)进行探测追踪。它使用计算机视觉和机器学习技术来处理视频帧，可以同时跟踪多个人的运动。用于训练的各个部件的姿态数据集在专门球形装置(大众多视觉系统)中进行收集，充足的样本数据保证了模型的鲁棒性。装置的部分参数如下：480VGA摄像机视图，30+高清视图，10个RGB-D传感器，基于硬件的同步等。相对于同类其他的方法，基于OpenPose的方法能更好的减少多人场景下互相遮挡导致的肢体畸形，能够准确区分每个人物框架。
[0003] 在控制机械臂运动之前，需要建立每个关节的坐标系和机械臂的D-H参数矩阵，进而建立一个机械臂的空间模型并对机械臂进行描述。
[0004] D-H法是由Denavit和Hartenberg提出的用于机械臂结构分析的方法，这种方法在机器人的每个连杆上都固连一个坐标系，然后使用4x4的齐次变换矩阵来描述两连杆之间的空间位置关系，并最终得到机械臂末端坐标系相对于参考坐标系的等价齐次变换矩阵，建立机械臂的运动方程。
[0005] 目前，中国专利文献(申请号：2018105583151.2，申请日：2018.06.05，申请公布号：CN 108714914 A)公开了一种机械臂视觉系统，双目摄像机采集机械臂工作区的特征对象图像数据；图像处理模块接收双目摄像机传输的图像数据，采用对称式卷积神经网络图像去噪模式对双目摄像机摄取的图像数据提取特征值，进行滤波处理；并根据提取的特征值重建原始图像数据；手眼标定模块标定图像处理模块传输的图像数据上特征对象的坐标，并统一特征对象的坐标和机械臂末端坐标；机械臂控制模块实时接收机械臂的运动参数和运动轨迹，根据统一后的坐标生成机械臂运动轨迹指令，实时控制机械臂运动；提高了机械臂控制智能化程度。但是该方法相对于人机交互的机械臂控制而言，缺乏一定的灵活性。
发明内容
[0006] 本发明要解决的技术问题是针对现有的机械臂运动控制方法，相对于人机交互的机械臂控制而言，缺乏灵活性的问题，提供一种针对机械臂运动控制的手臂引导运动控制方法，以提高机械臂控制的灵活性。
[0007] 为解决上述技术问题，本发明提供一种针对机械臂运动控制的手臂引导运动控制方法，所述手臂引导运动控制方法包括：
[0008] 通过视觉传感器捕获视觉范围内的彩色图像和深度图像，实现三维人体姿态识别，并提取出人体骨架模型，得到手臂关节的三维坐标；
[0009] 建立手臂模型和机械臂的空间模型，并建立手臂与机械臂之间的映射关系；
[0010] 将机械臂与手臂转换到同一坐标系，根据手臂与机械臂之间的映射关系，通过手臂关节的三维坐标得到对应的机械臂关节的三维坐标；
[0011] 将机械臂关节的三维坐标转换为空间向量，利用空间向量法获得机械臂的关节值，基于获得的机械臂的关节值完成机械臂的运动控制。
[0012] 进一步地，所述通过视觉传感器捕获视觉范围内的彩色图像和深度图像，实现三维人体姿态识别，并提取出人体骨架模型，包括：
[0013] 通过RGB-D深度相机捕获视觉范围内的彩色图像和深度图像，并对捕获的彩色图像和深度图像进行配准；
[0014] 将配准后的彩色图像输入OpenPose框架，得到二维人体姿态识别图像，并结合配准后的深度图像，实现三维人体姿态识别，并提取出人体骨架模型。
[0015] 进一步地，所述对捕获的彩色图像和深度图像进行配准，包括：
[0016] 设Pir为在深度摄像头坐标下某点的空间坐标，pir为该点在像平面上的投影坐标，Hir为深度摄像头内参矩阵，由小孔成像模型可知，其满足以下关系：
[0017] pir＝HirPir
[0018]
[0019] 设Prgb为在RGB摄像头坐标下同一点的空间坐标，prgb为该点在RGB像平面上的投影坐标，Hrgb为RGB摄像头的内参矩阵；由于深度摄像头的坐标和RGB摄像头的坐标不同，它们之间可通过一个旋转平移变换联系起来，即：
[0020] Prgb＝RPir+T
[0021] 其中，R为旋转矩阵，T为平移向量；
[0022] 最后再用Hrgb对Prgb投影，即可得到该点对应的RGB坐标：
[0023] prgb＝HrgbPrgb
[0024] 外参矩阵实际上也是由一个旋转矩阵Rir或Rrgb和平移向量Tir或Trgb构成的，它表示将一个全局坐标系下的点P变换到摄像头坐标系下，分别对深度摄像头和RGB摄像头进行变换，有以下关系：
[0025] Pir＝RirP+Tir
[0026] Prgb＝RrgbP+Trgb
[0027] 通过以上公式进行推算对比可得：
[0028]
[0029]
[0030] Zrgb*prgb＝R*Zir*pir+T
[0031] 通过最后一个公式即可实现彩色图像和深度图像的配准。
[0032] 进一步地，所述将配准后的彩色图像输入OpenPose框架，得到二维人体姿态识别图像，包括：
[0033] 将配准后的彩色图像输入二分支的卷积神经网络，并预测身体监测的二维置信图S(J)和亲和域L(c)，在L(c)中的每个图像位置都编码一个2D向量后，通过二分配匹配解析置信图和亲和域来确定关节点所属对象，并将相邻关节点连接为肢体，输出图像中所有人的2D识别图，并提取关节点的二维坐标数据，其中，所提取数据包括15个关节点。
[0034] 进一步地，所述结合配准后的深度图像，实现三维人体姿态识别，并提取出人体骨架模型，包括：
[0035] 在彩色图像中得到的关节点坐标为(u，v)，映射到深度图像中的坐标为(u，v，d)，通过针孔相机原理可得：
[0036]
[0037]
[0038] d＝z*s
[0039] 其中，fx，fy是相机在x轴、y轴的焦距，cx,cy是相机的光圈中心，s为深度图的缩放因子；
[0040]
[0041]
[0042] z＝d/s
[0043] 通过转换，得到(u，v，d)对应的空间坐标(x，y，z)，从而实现从2D关节点到3D关节点的转换；
[0044] 转换后根据三维关节点数据，提取人体骨架模型；其中，人体骨架模型包含15个预定义的点和14条连接线，这些点定义为He：头部，Ne：颈部，Ls：左肩部，Rs：右肩部，Le：左肘，Re：右肘，Lw：左手腕，Rw：右手腕，Hb：半身，Lt：左大腿，Rt：右大腿，Lk：左膝，Rk：右膝，La：左脚踝，Ra：右脚踝；这些点之间有14条线，包括He-Ne，Ne-Ls，Ne-Rs，Ls-Le，Le-Lw，Rs-Re，Re-Rw，Nk-Hb，Hb-Lt，Lt-Lk，Lk-La，Hb-Rt，Rt-Rk和Rk-Ra。
[0045] 进一步地，所述建立手臂模型和机械臂的空间模型，包括：
[0046] 基于手臂自由度建立相应手臂模型；其中，手臂自由度包括：肩关节的水平自由度和垂直自由度、大臂的旋转自由度、肘关节的旋转自由度、小臂关节的垂直自由度、手腕关节的旋转自由度以及手的自由度；
[0047] 根据关节坐标系建立规则和右手定则建立机械臂每个关节的坐标系，并在建立的关节坐标系的基础上描述机械臂的四个参数，其中连杆描述的连杆参数有两个，一个是连杆的长度a，另一个是连杆的转角α，其次就是描述连杆之间关系的连杆偏矩和关节角；利用上面四个参数建立D-H参数矩阵，从而构建一个机械臂的空间模型。
[0048] 进一步地，所述建立手臂与机械臂之间的映射关系，包括：
[0049] 根据机械臂的关节自由度，选取大臂的旋转自由度、肩部的水平自由度、肩部的垂直自由度和肘部的垂直自由度与机械臂自由度建立映射关系，从而实现操作者的手臂动作和机械臂之间实时和连续地一对一的映射。
[0050] 进一步地，所述将机械臂与手臂转换到同一坐标系具体为通过手眼标定方法将机械臂与手臂转换到同一坐标系；其中，手眼标定包括相机标定和机械臂标定；机械臂标定是将机械臂坐标系转换为世界坐标系，相机标定是将像素坐标系转换到图像坐标系转换到相机坐标系再到世界坐标系，由此可以确定像素坐标系与机械臂坐标系之间的转化关系；具体标定方法如下：
[0051] 通过在机械臂末端粘贴标签并在捕获的图像中识别出标签，并计算标签的中心点，来获得机械臂末端在像素坐标系中的坐标；之后通过深度相机得到的深度值，得到相机坐标系下机械臂末端的三维坐标；最后通过一个旋转矩阵R和平移矩阵T，即可获得两个坐标系之间的转换关系。
[0052] 进一步地，所述利用空间向量法获得机械臂的关节值，包括：
[0053] 将肩部(S)、肘部(E)和腕部(W)的三维坐标信息转化为空间向量ES，即肘部到肩部连接为向量，方向指向肩部；和空间向量EW，即肘部和腕部连接为向量，方向指向腕部；
[0054] 对于肘部垂直关节值的计算，使用空间向量ES和EH的夹角得出，计算过程如下：
[0055] ES＝(SX-EX,SY-EY,SZ-EZ)
[0056] EW＝(WX-EX,WY-EY,WZ-EZ)
[0057]
[0058] 其中，SX、SY、SZ为肩部(S)的三维坐标值；EX、EY、EZ为肘部(E)的三维坐标值；WX、WY、WZ为腕部(W)的三维坐标值；
[0059] 肩部的垂直旋转角度，是将向量ES投影到xoy平面，通过求解其与y坐标轴的夹角得到，计算过程如下：
[0060] ES＝(SX-EX,SY-EY,0)
[0061] n1＝(0,100,0)
[0062]
[0063] 肩部的水平旋转角度，是将向量ES投影到xoz平面，求解其与x轴夹角：
[0064] ES＝(SX-EX,0,SZ-EZ)
[0065] n1＝(100,0,0)
[0066]
[0067] 对于大臂的旋转角度，采用一种渐进算法，求取空间平面xoz与肩部、肘部、腕部所组成的平面的夹角作为大臂的旋转角度，计算公式如下：
[0068] ES＝(SX-EX,SY-EY,SZ-EZ)
[0069] EW＝(WX-EX,WY-EY,WZ-EZ)
[0070] n1＝EW*ES
[0071] n2＝(0,100,0)
[0072]
[0073] 通过空间向量法，计算出肘部、肩部和大臂的四个自由度的旋转角度θ1、θ2、θ3、θ4，即可完成机械臂对人手臂的动作跟随。
[0074] 进一步地，所述基于获得的机械臂的关节值完成机械臂的运动控制，包括：
[0075] 上位机将获得的机械臂的关节值通过ROS控制系统传递给Arbotix-M控制板，所述Arbotix-M控制板根据机械臂的关节值产生控制信号，并将所述控制信号发送给机械臂的舵机，所述舵机收到所述控制信号后，驱动电机改变舵角，同时机械臂将自身关节信息通过所述Arbotix-M控制板反馈回所述上位机，随时监控机械臂的关节状态，从而实现对机械臂的运动闭环控制。
[0076] 本发明的上述技术方案的有益效果如下：
[0077] 本发明的针对机械臂运动控制的手臂引导运动控制方法，针对像素不匹配的问题，进行深度图像与彩色图像的配准；为了获取手臂关节三维坐标数据，提出基于视觉的三维人体姿态识别，并提取人体骨架模型；建立手臂模型和基于D-H参数的机械臂的空间模型，建立手臂与机械臂映射关系；通过手眼标定方法将机械臂与手臂转换到同一坐标系；将三维坐标转化为空间向量，利用空间向量法获得机械臂的关节值，将关节值通过ROS控制系统传递到Arbotix控制板，驱动舵机完成机械臂的运动控制；提高了机械臂控制的灵活性。
附图说明
[0078] 图1为本发明实施例提供的针对机械臂运动控制的手臂引导运动控制方法的流程示意图；
[0079] 图2为本发明实施例提供的二维人体姿态识别示意图；
[0080] 图3为本发明实施例提供的三维人体骨架提取示意图；
[0081] 图4为本发明实施例提供的人体骨架模型示意图；
[0082] 图5为本发明实施例提供的手臂模型示意图；
[0083] 图6为本发明实施例提供的机械臂空间模型示意图；
[0084] 图7为本发明实施例提供的肘部垂直自由度、肩部垂直自由度和肩部水平自由度空间向量法示意图；
[0085] 图8为本发明实施例提供的大臂旋转自由度空间向量法示意图；
[0086] 图9为本发明实施例提供的机械臂运动控制流程示意图。
具体实施方式
[0087] 为使本发明要解决的技术问题、技术方案和优点更加清楚，下面将结合附图及具体实施例进行详细描述。
[0088] 如图1所示，本实施例提供一种针对机械臂运动控制的手臂引导运动控制方法，该手臂引导运动控制方法包括：
[0089] S101，通过视觉传感器捕获视觉范围内的彩色图像和深度图像，实现三维人体姿态识别，并提取出人体骨架模型，得到手臂关节的三维坐标；
[0090] S102，建立手臂模型和机械臂的空间模型，并建立手臂与机械臂映射关系；
[0091] S103，将机械臂与手臂转换到同一坐标系，根据手臂与机械臂之间的映射关系，通过手臂关节的三维坐标得到对应的机械臂关节的三维坐标；
[0092] S104，将机械臂关节的三维坐标转换为空间向量，利用空间向量法获得机械臂的关节值，基于获得的机械臂的关节值完成机械臂的运动控制。
[0093] 具体地，在本实施例中，上述通过视觉传感器捕获视觉范围内的彩色图像和深度图像，实现三维人体姿态识别，并提取出人体骨架模型，包括：
[0094] 通过RGB-D深度相机捕获视觉范围内的彩色图像和深度图像，并针对像素不匹配的问题，对捕获的彩色图像和深度图像进行配准；
[0095] 将配准后的彩色图像输入OpenPose框架，得到二维人体姿态识别图像，并结合配准后的深度图像，实现三维人体姿态识别，并提取出人体骨架模型。
[0096] 进一步地，在本实施例中，对捕获的彩色图像和深度图像进行配准，包括：
[0097] 设Pir为在深度摄像头坐标下某点的空间坐标，pir为该点在像平面上的投影坐标(x、y单位为像素，z为深度值，其单位为毫米)，Hir为深度摄像头内参矩阵，由小孔成像模型可知，它们满足以下关系：
[0098] pir＝HirPir
[0099]
[0100] 设Prgb为在RGB摄像头坐标下同一点的空间坐标，prgb为该点在RGB像平面上的投影坐标，Hrgb为RGB摄像头的内参矩阵；由于深度摄像头的坐标和RGB摄像头的坐标不同，它们之间可通过一个旋转平移变换联系起来，即：
[0101] Prgb＝RPir+T
[0102] 其中，R为旋转矩阵，T为平移向量；
[0103] 最后再用Hrgb对Prgb投影，即可得到该点对应的RGB坐标：
[0104] prgb＝HrgbPrgb
[0105] 外参矩阵实际上也是由一个旋转矩阵Rir(Rrgb)和平移向量Tir(Trgb)构成的，它表示将一个全局坐标系下的点P变换到摄像头坐标系下，分别对深度摄像头和RGB摄像头进行变换，有以下关系：
[0106] Pir＝RirP+Tir
[0107] Prgb＝RrgbP+Trgb
[0108] 通过以上公式进行推算对比可得：
[0109]
[0110]
[0111] Zrgb*prgb＝R*Zir*pir+T
[0112] 其中，Zrgb为配准后的RGB摄像头坐标下的坐标值，Zir为配准后的深度摄像头坐标下的坐标值；通过最后一个公式即可实现彩色图像和深度图像的配准。
[0113] 进一步地，如图2所示，在本实施例中，将配准后的彩色图像输入OpenPose框架，得到二维人体姿态识别图像，包括：
[0114] 将配准后的彩色图像输入二分支的卷积神经网络，并预测身体监测的二维置信图S(J)和亲和域L(c)，在L(c)中的每个图像位置都编码一个2D向量后，通过二分配匹配(贪婪推断)解析置信图和亲和域来确定关节点所属对象，并将相邻关节点连接为肢体，输出图像中所有人的2D识别图，并提取关节点的二维坐标数据，其中，所提取数据包括15个关节点。
[0115] 进一步地，在本实施例中，如图3所示，骨架模型是根据结合配准后的深度图像和二维识别图像得到三维姿态数据提取构建的，其过程包括：
[0116] 在彩色图像中得到的关节点坐标为(u，v)，映射到深度图像中的坐标为(u，v，d)，通过针孔相机原理可得：
[0117]
[0118]
[0119] d＝z*s
[0120] 其中，fx，fy是相机在x轴、y轴的焦距，cx,cy是相机的光圈中心，s为深度图的缩放因子；
[0121]
[0122]
[0123] z＝d/s
[0124] 通过转换，得到(u，v，d)对应的空间坐标(x，y，z)，从而实现从2D关节点到3D关节点的转换；
[0125] 转换后根据三维关节点数据，提取人体骨架模型；如图4所示，其中，人体骨架模型包含15个预定义的点和14条连接线，这些点定义为：He(头部)，Ne(颈部)，Ls(左肩部)，Rs(右肩部)，Le(左肘)，Re(右肘)，Lw(左手腕)，Rw(右手腕)，Hb(半身)，Lt(左大腿)，Rt(右大腿)，Lk(左膝)，Rk(右膝)，La(左脚踝)，Ra(右脚踝)；这些点之间有14条线，包括He-Ne，Ne-Ls，Ne-Rs，Ls-Le，Le-Lw，Rs-Re，Re-Rw，Nk-Hb，Hb-Lt，Lt-Lk，Lk-La，Hb-Rt，Rt-Rk和Rk-Ra。这些线用于表示躯干信息，他们也是模型的关键限制。
[0126] 进一步地，在本实施例中，建立手臂模型和机械臂的空间模型，包括：
[0127] 如图5所示，基于手臂自由度建立相应手臂模型；其中，手臂自由度包括：肩关节的水平自由度和垂直自由度、大臂的旋转自由度、肘关节的旋转自由度、小臂关节的垂直自由度、手腕关节的旋转自由度以及手的自由度；
[0128] 如图6所示，本实施例以一个四轴机械臂为例，基于建立的关节坐标系和D-H参数矩阵建立机械臂空间模型；机械臂由关节和连杆组成，根据关节坐标系建立规则和右手定则建立机械臂每个关节的坐标系，并在建立的关节坐标系的基础上描述机械臂的四个参数，其中连杆描述的连杆参数有两个，一个是连杆长度a，另一个是连杆转角α，其次就是描述连杆之间关系的连杆偏矩和关节角；利用上面四个参数建立D-H参数矩阵，从而构建一个机械臂的空间模型。
[0129] 进一步地，在本实施例中，建立手臂与机械臂之间的映射关系，包括：
[0130] 由于机械臂的自由度少于人体手臂自由度，并且限制角度小于手臂，因此根据机械臂的关节自由度，选取大臂的旋转自由度、肩部的水平自由度、肩部的垂直自由度和肘部的垂直自由度与机械臂自由度建立映射关系，从而实现操作者的手臂动作和机械臂之间实时和连续地一对一的映射。
[0131] 进一步地，在本实施例中，将机械臂与手臂转换到同一坐标系具体为通过手眼标定方法将机械臂与手臂转换到同一坐标系；其中，手眼标定包括相机标定和机械臂标定；机械臂标定是将机械臂坐标系转换为世界坐标系，相机标定是将像素坐标系转换到图像坐标系转换到相机坐标系再到世界坐标系，由此可以确定像素坐标系与机械臂坐标系之间的转化关系；具体标定方法如下：
[0132] 首先需要获取机械臂末端在像素坐标系下的坐标；此处为了让标定过程简单化，通过在机械臂末端粘贴标签并在捕获的图像中识别出标签，并计算标签的中心点，来获得机械臂末端在像素坐标系中的坐标；之后通过深度相机得到的深度值，得到相机坐标系下机械臂末端的三维坐标；
[0133] 这样就获得了机械臂末端在像素坐标系和在机械臂坐标系中的坐标，只需要通过一个旋转矩阵R和平移矩阵T，即可获得两个坐标系之间的转换关系。
[0134] 进一步地，本实施例中，利用空间向量法获得机械臂的关节值，包括：
[0135] 了获取手臂的关节值，将获取的三维关节点坐标转换为空间向量，建立模型利用空间向量求取关节值。将肩部(S)、肘部(E)和腕部(W)的三维坐标信息转化为空间向量ES，即肘部到肩部连接为向量，方向指向肩部；和空间向量EW，即肘部和腕部连接为向量，方向指向腕部；
[0136] 如图7所示，对于肘部垂直关节值的计算，使用空间向量ES和EH的夹角得出，计算过程如下：
[0137] ES＝(SX-EX,SY-EY,SZ-EZ)
[0138] EW＝(WX-EX,WY-EY,WZ-EZ)
[0139]
[0140] 其中，SX、SY、SZ为肩部(S)的三维坐标值；EX、EY、EZ为肘部(E)的三维坐标值；WX、WY、WZ为腕部(W)的三维坐标值；
[0141] 肩部的垂直旋转角度，是将向量ES投影到xoy平面，通过求解其与y坐标轴的夹角得到，计算过程如下：
[0142] ES＝(SX-EX,SY-EY,0)
[0143] n1＝(0,100,0)
[0144]
[0145] 同理，肩部的水平旋转角度，是将向量ES投影到xoz平面，求解其与x轴夹角：
[0146] ES＝(SX-EX,0,SZ-EZ)
[0147] n1＝(100,0,0)
[0148]
[0149] 如图8所示，对于大臂的旋转角度，就不能简单的只计算两空间向量的夹角，因此采用一种渐进的算法，求取空间平面xoz与肩部、肘部、腕部所组成的平面的夹角作为大臂的旋转角度，计算公式如下：
[0150] ES＝(SX-EX,SY-EY,SZ-EZ)
[0151] EW＝(WX-EX,WY-EY,WZ-EZ)
[0152] n1＝EW*ES
[0153] n2＝(0,100,0)
[0154]
[0155] 通过空间向量法，计算出肘部、肩部和大臂的四个自由度的旋转角度θ1、θ2、θ3、θ4，已能够完成机械臂对人手臂的动作跟随。
[0156] 进一步地，如图9所示，本实施例中，基于获得的机械臂的关节值完成机械臂的运动控制，具体过程如下：
[0157] 视觉传感器(RGB-D深度相机)将获取的图像传递到上位机，上位机进行图像处理获取机械臂的关节值，并将所获取的机械臂的关节值通过ROS控制系统传递给Arbotix-M控制板，Arbotix-M控制板根据机械臂的关节值产生控制信号(PWM信号)，并将控制信号发送给机械臂的伺服舵机，伺服舵机收到控制信号后，驱动电机改变舵角，从而实现对机械臂的驱动，同时机械臂将自身关节信息通过Arbotix-M控制板反馈回上位机，随时监控机械臂的关节状态，从而实现对机械臂的运动闭环控制。
[0158] 本实施例的针对机械臂运动控制的手臂引导运动控制方法，针对像素不匹配的问题，进行深度图像与彩色图像的配准；为了获取手臂关节三维坐标数据，提出基于视觉的三维人体姿态识别，并提取人体骨架模型；建立手臂模型和基于D-H参数的机械臂的空间模型，建立手臂与机械臂映射关系；通过手眼标定方法将机械臂与手臂转换到同一坐标系；将三维坐标转化为空间向量，利用空间向量法获得机械臂的关节值，将关节值通过ROS控制系统传递到Arbotix控制板，驱动舵机完成机械臂的运动控制；提高了机械臂控制的灵活性。
[0159] 此外，需要说明的是，本领域内的技术人员应明白，本发明实施例的实施例可提供为方法、装置、或计算机程序产品。因此，本发明实施例可采用完全硬件实施例、完全软件实施例、或结合软件和硬件方面的实施例的形式。而且，本发明实施例可采用在一个或多个其中包含有计算机可用程序代码的计算机可用存储介质(包括但不限于磁盘存储器、CD-ROM、光学存储器等)上实施的计算机程序产品的形式。
[0160] 本发明实施例是参照根据本发明实施例的方法、终端设备(系统)、和计算机程序产品的流程图和/或方框图来描述的。应理解可由计算机程序指令实现流程图和/或方框图中的每一流程和/或方框、以及流程图和/或方框图中的流程和/或方框的结合。可提供这些计算机程序指令到通用计算机、嵌入式处理机或其他可编程数据处理终端设备的处理器以产生一个机器，使得通过计算机或其他可编程数据处理终端设备的处理器执行的指令产生用于实现在流程图一个流程或多个流程和/或方框图一个方框或多个方框中指定的功能的装置。
[0161] 这些计算机程序指令也可存储在能引导计算机或其他可编程数据处理终端设备以特定方式工作的计算机可读存储器中，使得存储在该计算机可读存储器中的指令产生包括指令装置的制造品，该指令装置实现在流程图一个流程或多个流程和/或方框图一个方框或多个方框中指定的功能。这些计算机程序指令也可装载到计算机或其他可编程数据处理终端设备上，使得在计算机或其他可编程终端设备上执行一系列操作步骤以产生计算机实现的处理，从而在计算机或其他可编程终端设备上执行的指令提供用于实现在流程图一个流程或多个流程和/或方框图一个方框或多个方框中指定的功能的步骤。
[0162] 尽管已描述了本发明的优选实施例，但本领域内的技术人员一旦得知了基本创造性概念，则可对这些实施例做出另外的变更和修改。所以，所附权利要求意欲解释为包括优选实施例以及落入本发明实施例范围的所有变更和修改。
[0163] 还需要说明的是，在本文中，术语“包括”、“包含”或者其任何其他变体意在涵盖非排他性的包含，从而使得包括一系列要素的过程、方法、物品或者终端设备不仅包括那些要素，而且还包括没有明确列出的其他要素，或者是还包括为这种过程、方法、物品或者终端设备所固有的要素。在没有更多限制的情况下，由语句“包括一个……”限定的要素，并不排除在包括所述要素的过程、方法、物品或者终端设备中还存在另外的相同要素。
[0164] 以上所述是本发明的优选实施方式，应当指出，对于本技术领域的普通技术人员来说，在不脱离本发明所述原理的前提下，还可以做出若干改进和润饰，这些改进和润饰也应视为本发明的保护范围。
