运动规划 (Motion Planning): MoveIt! 与 OMPL

最近有不少人询问有关MoveIt!与OMPL相关的话题，但是大部分问题都集中于XXX功能怎么实现，XXX错误怎么解决。表面上看，解决这些问题的方法就是提供正确的代码，正确的编译方法，正确的运行步骤。 

然而，这种解决方法只能解决这个特定的问题，而且解决之后我们也无法学到一些实际的东西。要想彻底明白，需要从源头入手，也就是说， 不要问“MoveIt! 怎么把机械手从空间一个点移到另一个点？“ ， 而是要问”MoveIt! 为什么能把机械手从空间一个点移到另一个点？“ 。 这一点明白之后，遇到类似的问题，才能从容应对。同理，这不仅适用于MoveIt，也同样适用于其他任何ROS功能。 

所以，下文中我们会见到一些具体的例子，但整体上，更倾向于宏观的概念和一些基础的方法，希望对大家能有所帮助。这里的帮助指的是增强对运动规划和Moveit, OMPL的整体理解，而非局限于完成某一个功能，编译运行某一个文件。 


我尝试用最简单，最通俗的表达方式来解释这些问题，其中不免会有一些学术上的错误用词和解释，请专业的朋友们见谅，也欢迎指出错误。 



一. 基础概念 


首先，我们要了解一些基础的概念，了解各个名词的意义和区别。 


1.1. 运动规划 (Motion Planning)
我们这里讲的 运动规划 ，有别于 轨迹规划 (Path Planning)。一般来说，path planning用于无人车/无人机领域，而motion planning主要用于机械臂，类人机器人领域。当然了，这两者没有本质的区别，理论上说MoveIt!和OMPL同样可以用于无人车无人机的规划，但不免有些杀鸡用牛刀的感觉。两者规划的空间维度不同，导致他们的难易程度不同。举例说明，如果不考虑速度加速度，只考虑位置的话，无人车轨迹规划维度是3 (x,y,和角度), 无人机是6 (x,y,z,和另外3个量确定空间的旋转角度)。确定3D空间的一个姿势(pose)需要6个变量，而对于关节数大于6的机械臂结构，它的规划空间维度就大于6，成为冗余系统(redundant system)，从而使规划问题变得更为复杂。所谓冗余系统，就是说，存在多种关节角度配置能够使得终端达到相同的位姿，存在无数的解。这是达到的最终姿势有无数个解，那么如何到达这个最终姿势，整个运动的轨迹，更是存在无数个解。 


既然存在无数的解，那么问题来了。很明显，存在两种不同的方向，一种是找到最好的那个解，另一种是快速的找到一个有效的解。前者，大部分算法使用最优规划 (Optimization-based Planning)，后者使用采样规划 (Sampling-based Planning)。具体的区别和算法，不在这里赘述。 


1.2. 开源运动规划库 (OMPL).
接上文，而OMPL (Open Motion Planning Library)， 开源运动规划库，就是一个运动规划的C++库，其包含了很多运动规划领域的前沿算法。虽然OMPL里面提到了最优规划，但 总体来说OMPL还是一个采样规划算法库 。而采样规划算法中，最出名的莫过于 Rapidly-exploring Random Trees ( RRT ) 和 Probabilistic Roadmap ( PRM )了, 当然，这两个是比较老的，还有很多 其他 新算法。 

• OMPL能做什么？ 简单说，就是提供一个运动轨迹。给定一个机器人结构(假设有N个关节)，给定一个目标(比如终端移到xyz)，给定一个环境，那么OMPL会提供给你一个轨迹，包含M个数组，每一个数组长度是N，也就是一个完整的关节位置。沿着这个轨迹依次移动关节，就可以最终把终端移到xyz，当然，这个轨迹应当不与环境中的任何障碍发生碰撞。
• 为什么用OMPL？ 运动规划的软件库和算法有很多，而OMPL由于其模块化的设计和稳定的更新，成为最流行的规划软件库之一。很多新算法都在OMPL开发。很多其他软件（包括ROS/MoveIt）都使用OMPL做运动规划。

1.3. 逆运动学 (Inverse Kinematics)

• 什么是逆运动学(IK)？简单说，就是把终端位姿变成关节角度，q=IK(p)。p是终端位姿(xyz)，q是关节角度。
• 为什么要用IK？OMPL是采样算法，也就是要在关节空间采样。 这与无人车的规划有一个最明显的区别，无人车的目标就是在采样空间， e.g. 目标是(x,y), 采样空间也是(x,y). 但是对于机械臂，目标是终端空间位置(xyz), 但采样空间却是关节空间(q0,q1,…qN)。有了IK之后，我们就可以把三维空间的目标p转化为关节空间的目标q。那么这样就会让采样算法能算的更快，具体方法不赘述，这样的算法有RRT-Connect，BKPIECE等等双向采样算法。

1.4. MoveIt!
问：我不想看也看不懂OMPL和各种算法，但是我想让机械臂动起来，怎么办？
答：那这正是MoveIt!的设计初衷。Move It！让它动起来！
OMPL是运动规划的“规划”部分，而MoveIt!是OMPL的ROS接口。当然这不完全准确，OMPL有单独的ROS接口，但依旧很繁杂，而MoveIt是OMPL ROS接口的接口。。。而且MoveIt!还结合了其他一些功能，总之MoveIt!就是个大接口。。 

• MoveIt!能做什么？一句话，MoveIt!就是一个模块化的接口，让你在最短时间内，不用自己写太多代码，就能配置出一个ROS Package来为你的机械臂做运动规划。

二. 创建MoveIt! Package 

2.1 准备URDF package
首先我们要准备一个机械臂的urdf，如果你已有URDF，可以使用自己的urdf模型。若手头没有现成的URDF，可以从 此处 下载一个库卡LWR简化模型URDF，这是一个固定底座7自由度的机械臂。 
从该连接处依次进入examples/sovlers/ik_solver_demo/resources，下载里面的lwr_simplified.urdf。 

复制代码
将下载好的lwr_simplified.urdf放入urdf文件夹中，这样一个urdf package便创建好了。 





2.2 MoveIt!配置助手 (MoveIt! Setup Assistant)

2.2.1 打开MoveIt! Setup Assistant





复制代码

MoveIt! Setup Assistant 是一个图形界面，可以让我们不用写代码看代码，直接用鼠标点击就可以配置机器人的运动规划所需要的信息。点击Create New MoveIt Configuration Package来创建新的配置包，选择刚刚下载的urdf，然后点击Load Files 载入文件。 



2.2.2 创建碰撞免检矩阵(ACM) 



点击Setup Assisant的左边第二项’Self-Collisions’，在这里我们将创建碰撞免检矩阵(Avoid Collision Matrix, ACM)。再次强调，怎么创建很简单，点击一下’Regenerate Default Collision Matrix’就可以了，问题是，为什么？ACM是做什么的？ 

我们知道，碰撞检测是非常复杂的运算过程。对于多关节机械臂或者类人机器人来说，机械结构复杂，肢体多，碰撞检测需要涉及很多的空间几何计算。但是对于刚体机器人来说，有些肢体之间是不可能发生碰撞的，比如原本就相邻的肢体，比如类人机器人的脚和头。这里生成的ACM就是告诉我们，这个URDF所描述的机器人，哪些肢体之间是不会发生碰撞的。那么在之后的碰撞检测算法中，我们就可以略过对这些肢体之间的检测，以提高检测效率。 





2.2.3 创建虚拟关节 (Virtual Joints) 

在Setup Assistant 第三项Virtual Joints里面，我们要创建所谓的虚拟关节。这个虚拟关节，可以理解为一个连接机器人和世界的关节。

2.2.5 创建机器人预设位姿 (Robot Poses)
2.2.6 配置终端控制器(End Effectors)
2.2.7 配置被动关节(Passive Joints)
所谓被动关节，就是指现实中不配置电机的关节，也就是不会出现在机器人的Joint State Msg里，以避免MoveIt与JointState出现匹配错误。这里我们的LWR机械臂并没有此类被动关节，所以可以直接跳过。

2.2.8 生成配置文件(Configuration Files)
最后一步，在Configuration Package Save Path里面选择一个保存地址，一般我们把他放在path_to_catkin_ws/src/lwr_moveit_config 然后点击Generate Package，这样一个完整的MoveIt Configuration Package就创建好了！ 先不要急着运行，我们先来看看都生成了哪些东西，还有一些重要的配置参数都是在哪定义的。

三. MoveIt 配置包详解
打开刚刚创建好的lwr_moveit_config文件夹，我们发现有config和launch两个文件夹。 3.1 MoveIt! 配置文件 先看config，里面有

• fake_controllers.yaml：这是虚拟控制器配置文件，方便我们在没有实体机器人，甚至没有任何模拟器（如gazebo）开启的情况下也能运行MoveIt。
• joint_limits.yaml：这里记录了机器人各个关节的位置速度加速度的极限，这些都会被用于以后的规划中。
• kinematics.yaml：这里就是上一章2.2.4里面设置的东西，用于初始化运动学求解库
• lwr.srdf：这个是一个重要的MoveIt配置文件，我们将在下一节详解。
• ompl_planning.yaml：这里是配置OMPL各种算法的各种参数。

3.2 SRDF文件
SRDF是moveit的配置文件，配合URDF使用。打开lwr.srdf，

下面，我们看看launch文件夹，一打开发现有很多文件，瞬间不想看了。。不要急，我们来看看几个重要的文件。 





3.3.1 demo.launch 

demo是运行的总结点，打开我们可以看到他include了其他的launch文件。其中第14行说，如果有需要，发布静态的tf。比如说，你的机器人基座不在世界坐标的原点，你可以发布一个静态tf来描述机器人在世界坐标中的位置。第17-21行，就是我们发布虚拟机器人状态的地方了，当然，如果你有实体机器人或者有gazebo之类的模拟器，你需要去掉这一部分，有其他相应的节点来发布机器人状态。26-32行运行了另一个moveit重要的节点，move
group。 





3.3.2 move_group.launch 

顾名思义，move group的功能是让一个规划组群动起来。怎么动，那就要做运动规划了，在move_group.launch第24-26行定义了运动规划库的使用，我们可以看到，默认的是使用ompl运动规划库。同样的，如果以后有时间，我会发帖详解如何创建新的运动规划库插件并让moveit使用其他的运动规划算法。其他的都是设置一些基本参数，暂时可以略过。 





3.3.3 planning_context.launch 

这里我们可以看到，定义了所使用的urdf和srdf文件，以及运动学求解库。不建议手动更改这些，但是如果你需要使用不同的urdf，srdf，可以在这里更改。 





3.3.4 setup_assistant.launch 

如果你需要更改一些配置，那么可以直接运行 



复制代码

这样就可以基于当前设置做更改，而不是重新设置。 



四. 运行MoveIt!



4.1 Launch Demo

现在我们可以来尝试运行moveit了！



复制代码
等待几秒，当看到  All is well! Everyone is happy! You can start planning now! 的时候，就代表启动成功了。我们可以看到一个Rivz窗口，左下角有一个运动规划MotionPlanning模块。

4.3 运动规划终于，到了运动规划的时候了。。在Planning子模块中单击Plan，一个运动轨迹就会出现与Rviz窗口中并循环播放。你可以在Display->MotionPlanning->Planned
Path里面设置各种显示参数。

接下来可以尝试不同的OMPL算法，不同的目标位姿和不同的环境。来看看MoveIt的鲁棒性如何。 





那么，MoveIt!和OMPL的运动规划就差不多讲完了，当然这是很浅显的，与实用性的东西都还有距离。以上都是纯手打，现做的例子，希望有所帮助。接下来该讲什么，你们可以在下面留言，是更深入的讲MoveIt!，还是讲OMPL运动规划算法，还是讲如何模拟具体实例。