Informed RRT*

论文：点击打开链接

 

1. 论文主要方法，根据RRT*首先找到路径后的一个Cbest，在根据Cbest构建一个椭圆进行采样，当Cbest不断减小，椭圆的范围也不断减小，最后收敛成一条直线（无障碍时）。
2. 大幅减少搜索范围。

上图则是，椭圆的建立以及收敛。

点击打开链接基于椭圆随机采样论文。

采样思路：

这部分我采用matlab

start=[100,120];goal=[100,80];
cmin=norm(goal-start);
% plot(start(1),start(2),'o')
% hold on
% grid on
% plot(goal(1),goal(2),'*')
% hold on
% grid on
cbest=120;
axis([0,200,0,200])
while cbest>=cmin
    x_center=[(start+goal)/2,0];
    x_center=x_center';
    a_1=[(goal(1)-start(1))/cmin;(goal(2)-start(2))/cmin;0];
    id_t=[1,0,0];
    M=a_1*id_t;
    [U,S,Vh]=svd(M);
    C=(U*diag([1,1,det(U)*det(Vh')]))*(Vh);
    r=[cbest/2,sqrt(cbest.^2-cmin.^2)/2,sqrt(cbest.^2-cmin.^2)/2];
    L=diag(r);
    
    a=rand();
    b=rand();
    if b

采样结果可以看出会慢慢收敛成椭圆，最后收敛成一条线。

python

import numpy as np
import matplotlib.pyplot as plt
from random import random

start=[100,120]
goal=[100,80]
tempcmin=[start[0]-goal[0],start[1]-goal[1]]

cmin=np.linalg.norm(tempcmin,ord=2)
cbest=120
plt.figure()
while (cbest>=cmin):
    x_center=np.matrix([(start[0]+goal[0])/2.0,(start[1]+goal[1])/2.0,0])
    x_cneter0=np.transpose(x_center)
    a_1=np.matrix([[(goal[0]-start[0])/cmin],[(goal[1]-start[1])/cmin],[0]])
    id_t=np.matrix([1.0,0,0])
    M=np.dot(a_1,id_t)
    U,S,Vh=np.linalg.svd(M)
    C=np.dot(np.dot(U,np.diag([1,1,np.dot(np.linalg.det(U),np.linalg.det((np.transpose(Vh))))])),Vh)
    r=[cbest/2.0,np.sqrt(cbest**2-cmin**2)/2.0,np.sqrt(cbest**2-cmin**2)/2.0]
    L=np.diag(r)

    a=random()
    b=random()
    if (b

pycharm

informed-RRT*