RRT_STAR路径规划代码

这是一段使用MATLAB编写的代码，实现了一个基于RRT*（Rapidly-exploring Random Trees Star）算法的路径规划。RRT*是一种用于在配置空间中搜索路径的采样算法，常用于机器人路径规划等领域。以下是代码的主要功能和结构：
主函数RRT_STAR_realize：这是实现RRT*算法的主要函数，接收起始点、目标点、障碍物、最大迭代次数等参数，输出路径、迭代次数、路径成本等信息。
辅助函数：
GenerateRandomNode：生成一个随机的节点。
Extend：从现有树中扩展到一个新的随机节点，如果新节点与障碍物碰撞，则放弃该节点。
isConnect：检查新生成的节点是否可以与目标点直接连接。
ConstructPath：从目标点回溯，构建从起始点到目标点的路径。
FindNearestNode：在树中找到距离目标节点最近的节点。
CollisionCheck：检查从节点A到节点B的路径是否与任何障碍物碰撞。
Intersect：检查两条线段是否相交。
DoLinesIntersect：具体实现线段相交的判断逻辑。
reparent：为新生成的节点重新选择父节点，以优化路径。
rewriteTree：在重新选择父节点后，更新树的结构。
绘图和可视化：
代码中使用了MATLAB的绘图函数，如rectangle、plot等，以可视化起始点、目标点、障碍物和生成的路径。
注释：
代码中包含了一些中文注释，解释了函数和代码块的功能。
性能评估：
代码使用toc函数计算并返回算法的运行时间。
整体来说，这是一个实现路径规划算法的MATLAB脚本，其中涉及随机采样、路径优化、碰撞检测等多个方面。

1、主函数RRT_STAR.m

Matlab 复制代码

    [f,figure_num]=ob1()
    start=[1,1];
    goal=[35,40];
    [path,real_iterations,path_cost,node_num,effect_node_num,Time_Cost]=RRT_STAR_realize(start,goal,f,3000,1,3,30);

%%缁樺埗鎬讳綋璺緞
    if ~isempty(path)
        Fx=path(1,1);Fy=path(1,2);  %绗竴涓妭鐐圭殑x锛寉鍧愭爣
        totalIndex=size(path,1);
        i=2;
        while i<=totalIndex
            plot(Fx,Fy,'r.');
            x=path(i,1);
            y=path(i,2);
            plot([x,Fx],[y,Fy],'r-','LineWidth', 1);hold on;%pause(0.05);
            Fx=x;Fy=y;
            i=i+1;
        end
    end





%杈撳叆鍙傛暟锛氳捣濮嬬偣銆佺洰鏍囩偣銆侀殰纰嶇墿銆佹渶澶ц凯浠ｆ鏁般?姝ラ暱銆佹敹鏁涙椂鐐逛笌鐩爣鐐规渶杩戣窛绂汇?閲嶆柊閫夋嫨鐖惰妭鐐瑰拰閲嶆柊甯冪嚎鏃跺?鑼冨洿
function [path,real_iterations,path_cost,node_num,effect_node_num,Time_Cost] = RRT_STAR_realize(start, goal, obstacles, maxIterations, stepSize,threshold,re_plan_threshold)
    tic;%鍚姩瀹氭椂鍣?
    % 鍒濆鍖栬捣濮嬫爲鍜岀洰鏍囨爲
    treeA = struct('node', start, 'parent', 0,'NP_Cost',0);%NP_Cost----Node Path Cost浠庤捣濮嬬偣鍒扮洰鍓嶇偣鐨勮矾寰勪唬浠?
    radius=0.5;
    rectangle('Position', [start(1)-radius, start(2)-radius, 2*radius, 2*radius], 'Curvature', [1, 1], 'FaceColor', 'b', 'EdgeColor', 'none');
    rectangle('Position', [goal(1)-radius, goal(2)-radius, 2*radius, 2*radius], 'Curvature', [1, 1], 'FaceColor', 'r', 'EdgeColor', 'none');
%     plot(start(1),start(2),'ro');
%     plot(goal(1),goal(2),'ko');
    real_iterations=0;



    % 杩唬鎼滅储
    for i = 1:maxIterations
        % 鐢熸垚闅忔満鐐?
        randomNode = GenerateRandomNode();
        real_iterations=real_iterations+1; %瀹為檯杩唬娆℃暟澧炰竴
        % 鎵╁睍璧峰鏍?
        [newNodeA,nearestNodeAIndex] = Extend(treeA, randomNode, obstacles, stepSize);
        
         % 灏嗘柊鑺傜偣娣诲姞鍒拌捣濮嬫爲鍜岀洰鏍囨爲
        if ~isempty(newNodeA)
            treeA(end+1) = struct('node', newNodeA, 'parent', nearestNodeAIndex,'NP_Cost',treeA(nearestNodeAIndex).NP_Cost+stepSize);


        %閲嶉?鐖惰妭鐐?
        [newParent,newcost,potential_son]=reparent(treeA,re_plan_threshold,obstacles);
        if newParent
            treeA(end).parent=newParent;
            treeA(end).NP_Cost=newcost;
        end
             plot([newNodeA(1),treeA(treeA(end).parent).node(1)],[newNodeA(2),treeA(treeA(end).parent).node(2)],'g-','LineWidth', 0.8);
                         pause(0.005);
           plot(newNodeA(1),newNodeA(2),'r.');
            hold on;
        %閲嶆柊甯冪嚎
            rewriteTree(treeA,potential_son,obstacles);

             % 妫?煡鏂拌妭鐐规槸鍚︿笌鐩爣鐐圭浉杩?
            [connect_flag,Last_NP_Cost]= isConnect(newNodeA, goal, obstacles,threshold);%Last_NP_Cost涓烘渶鍚庝竴涓妭鐐逛笌鐩爣鑺傜偣涔嬮棿鐨勮矾寰勪唬浠?
            if connect_flag
            % 鐩爣鏍戜腑鐨勮妭鐐逛笌璧峰鏍戜腑鐨勬柊鑺傜偣鐩歌繛锛屾瀯寤鸿矾寰?
            path = ConstructPath(treeA,numel(treeA),goal);%numel(treeA)涓烘渶鍚庝竴涓妭鐐圭储寮曪紝Last_NP_Cost涓烘渶鍚庝竴涓妭鐐瑰埌鐩爣鑺傜偣鐨勮矾寰勪唬浠?
            path_cost=treeA(end).NP_Cost+Last_NP_Cost;
            node_num=numel(treeA);
            effect_node_num=size(path,1);
            Time_Cost = toc;  % 鍋滄璁℃椂鍣ㄥ苟杩斿洖缁忚繃鐨勬椂闂?
%             elapsedTime_ms = round(elapsedTime * 1000);  % 灏嗘椂闂磋浆鎹负姣骞跺洓鑸嶄簲鍏?
            return;
            end
        end
        
    end
    
    % 杈惧埌鏈?ぇ杩唬娆℃暟锛屾湭鎵惧埌璺緞
    path = [];
    path_cost=0;
    node_num=0;
    effect_node_num=0;
    Time_Cost=0;
end



% 鐢熸垚闅忔満鐐?
function randomNode = GenerateRandomNode()
     randomNode = round(rand(1,2)*500)+[1 1];
end

% 鎵╁睍鏍?
function [newNode,nearestNodeIndex] = Extend(tree, target, obstacles, stepSize)
    [nearestNode,nearestNodeIndex] = FindNearestNode(tree, target);
    direction = (target - nearestNode) / norm(target - nearestNode);
    newNode = nearestNode + direction * stepSize;
    
    if CollisionCheck(nearestNode, newNode, obstacles)
        newNode = [];
    end
end

% 妫?煡涓や釜鏍戜腑鐨勮妭鐐规槸鍚︾浉杩?
function [flag,NP_Cost] = isConnect(newNodeA, node,obstacles ,threshold)
     NP_Cost=distance(newNodeA(1),newNodeA(2),node(1),node(2));
    if(NP_Cost<=threshold)
           if CollisionCheck(node, newNodeA, obstacles)==false
                flag=true;
           else
                flag=false;
           end
    else
        flag=false;
    end
%     if CollisionCheck(node, nearestNode, obstacles)
%         connectedNodeIndex = [];
%     else
%         connectedNodeIndex = nearestNodeIndex;
%     end
    
end


% 鏋勫缓璺緞A
function path = ConstructPath(treeA,AendIndex,goal)
    path = [];
    path=[path;goal]
    nodeA=AendIndex;
    while nodeA ~= 0
        path = [treeA(nodeA).node;path];
        nodeA = treeA(nodeA).parent;
    end

end




% 瀵绘壘鏈?繎鑺傜偣
function [nearestNode, nearestNodeIndex] = FindNearestNode(tree, targetNode)
    distances = zeros(size(tree));  % 鍒涘缓涓?釜涓庢爲澶у皬鐩稿悓鐨勯浂鐭╅樀鐢ㄤ簬瀛樺偍璺濈
    for i = 1:numel(tree)
        distances(i) = norm(tree(i).node - targetNode);  % 璁＄畻姣忎釜鑺傜偣涓庣洰鏍囪妭鐐圭殑璺濈
    end
    [~, nearestNodeIndex] = min(distances);
    nearestNode = tree(nearestNodeIndex).node;
end


% 妫?煡绾挎涓庨殰纰嶇墿鐨勪氦鍙?
function collision = CollisionCheck(nodeA, nodeB, obstacles)
    collision = false;
    for i = 1:size(obstacles, 1)  %閬嶅巻闅滅鐗?
        if Intersect(nodeA, nodeB, obstacles(i, :))
            collision = true;
            break;
        end
    end
end

% 鍒ゆ柇绾挎鏄惁鐩镐氦
function intersect = Intersect(nodeA, nodeB, obstacle)
    lineSegment = [nodeA; nodeB];
    obstacleEdges = [
        obstacle(1), obstacle(2), obstacle(1), obstacle(4);
        obstacle(1), obstacle(2), obstacle(3), obstacle(2);
        obstacle(3), obstacle(2), obstacle(3), obstacle(4);
        obstacle(1), obstacle(4), obstacle(3), obstacle(4);
    ];
    
    intersect = false;
    for i = 1:size(obstacleEdges, 1)
        intersect = DoLinesIntersect(lineSegment, obstacleEdges(i, :));
        if intersect
            break;
        end
    end
end

% 鍒ゆ柇绾挎涓庨殰纰嶇墿鐨勪氦鍙?
function intersect = DoLinesIntersect(line1, line2)
    x1 = line1(1, 1);
    y1 = line1(1, 2);
    x2 = line1(2, 1);
    y2 = line1(2, 2);
    
    x3 = line2(1);
    y3 = line2(2);
    x4 = line2(3);
    y4 = line2(4);
    
    intersect = ((x1 - x2) * (y3 - y1) - (y1 - y2) * (x3 - x1)) * ((x1 - x2) * (y4 - y1) - (y1 - y2) * (x4 - x1)) < 0 && ...
                ((x3 - x4) * (y1 - y3) - (y3 - y4) * (x1 - x3)) * ((x3 - x4) * (y2 - y3) - (y3 - y4) * (x2 - x3)) < 0;
end


%閲嶆柊閫夋嫨鐖惰妭鐐?
function [newParent,newcost,potential_son]=reparent(treeA,re_plan_threshold,obstacle)
   potential_parent=[];%[璺濈,绱㈠紩;]
    %閬嶅巻鏁存５鏍戯紝鎵惧埌閲嶉?鐖惰妭鐐瑰湀鍐呰妭鐐?
    for i=1:numel(treeA)
        distance=norm(treeA(i).node - treeA(end).node);
        if distance<=re_plan_threshold
            potential_parent=[potential_parent;distance,i];
        end
    end
    newParent=0;%鍒濆鍊间负0锛屽鏋滆鍑芥暟杩斿洖鍊间负0锛屽垯琛ㄧず鏃犳洿浼樼埗鑺傜偣
    newcost=0;
    min=treeA(end).NP_Cost;
    %閬嶅巻娼滃湪鐖惰妭鐐癸紝鎵惧嚭璺緞浠ｄ环鏈?皬鐨勭埗鑺傜偣
    for i=1:size(potential_parent,1)
        potential_parent_cost=treeA(potential_parent(i,2)).NP_Cost+potential_parent(i,1);
        if potential_parent_cost<min
              if CollisionCheck(treeA(end).node,treeA(potential_parent(i,2)).node,obstacle)==false%纰版挒妫?祴
                 potential_index=i;%鏂扮埗鑺傜偣鐨勭储寮?
                 newParent=potential_parent(i,2);
                 newcost=potential_parent_cost;
              end
        end
    end

    if newcost
        potential_parent(potential_index,:)=[];%灏嗘柊鐨勭埗鑺傜偣鍒犻櫎锛屽墿浣欑殑浣滀负閲嶆柊甯冪嚎鐨勬綔鍦ㄥ効鑺傜偣
    end
    potential_son=potential_parent;
end

%閲嶆柊甯冪嚎
function rewriteTree(treeA,potential_son,obstacle)
    for i=1:size(potential_son,1)
        newcost=treeA(end).NP_Cost+potential_son(i,1);
        potential_index=potential_son(i,2);
        if newcost<treeA(potential_index).NP_Cost %灏嗘柊鑺傜偣浣滀负鐖惰妭鐐硅矾寰勪唬浠锋洿灏忥紝鍙互杩涜閲嶆柊甯冪嚎锛屼絾杩橀渶瑕佽繘琛岀鎾炴娴?
             if CollisionCheck(treeA(end).node,treeA(potential_index).node,obstacle)==false%纰版挒妫?祴
                    lines = findobj('Type', 'line', 'XData', [treeA(potential_index).node(1), treeA(treeA(potential_index).parent).node(1)], 'YData', [treeA(potential_index).node(2), treeA(treeA(potential_index).parent).node(2)]);
                    delete(lines);%鍒犻櫎鏃х嚎
                    treeA(potential_index).parent=numel(treeA);
                    treeA(potential_index).NP_Cost=newcost;
%                     plot([treeA(potential_index).node(1),treeA(end).node(1)],[treeA(potential_index).node(2),treeA(end).node(2)],'g-','LineWidth', 0.8);
             end
        end
    end
end

2、ob1.m

Matlab 复制代码

% %鐢熸垚闅滅鐗╁湴鍥?
function [f,figure_num]=ob1() %杩斿洖鍊糵瀛樺偍闅滅鐗╀俊鎭痆;;;]锛屾瘡涓?涓轰竴涓殰纰嶇墿锛屽垎鍒负鐭╁舰闅滅鐗╁乏涓嬭鍧愭爣锛屽彸涓婅鍧愭爣
    figure_num=figure;
    % 瀹氫箟淇濆瓨灏忔鏂瑰舰鏁扮粍鐨?MAT 鏂囦欢鍚?
    fileName = 'first_mapb.mat';

    % 鍔犺浇灏忔鏂瑰舰鏁扮粍
    load(fileName);

    a=50;b=0.1;%鍦板浘瀹藉害
    grid on;
    for i=1:size(f,1)
         rectangle('Position', [f(i,1), f(i,2), f(i,3)-f(i,1), f(i,4)-f(i,2)], 'FaceColor', 'k');
    end
    axis equal;
    hold on;
    box on;
    xlim([0,a]);ylim([0,a]);
      rectangle('Position', [0, 0, a, b], 'FaceColor', '[0.2 0.2 0.2]');
      rectangle('Position', [0, a-b, a, b], 'FaceColor', '[1 1 1]');
      rectangle('Position', [0, 0, b, a], 'FaceColor', '[1 1 1]');
      rectangle('Position', [a-b, 0, b, a], 'FaceColor', '[1 1 1]');
    axis off;
end

3、distance.m

Matlab 复制代码

% 距离函数
function d = distance(x1, y1, x2, y2)
    d = sqrt((x2 - x1)^2 + (y2 - y1)^2);
end

4、first_map.mat文件表格

第一列

1
2
2
3
3
3
4
4
4
5
5
5
5
5
5
6
6
6
6
6
6
6
6
6
7
7
7
7
7
7
7
7
8
8
9
10
10
10
10
10
10
10
11
11
11
12
12
12
13
13
13
14
14
14
14
14
14
14
15
15
15
15
16
16
16
16
16
16
16
16
17
17
17
17
18
18
18
19
19
19
21
21
21
21
21
21
21
21
22
22
22
22
22
22
22
22
23
23
23
23
24
24
24
24
24
24
24
25
26
26
27
28
29
29
29
29
29
29
29
29
30
30
30
30
30
30
30
30
31
31
31
31
32
32
32
32
32
32
32
32
32
32
32
32
33
33
33
33
34
34
35
35
36
36
36
36
36
36
36
36
36
36
37
37
37
37
37
37
37
37
37
37
38
38
38
38
38
38
38
38
39
39
39
39
39
39
39
39
40
40
40
40
40
40
40
40
41
41
41
41
42
43
43
44
45
45
45
45
46
47
48
49

第二列：

8
37
39
9
26
47
15
32
39
3
7
15
26
37
49
4
8
15
17
18
28
37
39
43
8
14
18
25
34
37
42
49
8
37
39
1
8
17
26
34
38
39
17
37
38
17
18
21
8
27
39
8
11
17
29
41
45
47
6
14
38
39
7
14
18
27
28
31
48
50
3
11
38
47
29
31
48
17
26
34
3
7
9
14
22
27
39
41
5
7
17
2
9
17
27
37
7
8
9
18
8
17
18
21
28
37
39
11
8
17
29
41
4
6
8
9
13
16
18
19
3
5
8
13
14
15
33
44
14
15
33
44
14
18
20
21
22
23
24
27
37
38
43
49
18
19
22
25
28
29
31
47
14
15
22
23
24
25
26
27
28
29
16
18
20
22
23
29
34
39
41
44
14
15
20
21
22
23
32
44
2
5
18
21
27
28
29
30
3
13
23
29
33
34
35
47
3
13
23
33
3
13
23
33
3
15
21
28
22
28
33
28

第三列：

第四列：

实验结果：

RRT_STAR

f =

1 8 1 8

2 37 2 37

2 39 2 39

3 9 3 9

3 26 3 26

3 47 3 47

4 15 4 15

4 32 4 32

4 39 4 39

5 3 5 3

5 7 5 7

5 15 5 15

5 26 5 26

5 37 5 37

5 49 5 49

6 4 6 4

6 8 6 8

6 15 6 15

6 17 6 17

6 18 6 18

6 28 6 28

6 37 6 37

6 39 6 39

6 43 6 43

7 8 7 8

7 14 7 14

7 18 7 18

7 25 7 25

7 34 7 34

7 37 7 37

7 42 7 42

7 49 7 49

8 8 8 8

8 37 8 37

9 39 9 39

10 1 10 1

10 8 10 8

10 17 10 17

10 26 10 26

10 34 10 34

10 38 10 38

10 39 10 39

11 17 11 17

11 37 11 37

11 38 11 38

12 17 12 17

12 18 12 18

12 21 12 21

13 8 13 8

13 27 13 27

13 39 13 39

14 8 14 8

14 11 14 11

14 17 14 17

14 29 14 29

14 41 14 41

14 45 14 45

14 47 14 47

15 6 15 6

15 14 15 14

15 38 15 38

15 39 15 39

16 7 16 7

16 14 16 14

16 18 16 18

16 27 16 27

16 28 16 28

16 31 16 31

16 48 16 48

16 50 16 50

17 3 17 3

17 11 17 11

17 38 17 38

17 47 17 47

18 29 18 29

18 31 18 31

18 48 18 48

19 17 19 17

19 26 19 26

19 34 19 34

21 3 21 3

21 7 21 7

21 9 21 9

21 14 21 14

21 22 21 22

21 27 21 27

21 39 21 39

21 41 21 41

22 5 22 5

22 7 22 7

22 17 22 17

22 2 22 2

22 9 22 9

22 17 22 17

22 27 22 27

22 37 22 37

23 7 23 7

23 8 23 8

23 9 23 9

23 18 23 18

24 8 24 8

24 17 24 17

24 18 24 18

24 21 24 21

24 28 24 28

24 37 24 37

24 39 24 39

25 11 25 11

26 8 26 8

26 17 26 17

27 29 27 29

28 41 28 41

29 4 29 4

29 6 29 6

29 8 29 8

29 9 29 9

29 13 29 13

29 16 29 16

29 18 29 18

29 19 29 19

30 3 30 3

30 5 30 5

30 8 30 8

30 13 30 13

30 14 30 14

30 15 30 15

30 33 30 33

30 44 30 44

31 14 31 14

31 15 31 15

31 33 31 33

31 44 31 44

32 14 32 14

32 18 32 18

32 20 32 20

32 21 32 21

32 22 32 22

32 23 32 23

32 24 32 24

32 27 32 27

32 37 32 37

32 38 32 38

32 43 32 43

32 49 32 49

33 18 33 18

33 19 33 19

33 22 33 22

33 25 33 25

34 28 34 28

34 29 34 29

35 31 35 31

35 47 35 47

36 14 36 14

36 15 36 15

36 22 36 22

36 23 36 23

36 24 36 24

36 25 36 25

36 26 36 26

36 27 36 27

36 28 36 28

36 29 36 29

37 16 37 16

37 18 37 18

37 20 37 20

37 22 37 22

37 23 37 23

37 29 37 29

37 34 37 34

37 39 37 39

37 41 37 41

37 44 37 44

38 14 38 14

38 15 38 15

38 20 38 20

38 21 38 21

38 22 38 22

38 23 38 23

38 32 38 32

38 44 38 44

39 2 39 2

39 5 39 5

39 18 39 18

39 21 39 21

39 27 39 27

39 28 39 28

39 29 39 29

39 30 39 30

40 3 40 3

40 13 40 13

40 23 40 23

40 29 40 29

40 33 40 33

40 34 40 34

40 35 40 35

40 47 40 47

41 3 41 3

41 13 41 13

41 23 41 23

41 33 41 33

42 3 42 3

43 13 43 13

43 23 43 23

44 33 44 33

45 3 45 3

45 15 45 15

45 21 45 21

45 28 45 28

46 22 46 22

47 28 47 28

48 33 48 33

49 28 49 28

figure_num =

Figure (1) - 属性:

Number: 1

Name: ''

Color: [0.9400 0.9400 0.9400]

Position: [488 242 560 420]

Units: 'pixels'

显示所有属性

path =

35 40

>> load('first_mapb.mat')

>> load('first_mapb.mat', 'f')

实验图像：