机器人航天员双臂操作仿真系统的研究与实现

熊明华，张晓东，肖 涛，王耀兵

（空间智能机器人系统技术与应用北京市重点实验室北京空间飞行器总体设计部，北京100094）

机器人航天员双臂操作仿真系统的研究与实现

熊明华，张晓东，肖 涛，王耀兵

（空间智能机器人系统技术与应用北京市重点实验室北京空间飞行器总体设计部，北京100094）

针对机器人航天员复杂操作场景下的任务规划和仿真验证问题，基于仿真模块集成化的设计思想，集成通信模块、任务调度管理模块、路径规划模块、力控制模块、碰撞检测模块、双臂3D几何模型，通过OSG建立了实体模型及仿真场景，并建立插拔、抓握、对准、旋拧、搬运、转移等典型操作库，实现了机器人航天员双臂多种复杂操作任务的仿真。最后，通过机器人航天员搬运物体任务仿真实现双臂协调操作任务规划的仿真，验证了该系统的有效性和任务规划验证的合理性。

双臂机器人；路径规划；碰撞检测；一体化仿真

1 引言

随着计算机图形处理能力的提升，实时三维可视化仿真成为机器人任务规划和运动控制仿真验证的重要工具［1］。基于仿真模块集成化设计思想而开发的机器人航天员双臂操作一体化仿真系统，可有效指导空间机器人系统任务的仿真验证［2］。在该领域已经有许多研究，Burdea［3］基于虚拟现实技术建立了Puma560模型并进行目标抓取力的仿真验证；Xu Hua［4］设计了一种双臂机器人的图形化编程和仿真控制系统，用户通过图形编程对机器人进行任务规划，可在三维仿真环境中分析规划结果；周军［5］针对冗余度双臂机器人进行轴孔装配的运动学约束关系，对冗余双臂机器人的轴孔装配作业进行三维动态仿真验证；Univ Malaya［6］建立一个集成的三维模拟软件和虚拟现实系统以优化机器人的运动时间并解决机器人的编程问题，具备在虚拟现实和三维机器人真实环境中传递和优化数据的功能。

以上研究都是针对双臂机器人某单一作业任务进行规划仿真，而实际的机器人航天员双臂操作任务是对机械臂单臂运动和双臂协调操作的综合，是由一系列基本动作序列组成的，这些基本序列涉及到单臂的路径规划、抓取物体和双臂之间的协调操作。机器人的动作很多具有相同的性质，因此将相似的动作看作一个基本动作单元，通过调用基本动作单元，并按特定顺序组合就可完成复杂的任务，从而基于仿真模块集成化实现任务仿真。因此，本文基于机器人航天员双臂操作任务分解和动作规划为基础，在 Visual Studio 2010的开发平台下，以C语言编程实现各主要算法，利用开源三维引擎OSG（Open Scene Graph）建立双臂机器人模型、基本动作库及工作环境，通过模块化的设计思想，集成了机器人航天员双臂操作的几大关键模块，开发出一套集协调操作规划、三维可视化处理、机器人力控制及碰撞检测于一体化的仿真系统，并进行了仿真验证。

2 一体化仿真系统总体方案

通过机器人航天员双臂操作任务需求分析，在覆盖所需功能需求的基础上，建立了完整的机器人航天员双臂操作的一体化仿真系统，主要包括任务调度管理模块、路径规划模块、力控制模块、碰撞检测模块、通信模块、视觉模块及三维仿真模块等，集成仿真系统结构如图1所示。

各模块功能如下：

1）任务调度管理模块：该仿真系统最主要的模块之一，在实际控制中，主要负责底层任务规划；任务调度管理模块从指令接收模块中获取当前任务的信息，结合接收到的机器人视觉／力测量数据和规划库来完成操作人员设定的相关任务；

2）路径规划模块：主要包括单臂笛卡尔空间路径规划、关节空间路径规划、双臂协调操作及避障路径规划，路径规划算法采用C语言进行编写，形成双臂机器人动作的基本规划库，并以动态链接库形式供任务调度管理模块进行调用；

3）力控制模块：主要完成机器人的柔顺控制任务，根据机器人力传感器反馈信息与任务规划中理想力矩的差值，调用自适应阻抗控制算法库，实现对机器人的力位混合控制；

4）碰撞检测模块：根据整个系统的当前位置与状态，判断是否与自身及周围物体碰撞；

5）通信模块：数据通信模块主要完成对机器人航天员控制过程中指令的收发，以及实现与数据管理、三维仿真、视觉模块等外部单元之间的数据交互，操作输入端、数据管理以及三维仿真通过以太网与任务调度管理模块进行通信；

6）视觉模块：机器人航天员的视觉处理模块，一方面用于拍摄视觉图像，作为操作人员的操作输入；另一方面用于测量目标位姿，作为路径规划的依据；

7）三维仿真模块：实现三维仿真环境建模，增强临场感，提升人机交互体验。

3 关键模块的设计与实现

为完成搭建机器人航天员双臂操作任务仿真，本文重点对操作任务仿真所必需的三维仿真模块、任务调度管理模块、双臂协调操作路径规划模块进行论述，力控制模块、视觉等模块可参考文献［5］等进行设计。

3.1三维仿真模块

OSG是一套基于C＋＋平台的应用程序接口，能够快速、便捷地创建高性能、跨平台的交互式图形程序。通过OSG可绘制出逼真的三维仿真环境，增强操作员的临场感，提升控制单元的人机交互体验。因此，本模块基于OSG接口，采用Open⁃GL技术进行开发，模型创建流程如下：

1）将基于PROE软件绘制出的机器人航天员模型导出至3DMAX中，并进行渲染；

2）完成渲染后，保存并导出3ds格式文件；

3）将3ds格式文件导入OSG，创建组织节点，并获取其参考坐标系，通过确定各节点之间的父子关系即矩阵变换，使其在三维空间中组装成期望的仿真模型；

4）设置关节角度的驱动变量，建立模型驱动接口。

其中，三维模型建立过程为首先建立运动学参数矩阵，之后进行模型导入及装配，其基本流程简述如下：

1）根据机器人航天员各组合体坐标系创建运动学参数矩阵，并关联坐标系模型，使关联的坐标系模型与建立的运动学参数坐标系完全一致；

2）将所有三维模型读入到三维可视化软件中，使之关联到对应的模型节点，读入所有模型成功后转入第三步，否则给出提示；

3）为每个模型节点关联一个姿态调整矩阵，通过其调整各个模型的初始姿态，直至所有模型的位置都符合要求；

4）将每个模型节点姿态调整矩阵关联到对应的运动学参数矩阵；

5）根据装配关系组装运动学参数矩阵；

6）将运动学参数矩阵关联到OSG根节点中，使之能够在OSG仿真窗口中显示。

3.2任务调度管理模块

任务调度管理模块是仿真系统最主要的模块之一，在实际的控制中，主要负责底层任务规划。对于机器人航天员系统的某个任务，其控制过程被分解为多个步骤，任务调度管理模块根据当前机械臂的执行情况来规划下一步的执行内容，此时需要一个定时器定时启动规划下一步内容来完成任务规划。在双臂机器人的任务规划阶段，需要根据机器人的当前位姿和目标物的状态等信息做出合适的路径规划方案。任务规划过程中，任务调度管理模块从指令收发模块中获取当前任务的信息，并结合接收到的机械臂实时数据来完成操作人员设定的相关任务。

对于操作人员设置的具体任务，每次根据任务期望结果与当前机械臂的状态及当前执行的时间来规划下一步操作。由于任务规划过程中需要等待机械臂返回的实时状态和数据以规划下一步操作，进而保证机械臂准确安全地执行工作，故底层任务规划采用“等待－判别－执行”机制来完成每份指令或数据的发送，任务调度管理模块流程逻辑图如2所示。

该系统中的控制平台可以提前对任务进行规划和调整，通过任务调度管理模块接口，完成任务信息的规划。任务调度管理模块能够在障碍环境中规划多个无障碍动作序列，利用简单直线路径规划算法拼接复杂任务，使机械臂在轨迹规划阶段前就可以提前检测出任务路径中可能出现的障碍物并进行躲闪。

任务调度管理模块提供多种规划方案，并以任务中间点的形式给出，以此达到躲避障碍目标、优化路径的目的，降低机械臂在运行过程中发生碰撞的可能性，提高机械臂在不同环境下的工作能力和适应性。

任务规划算法总体思路遵循简单路径拼接复杂任务、添加任务中间点数量最小化。算法从点到点直线运动方案尝试，逐步增加任务中间点，任务中间点只在遇到障碍的任务阶段添加，无障碍阶段保证动作精简。具体算法如下：

1）对任务目标点反解，如果目标构型有碰撞，则返回任务失败；

2）对任务始末点的末端轨迹进行碰撞检测，如果有碰撞则根据末端轨迹碰撞算法插入任务中间点。如果无碰撞，则返回任务规划结果；

3）通过碰撞算法检测任务中间点碰撞情况，如果无碰撞，则将当前动作按中间点划分为两个动作，递归调用step2。如果有碰撞，对中间点增加安全距离进行调整；

4）调整后的中间点如果依然碰撞，则任务失败。否则返回规划结果；

5）路径规划中，当遇到障碍，则增加中间点，并进入step3。

3.3双臂协调路径规划模块

机器人航天员双臂操作具有类人双臂的特点，可以代替或协助人类完成工具组装、更换等复杂作业。通过对双臂系统任务操作分析，可得出双臂系统在执行任务过程中需进行的基本操作包括：插拔、抓握、对准、旋拧、搬运、转移等。通过对双臂机器人系统的基本操作特点进行分析，可以将基本操作划分为两大类：

第一类：双臂机器人系统双臂末端有相对运动方式，比如插拔、旋拧等；

第二类：双臂机器人系统双臂末端无相对运动方式，比如搬运、转移等。不同的运动方式将得到不同的位置、速度和加速度约束方程。

路径规划模块主要包括：单臂笛卡尔空间路径规划、关节空间路径规划；双臂协调操作及避障路径规划。路径规划算法采用标准C语言进行编写，以动态链接库形式进行调用。各路径规划函数的输入输出概括如下：

1）单臂笛卡尔空间路径规划以机械臂初始和目标位姿为输入，以机械臂关节角序列为输出；关节空间路径规划函数以机械臂初始及目标关节角序列为输入，以规划后的关节角序列为输出；

2）双臂协调操作包括双臂笛卡尔空间及双臂关节空间路径规划。其中，双臂笛卡尔空间路径规划以双臂运动模式，初始位姿及目标位姿为输入，以双臂关节角序列为输出；双臂关节空间路径规划以左右臂初始及目标关节角序列为输入，以规划后的关节角序列为输出；

3）避障路径规划算法以避碰关键点、有障碍下运动方程、避碰速度为输入，以机械臂关节角序列为输出。

本文将双臂复杂的运动过程分解为基本的路径规划，在需要时进行调用。在双臂机器人系统运动学建模的基础上，得到系统的正逆解运动学方程，进而可以按照图3所示的基本流程调用轨迹规划模块，实现双臂协调操作。

3.4碰撞检测模块

碰撞检测模块作为仿真验证平台的重要组成单元，主要检测在仿真环境中检测场景内各个物体之间是否会发生碰撞，完善仿真验证环境的真实性，从而避免机器人航天员双臂在路径规划过程中可能会发生双臂之间、臂与基座以及所在空间外部障碍物之间的碰撞情况。

为简化模型，采用长方体和圆柱体两种简化模型代替杆件，球形简化模型代替障碍物，继而运用空间几何和最优化理论中的可行方向法得出圆柱体与球体、圆柱体与圆柱体、长方体与球体、长方体与长方体四种类型的最短距离计算方法，最后将其应用于杆件与障碍物、杆件与杆件之间的碰撞检测中。

整个机器人航天员双臂操作的碰撞检测流程如图4所示。

4 仿真验证

为验证机器人航天员仿真平台的有效性及在机械臂研究应用方面的可行性，设计机器人航天员双臂协调操作搬运物体任务。任务要求如下：机器人航天员将一个物体从一个位置移动到另一个位置，保证双臂的协调运动，即需要位置控制实现双臂末端保持一定的相对位姿；为了保证物体不会掉落、损坏，通过力控制保持接触力在某一范围内，任务过程如图5所示。

根据机器人航天员双臂协调操作搬运物体任务需求，首先利用基于模块化组件的通用仿真平台搭建双臂协调机械臂及虚拟仿真环境，根据任务路径规划需求，添加路径规划功能模块、碰撞检测模块及力控制模块。通过设计任务的仿真，验证模块化通用仿真平台的有效性。具体步骤如下：

1）创建模块化组件库。收集机器人对象及其工作环境信息，利用模块化预处理软件创建相关节点信息问题，导入模块化组件库；

2）搭建平台虚拟仿真工作环境。根据机器人工作环境布置需求，设置环境节点安装参数，完成平台环境节点布置；

3）搭建仿真机器人对象。利用机器人节点信息文件，搭建平台机器人对象；

4）仿真平台环境检验。在平台仿真环境搭建完成后，对机器人及工作环境信息进行校验，利用静态分析单元对各结构进行分析，保证虚拟仿真环境的正确性；

5）添加路径规划、碰撞检测及力控制模块，并进行相应的功能测试。

在仿真环境搭建完成后，利用本平台进行机器人航天员双臂协调搬运物体任务验证。首先，机器人航天员双臂运动到夹持位置，夹紧搬运物体，采用双臂协调路径规划算法，将其搬运至目标点，此过程需要考虑双臂力位混合控制，同时双臂末端无相对运动，保证在搬运过程中不会产生内力，任务仿真过程如图6、图7所示。

构型规划总时间为20 s，时间步长为0.05 s。规划过程中的关节角速度如图8所示。

双臂将搬运物体要求误差为0.1 mm，根据图9所示仿真的结果，双臂将搬运物体搬运到目标位置后，最大角度误差为0.68×10－7。产生的机械臂末端误差在允许范围内。双臂机器人的末端到达了指定位姿，完成了双臂机器人系统搬运物体的全部任务。

5 结论

本文采用仿真模块集成化设计思想建立了一体化仿真系统，集成了任务调度管理、双臂协调操作规划、三维可视化、力控制和碰撞检测等功能模块，通过调用双臂协调典型操作库，实现了机器人航天员双臂协调搬运操作任务仿真，仿真结果表明，本文建立的机器人航天员任务仿真验证平台可有效的实现机械臂应用可行性验证和任务规划合理性验证。

（References）

［1］ 郑洁，张卫民，吴昊，等.基于Vega的柔性双臂空间机器人的三维动画仿真［J］.计算机工程与科学，2004，26（10）：72⁃74. Zheng Jie，Zhang Weimin，Wu Hao，et al.3⁃D animational simulation of a flexible dual⁃manipulator space robot based on vega［J］.Computer Engeneering＆Science，2004，26（10）：72⁃74.（in Chinese）

［2］ Shin S Y，Kim C H.Human⁃like motion generation and con⁃trol for humanoid＇s dual arm object manipulation［J］.IEEE Transactions on Industrial Electronics，2015，62（4）：2265⁃2276.

［3］ Burdea G C.Invited review：the synergy between virtual real⁃ity and robotics［J］.IEEE Transactions on Robotics and Au⁃tomation，1999，15（3）：400⁃410.

［4］ Cao Qixin，Zhang Zhen，Gu Jiajun.Graphical programming，simulation and control system for dual⁃arm mobile robot［J］. Journal of System Simulation，2006，18（9）：2542⁃2546.

［5］ 周军，丁希仑，陆震.冗余度双臂机器人轴孔装配的三维动态仿真与实验［J］.机器人，2006，28（4）：422⁃427. Zhou Jun，Ding Xilun，Lu zhen.3⁃D dynamic simulation and experiment for shaft and hole mating of redundant dual⁃arm ro⁃bot［J］.Robot，2006，28（4）：422⁃427.（in Chinese）

［6］ Tahriri F，Mousavi M，Yap H J，et al.Optimizing the robot arm movement time using virtual reality robotic teaching sys⁃tem［J］.International Journal of Simulation Modelling，2015，14（1）：28⁃38.

（责任编辑：龙晋伟）

Research and Realization of Integrated Simulation System for Robonaut Dual⁃arm Manipulation

XIONG Minghua，ZHANG Xiaodong，XIAO Tao，WANG Yaobing

（Beijing Key Laboratory of Intelligent Space Robotic Systems Technology and Applications，Institute of Spacecraft System Engineering CAST，Beijing 100094，China）

An integrated simulation system for dual⁃arm robonaut was developed in the paper to deal with the mission planning and validation problems in the complicated scenarios.Based on the modu⁃lar design concept，the simulation system was composed of the communication module，task schedu⁃ling management module，path planning module，dynamic control module，3D geometric model and collision detection module.The simulation scenarios were created by OSG（Open Scene Graph）. Then，the missions of the dual⁃arm manipulator were divided into a series of basic movements such as the plug，grasp，screw，transport，and the simulation of the complicated mission was realized by using the special combination of the basic movements.Finally，the algorithms of coordinated dual⁃arm robot was verified by the mission of transporting objects.The results demonstrated the effective⁃ness of the system and verified the feasibility of mission planning.

dual⁃arm robot；path planning；collision detection；integrated simulation

TP242.3

：A

：1674⁃5825（2017）02⁃0150⁃06

2015⁃11⁃02；

2016⁃08⁃08

国家自然科学基金（61573058）

熊明华，男，硕士，高级工程师，研究方向为空间机器人技术。E⁃mail：13910830761＠139.com