MoveIt！运动规划性能分析

MoveIt！为多自由度机械臂的运动规划提供了一个统一架构，可实现在关节空间和笛卡尔空间的运动规划任务。MoveIt！调用已集成多种采样算法的运动规划库OMPL来实现两种规划空间的运动规划任务。

在笛卡尔空间的运动规划时，由于已知信息为机械臂末端或工具坐标系的位姿，因此，在规划过程中需要使用逆运动学实现从笛卡尔空间到关节空间的转换。MoveIt！默认使用一个通用的第三方运动学与动力学库（Kinematics and Dynamics Library，KDL¹）对机械臂进行逆运动学求解。此外，用户也可以实现自己的逆运动学求解程序，并以插件形式将其应用于MoveIt！，如本文实现的UR5安装夹持器后的逆运动学插件，UR5-IK。

由于OMPL具有多种基于采样的运动规划算法，且MoveIt！可调用不同的逆运动学插件，因此，需要对这些算法和插件的性能进行比较分析，从中选择性能较优者。

为实现对比分析的目的，本文设计了如下入所示的仿真实验。实验思路为：机械臂从其实位姿开始进行规划，避开图1中障碍物A、障碍物B和桌面三个障碍物，最后达到目标位姿。每一种采样规划算法进行10组规划，每一组规划最多可尝试5次，直至规划次数超过5次才认定为该组规划失败。在笛卡尔空间进行规划时，使用KDL和本文推导的UR5逆运动学分别于各采样规划算法配合实现运动规划任务。

图 1. 仿真实验场景

算法性能分析主要针对成功次数、规划时间两项重要指标进行对比分析。机械臂的起始构型为c0=[ -0.7854，-1.5708，0.0，-1.5708, 0.0, 0.0 ], 目标姿态构型为c1=[-0.2515，-1.6263，1.4942，-1.4370，-1.5708,-1.8223 ]。上图中夹持器中心圆球所表示的目标位姿为p=[0.4722, -0.0086, 0.3327, 1.3435, 1.569, 1.9143]$$p=\left [ 0.4722，-0.0086，0.3327，1.3435，1.569,1.9143 \right ]$$$$p=\left [ 0.4722，-0.0086，0.3327，1.3435，1.569,1.9143 \right ]$$。选中的采样规划算法为SBL, KPIECE, RRT, RRTConnect, PRM, PRMstar。

表1为在关节空间进行运动规划时获得的几种算法的实验数据。表2为KDL与各种采样规划算法配合使用时的实验数据。表3为本文推导的UR5逆运动学插件与各采样算法配合使用时得到的实验数据。

表 1. 关节空间运动规划实验数据

表 2. 基于KDL的笛卡尔空间运动规划实验数据

表 3. 基于UR5-IK的笛卡尔空间运动规划实验数据

由表1、表2和表3可以看出，RRTConnect算法在两种规划空间内都具有比较明显的速度优势和成功率优势。在笛卡尔空间进行规划时，基于UR5-IK的规划成功率比使用KDL时要高，并且RRTConnect的平局耗时只需要0.0842s，优势非常明显。

由于KDL是一种通用的运动学库，不能对同一位姿的多解根据真实机器人的构型进行正确选择，会得出比较怪异的关节构型，如图2所示，因此，后续的机械臂运动规划实验中，本文选择RRTConnect与UR5-IK的组合。

图 2. KDL求解的另一种手臂构型

2017.6.20

参考文献：

1. 徐保来. 基于ROS的移动操作臂自主导航与抓取实现[D]. 广东工业大学, 2016.
2. Bruyninckx, H. and Soetens D, P. Open Robot Control Software. http://www.orocos.org/kdl.2002.

¹. Bruyninckx, H., 2001. Open robot control software: the OROCOS project. In Robotics and Automation, 2001. Proceedings 2001 ICRA. IEEE International Conference on (Vol. 3, pp. 2523-2528). IEEE. ↩