规划算法之informed RRT star

从RRT star到informed RRT star

上篇博客中已经对RRT* 进行了介绍,可以知道RRT是一个渐进最优的基于采样的规划算法。但是RRT在提高了规划出来的路径的质量的同时，消耗的时间也大大增长。因此在保证规划出来的路径的质量的同时，缩短搜索时间是一个探索的方向，也就是想办法在同样长的时间内，得到尽可能好的规划结果。informed RRT*就是向这个方向探索的一种。

根据高中数学知识可以知道，在椭圆上的点到椭圆两焦点的距离之和相同，椭圆外的点的距离到两焦点的距离之和大于椭圆上的点到两焦点的距离之和，椭圆内的点反之。

回顾一下RRT的搜索图，根据上面这个知识点可以发现，其实RRT在已经得到一条可行路径之后，可以将采样空间收缩到一个椭圆形区域中，区域之外的点对于缩短规划出的路径长度并没有实际价值。

informed RRT就是的主要思想就是上面这种思想，在获取可行路径之后，将采样空间限制在一个椭圆形区域中，并且随着路径长度的不断缩短，逐渐缩小该椭圆形区域。这个思想其实在以前就有，但是提出informed RRT的论文中提出了对这个椭圆形区域直接采样的方法。

可能有人会直接想，这里只不过是缩小了采样空间，并不会明显改进算法。但是实际上，当拓展到高维空间时，效率的提升是巨大的。

在B站居然发现了作者介绍informed RRT* 的视频，这里贴出来。可以看出，效率提升还是挺明显的。
informed RRT* 视频

informed RRT*

算法伪代码如下：

伪代码中是在RRT的伪代码基础上改的，标红的地方是informed RRT 更改的地方。可以看出，其实主体框架上面并没有太多更改，实际上也是，主要的更改都在第七行，也就是采样这一步。

这是采样这一步的伪代码。结合下面这个图会更加清晰。informed RRT将目前已经搜索到的最短路径作为cbest，起点和终点之间的距离作为cmin，以此构建椭圆。当还没有规划结果时，cbest为inf，也就是和经典RRT没有区别。

采样这部分涉及到了如何在椭圆中均匀采样，论文里用了一些数学来表述这个方式，先在一个单位大小的n维超球面进行采样，然后再映射到超椭球中。（其实这部分数学我也没弄很懂。。感兴趣的还是自己去看看论文吧）

按照惯例，贴出informed RRT* 的python源码：
informed RRT* in python
其实我在这个基础上进行了一些修改，但基本都是小细节，推荐大家还是看这个代码。代码结果如下：

还有论文链接
最后，还想说一点，其实我一开始没想清楚为什么叫informed RRT*，也就是这个informed是怎么来的。后面在看论文的时候才发现，informed是加入了先验知识，也就是通过使用者加入的先验知识作为规划过程中的指导，比如A*，引入了启发函数，就是典型的informed规划算法。

以上均是个人理解，有不合适的地方欢迎交流。