基于A*算法的无人机三维动态避障路径规划及MATLAB实现

本文介绍了基于A算法的无人机三维动态避障路径规划方法，并使用MATLAB进行编程实现。内容包括三维障碍物的生成与可视化、A节点类的设计（包含坐标、代价函数）、路径搜索主循环中的碰撞检测与障碍物动态更新机制、以及路径结果的三维展示。此外，还探讨了邻居节点生成策略的优化以减少算力消耗，并展示了通过回调函数实现交互式障碍物拖拽的功能。

首先，生成三维地图障碍物数据：

% 生成三维障碍立方体
obstacles = [20,30, 5,15, 0,10; 
             50,60,25,35,15,25; 
             80,90, 8,18,30,40 ];
% 可视化障碍物
hold on;
for i=1:size(obstacles,1)
    plotcube([obstacles(i,2)-obstacles(i,1),... 
              obstacles(i,4)-obstacles(i,3),... 
              obstacles(i,6)-obstacles(i,5)],...
             [obstacles(i,1) obstacles(i,3) obstacles(i,5)],...
             0.5,[1 0 0]);
end

该代码定义了三个三维立方体障碍物。plotcube函数参数分别是长宽高、起始坐标、透明度和颜色。想自定义障碍物？直接修改obstacles矩阵即可，每行六个数字分别表示X起止、Y起止、Z起止范围。

核心在于定义A*算法的节点结构：

classdef Node < handle
    properties
        x; y; z;
        gCost; hCost;
        parent;
    end
    methods
        function obj = Node(x,y,z)
            obj.x = x;
            obj.y = y;
            obj.z = z;
        end
        function fCost = getFCost(obj)
            fCost = obj.gCost + obj.hCost;
        end
    end
end

这里用面向对象搞了个三维节点，每个节点存着三维坐标、实际代价gCost（已走路程）、预估代价hCost（到终点的直线距离）。重点是这个getFCost方法，A*选路就靠fCost=g+h这个值来决策，相当于导航系统的"预计到达时间"。

路径搜索主循环里有段关键代码：

while ~isempty(openList)
    current = openList(1); % 找fCost最小的节点
    for i = 1:length(openList)
        if openList(i).getFCost() < current.getFCost() || ...
           (openList(i).getFCost() == current.getFCost() && ...
            openList(i).hCost < current.hCost)
            current = openList(i);
        end
    end
    % 遇到障碍物就动态更新
    if checkCollision(current, obstacles)
        updateObstacles(); % 动态更新障碍物位置
        continue;
    end
    % 此处省略邻居节点生成代码...
end

这个checkCollision函数暗藏玄机——它实时检测当前节点是否与新出现的障碍物碰撞。一旦发现危险，立即调用updateObstacles更新障碍物信息，相当于给无人机装了急刹系统。注意这里continue直接跳过当前节点，算法会自动寻找新路径。

来看看动态避障的碰撞检测：

function collision = checkCollision(node, obstacles)
    safetyMargin = 3; % 安全距离
    for i = 1:size(obstacles,1)
        if node.x >= (obstacles(i,1)-safetyMargin) && ...
           node.x <= (obstacles(i,2)+safetyMargin) && ...
           node.y >= (obstacles(i,3)-safetyMargin) && ...
           node.y <= (obstacles(i,4)+safetyMargin) && ...
           node.z >= (obstacles(i,5)-safetyMargin) && ...
           node.z <= (obstacles(i,6)+safetyMargin)
            collision = true;
            return;
        end
    end
    collision = false;
end

这里的安全距离参数就像无人机的"怕死程度"，设置3米意味着离障碍物3米开外就判定危险。实际飞行时可以配合传感器数据动态调整这个值，雨天就调大点，晴朗天调小点。

跑完算法后，用这个三维可视化代码展示成果：

path = reconstructPath(current);
plot3(path(:,1), path(:,2), path(:,3), 'b-o', 'LineWidth',2);
plot3(start(1),start(2),start(3),'gp','MarkerSize',15);
plot3(goal(1),goal(2),goal(3),'rh','MarkerSize',15);
view(3);
axis equal;
grid on;

蓝色折线就是规划的路径，绿色五角星是起点，红色六角星是终点。按住图形旋转按钮可以看到三维路径如何巧妙绕过障碍物。

想要路径更丝滑？在A*的邻居生成策略里动手脚：

neighborSteps = [ -1,-1,-1; 0,-1,-1; 1,-1,-1; -1,0,-1; 0,0,-1; 1,0,-1; ... 
                  % 此处省略其他25种组合 
                  1, 1, 1 ]; % 总共26个方向
stepSize = 5; % 搜索步长
neighbors = [];
for i = 1:size(neighborSteps,1)
    newX = currentNode.x + neighborSteps(i,1)*stepSize;
    newY = currentNode.y + neighborSteps(i,2)*stepSize;
    newZ = currentNode.z + neighborSteps(i,3)*stepSize;
    % 边界检查...
end

这个邻居生成策略允许无人机斜着飞，甚至上下盘旋。但26个方向全遍历太费算力，实战中可以精简为14个主要方向，既保持路径灵活性又不拖慢速度。

最后在MATLAB命令行这么调用：

% 设置起点终点
startPos = [5, 5, 5];
goalPos = [95, 95, 95];
% 运行A*路径规划
[path, obs] = astar3D(startPos, goalPos, obstacles);
% 动态演示
animateDrone(path, obs);

运行后会看到无人机像跳障碍赛一样，在三维障碍物之间穿梭。按R键可以随机生成新障碍物，算法会立即重新规划路线。

这套系统有个彩蛋功能——在算法运行中，直接拖动MATLAB图窗里的障碍物，路径会实时更新。这得益于MATLAB的图形对象回调机制，实现了真正意义上的动态交互。想体验这个功能？记得在代码里加上set(gcf,'WindowButtonDownFcn',@dragObstacle)之类的回调函数。