논문

[path][dynamic]DPMPC-Planner: A real-time UAV trajectory planning framework for complex static environments with dynamic obstacles

정지홍 2025. 7. 1. 12:38

Abstract

  • 안전한 UAV 내비게이션은 복잡한 환경 구조, 동적 장애물, 그리고 measurement noise과 예측 불가능한 움직이는 장애물 행동에서 비롯되는 불확실성 때문에 도전적이다.
  • 최근 많은 연구들이 점유 맵(occupancy map)이나 ESDF 맵과 같은 정교한 매핑 알고리즘을 통해 복잡한 정적 환경에서의 안전한 내비게이션을 달성했다.
    • 하지만 이러한 방법들은 움직이는 장애물로 인한 매핑의 한계 때문에 동적 환경에서는 신뢰성 있게 작동하지 못한다.
    • ==> 즉, occupancy map과 ESDF map은 동적환경을 표현하는데 어렵다.
  • 이 한계를 해결하기 위해, 본 논문에서는 복잡한 정적 환경에서 동적 장애물을 고려해 안전한 내비게이션을 달성할 수 있는 경로 계획 프레임워크를 제안한다.
    • 동적 장애물을 신뢰성 있게 다루기 위해, 우리는 환경 표현을 정적 매핑(static mapping)과 동적 객체 표현(dynamic object representation)으로 나누었으며, 동적 객체 정보는 컴퓨터 비전 방법을 통해 얻을 수 있습니다.
  • 우리의 프레임워크는 우선 제안하는 반복적 통로 축소(Iterative Corridor Shrinking) 알고리즘을 기반으로 정적 경로를 생성합니다.
    그 다음, 동적 장애물과 불확실성을 회피하기 위해 시간적 목표 추적이 가능한 확률 제약 반응적 모델 예측 제어(reactive chance-constrained model predictive control)를 적용합니다.
  • 다양한 환경에서의 시뮬레이션 결과는, 우리 알고리즘이 complex static environments with dynamic obstacles에서도 안전하게 내비게이션할 수 있음을 보여줍니다.

 

I. INTRODUCTION

  • 산업 분야에서 자율 UAV의 사용이 증가함에 따라, 온라인 경로 생성(online trajectory generation)은 복잡한 구조의 환경에서 사람과 로봇이 움직이는 상황(그림 1 참조)에서 안전성과 자율성을 위해 매우 중요해지고 있습니다.
    • 이러한 시나리오에서 로봇은 복잡하게 장애물이 많은 환경에서 목표 지점까지 이동해야 하며, 사람과의 안전도 보장해야 합니다.
    • 따라서 복잡한 환경 구조, 동적 장애물, 측정 잡음 및 예측 불가능한 움직이는 장애물 행동에서 오는 불확실성을 다룰 수 있는 안전한 경로 계획 프레임워크가 필수적입니다.
  • 동적 환경에서의 안전한 경로 계획에는 두 가지 주요 도전과제가 있으며, 이는 과거 연구들로는 완전히 해결되지 않았습니다.
    • 첫 번째로, 환경의 복잡성 때문에 차량의 동역학을 고려한 최적 경로 생성을 계산하는 데 매우 많은 연산이 필요합니다.
      • 일부 연구[1][2][3][4]는 정적 환경에서 실시간 성능을 입증하였지만, 이 방법들은 점유 맵[5]이나 ESDF 맵[6][7] 같은 특정 맵 표현에만 의존하여, 동적 장애물을 신뢰성 있게 표현할 수 없습니다.
    • 두 번째로, 알고리즘은 정적 장애물과 동적 장애물을 모두 고려하면서도 충분히 빠르게 동작해야 합니다.
      • 이전 연구들[8][9][10]은 모델 예측 제어(MPC)와 동적 장애물의 기하학적 표현을 활용하여 신뢰할 수 있는 솔루션을 제공하지만, 복잡한 정적 환경 구조는 경로 생성에서 고려하지 않았습니다.
  • 본 논문에서는 Dynamic Polynomial-based Model Predictive Control (DPMPC) planner라는 새로운 경로 계획 프레임워크를 제안합니다.
    • 이 프레임워크는 정적 환경과 동적 장애물을 다루기 위해 "두 개의 계획 계층(planning layers)"을 포함하고 있으며, 점유 맵(occupancy map)과 기하학적 장애물 표현(geometric obstacle representation)을 모두 효과적으로 활용합니다.
    • 정적 계층(static layer)에서는, 다항식 기반 최소 snap(minimum snap) 경로 최적화 기법과 반복적 통로 축소(Iterative Corridor Shrinking) 알고리즘을 결합하여 정적 경로를 찾습니다.
    • 이후 동적 계층(dynamic layer)에서는, "확률 제약 모델 예측 제어(chance-constrained model predictive control)"를 사용해 반응형 온라인 경로를 생성하며, 시간적 목표 추적(temporal goal tracking) 방법을 통해 동적 장애물을 회피합니다.
      • ==>시뮬레이션 실험 결과, 제안된 방법이 장애물이 많은 동적 환경에서 UAV가 안전하게 이동하는 데 도움이 됨을 보여줍니다.

 

  •  이 논문의 새로운 점과 주요 기여는 다음과 같습니다
    • 불확실성을 고려한 경로 계획기( uncertainty-aware trajectory planner): 본 연구는 복잡한 정적 환경에서 불확실성이 있는 동적 장애물을 안전하게 다룰 수 있는 2계층 DPMPC 플래너를 제시합니다.
    • MPC 기반 시간적 목표 추적( temporal goal tracking with MPC ): 시간적 목표 추적은 측정 및 장애물의 불확실성을 처리하기 위해 확률 제약 모델 예측 제어를 활용하여 동적 장애물 회피를 달성합니다.
    • 반복적 통로 축소 알고리즘( Iteratice corridor shrinking algorithm): 제안한 반복적 통로 축소 알고리즘은 정적 경로를 효율적으로 생성할 수 있는 계산적으로 효율적인 방법을 제공합니다.



 

II. RELATED WORK

  • 정적 환경에서의 경로 생성은 제약 조건이 있는 최적화 문제로 볼 수 있습니다.
    • [11]에서는 쿼드로터(쿼드콥터)의 미분 평탄성(differential flatness)이 최소 snap(minimum snap) 최적화의 효과를 잘 보여줍니다.
      • 이를 바탕으로, [12]와 [4]는 혼합 정수 계획법(mixed integer programming)을 적용하여 정적 장애물을 회피합니다.
    • [13]은 유사한 방식이지만, 중간 웨이포인트(waypoints)를 추가하여 경로가 충돌 없이 생성되도록 보장합니다.
    • [14][15]에서 영감을 받아, 일부 연구들은 최적화 문제를 제약이 없는(unconstrained) 형태로 만듭니다.

    • [1]은 ESDF 맵을 이용한 비제약 최적화(unconstrained optimization)로, 미지의 정적 장애물이 있는 환경에서 공격적인(aggressive) 비행을 실현합니다.
    • [2]는 ESDF에 대한 의존성을 줄여 계산량도 절감합니다.
    • 또한, [3]은 최적 제어 문제에서 오는 계산 복잡성을 피하기 위해 탐색 기반(search-based) 방법을 제안합니다.
    • UAV 탐사 분야에서는 [16]이 점진적 샘플링 방법(incremental sampling method)을 적용해 재계획(replanning) 시간을 줄여 장애물 회피를 수행합니다.
    • ===> 위의 방법들이 복잡한 환경에서의 성공적인 내비게이션을 입증하였으나, 대부분의 알고리즘은 점유 맵(occupancy map)이나 ESDF 맵에만 의존하기 때문에, 움직이는 장애물을 신뢰성 있게 반영하는 데 한계가 있습니다.
  • 동적 장애물 회피는 초기 연구에서도 다루어졌으며, 최근까지도 여전히 미해결 과제로 남아 있습니다.
    • [17]에서는 최초로 인공 잠재장(artificial potential field)을 이용한 장애물 회피를 제안하였고,
      [18]에서는 비충돌 속도(velocity obstacle) 개념을 정의하여 충돌이 발생하지 않는 속도를 선택하도록 하였습니다.

    • UAV 경로 계획 분야에서는 최근 몇 년간 **모델 예측 제어(MPC)**를 이용한 장애물 회피가 큰 주목을 받았습니다.
    • [19]에서는 MPC에 [17]의 장애물 잠재장 비용을 추가하여 로봇 간 충돌 회피를 수행했습니다.
      불확실성을 고려하기 위해 [20]은 확률 제약 MPC(chance-constrained MPC)를 해결하기 위해 분리적 프로그래밍(disjunctive programming)을 적용했으나, 여전히 계산량이 많습니다.

    • [19]와 [20]을 기반으로 [8]은 실시간 성능을 보장하면서 동적 장애물을 다룰 수 있는 온라인 선형화 확률 제약 MPC(online linearized chance-constrained MPC)를 제안하였습니다.

    • [21]의 결정적 제약(deterministic constraint)과 비교하면 [8]은 경로를 덜 보수적으로 만듭니다.
    • [8]과 유사하게 [10]도 확률 제약 MPC 내에 분리적 제약(disjunctive constraints)에 상한을 추가하여 온라인 회피를 달성합니다.
    • [9]는 [8]의 아이디어를 UAV의 온보드 비전을 활용하는 방식으로 확장했습니다.
    • ===> 하지만 이러한 방법들은 장애물 표현에 기하학적 및 해석적 공식만을 사용하여, 점유 맵(occupancy map)과 비교했을 때 환경의 복잡한 구조를 충분히 묘사하지 못합니다.
    • =====> 이로 인해 로봇이 장애물이 많은(cluttered) 환경에서는 내비게이션에 실패할 수 있습니다.

 

  • 본 논문에서 제안하는 방법은 기존과 달리, 정적 매핑(예: 점유 맵)과 기하학적 장애물 표현의 장점을 결합하여, 복잡한 구조의 환경에서도 온라인 동적 장애물 회피와 함께 경로를 생성할 수 있습니다.

 

필자가 제안하는 시스템

III. METHODOLOGY -  A. System Framework

  • 필자가 제안하는 시스템은 주로 4가지 부분으로 구성된다.
    • global path planner , map module , dynamic obstacle representation , 제안하는 DPMPC planner
    • 1. global path planner가 goal을 입력으로 받아서, high - level의 "collision-free waypoint path"를 생성한다.
    • 2. DPMPC planner는 waypoints를 static layer로 받아서, static trajectory를 최적화한다.
    • 3. static trajectory는 dynamic layer에 입력되어, 최종적으로 내비게이션과 장애물 회피를 위한 reactive trajectory가 생성된다.
      • ==> static trajectory이외에도, dynamic layer는 "장애물의 위치( bounding box포함)"와 "추정 속도"가 포함된 동적 장애물 정보를 MPC 제약 조건에 추가로 사용한다.
      • 전 과정에서, map module은 각 planning later마다 collision-check하는 기능을 제공한다.

 

알고리즘 1

III. METHODOLOGY -  B. Static Layer

  • staic layer은 제안된 반복적 통로 축소(iterative corridor shrinking)알고리즘(Alg.1 참조)에 기반하여, 다항식 기반 최소 snap(minimum-snap) 경로 최적화를 활용하여 collision-free static trajectory를 생성합니다.
  • 2번째 줄에서는, 경로 상 각 샘플 위치마다 통로 바운딩 박스 크기 "C_box"를 정의합니다.
    • 여기서 바운딩 박스(C_box)가 반드시 충돌이 없어야 할 필요는 없고, 합리적인 초기 크기(예: 0.5m)로 설정만 하면 됩니다.
  • sample position은 연속적인 경로를 ΔT해상도로 분할하여 얻습니다.
    • ( 여기서 "ΔT해상도"는 trajectory를 "시간간격 ΔT "로 일정하게 샘플링 한다는 뜻. 왜냐하면 경로를 무한히 연속적으로 검사하는 것은 불가능하기 때문에, 일정한 간격( ΔT )마다 위치를 샘플링해서 확인한다. )
  • 주요 반복 루프(6~11번째 줄)는 충돌 없는 경로가 생성될 때까지 반복합니다.
  • 루프 내부에서, 먼저 다항식 기반 최적화( polynominal - based optimization)를 초기화합니다(이 부분은 이후에서 더 논의).
  • 그 후, 통로 바운딩 박스(C_box)들은 선형 부등식 제약조건(linear inequality constraints)으로 최적화 문제에 추가됩니다.
  • 그 다음, 맵 모듈을 사용해 최적화된 경로의 충돌 여부를 검사합니다.
  • 마지막으로, 경로의 바운딩 박스 크기는 사용자가 정의한 축소 계수(α, 4번째 줄)에 따라 줄어들며, 이 과정은 유효한(충돌 없는) 경로가 생성될 때까지 반복됩니다.
  • 축소 계수(α)는 보통 0.5에서 1 사이의 튜닝 파라미터입니다.
  • 다항식 기반 최적화는 [11][13]에서 제안된 것처럼 **경로의 snap(4차 미분)**을 최소화합니다.
  • 웨이포인트(waypoint) 총 개수가 N+1개라고 가정하면, 전체 경로는 인접한 웨이포인트를 지나는 **구간별 연속 다항식(segment)**으로 구성됩니다.

식 1

  • [11]을 바탕으로, 두 웨이포인트 사이의 각 경로 구간은 다음과 같은 다항식으로 표현됩니다.

식 2

  • 여기서 t_n은 n번째 웨이포인트에 도달하는 시간을 나타내고, d는 다항식의 차수(degree)입니다.
  • 따라서 최적화 문제는 다음과 같은 이차 프로그래밍(quadratic programming, QP)으로 표현할 수 있습니다.
    •  식 3a에서, w는 경로를 표현하는 다항식의 계수 벡터이다. ( 가운데는 시간 t에 대해서 k번 미분한것. )
      • 제일 뒤의  η||w||는 정규화 항

  • 목적 함수(3a)에서 는 모든 다항식의 계수 벡터이고, 는 경로의 snap(4차 미분)을 최소화합니다. ( w에는 trajectory를 구성하는 모든 다항식(segment)의 모든 계수를 하나의 벡터로 모은 것이다.)
    • 안정적인 최적화를 위해 정규화 항 η || 2 || ^2 도 추가합니다.
  • 제약조건 (3b)-(3d)는 경로가 모든 웨이포인트를 통과하고, 접속점에서 연속성을 유지하도록 합니다.
    • P_wp는 웨이포인트의 위치입니다. 접속점은 두 segment가 만나는 지점이다.
  • 제약조건 (3e)-(3f)는 경로를 ΔT 해상도만큼 샘플링해, 각 샘플 포인트가 인접 웨이포인트 P_ip를 중심으로 바운딩 박스 내에 있도록 합니다.

반복적 통로 축소 알고리즘은 최적화 반복 횟수만큼만 계산하기 때문에, 이 QP 공식은 실시간 알고리즘 구현에도 적합합니다.

==> 결론적으로는 static layer에서는 다항식 기반 경로 최적화 방법으로 제일 짧고, 부드럽고, 충돌없는 최적화된 경로를 만든다는 것 

 

 

 

III. METHODOLOGY -  C. DynamicLayer

  • 이 절 에서는 먼저 dynamic layer에서의 확률 제약 모델 예측 제어(chance‐constrained MPC) 수식화를 소개합니다.
    • 이어서 회피 목표(avoidance target)의 정의를 제시하고, 이 회피 목표 선택을 기반으로 동적 장애물을 피하기 위한 시간적 목표 추적(temporal goal tracking) 방법을 설명합니다.

 

 

III -  C - 1. Chance-constrained MPC formulation

  • 모델 예측 제어에는 (a) 정적 경로(static trajectory) 추종과 (b) 동적 장애물 회피, 이 두 가지 요구사항이 있습니다.
    • 로봇의 위치 추정 및 장애물 인식의 불확실성을 다루기 위해, [8]을 바탕으로 다음과 같이 확률 제약 문제를 정의합니다.
      • 4a :  목적 함수는 로봇이 장애물을 만나지 않은 경우에는 정적 경로 x^k_ref를... 만난 경우에는 이후에 정의된 "시간적 목표"를 추종하도록 구성한다.
      • 4b : 초기 상태와 동역학 모델 f(⋅)를 나타낸다.
      • 4c : 초기 상태와 동역학 모델 f(⋅)를 나타낸다.
      • 4d : 각 시점 k에서 i번째 동적 장애물과의 충돌 확률이 이하가 되도록 제약을 걸어준다.
      • 4e : 입력 u의 허용 범위를 지정한다.

 

III -  C - 2. 충돌 제약의 확률 제약 처리 

  • 충돌 제약(4d)는 위치 불확실성을 고려해 확률 제약 방식으로 적용합니다.
    로봇 위치 P_r와 장애물 위치 를 각각 가우시안 분포로 가정한다.

P_r과 P_o를 가우시안 분포로...

  • 로봇은 반지름 r인 구(sphere), 장애물은 반경 a,b,c의 최소 타원체(ellipsoid)로 근사할 때, 아래식으로 정의할 수 있다.

식a

  • 식a의 조건 하에서 충돌확률은 식 6과 같다.  식 6처럼 타원체 영역에 대한 가우시안 적분이 필요해 해석적 해가 없다.

식 6

  • 수학적 내용이 많다..... 다음에 필요할때 이 부분은 다시 읽자.

 

III -  C - 3. 시간적 목표 추적(Temporal Goal Tracking)

  • 로봇이 동적 장애물에 근접하면, 충돌 회피용 우회 목표(detour goal) 로 사용할 정적 경로 상의 특수 지점(avoidance target)을 선택해야 합니다.
  • 이 과정을 통해 MPC의 기준 궤적(reference)을 순간적으로 전환하여 안전하게 장애물을 회피합니다.

 

 

III -  C - 알고리즘

  • 알고리즘 2에서는 회피 목표를 찾는 방법을 보여줍니다.
    • 일반적으로 이 알고리즘은 장애물로부터의 "경로 상 최근접 위치 p_near"와 "로봇의 현재 위치p_avoid"를 기준으로 두 가지 후보 지점을 평가하여(Line 10, 16) 회피 목표를 반환합니다.
  • 서로 다른 관점에서 선택된 두 후보는 안전한 우회 경로의 신뢰성을 높여 줍니다.
    1. 첫 번째 후보
      Lines 4–6에서, 먼저 정적 경로 상에서 움직이는 장애물과 가장 가까운 지점 을 찾고, 그 지점 이후의 경로 상 위치 σ_near들을 순차 탐색합니다.
      • 탐색 지점과 최근접 점 사이의 경로 거리 D_near가 임계값(threshold)보다 크고,
      • 해당 지점에서의 진행 방향과 장애물 방향 사이의 각도 θ_near가 90°보다 클 때(Line 8),
        가장 처음 조건을 만족하는 지점을 첫 번째 회피 목표 p^near_avoid로 저장합니다(그림 3 상단의 빨간 십자).
    2. 두 번째 후보
      Lines 11–12에서는 로봇의 현재 위치 p^r_avoid에서부터 경로 상을 순차 탐색합니다.
      • 장애물과의 거리 D_o가 임계값보다 크고,
      • 로봇 진행 방향과 장애물 방향 사이의 각도 θ_r가 90°보다 클 때(Lines 13–16),
        첫 번째로 조건을 만족하는 지점을 두 번째 회피 후보로 선택합니다(그림 3 하단의 빨간 십자).
    마지막으로(Line 17), 두 후보 중 현재 위치에서 경로 거리가 더 먼 지점을 최종 회피 목표로 결정합니다.

  • 장애물을 만나는 조건은 두 가지로 정의합니다.
    • (a) 로봇 진행 방향과 장애물-로봇 방향 사이의 각도 θ(그림 3)가 90° 미만일 것.
    • (b) 로봇과 장애물 사이의 거리가 미리 정한 임계값 이내일 것.
    • 우회 목표는 항상 θ>90 ° 이 되도록 선택하여, 로봇이 장애물로부터 멀어지는 방향으로 유도합니다.

 

  • 위의 알고리즘2를 통해서 회피 목표를 얻은 뒤에는, 이를 MPC의 시간적 목표(temporal goal) 로 구성합니다.
  • 시간적 목표는 정적 경로와 같은 형태이되, MPC 예측 지평선(prediction horizon) 길이만큼만 포함합니다.
  • 회피 목표가 경로 상에서 시간 인덱스 t_a에 있을 때, 예측 단계 k의 목표 지점은 σk(t_a)이며(이하 정적 경로 첨자는 생략), 시간적 목표는 식 11과 같다.

식11

 

  • 여기서 은 MPC의 예측 지평선 길이입니다.
  • 첫 N-n개의 반복 항목은 모두 회피 목표로 채워져, 로봇이 충분히 장애물을 벗어나도록 보장합니다.
  • 이어지는 개의 항목은 회피 목표 이후 정적 경로를 따라가기 위한 지점들로, 장애물 회피 후 원래 경로로 부드럽게 복귀하도록 돕습니다.
  • 만약 n=0이면 미래 정적 경로를 전혀 사용하지 않으므로, 원래 경로로 돌아오기 어려워집니다.
  • 반대로 n=N 이면 반복되는 회피 목표만 사용하게 되어 해결책을 찾지 못할 수 있습니다.
  •  
  • 마지막으로 동적 계층은 맵 모듈을 이용해 충돌 검사를 수행하고, 이 시간적 목표 궤적 중 충돌 없는 구간을 반응형(reactive) 최종 경로로 출력합니다.

 

 

IV. RESULT AND DISCUSSION - A. Experiments Setup

  • 실험은 PX4 Gazebo 시뮬레이션 환경에서 수행되었으며, 이 환경은 차량의 동역학(Vehicle Dynamics)을 포함합니다.
  • 알고리즘은 ROS 환경에서 **C++**로 구현되었고, Intel Core i7-10750H CPU @ 2.6GHz에서 실행되었습니다.
  • **Octomap [5]**은 정적 맵(Static Map) 표현에 사용되었습니다.
    정적 계층(Static Layer)의 이차 프로그래밍(Quadratic Programming)에는 **Quadprog++ [23]**을,
    모델 예측 제어(Model Predictive Control, MPC) 구현에는 **Acado Toolkit [24]**을 적용했습니다.
  • 저희 구현에서는, 정적 플래너(Static Planner)에서 **다항식 차수(polynomial degree)**를 7 또는 8로 선택하여
    실시간 성능과 트래젝토리의 부드러움(smoothness)을 모두 만족시켰습니다.
  • MPC의 **예측 지평선(Prediction Horizon)**은 N = 20,
    시간 간격(Time Step)은 0.1초로 설정했습니다.
  • 장애물의 위치와 그 불확실성은 다음과 같은 선형 모델로 예측(Propagation)했습니다.

 

IV. RESULT AND DISCUSSION -  B. Simulation Experiments

  • 전체적인 내비게이션 안전성을 더 잘 평가하기 위해, 표 I에 나타난 것처럼 세 가지 시뮬레이션 환경을 준비했습니다.
  • 이 환경들은 크기와 동적 장애물의 수가 서로 다릅니다.
    • 모든 동적 장애물들은 미리 정의된 구간별 선형 경로(piecewise linear trajectory)를 따라 움직이며, 동적 장애물들은 환경 내에 고르게 분포하도록 수동으로 배치했습니다.
    • 로봇이 동적 장애물을 회피하는 예시는 그림 4에 나타나 있습니다.
      • 빨간색 타원체(ellipsoid)는 동적 장애물을 나타냅니다.
      • 빨간색 곡선은 로봇이 동적 장애물을 만나지 않았을 때 따라야 하는 **정적 경로(static trajectory)**를 의미합니다(그림 4a).
      • 장애물을 만난 후에는, **회피 목표(avoidance target)**를 기반으로 시간적 목표(temporal goal)가 생성되어, 확률 제약 MPC의 기준 경로(reference trajectory)로 사용됩니다.
    • 파란 화살표로 표시된 초록색 곡선은 충돌 회피를 위해 생성된 **반응형 경로(reactive trajectory)**입니다(그림 4b).
    • 마지막으로, 그림 4c에서는 로봇이 동적 장애물을 안전하게 우회한 뒤, 다시 원래의 정적 경로로 복귀하는 모습을 보여줍니다.
    • 그림 1의 하단은, 여러 장애물이 로봇을 둘러싸고 있을 때 장애물 회피 사례도 보여줍니다.