CN114131616B - 一种应用于机械臂操控的三维虚拟力场视觉增强方法 - Google Patents

Abstract

本发明属于机器人遥操作相关技术领域，更具体地，涉及一种应用于机械臂操控的三维虚拟力场视觉增强方法。该方法包括下列步骤：S1在机械臂的运动空间中计算该机械臂末端受到的势场合力；S2在虚拟空间构建一个三维虚拟球体，并对其表面进行网格划分，球心等效为机械臂末端的质心，将质心受到的合力作用在对应方向的网格节点，球体表面其它节点处赋予的力按照预设规则获得，并产生位移；S3将三维虚拟球体采用增强现实技术映射到机械臂末端，当机械臂末端受到的势场合力发生变化，三维虚拟球体形状发生变化，即实现对机械臂末端力场的视觉增强。通过本发明，解决在单一视觉信息的指导下易发生非预期碰撞、操作错误等问题。

Description

一种应用于机械臂操控的三维虚拟力场视觉增强方法

技术领域

本发明属于机器人遥操作相关技术领域，更具体地，涉及一种应用于机械臂操控的三维虚拟力场视觉增强方法。

背景技术

面向极端、恶劣环境，如深空、深海探测，操作者无法抵达作业现场，机器人作为助手，通过遥操作技术代替操作者完成作业任务。人类主要依靠视觉感知操作状态。对于遥操作领域，当控制空间机器人进行有关空间载荷的在轨服务，如空间装配、空间科学实验、部件更换、设备维修这些复杂的在轨遥操作任务，应用场景、工作空间的复杂动态化，机械臂的任务路径周围难免会出现在任务路径中的静态障碍物，又或者是持续的动态障碍物。仅仅依靠视觉来认识从端操作环境的位置是不够的，尤其是二维的视频反馈，存在视角盲区、错位和缺少深度信息等问题，还需要增强操作者对从端操作环境的力觉感受。

操作者与被操作物体发生直接物理接触，刺激人体皮肤的力触觉感受器，如麦斯纳小体，产生刺激信号。然而，遥操作任务中，人类与被操作对象并不处于共享空间，无法直接获取力触觉反馈。一些力反馈设备，如手持型的力觉反馈装置、穿戴型的振动反馈装置，能带给操作者带来远端的接触力信息。但力反馈设备昂贵、占用空间以及灵活性差，未能达到预期效果。本发明提出一种应用于机械臂操控的三维虚拟力场视觉增强方法，通过视觉表达力觉信息。

发明内容

针对现有技术的以上缺陷或改进需求，本发明提供了一种应用于机械臂操控的三维虚拟力场视觉增强方法，解决在在单一视觉信息的指导下易发生非预期碰撞、操作错误等问题。

为实现上述目的，按照本发明，提供了一种应用于机械臂操控的三维虚拟力场视觉增强方法，该方法包括下列步骤：

S1在机械臂的运动空间中计算该机械臂末端受到的合力；

S2在虚拟空间构建一个三维虚拟球体，并将该三维虚拟球体表面进行网格划分，以此获得网格化后的三维虚拟球体以及各个网格节点的坐标，所述三维虚拟球体的球心等效为所述机械臂的末端的质心，在所述三维虚拟球体上选取一个节点作为初始节点，该节点是从所述三维虚拟球心出发沿所述合力的方向指向三维虚拟球体表面并与所述三维虚拟球体表面相交的节点，将所述合力赋予该初始节点处，所述三维虚拟球体表面上其它节点处赋予的力按照预设规则逐步衰减，以此获得三维虚拟球体表面所有节点处受到的力以及在该力下产生的位移；

S3将所述三维虚拟球体采用增强现实技术映射指所述机械臂末端，当所述机械臂末端位置发生变化时，所述三维虚拟球体表面各个节点处受到的力发生变化，通过监测所述三维虚拟球体表面各个节点处力的变化实现对机械臂末端受到的力的监测，即实现对机械臂末端力场的视觉增强。

进一步优选地，在步骤S1中，所述合力按照下列方式进行计算：

S11在机械臂的运动空间中构建一个虚拟势场，该势场由引力势场和斥力势场组成；

S12分别计算引力势场中的引力和斥力势场中受到的斥力；

S13将所述引力和斥力求合力，及获得所学的合力。

进一步优选地，在步骤S12中，所述引力按照下列公式计算：

F_att(X)＝-k_att·(X-X_g)

所述斥力按照下列公式计算：

其中，F_att(X)是引力势场对机械臂末端产生的引力，k_att是引力势场正比例增益因子，X是机械臂末端的位置坐标，X_g是目标点的位置坐标，F_rep(X)是斥力势场对机械臂末端产生的斥力，k_rep是斥力势场正比例增益因子，X是机械臂末端的位置坐标，X₀是障碍物的位置坐标，ρ(X，X₀)是ρ(X，X₀)＝||X₀-X||为机械臂末端与障碍物的相对距离，ρ₀障碍物斥力势场的最大影响距离。

进一步优选地，在步骤S2中，所述三维虚拟球体表面上其它节点处赋予的力按照下列公式计算：

其中，F是所述合力，F_v是节点v处赋予的力，d是节点与所述初始节点之间的距离。

进一步优选地，在步骤S2中，各个节点处的位移按照下列公式计算：

Δp＝v_dΔt

其中，Δp为单位时间内节点的位移，v_d为节点运动的速度，Δt为单位时间。

进一步优选地，所述v_d按照下列公式计算：

v_d＝v(1-dΔt)

其中，v_d为节点运动的速度，Δt为单位时间，v是速度增量Δv之和。

速度增量Δv，从0开始增加，由和Δv＝aΔt求出Δv。

Δv＝FΔt

v＝∑Δv

进一步优选地，在步骤S3中，将所述三维虚拟球体采用增强现实技术映射指所述机械臂末端，需要通过坐标系之间的转换关系将所述三维虚拟球体叠加至所述机械臂末端，实现虚实融合。

总体而言，通过本发明所构思的以上技术方案与现有技术相比，具备下列有益效果：

1.本发明使用人工势场法，仿照静电场模型而构造出来势场模型。目标、障碍物在势场中产生势场力，将多个障碍物产生的斥力和目标产生的引力进行叠加，得到机械臂末端在人工势场中受到的总作用力。通过在操控机械臂时实时获取虚拟力，可将此力觉信息作为视觉信息的补充，避免单一通道的信息缺失；

2.本发明创建基于立方体的虚拟网格球体，将人工势场法计算得到的总作用力作用到网格顶点，并使用数学推理对其进行改进，通过数学计算得到网格运动的速度及位置，使球体达到变形的目的。势场中的引力与斥力，使球体产生凸起或凹陷，同时使用平方反比定律找到衰减力，保证球体有限制的局部变形。虚拟球体具有HSV颜色梯度，与受力大小形成线性映射，通过这种凹陷或凸起的程度以及颜色来增强力觉信息，保证避障任务高效精准的完成；

3.本发明针对机械臂避障和目标位置接近操作，引入人工势场法和增强现实技术对机械臂末端的力觉信息进行视觉增强，改善单一视觉反馈的视角盲区、错位引发的误操作，可应用于机械臂操控系统和交互界面；

4.本发明使用无标记注册的增强现实技术，通过跟踪算法对机械臂进行跟踪，实时获取机械臂的位置，然后利用图像边缘灰度变化率方法判断图像是否发生运动模糊，用ORB算法及KAZE算法检测机械臂特征点并生成特征描述子，用RANSAC算法建立单应性矩阵，完成渲染虚拟三维球体的注册工作；

5.本发明基于可视化三维球体、虚拟力场和视觉增强技术的机械臂操作方法具有提升机械臂避障率，减少操作者操作负荷等优点，基于增强现实技术的三维虚拟球体通过变形和颜色等视觉渲染手段，对力觉信息进行增强，有效的帮助操作者控制机械臂。

附图说明

图1是按照本发明的优选实施例所构建的应用于机械臂操控的三维虚拟力场视觉增强方法的结构框图；

图2是按照本发明的优选实施例所构建的合成势场的受力分析图；

图3是按照本发明的优选实施例所构建的机械臂末端的反映势场力的三维虚拟球体效果图；

图4是按照本发明的优选实施例所构建的由二维正方形映射至圆形的映射图；

图5是按照本发明的优选实施例所构建的三维虚拟球体受到引力的效果图；

图6是按照本发明的优选实施例所构建的三维虚拟球体受到斥力的效果图。

具体实施方式

为了使本发明的目的、技术方案及优点更加清楚明白，以下结合附图及实施例，对本发明进行进一步详细说明。应当理解，此处所描述的具体实施例仅仅用以解释本发明，并不用于限定本发明。此外，下面所描述的本发明各个实施方式中所涉及到的技术特征只要彼此之间未构成冲突就可以相互组合。

本发明所提出虚拟力增强现实注册技术，通过机械臂运动学计算和基于视觉的几何特征匹配联合估计的增强现实三维注册方法。通过机械臂运动学求解机械臂末端的实际位置，通过相机位姿变换、投影变换，求解末端的在图像坐标系的候选框的大小和位置，通过计算机视觉的特征提取方法，在候选框内进行搜索，与机械臂模型的特征进行匹配以获取帧内虚拟球体的注册位置。

如图1所示，一种应用于机械臂操控的三维虚拟力场视觉增强方法，总体流程可以分为以下三步：

第一步，将机器人所在的工作空间虚拟为充满引力势场和斥力势场的空间。目标点与机器人产生的相互作用场为引力势场，障碍物与机器人产生的相互作用场为斥力场，将斥力和引力进行叠加，其合力为机械臂末端在人工势场中受到的总作用力，机械臂根据势场合力的作用规划路径；

根据以上描述，在机械臂的运动空间中人为构建一个虚拟势场U，该虚拟势场由引力势场和斥力势场组成，目标产生引力势场U_att，其形状在势场中类似一座“低谷”，具有负势能,机械臂末端具有向低势能方向移动的趋势。机械臂末端的引力势场势能随着其与目标距离的增加而单调递增，目标距离机械臂末端越远，其所具有的引力势能就越大；反之，目标位置点距离机械臂末端越近，其所具有的引力势能就越小，且当机械臂末端处于目标位置时，其所具有的引力势能为零。

机械臂末端可以当作二维空间环境下的质点X＝(x,y)^T，其运动空间为二维空间。在传统人工势场法中，目标点所产生的引力势场函数U_ATT一般表示式为：

式中k_att———引力势场正比例增益因子；

X——机械臂末端的位置坐标；

X_g——目标点的位置坐标；

M——引力势场因子；

ρ(X，X_g)-ρ(X，X_g)＝||X_g-X||为机械臂末端和目标的欧几里得距离；在上述公式中，引力势场因子0<m≤1，引力势场的形状为锥形凸起，所产生的引力具有固定的势场强度，其势场强度等比例于机械臂末端与目标点的欧几里得距离。

目标点的引力势能为全局最小值，其引力势场函数为：

引力势场U_att实现对质点位置X的闭环控制，控制质点趋近目标X_g，引力势场正比例增益因子k_att决定引力势场的比例增益，可以通过人为调整来改变引力势场的强弱，从而影响机械臂末端的路径规划效率。

引力势场U_att对机械臂末端产生引力F_att，其数学表达式为：

F_att(X)＝-k_att·(X-X_g)

本发明将人工引力势场因子设置为m＝2，对应的引力势场为二次势场，因此其负梯度作用力的递增是一次线性的，引力函数F_att(X)是一次函数，具有很大的灵活性,还可以创建复合函数。

环境中的障碍物产生斥力势场U_rep，通常可以表示为：

式中k_rep———斥力势场正比例增益因子；

X———机械臂末端的位置坐标；

X₀———障碍物的位置坐标；

ρ(X，X₀)———ρ(X，X₀)＝||X₀-X||为机械臂末端与障碍物的相对距离；

ρ₀障碍物斥力势场的最大影响距离；

当机械臂末端与障碍物之间的距离ρ(X，X₀)超过ρ₀时，可认为此障碍物对机械臂末端的运动不再具有影响作用，对应到数学模型中即为U_rep(X)＝0的分段函数情况，此时斥力势场为零。

从上述公式可知，在斥力势场对机械臂末端存在斥力影响的情况下，斥力势场U_rep(X)与机械臂末端和障碍物的相对距离ρ(X，X₀)的倒数成平方关系。斥力势场正比例增益因子k_rep决定了斥力势场的比例增益强调，可以人为调整斥力势场的强弱来影响机械臂末端的避障效率。

机械臂在障碍物的斥力势场作用下逐渐远离障碍物，实现避障功能。当远离障碍物到距离趋向与ρ₀时，斥力势场势能也逐渐趋向于零，多个斥力势场对机械臂末端的作用力的大小与方向遵循矢量运算规则。

障碍物产生的斥力势场U_rep对机械臂末端产生斥力作用，斥力F_rep为斥力势场U_rep的负梯度作用力，其数学表达式为:

由上述公式可知，斥力F_rep的方向在机械臂末端与障碍物的连线上，并且背离障碍物指向机械臂末端。

将引力势场函数U_att(X)和斥力势场函数U_rep(X)求出来的引力与斥力进行叠加，其总势场函数U(X)为：

U(X)＝U_att(X)+U_rep(X)

如图2所示为最终合成势场的受力分析图，机械臂末端所受到的合力F(X)为引力F_att(X)和斥力F_rep(X)的矢量合力，

由上述公式可以计算出运动空间中每一点的势场势能以及势场力，机械臂末端简化为二维空间运动的一个质点,在总作用力的引导下运动，直到抵达目标点为止。

第二步，生成带网格球体，并将人工势场法实时计算出来的引力与斥力叠加到球体之上，具有引力的方向球体会凸起，具有斥力的方向球体会收缩凹陷，用以显示环境中的障碍物。

在建立势场之后，为凸显力觉信息，同时由于大部分机器人系统缺少力反馈装置，因此采用增强现实技术将反映势场力的三维虚拟球体注册到机械臂末端，来增强力觉信息，如图3所示。

首先，需创建基于立方体的球体网格，而三维球体网格是基于二维圆形网格建立而成，需创建正方形边缘顶点，对于每个点，我们还将使用白色球体显示相应的圆顶点。归一化将顶点直接拉到中心，直到它们到达单位圆，靠近正方形角的顶点比靠近主轴的顶点被拉入更多，此时，恰好位于主轴上的顶点根本不会移动，而恰好位于对角线上的顶点则移动了单位，如图4所示，因此，单元格的大小会有不同。

由于点是用向量描述的，我们实际上是将一个向量映射到另一个向量，v_s→v_c，而归一化一个向量则是通过其除以自己的长度：

二维向量的长度则是由点的X,Y坐标组成，使用勾股定理即可获得，再对其进行平方根运算即可获得其长度，此时，从正方形到圆形的映射为：

然后，将二维推广到三维，从立方体到球体的映射需要合并第三个坐标

||v_o||²＝1-(1-x²)(1-y²)(1-z²)

通过链接到所有三个坐标的部分，并在最终向量的坐标之间进行平均分割，得到

最终的映射距离为

然后使用数学推理对其进行改进，使网格获得弹性，即通过数学计算网格运动后的位置使球体达到可变形的目的。

最终，将人工势场法计算出来的总作用力作用到网格顶点，使球体凸起或凹陷，对应势场中的引力与斥力，此时需对力进行偏移，改变力的方向，使球体变形不局限于一个点，囊括周围的表面。将力转换为网格点运动的速度，使用平方反比定律找到衰减力，距离受力点越远的网格面受力越小，保证距离为0时力处于最大强度。

将其转换为速度增量Δv，速度从0开始增加，由和Δv＝aΔt求出Δv。

Δv＝FΔt

v＝∑Δv

同时给每个网格点加上虚拟的弹簧力，防止速度一直增加，从而导致网格点其无限制移动，保证球体有限制的局部变形。直接使用位移矢量作为速度调整，乘以可配置的弹簧力即可，最后为减轻网格点的震荡效果，给其加上阻尼。

v_d＝v(1-dΔt)

获得速度之后，将其给予对应的顶点，调整其位置

Δp＝v_dΔt

显示效果如图5和图6所示。

通过这种凹陷或凸起的程度以及颜色来增强力觉信息，保证避障任务高效精准的完成。

第三步，将创建的三维虚拟球体通过增强现实技术注册到机械臂末端，在虚拟力场(引力和斥力)的影响下，三维虚拟球体通过变形和颜色等视觉渲染手段，对力觉信息进行增强。

最重要的是获取机械臂末端的相对位置，其是通过一系列的坐标转换来实现的，包括摄像机坐标系，图像平面坐标系，世界坐标系，对虚拟物体进行描述的虚拟坐标系。

首先使用摄像机标定以确定摄像机内外部参数，获得世界坐标系、摄像机坐标系、图像平面坐标系之间的转换换关系；

然后通过实时检测摄像机相对于世界坐标系的位置和方向，以确定世界坐标系和摄像机坐标系之间的转换关系。这一部分又分为检测和跟踪两个部分；

最后利用计算机图形学将第二步得到的三维虚拟球体，根据之前得到的坐标变换关系将虚拟球体叠加到真实场景中的目标区域，实现虚实融合。

以上三个方面并不是独立的三个过程，而是相互之间紧密结合的三个过程，在增强现实系统的配准过程中三个过程同时工作进而生成虚实融合场景的每一帧图像，并将之表现在显示平面上，这就是呈现给用户的增强现实效果。

我们使用针孔相机成像模型，用齐次坐标表示世界坐标系下的点X_w＝(x_w，y_w，z_w，1)^T，X＝(x，y，1)^T为其在像平面上的投影，他们之间的关系表示为：

当X_w在标识平面上时，Z_w＝0，则有：

其中，式中λ的表示图像平面坐标系转换到摄像机坐标系时的比例因子，矩阵P表示的是投影矩阵，K与M分别表示摄像机的内外参数。求出摄像机的内外参，即可实现增强现实中的三维注册过程。由上述分析可知，可以通过摄像机标定确定内参矩阵K，外参矩阵M则包含一个平移分量T和3个旋转分量R₁、R₂、R₃，此时只要得到T、R₁、R₂、R₃就能够实现三维注册。

只要得到单应性矩阵H_w和内参矩阵K，就能够得到分量T、R₁、R₂、R₃，即摄像机的外参矩阵。每一帧获得外参矩阵M后，能唯一地确定X点的位置，即虚拟三维球体在真实场景中的准确注册位置。

通过摄像机标定得到摄像机内参K后，还需求解出每一帧图像的单应性矩阵才能完成对虚拟三维球体的三维注册。

此时用跟踪算法对机械臂进行跟踪，从而得到机械臂的位置，然后利用图像边缘灰度变化率方法判断图像是否发生运动模糊，用ORB算法及KAZE算法检测特征点并生成特征描述子，用RANSAC算法建立单应性矩阵，完成渲染虚拟三维球体的注册工作。

本领域的技术人员容易理解，以上所述仅为本发明的较佳实施例而已，并不用以限制本发明，凡在本发明的精神和原则之内所作的任何修改、等同替换和改进等，均应包含在本发明的保护范围之内。

Claims (4)

1.一种应用于机械臂操控的三维虚拟力场视觉增强方法，其特征在于，该方法包括下列步骤：

S1在机械臂的运动空间中计算该机械臂末端受到的合力；

S2在虚拟空间构建一个三维虚拟球体，并将该三维虚拟球体表面进行网格划分，以此获得网格化后的三维虚拟球体以及各个网格节点的坐标，所述三维虚拟球体的球心等效为所述机械臂的末端的质心，在所述三维虚拟球体上选取一个节点作为初始节点，该节点是从所述三维虚拟球体的球心出发沿所述合力的方向指向三维虚拟球体表面并与所述三维虚拟球体表面相交的节点，将所述合力赋予该初始节点处，所述三维虚拟球体表面上其它节点处赋予的力按照预设规则逐步衰减，以此获得三维虚拟球体表面所有节点处受到的力以及在该力下产生的位移；

S3将所述三维虚拟球体采用增强现实技术映射至所述机械臂末端，当所述机械臂末端位置发生变化时，所述三维虚拟球体表面各个节点处受到的力发生变化，通过监测所述三维虚拟球体表面各个节点处力的变化实现对机械臂末端受到的力的监测，即实现对机械臂末端力场的视觉增强；

在步骤S1中，所述合力按照下列步骤进行计算：

S11在机械臂的运动空间中构建一个虚拟势场，该势场由引力势场和斥力势场组成；

S12分别计算引力势场中的引力和斥力势场中受到的斥力；

S13将所述引力和斥力求合力，以此获得所述的合力；

在步骤S12中，所述引力按照下列公式计算：

F_att(X)＝-k_att·(X-X_g)

所述斥力按照下列公式计算：

其中，F_att(X)是引力势场对机械臂末端产生的引力，k_att是引力势场正比例增益因子，X是机械臂末端的位置坐标，X_g是目标点的位置坐标，F_rep(X)是斥力势场对机械臂末端产生的斥力，k_rep是斥力势场正比例增益因子，X₀是障碍物的位置坐标，ρ(X，X₀)是ρ(X，X₀)＝||X₀-X||为机械臂末端与障碍物的相对距离，ρ₀是障碍物斥力势场的最大影响距离；

在步骤S2中，所述三维虚拟球体表面上其它节点处赋予的力按照下列公式计算：

其中，F是所述合力，F_v是节点v处赋予的力，d是节点与所述初始节点之间的距离。

2.如权利要求1所述的一种应用于机械臂操控的三维虚拟力场视觉增强方法，其特征在于，在步骤S2中，各个节点处的位移按照下列公式计算：

Δp＝v_dΔt

其中，Δp是单位时间内节点的位移，v_d是节点运动的速度，Δt是单位时间。

3.如权利要求2所述的一种应用于机械臂操控的三维虚拟力场视觉增强方法，其特征在于，所述v_d按照下列公式计算：

v_d＝v(1-dΔt)

其中，v是速度增量Δv之和。

4.如权利要求1或2所述的一种应用于机械臂操控的三维虚拟力场视觉增强方法，其特征在于，在步骤S3中，将所述三维虚拟球体采用增强现实技术映射至所述机械臂末端，需要通过坐标系之间的转换关系将所述三维虚拟球体叠加至所述机械臂末端，实现虚实融合。