路径规划 | 随机采样算法:Informed-RRT*

在文章路径规划 | 随机采样算法:PRM、RRT、RRT-Connect、RRT*中,介绍了具备渐近最优性的RRT*算法。随着采样点数的增多,RRT*算法的规划结果会逐渐收敛到最优。

但是可以观察到,RRT*算法是对自由空间进行均匀采样,搜索树上会生成很多冗余的分支,我们可以对RRT*算法的采样过程进行改进。

Informed-RRT*算法就是对RRT*的采样过程进行优化得到的算法,它采用一个椭圆采样方式来代替全局均匀采样,如图:

路径规划 | 随机采样算法:Informed-RRT*_第1张图片

接下来介绍椭圆采样区域的表示方式

标准椭圆方程为:
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+c2

Informed-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=2cbest2cmin2 。这样就可以得到椭圆方程的所有参数。
路径规划 | 随机采样算法:Informed-RRT*_第2张图片

在之后的迭代中,没找到一次更短的路径,就用这条更短路径的长度作为新的 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] [xy]=[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*是一样的。

路径规划 | 随机采样算法:Informed-RRT*_第3张图片
程序见:ghowoght/motion-planner

参考

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.

你可能感兴趣的:(移动机器人,路径规划,算法,移动机器人,RRT)