本文共 1934 字，大约阅读时间需要 6 分钟。
在文章中，介绍了具备渐近最优性的RRT*算法。随着采样点数的增多，RRT*算法的规划结果会逐渐收敛到最优。
但是可以观察到，RRT*算法是对自由空间进行均匀采样，搜索树上会生成很多冗余的分支，我们可以对RRT*算法的采样过程进行改进。
Informed-RRT*算法就是对RRT*的采样过程进行优化得到的算法，它采用一个椭圆采样方式来代替全局均匀采样，如图：
接下来介绍椭圆采样区域的表示方式
标准椭圆方程为：
x 2 a 2 + y 2 b 2 = 1 \frac{x^2}{a^2} + \frac{y^2}{b^2} = 1 a2x2+b2y2=1 焦点坐标为 ( ± c , 0 ) (±c, 0) (±c,0)，长轴长度为 a a a，短轴长度为 b b b，它们间满足：椭圆上任一点到两焦点的距离之和等于 2 a 2a 2a，可以得到： a 2 = b 2 + c 2 a^2=b^2+c^2 a2=b2+c2Informed-RRT*算法椭圆采样区域可由下图来表述。在Informed-RRT*算法中，以起点 x s t a r t x_{start} xstart和终点 x g o a l x_{goal} xgoal作为椭圆的焦点，令 a a a等于初始路径长度的一半，即 a = c b e s t 2 a=\frac{c_{best}}{2} a=2cbest，则 c = c m i n 2 c=\frac{c_{min}}{2} c=2cmin， b = c b e s t 2 − c m i n 2 2 b=\frac{\sqrt{c_{best}^2-c_{min}^2}}{2} b=2cbest2−cmin2。这样就可以得到椭圆方程的所有参数。
在之后的迭代中，没找到一次更短的路径，就用这条更短路径的长度作为新的 c b e s t c_{best} cbest，更新采样椭圆。
然后在椭圆采样区域中进行采样。
先在标准方程中采样，再将采样点旋转平移到实际采样区域，需要两个矩阵：平移向量、旋转矩阵。这两个参数只需要在初始化时计算即可
转换后的坐标为：
[ x ′ y ′ ] = [ c o s θ s i n θ − s i n θ c o s θ ] ⋅ [ x y ] + [ x c e n t e r y c e n t e r ] \left[\begin{matrix}x'\\y'\end{matrix}\right]=\left[\begin{matrix}cos\theta&sin\theta\\-sin\theta &cos\theta\end{matrix}\right]\cdot\left[\begin{matrix}x\\y\end{matrix}\right]+\left[\begin{matrix}x_{center}\\y_{center}\end{matrix}\right] [x′y′]=[cosθ−sinθsinθcosθ]⋅[xy]+[xcenterycenter] 其中 R = [ c o s θ s i n θ − s i n θ c o s θ ] R=\left[\begin{matrix}cos\theta&sin\theta\\-sin\theta &cos\theta\end{matrix}\right] R=[cosθ−sinθsinθcosθ]是旋转矩阵， θ \theta θ表示起点 x s t a r t x_{start} xstart和终点 x g o a l x_{goal} xgoal连线与 x x x轴的夹角， T = [ x c e n t e r y c e n t e r ] T=\left[\begin{matrix}x_{center}\\y_{center}\end{matrix}\right] T=[xcenterycenter]是平移向量，可以用起点 x s t a r t x_{start} xstart和终点 x g o a l x_{goal} xgoal的中点来表示。除了采样过程外，Informed-RRT*的流程和RRT*是一样的。
参考
Gammell J D , Srinivasa S S , Barfoot T D . Informed RRT*: Optimal Sampling-based Path Planning Focused via Direct Sampling of an Admissible Ellipsoidal Heuristic[J]. 2014.
转载地址：http://qmvgn.baihongyu.com/