vrep学习笔记_V-rep学习笔记：机器人路径规划1

Motion Planning Library

V-REP 从3.3.0开始，使用运动规划库OMPL作为插件，通过调用API的方式代替以前的方法进行运动规划(The old path/motion planning functionality is still functional for backward compatibility and available, but it is recommended not to use it anymore)，这样更具灵活性。

运动规划就是在给定的位置Start与位置Goal之间为机器人找到一条符合约束条件的路径。具体的案例可以是为移动机器人规划出到达指定地点的最短距离，或者是为机械臂规划出一条无碰撞的运动轨迹，从而实现物体抓取等。目前运动规划的研究取得长足进展，栅格法、 路标法、 人工势场法等全局或局部规划算法被相继提出。随着研究的深入，基本的运动规划问题不断扩展，空间的维度也随之增加。但基于确定性空间的精确解法是以巨大的存储空间和计算量为代价的，仅仅在低维空间或者特殊条件下才可能存在精确的解法。在高维空间面前，运动规划的解析算法面临极大的困难。

Motion planning task from Start to Goal while avoiding obstacles and joint limits

传统方法的问题在于，始终无法避免在一个确定性空间内对障碍物进行确定的建模和描述，而这对于复杂环境和高维空间很难做到。基于采样的运动规划方法在这一点与传统方法有本质的不同，它仅仅通过对位形空间或状态空间中的采样点进行碰撞检测来获取障碍物信息，并在此基础上进行运动规划。采样规划方法避免了对空间的建模，即对障碍空间和自由空间进行描述的复杂的前期计算，这使它完全能够处理高维空间和复杂系统的运动规划问题。OMPL是一个运动规划的C++开源库，其包含了很多运动规划领域的前沿算法，总体来说OMPL主要是一个采样规划的算法库(OMPL specializes in sampling-based motion planning)

基于采样的运动规划

运动规划算法通常有两个评价指标：

完备性Complete：利用该算法，在有限时间内能解决所有有解问题；

最优性Optimality：利用该算法，能找到最优路径(路径最短、能量最少等)；

PRM是一种基于图搜索的方法，它将连续空间转换成离散空间，再利用A*等搜索算法在路线图上寻找路径。这种方法能用相对少的随机采样点来找到一个解，对多数问题而言，相对少的样本足以覆盖大部分可行的空间，并且找到路径的概率为1(随着采样数增加，P(找到一条路径)指数的趋向于1)。显然，当采样点太少，或者分布不合理时，PRM算法是不完备的，但是随着采用点的增加，也可以达到完备。所以PRM是概率完备且不最优的。

大致步骤如下面几幅图所示：

除了基于图搜索的方法，还有另外一大类基于树状结构的搜索算法，其中最著名的就是快速扩展随机树法(Randomly Exploring Randomized Trees，RRT)了。RRT算法是从起始点开始向外拓展一个树状结构，而树状结构的拓展方向是通过在规划空间内随机采点确定的。与PRM类似，该方法是概率完备且不最优的。

虽然基于采样的规划算法速度很快，但他们也有缺点，那就是由随机采样引入的随机性。利用RRT和PRM算法进行运动规划，用户无法对规划结果进行预判，每次规划的结果都不一样，缺乏可重复性。与解析算法的确定结果不同，随机方法对同一个规划问题的表现可能时好时坏，连续出现完全相同的规划结果的概率很低。要判断算法对于某一规划问题的效果，往往需要多次反复的试验。基于随机采样的运动规划方法无法预测下一次规划是否能够成功，更无法预测下一次的路径或者轨迹的参数。

V-REP OMPL plugin

V-REP中路径规划通常需要以下变量或参数作为输入：

a collidable or measurable robot entity: this entity represents the device that should avoid obstacles and is referred to hereafter as robot.

a start dummy: the start dummy represents the initial configuration (or start configuration)of the robot. The robot entity should be built on top of the start dummy. Make sure that the center of the robot approximately matches the position of the start dummy. 即需要创建start dummy(相当于一个marker)用于标记机器人的初始构型/位姿。

a goal dummy: the goal dummy represents the desired configuration(or goal configuration) of the robot. The path planning algorithms will try to move the start dummy towards the goal dummy while avoiding collisions between the robot and obstacle.

a collidable or measurable obstacle entity: this entity represents the obstacles that the robot should avoid, while following a path from the start to the goal configuration.

即进行路径规划需要已知：机器人，机器人的初始状态，机器人的目标状态以及周围环境(障碍物)。

V-REP中进行运动/路径规划时，可以按照以下几步进行：

(1)调用simExtOMPL_createTask函数创建一个规划任务；

(2)调用simExtOMPL_setAlgorithm函数选择一个合适的算法；

(3)调用simExtOMPL_createStateSpace和simExtOMPL_setStateSpace创建所需的状态空间(State space:The parameter space for the robot. This space represents all possible configurationsof the robot in the workspace. A single point in the state space is a state)；

(4)调用simExtOMPL_setCollisionPairs设置不应与环境发生碰撞的对象(Specify which entities are not allowed to collide)；

(5)调用simExtOMPL_setStartState和simExtOMPL_setGoalState设置机器人的初始状态以