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<a
        tmp=b;
        b=a;
        a=tmp;
    end
    x_ball=[b*cos(2*pi*a/b);b*sin(2*pi*a/b);0];
    randpoint=C*L*x_ball+x_center;
    plot(randpoint(1,1),randpoint(2,1),'.')
    hold on 
    grid on
    cbest=cbest-rand()*0.05;
 end

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

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<a):
        tmp=b
        b=a
        a=tmp

    x_ball=np.array([[b*np.cos(2*np.pi*a/b)],[b*np.sin(2*np.pi*a/b)],[0]])
    randpoint=np.dot(np.dot(C,L),x_ball)+x_cneter0
    cbest=cbest-random()*0.05

    plt.plot(randpoint[0],randpoint[1],'.')
    plt.hold("on")
    plt.grid("on")

plt.show()

pycharm

informed-RRT*

 

标签：goal,Vh,cbest,Informed,start,np,RRT,cmin
来源： https://blog.csdn.net/ljq31446/article/details/79708228