路径规划(七)RRT*算法

标签: 函数 工具箱 建模 算法

王昊 2023-01-05 14:26:52

7.1 原理

        RRT*是一种基于采样的最优化路径规划方式,与RRT的区别是,RRT尽量使新节点以及其周围的节点到起点的cost(可以是路径或者时间等目标函数)最短,而不是仅仅寻找离它近的节点,而且在找到路径后不会停止,而是继续进行采样来优化得到的路径。

        尽管RRT算法是一个相对高效率,同时可以较好的处理带有非完整约束的路径规划问题的算法,并且在很多方面有很大的优势,但是RRT算法并不能保证所得出的可行路径是相对优化的。因此许多关于RRT算法的改进也致力于解决路径优化的问题,RRT*算法就是其中一个。RRT*算法的主要特征是能快速的找出初始路径,之后随着采样点的增加,不断地进行优化直到找到目标点或者达到设定的最大循环次数。RRT*算法是渐进优化的,也就是随着迭代次数的增加,得出的路径是越来越优化的,而且永远不可能在有限的时间中得出最优的路径。因此换句话说,要想得出相对满意的优化路径,是需要一定的运算时间的。所以RRT*算法的收敛时间是一个比较突出的研究问题。但不可否认的是,RRT*算法计算出的路径的代价相比RRT来说减小了不少。RRT*算法与RRT算法的区别主要在于两个针对新节点 xnew 的重计算过程,分别为:

·重新为 xnew 选择父节点的过程

·重布线随机树的过程


7.1.1 重新选择父节点过程

微信图片_20230105141406.jpg微信图片_20230105141434.jpg

        在新产生的节点 xnew 附近以定义的半径范围内寻找“近邻”,作为替换 xnew 父节点的备选。依次计算“近邻”节点到起点的路径代价加上 xnew 到每个“近邻”的路径代价,具体过程见上图。

        图(a)中表现的是随机树扩展过程中的一个时刻,节点标号表示产生该节点的顺序,0节点是初始节点,9节点是新产生的节点 xnew,6节点是产生9节点的 xnear,节点与节点之间连接的边上数字代表两个节点之间的欧氏距离(这里我们用欧氏距离来表示路径代价)。

        在重新找父节点的过程中,以9节点 xnew 为圆心,以事先规定好的半径,找到在这个圆的范围内 xnew 的近邻,也就是4,5,8节点。

        原来的路径0 - 4 - 6 - 9代价为10 + 5 + 1 = 16,备选的三个节点与 xnew 组成的路径0 - 1 - 5 - 9,0 - 4 - 9和0 - 1 - 5 - 8 - 9代价分别为3 + 5 + 3 = 11,10 + 4 = 14和3 + 5 + 1 + 3 = 12,因此如果5节点作为9节点的新父节点,则路径代价相对是最小的,因此我们把9节点的父节点由原来的节点4变为节点5,则重新生成的随机树如图(b)所示。


7.1.2 重布线随机树过程

微信图片_20230105141808.jpg微信图片_20230105141811.jpg


    在为xnew 重新选择父节点之后,为进一步使得随机树节点间连接的代价尽量小,为随机树进行重新布线。过程示意如上图重布线的过程也可以被表述成:如果近邻节点的父节点改为 xnew 可以减小路径代价,则进行更改。

    如图(c),9节点为新生成的节点 xnew ,近邻节点分别为节点4 , 6 , 8 。它们父节点分别为节点0 , 4 , 5。路径分别为0 - 4,0 - 4 - 6,0 - 1 - 5 - 8,代价分别为10,10 + 5 = 15 和3 + 5 + 1 = 9。

    如果将4节点的父节点改为9节点 xnew ,则到达节点4的路径变为0 - 1 - 5 - 9 - 4,代价为3 + 5 + 3 + 4 = 15 大于原来的路径代价10,因此不改变4节点的父节点。

    同理,改变了8节点的父节点,路径代价将由原来的9变为14,也不改变8节点的父节点。如果改变6节点的父节点为 xnew 则路径变为0 - 1 - 5 - 9 - 6,代价为3 + 5 + 3 + 1 = 12小于原来的路径代价15,因此将6的父节点改为节点9,生成的新随机树如图(d)。

    重布线过程的意义在于每当生成了新的节点后,是否可以通过重新布线,使得某些节点的路径代价减少。如果以整体的眼光看,并不是每一个重新布线的节点都会出现在最终生成的路径中,但在生成随机树的过程中,每一次的重布线都尽可能的为最终路径代价减小创造机会。

    RRT*算法的核心在于上述的两个过程:重新选择父节点和重布线。这两个过程相辅相成,重新选择父节点使新生成的节点路径代价尽可能小,重布线使得生成新节点后的随机树减少冗余通路,减小路径代价。


7.2 伪码

7f7294eba6a7081b90ae4ec03a2859c.png

7.3 程序示例

438e6e42c0105b48b49d1329bbc59e4.png33bbdcbeef3c7123e6aa89191f9f88f.png1688d74c870c2f6cb6219a0f1ff779b.pngb563362c0cb07ac25f053f037e531c6.png64bc3a13137dcdecd072ab62b95c47a.png


7.4 参考

1、Sampling-based Algorithms for Optimal Motion Planning

2、https://blog.csdn.net/weixin_43795921/article/details/88557317#t0

3、https://zhuanlan.zhihu.com/p/51087819

2993 0 1 收藏 回复

回复

回复

重置 提交