基于FD on Desk 那智机器人在线控制及运动轨迹仿真

2022-10-31文旭仲梁维

农业装备与车辆工程 2022年4期

文旭，仲梁维

（200093 上海上海理工大学机械工程学院）

0 引言

近几年，我国在机械工程、计算机技术、自动控制以及人工智能等领域快速发展，机器人领域也越来越受关注，工业机器人在制造业扮演着重要角色。目前，主流的机器人有瑞典的ABB、德国的KUKA 以及日本的FANUC 和那智不二越机器人。我国在工业机器人的应用上还处于较低层面，主要是离线示教编程方式，内容繁琐复杂，精度低。传统的人工示教效率低、不科学。随着计算机与通信技术的快速发展，离线编程成为当前工业机器人仿真的主要方式。主流的机器人都有专门的离线编程软件，比如ABB 的RobotStudio、库卡的KUKA SIM PRO 以及那智不二越的FD on Desk，缺点是简单的示教仿真。目前在线编程以及运用算法进行路径规划仍不成熟。通过MATLAB 中的Robotics Toolbox 以及机器人工具箱对机器人仿真，在仿真精度、结果、与实际机器人的匹配都存在相当误差。

本文在VisualStudio 平台利用C#编程语言，以Socket 的C/S(客户端/服务端)结构以及那智编程语言为基础，在FD on Desk 上建立机器人及工作空间模型，实现机器人末端轨迹在线运动仿真。

1 Socket 通信及控制系统的搭建

1.1 基于以太网的数据传输

那智不二越机器人本体系统与外部设备（PC、外部传感器）的数据通过以太网传输，支持TCP以及UDP 协议。UDP 传输数据，在数据的传输安全与可靠性方面存在不足，数据很容易丢失，本文采用TCP/IP 通讯方式实现数据通讯，保证数据传输的可靠性[1]。将PC 端（机器人上位机）作为客户端，那智机器人系统作为服务端，通过在那智机器人系统的KLI 接口和电脑的以太网接口，以XML 文件格式，实现PC 端对那智机器人的远程在线控制。

1.2 服务端程序

本文以机器人控制系统作为服务端，根据那智机器人系统提供的编程语言，以用户程序后端运行的方式实现数据的接收与发送，服务端程序的连接部分主要包括IP 地址与端口号。一个完整的网络通信需要的是一对socket,，一个在服务端运行，一个在客户端运行[2]。服务端程序是被动监听状态，监听是否有客户端连接并对相应的信息执行相应反馈响应。

那智机器人提供的很多系统函数，通过系统函数的调用，直接可以获取那智机器人的实时状态，比如机器人各轴的角度、笛卡尔坐标系下的位置、各轴的力矩情况[3]。部分代码程序如下。

1.3 客户端程序

电脑端作为客户端，程序在VisualStudio 2017 平台上用C#编程语言编写，通过C#中的IPAddress和IPEndPoint 获取对方的IP 与端口号，并通过Connect 方法实现连接，部分代码如下：

2 机器人运动仿真系统的建立

机器人仿真系统主要包括2 个模块：物理仿真模块和运动轨迹规划模块。在FD on Desk 仿真软件中建立相应的物理模型，并通过RRT 算法为机器人从起始点到终点规划出路径，并将路径坐标存到SQL 数据库[4]，在VisualStudio 平台，通过套接字通信的方式，实现对机器人的在线控制及路径规划功能。

FD on Desk 是那智机器人公司开发的一款专门为那智机器人仿真的离线仿真软件（如图1 所示），其中包括虚拟手柄（Visual TP）、虚拟控制器面板（Visual FD）、虚拟IO(Virtual IO)以及三维模型模块（Virtual ROBOT）。Virtual ROBOT 模块具有三维物理建模的功能，同时也支持外部导入模型的方式。

图1 FD on Desk 软件Fig.1 FD on Desk software

2.1 机器人及相关模型的建立

本文采用的机器人模型为那智MZ0701-01 型六自由度机器人。通过调用那智机器人内置MOVE函数控制机器人的移动，并且调用它的系统函数（SYSTEM 函数），可以直接获取机器人姿态，绕开了通过dh 参数以及逆解矩阵的方式来获取机器人的姿态，规避了dh 及逆解带来的仿真误差。如图2 所示，机器人从起点抓取目标，通过RRT 算法绕开障碍物的到达终点。

图2 工作空间模型Fig.2 Workspace model

2.2 碰撞检测

目前广泛应用的碰撞检测方法为层次包络盒法，主要包括OBB、包围球以及AABB 等。本文用到的机器人为六自由度机械臂，采用的是长方体包络法。为了仿真结果最优化，将机器人基座、一轴和二轴作为一个圆柱体包络，三轴、四轴以及末端分别用3 个圆柱体包络[5]，那么机械臂与障碍物之间的碰撞被转换成三维空间下圆柱体间的干涉问题。包络模型如图3 所示。

图3 碰撞模型Fig.3 Collision model

2 个圆柱体发生碰撞有3 种碰撞形式：（1）圆柱两端面之间发生碰撞；（2）端面与圆柱的侧面发生碰撞；（3）侧面与端面发生碰撞。如图4 所示。

图4 碰撞形式Fig.4 Collision form

按照碰撞的不同形式，可以依次按照圆柱中心距检测、检测端面圆相交、检测侧面相交的顺序进行判断。若排除3 种碰撞情况，则可以认为这2个物体不发生碰撞[6]。

2.3 机器人路径规划

2.31 RRT 算法

RRT 算法又叫随机快速扩展随机数算法，如图5 所示。

图5 原始RRT 模型Fig.5 Original RRT model

主要思路如下：（1）将抓取物的坐标作为起始节点Xinit，并以此节点作为根节点，产生随机树；（2）设定一个概率P值引导随机数搜索方向，在此方向上随机产生一个采样点Xrand作为随机数实际扩展方向；（3）在产生的2 个节点中选取离Xrand最近的节点Xnearest；（3）通过扩展函数Extend（Xnearest，Xrand），以某一步长沿XnearestXrand方向产生一个新的节点Xnew，如果在随机扩展中检测到发生碰撞，则舍去此节点，重新回到（2）重复执行[7]。

原始的RRT 算法在搜索的方向存在一定的盲目性，导致实际规划的路径不够理想，除了设定一个搜索方向概率P对路径进行优化，同时可以采用RRT*对规划的路径再次优化。RRT*与RRT 算法的不同在于，在人为设定的某个区域内，在区域内的节点重新为Xnew选择父节点，重布线随机树[8]。如图6 所示。

图6 改进后的RRT 模型Fig.6 Improved RRT model

图6 中的圆圈部分表示节点，两节点之间的数字表示路径的长度，对椭圆包围的区域内进行路径优化[9]。从图6（a）得知，原始路径为0-4-9-6，路径长度为16。重新为Xnew寻找父节点，分别以4、5、8 节点作为父节点，则路径分别为0-1-5-9，0-4-9，0-1-5-8-9，路径总长度为14，11，12。选取长度最小的节点5 作为新的父节点，优化后的路径为图图6（b）[10]。

伪代码如下[11]：