基于VC平台的服务机器人运动控制与仿真方法*

□ 黄万永 □ 屠大维,2 □ 江济良

1.上海大学 机电工程与自动化学院 上海 200072

2.上海市智能制造及机器人重点实验室 上海 200072

机器人仿真技术为机器人的设计和研究提供了一个高效、便利的手段，是机器人学研究中的一项重要内容。迄今为止，国内外进行了大量的机器人仿真方法和仿真系统的研究，主要研究工作都集中在3D制图软件仿真模块的应用、MATLAB仿真以及OpenGL仿真［1-5］等方面。

3D制图软件包括UG、Pro/E等主要应用于产品的设计和分析，仿真只在虚拟制造和虚拟加工领域应用较广［6］，其目的主要在于制作动画、展示加工过程；可扩展性低，不具备开放性。MATLAB在机器人领域应用比较广泛，能够借助MATLAB的强大算法进行仿真实验。但MATLAB具有两大先天的缺陷，一是它在3D图形显示方面有着明显的不足，无法直观地表达出机器人的末端位姿和工具手姿态，很难判断出机器人与外部环境之间的干涉或碰撞［7］；另一个是它作为一个独立的数学计算平台，缺少与外部控制系统的接口，一般只应用于算法的验证［8］。OpenGL是计算机图形技术领域的集大成者，它独立于硬件与窗口系统，具有内核小、运行快、可移植等特点，并能够与VC平台“无缝”链接，因此被广泛应用于虚拟仿真系统的开发［9-11］。

笔者针对3D制图软件和MATLAB的不足，采用OpenGL 3D图形接口建立服务机器人虚拟仿真模型，通过对仿真体系结构、系统的通信方法、3D模型的驱动、运动学算法等的设计，建立服务机器人虚拟仿真控制平台。该平台可以进行虚拟机器人的实时运动控制、轨迹规划、状态监控、虚拟漫游、虚拟示教等工作，并能够结合智能规划方法完成虚拟倒水等仿真作业。

1 服务机器人仿真系统的实现步骤

1.1 仿真模型的构建与存储

虚拟机器人是以现实的服务机器人为蓝本，借助OpenGL 3D强大的图形接口，完成机器人基本模型的绘制。如图1所示，机器人的几何模型由若干个部件模型拼装而成，部件间的相互位置关系由装配信息（关节的结构参数和运动参数）唯一确定，装配信息包括关节类型、关节之间相对坐标关系、关节取向等。虚拟服务机器人模型包括底座、中间躯干、模块化双臂、头部，为了控制服务机器人的组合运动和整体运动，服务机器人采用树状连接，每个节点相当于一个模块，各个模快是分级的，上一节点可以带动下一节点的运动，这样就可以通过编程来控制各个模块的独自运动和整体运动来驱动仿真机器人运动。

为了提升仿真的真实性，构建了仿真环境模型，该模型中的室内房间、走廊、门等位置尺寸固定不变，室内家具、杂物可以通过3D模型数据库导入，如图2所示。仿真机器人在仿真环境中可以模拟现实机器人在现实环境中的各种任务，用户可以通过操控仿真机器人在仿真环境中完成相应的任务。

▲图1 虚拟双臂服务机器人模型

▲图2 仿真环境模型

1.2 仿真系统体系结构

为了使仿真系统能够真正模拟真实机器人的控制和操作过程，并能够与虚拟机器人相连完成模拟真实机器人的控制过程，因此必须采用合理的仿真体系结构，保证仿真系统的可靠性和对机器人仿真的真实性。

▲图3 仿真系统的体系结构

仿真系统的体系结构如图3所示。仿真系统采用MVC（Model View Controller）结构设计，使控制器、视图、模型相分离，形成一个开放式的闭环仿真系统。仿真系统控制器的输入可以是键盘（鼠标）、屏幕点动、遥控操作控制器等；这些输入产生的空间位移和方向转换是一个空间矢量，这个空间矢量为机器人的目的矢量。通过对这个目的矢量的输入进行位置计算、运动学计算、限制监测计算等来驱动相应的模型，最后通过业务模型的选择来驱动相应的视图展现给用户。

1.3 仿真系统内部的数据通信

在VC平台下编写仿真系统，需要封装很多不同功能的类，并且从属于不同的线程，这些线程和类之间在不断地进行大量的数据交换。例如：机器人运动学模块计算出当前各个模块的位姿，此时位姿显示对话框类、3D视图变换类、机器人关节限位检测类、机器人正反解计算的运动学类等位姿都需进行更新，这种典型的广播式通信可以采用设计模式中的观察者模式来解决。定义共享数据源为观察者模式中的“主题类”，需要数据源数据的其它类为“观察者类”，当观察者需要接收主题类的信息时，可以通过主题类进行登记，这样主题类就可以对登记的观察者进行消息广播，更新观察者类的自身数据。这种方式避免了对C++数据封装的破坏，同时也消除了内存在不同线程和类之间快速切换中导致乱序的风险。

1.4 仿真系统的运动控制

根据手臂硬件连接结构和D-H （Denavit-Hartenberg）参数描述方法，建立双臂服务机器人六自由度手臂的连杆坐标系模型，如图4所示，并得到其D-H参数。D-H参数和各关节范围列于表1中。

表1 服务机器人手臂D-H参数

其中，α表示相邻Z轴之间的转角，a表示相邻Z轴之间公垂线段长度，d表示相邻X轴之间的距离，θ表示绕Z轴的旋转角。

手臂各相邻杆件之间的D-H变换矩阵：

i-1Ti表示连杆坐标系｛i｝相对于连杆坐标系｛i-1｝的齐次变换矩阵，Rot（X，αi-1）表示相对于 X 轴旋转 αi-1角度，Trans（X，ai-1）表示相对于 X 轴平移 ai-1，同理可知Rot （Z，θ）、Trans （Z，di）。 根据表 1 中 D-H 参数和式（1）， 可得各连杆之间奇次变换矩阵0T1、1T2、2T3、3T4、4T5、5T6。

机器人手臂采用图4所示与竖直方向成10°的安装。将左右手臂的坐标统一到同一个基坐标系 ｛Ob-XbYbZb｝下，其中Ob为服务机器人两主动轮中心连线的中点，根据机器人本身的结构尺寸和坐标旋转角度可得左右手臂的奇次变换矩阵bT0。因此，服务机器人手臂末端坐标系｛O6｝相对于服务机器人基座坐标系｛Ob｝的奇次变换矩阵为：

式中：［n s a］表示手臂末端的姿态，[p]表示手臂末端坐标系原点位于基坐标系中的位置。

根据欧拉公式可得手臂末端位姿［x，y，z，O，A，T］，O、A、T 为欧拉角。

手臂逆运动学求解流程如图5所示，采用几何法求解出手腕坐标（pAx，pAy，pAz），进而通过代数法求解出对应的关节角。主要求解公式：

式中：si、ci表示 sinθi、cosθi。

手臂的后3个关节为典型的球腕结构，对于给定的手臂姿态存在上仰腕和下垂腕两种情况，前3个关节确定手腕的位置，后3个关节确定手腕的姿态。因此，最多存在8组逆运动学封闭解。

根据经验，遵循运动的关节角加权和最小原则并考虑基座约束等因素，选择一组实际可行的最优解。假设服务机器人手臂当前关节角为 θi（i=0，1，…6），通过逆运动学求解总共得到n组解（1≤n≤8，n∈Z），其中第 j组封闭解为 θij（i=0，1，…，6，1≤j≤n），定义第 j组封闭解运动的关节角加权和为，则有：

式中：λi为第 i关节的加权系数，i=0，1，…，6。

考虑各个关节所承受的力矩、载荷和冲击等因素，确定出加权系数集｛λi｝=｛0.29，0.24，0.19，0.14，0.09，0.05｝，通过比较选择出最小的关节角加权和，即有：

至此即选择出第k组封闭解为实际可行的最优解。在实际控制中，除了要考虑基座约束的影响，还要考虑双臂协作防碰、空间可达、安全高效等要求，选择出一组空间可达、安全、运动量小的最优解。

采用以上的方法建立的仿真系统运动过程如图 6 所示，图 6（a）为运动学求解视图，图6（b）为机器人根据求解值的运动视图。机器人根据指定的目标点走出一条平稳连续光滑的轨迹路线图。

▲图5 逆运动学求解流程

2 仿真实验

通过以上方法，建立起模块化的与真实机器人控制器相同的实时仿真系统，在3D仿真功能的基础上可以很方便地实现其它功能扩展，如虚拟示教、虚拟漫游、轨迹规划等，完成机器人虚拟任务作业。

▲图6 运动学过程

虚拟机器人倒水任务作业是一个复杂的过程，本文提出了一种机器人智能规划方法来完成虚拟机器人的任务作业。机器人智能规划方法采用语义网络推理的方式，如图7所示，围绕着取水这个语义，据此推理出取一个水杯、找到茶壶、把水倒入杯中这一系列子任务语义网络。语义网络可以不断分层，根据子任务语义把水倒入杯中可以继续推理出把茶壶放到水杯上方、把茶壶倾斜、把茶壶竖直等动作规划语义，最终再根据动作规划语义，推理出轨迹规划语义，完成仿真任务。

根据机器人的智能规划方法，明确机器人的任务规划过程及子任务的顺序。例如机器人对茶壶的抓取是采用示教再现的方式，由仿真系统中提供的室内物品空间位置数据库（桌子、椅子、杯子、茶壶等），通过虚拟示教，记录手动控制虚拟机器人在仿真系统中的一系列空间离散位置，示教整个过程。图8为示教过程的主要视图，位置显示视图实时显示空间物品相对于机器人和世界坐标的位置信息，示教视图可以完成虚拟机器人运动的示教再现控制。当物品到达手臂可达空间区域内，通过手臂轨迹规划完成目标抓取。

图9所示是完成仿真系统的机器人倒水任务作业过程中的几幅画面，其中图9（a）～（d）为智能规划抓取茶壶、抓取杯子、倒水作业、放回茶壶和水杯，整个过程通过智能规划、虚拟示教、虚拟漫游、轨迹规划器完成。

▲图7 机器人智能规划推理及语义网络

▲图8 虚拟示教

▲图9 仿真倒水作业过程

3 结论

本文提出并实现了服务机器人虚拟运动控制与仿真系统的构建方法，建立完整的虚拟仿真控制平台。应用MVC软件架构，形成了一个开放式仿真体系；通过“观察者”设计模式，实现了系统中各个模块的实时通信；分析了机器人的运动学模型，完成了机器人的虚拟示教、轨迹规划；通过引入语义智能规划，完成机器人的虚拟倒水作业等。该平台下的虚拟仿真控制系统具有的意义如下。

（1）采用MVC的软件架构，系统具有良好的开放性和可移植性，能够不断扩展仿真系统的应用领域，例如教学、科研。

（2）该系统具有独立的仿真功能，为机器人智能规划方法和离线编程系统的设计提供了验证平台。

［1］ Wang Y S,Gai Y X,Wu F Y.A Robot Kinematics Simulation System Based on OpenGL ［C］.Robotics,Automation and Mechatronics （RAM）,Qingdao,China:IEEE,2011.

［2］ 杨文通，张第，何煜琛，等．基于Pro/E的仿真分析平台的建立［J］．机械设计与制造，2005（10）：63-64.

［3］ 龚建球，刘守斌．基于ADAMS和MATLAB的自平衡机器人仿真［J］．机电工程，2008，25（2）：8-10,73.

［4］ 樊雅萍,黄生学,温佩芝,等.基于 OpenGL的机器人虚拟漫游系统开发 ［J］.系统仿真学报,2005，17 （10）：2426-2429.

［5］ Zhang Z,Ma S,Li B,et al.Development of an OpenGL Based Multi-robotSimulating Platform ［C］.Control,Automation,Robotics and Vision Conference,Kunming，China：IEEE,2004.

［6］ 张飞，芮延年，乔冬冬，等．基于Pro/E和ADAMS的三轴联动平台设计与运动学仿真 ［J］．组合机床与自动化技术，2012（9）：35-37.

［7］ 祁若龙,周维佳,刘金国,等.VC平台下机器人虚拟运动控制及 3D运动仿真的有效实现方法 ［J］.机器人,2013,35（5）:594-599.

［8］ 甘亚辉，戴先中．一种高效的开放式关节型机器人3D仿真环境构建方法［J］．机器人，2012，34（5）：628-633.

［9］ 李清玲,李爽.基于 OpenGL的三维人体运动仿真 ［J］.计算机仿真,2011,28（4）:270-273.

［10］马壮,周建利,柳延领.基于 VB与 OpenGL的机器人三维运动仿真系统的设计［J］.煤矿机械,2008,29（11）:14-16.

［11］ Ma Y,Dong T,Lan X,et al.Industrial Robot Simulation Based on OpenGL ［C］.Proceedings of the International Conference on Information Engineering and Applications（IEA）,Springer London,2013.