一种基于人眼凝视点的手眼协调抓取方法与流程
本发明属于手眼协调技术领域，更具体地，涉及一种基于人眼凝视点的手眼协调抓取方法。
背景技术：
一方面，人眼凝视点可以由人毫不费力的发出来，并且可以向外界传达出一定的信息，例如，通过观察他人的眼睛，能够知道该人大致的凝视方向和位置，结合场景信息，还能得知该人感兴趣的物体。另一方面，对于手脚不便的残疾人而言，其有抓取东西的需求，但无法使用传统的人机交互方式，此时可以通过眼动仪提取凝视点分析其意图来实现简单直观的人机交互。综上，设计一个操作简便并且能捕捉人眼凝视点用于控制机械臂手抓取用户感兴趣物体的系统是一个亟待解决的问题。
目前具有较多的眼动仪装置和机械臂手装置，但机械臂大多是基于机器视觉的，很少有利用到人眼视觉，更没有人将眼动仪装置提取的三维凝视点用于控制机械臂手抓取感兴趣物体(即所谓的手眼协调)。专利cn109079777a公开了一种机械臂手眼协调作业系统，通过机器视觉的方法估计目标工件的位置和姿态，控制机械臂进行作业，该系统并未用到人眼视觉控制作业，需预先手动设置作业对象。专利cn109048918a公开了一种轮椅机械臂机器人的视觉引导方法，通过机器视觉寻找用户手动输入的感兴趣物体并由机械臂执行抓取任务，同样的，该方法并未考虑人眼视觉，需由残疾人通过传统交互方式预先确定操作对象。专利us8888287b2提出了一种基于三维凝视点追踪器的人机交互系统，其采用了一个深度相机和一个rgb相机提取凝视方向和凝视点用于人机交互，该系统可以提取人眼三维凝视点用于人机交互，但并未用于在实际场景中控制机械臂手抓取用户感兴趣的物体。
技术实现要素：
针对现有技术的以上缺陷或改进需求，本发明提供了一种基于人眼凝视点的手眼协调抓取方法，其基于人眼凝视点确定出用户感兴趣物体的位置，并基于该位置指导机械臂实现物体的抓取，具有抓取准确、操作方便的优点。
为实现上述目的，本发明提出了一种基于人眼凝视点的手眼协调抓取方法，其包括如下步骤：
s1利用眼动仪确定人眼凝视点，并基于人眼凝视点确定用户感兴趣物体的位置；
s2离散化遍历得到用于执行抓取动作的机械臂在空间各位置的可行抓取姿态集合并存储在服务器中；
s3根据步骤s1确定的用户感兴趣物体的位置访问服务器，以查询获得该位置的可行抓取姿态集合；
s4基于可行抓取姿态集合中各抓取姿态反解得到机械臂各关节角的多组解，并在关节角的多组解中确定出一组最优解，即获得机械臂各关节对应的最优关节角，该组最优解对应的抓取位姿即为最佳抓取位姿；
s5机械臂各关节运动至对应的最优关节角，然后以最佳抓取位姿抓取用户感兴趣物体，以此完成基于人眼凝视点的手眼协调抓取。
作为进一步优选的，步骤s1包括如下子步骤：
s11利用眼动仪上的左眼相机和右眼相机分别识别用户的左右瞳孔的中心以提取人眼的信息；
s12将识别获得的左右瞳孔中心映射至前景左相机中以得到二维凝视点；
s13提取前景左相机中的物体锚框，然后根据二维凝视点与物体锚框的位置关系确定用户感兴趣的物体；
s14对用户感兴趣的物体进行三维重建以得到感兴趣物体在前景左相机中的位置；
s15将感兴趣物体在前景左相机中的位置转换至机械臂基坐标系下以此确定用户感兴趣物体的位置。
作为进一步优选的，步骤s4中，优选采用ikfast反解模块反解得到机械臂的各关节角。
作为进一步优选的，步骤s13中，优选利用目标识别与追踪算法提取前景左相机中的物体锚框，具体为：
首先，利用目标识别算法识别得到前景左相机中物体的锚框，利用物体锚框初始化追踪算法中的追踪目标，该目标识别算法和追踪算法同步进行；
然后，初始化后的追踪算法进行物体的追踪，若跟丢物体，则利用目标识别算法的结果重新初始化追踪算法，以此追踪获得物体的锚框。
作为进一步优选的，步骤s14中，利用三维重建算法对用户感兴趣的物体进行三维重建，具体为：
s141标定得到前景左相机和前景右相机的内外参数及重投影矩阵；
s142利用前景左相机和前景右相机内外参数校正并对齐前景左相机和前景右相机中的图像；
s143通过特征匹配算法得到前景左相机和前景右相机图像像素的双目视差值；
s144利用双目视差值与双目标定得到的重投影矩阵重建得到图像中各个像素点对应在前景左相机坐标系下的三维坐标，以此完成物体的三维重建。
作为进一步优选的，步骤s15具体包括如下子步骤：
s151利用眼动仪上的多个红外接收器结合位于机械臂基坐标系内的红外发生器得到眼动仪坐标系相对于机械臂基坐标系的变换矩阵；
s152通过标定获得前景左相机与眼动仪坐标系之间的变换矩阵；
s153根据上述两个变换矩阵将感兴趣物体在前景左相机中的位姿转换至机械臂基坐标系下。
作为进一步优选的，步骤s4中，以最大化所有关节的相对角度裕度的和为优化准则在关节角的多组解中确定出一组最优解，优选以最小化相对角度偏差的绝对值的均值为优化准则在关节角的多组解中确定出一组最优解。
作为进一步优选的，步骤s5中，机械臂各关节运动至对应的关节角后再运动预先获取的执行误差后以使得机械臂到达目标位置，然后以最佳抓取位姿抓取用户感兴趣物体。
作为进一步优选的，所述执行误差采用如下步骤获取：
首先，根据用户感兴趣物体的位置及最佳抓取位姿沿抓取反方向偏离预设的偏置距离得到预抓取位置，访问服务器以查询获得该预抓取位置的可行抓取姿态集合；
然后，基于可行抓取姿态集合中各抓取姿态反解得到机械臂各关节角的多组解，在关节角的多组解中确定出最优解，即目标关节角；
接着，使机械臂各关节运动至对应的目标关节角，然后测量机械臂末端的位置并反解得到机械臂各关节的真实关节角；
最后，计算目标关节角与真实关节角之差得到机械臂各关节的执行误差。
总体而言，通过本发明所构思的以上技术方案与现有技术相比，主要具备以下的技术优点：
1.本发明通过眼动仪确定人眼凝视点，并基于人眼凝视点确定出用户感兴趣物体的位置，然后基于确定出的用户感兴趣物体的位置指导机械臂实现用户感兴趣物体的抓取，具有抓取准确、操作方便的优点，非常适用于手脚不便的残疾人，可有效抓取其感兴趣的物体。
2.本发明通过运动学反解模块ikfast实现抓取姿态的反解，可准确得到机械臂各个关节的目标转角。
3.本发明通过预先离散化遍历工作空间的位置与姿态，生成可达空间数据集合，提高了运动学反解的效率，从而可以实现实时反解。
4.本发明通过使机械臂各关节运动至对应的关节角后再运动预先获取的执行误差(即进行误差补偿)，以此使得机械臂可准确到达目标位置，实现物体的精确抓取。
5.本发明的方法提取了双眼瞳孔中心以确定用户的凝视点，可有效保证凝视点识别的准确性，进而保证感兴趣物体位姿确定的准确性；本发明借助两个前景相机和两个眼睛相机实现用户感兴趣物体的识别与位姿估计，可用于实际工作场景中，利用用户双眼信息控制机械臂手为用户抓取感兴趣物体，为残疾人士带来便利。
6.现有的一般单独使用目标识别算法提取相机中的物体，由于实际工作背景复杂且由于相机抖动其获取的画面不稳定，导致识别率低下，本发明利用目标识别算法融合追踪算法的方式提取前景左相机中的物体，其中目标识别算法识别得到物体锚框用来初始化追踪算法，追踪算法跟丢时利用目标识别算法的结果来重新初始化追踪算法，从而大大提高前景相机中常见物体的识别成功率。
7.本发明利用三维重建算法对用户感兴趣的物体进行三维重建，可以精确定位目标物体在前景相机坐标系下的坐标，结合目标物体姿态识别可以得到目标物体的位姿。
附图说明
图1是本发明实施例提供的基于人眼凝视点的手眼协调抓取方法的流程图；
图2是本发明实施例提供的用于执行手眼协调抓取方法的抓取装置的实际应用图；
图3是本发明实施例提供的用于执行手眼协调抓取方法的抓取装置的实物图；
图4是本发明实施例提供的眼动仪的结构示意图；
图5是本发明实施例提供的感兴趣物体提取的示意图；
图6是由关节角与齐次变换矩阵的转换关系实现反解的示意图。
在所有附图中，相同的附图标记用来表示相同的元件或结构，其中：
1-红外线接收器，2-头戴内圈，3-外圈主体，4-前景左相机，5-前景右相机，6-左眼相机，7-右眼相机，8-外圈上扣板，9-移动旋转部分，10-支架扣板，11-机械臂，12-机架，13-机械手，14-小臂tracker，15-第一连杆，16-第二连杆，17-灯塔，18-机械臂基座，19-小球，20-瓶子。
具体实施方式
为了使本发明的目的、技术方案及优点更加清楚明白，以下结合附图及实施例，对本发明进行进一步详细说明。应当理解，此处所描述的具体实施例仅仅用以解释本发明，并不用于限定本发明。此外，下面所描述的本发明各个实施方式中所涉及到的技术特征只要彼此之间未构成冲突就可以相互组合。
如图1所示，本发明实施例提供了一种基于人眼凝视点的手眼协调抓取方法，其包括如下步骤：
s1利用眼动仪确定人眼凝视点，并基于人眼凝视点确定用户感兴趣物体的位置；
s2离散化遍历得到用于执行抓取动作的机械臂在空间各位置的可行抓取姿态集合并存储在服务器中；
s3根据步骤s1确定的用户感兴趣物体的位置访问服务器，以查询获得该位置的可行抓取姿态集合；
s4基于可行抓取姿态集合中各抓取姿态反解得到机械臂各关节角的多组解，在关节角的多组解中确定出最优解，即获得机械臂各关节对应的最优关节角，确定一组最优的关节角后该组关节角对应的抓取位姿即为最佳抓取位姿；
s5机械臂各关节运动至对应的最优关节角，然后以最佳抓取姿态抓取用户感兴趣物体，以此完成基于人眼凝视点的手眼协调抓取。
如图2-3所示，本发明用于执行上述方法的装置包括机械臂11及眼动仪，机械臂固定在机架12上，充当残疾人右手(也可以左手)，待抓取的物体(瓶子、球等)置于眼动仪前方。如图4所示，眼动仪包括外圈主体3和位于外圈主体3内部的头戴内圈2，外圈主体3和头戴内圈2通过两个移动旋转部分9连接，该外圈主体3上方设置有外圈上扣板8、下方两侧设置有支架扣板10，使用时，眼动议戴在人体头部，外圈上扣板8处于人体头部的后方，支架扣板处于人体面部的前方；前景左相机4和前景右相机5设于外圈主体3的前侧，用于采集用户前方场景图像信息，左眼相机6和右眼相机7设于两支架扣板的前端，用于采集人眼左右图像的信息，在外圈上扣板上均匀分布有多个红外线接收器1，用于接收外部的红外线，该红外线例如由设于眼动仪后方的红外发生器发射，红外发生器与眼动仪之间的距离优选超过一米。使用时，眼动仪可通过数据线将前景左相机4、前景右相机5、左眼相机6和右眼相机7数据传入数据处理系统(电脑)，红外线接收器的相关数据通过无线蓝牙发送至数据处理系统(电脑)，以用于头部姿态估计。
进一步的，本发明采用灯塔定位技术，在眼动仪上安装定位标记(红外接收器)，实现较高精度的头部姿态估计。灯塔定位技术由灯塔(红外发生器)和红外接收器构成，灯塔设置在机械臂基坐标系内，其内部有两个旋转方向正交的电机，两个电机上分别安装有线性红外激光发射器，每个旋转周期都能扫过视野内空间中的所有点。红外接收器安装在眼动仪上，具体为一组红外接收二极管，当接收到红外激光时，能产生响应脉冲，输入到数据处理系统中。灯塔在每个旋转周期的起始时刻有一个全局曝光，随后电机带着线性激光发射器扫过整个空间，每个周期中两个正交电机各进行一次。眼动仪上的红外接收器在全局曝光时产生一个较长时间的脉冲信号，在随后的激光扫射时产生一个较短时间的脉冲信号。处理这些脉冲信号，能够得到全局曝光与激光扫射到某个红外接收器的时间差，假设第i个接收器水平方向和竖直方向旋转激光的扫射时间差分别为δti1、δti2，由于电机转速r已知，可进一步算出对应水平电机和竖直电机的扫射角度：
进而，每个红外接收器在灯塔虚拟平面上2d点的坐标可表示为：
同时，眼动仪坐标系下各红外接收器的3d点坐标pi＝(xi,yi,zi)，可由外圈主体设计参数得到；
3d到2d点对的运动是一个pnp问题，可以利用直接线性变换求解：
sxi＝[r|t]pi
其中，s为放大因子，xi＝(ui,vi,1)t，为特征点的归一化平面齐次坐标表示，r和t分别表示3×3的旋转矩阵和3×1的平移向量，pi＝(xi,yi,zi,1)t，为特征点对应空间点的齐次坐标，每组点对提供了两个线性约束，[r|t]共有12维，最少可通过6对匹配点可以求解该变换。
前景左相机4与前景右相机5为双目相机，通过安装孔固定在外圈主体前部，两个前景相机可以采集用户前方场景图像信息，并通过目标识别算法识别目标物体，接着通过双目三维重建算法得到目标物体的三维坐标。左眼相机6和右眼相机7采用的是两个红外相机，固定在支架扣板10前端，位于外圈主体3下方，通过调节两支架扣板10可使得左眼相机6和右眼相机7分别对准用户左右眼，两个眼睛相机外侧分别有一个红外光源，当红外光源靠近相机光轴时会产生暗瞳效应，即红外相机图像中瞳孔区域颜色变深，虹膜及其他区域颜色变浅，有利于瞳孔的提取。
具体的，步骤s1包括如下子步骤：
s11利用眼动仪上的左眼相机6和右眼相机7分别识别用户的左右瞳孔的中心，以此提取人眼的信息；
s12将识别获得的左右瞳孔中心映射至前景左相机4中以得到二维凝视点；
s13提取前景左相机中的物体锚框，然后根据二维凝视点与物体锚框的位置关系确定用户感兴趣的物体；
s14对用户感兴趣的物体进行三维重建以得到感兴趣物体在前景左相机4中的位置，也可采用现有常规姿态估计方法对用户感兴趣的物体进行姿态估计以获得感兴趣物体在前景左相机4中的姿态；
s15将感兴趣物体在前景左相机中的位置转换至机械臂基坐标系下以此确定用户感兴趣物体的位置。
进一步的，步骤s11包括如下子步骤：
s111分别采集左眼相机6和右眼相机7拍摄的图像并进行平滑处理以获得平滑后的灰度图，其中平滑处理为进行灰度化与滤波操作；
s112将平滑后的灰度图送入边缘检测器(优选送入canny边缘检测器)中得到边缘点，并进行过滤处理以滤除杂点(即过滤掉明显不属于瞳孔边界的边缘点)留下瞳孔边界对应的边缘点，具体如何过滤，本领域技术人员可以根据需要设计过滤规则，在此不赘述，留下的瞳孔边界对应的边缘点构建为瞳孔边缘点集，采用canny边缘检测器进行边缘检测，检测效率高，精度高；
s113对过滤剩下的瞳孔边界对应的边缘点进行椭圆拟合以得到左右瞳孔中心的坐标(xl,yl)和(xr,yr)，优选的利用随机抽样一致性算法对瞳孔边界对应的边缘点进行椭圆拟合以得到左右瞳孔中心的坐标(xl,yl)和(xr,yr)，由椭圆的参数方程知其自由变量有5个，因此在使用随机抽样一致性算法进行椭圆拟合时，至少需要5个边缘点才能拟合出椭圆的参数方程，为了得到最佳的拟合椭圆，随机抽样一致性采用了迭代的方式，其具体步骤如下：
s1131从瞳孔边缘点集中随机选取5个点以拟合出椭圆的平面参数方程，椭圆的平面参数方程为：
q(x,y)＝ax2+bxy+cy2+dx+ey+f
式中，a～f为待求系数；
s1132计算瞳孔边缘点集中所有内点到椭圆的支撑函数值，具体的，内点定义为：
inliers＝{(x,y)|error(q,x,y)<ε}
式中，
支撑函数定义为，即将椭圆对应的内点代入下式中计算内点到椭圆的支撑函数值：
式中，a,b分别为椭圆的长轴和短轴，
s1133重复步骤s1131～s1132预设次数(例如20次)以拟合出多个椭圆，选取支撑函数值最大时对应的椭圆，该椭圆的中心即为瞳孔中心。
更进一步的，可采用现有技术中常用的算法例如多项式拟合方法等将识别的瞳孔中心映射至前景左相机中以得到二维凝视点，本发明优选利用高斯过程回归算法将瞳孔中心的坐标(xl,yl)和(xr,yr)映射到前景相机中得到凝视点(xs,ys)，具体的，高斯过程是任意有限个随机变量均具有联合高斯分布的集合，其基本原理是在预测之前构建训练集，该训练集中包括一系列左右瞳孔中心坐标以及与其对应的在前景左相机上的凝视点坐标，训练集中的数据预先采集，预测时通过左右瞳孔中心构成四维向量(即测试点x*)，计算k(x,x)、k(x*,x)，然后代入期望值计算公式中获得期望值，其即为对应凝视点(xs,ys)，具体的，其数学模型为：
其中，f为训练集中前景左相机上凝视点坐标的集合，x为输入向量x的集合，此处输入向量为左右瞳孔中心坐标组成的4维向量x＝(xl,yl,xr,yr)，k(x,x)为训练集的对称正定协方差矩阵，k(x*,x)为实际测得的x*与训练集x之间的n×1协方差矩阵，k(x*,x*)为测试点自身的协方差，预测值的期望为：
其中，
优选的，步骤s13包括如下子步骤：
s131利用目标识别算法识别得到前景左相机中物体的锚框，如图5所示，左边的圆柱体代表水杯，右边的圆代表球，将水杯和球包含在内的虚线即为水杯和球对应的锚框，利用物体锚框初始化追踪算法中的追踪目标，该目标识别算法和追踪算法同步进行，目标识别算法和追踪算法均可采用现有常规的方法；
s132初始化后的追踪算法进行物体的追踪，若跟丢物体，则利用目标识别算法实时识别的结果重新初始化追踪算法，然后利用初始化后的追踪算法继续进行物体追踪，以此追踪获得物体的锚框，通过上述方法可有效提高前景左相机中物体识别的成功率；
s133然后根据映射至前景左相机中的二维凝视点与物体锚框的位置关系确定用户感兴趣的物体，其中，如图5所示，若凝视点落入水杯锚框内(如图5中位于水杯锚框内的黑点)认为用户对该水杯感兴趣，若凝视点未落入水杯锚框内(如图5中位于水杯锚框之外的白点)认为用户对该水杯不感兴趣；若凝视点落入球的锚框内(如图5中位于球锚框内的黑点)认为用户对该球感兴趣，若凝视点未落入球的锚框内(如图5中位于球锚框之外的白点)认为用户对该球不感兴趣。
优选的，步骤s14对用户感兴趣的物体进行三维重建具体为：
s141通过双目标定得到前景左相机和前景右相机的内外参数，具体包括前景左右相机的内参矩阵和外参矩阵，通过内外参数矩阵推导出重投影矩阵q；
s142利用双目相机内外参数校正对齐前景左相机和前景右相机中的图像，其为现有技术，在此不赘述；
s143通过特征匹配算法得到前景左相机和前景右相机图像像素的双目视差值d，优选的，特征匹配采用归一化互相关方法，其相关性度量方式为：
其中，p(x,y)表示前景左相机中任意点的坐标，wp表示以p为中心的矩形区域，l(x,y)表示前景左相机图像中点(x,y)处的灰度值，
s144利用双目视差值与双目标定得到的重投影矩阵q重建得到前景左相机图像中各个像素点在前景左相机坐标系下的三维坐标，以此完成物体的三维重建，其原理为：
[xyzw]t＝q*[xyd1]t
其中，[xyzw]为前景左相机坐标系下的齐次坐标，(x,y)为前景左相机图像坐标系下的二维坐标。
更具体而言，步骤s15优选采用下述步骤将感兴趣物体在前景左相机中的位置转换至机械臂基坐标系下：
s151利用眼动仪上的多个红外接收器结合位于机械臂基坐标系内的红外发生器(即灯塔)得到眼动仪坐标系相对于机械臂基坐标系的变换矩阵，具体包括：
s1511测得每个红外接收器在红外发生器虚拟平面上的2d坐标(ui,vi)：
其中，αi为带动红外发生器水平旋转的电机在水平方向的扫射角度，βi为带动红外发生器竖直旋转的电机在竖直方向的扫射角度；
s1512利用下式进行直接线性变换求解获得眼动仪坐标系相对于机械臂基坐标系的变换矩阵[r|t]：
sxi＝[r|t]pi
上式即是求解3d到2d点对运动的方法(pnp问题)，其中，s为放大因子，xi＝(ui,vi,1)t，r和t分别表示3×3的旋转矩阵和3×1的平移向量，pi＝(xi,yi,zi,1)t，(xi,yi,zi)表示红外接收器在眼动仪坐标系下的3d坐标，每组点对提供了两个线性约束，[r|t]共有12维，因此最少可通过6对匹配点求解变换矩阵[r|t]，故红外接收器设置6个以上；
s152通过标定获得前景左相机与眼动仪坐标系之间的变换矩阵；
s153根据上述两个变换矩阵将感兴趣物体在前景左相机中的位姿转换至机械臂基坐标系下。
步骤s2中，离散化遍历得到用于执行抓取动作的机械臂在空间各位置的可行抓取姿态指的是离散化获得机械臂在其工作空间内可达的各位置点，然后离散化获得各位置点对应的可行抓取姿态集合。
在一个优选实施例中，位置点的离散化方式为：
机械臂末端离机械臂肩关节的最大距离为r，以此为半径的球体体积
抓取姿态的离散化方式为：
将姿态角以间隔为0.255弧度等分，则一点的姿态采样数(即可行抓取姿态)为576个，利用ikfast反解工具包判断各位置点对应的姿态可解性，建立机械部可达空间数据库：
其中，pi为第i个位置点，i的上限是n＝v/d3，
步骤s4中，具体采用运动学反解模块基于抓取姿态反解得到机械臂的各关节角，优选采用ikfast反解模块，反解模块求解的问题如下：
其中，
其中，c为cos的简写，θi为第i关节角，s为sin的简写，ai-1为沿xi-1轴从zi-1移动到zi的距离，αi-1为绕xi-1轴从zi-1旋转到zi的角度，di为沿zi轴从xi-1移动到xi的距离，xi为坐标系oi的x轴，zi为坐标系oi的z轴，可参见图6，图6中的各参数如表1所示。
表1
具体的，本发明采用的机械臂包含7个自由度，有一个冗余自由度，在工作空间内，它的运动学反解可能会有无穷多组(即通过公式(二)可求解出多组关节角，每组关节角中均包含7个关节角，即θ1～θ7)，因此，根据实际情况设定一定的规则从中找到最优的一组解。机器人运动学反解一般遵循最短行程原理，即使得每个关节的活动量最小，同时，一般机械手臂是串联机构，肩关节转动一个很小的角度都会对末端手腕关节的位置造成很大的影响，所以应该加权处理，即多移动末端关节，少移动前段关节。基于此，本发明以最大化所有关节的相对角度裕度的和为优化准则以找到最优的一组关节角：
其中，θ＝{θi},i＝1…7，即每个关节角离极限位置越远越好，这样关节角的灵活度更高，更少受关节范围的限制。
式(1)的优化目标不便于优化，可以等效为最小化相对角度偏差的绝对值的均值，即：
其中，μ＝mean(aad)为相对角度偏差绝对值的均值，相对角度偏差的绝对值aad表示为：
同时为了防止个别关节角出现较大偏差，可以将标准差加入到优化函数当中，故优化目标函数变为：
其中，σ＝var(aad)为相对角度偏差绝对值的方差。
实际操作中，采用上述方法实现物体的抓取时，仍存在一定的误差，为了提高抓取的精度，本发明优选进行补偿处理。具体的，在步骤s4之后计算出各关节角的执行误差，包括：
首先，根据用户感兴趣物体的位置与确定的最佳抓取姿态沿抓取反方向偏离预设的偏置距离得到预抓取位置，访问服务器以查询获得该预抓取位置的可行抓取姿态集合；具体的，用户感兴趣物体的位置为c，得到预抓取位置为p＝c+zr·b，其中，zr为最佳抓取姿态下机械手的主轴向量(垂直于掌心的方向)，b为预设的偏置距离，优选设为10cm；
然后，基于可行抓取姿态集合中的各抓取姿态反解得到机械臂各关节角的多组解，在关节角的多组解中确定出最优解，即目标关节角；
接着，使机械臂各关节运动至对应的目标关节角，然后测量机械臂末端的位置并反解得到机械臂各关节的真实关节角，具体是获得机械臂1-5关节的真实转角，具体的利用固定在机械臂小臂(为第二连杆)上的tracker(追踪器)测量得到机械臂末端的位置：通过机械臂小臂上的tracker可以得到该tracker坐标系相对于机器人基坐标系的变换矩阵
最后，计算目标关节角与真实关节角之差得到机械臂各关节的执行误差，即用目标转角与对应的实际转角做差得到前五个关节的执行误差，然后利用各执行误差对步骤s4获得的各最优关节角进行补偿，即将各最优关节角加上对应的执行误差获得补偿后的关节角，最后在步骤s5中使机械臂各关节运动至对应的补偿后的关节角，以此保证机械臂实际到达的位置为目标位置(即用户感兴趣物体的位置)，以实现高精度抓取。
本发明可以精确定位用户观察(感兴趣)的物体，并控制机械臂末端以拟人的方式到达物体位置，拟人机械手可以抓取感兴趣物体并执行指定的动作，比如说喝水与搬运小球，可帮助严重肢体残疾患者抓取感兴趣物体。
本领域的技术人员容易理解，以上所述仅为本发明的较佳实施例而已，并不用以限制本发明，凡在本发明的精神和原则之内所作的任何修改、等同替换和改进等，均应包含在本发明的保护范围之内。
- 还没有人留言评论。精彩留言会获得点赞！