UAV(무인 항공기)의 3차원 공간 경로 계획은 복잡한 환경에서 안정적인 비행을 보장하는 핵심 기술입니다. A* 알고리즘은 휴리스틱 평가를 통해 최적의 경로를 효율적으로 탐색하며, 여기에 동적 장애물 회피 로직을 결합하면 실시간 환경 변화에 대응하는 자율 비행 시스템을 구축할 수 있습니다. 본 글에서는 MATLAB을 활용하여 3차원 A* 경로 계획 및 동적 충돌 회피 시스템을 구현하는 기술적 접근 방식을 다룹니다.

3차원 공간 모델링 및 장애물 구성

3차원 환경을 구성하기 위해 직육면체 형태의 장애물 영역을 정의합니다. 각 장애물은 X, Y, Z 축의 최소 및 최대 좌표값으로 표현되며, 시각화를 위해 반투명 큐브로 렌더링됩니다.

% 3차원 공간 내 장애물 영역 정의 (X, Y, Z의 최소/최대값)
obs_bounds = [
    15, 25, 10, 20, 5, 15; 
    45, 55, 30, 40, 20, 30;
    75, 85, 12, 22, 35, 45
]; 

figure; hold on;
for idx = 1:size(obs_bounds, 1)
    dims = [obs_bounds(idx,2)-obs_bounds(idx,1), ...
            obs_bounds(idx,4)-obs_bounds(idx,3), ...
            obs_bounds(idx,6)-obs_bounds(idx,5)];
    origin = [obs_bounds(idx,1), obs_bounds(idx,3), obs_bounds(idx,5)];
    
    % 사용자 정의 plotcube 함수를 활용한 큐브 렌더링
    plotcube(dims, origin, 0.6, [0.8 0.2 0.2]); 
end

A* 알고리즘을 위한 노드 자료구조 설계

3차원 공간에서의 경로 탐색을 위해 객체 지향 방식을 사용하여 노드 클래스를 설계합니다. 각 노드는 3차원 좌표, 시작점부터의 실제 비용(G), 목표점까지의 예상 비용(H), 그리고 경로 추적을 위한 부모 노드 참조를 포함합니다.

classdef SpatialNode < handle
    properties
        coord; % [x, y, z] 좌표 벡터
        g_val; % 시작점부터의 실제 누적 비용
        h_val; % 목표점까지의 휴리스틱 비용
        parent_node; % 경로 재구성을 위한 부모 노드
    end
    methods
        function obj = SpatialNode(x, y, z)
            obj.coord = [x, y, z];
            obj.g_val = inf;
            obj.h_val = inf;
            obj.parent_node = [];
        end
        function f = get_f_val(obj)
            f = obj.g_val + obj.h_val;
        end
    end
end

핵심 탐색 로직과 동적 환경 업데이트

열린 목록(Open List)에서 F값이 가장 작은 노드를 추출하여 탐색을 진행합니다. 이 과정에서 현재 노드가 동적으로 생성되거나 이동한 장애물과 충돌하는지 평가하며, 충돌 발생 시 환경 데이터를 갱신하고 대체 경로를 탐색합니다.

while ~isempty(open_set)
    % F값이 가장 작은 노드 추출 (우선순위 큐 대체 로직)
    f_scores = arrayfun(@(n) n.get_f_val(), open_set);
    [~, min_idx] = min(f_scores);
    current_node = open_set(min_idx);
    
    % 동적 장애물 충돌 시 환경 업데이트 및 재탐색
    if detect_collision(current_node.coord, obs_bounds)
        obs_bounds = refresh_environment(obs_bounds);
        open_set(min_idx) = [];
        continue;
    end
    
    % 목표점 도달 여부 확인 및 이웃 노드 확장 로직 수행
    % ...
end

충돌 감지 및 안전 마진 계산

UAV의 물리적 크기와 센서 오차를 고려하여 안전 마진을 적용한 충돌 감지 함수를 구현합니다. 이 마진 값은 비행 환경이나 기상 조건에 따라 동적으로 조절될 수 있습니다.

function is_hit = detect_collision(pos, obstacles)
    safe_dist = 2.5; % 충돌 방지를 위한 안전 마진 (미터)
    is_hit = false;
    
    for k = 1:size(obstacles, 1)
        if pos(1) >= (obstacles(k,1) - safe_dist) && pos(1) <= (obstacles(k,2) + safe_dist) && ...
           pos(2) >= (obstacles(k,3) - safe_dist) && pos(2) <= (obstacles(k,4) + safe_dist) && ...
           pos(3) >= (obstacles(k,5) - safe_dist) && pos(3) <= (obstacles(k,6) + safe_dist)
            is_hit = true;
            return;
        end
    end
end

3차원 이웃 노드 확장 전략

3차원 공간에서는 최대 26개의 이동 방향을 고려할 수 있습니다. `ndgrid`를 활용하여 방향 벡터를 생성하고, 불필요한 정지 상태(0,0,0)를 제거하여 이웃 노드를 확장합니다.

% 3차원 26방향 이동 벡터 생성
[x_dir, y_dir, z_dir] = ndgrid(-1:1, -1:1, -1:1);
directions = [x_dir(:), y_dir(:), z_dir(:)];
directions(14, :) = []; % 중심점(0,0,0) 제거

move_step = 4.0; % 기본 탐색 보폭
for d = 1:size(directions, 1)
    next_pos = current_node.coord + directions(d, :) * move_step;
    
    % 공간 경계 검사 및 유효한 노드일 경우 열린 목록에 추가
    if is_within_bounds(next_pos)
        % 노드 생성 및 비용 계산...
    end
end

경로 시각화 및 시뮬레이션 실행

탐색이 완료되면 부모 노드 링크를 통해 최종 경로를 역추적하고, 3차원 그래픽스를 사용하여 시작점, 목표점, 최적 경로 및 장애물을 시각화합니다.

% 메인 실행 스크립트
start_pt = [10, 10, 10]; 
end_pt = [90, 90, 90];
[opt_path, final_obs] = run_a_star_3d(start_pt, end_pt, obs_bounds);

% 경로 추출 및 3D 시각화
final_path = extract_path(opt_path);
plot3(final_path(:,1), final_path(:,2), final_path(:,3), 'k-', 'LineWidth', 2.5);
plot3(start_pt(1), start_pt(2), start_pt(3), 'go', 'MarkerSize', 10, 'MarkerFaceColor', 'g');
plot3(end_pt(1), end_pt(2), end_pt(3), 'rs', 'MarkerSize', 10, 'MarkerFaceColor', 'r');

view(3); axis equal; grid on;

MATLAB의 그래픽스 콜백 메커니즘을 활용하면 시뮬레이션 중 사용자가 마우스로 장애물의 위치를 드래그할 때 경로를 실시간으로 재계산하는 상호형 환경을 구축할 수 있습니다. 이를 위해 Figure 객체의 `WindowButtonDownFcn` 및 `WindowButtonMotionFcn` 속성에 장애물 좌표 업데이트 및 A* 알고리즘 재실행을 트리거하는 콜백 함수를 할당합니다.