matlab智能体的自动目标搜索 + 自动避障路径规划

matlab智能体的自动目标搜索 + 自动避障路径规划

• 已知地图 / 稀疏障碍 → A*（全局） + DWA（局部避障）
• 未知 / 部分已知 → Frontier 探索（自动搜目标）+ A* 回航
• 高维 / 动态 → RRT*（备选）

一、问题建模（先对齐）

1、环境表示

% 栅格地图：0=可行，1=障碍
map = zeros(50,50);
% 随机 obstacle
map(10:15, 20:25) = 1;
map(30:35, 10:18) = 1;
map(40, 5:45) = 1;

• 智能体位置：[x, y]
• 目标位置：[x_g, y_g]
• 传感器：雷达/深度，返回局部占用（未知环境用）

2、智能体运动模型（差分小车）

$$\begin{array}{rl}{x}_{k+1}& =x_k+v\Delta t\cos {\theta }_k\\ y_{k+1}& =y_k+v\Delta t\sin {\theta }_k\\ {\theta }_{k+1}& ={\theta }_k+\omega \Delta t\end{array}$$

二、A* 全局路径规划（已知地图避障）

核心代码：astar.m

%% astar.m — A* 栅格规划
function path = astar(map, start, goal)
% map: 0可行 1障碍, start/goal = [row, col]

[m, n] = size(map);
% 启发函数：曼哈顿 + 欧氏混合
h = @(r,c) abs(r-goal(1)) + abs(c-goal(2)) + ...
            0.3*sqrt((r-goal(1))^2+(c-goal(2))^2);

% 8 方向移动
dirs = [-1 0; 1 0; 0 -1; 0 1; -1 -1; -1 1; 1 -1; 1 1];
cost_m = [1 1 1 1 1.414 1.414 1.414 1.414];

% 初始化
g = inf(m,n); g(start(1),start(2)) = 0;
parent = zeros(m,n,2);
openList = start;

while ~isempty(openList)
    % 取 f 最小
    [~, idx] = min(g(sub2ind([m,n], openList(:,1), openList(:,2))) ...
                 + h(openList(:,1), openList(:,2)));
    cur = openList(idx,:);
    openList(idx,:) = [];

    if isequal(cur, goal), break; end

    % 拓展邻域
    for d = 1:8
        nr = cur(1)+dirs(d,1); nc = cur(2)+dirs(d,2);
        if nr<1 || nr>m || nc<1 || nc>n, continue; end
        if map(nr,nc)==1, continue; end
        % 对角穿墙检查
        if abs(dirs(d,1))+abs(dirs(d,2))==2
            if map(cur(1)+dirs(d,1), cur(2))==1 && map(cur(1), cur(2)+dirs(d,2))==1
                continue;
            end
        end
        tg = g(cur(1),cur(2)) + cost_m(d);
        if tg < g(nr,nc)
            g(nr,nc) = tg;
            parent(nr,nc,:) = [nr-cur(1), nc-cur(2)];
            if ~ismember([nr,nc], openList, 'rows')
                openList = [openList; nr nc];
            end
        end
    end
end

% 回溯
path = goal;
while ~isequal(path(1,:), start)
    d = squeeze(parent(path(1,1), path(1,2), :));
    next = path(1,:) - d;
    path = [next; path];
end
end

三、DWA 局部避障（动态障碍 / 机器人本体）

DWA（Dynamic Window Approach）是在速度空间采样 + 评价函数打分，自动避开突发障碍，配合 A* 全局路径用是工业标配（ROS dwa_local_planner 就是这个）。

简化版 DWA：dwa_planner.m

%% dwa_planner.m — 局部避障速度决策
function [v_sel, w_sel, traj] = dwa_planner(...
    pos, goal, v, w, v_prev, w_prev, map, params)
% pos = [x,y,theta], goal = [x_g,y_g]
% v ∈ [0, v_max], w ∈ [-w_max, w_max]

persistent dt; if isempty(dt), dt = 0.1; end

% 速度窗口
v_samples = v_prev + linspace(-params.a_lin*dt, params.a_lin*dt, 5);
w_samples = w_prev + linspace(-params.a_ang*dt, params.a_ang*dt, 7);
v_samples = v_samples(v_samples>=0 & v_samples<=params.v_max);
w_samples = w_samples(abs(w_samples)<=params.w_max);

best_score = -inf;
v_sel = v_prev; w_sel = w_prev;
traj = [];

for vi = v_samples
    for wi = w_samples
        % 预测轨迹（N=10 步）
        pt = pos;
        dist_to_goal = inf;
        collide = false;
        pred = pt;
        for k = 1:10
            pt = [pt(1)+vi*dt*cos(pt(3)), ...
                  pt(2)+vi*dt*sin(pt(3)), ...
                  pt(3)+wi*dt];
            pred = [pred; pt];
            % 碰撞检查（栅格膨胀机器人半径）
            ri = round(pt(1)/params.res + 1);
            ci = round(pt(2)/params.res + 1);
            if ri<1||ci<1||ri>size(map,1)||ci>size(map,2) || map(ri,ci)==1
                collide = true; break;
            end
            if k==10, dist_to_goal = norm(pt(1:2)-goal(1:2)); end
        end
        if collide, continue; end

        % 评价函数（加权）
        score_heading = -abs(wrapToPi(atan2(goal(2)-pt(2),goal(1)-pt(1)) - pt(3)));
        score_dist   = -dist_to_goal;
        score_vel    = vi;
        score = 0.1*score_heading + 0.6*score_dist + 0.3*score_vel;

        if score > best_score
            best_score = score;
            v_sel = vi; w_sel = wi;
            traj = pred;
        end
    end
end
end

参数结构体：

params.v_max = 1.5; params.w_max = pi/2;
params.a_lin = 0.8; params.a_ang = 1.2;
params.res = 0.2;   % 栅格分辨率 m

四、未知环境「自动目标搜索」—— Frontier 探索

如果目标是未知位置（比如"屋里找人"），A* 用不了，得用 Frontier-based 探索：不断去"已知/未知交界"扫，直到目标出现在传感器视野。

核心逻辑

%% frontier_search.m — 自动目标搜索主循环
function [traj, found] = frontier_search(map_true, map_known, robot, goal_true, params)
% map_true: 真实地图（上帝视角，仅评测用）
% map_known: 智能体当前建图
% robot = [x,y,theta]

maxSteps = 500;
traj = robot(1:2);
found = false;

for step = 1:maxSteps
    % 1. 传感器更新（雷达 5m 半径）
    map_known = update_map(robot, map_true, map_known, 5, params.res);

    % 2. 检查目标是否在视野
    if map_true(round(goal_true(1)/params.res)+1, round(goal_true(2)/params.res)+1)==0
        d = norm(robot(1:2) - goal_true);
        if d < 5  % 看到目标
            found = true;
            fprintf('目标发现于 step %d!\n', step);
            break;
        end
    end

    % 3. 提取 frontier（已知自由格 邻 未知格）
    front = extract_frontier(map_known);

    if isempty(front)
        fprintf('无 frontier，搜索失败\n'); break;
    end

    % 4. 选最近 frontier 作 subgoal
    dists = vecnorm(front' - robot(1:2), 2, 1);
    [~, idx] = min(dists);
    subgoal = front(:,idx);

    % 5. A* 到 subgoal（已知部分）
    start_rc = [round(robot(1)/params.res)+1, round(robot(2)/params.res)+1];
    goal_rc  = [round(subgoal(1)/params.res)+1, round(subgoal(2)/params.res)+1];
    path_rc = astar(map_known, start_rc, goal_rc);

    % 6. DWA 跟踪
    for k = 1:min(5,size(path_rc,1)-1)
        next_rc = path_rc(k+1,:);
        next_xy = [(next_rc(2)-1)*params.res, (next_rc(1)-1)*params.res];
        [v,w,traj_loc] = dwa_planner(robot, next_xy, 0.8, 0, 0.8, 0, map_known, params);
        robot(1:2) = traj_loc(end,1:2);
        robot(3)   = traj_loc(end,3);
        traj = [traj; robot(1:2)];
    end
end
end

Frontier 提取（关键）

function front = extract_frontier(map)
% map: 0=自由 0.5=未知 1=障碍
[fy, fx] = find(map==0);
front = [];
for i = 1:length(fx)
    % 3×3 邻域是否有未知
    r0=fy(i); c0=fx(i);
    patch = map(max(1,r0-1):min(end,r0+1), max(1,c0-1):min(end,c0+1));
    if any(patch(:)==0.5)
        front = [front, [c0*0.2, (size(map,1)-r0)*0.2]]; % 转回世界坐标示意
    end
end
end

💡 多智能体协同搜索时，frontier 评分可加 信息熵 / 距离 / 队友覆盖度 惩罚，避免扎堆。

五、主脚本：串起来跑一遍

%% main_agent_planning.m
clear; clc; close all;

% 地图
map = zeros(60,60);
map(10:18, 25:35) = 1;
map(35:45, 10:20) = 1;
map(50, 5:55) = 1;

% 智能体 & 目标
robot0 = [2, 2, 0];           % [x,y,θ] (m)
goal_true = [10, 10];         % 目标真实位置（未知环境下才隐藏）

params.res = 0.2;
params.v_max = 1.2; params.w_max = pi/2;

% ===== 场景 A：已知地图 → A* + DWA =====
start_rc = [11, 11]; goal_rc = [51, 51]; % 栅格坐标
path = astar(map, start_rc, goal_rc);

figure('Color','white')
imagesc(map'); axis equal; colormap(gray); hold on
plot(path(:,2), path(:,1), 'r-o', 'LineWidth',1.5)
plot(start_rc(2), start_rc(1), 'go', 'MarkerSize',10,'LineWidth',2)
plot(goal_rc(2), goal_rc(1), 'b*', 'MarkerSize',12,'LineWidth',2)
title('A* Global Path'); xlabel('col'); ylabel('row')

% ===== 场景 B：未知 → Frontier 搜索 =====
map_known = 0.5*ones(size(map));  % 全未知
map_known(robot0(1)/params.res+1, robot0(2)/params.res+1) = 0; % 初始位自由

[traj_found, ok] = frontier_search(map, map_known, robot0, goal_true, params);

figure('Color','white')
imagesc(map'); axis equal; colormap(gray); hold on
plot(traj_found(:,1)/params.res, size(map,1)-traj_found(:,2)/params.res, 'r-','LineWidth',1.2)
title('Frontier 自动搜索轨迹'); 

参考代码 用于智能体自动目标搜索或自动避障路径规划 www.youwenfan.com/contentcsw/82688.html

六、算法选型速查