| 编辑推荐: |
本文主要分机械、电子和软件三个方面，详细地讲解一下自己在开发过程中所总结的一些技术要点
，希望对您的学习有所帮助。
本文来自于知乎，由火龙果软件Alice编辑、推荐。 |
|
以下，我从技术介绍和学习流程两个方面来进行讲解。其中技术介绍方面又分为机械、电子和软件三个章节。由于我在项目组里做的是软件开发工作，对机械和电子了解得相对较少，因此机械和电子这两个章节的内容我会简略地介绍，而讲解的重点自然便是软件章节。
技术介绍
机械臂作为机器人最为重要的执行器，赋予了机器人与外部世界环境进行物理交互的能力。这使得机器人的智能不仅仅停留在识别和规划层上，还能通过实际的操作将其真正的表达出来。目前国内外机械臂研究的热点主要集中在机械臂自主精确抓取、放置物体上，而根据本人之前的开发经历，要想让机械臂能完美地执行一系列复杂且精确的操作从而使末端执行器到达预定的位置是很难的，这需要我们理解并掌握很多与机械臂解算相关的数学知识。我相信这会让很多非专业开发者感到头疼，不过借助于第三方开源软件，比如 ROS 和 MoveIT! 机械臂的开发就会相对来说简单很多。下面，我分机械、电子和软件三个方面，详细地讲解一下自己在开发过程中所总结的一些技术要点。最后，你可以在 开发代码 （在ROS Indigo下能成功编译并运行，不支持ROS Kinetic）和 问题汇总 中找到我之前整理的相关资料。
机械
我们机器人的机械臂在机械设计上有一些不一样的地方。相比于其他使用大扭矩舵机或电机作为关节的机械臂，我们使用了由同步轮和同步带所组成的机械结构，这种独特的结构使得我们的机械臂在一定程度上拥有了抓取更重物体的负载能力。除此之外，机械臂末端的手爪也由上一代的对称张开闭合的结构变成了平行夹紧的形式，即两个金属滑块可以通过在滑轨上对称平行的移动。这使得手爪可以适应不同粗细、形状的物体，为抓紧物体提供最基本的保障。当然，这个机械臂也曾经给我造成过一些小困扰，我会在下面的软件部分讲到，通过这个困扰的解决，你就可以理解为什么机械的设计在某种程度上会影响软件代码的编写。
电子
电子部分作为连接机械臂软硬件的重要组成部分，主要的任务是负责将软件组通过RS-485传过来的数据进行解析，并且以最快的速度传递给接有驱动盒的电机来实时地控制机械臂。同样的，通过电机编码器返回的数据可以用来记录每个机械臂关节的实时状态，经过一定的数学解算，就可以将其作为运动规划层的输入，为复杂运动的规划提供基本的保证。机械臂控制使用的是位置闭环算法，具体这方面我了解的并不是很多，不过之前用起来还是很稳定的。
软件
晓萌机械臂软件架构图
从软件架构图中可以很清楚的看到，机械臂的软件层主要由三个部分组成，从下到上依次为：硬件接口层、运动规划层和任务决策层。
硬件接口层
机械臂硬件接口层的设计理念来源于 ROS Control 。ROS Control是ROS提供的软件与硬件之间进行数据通信的中间件，它对硬件进行了抽象，统一了数据通信的接口，并通过插件的形式封装了一些常用的运动控制算法，为建立机器人软硬件模块之间的数据通路提供了便捷。
ROS Control提供的硬件抽象层主要负责管理机器人的硬件资源，而控制器从抽象层请求资源即可，并不直接接触硬件。这提高了代码的复用率和可移植性。
首先，让我们先看一下ROS Control官方提供的数据流图是什么样子的：
ROS Control数据流程图
细心的读者可能会发现这两个架构图在硬件接口层部分有一些不一样的地方。接下来我就讲解一下硬件接口层部分各子模块的功能，并解释彼此不同的原因。
实体机械臂：
这一部分指的就是真实客观存在的机械臂。STM32嵌入式控制器使用位置PID闭环算法来计算由硬件抽象层通过串口通信方式发过来的关节数据，并将计算好的数据直接发送给电机对其进行控制。同时，电机的 编码器 也将电机实时的位置数据经串口通信返回给上面的硬件抽象层。
硬件抽象层：
硬件抽象层和硬件直接打交道，通过 write 和 read 方法来完成对硬件的相关操作。硬件抽象层跟上面官方提供（红色的部分）的有一些不太一样的地方在于我并没有使用ROS Control提供的 Transmissions （数据转换）和 Joint Limits (关节限位）的API。原因的话，我在下面会讲到。这首先简要的介绍一下什么是Transmissions和Joint Limits。
Transmissions：
Transmissions就是机器人每个运动关节和电机之间的数学映射。因为机械臂关节结构的不同，会导致机器人上层规划所使用的Joint与Actuator数据之间存在明显的偏差。比如说有简单齿轮和同步带驱动的，有锥齿轮差动机构，四连杆机构等。Transmissions提供的接口中包含有解决上面这些结构进行数据转换的映射公式。
Joint Limits：
Joint Limits主要是维护了一个关节限位的数据结构，里面可以包含的数据种类不仅仅是常用的关节位置、速度、力矩、加速度等方面的限位，还可以储存具有安全作用的位置软限位、 位置边界 和 速度边界 等。
至于我为什么没有使用以上两个模块的原因，主要是参考了 西工大一小学生 曾经在 Exbot 上发表的有关ROS Control的 文章 。下面就截取其中的一小部分：
以上两个模块是因为URDF中有相应的标签，写了一堆可以直接Load的，但是实际用处并不是很大。它的设计思想是想在URDF中表示更多的信息，这些信息在Gazebo中可以给出更多的细节。但是解析URDF的程序使得RobotHW体量很大，而且这些细节会因为机器人本体通讯暴露给ROS的细节量而不尽相同，而且还会显著提高编程复杂度，所以这些信息显得冗余，而应用这些信息的库也就显得冗余。
之前，我是有尝试过在RobotHW中加载URDF中的相关标签，不过用起来确实就如同 小学生 所说的那样，比较麻烦，而且还很冗余。因此，我根据机械组队员提供的有关机械臂关节电机转换的数学公式封装了相应的函数，至于Joint Limits，我是在规划层的地方进行了指定。
控制器管理器：
控制器管理器提供一种通用的接口来管理不同的 ROS Controllers ，它可以加载、开始运行、停止运行、卸载不同的Controller，并且提供了多种工具来完成这些操作。 Controller Manager 的输入就是ROS上层应用的输出。在这里面，我用到了Joint Command Controller和Joint State Controller，它们分别可以完成对每个关节的控制以及读取硬件接口中关节的状态。
好的。前面讲了很多新的概念，这里我们还是找个案例来具体的分析一下。这里，以我之前整理的 源码 作为参考，分析一下机械臂分别在 Gazebo 仿真和物理物理环境中是如何体现上面那些概念的。
首先看一张来自Gazebo官网的ROS Control架构图：
ROS Control架构图
从图中可以看到， Simulation 和 Hardware 之上的Hardware Resource和Controller Manager是一样的，这很清晰地体现了ROS Control的底层无关性，即无论使用的是抽象的仿真还是具体的硬件，只要程序能继承RobotHW硬件抽象层的基类来做到数据接口的统一，Controller Manager就可以对相应的资源进行管理。
对于 Simulation 和 Hardware 来说，它们内部架构相似，但配置以及使用方式是不一样的。 Simulation 的RobotHW部分，Gazebo官方已经将其实现，并且提供了相应的ROS Control插件来从机械臂的URDF文件中载入所需的数据。用户只需写好URDF和YAML文件，并使用ROS Launch将其整合到一起就万事大吉了。
而对于 Hardware 这部分来说，除了上面说到的配置之外，我们还需要自己编写C++代码来继承RobotHW基类，并在里面分别使用命令和状态硬件接口句柄对相应的关节数据进行 注册 ，然后再将不同的硬件接口注册到RobotHW上。最后，我们还要自己编写函数完成对关节和电机数据的相互转换，并且根据指定的通信协议，实现 read 和 write 函数。
仿真部分：
在终端中输入以下命令启动Motion Control测试。
| $> roslaunch xm_arm_bringup xm_arm_bringup_gazebo_joint_control.launch $> rosrun xm_arm_teleop xm_arm_teleop_position_keyboard |
你可以使用键盘上的按键来控制机械臂每个关节的移动位置。
机械臂运动控制测试
硬件部分：
（1）创建子类继承RobotHW父类，并且声明了一些函数和变量。
（2）初始化关节和电机数据，并使用Hardware Interface对相应的数据进行注册，最后初始化与串口通信有关的Topic。
（3）根据机械臂公式，实现关节和电机数据之间的互相转换。
（4）实现定制的read和write函数。
（5）加载关节名字到ROS的参数服务器中。
以上就是硬件接口层的全部内容了。作为整个机械臂软件架构最底层的部分，它的重要性不言而喻。根据我之前开发机械臂的经验，只有编写出稳定且鲁棒的的硬件接口层，才能为之上的运动规划提供强有力的保证。否则，等到机械臂出现暴走失控的情况的时候就麻烦了（我之前就曾入过这样的坑！）。
运动规划层
运动规划层在机械臂的自主抓取中扮演了非常重要的角色。而对于运动规划本身来说，里面涉及了非常多的专业知识，比如运动学正逆解算、碰撞检测算法、3D环境感知、动作规划算法等，以上任何一个方面都需要我们长时间的积累才能理解清楚，而对于那些想立马上手机械臂的初学者来说，这简直就是一个灾难。
而幸运的是，ROS提供了强大且易用的 MoveIt! 包，它可以让你在较短的时间内实现仿真乃至实体机械臂的运动学规划演示。
首先，简要地介绍一下什么是MoveIt!。以下是MoveIt官网给出的定义：
| MoveIt! is state of the art software for mobile manipulation, incorporating the latest advances in motion planning, manipulation, 3D perception, kinematics, control and navigation. It provides an easy-to-use platform for developing advanced robotics applications, evaluating new robot designs and building integrated robotics products for industrial, commercial, R&D and other domains. |
概括来说，MoveIt!是ROS中与移动操作相关的组件包的集合。它包含了运动规划、操作控制、3D感知，运动学，碰撞检测等。当然，最重要的是MoveIt!提供了非常友好的配置和调试界面。
下图是MoveIt的总体框架：
MoveIt核心架构图
这张图我在学习MoveIt!的时候看过很多遍，理解这个架构图对于学习MoveIt!非常重要。从图中可以看到， move_group 是MoveIt!最核心的部分。它将其他独立的组件集成到一起，为使用者提供了一系列可以使用的命令和服务。
用户接口：
用户可以使用C++、Python或者GUI来访问 move_group 。一般对于初学者来说，GUI和Python的使用会更多一些。
配置：
move_group 本质上还是一个ROS的节点，它需要使用ROS的参数服务器来获取以下三种信息。
URDF：
move_group 需要机械臂的URDF文件来进行运动规划。
SRDF：
move_group 在启动时会寻找机械臂的SRDF文件，它可以通过使用MoveIt! Setup Assistant自动生成。
MoveIt!配置：
move_group 在启动时会加载机械臂的关节限位、动力学、运动规划、感知以及其他相关信息。所有以上的配置信息都可以通过使用MoveIt! Setup Assistant自动生成。
机器人接口：
move_group 使用ROS中的Topic和Action两种机制来与机械臂进行数据通信。它可以获取当前机械臂的位置信息，点云数据以及其他传感器数据，并且发送命令给机械臂的Controller。
关节状态信息：
move_group 会监听机械臂的/joint_states主题来获取当前的状态信息。注意： move_group 只管监听，你需要自己给机械臂配置好 Joint State Controller 。
坐标转换信息：
move_group 可以订阅机械臂的TF主题来确定机械臂内部各关节之间的位置变换关系。跟上面一样，你需要自己运行 Robot State Publier 节点来发布坐标转换。
控制器接口：
move_group 使用 Follow Joint Trajectory 类型的Action接口来与Controller进行数据通信。 move_group 自己是不带Action接口的，它是使用了一个特殊的插件来发布上述Follow Joint Trajectory类型的Action，而对于机械臂来说，你依然需要自己配置上述类型的Controller来订阅机械臂的数据。
规划场景：
Planning Scene指的是机械臂本身以及其周围环境的表示。
扩展能力：
move_group 的所有组件都是以独立插件的形式实现的，而且这些插件可以通过使用ROS的参数文件或插件库来进行配置，这使得 move_group 拥有了强大的定制以及可扩展能力。
接下来，我们介绍一下 Motion Planning 。
这里我引用古月居前辈对运动规划的解释：
假设我们已知机器人的初始姿态和目标姿态，以及机器人和环境的模型参数，那么我们就可以通过一定的算法，在躲避环境障碍物和放置自身碰撞的同时，找到一条到达目标姿态的较优路径，这种算法就称为机器人的运动规划。机器人和环境的模型静态参数由URDF文件提供，在默写场景下还需要加入3D摄像头、激光雷达来动态检测环境变化，避免与动态障碍物发生碰撞。
在MoveIt!中，运动规划算法是由运动规划器算出来的。当然，运动规划算法有很多，每一个运动规划器都是MoveIt的一个插件，可以根据需求选用不同的规划算法。MoveIt!默认使用的是 OMPL 。OMPL(Open Motion Planning Library)是开源运动规划库的简写，它提供基于随机化的运动规划器。
Motion Planner执行流程图
运动规划请求：
在让运动规划器进行运动规划之前，我们要先发送一个运动规划的请求。这个请求可以是新的机械臂或末端执行器的位置。为了让运动规划器规划出来的轨迹符合要求，我们需要指定一些约束条件：
位置约束：
约束机械臂Link的位置。
方向约束：
约束机械臂Link的方向。
可见性约束：
约束Link上的某点在某些区域的可见性。
关节约束：
约束Joint的运动范围。
自定义约束:
使用自定义的回调函数来指定约束条件。
运动规划结果：
move_group 节点最终将会根据上面的运动规划请求，生成一条运动轨迹。这条轨迹可以使机械臂移动到预想的目标位置。请注意： move_group 输出的是一条轨迹，而不是路径。对于机械臂来说，路径是使末端执行器移动到目标位置的过程中，中间所经历的一系列独立的位置点。而轨迹则是在路径的基础上，通过加入速度、加速度约束以及时间参数来使机械臂运动的更加平滑。
规划请求适配器：
在运动规划器的输入输出端分别有两个规划请求适配器。它们的作用分别是对规划请求和规划结果进行预处理和后期处理。MoveIt!提供了几种默认的 适配器 来完成一些特定的功能。
FixStartStateBounds:
当机械臂的一个或多个关节的初始状态 稍微 超出了URDF文件中所定义的Joint Limits后，为了能让运动规划器可以运行，FixStartStateBounds适配器会通过将关节状态移动到Joint Limits处来解决这个问题。不过，如果机械臂关节的偏差很大的话，这种靠软件方式修正的方式就不适用了。
FixWorkspaceBounds:
这个适配器会默认地生成一个10x10x10立方米的机械臂规划空间。
FixStartStateCollision:
如果已有的关节配置文件会导致碰撞，这个适配器可以采样新的配置文件，并根据摇摆因子来修改已有的配置文件，从而保证新的机械臂不会发生碰撞。
FixStartStatePathConstraints:
如果机械臂的初始姿态不满足路径约束，这个适配器可以找到附近满足约束的姿态作为机械臂的初始姿态。
AddTimeParameterization:
这个适配器非常重要。它把从运动规划器中输出的空间路径按等距离进行划分，并在其中添加加速度、加速度约束，以及 时间戳 等必要信息。
Planning Scene
Planning Scene架构图
Planning Scene用来表示机械臂周围的外部世界并且保存机械臂自己本身的状态。它通过监听对应的Topic来获取关节状态信息、传感器信息。并可以根据传感器信息和用户的输入，生成机器人周围3D世界空间的表示。
3D Perception
3D Perception架构图
简单来说，3D Perception使用插件来获取点云和深度图像数据，并据此生成 OctoMap ，为之后机械臂的碰撞检测提供基础。
Kinematics
运动学算法是机械臂各种算法中的核心，尤其是反向运动学算法IK(Inverse Kinematics)。MoveIt!使用插件的形式可以让用户灵活的选择需要使用的反向运动学算法，也可以选择自己的算法。
Collision Checking
MoveIt!使用CollisionWorld对象进行碰撞检测，采用 FCL (Flexible Collision Library)功能包。碰撞检测是运动规划中最耗时的运算，往往会占用90%左右的时间，为了减少计算量，可以通过设置ACM(Allowed Collision Matrix)来进行优化。
好的，讲了这么多抽象的概念，就让我们像上一节讲ROS Control一样，用具体的例子来实践一下。
首先，你需要机械臂的URDF文件，而且保证里面所包含的Link(连杆)、Joint(关节)、运动学参数、动力学参数、可视化和碰撞模型没有问题。这里要注意一下，通过 SolidWorks插件 导出的URDF文件，它默认使用的碰撞检测模型和可视化模型是一样的。为了提高运动规划的执行速度，你可以使用 MeshLab 来简化模型（.stl或.dae零件）的点和面。
之后，运行下面命令来启动MoveIt! Setup Assistant。
| $> rosrun moveit_setup_assistant moveit_setup_assistant |
MoveIt! Setup Assistant启动界面
之后你可以根据 MoveIt! Setaup Assistant官网教程 完成机械臂的配置。虽然这里我没有详细讲解配置的每一步（其实是我忘了截图），但这一步是非常重要的。我之前就配置过很多次，但总有问题。所以说配置机械臂MoveIt!参数是需要一定经验的。这里，我主要讲两个我在配置过程中遇到的问题，希望对你有所帮助。
交互式Marker没有在末端执行器上生成：
这个问题曾经困扰了我很久，后来我在Google上搜索了一段时间，终于找到问题的原因和解决办法。出现这个问题的原因是我在配置末端执行器的时候， parent_link 没有选择 arm 组中的link，而是选了 gripper 组中的。因此，解决办法就是选择 arm 组中的最顶端的link填入到 parent_link 中就没问题了。
MoveIt!根据点云数据生成的OctoMap在Rviz中的位置、方向与实际不符：
这个问题的原因，你可以通过可视化Rviz中的TF插件来看到。每个Link都有自己的XYZ方向，如果你机器人的Camera Link的XYZ方向恰好与Rviz所使用的XYZ方向不符，就会出现上述问题。我的解决办法是在URDF中再添加一个或两个虚拟的Link来修正方向上的偏差。
接下来，让我们运行两个例子来测试一下MoveIt!。
首先，我们测试一下MoveIt!的Motion Planning。请在终端中输入下列命令：
| $> roslaunch xm_arm_bringup xm_arm_bringup_moveit_and_gazebo.launch |
在Rviz中，你可以使用末端执行器上的交互式Marker来移动机械臂到目标位置。接着，你可以在Planning Library的下拉式菜单中选择OMPL库中的某个特定规划算法。然后，点击Plan按钮，Rviz窗口中就会出现一条从初始位置到目标位置的运动轨迹并循环不断地播放。最后，点击Execute按钮，MoveIt!会将上一步规划出来的机械臂关节轨迹通过FollowJointTrajectoryAction接口发送给Gazebo中对应类型的Controller，使得Gazebo中的机械臂可以移动到目标位置，以下是测试的截图。注意：不同的规划算法所用的时间是不一样的。请尝试每一种算法，并记住最优算法的名字，这是为了之后方便在代码中对其进行调用做准备。
机械臂运动规划测试（MoveIt!界面）
机械臂运动规划测试（Gazebo界面）
第二个例子，我们来测试一下带有Avoid Collision的Motion Planning。同样的，请在终端中输入下列命令：
| $> roslaunch xm_arm_bringup xm_arm_bringup_moveit_and_gazebo.launch |
因为我在Gazebo中给机器人的头部添加了 深度传感器 的插件，所以当你把桌子放到机器人前方的时候，MoveIt!可以立马从点云Topic中获取物体的信息，并在Rviz中生成可视化的OctoMap。在下一次做运动规划的时候，MoveIt!会将由 正方体 组成的OctoMap看成障碍物并考虑在内。图中，机械臂的初始位置为伸直形态，我将其从桌子的下方移动到了桌子的正上方，规划的效果如下图所示。
带碰撞检测的机械臂运动规划测试（MoveIt!界面）
带碰撞检测的机械臂运动规划测试（Gazebo界面）
当然，在使用MoveIt!对机械臂进行运动规划的时候并不是每一次都能成功，有些时候会出现超时报错的情况。遇到这种问题的时候，你可以尝试尝试其他OMPL算法，因为不同的OMPL算法可能对不同的情况有各自的优化。
最后，由于时间的缘故，我没能将MoveIt!的C++和Python的使用代码给整理出来，这对于我来说非常遗憾。不过，我相信随着MoveIt!学习教程的越来越丰富，有关如何用代码来做MoveIt!的运动规划会更加容易。
任务决策层
任务决策层处于整个架构图最顶端，是控制整个机械臂的大脑所在。首先，我要阐明的一点是：这一部分在我那一届机械臂软件代码中并没有实现，这是我后来总结机械臂开发经验的时候重新设计的。
至于我为什么要选择重新设计新的任务决策层，原因其实很明显，就是我们之前的那个存在着很多的不足的地方。我们之前的那个任务决策层架构比较松散，特别是机械臂的Action接口，我们定义了好几种，但是其中有几种的功能比较相近，显得比较冗余。而且 状态机 可以直接发送Action的Goal到机械臂模块中，换句话说就是没有经过封装的数据是一直暴露在整个任务决策的各个时间段，这会影响数据通信的稳定性，而且这样的代码也是不容易维护和重构的。
当然，除了上面存在的历史问题，还有一个影响我做出改进的因素就是：中科大蓝鹰队可佳服务机器人早期的 抓取视频 。视频中，可佳机器人的机械臂可以非常精准地操作微波炉，并抓取桌子上三种不同的容器。其中最难的是那个盛有牛奶的碗，整个碗只有一个地方向外伸出了一个手柄，能让机械臂末端手爪精准地抓住碗并且在移动其的过程中不让碗中的牛奶洒出来，这是非常厉害的。当时看完视频之后，我就被深深地震撼了，原来机械臂的自主抓取可以做到这样的程度！后来， 西工大一小学生 跟我们讲：
这个视频里最厉害的还不是机械臂的抓取，而是可佳机器人的任务决策部分。可佳可以通过对人语义的理解来自动生成相应的任务序列，而且这个任务序列是能被实时的修改和更新的。
因此，我决定对之前的机械臂任务决策模块进行封装，把所有与机械臂相关的软件细节都隐藏起来，最后暴露给外面的只有数据和任务接口。这样可以大大降低模块与模块之间的耦合性，并减少了不必要的进程通信开销，提高了程序运行时的效率。下面，我简要地讲解一下这个部分的原理。
任务决策层的核心简单来说是在其内部定义了一个小型的状态机，它可以根据不同的任务类型、物体位置以及物体的类型来选择不同的数据发给下面的运动规划层。这里我举一个具体的例子来说明其工作的整个流程：首先，机器人决策模块给机械臂的任务决策层发送了一个 抓取 的状态，任务决策层接收到之后就会在自己事先存储好的状态表中进行查找，如果匹配 抓取 状态成功，就把表中的状态链取出并放到状态队列中去。每次状态控制器会根据当前状态队列中的子动作来分析其所需要的数据。比如说， 抓取 状态可以拆分成很多子动作：初始、准备、抓取、手爪张开、手爪夹紧、手握物体等。此时，如果队列中第一个动作是初始，那控制器便会从预先设定好的机械臂位置池中取出相应的位置，并从MoveIt!参数表中取出其所需要的数据，最后通过MoveIt!接口把初始动作发送给运动规划层进行规范和执行。当机械臂完成这个动作后，任务决策层会比较机械臂实际运动的位置和预想位置之间的差值，如果误差小于某个值，其便会返回执行成果给控制器，控制器则会继续地执行下一个状态，直到整个状态队列中的动作都被执行完。如果误差过大，则报错退出，以防止机械臂出现任何不可控的意外情况。
最后，鉴于这一部分只停留在我的设想阶段，目前只供参考。至于最终能不能实现出来，还有待日后的验证。
| 
