双向RRT融合DWA算法无人船路径规划(附MATLAB源代码)

路径规划666

已于 2025-02-02 20:33:18 修改

阅读量1.3k

点赞数 24

分类专栏：路径规划文章标签： python 算法开发语言

于 2025-02-02 19:12:29 首次发布

本文链接：https://blog.csdn.net/ljghz/article/details/145421731

版权

路径规划专栏收录该内容

12 篇文章

订阅专栏

RRT+DWA

在无人水面船（USV）路径规划问题中，如何在复杂且动态的海洋环境中确保USV的路径既安全又高效是一个关键挑战。本文提出了一种改进的双向RRT（Bi-RRT）算法与动态窗口法（DWA）融合的混合路径规划算法——GIBi-RRT-IDWA算法。该算法结合了全局路径规划和局部动态避障的优势，能够应对复杂的海洋环境变化并有效避开障碍物。

在本文的博客中，我们将详细解释该算法的实现，包括其各个部分的代码设计和关键技术的解读。通过这些代码和方法，我们能够理解如何实现该算法并将其应用于USV的路径规划中。

1. Bi-RRT算法概述

Bi-RRT是一种基于采样的路径规划算法，通过构建两棵树（从起点和终点分别开始）并交叉搜索来寻找最短路径。它有两个优点：

快速搜索：通过双向扩展树，减少了搜索时间。
高效扩展：比传统的RRT算法更加高效，能够快速找到一条连接起点和终点的路径。

然而，传统的Bi-RRT算法存在一些缺陷，尤其是在海洋环境中，路径规划时的节点较多，路径拐角尖锐，不适合船舶操作。因此，我们对Bi-RRT进行了改进，使其在路径节点上更加平滑，并能适应船舶的操纵需求。

1. Bi-RRT算法概述（MATLAB版）

Bi-RRT扩展与冗余节点剔除

classdef BiRRT
    properties
        start
        goal
        map
        tree_start
        tree_goal
    end
    
    methods
        % 构造函数
        function obj = BiRRT(start, goal, map)
            obj.start = start;
            obj.goal = goal;
            obj.map = map;
            obj.tree_start = start;
            obj.tree_goal = goal;
        end
        
        % 扩展树的节点
        function new_node = extend(obj, tree, target)
            new_node = obj.sample(target);
            if obj.is_valid(new_node)
                tree = [tree; new_node];  % 添加到树中
            else
                new_node = [];  % 无效节点
            end
        end
        
        % 判断节点是否有效
        function valid = is_valid(obj, node)
            valid = ~obj.map.is_obstacle(node);
        end
        
        % 剔除冗余节点
        function pruned_tree = prune_path(obj, tree)
            pruned_tree = [];
            for i = 1:length(tree)
                node = tree(i, :);
                if ~obj.is_redundant(node, pruned_tree)
                    pruned_tree = [pruned_tree; node];  % 保留非冗余节点
                end
            end
        end
        
        % 判断节点是否冗余
        function redundant = is_redundant(obj, node, tree)
            redundant = any(vecnorm(tree - node, 2, 2) < 1e-3);  % 距离小于阈值
        end
        
        % 计算节点之间的距离
        function dist = distance(~, node1, node2)
            dist = norm(node1 - node2);
        end
    end
end

2. DWA局部路径规划（MATLAB版）

DWA评价函数的改进

classdef DWA
    properties
        usv
        goal
        map
        wind
        current
    end
    
    methods
        function obj = DWA(usv, goal, map, wind, current)
            obj.usv = usv;
            obj.goal = goal;
            obj.map = map;
            obj.wind = wind;
            obj.current = current;
        end
        
        % 评价当前速度和角度的代价
        function cost = evaluate(obj, velocity, angle)
            cost = obj.calculate_cost(velocity, angle);
            if obj.is_collision(velocity, angle)
                cost = cost + 1000;  % 碰撞时代价很高
            end
        end
        
        % 计算引导代价
        function cost = calculate_cost(obj, velocity, angle)
            heuristic_cost = norm(obj.usv.position - obj.goal);
            dynamic_cost = obj.wind_impact(velocity) + obj.current_impact(angle);
            cost = heuristic_cost + dynamic_cost;
        end
        
        % 风的影响
        function cost = wind_impact(obj, velocity)
            cost = dot(obj.wind, velocity);
        end
        
        % 海流的影响
        function cost = current_impact(obj, angle)
            cost = dot(obj.current, [cos(angle), sin(angle)]);
        end
        
        % 判断是否发生碰撞
        function collision = is_collision(obj, velocity, angle)
            new_position = obj.usv.predict_next_position(velocity, angle);
            collision = obj.map.is_obstacle(new_position);
        end
    end
end

3. GIBi-RRT-IDWA算法（MATLAB版）

将改进的Bi-RRT算法与DWA算法结合，实现GIBi-RRT-IDWA混合路径规划。

classdef GIBiRRTIDWA
    properties
        bi_rrt
        dwa
        usv
    end
    
    methods
        function obj = GIBiRRTIDWA(start, goal, map, wind, current)
            obj.bi_rrt = BiRRT(start, goal, map);
            obj.dwa = DWA(obj.usv, goal, map, wind, current);
        end
        
        % 路径规划主函数
        function path = plan(obj)
            % 执行全局路径规划
            global_path = obj.bi_rrt.plan();
            
            % 局部路径规划：动态调整路径
            for i = 1:size(global_path, 1)
                point = global_path(i, :);
                [velocity, angle] = obj.dwa.local_planning(point);
                % 更新USV位置
                obj.usv.move(velocity, angle);
            end
            path = global_path;  % 返回全局路径
        end
    end
end