30.5.6. 충돌 회피 시스템(Obstacle Avoidance) 개입 시 벡터장 히스토그램(Vector Field Histogram, VFH) 기반 우회 경로 세그먼트 생성 및 `safepoint` 배열 병합

30.5.6. 충돌 회피 시스템(Obstacle Avoidance) 개입 시 벡터장 히스토그램(Vector Field Histogram, VFH) 기반 우회 경로 세그먼트 생성 및 safepoint 배열 병합

비상 상황에서 발동되는 RTL(Return To Launch) 궤적은 원칙적으로 목적지를 향해 직선으로 그어진다. 그러나 도심지나 산림 지역과 같이 동적인 장애물(크레인, 건물, 수목 등)이 존재하는 환경에서 직선 궤적만을 고집하면 치명적인 충돌 사고로 이어질 수 있다.

PX4-Autopilot은 navigator 계층 위에 보조 컴퓨터(Companion Computer, ROS2) 또는 기체 자체에 내장된 장애물 회피(Obstacle Avoidance, OA) 모듈을 연동할 수 있는 표준 MAVLink 인터페이스(TRAJECTORY_REPRESENTATION_WAYPOINTS)를 제공한다. 본 장에서는 RTL 모드 중에 전방 구조물이 식별되었을 때, 궤적 생성기가 벡터장 히스토그램(Vector Field Histogram, VFH) 모델을 바탕으로 어떻게 스스로 우회 경로 세그먼트를 생성하고 이를 기존 RTL 경로(Safepoint 배열)에 동적으로 병합하는지 그 알고리즘을 분석한다.

1. 장애물 회피 시스템의 개입 아키텍처 (PX4-Avoidance)

PX4의 충돌 회피 로직은 네비게이터가 직접 점군(Point Cloud) 센서 데이터를 씹어먹지 않고, 역할을 분리(Decoupling)하여 처리한다.

  • 네비게이터(rtl.cpp) 측: 기체가 현재 위치에서 가야 할 ’이상적인 다음 목표(Target Setpoint)’를 계산하여 ROS2(컴패니언 컴퓨터) 기반의 회피 모듈로 지속적으로 퍼블리싱한다.
  • 회피 모듈(Avoidance Node) 측: VDM(VO, LiDAR, Realsense 카메라 등) 센서가 수집한 3D 장애물 맵(OctoMap 등)을 분석하여, 네비게이터가 지시한 목표로 향하는 직선 상에 장애물이 있는지 검사한다. 장애물이 있다면 이를 우회하는 안전한 로컬 경로점(Local Waypoints) 배열을 생성하여 다시 PX4(비행 제어기)로 피드백한다.

PX4 FlightTaskAuto는 이 외부 컴패니언 컴퓨터로부터 들어오는 개입 경로와 원래의 RTL 목표 간의 동기화를 수행한다.

2. 벡터장 히스토그램 (VFH+) 기반의 로컬 궤적 탐색

회피 모듈(일반적으로 px4/avoidance 저장소 기반)이 장애물을 피하기 위해 사용하는 핵심 수학적 필터가 벡터장 히스토그램(Vector Field Histogram, VFH) 확장 모델이다.

  1. 2D 극좌표계 매핑 (Polar Histogram): 기체 주변의 3D 공간을 전방 180^\circ (또는 360^\circ)의 각도 섹터(Sector)로 나눈다. 각 섹터 방향으로 센서가 감지한 장애물의 밀도와 근접도를 합산하여 1차원 히스토그램 형태의 장애물 밀도계(Obstacle Density)를 생성한다.
  2. 골짜기(Valley) 탐색 규칙: 히스토그램 상에서 밀도 한계치(Threshold) 미만인 빈 공간(계곡)을 찾는다. 이 빈 섹터가 드론이 날아갈 수 있는 ’안전한 방위각(Safe Heading)’의 후보가 된다.
  3. 비용 함수(Cost Function) 필터링:
  • 현재 RTL 목표 지점을 향하는 방위각 곡선 (Cost_{target})
  • 현재 기체의 진행 관성을 유지하려는 각선 (Cost_{current})
  • 이전 사이클에서 선택했던 우회 방향 각선 (Cost_{previous})
    이 세 가지를 가중 합산하여 안전한 골짜기 중에서 비용이 가장 적은 1\sim3차 방위 후보를 최종 비행 방향으로 산출한다.

3. 우회 경로(Waypoint Segments)의 생성과 배열 병합 (Data Blending)

VFH+ 알고리즘에 의해 장애물을 부드럽게 돌아가는 우회 방향이 결정되면, 이를 즉각 비행 속도 벡터 구조로 변환하여 FlightTaskAutoMapper를 거쳐 PositionController로 하달해야 한다.

  • safepoint 배열의 인터셉트: 본래 네비게이터의 RTL 루프는 목적지 \rightarrow 기체의 현재 위치를 일직선 세그먼트 하나로 정의하지만, MAVLink(TRAJECTORY_REPRESENTATION_WAYPOINTS) 메시지를 통해 회피 모듈이 개입하면 이 직선의 소유권을 일시적으로 빼앗는다(Takeover).
  • 장해물 우회를 위해 계산된 여러 개의 미시적인 꺾임 점(Intermediate Safepoints, 최대 5개) 배열이 기존의 RTL 직선 목표를 덧씌워 병합(Blended)된다. 드론은 이 짧은 Setpoint들을 순차 추종함으로써 마치 물 흐르듯 바위를 피해 가는 모양새로 우회 궤적 구간을 소화한다.
graph TD
    A[RTL: A(현재) -> B(목적지) 직선 Setpoint 생성] --> B[MAVLink 통해 Companion Computer 에 목표 전달]
    C[센서(LiDAR/Depth) 3D Point Cloud 획득] --> D[VFH+ 극좌표계 장애물 밀도 히스토그램 매핑]
    B --> D
    D --> E{직선 A-B 궤적 상에 장애물이 존재하는가?}
    E -->|No| F[기존 RTL 궤적 그대로 패스스루(Pass-through)]
    E -->|Yes| G[빈 공간(Valley) 탐색 및 최저 Cost 방위각 산출]
    G --> H[P1, P2, P3 등의 로컬 우회 Safepoints 배열 생성]
    H --> I[PX4 내부로 MAVLink 궤적 제안 전송<br>(Trajectory Override)]
    I --> J[FlightTaskAuto: 기존 RTL Setpoint를 버리고<br>임시 수신된 Safepoint 추종(우회 실행)]
    J --> K[장애물 구간 통과 후 다시 A->B 직진 제어권 복귀]
    F --> K

4. 로직의 한계점과 데드락(Deadlock) 예방 (Local Minima)

VFH 기반의 자율 우회 시스템은 즉각적인 반응성(Reactive Avoidance) 면에서는 탁월하나, 전역 경로 탐색 알고리즘(A나 RRT)이 아니기에 한계가 명확하다.

  • 로컬 미니마(Local Minima, U자형 함정): 기체가 ㄷ 자형이나 U 자형 모양의 구조물 한복판으로 들어섰을 때, VFH는 측면과 전방이 모두 장애물로 막혀 있어 밀도가 낮아진 쪽(즉 뒤 쪽)을 가리키려 하지만, RTL 목적지는 계속 앞쪽을 가리키므로 제어 비용 함수가 교착 상태(Deadlock)에 빠져 기체가 제자리에서 멈춰 덜덜 떠는(Jittering) 패닉 상태에 빠질 수 있다.
  • 예방 설계: rtl.cpp는 우회 시스템 개입으로 인해 기체가 일정 시간(Timeout) 이상 목표를 향한 전진 진척도(Progress)를 보이지 못할 경우, OA 개입을 강제로 무시하고 무조건 설정된 RTL 안전 고도(RTL_RETURN_ALT) 위로 수직 상승해버리는 예외 탈출(Failsafe Pop-up) 로직을 트리거하여 이런 로컬 미니마 오류를 극복한다.

5. 결론 및 GCS 연계 방향성

충돌 회피 시스템(COM_OBS_AVOID 파라미터가 1로 활성화된 상태)이 결합된 RTL은 단순히 위치(Position)만을 추종하던 1세대 귀환 모드에서 벗어나, 동적 환경 인지(Environmental Perception) 속성을 포섭한 지능형 경로 탐색의 2세대로 진화했음을 상징한다.
비전 기반 OA 시스템이 보편화됨에 따라 전신주, 타워 크레인 등이 난무하는 건설 현장에서도 VFH 벡터장 필터링과 safepoint 배열 병합의 시너지 덕분에, PX4 기체는 별도의 인간 조종 개입 없이 스스로 시각적 블라인드 스팟(Blind Spot)을 돌파하며 무사 귀환할 수 있는 핵심 지능을 획득하였다.