matlab rrt star学习

matlab rrt star学习

function problem = rrt_star_fn(map, max_iter, max_nodes, is_benchmark, rand_seed, variant)
%RRT_STAR_FN -- RRT*FN is sampling-based algorithm. It is a new variant 
% of RRT* algorithm,  which limits the number of nodes in the tree 
% and hence decreases the memory needed for storing the tree. 
% 
% problem = RRT_STAR_FN(map, max_iter, max_nodes, is_benchmark, rand_seed, variant)
% function returns the object of the respective class with the result
%
% map           -- struct with appropriate fields (developer of 
%               the class provides more information on this topic)
% max_iter      -- number of iteration to solve the problem
% max_node      -- number of nodes in the tree
% is_benchmark  -- if true saves snapshots of the tree in a special directory
%               boolean variable
% rand_seed     -- a random seed 
% variant       -- what class to choose, class used defines the problem space
%
%
% for detailed information consult with the help of the _RRT*FN Toolbox_
%
% Olzhas Adiyatov
% 05/26/2013

%%% Configuration block
if nargin < 6
    clear all;
    close all;
    clc;
    
    % load conf
    RAND_SEED   = 1;
    MAX_ITER    = 30e3;
    MAX_NODES   = 4001;
    
    % here you can specify what class to use, each class represent
    % different model.
    % FNSimple2D provides RRT and RRT* for 2D mobile robot represented as a dot 
    % FNRedundantManipulator represents redundant robotic manipulator, DOF is
    % defined in configuration files.

        variant     = 'FNSimple2D';
        %variant     = 'FNSimple3D';
        MAP = struct('name', 'bench_june1.mat', 'start_point', [-12.5 -5.5], 'goal_point', [7 -3.65]);
%     variant     = 'FNRedundantManipulator';
%     MAP = struct('name', 'bench_redundant_3.mat', 'start_point', [0 0], 'goal_point', [35 35]);
%     variant     = 'GridBased2Dimrotate';
%     MAP = struct('name', 'grid_map.mat', 'start_point', [150 150], 'goal_point', [250 50]);
    %[180 284]

    % do we have to benchmark?
    is_benchmark = false;
else
    MAX_NODES   = max_nodes;
    MAX_ITER    = max_iter;
    RAND_SEED   = rand_seed;
    MAP         = map;  
end

addpath(genpath(pwd));

%%% loading configuration file with object model specific options 
if exist(['configure_' variant '.m'], 'file')
    run([ pwd '/configure_' variant '.m']);
    CONF = conf;
else
    disp('ERROR: There is no configuration file!')
    return
end

ALGORITHM = 'RRTSFN';

problem = eval([variant '(RAND_SEED, MAX_NODES, MAP, CONF);']);
%problem = FNRedundantManipulator(RAND_SEED, MAX_NODES, MAP, CONF);


max_nodes_reached = false;

if(is_benchmark)
    benchmark_record_step = 250;
    benchmark_states = cell(MAX_ITER / benchmark_record_step, 1);
    
    timestamp = zeros(MAX_ITER / benchmark_record_step, 1);
    iterstamp = zeros(MAX_ITER / benchmark_record_step, 1);
end

%%% Starting a timer
tic;
for ind = 1:MAX_ITER
    
    if is_benchmark && max_nodes_reached == false && problem.nodes_added == MAX_NODES
        max_nodes_reached = true;
        before_removal_state = problem.copyobj();
    end
    
    new_node = problem.sample();
    nearest_node_ind = problem.nearest(new_node);
    new_node = problem.steer(nearest_node_ind, new_node); % if new node is very distant from the nearest node we go from the nearest node in the direction of a new node
    if(~problem.obstacle_collision(new_node, nearest_node_ind))
        neighbors = problem.neighbors(new_node, nearest_node_ind);
        if numel(neighbors) == 0 %this expression will show us that something is completely wrong
            %disp([' no neighbors detected at ' num2str(ind)]);
        end
        min_node = problem.chooseParent(neighbors, nearest_node_ind, new_node);
        if (problem.nodes_added == MAX_NODES)
            new_node_ind = problem.reuse_node(min_node, new_node);
        else
            new_node_ind = problem.insert_node(min_node, new_node);
        end
        problem.rewire(new_node_ind, neighbors, min_node);
        if (problem.nodes_added == MAX_NODES)
                problem.best_path_evaluate();
            problem.forced_removal();
        end
    end
    
    if is_benchmark && (mod(ind, benchmark_record_step) == 0)
        benchmark_states{ind/benchmark_record_step} = problem.copyobj();
        timestamp(ind/benchmark_record_step) = toc;
        iterstamp(ind/benchmark_record_step) = ind;
    end
    
    % display progress each 100 iterations
    if(mod(ind, 100) == 0)
        disp([num2str(ind) ' iterations ' num2str(problem.nodes_added-1) ' nodes in ' num2str(toc) ' rewired ' num2str(problem.num_rewired)]);
    end
end

if (is_benchmark)
    if strcmp(computer,'GLNXA64') == true
        result_dir = '/home/olzhas/june_results/';
    else
        result_dir = 'F:\Github\repository_2020\rrt_toolbox\june_results\';
    end
    dir_name = [result_dir datestr(now, 'yyyy-mm-dd')];
    mkdir(dir_name);
    save([dir_name '/' ALGORITHM '_' MAP.name '_' num2str(MAX_NODES) '_of_' num2str(MAX_ITER) '_' datestr(now, 'HH-MM-SS') '.mat']);
    set(gcf,'Visible','off');
    
    % free memory, sometimes there is a memory leak, or matlab decides to
    % free up memory later.
    
    clear all;
    clear('rrt_star_fn.m');
%     problem.plot();
%     saveas(gcf, [dir_name '\' ALGORITHM '_' MAP.name '_' num2str(MAX_NODES) '_of_' num2str(MAX_ITER) '_' datestr(now, 'HH-MM-SS') '.fig']);
else
    problem.plot();
end

 

输出结果

100 iterations 87 nodes in 0.76167 rewired 0
200 iterations 181 nodes in 0.7986 rewired 5
300 iterations 274 nodes in 0.82317 rewired 9
400 iterations 371 nodes in 0.84594 rewired 20
500 iterations 468 nodes in 0.87128 rewired 28
600 iterations 566 nodes in 0.89928 rewired 45
700 iterations 662 nodes in 0.91967 rewired 48
800 iterations 761 nodes in 0.94793 rewired 64
900 iterations 858 nodes in 0.97181 rewired 86
1000 iterations 957 nodes in 1.0046 rewired 129
1100 iterations 1052 nodes in 1.0387 rewired 167
1200 iterations 1149 nodes in 1.0647 rewired 210
1300 iterations 1243 nodes in 1.0929 rewired 249
1400 iterations 1339 nodes in 1.1236 rewired 288
1500 iterations 1433 nodes in 1.1526 rewired 354
1600 iterations 1529 nodes in 1.1781 rewired 408
1700 iterations 1624 nodes in 1.2044 rewired 468
1800 iterations 1718 nodes in 1.2363 rewired 520
1900 iterations 1813 nodes in 1.26 rewired 559
2000 iterations 1911 nodes in 1.2951 rewired 657
2100 iterations 2005 nodes in 1.3205 rewired 744
2200 iterations 2101 nodes in 1.3581 rewired 835
2300 iterations 2193 nodes in 1.3874 rewired 936
2400 iterations 2290 nodes in 1.4128 rewired 1008
2500 iterations 2384 nodes in 1.4412 rewired 1111
2600 iterations 2478 nodes in 1.4851 rewired 1256
2700 iterations 2574 nodes in 1.5285 rewired 1427
2800 iterations 2666 nodes in 1.5677 rewired 1566
2900 iterations 2763 nodes in 1.6061 rewired 1721
3000 iterations 2859 nodes in 1.6444 rewired 1898
3100 iterations 2957 nodes in 1.691 rewired 2026
3200 iterations 3051 nodes in 1.736 rewired 2192
3300 iterations 3147 nodes in 1.7925 rewired 2378
3400 iterations 3244 nodes in 1.8435 rewired 2581
3500 iterations 3335 nodes in 1.8824 rewired 2737
3600 iterations 3430 nodes in 1.937 rewired 2920
3700 iterations 3527 nodes in 1.982 rewired 3125
3800 iterations 3623 nodes in 2.0332 rewired 3344
3900 iterations 3721 nodes in 2.0778 rewired 3568
4000 iterations 3816 nodes in 2.124 rewired 3738
4100 iterations 3912 nodes in 2.1577 rewired 3929
4200 iterations 4000 nodes in 2.3428 rewired 4145
4300 iterations 4000 nodes in 2.4534 rewired 4327
4400 iterations 4000 nodes in 2.567 rewired 4542
4500 iterations 4000 nodes in 2.6731 rewired 4715
4600 iterations 4000 nodes in 2.773 rewired 4895
4700 iterations 4000 nodes in 2.8769 rewired 5049
attempt to delete last node in the feasible path
4800 iterations 4000 nodes in 2.9947 rewired 5277
4900 iterations 4000 nodes in 3.11 rewired 5461
5000 iterations 4000 nodes in 3.216 rewired 5652
5100 iterations 4000 nodes in 3.33 rewired 5798
5200 iterations 4000 nodes in 3.4367 rewired 5952
5300 iterations 4000 nodes in 3.5433 rewired 6143
5400 iterations 4000 nodes in 3.6644 rewired 6289
5500 iterations 4000 nodes in 3.7844 rewired 6455
5600 iterations 4000 nodes in 3.894 rewired 6583
5700 iterations 4000 nodes in 3.9988 rewired 6713
5800 iterations 4000 nodes in 4.0969 rewired 6892
5900 iterations 4000 nodes in 4.195 rewired 7034
6000 iterations 4000 nodes in 4.3146 rewired 7174
6100 iterations 4000 nodes in 4.4289 rewired 7319
6200 iterations 4000 nodes in 4.554 rewired 7440
6300 iterations 4000 nodes in 4.6563 rewired 7589
6400 iterations 4000 nodes in 4.7617 rewired 7718
6500 iterations 4000 nodes in 4.863 rewired 7847
6600 iterations 4000 nodes in 4.9587 rewired 7949
6700 iterations 4000 nodes in 5.0486 rewired 8039
6800 iterations 4000 nodes in 5.1506 rewired 8151
6900 iterations 4000 nodes in 5.2416 rewired 8257
7000 iterations 4000 nodes in 5.334 rewired 8361
7100 iterations 4000 nodes in 5.424 rewired 8441
7200 iterations 4000 nodes in 5.5147 rewired 8552
7300 iterations 4000 nodes in 5.6224 rewired 8631
7400 iterations 4000 nodes in 5.7275 rewired 8728
7500 iterations 4000 nodes in 5.8244 rewired 8828
7600 iterations 4000 nodes in 5.9167 rewired 8919
7700 iterations 4000 nodes in 6.0085 rewired 8992
7800 iterations 4000 nodes in 6.1098 rewired 9100
7900 iterations 4000 nodes in 6.2041 rewired 9201
8000 iterations 4000 nodes in 6.2943 rewired 9282
attempt to delete last node in the feasible path
8100 iterations 4000 nodes in 6.3927 rewired 9386
8200 iterations 4000 nodes in 6.4835 rewired 9473
8300 iterations 4000 nodes in 6.5713 rewired 9560
8400 iterations 4000 nodes in 6.6595 rewired 9650
8500 iterations 4000 nodes in 6.7409 rewired 9711
8600 iterations 4000 nodes in 6.8473 rewired 9793
8700 iterations 4000 nodes in 6.9422 rewired 9873
8800 iterations 4000 nodes in 7.0363 rewired 9954
8900 iterations 4000 nodes in 7.1404 rewired 10030
9000 iterations 4000 nodes in 7.2288 rewired 10095
attempt to delete last node in the feasible path
9100 iterations 4000 nodes in 7.3163 rewired 10177
9200 iterations 4000 nodes in 7.4032 rewired 10239
9300 iterations 4000 nodes in 7.4937 rewired 10303
9400 iterations 4000 nodes in 7.5792 rewired 10367
9500 iterations 4000 nodes in 7.6631 rewired 10430
9600 iterations 4000 nodes in 7.7572 rewired 10493
9700 iterations 4000 nodes in 7.8493 rewired 10541
9800 iterations 4000 nodes in 7.9419 rewired 10608
9900 iterations 4000 nodes in 8.0445 rewired 10687
10000 iterations 4000 nodes in 8.1365 rewired 10756
10100 iterations 4000 nodes in 8.2312 rewired 10818
10200 iterations 4000 nodes in 8.3238 rewired 10874
10300 iterations 4000 nodes in 8.4172 rewired 10921
10400 iterations 4000 nodes in 8.5051 rewired 10978
10500 iterations 4000 nodes in 8.5991 rewired 11052
10600 iterations 4000 nodes in 8.6918 rewired 11110
10700 iterations 4000 nodes in 8.7882 rewired 11164
10800 iterations 4000 nodes in 8.8794 rewired 11229
10900 iterations 4000 nodes in 8.9822 rewired 11292
11000 iterations 4000 nodes in 9.0774 rewired 11347
11100 iterations 4000 nodes in 9.1714 rewired 11398
11200 iterations 4000 nodes in 9.2641 rewired 11443
11300 iterations 4000 nodes in 9.3551 rewired 11501
11400 iterations 4000 nodes in 9.4426 rewired 11564
11500 iterations 4000 nodes in 9.5363 rewired 11622
11600 iterations 4000 nodes in 9.6445 rewired 11671
11700 iterations 4000 nodes in 9.7573 rewired 11726
11800 iterations 4000 nodes in 9.8672 rewired 11780
11900 iterations 4000 nodes in 9.9629 rewired 11827
12000 iterations 4000 nodes in 10.0552 rewired 11870
12100 iterations 4000 nodes in 10.1437 rewired 11917
12200 iterations 4000 nodes in 10.2357 rewired 11983
12300 iterations 4000 nodes in 10.3291 rewired 12021
12400 iterations 4000 nodes in 10.4178 rewired 12058
12500 iterations 4000 nodes in 10.5099 rewired 12117
12600 iterations 4000 nodes in 10.6073 rewired 12168
12700 iterations 4000 nodes in 10.6983 rewired 12218
12800 iterations 4000 nodes in 10.7919 rewired 12260
12900 iterations 4000 nodes in 10.8915 rewired 12301
13000 iterations 4000 nodes in 10.9943 rewired 12331
13100 iterations 4000 nodes in 11.1056 rewired 12380
13200 iterations 4000 nodes in 11.1973 rewired 12432
13300 iterations 4000 nodes in 11.2874 rewired 12474
13400 iterations 4000 nodes in 11.3768 rewired 12521
13500 iterations 4000 nodes in 11.4662 rewired 12567
13600 iterations 4000 nodes in 11.5571 rewired 12606
13700 iterations 4000 nodes in 11.6554 rewired 12660
13800 iterations 4000 nodes in 11.7503 rewired 12709
attempt to delete last node in the feasible path
13900 iterations 4000 nodes in 11.8459 rewired 12750
14000 iterations 4000 nodes in 11.9432 rewired 12794
14100 iterations 4000 nodes in 12.0357 rewired 12845
14200 iterations 4000 nodes in 12.129 rewired 12879
14300 iterations 4000 nodes in 12.2219 rewired 12928
14400 iterations 4000 nodes in 12.3205 rewired 12968
14500 iterations 4000 nodes in 12.4142 rewired 13031
14600 iterations 4000 nodes in 12.5062 rewired 13081
14700 iterations 4000 nodes in 12.6034 rewired 13132
14800 iterations 4000 nodes in 12.6975 rewired 13174
14900 iterations 4000 nodes in 12.7888 rewired 13206
15000 iterations 4000 nodes in 12.8847 rewired 13250
15100 iterations 4000 nodes in 12.9853 rewired 13284
15200 iterations 4000 nodes in 13.0759 rewired 13321
15300 iterations 4000 nodes in 13.1687 rewired 13353
15400 iterations 4000 nodes in 13.2593 rewired 13390
15500 iterations 4000 nodes in 13.3489 rewired 13431
15600 iterations 4000 nodes in 13.4407 rewired 13464
15700 iterations 4000 nodes in 13.5367 rewired 13521
15800 iterations 4000 nodes in 13.6307 rewired 13568
15900 iterations 4000 nodes in 13.7245 rewired 13610
attempt to delete last node in the feasible path
16000 iterations 4000 nodes in 13.8206 rewired 13643
16100 iterations 4000 nodes in 13.9153 rewired 13679
16200 iterations 4000 nodes in 14.0123 rewired 13723
16300 iterations 4000 nodes in 14.1031 rewired 13751
16400 iterations 4000 nodes in 14.2005 rewired 13787
16500 iterations 4000 nodes in 14.2967 rewired 13825
16600 iterations 4000 nodes in 14.3878 rewired 13865
16700 iterations 4000 nodes in 14.4837 rewired 13896
16800 iterations 4000 nodes in 14.5738 rewired 13931
16900 iterations 4000 nodes in 14.6663 rewired 13970
17000 iterations 4000 nodes in 14.7585 rewired 14003
17100 iterations 4000 nodes in 14.8573 rewired 14034
17200 iterations 4000 nodes in 14.9773 rewired 14076
17300 iterations 4000 nodes in 15.0939 rewired 14110
17400 iterations 4000 nodes in 15.2174 rewired 14148
17500 iterations 4000 nodes in 15.3122 rewired 14188
17600 iterations 4000 nodes in 15.4131 rewired 14225
17700 iterations 4000 nodes in 15.5113 rewired 14257
17800 iterations 4000 nodes in 15.5992 rewired 14290
17900 iterations 4000 nodes in 15.6913 rewired 14332
18000 iterations 4000 nodes in 15.7936 rewired 14373
18100 iterations 4000 nodes in 15.892 rewired 14419
18200 iterations 4000 nodes in 15.9888 rewired 14443
18300 iterations 4000 nodes in 16.0856 rewired 14477
18400 iterations 4000 nodes in 16.1932 rewired 14509
18500 iterations 4000 nodes in 16.2952 rewired 14544
18600 iterations 4000 nodes in 16.4046 rewired 14578
18700 iterations 4000 nodes in 16.5012 rewired 14614
18800 iterations 4000 nodes in 16.5984 rewired 14652
18900 iterations 4000 nodes in 16.6997 rewired 14699
19000 iterations 4000 nodes in 16.7913 rewired 14729
19100 iterations 4000 nodes in 16.8909 rewired 14756
19200 iterations 4000 nodes in 16.9894 rewired 14792
19300 iterations 4000 nodes in 17.0847 rewired 14826
19400 iterations 4000 nodes in 17.1754 rewired 14846
19500 iterations 4000 nodes in 17.2691 rewired 14883
19600 iterations 4000 nodes in 17.3658 rewired 14914
19700 iterations 4000 nodes in 17.4611 rewired 14958
19800 iterations 4000 nodes in 17.5549 rewired 14999
19900 iterations 4000 nodes in 17.6533 rewired 15020
20000 iterations 4000 nodes in 17.7531 rewired 15056
20100 iterations 4000 nodes in 17.8512 rewired 15096
20200 iterations 4000 nodes in 17.9632 rewired 15136
20300 iterations 4000 nodes in 18.0593 rewired 15177
20400 iterations 4000 nodes in 18.1567 rewired 15211
20500 iterations 4000 nodes in 18.2522 rewired 15237
20600 iterations 4000 nodes in 18.3465 rewired 15265
20700 iterations 4000 nodes in 18.4429 rewired 15299
20800 iterations 4000 nodes in 18.5334 rewired 15323
20900 iterations 4000 nodes in 18.6302 rewired 15348
21000 iterations 4000 nodes in 18.7307 rewired 15381
21100 iterations 4000 nodes in 18.832 rewired 15420
21200 iterations 4000 nodes in 18.9251 rewired 15462
21300 iterations 4000 nodes in 19.0213 rewired 15491
21400 iterations 4000 nodes in 19.119 rewired 15517
21500 iterations 4000 nodes in 19.2159 rewired 15548
21600 iterations 4000 nodes in 19.3078 rewired 15577
21700 iterations 4000 nodes in 19.4061 rewired 15614
21800 iterations 4000 nodes in 19.5012 rewired 15641
21900 iterations 4000 nodes in 19.5959 rewired 15674
22000 iterations 4000 nodes in 19.6913 rewired 15703
22100 iterations 4000 nodes in 19.7912 rewired 15740
22200 iterations 4000 nodes in 19.8898 rewired 15764
22300 iterations 4000 nodes in 19.9864 rewired 15802
22400 iterations 4000 nodes in 20.0885 rewired 15838
22500 iterations 4000 nodes in 20.1909 rewired 15865
22600 iterations 4000 nodes in 20.3011 rewired 15901
22700 iterations 4000 nodes in 20.4191 rewired 15924
22800 iterations 4000 nodes in 20.5422 rewired 15956
22900 iterations 4000 nodes in 20.6487 rewired 15989
23000 iterations 4000 nodes in 20.7439 rewired 16022
23100 iterations 4000 nodes in 20.8417 rewired 16047
23200 iterations 4000 nodes in 20.9375 rewired 16088
23300 iterations 4000 nodes in 21.0296 rewired 16110
23400 iterations 4000 nodes in 21.1227 rewired 16135
23500 iterations 4000 nodes in 21.2173 rewired 16168
23600 iterations 4000 nodes in 21.3108 rewired 16198
23700 iterations 4000 nodes in 21.4122 rewired 16244
23800 iterations 4000 nodes in 21.5266 rewired 16268
23900 iterations 4000 nodes in 21.6403 rewired 16306
24000 iterations 4000 nodes in 21.7468 rewired 16346
24100 iterations 4000 nodes in 21.8463 rewired 16377
24200 iterations 4000 nodes in 21.9484 rewired 16411
24300 iterations 4000 nodes in 22.0442 rewired 16433
24400 iterations 4000 nodes in 22.1443 rewired 16462
24500 iterations 4000 nodes in 22.244 rewired 16496
24600 iterations 4000 nodes in 22.3457 rewired 16525
24700 iterations 4000 nodes in 22.4467 rewired 16564
24800 iterations 4000 nodes in 22.5427 rewired 16589
24900 iterations 4000 nodes in 22.6459 rewired 16639
25000 iterations 4000 nodes in 22.7411 rewired 16672
25100 iterations 4000 nodes in 22.8429 rewired 16696
25200 iterations 4000 nodes in 22.9441 rewired 16737
25300 iterations 4000 nodes in 23.0509 rewired 16765
25400 iterations 4000 nodes in 23.1524 rewired 16800
25500 iterations 4000 nodes in 23.2563 rewired 16833
25600 iterations 4000 nodes in 23.3585 rewired 16858
25700 iterations 4000 nodes in 23.4581 rewired 16875
25800 iterations 4000 nodes in 23.5553 rewired 16893
25900 iterations 4000 nodes in 23.6545 rewired 16925
26000 iterations 4000 nodes in 23.755 rewired 16959
26100 iterations 4000 nodes in 23.8558 rewired 16996
26200 iterations 4000 nodes in 23.9566 rewired 17024
26300 iterations 4000 nodes in 24.0543 rewired 17049
26400 iterations 4000 nodes in 24.155 rewired 17085
26500 iterations 4000 nodes in 24.2538 rewired 17117
26600 iterations 4000 nodes in 24.3605 rewired 17172
26700 iterations 4000 nodes in 24.4611 rewired 17207
26800 iterations 4000 nodes in 24.5614 rewired 17233
26900 iterations 4000 nodes in 24.6617 rewired 17262
27000 iterations 4000 nodes in 24.7685 rewired 17290
27100 iterations 4000 nodes in 24.8765 rewired 17325
27200 iterations 4000 nodes in 24.9754 rewired 17366
27300 iterations 4000 nodes in 25.0738 rewired 17400
27400 iterations 4000 nodes in 25.18 rewired 17432
27500 iterations 4000 nodes in 25.2816 rewired 17465
27600 iterations 4000 nodes in 25.3833 rewired 17487
27700 iterations 4000 nodes in 25.4795 rewired 17507
27800 iterations 4000 nodes in 25.5823 rewired 17528
27900 iterations 4000 nodes in 25.7077 rewired 17557
28000 iterations 4000 nodes in 25.8414 rewired 17586
28100 iterations 4000 nodes in 25.9604 rewired 17615
28200 iterations 4000 nodes in 26.0599 rewired 17643
28300 iterations 4000 nodes in 26.1661 rewired 17676
28400 iterations 4000 nodes in 26.2694 rewired 17710
28500 iterations 4000 nodes in 26.3685 rewired 17739
28600 iterations 4000 nodes in 26.4719 rewired 17768
28700 iterations 4000 nodes in 26.571 rewired 17797
28800 iterations 4000 nodes in 26.6762 rewired 17823
28900 iterations 4000 nodes in 26.7923 rewired 17851
29000 iterations 4000 nodes in 26.9044 rewired 17882
29100 iterations 4000 nodes in 27.0092 rewired 17912
29200 iterations 4000 nodes in 27.1154 rewired 17930
29300 iterations 4000 nodes in 27.2201 rewired 17955
29400 iterations 4000 nodes in 27.3264 rewired 17978
29500 iterations 4000 nodes in 27.4281 rewired 18007
29600 iterations 4000 nodes in 27.5336 rewired 18030
29700 iterations 4000 nodes in 27.634 rewired 18064
29800 iterations 4000 nodes in 27.7426 rewired 18091
29900 iterations 4000 nodes in 27.8443 rewired 18116
30000 iterations 4000 nodes in 27.9506 rewired 18152
23.9012

ans = 

  FNSimple2D (具有属性):

                tree: [2×4001 double]
              parent: [1×4001 double]
            children: [1×4001 double]
          free_nodes: [1×4001 double]
      free_nodes_ind: 2
                cost: [1×4001 double]
             cumcost: [1×4001 double]
         XY_BOUNDARY: [-20 20 -20 20]
          goal_point: [7 -3.6500]
    delta_goal_point: 1
          delta_near: 1.5000
         nodes_added: 4001
            max_step: 0.5000
            obstacle: [1×1 struct]
    dynamic_obstacle: []
      best_path_node: 1123
        goal_reached: 1
           max_nodes: 4001
            bin_size: 7
                 bin: [1×36 struct]
               bin_x: 6
               bin_y: 6
          bin_offset: 22
               nbins: 36
             bin_ind: [10×4001 double]
       compare_table: [1×4001 double]
               index: [1×4001 double]
                list: [1×4001 int32]
         num_rewired: 18152

>> 

 

 

代码参考：https://github.com/olzhas/rrt_toolbox