路径规划（十）启发式Informed RRT *算法

10.1 原理

        在RRT中，当初始路径已经生成之后，如果重点在初始路径周围进行采样的话，可以明显提高路径优化效率。Informed RRT就是进一步优化了采样函数，采样的方式是以起点和终点为焦点构建椭圆形采样区域。

        回顾一下RRT*-smart，因为在某区域撒点越多，该区域的优化效果越好，而单纯的RRT*是在全域内随机撒点，优化效果没有得以集中，RRT*-smart认为经过路径优化后的路径的拐点在障碍物的附近，它认为这个拐点的附近需要着重优化，所以RRT*-smart在进一步撒点的过程中，将一些随机点偏袒的撒在这个拐点的附近邻域。

        这里的informed RRT*也是这样认为，它认为单纯的RRT*在整个区域内随机撒点，优化效果太过分散，如果我能知道我最终优化的路径在哪一块区域，那我就只在这一区域内撒点不就好了吗？informed RRT*就是这样做的。

注意：

        informed RRT*是在RRT*算法给出一条初始路径后，对这个初始路径继续优化的步骤才起作用的，它对于这个初始路径的生成没有帮助。

10.2 思路

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

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

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

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

那么，如何表达这个椭圆呢？下面介绍椭圆采样区域的表达方式

方法1：

先在标准椭圆的方程中采样，再将采样点旋转平移到实际采样区域，需要两个矩阵：平移向量、旋转矩阵。这两个参数只需要在初始化时计算即可

转换后的坐标为：

方法2：

利用超椭圆体然后在二维平面映射

这里放一段.m文件取椭圆随机点的代码（思路如方法2）：

除了采样过程外，Informed RRT*的流程和RRT*是一样的。

10.3 伪码

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

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

10.4 程序示例

程序在寻找初始路径的过程和普通RRT*一样，在全局域中随机撒点，迭代到1282次时首次找到初始路径，然后我们以起始点和目标点为焦点，初始路径的长度为点到两焦点的距离之和，画出一个椭圆：

我们随后的随机点的选取范围不再是全局域了，新采的样本点被限制在这个椭圆中，下图中的圆圈代表迭代1283-2509次的随机点的分布，可见，新的随机点全部被限制在椭圆中：

当迭代到2510次时，新的总长度更短的路径被找到，，随后，我们以起始点和目标点为焦点，以这个新的路径的长度为到两焦点的距离，画出一个比之前更小的椭圆：

同样的，迭代次数为2510-2865次的循环中的新的随机点被限制在这个新的更小的椭圆中，使随机点资源进一步集中：

当迭代到2866次时，找到一个路径更短的路径：

10.5 参考

Informed RRT*: Optimal Sampling-based Path Planning Focused via Direct Sampling of an Admissible Ellipsoidal Heuristic