基于采样的路径规划算法RRT的优化
基本的基于采样的路径规划算法RRT,在地图中进行采样取点,直到新的节点取到终点的一定阈值范围内,视为查找到路径,( RRT算法详细)。但是在这个规划过程中,找出的路径总是曲折的。为此对RRT算法进行了一系列优化:
RRT * 算法
RRT*算法针对性地解决RRT算法生成路径不最优的缺陷
左图是RRT算法原本生成的不平滑的路径,右图是RRT * 算法伪代码。
RRT * 算法伪代码中,在新产生的节点 x(new) 画一个规定半径的圆,找到最近的节点为 x1, x2, x(near),分别求 x(new) 经过几个最近节点到达红色起始点的路径大小,取路径最短的节点与 x(new) 连接起来。
RRT * 算法选择父节点没有直接把最近的节点当父节点,而是搜索了多个邻域节点,从而选定父节点。RRT*突出的特点就展现在伪代码中的 rewire() 重写步骤,帮助修改链接过程,使数据更优化。
rewire() 的过程是重写节点的父节点:在上面例子中将 x(new) 其余的两个点 (x1,x2) 与 x(new) 连接,比较( x1 原来的路径到红色起始点的路径长度 d1 )和 (x1 经过 x(new) 再到达红色起始点的路径长度 d2),如果后者的路径更短,则会更换 x1 的父节点为 x(new) 。在上面例子中显然 d1 小于 d2 ,所以其父节点不会改变。而 ( x2 原来的路径到红色起始点的路径长度 d1 )和 (x2 经过 x(new) 再到达红色起始点的路径长度 d2),后者的路径更短,所以 x2 的父节点重写为 x(new),如下图:
RRT * 算法会通过不断地修改父节点来优化轨迹,从而能够产生一个较平滑的轨迹,
在这个视频:RRT*算法路径规划演示 中可以较快理解RRT *的路径规划过程,以及轨迹优化的过程。
/
Kinodynamic-RRT*
传统的RRT * 算法 x(new) 和 x(near) 的连接直接用直线连接,在下图左图的连接中会碰到障碍物,但实际上根据机器人的动力学约束,实际路径可能不会碰到障碍物。
Kinodynamic-RRT * 算法中用曲线代替直线,让生成的路径更加满足动力学约束,
在这个视频:Kinodynamic-RRT*算法路径规划演示 中可以直观地发现Kinodynamic-RRT* 的路径规划过程。
Anytime-RRT *
Anytime-RRT * 是在机器人运动的过程中不断地更新路径,在机器人执行当前路径时保持最优的树节点路径,即实时的RRT * 算法,能更好地适应机器人运动过程中环境变化比较大的情况。在这个视频:Anytime-RRT * 算法路径规划演示 中可以容易地理解 Anytime-RRT * 的路径规划过程。
Informed RRT *
RRT * 在地图空间中采样进行均匀撒点,采样点会布及整个地图会进行很多不必要的采样。
Informed RRT *把采样的范围限制在一个椭圆里面:
以起始点和终点作为椭圆的焦点,以 RRT * 生成的路径长度 L 作为椭圆上点到焦点的距离之和,在椭圆内进行采样,随着生成的路径越来越优化,长度越来越短,椭圆也会越来越扁,从而集中采样点进行了有效的路径优化。
关于搜索树按搜索方向生长的计算方法
三角函数的方法
先计算出夹角 𝜃 ,采用三角函数的方法沿生长方向延伸:生长长度为 L
𝜃 = arctan((Xrand.y - Xnear.y) / (Xrand.x - Xnear.x) )
得到夹角 𝜃 后,计算 Xnear 沿 x 方向 和 沿 y 方向对应的增长值:
Xadd = L * sin𝜃
Yadd = L * cos𝜃
所以 Xnew:(Xnear.x + Xadd , Xnear.y + Yadd)得到生长后的节点。
向量方法
通过上图中的公式得到长度比例关系:k = ||vw|| / ||vo||。(图中有误,最后写反了)
o.x = v.x + (w.x-v.x) * k
o.y = v.y + (w.y-v.y) * k
得到生长节点 o 的坐标