基于环境建模与修正的视觉/力觉辅助遥操作系统

徐效农，宋爱国，朱澄澄，倪得晶(东南大学仪器科学与工程学院，南京210096)



基于环境建模与修正的视觉/力觉辅助遥操作系统

徐效农，宋爱国，朱澄澄，倪得晶
(东南大学仪器科学与工程学院，南京210096)

摘要:为增强空间遥操作系统的力交互性，提出了一种基于视觉/力觉辅助的空间遥操作系统。结合虚拟现实技术，借助力觉反馈设备搭建了基于虚拟3D视觉反馈和虚拟力觉反馈的空间遥操作实验平台。使用KINECT相机采集3D点云数据，采用滑动最小二乘法(SLSM)在线识别未知环境中的模型参数并更新本地虚拟环境。通过虚拟预测环境产生实时的视觉、力觉反馈。基于人工势场的共享控制方法，产生引力或斥力信号来增强操作者控制远端机器人的避障及接近操作目标的能力。该方法在7自由度Schunk机械臂上得到了实现，实验表明此方法能够克服2 s时延影响并显著提高操作效率。

关键词:虚拟环境；3D建模；模型修正；遥操作；视觉/力觉辅助

1　引言

空间机器人是深入开展载人航天活动必不可少的工具，它在空间站系统中承担着舱段捕获与转移、仪器设备转移与安装、辅助航天员作业等功能[1]。

自上世纪90年代以来，空间遥操作机器人技术受到了国内外的广泛关注，欧洲、美国、日本等地区和国家都对空间机器人的遥操作技术进行了深入的研究。由于操作者与机器人在地理上分开，星地链路大通信时延总是存在[2]，会影响整个系统的稳定，在地面采用遥操作机械手远程完成一系列复杂的任务是一个巨大的挑战。为了解决这一问题，科研人员提出了很多方法[3]。

Anderson与Spong提出散射理论来设计双边遥操作以实现系统稳定[4]。Niemeyer等提出了波变量理论，提高了稳定性但降低了位置跟踪的能力[5]。针对ROTEX任务5～7 s的变化时延，Hirzinger等提出了利用虚拟现实技术构建虚拟模型实现预测显示的解决方法，但该方法存在虚拟模型很难准确反映真实世界的问题[6]。Hannaford指出必须综合操作员的手动和机器人自主控制的优点，提出了基于共享控制的解决方案[7]。Hashtrudi-Zaad等研究了遥操作系统中透明性的概念，并对引入力反馈对遥操作系统的稳定性的影响做出了系统的分析[8]。

Khatib提出人工势场理论[9]，认为机器人在一个由障碍物和目标产生的虚拟的斥力场和引力场中运动，所受的合力使得机器人沿着势函数梯度下降的方向趋于目标，但该理论主要被用于移动机器人的路径规划中[8-11]。吉林大学的倪涛等人将这一方法引入到挖掘机器人的远程控制中，提高了远程作业任务的安全性[12]。

在本地端构建远端场景的虚拟环境是解决遥操作中大时延问题的一个有效方法，ETS-VII的实验也采用了这项技术[13]。东南大学[14]、哈尔滨工业大学[15]等对此做出了相应的研究。其中，虚拟环境的几何场景的构建，是该方法的难点之一。北京航空航天大学的徐祯琦等人使用KINECT摄像头，利用点云信息对物体表面进行重建[16]。在虚拟环境中力反馈建模的问题上，东南大学的李会军和宋爱国提出了基于阻抗在线估计的自适应无源控制算法，可保证较大时延下遥操作系统的稳定性，在2D场景中实现并取得了较好的效果[17]。

针对上述现状，由于视觉和力觉作为操作者感知作业环境现场的最主要的信息提示方式和手段，本文提出了基于环境建模与修正的视觉/力觉辅助的遥操作系统，并搭建了操作平台。使用KINECT相机采集三维点云信息，用于构建虚拟环境中的3D场景。采用滑动最小二乘法在线辨识未知环境中的模型参数并实时更新。该方法中的力反馈不仅仅限于现场真实作业力，还利用人工势场的控制方法，在机器人自由空间运动时，提供相应的引力或斥力，提高了操作者控制远端机器人壁障及接近目标效率。最后，在2 s的时延下，利用上述方法，对其操作性能和效率进行了实验和评价。

2　系统平台结构

典型的主-从式遥操作系统主要由5个部分组成:操作者、主机器人(手控器设备)、通信链路、从机器人、远端环境[18]。在本文所述的视觉/力觉辅助的遥操作系统中，加入了虚拟预测环境显示和虚拟力反馈fv，如图1所示。

图1　视觉/力觉辅助遥操作系统Fig. 1 Visual-haptic aid teleoperation system

操作者在本地端操作主机械臂，远端的从机械臂按照位置命令跟随主机械臂运动。在本地端，压缩后的视频信息通过通信链路传递给本地端，并显示给操作者。同时，利用KINECT相机进行3D模型重建，在虚拟环境界面中重建从机械臂及其周围环境的几何模型。由于从端环境的不确定性以及传感器的误差因素，环境模型的几何形状和位置等信息需要在线更新。利用滑动最小二乘法对物体的动力学模型参数进行不断地修正。修正后的信息在链路时延后传递给主端并在虚拟环境界面中显示。

在本文提出的方法里，虚拟力fv包括两个部分。在从端机械臂自由运动空间中，主要依靠虚拟场景产生的力势场，提供给操作者引力或斥力引导其更安全快捷地到达操作目标处。而虚拟力fv的另外一部分为机械臂末端与环境相接触时的接触力，通过在线辨识并更新环境动力学参数，将参数值赋予虚拟环境的模型中，当虚拟场景中需要渲染接触力时，再根据环境的动力学模型计算得出。

3　虚拟环境建模

3. 1 七自由度机械臂虚拟场景建模

在本文的实验系统中，采用Schunk LWA4D 7自由度轻量级机械手作为从机器人(见图2)。

图2　七自由度机械臂的初始位置Fig. 2 Robot manipulator ordinate at zero position

该机械臂的Denavit- Hartenberg(DH)参数设置如表1:

表1　Schunk LWA4D机械臂的DH参数Table 1 DH parameters for Schunk LWA4D manipulator/ (°)

在笛卡尔坐标系中，主机器人末端的空间位置xm将通过虚拟环境中的机械臂虚拟模型直接体现出来。为了显示出虚拟环境中的虚拟机械臂末端位置，首先需要通过机器人运动学逆解解算为7个关节角的角度值。在6自由度空间中使用7自由度机械臂，有冗余的问题，会有无限多的解析解。KDL库(Kinematics and Dynamics Library)能够对超过6个自由度的冗余机械臂进行逆解，这一通用的方法通过牛顿迭代法提供数值解[19]。我们使用KDL库函数求得各关节角度值，从而控制虚拟机械臂运动到指定位置并同时更新虚拟场景中的机械臂模型。

3. 2 基于点云信息的三维场景建模

在不同的环境中，只有少部分模型固定在操作平台上，从而可以预先创建，如从机械臂本身可以事先建好模型并且在虚拟场景中设定在相应的位置。而更多的模型是事先未知的，例如远端环境中的障碍物或目标物体。为了获未知物体的形状和位置，采用Kinect传感器来构建相对准确的几何模型。

3. 2. 1 点云采集

利用OpenNI提供的SDK接口可以直接将Kinect采集到的彩色图像和深度图像转换成空间中的三维点云[20-21]。由于Kinect的深度摄像头和彩色摄像头不在同一位置，在计算空间三维点之前应当将两幅图像转换到同一个图像坐标系下。

3. 2. 2 感兴趣区域提取与降噪

由于Kinect获得的是镜头视野范围内整个环境的信息，若要对我们感兴趣的物体进行后续处理，则需要把物体从背景中分离出来。根据物体形状及其距离镜头的大致深度值，设定距离的最大和最小阈值，然后利用阈值分割方法分割出有效区域。

针对场景中物体一般摆放在平面上的特点，可以再简化为提取一定高度的平面并且定位其上的物体。之后，本文采取人机交互与感兴趣区域自动滤除的方式进行感兴趣区域提取。定义彩色图像全局区域为，感兴趣区为，有。为了方便提取，指定彩色图像中的ROI为一矩形区域

该矩形区域对应的点云区域为P′roi，最终获取的感兴趣区为一点云数据Proi，其中Proi⊂P′roi。具体步骤如下:

3)对P′roi采用点云的欧式距离聚类分割算法，将点云中没有明显间隔的点聚集为一类。

得到感兴趣区点云数据Proi后，对其进行体素栅格下采样( voxel grid downsampling)。最后，对降采样后的点集进行移动最小二乘滤波，将得到的点云进行重采样。

3. 2. 3 物体表面重建

本文采用文献[17]中提出的贪婪投影三角化方法。该方法的基本思路是将有向点云投影到某一局部二维坐标系内，在该坐标系内进行平面内的三角化，在根据平面内点的拓扑连接关系获得一个三角网格曲面模型。

算法输入参数为搜索半径r，最大近邻个数N，近邻最远距离D，三角形的最大最小角α1、α2，最大表面角β，表面法向连续性，输出为三角化后的点云。

经过上述算法生成的物体表面网格会存在局部的不平滑区域，需要经过网格的平滑处理。这里采用文献[18]提出的一种表面网格双边滤波(bilateral denoising)的方法。双边滤波最初被使用在图像的去噪处理上，这种方法与传统的高斯平滑等方法相比，能保持图像中的局部特征。将此方法应用在三维网格的去噪上，主要是将空间离散的三维点到局部表面的距离看作是二维图形中单个像素的灰度值，来进行类似的双边滤波，最终得到重构后的物体表面。

4　虚拟交互力的实现

4. 1 基于力势场的辅助力建模

在传统的双边操控系统中，操作者获得的力反馈通常直接来自于机械臂的末端安装的力传感器的数据。然而，由于遥操作系统中大时延的影响，接触力数据滞后于操作者的操作。在机械臂高速运行时，或刚性接触下，从端机械臂可能会因为机械碰撞而导致损坏。

人工势场法(Artificial Potential Field，APF)是由Khatib于1986年提出的，广泛应用于自主移动机器人的路径规划中。其基本思想是，机器人的工作环境被转化为一个势量场，从而将机器人在周围环境中的运动，设计成为一种在抽象的人造引力场中的运动；目标点对处于势量场中的机器人产生“吸引”趋势，障碍物对机器人产生“排斥”趋势；最后通过两者的合成效果来控制机器人的运动[8]。

为了简化这个问题，我们首先考虑单独一个物体O，机器人末端执行器的位置为x，xd为目标位置，人工势场可以表达为式(1):

虚拟力反馈为式(2):

其中，各项分别满足式(3)～(8):

Fxd为使机械臂末端从x到达目标xd位置的吸引力，而FO为排斥力。

4. 2 基于模型参数修正的接触力预测方法

从端机械臂和环境的几何模型只描述了机械手和环境对象的静态特性，而虚拟预测环境模型要求能与操作者发生交互，响应操作者指令，因此虚拟从机械手和虚拟环境对象相接触时还必须能够提供真实的接触力的感觉，所以需要对与机械臂接触的环境进行动力学建模。

当机械手与环境稳定接触时，环境动力学可以通过线性动力学模型进行分析。假定在机械手同环境平稳接触过程中，机械手与环境均未受到损坏，环境动力学特性可以用如图3的二阶弹簧-质量-阻尼体表示。

图3　弹簧-质量-阻尼模型Fig. 3 Model of spring-mass-damp

其中me，be，ke分别表示环境动力学参数:惯性质量、阻尼系数和弹性系数。xe为环境的位移，则环境的动力学特性可用式(9)所示线性动力学方程描述:

fe为从机械手末端与环境的交互作用力。

在3D环境中，在x、y、z三个维度上存在式(10):

令F = [FxFyFz]T表示计算出的从机器人末端与环境的接触力，a = [axayaz]T是一组三维空间向量表示目标物体接触面由于刚性接触产生的形变量。和度。m、b = [bxbybz]T和k = [kxkykz]T分别表示环境的质量、阻尼、弹性三种动力学参数。即在三维空间中虚拟接触力的计算式为式(11):

于是我们得到，为使得方差最小，各个维度需要满足式(13):

展开后得到式(14):

当环境的动力学参数相对稳定且变化不大时，采用SLSM算法在线辨识动力学参数。在采样时间t，通过方程(15)(16)得到:

公式(16)中，[A(t) ]、[C(t) ]分别通过式(17)、(18)得出:

其中，i =1、2、3，j =1、2、3。

当虚拟机械臂接触虚拟环境中的物体在t时刻产生av的位移时，该接触力可以通过公式(19)计算:

5 实验过程与结果分析

5. 1 实验平台搭建

为了验证上文中提出的方法，我们在实验室环境中建立了一套遥操作实验平台。如图5所示，Phantom Omni作为主端机器人，7自由度SCHUNK LWA4D机械臂作为从端机器人，PG -70二指并行手抓用于抓取和移动物体，6自由度力/力矩传感器用于采集接触力信息。KINECT相机用于采集远端点云信息并以此重构虚拟场景。在Microsoft Visual Studio平台上使用Open-GL开发库建立虚拟环境界面。远端机械臂系统采用Ubuntu 12. 04下的ROS ( Robot Operation System)开发控制程序。为了模拟通信链路的时延特征，系统设置了上下行均为1000 ms的时延。

实验中，操作者通过观察本地建立的虚拟场景，使用力反馈设备控制机器人从起点S出发，绕过水平面上的障碍物A和B之后，到达目标位置T与一弹性平面C进行多次接触并感受反馈力。如图5所示:

图4　遥操作实验平台Fig. 4 Teleoperation exprimental platform

图5　遥操作实验任务Fig. 5 Teleoperation task

5. 2 实验结果

三维空间中，远端机械臂末端位置跟随主端手控器的位置的轨迹如图6～8所示，可以看出，在2 s的时延下，从端能够很好的跟随主端的操作者的意愿运动。

图6　X方向上末端位置Fig. 6 Position of end-effector in X-aix

图7　Y方向上末端位置Fig. 7 Position of end-effector in Y-aix

图8　Z方向上末端位置Fig. 8 Position of end-effector in Z-aix

图9　不同操作模式下机械臂末端轨迹Fig. 9 Paths under different operation method

图9表示了在不同控制模式下，机械臂末端的轨迹图。Line 1表示有力势场引导下的运动轨迹，而Line 2与Line 3表示没有力势场引导时的两种轨迹情况。从图中可以看出，没有力势场情况下，会出现意外碰撞障碍物(Line 2)或者绕远路(Line 3)的情形，而在有力势场的帮助下，操作者能够最快最安全的接近待操作目标，进而进行下一步抓取等操作任务。

观察Z方向上，腕力传感器采集到的数据与虚拟场景计算出的虚拟力的跟随曲线，如图10所示。可以看出，在与真实环境中的物体进行接触时，采用滑动最小二乘法来在线更新模型动力学参数，从而模拟出的接触力能够反应出真实环境的接触力的感觉，且误差在5%以内。

图10　Z方向上末端力Fig. 10 Forces in Z-aix

6　结论

本文对具有虚拟3D视觉和虚拟力交互的空间遥操作机器人系统进行了研究，在本地端基于VS2008和OpenGL，搭建了具有虚拟视觉/力觉反馈的主从遥操作平台。采用人工势场的方法，引导操作者控制主机器人设备避开障碍物更安全地到达目标位置。为了向操作者提供远端环境的精确模型，采用了滑动最小二乘法在线更新模型的动力学参数。在操作者实际操作过程中，未与环境接触时，系统同样提供给操作者一定强度的引导力；当操作者控制的主机器人末端与虚拟环境中物体相接触时，实时提供给操作者接触力反馈，以达到预测的目的，从而克服时延对系统带来的影响。实验证明，该方法在实现保证操作性能的前提下，提高了操作效率和安全性能。

参考文献(References)

[ 1 ] 李大明，饶炜，胡成威，等.空间站机械臂关键技术研究[J].载人航天，2014，20(3): 238-242. LI Daming，RAO Wei，HU Chengwei，et al. Key technology review of the research on the space station manipulator[J]. Manned Spaceflight，2014，20(3): 238-242. (in Chinese)

[ 2 ] Chopra N，Spong M W，Hirche S，et al. Bilateral teleoperation over the internet: the time varying delay problem1[J]. Urbana，2003，1(4-6): 155-160.

[ 3 ] Passenberg C，Peer A，Buss M. A survey of environment-，operator-，and task-adapted controllers for teleoperation systems[J]. Mechatronics，2010，20(7): 787-801.

[ 4 ] Anderson R J，Spong M W. Bilateral control of teleoperators with time delay[J]. Automatic Control，IEEE Transactions on，1989，34(5): 494-501.

[ 5 ] Hirzinger G，Brunner B，Dietrich J，et al. ROTEX-the first remotely controlled robot in space[C] / / Robotics and Automation，1994. Proceedings. 1994 IEEE International Conference on. IEEE，1994: 2604-2611.

[ 6 ] Kim W S，Hannaford B，Fejczy A K. Force-reflection and shared compliant control in operating telemanipulators with time delay[J]. Robotics and Automation，IEEE Transactions on，1992，8(2): 176-185.

[ 7 ] Hashtrudi-Zaad K，Salcudean S E. Transparency in time-delayed systems and the effect of local force feedback for transparent teleoperation [ J]. Robotics and Automation，IEEE Transactions on，2002，18(1): 108-114.

[ 8 ] Khatib O. Real-time obstacle avoidance for manipulators and mobile robots[J]. The international journal of robotics research，1986，5(1): 90-98.

[ 9 ] Cahyadi A，Ya-Chun C，Yamamoto Y. Stable mobile robots teleoperation via Potential Field method[C] / / Advanced Intelligent Mechatronics，2008. AIM 2008. IEEE/ ASME International Conference on. IEEE，2008: 347-352.

[10] Li X，Song A，Li H，et al. Real-time obstacle avoidance for telerobotic systems based on equipotential surface[J]. International Journal of Advanced Robotic Systems，2012，9(71): 1-8.

[11] Ni T，Zhang H，Xu P，et al. Vision-based virtual force guidance for tele-robotic system[J]. Computers & Electrical Engineering，2013，39(7): 2135-2144.

[12] Yamada H，Tao N，Dingxuan Z. Construction tele-robot system with virtual reality[C]/ / Robotics，Automation and Mechatronics，2008 IEEE Conference on. IEEE，2008: 36-40.

[13] Yoon W K，Goshozono T，Kawabe H，et al. Model-based space robot teleoperation of ETS-VII manipulator[J]. Robotics and Automation，IEEE Transactions on，2004，20(3): 602-612.

[14] Zhu B，Song A，Xu X，et al. Research on 3D virtual environment modeling technology for space tele-robot[J]. Procedia Engineering，2015，99: 1171-1178.

[15] Zhang G，Wang Z，Du J，et al. A generalized visual aid system for teleoperation applied to satellite servicing[J]. Int J Adv Robot Syst，2014，11(2): 1-7.

[16] 徐祯琦，龚光红.基于Kinect的物体表面重建方法研究[J].中国体视学与图像分析，2013 (3): 255-262. XU Zhenqi，GONG Guanghong. Study on Kinect-based surface reconstruction[J]. Chinese Journal of Stereology and Image Analysis，2013 (3): 255-262. (in Chinese)

[17] Huijun L，Aiguo S. Virtual-environment modeling and correction for force-reflecting teleoperation with time delay[J]. Industrial Electronics，IEEE Transactions on，2007，54(2): 1227-1233.

[18] 宋爱国.力觉临场感的理论与实验研究[D].南京:东南大学，1996. SONG Aiguo. Theoretical and Experimental Research on Force Telepresence [ D]. Nanjing: Southeast University，1996. (in Chinese)

[19] Smits R，Bruyninckx H，Aertbeliën E. KDL: Kinematics and dynamics library[J]. Avaliable: http:/ / www. orocos. org/ kdl，2011.

[20] 康金兰，晁建刚，林万洪，等.视觉手势识别技术在航天虚拟训练中的应用研究[J].载人航天，2014，20(3): 219-222. KANG Jinlan，CAO Jiangang，LIN Wanhong，et al. Research on application of visional gesture recognition technology in manned space virtual training [ J]. Manned Spaceflight，2014，20(3): 219-222. (in Chinese)

[21] 陈学文，晁建刚，安明，等.基于体态识别的航天员虚拟训练仿真技术研究[J].载人航天，2015，21(3): 217-223. CHEN Xuewen，CHAO Jiangang，AN Ming，et al. Research on Simulation Technology of Astronaut Virtual Training Based on Posture Recognition[J]. Manned Spaceflight，2015，21 (3): 217-223. (in Chinese)

·空间功能机构技术·

Visual-haptic Aid Teleoperation System Based on Environment Modeling and Updating

XU Xiaonong，SONG Aiguo，ZHU Chengcheng，Ni Dejing
(Southeast University，School of Instrument Science and Engineering，Nanjing 210096，China)

Abstract:In order to enhance the interactive performance of space teleoperation system，a new kind of teleoperation system based on visual-haptic aid method was put forward. Combined with virtual reality technology，virtual 3D visual-haptic feedback teleoperation experiment platform was set up with the haptic device. A KINECT camera was used to collected 3D point cloud data. Sliding Least Squares Method (SLSM) was also adopted to identify the model parameters and update the local virtual environment online. The real-time vision and force were fed back to the human operator through the virtual environment. A shared control paradigm was developed based on artificial potential field，which could create attraction or repulsion force，to extend the human’s ability for obstacle avoiding and target approaching. This method was implemented on a 7-DOF SCHUNK robot manipulator. It was verified that in the presence of a round-trip communication delay of up to 2s the operation efficiency increased significantly.

Key words:virtual environment；3D modeling；model updating；teleoperation；visual-haptic aid

作者简介:徐效农(1987 - )，男，博士研究生，研究方向为遥操作机器人技术。Email:xxn@ seu. edu. cn

基金项目:国家自然科学基金资助项目(61325018，61272379)

收稿日期:2015-08-24；修回日期:2016-01-06

中图分类号:TP242

文献标识码:A

文章编号:1674-5825(2016)01-0055-07