드론이 복잡한 환경에서 안전하게 비행하려면 정확한 거리 정보를 담은 지도 표현이 필수적이다. 본문에서는 점유 그리드(Occupancy Grid)를 출발점으로 하여 ESDF(Euclidean Signed Distance Field)까지 확장하는 지도 체계를 구현하고, 이를 실제 비행 제어에 활용하는 방법을 다룬다.

1. 시스템 파이프라인 설계

전체 흐름은 다음과 같이 구성된다. 각 단계는 ROS2 노드 단위로 분리되어 비동기 처리된다.

LiDAR/스테레오 카메라 → 포인트클라우드 필터링 → 로컬 그리드 갱신 → 
ESDF 볼륨 업데이트 → 최적 궤적 생성 → 기체 자세 추적 제어

핵심은 지도 표현의 계층화이다. 점유 그리드는 센서 관측을 직접 반영하는 측정 공간(measurement space) 역할을 하고, ESDF는 경로 계획이 직접 활용하는 임베딩 공간(embedding space) 역할을 한다. 두 표현을 명확히 분리하면 모듈 간 결합도를 낮출 수 있다.

2. 점유 그리드의 로그-오즈 구현

센서 노이즈를 다루기 위해 베이지안 갱신 대신 로그-오즈(log-odds) 누적 방식을 사용한다. 각 복셀(voxel)은 스칼라 값 l을 유지하며, 관측 시마다 다음과 같이 갱신된다.

// GridVoxel 구조체
struct Voxel {
    float log_odds;      // 누적된 로그-오즈
    uint8_t hit_count;   // 관측 횟수 (노이즈 필터링용)
    static constexpr float OCCUPIED = 0.85f;
    static constexpr float FREE = 0.15f;
};

void updateVoxel(Voxel& v, bool is_occupied) {
    const float prior = v.log_odds;
    const float sensor_model = is_occupied ? 0.7f : 0.3f;
    
    // 로그-오즈 갱신
    v.log_odds = prior + std::log(sensor_model / (1.0f - sensor_model));
    
    // 수치 안정성 클리핑
    v.log_odds = std::clamp(v.log_odds, -5.0f, 5.0f);
}

여기서 log_odds > 0이면 점유, log_odds < 0이면 자유 공간으로 판단한다. 클리핑(clamping)은 수치 오버플로우 방지와 오래된 관측의 영력 감소(aging) 효과를 동시에 달성한다.

3. ESDF 구축: 거리 변환 최적화

ESDF는 각 위치에서 최근 장애물 표면까지의 유클리드 거리를 저장한다. 양수 거리는 자유 공간 깊이, 음수 거리는 장애물 내부 침투 깊이를 의미한다.

3.1 3D 거리 변환의 계산 복잡도

Naïve하게는 모든 자유 셀-장애물 복셀 쌍을 비교해야 하므로 O(N³ × M³)의 복잡도가 발생한다. 이를 Felzenszwalb-Huttenlocher 알고리즘을 3차원으로 확장하여 O(N³)로 개선한다.

class ESDFUpdater {
    static constexpr int VOXEL_PER_SIDE = 64;
    using Volume = std::array<float, VOXEL_PER_SIDE * VOXEL_PER_SIDE * VOXEL_PER_SIDE>;
    
    Volume esdf_data_;
    Volume temp_x_, temp_y_;  // 중간 결과 버퍼
    
public:
    void computeESDF(const Volume& occupancy) {
        // 1차 통과: x축 방향 1D 거리 변환
        for (int z = 0; z < VOXEL_PER_SIDE; ++z) {
            for (int y = 0; y < VOXEL_PER_SIDE; ++y) {
                sdfPass1D(occupancy, temp_x_, /*axis=*/0, x, y, z);
            }
        }
        
        // 2차 통과: y축 방향 (x 결과를 입력으로)
        for (int z = 0; z < VOXEL_PER_SIDE; ++z) {
            for (int x = 0; x < VOXEL_PER_SIDE; ++x) {
                sdfPass1D(temp_x_, temp_y_, /*axis=*/1, x, y, z);
            }
        }
        
        // 3차 통과: z축 방향 (최종 ESDF)
        for (int y = 0; y < VOXEL_PER_SIDE; ++y) {
            for (int x = 0; x < VOXEL_PER_SIDE; ++x) {
                sdfPass1D(temp_y_, esdf_data_, /*axis=*/2, x, y, z);
            }
        }
    }
    
private:
    void sdfPass1D(const Volume& input, Volume& output, int axis, 
                   int fixed_a, int fixed_b, int line_idx);
};

핵심은 분리 가능한(separable) 필터로 처리함으로써 3D 문제를 세 번의 1D 스캔으로 환원하는 것이다. 각 1D 패스는 동적 프로그래밍으로 구현하며, 현재 위치에서 "최적 전파원"을 후방 탐색하는 방식이다.

3.2 부분 갱신(Partial Update) 전략

실시간 시스템에서 매 프레임 전체 ESDF를 재계산하면 10ms 제약을 만족하기 어렵다. 변경 영역(bounding box of change)만 선택적으로 갱신하는 방식을 채택한다.

struct ChangeRegion {
    Eigen::Vector3i min_corner, max_corner;
    
    bool intersects(const ChangeRegion& other) const {
        return (min_corner.array() <= other.max_corner.array()).all() &&
               (max_corner.array() >= other.min_corner.array()).all();
    }
};

void ESDFUpdater::updateRegion(const ChangeRegion& region, 
                               const VoxelMap& latest_occupancy) {
    // AABB 확장: 거리 영향 범위 고려
    constexpr int INFLATION_RADIUS_VOXEL = 8;
    ChangeRegion affected = region;
    affected.min_corner -= Eigen::Vector3i::Ones() * INFLATION_RADIUS_VOXEL;
    affected.max_corner += Eigen::Vector3i::Ones() * INFLATION_RADIUS_VOXEL;
    
    // 영향받는 영역만 재계산
    parallelFor(affected, [this, &latest_occupancy](const Eigen::Vector3i& idx) {
        recomputeDistanceAt(latest_occupancy, idx);
    });
}

4. 그래디언트 기반 궤적 최적화

ESDF의 핵심 이점은 거리 그래디언트를 통한 부드러운 경로 최적화가 가능하다는 점이다. 목적 함수는 다음과 같이 구성된다.

Eigen::Vector3d computeGradient(const Eigen::Vector3d& position) {
    // 삼선형 보간으로 연속 공간 그래디언트 획득
    Eigen::Vector3d voxel_coord = (position - origin_) * inv_resolution_;
    
    float dx, dy, dz;
    // x 방향 중심 차분
    dx = (interpolate(voxel_coord + Eigen::Vector3d(1,0,0)) -
          interpolate(voxel_coord - Eigen::Vector3d(1,0,0))) * 0.5f * inv_resolution_;
    // y, z 동일...
    
    return Eigen::Vector3d(dx, dy, dz);
}

// CHOMP 스타일 최적화
void optimizeTrajectory(std::vector<Eigen::Vector3d>& waypoints) {
    const double lambda_smooth = 0.5;
    const double lambda_obs = 10.0;
    const double safety_margin = 0.3;  // 30cm 안전 여유
    
    for (int iter = 0; iter < 50; ++iter) {
        for (size_t i = 1; i < waypoints.size() - 1; ++i) {
            Eigen::Vector3d grad_smooth = waypoints[i-1] - 2*waypoints[i] + waypoints[i+1];
            
            double dist = interpolate(waypoints[i]);
            Eigen::Vector3d grad_esdf = computeGradient(waypoints[i]);
            
            // 장애물 비용: safety_margin 이내로 들어오면 페널티
            double obs_cost = std::max(0.0, safety_margin - dist);
            
            waypoints[i] += 0.01 * (
                -lambda_smooth * grad_smooth + 
                lambda_obs * obs_cost * obs_cost * grad_esdf.normalized()
            );
        }
    }
}

위 최적화는 CHOMP(Covariant Hamiltonian Optimization for Motion Planning)의 핵심 아이디어를 간소화한 것이다. ESDF의 부드러운 거리 필드 덕분에 기울기 기반 방법이 안정적으로 수렴한다.

5. 하드웨어 동기화 및 지연 보상

실제 비행에서 가장 어려운 부분은 센서-제어 지연(latency) 처리이다. LiDAR 스캔 시점과 모터 명령 적용 시점 사이에 50-100ms 지연이 일반적이다.

class TimestampedPose {
public:
    Eigen::Vector3d position;
    Eigen::Quaterniond orientation;
    rclcpp::Time stamp;
};

class LatencyCompensator {
    std::deque<TimestampedPose> pose_history_;
    
public:
    Eigen::Vector3d predictPosition(const rclcpp::Time& target_time) {
        // 선형 보간 (고속 회전 시 SLERP 권장)
        auto it = std::lower_bound(pose_history_.begin(), pose_history_.end(), target_time,
            [](const TimestampedPose& p, const rclcpp::Time& t) {
                return p.stamp < t;
            });
        
        if (it == pose_history_.end()) return pose_history_.back().position;
        if (it == pose_history_.begin()) return pose_history_.front().position;
        
        auto prev = std::prev(it);
        double alpha = (target_time - prev->stamp).seconds() / 
                       (it->stamp - prev->stamp).seconds();
        
        return (1.0 - alpha) * prev->position + alpha * it->position;
    }
};

ESDF 지도를 조회할 때는 현재 시점이 아닌 미래 예측 시점의 기체 위치를 기준으로 한다. 이를 통해 지연으로 인한 장애물 충돌 위험을 효과적으로 감소시킨다.

6. 파라미터 튜닝 가이드

특히 collision_lookahead_time은 기체 속도에 따라 동적으로 조정하는 것이 바람직하다. 고속 비행 시에는 더 긴 예측 시간이 필요하지만, 이는 ESDF의 유효 위와도 연관되어 있어 주의가 필요하다.