基于OMPL的采样运动规划算法(Sampling-based Motion Planning)
redefine
redefine 11727 20
热门 2016-05-03 17:12
好久没上了...先占位。。待我想想这个话题要怎么写。。




一下内容我边想边写,也不知道什么时候能写完,各位先忽略吧。。很多东西是我自己的理解,有错误或者不妥的地方欢迎指出。


在上一个帖子:运动规划 (Motion Planning): MoveIt! 与 OMPL我们介绍了怎么用MoveIt! 和OMPL做运动规划。那么还是那句话,不是怎么用,而是为什么。这个帖子,我们来探讨一下具体的运动规划算法。运动规划算法主要分为两种,优化算法(Optimization-based motion planning)和采样算法(Sampling-based motion planning)。由OMPL使用的是采样算法(其中也包括一些基于采样的优化算法,不过说到底还是采样算法),那么我们就先讲采样算法。这个帖子主要分为两部分,首先是采样算法的一些理论,然后是在OMPL里面的具体实现。。


一。基于采样的运动规划算法(Sampling-based Motion Planning)
避免和上一个帖子重复,废话不多说了。之前我们提到最出名的采样算法莫过于
  • Rapidly exploring Random Tree (原论文:RRT)
  • Probabilistic RoadMap (原论文:PRM)
两个算法都是90年代末出现的,虽然年代久远一些,不过要想真正学好运动规划,还是要从这里入手,而且说实话,新的算法也都是这两种算法的延伸,没有本质的区别。网上也是有无数的文档教程,这里附上卡内基梅隆(CMU)的RRT算法课件


这里我们简单说一下RRT,其实非常的简单(所以才能如此通用)。我们先以质点(无方向)无人车路径规划举例(无人车路径规划可以看作二维运动规划),假设无人车当前位置是(x_init,y_init),想要到达(x_goal,y_goal)。那么首先建立一个空的随机树,把起始点添加到树中。然后随机选一个点(x_rand, y_rand),这里可以循环随机选点,知道选择的随机点有效(collision free)。接下来从随机树种选取与(x_rand,y_rand)最接近的点,称作(x_near,y_near)。如果是第一次,那么这个(x_near,y_near)就应该是起始点(x_init,y_init),因为此时树中只有一个点。接下来,检查(x_near,y_near)到(x_rand,y_rand)之间的路径是否有效。这里通常是通过密集线性/非线性插值来实现的,也就是说在(x_near,y_near)到(x_rand,y_rand)之间密集的插入很多点,如果每个点都有效,那么就认为这条路径有效。如果路径有效,那么就把随机点(x_rand,y_rand)添加到树中,并记录上一节点(parent node)为我们选取的最近点(x_near,y_near),如果路径无效,则舍弃随机点。重复这个过程,直到随机点正好取到(或接近)终点(x_goal,y_goal)。那么我们就得到了一条从起点到终点的有效路径。

最初的RRT就是这么这么简单,不过最好还是推荐看下原论文。当然,之后有无数种(毫不夸张,无数种。。)基于RRT的衍生算法。无非就是加入一些小技巧,sampling-bias, goal-bias, max steering, bio-directional search之类的。当了解本质之后,这些就容易理解了。



1.1 高维空间采样算法 (High-dimensional space sampling)
首先,高维度,听着好高端,其实很简单,只要抛开常规的三维理念,你会发现随便多少维都是一样的。


上面我们讲的是一个质点无人车,那么它的采样空间就是一个二维平面,我们习惯性把第一维称为x, 第二维称为y,当然我们也可以把第一维称为q1, 第二维称为q2。如果加入无人车的方向,就是三维采样空间。这第三维我们一般称为theta,当然也可以称为q3。同样的如果是无人飞机,不考虑方向,那就是三维(x,y,z),也可以标注为(q1,q2,q3),如果考虑方向那就是6维(x,y,z,r,p,y),或者(q1,...,q6). 再以我们人来说,一般说我们生活在三维空间,其实我们和无人机一样生活在6维空间。比如,你站在一个地方,那么前三维(x,y,z)的位置固定了,你原地转身,xyz不变,但是你的朝向变了。这里常说的6维空间就是我们ROS里的geometry_msgs::Pose,包含了position xyz, orientation xyzw. 这里xyzw是四元数表示,这样可以避免奇点(singularity point, 不过这是另一个话题了。。),而表示方向最少要三个值,比如飞行系统所用的roll, pitch, yaw。所以geometry::Pose的维度是xyz+rpy=6维,而不是xyz+xyzw=7维。说到底,维度不过是一个度量,而莫个维度系统里的一个状态就是每个维度的度量里一个值的集合。比如宇宙中xyz随便取一个点,就固定了你的位置,假如存在另一维度时间,那么随便取一个时间值t,组成xyzt,那么就代表了你在某个时间的位置,加入还有更高维(虽然不知道是什么,但假设是k),那么任意一个(xyztk)的向量,就决定了你在五维空间的位置。而前四维我们一般用xyz和时间来表示,但这不是必须的,你可以用q1,q2,q3,q4,q5来表示。那么也就是说,任意维度N, 我们都可以用q1,q2,...,qN来表示,每个维度任意取一个值,就构成了这个高维系统里的一个状态,一个点。当N=2,那就是一个无人车路径规划,N=3就是一个无人机路径规划,N=7就可能是一个7自由度机械臂运动规划。


N=4就是,额。。时空穿越规划?好像扯得有点远。。。那么扯这些的目的是什么?当然是为了更好的理解RRT。 上面说了无人车是一个二维运动规划(x_init,y_init)到(x_goal,y_goal),如果不用xy而是q1,q2, 那就是从(q1_init,q2_init)到(q1_goal,q2_goal)。那么如果是个多关节高纬度机器人,比如7关节机械臂,那么他的一个状态就可以由7维度向量来表示。假设要让机械臂从一个Pose移到另个Pose,就是一个从起始点(q1_init, q2_init, ...., q7_init)到终点(q1_goal,q2_goal,....,q7_goal)的RRT问题,每次随机取点的时候就取一个7维随机点(q1_rand,q2_rand,....,q3_rand)。其他所有步骤和上面无人车算法完全一样。 这也是RRT的魅力所在,无论机器人机械结构如何,维度如何,算法都一样。
游客
要评论请先登录 或者 注册
20条回应 只看楼主 最新
roswiki BOSS 2016-05-04 23:13 沙发
每周一更啊!!
寒冬夜行人54 一代型 2016-05-06 14:18 板凳
距离上次发文章已经四个月了,读者等的好心急
redefine 金牌作者 2016-05-07 07:31 地板
寒冬夜行人54:距离上次发文章已经四个月了,读者等的好心急回到原帖
额。。主要不知道要写什么。。
寒冬夜行人54 一代型 2016-05-13 10:04 4楼
redefine:额。。主要不知道要写什么。。回到原帖
写moveit的API文件怎么用吧,我发现群里好多人试过用moveit,但最终无从下手,放弃了。
luozhilzh 原型机 2016-05-19 10:09 5楼
楼主,你好,我是一个初学者,你写的对我理解帮助很大,能否结合一个示例来讲?比如一个机械手抓某一物体的实现过程。
gu_tq 原型机 2016-05-19 16:18 6楼
请问,Moveit中的运动规划是用OMPL规划出一条路径在用FCL库进行碰撞检测,还是在调用OMPL的时候通过OMPL调用FCL进行碰撞检测?大神知道这部分的代码在哪个CPP中实现的吗?
redefine 金牌作者 2016-05-19 18:23 7楼
gu_tq:请问,Moveit中的运动规划是用OMPL规划出一条路径在用FCL库进行碰撞检测,还是在调用OMPL的时候通过OMPL调用FCL进行碰撞检测?大神知道这部分的代码在哪个CPP中实现的吗?回到原帖
OMPL是一个模块化的库,里面有一个模块是state validation checker接口类,moveit里面有这个类的实现,然后里面调用FCL进行碰撞检测。在moveit里面好像是叫CollisionWorldFCL。
gu_tq 原型机 2016-05-20 16:28 8楼
redefine:OMPL是一个模块化的库,里面有一个模块是state validation checker接口类,moveit里面有这个类的实现,然后里面调用FCL进行碰撞检测。在moveit里面好像是叫CollisionWorldFCL。回到原帖
还想问一下,我现在在Moveit里做一个机械臂的规划,里面加入了一个障碍物,我把机械臂从木块的一侧移动到另外一侧,经常规划失败,出现的错误报告就是“发现一条路径,但是路径上有碰撞,失败”。但是OMPL算法应该是直接找到一条无碰撞的路径,而不是先找到一条路径,在进行检测路径上有没有碰撞。 这是为什么?大神知道么?是需要改变什么参数吗?用的是RRTconect算法。谢谢!
gu_tq 原型机 2016-05-20 16:31 9楼
redefine:OMPL是一个模块化的库,里面有一个模块是state validation checker接口类,moveit里面有这个类的实现,然后里面调用FCL进行碰撞检测。在moveit里面好像是叫CollisionWorldFCL。回到原帖
没有找到CollisionWorldFCL,麻烦能告诉我一下具体的cpp名称吗?多谢!
redefine 金牌作者 2016-05-21 16:44 10楼
gu_tq:没有找到CollisionWorldFCL,麻烦能告诉我一下具体的cpp名称吗?多谢!回到原帖
你需要问的是,你找那文件干什么? 你要自己重写碰撞检测?
gu_tq 原型机 2016-05-23 09:15 11楼
redefine:你需要问的是,你找那文件干什么? 你要自己重写碰撞检测?回到原帖
我的规划感觉不太对,在RVIZ里面有障碍的规划,从障碍物的一边规划到另一侧,在terminal中的输出信息是这样的,总是先找到一条有碰撞的路径,然后发现这条路径有碰撞,然后失败。需要重新规划。用的是RRTconnect的算法,但是根据RRTconect的方法,应该是在生成树的节点的时候就是一个物碰撞的,最终那个找到的路径也应该是无碰撞的。。。现在的情况倒像是先规划,在检测碰撞,大神知道为什么吗?所以我想看看源码,寻找一下原因。谢谢!
wxmxmu 原型机 2016-05-29 10:57 12楼
想请教一下,RRT所规划的路径,假设只考虑位置(x,y,z),应该是有很多曲折的线段组成。该怎么做优化,才能快速形成平滑的轨迹?
清风_123 原型机 2016-06-19 22:11 13楼
大神,请教下moveit用在真实机器人上时,输出的是什么,怎么样把输出消息传到下边去
morata21 原型机 2016-07-21 13:47 14楼
你好,我用arduino控制机械臂,请问使用moveit,如何和arduino通信,对手臂进行运动的规划?
1 2
返回顶部