遥操作机器人系统软件框架

第 2 章主从异构型遥操作机器人实验平台搭建

2.2 主从异构型遥操作机器人的软件平台

2.2.3 遥操作机器人系统软件框架

基于机器人操作系统 ROS 和 MATLAB SimuLink 软件搭建主从异构型遥操作机器人系统软件框架，如图 2-11 所示，设计的 ROS 系统中包括 teltouch、

Simulink 上层控制器、Simulink 下层控制器、communication.py 和 Baxter 机器人五个节点。这些节点将消息发送到特定话题中，同时通过订阅特定话题来获得所需的消息。具体来讲：

图 2-11 遥操作系统软件框架简图

（1）teltouch 节点通过 OpenHaptics 工具包中的用户 API 与 Geomagic Touch 设备底层程序进行交互，主要实现以下功能：1）获取 touch 设备的状态信息，对关节角度进行处理，对末端位姿矩阵进行坐标转化，编写两个按键的功能，将修改后的设备状态信息以

TouchState

的消息格式发送到

/robot/teltouch/left/touchstate 的话题中；2）订阅/robot/left/matlabtorque 话题，

收到格式为 geometry_msgs/Point 的消息，设定 Touch 设备的力矩反馈值；3）订 阅/robot/left/matlabforce 话题，收到格式为 geometry_msgs/Point 的消息，设定 Touch 设备的末端力反馈值。具体如图 2-12 所示。

图 2-12 teltouch 节点功能结构

（2）Simulink 上层控制器节点主要运行工作空间映射算法和计算模型补偿 值，具体实现以下功能：1）订阅/robot/teltouch/left/touchstate 话题，收到格式 为 TouchState 的消息，订阅/robot/limb/left/arm_state 话题，收到格式为

BaxterPos 的消息，根据这两个消息运行工作空间映射算法，计算期望位置，以 Stand7dof 格式发送到/robot/limb/left/d_joints 话题中；2）订阅/robot/tel-touch/left/touchstate 话题，收到格式为 TouchState 的消息，从而设定手爪的运

动，以 std_msgs/Float64 格式发送到/robot/limb/left/GripperCmd 话题中；3）根 据算出的期望位置、速度和加速度计算模型补偿值，以 InverseCommand 格式发 送到/robot/limb/left/MatlabInverseJntCmd 话题中；4）订阅

/robot/teltouch/left/touchstate 话题，收到格式为 TouchState 的消息，订阅/ro-bot/limb/left/arm_state 话题，收到格式为 BaxterPos 的消息，根据这两个消息计 算主端设备力矩反馈值或者力反馈值，以 geometry_msgs/Point 格式发送到

/robot/left/matlabtorque 或者/robot/left/matlabtorque 话题中；具体如图 2-13 所

示。

图 2-13 Simulink 上层控制器节点功能结构

（3）Simulink 下层控制器主要运行 PID 运动控制算法，实现基于误差的伺服 运动控制。订阅/robot/limb/left/d_joints 话题，收到格式为 Stand7dof 的消息，订 阅/robot/limb/left/arm_state 话题，收到格式为 BaxterPos 的消息，根据这两个消 息运行 PID 运动控制算法，计算控制量，以 MatlabCommand 格式发送到

/robot/limb/left/MatlabJntCmd 话题中，具体如图 2-14 所示。

图 2-14 SImulink 下层控制器功能结构

（4）communication.py 节点主要作用是搭建 Simulink 和 baxter 机器人节点通讯的桥梁，具体功能为：1）由于 Simulink 不能处理字符数据，而 baxter 机器人节点中包含很多字符数据，故需要进行格式转换。该节点通过订阅

/robot/limb/left/MatlabJntCmd 话题，收到格式为 MatlabCommand 的消息，订

阅/robot/limb/left/MatlabInverseJntCmd 话题，收到格式为 InverseCommand 的消 息，将这些消息和字符串数据进行匹配，重新打包，以 JointCommand 的格式发 送到/robot/limb/left/joint_command 话题中；2）调用 Limb 类，通过订阅

/robot/joint_states 话题，收到格式为 JointState 的消息，精简提取所需的关节状

态消息，对消息进行处理，去掉字符串，重新排序，以 BaxterPos 的格式发送到

/robot/limb/left/arm_state 话题中；3）调用 Gripper

类，通过订阅/ro-bot/limb/left/GripperCmd 话题，接受格式为 std_msgs/Float64 的消息，对消息进

行重新打包，转化成格式为 EndEffectorCommand 的消息，发送到

/robot/end_effector/left_gripper/command 话题中，同时通过订阅/robot/end_ef-fector/left_gripper/state 话题，获取格式为 EndEffectorState 的手爪的状态消息，

具体如图 2-15 所示。

图 2-15 communication.py 节点功能结构

（5）Baxter 机器人节点程序主要与 baxter 机器人进行交互，通过订阅

/robot/limb/left/joint_command 话题，接收格式为 JointCommand 的消息，控制

baxter 机器人的手臂 7

个执行器进行相应的运动；通过订阅/robot/end_effec-tor/left_gripper/command 话题，接受格式为 EndEffectorCommand 的消息，控制

baxter 机器人的手爪运动；同时将手臂传感器获得的数据以 JointState 的格式发 送到/robot/joint_states 话题中，将手爪传感器获得的数据以

EndEffectorState 的格式发送到 robot/end_effector/left_gripper/state 话题中，具体

如图 2-16 所示。

图 2-16 Baxter 机器人节点功能结构 2.2.4 遥操作机器人系统程序设计

teltouch 节点和 communication.py 节点主要根据机器人操作系统 ROS 程序包的编写原理，Simulink 上层控制器和 Simulink 下层控制器则直接在 MATLAB Simlink 中搭建框图，Baxter 机器人节点底层程序和 Touch 设备底层程序都有商家提供了相应的用户 API 来调用。

机器人操作系统 ROS 程序包可以通过在 Ubuntu 系统终端输入以下命令生成：

$ catkin_create_pkg package_name std_msgs rospy roscpp

其中 catkin_create_pkg 是命令行，package_name 是定义创建的程序包名字，

std_msgs、rospy、roscpp 是创建的程序包的依赖项。

通过上述命令创建的软件包主要包括 msg 文件夹、src 文件夹、scripts 文件件、CmakeLists.txt 和 package.xml 等，其中 msg 文件夹里面存放定义的消息格式文件；src 文件夹里面存放需要编译的源文件；scripts 文件夹存放可直接运行的解释性语言程序；CMakeLists.txt 是用于编译软件包的 Cmake 编译系统的输入，

它用于描述如何编译代码以及在何处安装它；package.xml 文件，定义程序包的属性，例如包名称，版本号，作者，维护者和对其他 catkin 包的依赖性，具体如图 2-17 所示

图 2-17 ROS 系统程序包的文件系统

根据上述文件系统和相应的遥操作系统软件框架，定义了所需的消息文件，

设计了相应的程序，同时在 Simulink 中搭建了相应的框图。

（1）消息文件的定义：机器人操作系统 ROS 本身定义了许多常用的消息格 式，如 std_msgs/Float64，geometry_msgs/Point，JointState 等，这些消息可以直 接在 ROS 系统中使用，而无需再定义，但遥操作系统中还存在许多需要定义的消息格式，共有 8 种自定义的消息，这里就 TouchState 消息进行阐述，其他的见附录 1。要定义 TouchState 消息，首先要编写相应的消息描述文件 TouchState.msg，

存放在 msg 文件夹中，然后更改 CMakeLists.txt 文件，设置所需要编译的消息，

最后编译程序包，使其可以运用到 ROS 系统中，具体如图 2-18 所示。

图 2-18 定义 TouchState 消息流程图

（2）teltouch 节点：由于 Geomagic Touch 设备提供的 OpenHaptics 是基于 c++程序语言的，为了方便使用其用户 API，针对 teltouch 节点用 C++程序语言实现。编写的 teltouch.cpp 源文件存放在 src 文件夹中，为了使用 OpenHaptics 提供的库函数，需要在 CMakeLists.txt 文件中添加下面几行代码：

…

include_directories(

${catkin_INCLUDE_DIRS}

usr/include )

…

target_link_libraries(teltouch ${catkin_LIBRARIES}

-lHDU -lHD -lrt -lstdc++ -lm )

同时在 teltouch.cpp 源文件中，添加下面几行代码：

…

#include <HD/hd.h>

#include <HDU/hduError.h>

#include <HDU/hduVector.h>

…

完成上面步骤后，便可以调用 OpenHaptics 提供的库函数来获取设备的状态信息，

设定设备的力反馈值等，如下面一行代码来获得 Touch 设备末端的位姿矩阵：

hdGetDoublev(HD_CURRENT_TRANSFORM,transform);

具体 teltouch.cpp 源代码见附录 2。

（3）Simulink 上层控制器：Simulink 上层控制器框图如图 2-19 所示，主要通过机器人系统工具箱来接收 ROS 系统中的消息并发送消息到 ROS 系统中，由于计算量比较大，设定相应的采样频率为 50Hz。

图 2-19 Simulink 上层控制器框图

（4）Simulink 下层控制器：Simulink 下层控制器框图如图 2-20 所示，主要也是通过机器人系统工具箱来接收 ROS 系统中的消息并发送消息到 ROS 系统中，

作为从端设备的伺服控制器，设定相应的采样频率为 500Hz。

图 2-20 Simulink 下层控制器框图

（5）communication.py 节点：由于 Baxter 机器人节点使用 python 程序语言编写，为了更好地与 baxter 机器人进行交互，communication.py 使用 python 程序语言编写，其源代码见附录 3。

2.3 主从异构型遥操作机器人实验平台测试

在搭建完主从异构型遥操作机器人系统的软硬件平台后，需要对这个实验平台进行测试，验证其是否可以正常工作。

ROS 机器人系统分布式的特点允许不同的 PC 使用 TCP/IP 协议接入 ROS 系统中。通过指定相应的 ROS MASTER（节点管理器，提供 ROS 名称服务，比如帮助节点找到彼此）的地址和端口号以及本地程序包运行的 IP 地址就可以实现分布式计算的功能。

在具体的实验平台中，指定了各个 PC 的 IP 地址和 ROS MASTER 的地址，如图 2-21 所示，Simulink 上层控制器和下层控制器运行在 IP 地址为 10.11.111.173 的电脑上； teltouch 节点运行在 IP 地址为 10.11.111.152 的电脑上； communication.py 节点运行在 IP 地址为 10.11.111.151 的电脑上；baxter 机器人节点和 ROS MASTER 运行在 IP 地址为 10.11.111.163 的 baxter 机器人的内置电脑上。

图 2-21 遥操作机器人实验系统 IP 地址分配图

在规定好各自的 IP 地址后，在每台 PC 机上分别测试与其他机器的网络连通性。

在测试完网络连通性后，进行下面的步骤：

1）运行 MATLAB Simulink 程序的电脑上，需要在 MATLAB 程序中输入以下命令：

setenv(‘ROS_MASTER_URI’,’http://10.11.111.163:11311’) setenv(‘ROS_IP’,’10.11.111.173’)

rosinit

如果 MATLAB 运行在 Windows 系统中，可能会出现无法接受来自 ROS 中的消息的问题，此时还需要关闭系统的防火墙，同时，在位于

c:/windows/system32/drivers/etc 下的 hosts 文件中添加下面一行代码：

10.11.111.163 011406P0015.local

将 Baxter 机器人主机名 011406P0015.local 映射到 10.11.111.163 的 IP 上。

2）运行 teltouch 节点和 communication.py 节点的装有 ubuntu 系统的电脑上，需要在终端输入以下命令：

export ROS_MASTER_URI=http://10.11.111.163:11311 export ROS_IP=10.11.111.173(151 152)

如果出现无法接收到来自 ROS 系统中的消息的问题，则很有可能终端以本地回

图 2-22 baxter 机器人 S0 关节跟踪主端设备运动信号

图 2-23 Touch 设备三个力矩反馈值

2.4 本章小结

本章主要对遥操作机器人系统实验平台搭建进行阐述，主要内容包括：

1、搭建主从异构型遥操作机器人系统硬件平台，通过调研相关资料，选择 Geomagic Touch 力反馈设备作为遥操作机器人系统的主端设备，Baxter 机器人作为遥操作机器人系统的从端设备，选择三台PC 机运行相应的程序，利用路由器建立了遥操作系统的小型工作站；

2、建立遥操作机器人系统的软件平台。主要基于机器人操作系统 ROS，设计了相应的程序包，并利用 MATLAB 程序中的机器人操作系统工具包将

Simulink 接入 ROS 系统中，方便工作空间映射算法和运动控制算法的设计，利用 ROS 系统分布式运算的特点，搭建分布式网络，最大化遥操作机器人系统的性能；

3、对搭建的主从异构型遥操作机器人系统实验平台进行测试，通过不断地测试，纠错，验证了遥操作机器人实验平台的物理连接性和软件设计的可行性。

第3章主从异构型遥操作机器人主端和从端设备建模

摘要：本章主要对主从异构型遥操作机器人系统主从设备进行建模。针对主端和从端设备建立运动学模型，为第四章工作空间映射方法的设计提供基础性工作。针对从端设备建立动力学模型，为搭建从端设备运动控制器提供基础性工作。

3.1 主从异构型遥操作机器人主端设备运动学建模

主端设备运动学建模主要为了建立主端设备各连杆间的位移关系、速度关系和加速度关系。主端设备可以看作为一个开式运动链，它由一系列连杆通过转动关节串联而成。为了研究这些连杆之间的关系，可以在每个连杆上固接一个坐标系，然后描述这些坐标系之间的关系。Denavit 和 Hartenberg^[28]提出了一种通用的方法，即 D-H 方法，用一个 4X4 的齐次变换矩阵描述相邻两连杆的关系，从而推导出末端坐标系相对于基坐标系的齐次变换矩阵，建立操纵臂的运动学方程。

对于一个拥有 N 个关节的操作臂来说，它有 N+1 个杆件，编号为 0 到 N，其中，

在文檔中硕士专业学位论文 (頁 39-0)

第 2 章 主从异构型遥操作机器人实验平台搭建