다중 로봇 경로 계획을 위한 앙상블 칼만 인버전을 이용한 분산 팩터 그래프 최적화 알고리즘

2.1 팩터 그래프 최적화

팩터 그래프는 복잡한 확률 분포를 변수 노드와 팩터 노드 간의 이분 그래프 G = {V,F,E}구조로 분해하여 표현하는 확률적 그래피컬 모델이다. 여기서 V는 변수 노드의 집합, F는 팩터 노드의 집합, E는 팩터 노드와 변수 노드 사이의 연결 관계를 의미한다. 이러한 표현은 고차원 시스템에서 나타나는 조건부 독립성과 구조적 희소성을 효과적으로 활용할 수 있게 하며, 다양한 로보틱스 및 머신러닝 문제에서 널리 사용되고 있다. 팩터 그래프에서 전체 변수 노드 V의 전역 상태 변수 집합을 Z라고 하면, Z에 대한 결합 확률 분포는 다음과 같이 개별 팩터 포텐셜 함수 φ_j들의 곱으로 인수 분해된다.

여기서 Z_η(j)는 j번째 팩터와 연결된 상태 변수의 부분 집합이며, φ_j는 센서 관측 모델, 로봇의 동역학 모델과 같은 각 팩터들이 가지는 일종의 비용함수에 해당한다. 이때 가장 확률이 높은 최적의 Z를 구하는 문제는 최대 사후 확률 추정 문제로 정식화할 수 있으며, 각 팩터 포텐셜 함수 φ_j가 식 (2)와 같이 가우시안 노이즈 모델을 따른다고 가정하고 음의 로그 우도를 취하여 식 (3)과 같은 비선형 최소 제곱법 문제로 정의할 수 있다.

초기 로보틱스 분야의 팩터 그래프 최적화는 주로 단일 로봇의 동시적 위치 추정 및 지도 작성(SLAM)이나 포즈 그래프 최적화 문제를 해결하기 위해 연구되었다. 앞서 설명한 바와 같이 문제를 팩터 그래프 형태로 표현하면 변수 간의 조건부 의존 관계가 희소구조로 나타나게 되며, 이러한 희소성은 대규모 최적화 문제의 계산 효율성을 크게 향상시킨다. 대표적으로 GTSAM과 같은 최적화 솔버는 팩터 그래프로부터 생성되는 희소한 헤시안 및 야코비안 구조를 활용하여 sparse Cholesky decomposition, variable elimination 등의 기법을 적용함으로써 대규모 그래프 문제를 실시간에 가깝게 해결할 수 있도록 한다.

2.2 팩터그래프 상에서의 분산 최적화

다중 로봇 시스템의 규모가 커짐에 따라, 모든 데이터를 중앙 집중형으로 처리하는 방식은 중앙 계산 장치의 부하와 단일 장애점 문제를 초래한다. 이런 문제점을 극복하기 위해 각 연산 장치가 연산 부하를 나누어 부담하는 분산 추론 프레임워크의 필요성이 대두되었으며, 대표적으로 가우시안 메시지를 반복적으로 교환해 전역 최적화를 달성하는 가우시안 신뢰 전파 알고리즘과 공유 변수를 통해 합의를 강제하는 ADMM 계열의 최적화 기법을 기존의 중앙 집중형 배치 기반의 프레임워크와 접목한 방법이 주된 접근법으로 활용된다. 그러나 이러한 분산 기법들은 실제 다중 로봇 환경의 복잡한 제약과 루프 구조의 그래프에서 다음과 같은 한계를 드러낸다.

GBP와 같은 메시지 전달 구조^[10]에서는 특정 문제에 필연적으로 존재하는 루프 구조에서는 메시지가 순환하며 동일한 정보가 반복적으로 반영되는 이중 계산 문제가 발생한다. 이는 추정된 신뢰도의 과신을 유발하며, 비선형 팩터를 처리하기 위한 테일러 전개 기반의 국소 선형화 오차와 결합될 경우 심각한 알고리즘 진동 및 발산으로 이어진다.

ADMM 계열의 알고리즘은 신념 전파 알고리즘과 달리 최적화 메커니즘에 기반하여 전역적 합의를 강제하므로, 특정 비선형 문제에서도 일정 수준 이상의 수렴성을 보장할 수 있다는 장점이 있다. 그러나 로봇의 이동과 상호 충돌 회피로 인해 통신 토폴로지가 동적으로 변화하는 다중 로봇 경로 계획 환경에서는 기존의 iMESA^[11]와 같은 분산 평활화 방법들이 명확한 한계를 드러낸다. 행렬 분해나 트리 구조에 의존하는 기존 기법들은 그래프 토폴로지가 변경될 때마다 전체 요인 그래프 구조를 실시간으로 재정렬해야 하므로 막대한 연산 오버헤드가 발생한다. 이로 인해 슬램과 같이 연산 구조가 정형화된 어플리케이션이 아니면 실제 시시각각 변하는 동적 계획 시스템에 직접 적용하기 어렵다는 치명적인 문제가 존재한다.

2.3 앙상블 칼만 인버전

EKI는 본래 기상학 및 해양학 분야에서 대규모 비선형 시스템의 상태를 추정하기 위해^[12] 고안된 앙상블 칼만 필터에 뿌리를 두고 있다. 이후 응용 수학 분야에서 이를 상태 추정이 아닌 파라미터 추정 및 결정론적 비선형 역문제를 풀기 위한 최적화 기법으로 재해석하면서^[13] EKI라는 독자적인 프레임워크가 확립되었다.

위의 식 (4)에서 h는 일반적으로 변수 z를 관측 공간으로 사상하는 비선형 순방향 모델을 의미한다. 역문제는 식 (5)와 같이 노이즈가 포함된 관측 데이터 y가 주어졌을 때, 이를 가장 잘 설명하는 최적의 파라미터 z^*를 추정하는 문제로 정의된다. EKI는 이러한 역문제를 미분없이 해결하기 위해 앙상블의 통계적 정보를 활용하는 최적화 기법으로, 식 (5)와 같이 비선형 최소 제곱 목적 함수의 최소화 문제를 효과적으로 풀 수 있다. EKI의 구체적인 업데이트 과정과 앙상블 통계를 이용한 최적화 메커니즘은 3.3절에서 자세히 설명한다.

3.1 전역 최적화 정식화의 국소 노드 단위 디커플링

식 (3)에서 정의된 전역 최적화 문제를 팩터 노드와 변수 노드 간의 분산 최적화 구조로 바꾸기 위해, 전역 변수 Z를 각 변수 노드 V_i의 국소적인 합의점 Z_i로 분할한다. 또한 계산의 분리를 위해 각 팩터 노드 F_j는 엣지 E로 연결된 이웃 변수 노드 V_i들에 대해 자신만의 독립적인 로컬 복사본 X_j,i를 할당받도록 한다. 이를 통해 원래의 전역 최적화 문제는 다음과 같이 엣지 상의 국소적 등식 제약 조건을 포함하는 형태로 재구성 된다.

X_j는 팩터 노드 F_j가 필요로 하는 이웃 변수 노드들의 로컬 복사본들의 집합(X_j = {X_j,i}_i∈η(j))을 의미한다. 이러한 변수 분할을 통해 결합되어 있던 목적 함수는 팩터 단위로 분리되며, 합의 제약으로 결합된다. 따라서 거대한 전역 최적화 문제가 노드 간의 로컬 합의 문제로 치환된다.

3.2 메세지 패싱 관점에서의 ADMM 확장

식 (6)에서 정의된 제약조건이 있는 최적화 문제를 풀기 위해 ADMM 알고리즘을 도입한다. 스케일링 된 쌍대 변수 U_j,i를 도입하고 다음과 같이 $$ f_j\left(X_j\right)=\frac{1}{2}{\parallel h_j\left(X_j\right)-y_j\parallel }_{{\Gamma }_j^{-1}}^2$$로 치환하면 다음과 같이 증강 라그랑지안을 구성할 수 있다.

ADMM은 이 증강 라그랑지안을 한 번에 최소화 하는 대신, 반복 k마다 변수 X_j, Z_i, 그리고 쌍대 변수 U_j,i를 교대로 업데이트하면서 해를 찾는다. 그 과정은 다음과 같이 세 가지 단계로 분할 할 수 있다.

• 1) x-update (로컬 변수 업데이트) :
  

  $\[ \begin{array}{l}{X}_j^{\left(k+1\right)}=\underset{X_j}{\mathrm{argmin}}{\mathcal{L}}_{\rho }\left(X_j,Z_i^{\left(k\right)},U_{j,i}^{\left(k\right)}\right)\\ =\underset{X_j}{\mathrm{argmin}}\left(f_j\left(X_j\right)+\frac{\rho }{2}\sum _i{\parallel X_{j,i}-Z_i^{\left(k\right)}+U_{j,i}^{\left(k\right)}\parallel }_2^2\right)\end{array} \]$

• 2) z-update (로컬 합의 업데이트) :
  

  $\[ \begin{array}{l}{Z}_i^{\left(k+1\right)}=\underset{Z_i}{\mathrm{argmin}}{\mathcal{L}}_{\rho }\left(X_j^{\left(k+1\right)},Z_i,U_{j,i}^{\left(k\right)}\right)\\ =\frac{1}{|ϵ\left(i\right)|}\sum _{j\in ϵ\left(i\right)}\left(X_{j,i}^{\left(k+1\right)}+U_{j,i}^{\left(k\right)}\right)\end{array} \]$

• 3) u-update (쌍대 변수 업데이트) :
  

  $\[ U_{j,i}^{\left(k+1\right)}=U_{j,i}^{\left(k\right)}+\left(X_{j,i}^{\left(k+1\right)}-Z_i^{\left(k+1\right)}\right) \]$

이때 z-update 식에서 ε(i)는 변수 노드 i에 연결된 팩터 노드들의 집합을 의미한다. 위와 같이 표현된 각 스텝들은 팩터 그래프 상에서 마치 BP 알고리즘과 유사한 형태의 메시지 패싱 알고리즘으로 자연스럽게 해석될 수 있다^[14]. 먼저, 로컬 변수 업데이트 과정은 각 팩터 노드가 이웃한 변수 노드들로부터 받은 이전 스텝에서의 합의점 $$ Z_i^k$$를 바탕으로 독립적인 로컬 최적화를 수행한다. 이때, $$ Z_i^k$$를 메시지 m_Vi→Fj라고 한다. 이후 각 팩터 노드는 계산된 최적화 결과를 바탕으로 대응되는 각 엣지의 쌍대 변수 $$ U_{j,i}^k$$를 더하여 $$ \left(X_{j,i}^{\left(k+1\right)}+U_{j,i}^{\left(k\right)}\right)$$를 메시지 m_Fj→Vi로 생성하여, 이를 다시 변수 노드 V_i에게 수신한다. 변수 노드는 팩터 노드로부터 받은 메시지를 바탕으로 단순히 산술 평균을 계산함으로써 새로운 합의점을 업데이트한다. 마지막으로, 국소 해와 합의점 간의 차이를 쌍대 변수에 누적하여 업데이트함으로써, 알고리즘의 매 반복에서 더 강하게 합의하도록 오차를 보정한다.

이와 같이 ADMM은 [Fig. 1]에서 보이는 것과 같이 팩터 그래프 상에서 팩터 노드와 변수 노드 간의 메시지 교환 과정으로 해석될 수 있으며, 본 연구에서 고려하는 팩터 그래프로 표현되는 최적화 문제의 각 제약들은 별도의 투영 연산 과정 없이 비용 함수 내의 잔차 형태로 포함된다. 따라서 이러한 제약들은 하드 제약조건이 아닌 비용함수로써 동작하며, 각 로컬 최적화 문제는 해당 잔차들을 최소화하는 최소제곱 문제로 표현된다. 다음 절에서는 이러한 ADMM 기반 메시지 패싱 알고리즘의 로컬 변수 업데이트 단계에 EKI를 통합함으로써, 비선형 또는 미분이 어려운 부드럽지 않은 비용함수 포함하는 최소제곱 문제를 야코비안 계산 없이 해결할 수 있는 프레임워크를 제안한다.

[Fig. 1]

Message-passing interpretation of ADMM on a factor graph, where local updates and consensus are achieved through factor-variable interactions

3.3 EKI를 통한 국소 업데이트

ADMM의 첫 번째 단계인 로컬 변수 업데이트는 현재의 합의 변수(consensus variable)와 크게 벗어나지 않으면서 각 로컬 목적 함수를 최소화하는 최적화 문제로 표현된다. 그러나 원래의 비용 함수가 강한 비선형성을 가지거나 미분 불가능한 항을 포함하는 경우, 이러한 부분 문제를 기존의 미분 기반 최적화 기법으로 효율적으로 해결하기 어렵다. 본 연구에서는 이러한 한계를 극복하기 위해 Ensemble Kalman Inversion (EKI)을 각 로컬 부분 문제를 해결하기 위한 최적화 솔버로 도입한다.

이를 위해 각 변수는 단일 추정값이 아닌 N개의 파티클로 구성된 앙상블로 표현된다. 즉, 상태 변수는 하나의 결정론적 값이 아니라 확률 분포를 나타내는 표본 집합으로 취급되며, 이후의 모든 최적화 과정은 이 앙상블 공간에서 수행된다. 일반적으로 앙상블의 크기는 추정하고자 하는 변수의 차원에 따라 결정된다. ^[12]에서는 10차원 이하의 문제에 대해 변수 차수의 약 10배 이상의 앙상블을 사용하는 것이 효과적임을 실험적으로 보고하였다. 따라서 본 논문에서는 변수 노드 중 가장 큰 차원을 갖는 변수의 차수의 10배에 해당하는 앙상블 크기를 사용하였으며, 모든 실험에서 동일한 설정을 적용하였다. 이러한 앙상블 표현을 통해 EKI는 샘플들로부터 계산된 경험적 공분산을 이용하여 상태를 갱신하며, 야코비안이나 헤시안 계산 없이도 비선형 최적화를 수행할 수 있다.

2.3절에서 설명한 바와 같이 EKI는 최소제곱 형태의 목적 함수를 최소화하는 최적화 기법으로 해석할 수 있다. 따라서 ADMM의 로컬 변수 업데이트 문제 역시 EKI를 적용할 수 있는 최소제곱 문제의 형태로 재구성할 필요가 있다. ^[15]에서는 ADMM의 패널티 항을 베이지안 추론 관점에서 가우시안 사전 분포로부터 유도된 가상의 관측 정보로 해석하고, 이를 이용하여 비선형 MPC 문제의 x-update를 EKI로 해결하는 방법을 제안하였다. 해당 연구는 ADMM과 EKI를 결합할 수 있는 이론적 기반을 제시하였으나, 중앙집중형 MPC 문제의 제약조건 처리를 대상으로 하며 팩터 그래프 기반의 분산 최적화 구조는 고려하지 않았다.

본 연구에서는 이러한 해석을 팩터 그래프 기반의 분산 최적화 문제로 확장한다. 구체적으로 각 팩터 노드가 유지하는 로컬 최적화 문제와 변수 노드에서의 합의 제약을 ADMM 형태로 분리하고, 각 로컬 부분 문제를 EKI를 이용하여 해결한다. 이를 통해 기존 EKI-ADMM 구조를 팩터 그래프 기반 메시지 패싱 프레임워크에 통합할 수 있으며, 비선형 및 비미분 제약을 포함하는 분산 최적화 문제를 야코비안 계산 없이 처리할 수 있다. 이를 위해 먼저 ADMM의 로컬 변수 업데이트 문제에 패널티 항을 포함시키는 형태로 확장된 최소제곱 형태로 다음과 같이 재정식화한다.

식 (8)에서 내부 목적 함수의 좌항은 확장된 순방향 모델 h_aug, 우항을 관측 벡터 y_aug로 정의한다. 그리고 확장된 순방향 모델에 대한 앙상블 편차를 정의하기 위해 팩터 노드가 가진 로컬 상태 변수 앙상블 $$ X_j^k$$의 편차를 $$ \mathrm{\Delta }{X}_j=X_j^k-\overline{X_j^k}$$, h_j를 통과한 앙상블의 편차를 $$ \mathrm{\Delta }{h}_j\left(X_j\right)=h_j\left(X_j^k\right)-{\overline{h}}_j\left(X_j^k\right)$$라고 하면, 확장된 앙상블에 대한 편차 벡터는 다음과 같이 두 편차를 세로로 쌓은 블록 형태가 된다.

따라서 우리가 원하는 확장된 형태에 대한 공분산은 다음과 같이 행렬곱에 의해 확장 이전의 공분산 행렬의 블록형태로 정의된다. 따라서 우리는 확장 이전의 공분산 행렬 3개를 구하여 확장 상태에 대한 공분산 행렬을 구성할 수 있다.

최종적으로 새로운 상태 변수 앙상블 $$ X_j^{k+1}$$ 칼만 이득 그리고 예측값과 실제 관측값의 차를 통해 갱신된다.

여기서 W^k ∼ N(0,σ²I)는 백색 잡음이다.

이때 EKI는 업데이트를 반복함으로써 점점 앙상블이 축소하여, 실제 해에 도달하기 전에 앙상블 분산이 0에 도달하여 갱신이 일어나지 않는 앙상블 붕괴 문제가 발생한다. 이를 방지하기 위해 임의의 additive white noise W^k를 매 반복마다 주입하여 앙상블 붕괴를 방지한다.

4.1 실험 A : 비선형 최적 제어 문제

본 실험은 제안하는 D-EKI가 일반적인 팩터 그래프 최적화 문제에서 어떠한 거동 특성을 보이는지 분석하기 위해 수행되었다. 특히 다중 로봇 환경이 아닌 단일 시스템의 최적 제어 문제를 대상으로 하며, 분산 최적화 성능보다는 최적화 알고리즘 자체의 수렴 특성과 해의 품질을 평가하는 데 목적이 있다.

비교 대상으로는 전통적인 최적 제어 알고리즘인 iLQR을 사용하였다. iLQR은 비선형 시스템의 동역학과 비용 함수를 현재 궤적 주변에서 반복적으로 선형화 및 이차 근사함으로써 최적 제어 입력을 계산하는 알고리즘이다. 매 반복마다 현재 궤적에 대해 시스템 동역학을 선형화하고, 이에 대응하는 LQR 문제를 해결하여 새로운 제어 입력을 계산한 뒤, 이를 이용해 궤적을 재생성하는 과정을 반복적으로 수행한다.

실험에서는 동일한 비선형 최적 제어 문제를 iLQR로 해결한 후, 이를 동일한 구조의 팩터 그래프 최적화 문제로 재구성하여 GTSAM과 제안하는 D-EKI를 각각 적용하였다. 이를 통해 D-EKI가 기존의 그래프 기반 최적화 기법과 비교하여 어떠한 수렴 특성을 보이는지 분석하고, 나아가 일반적인 팩터 그래프 최적화 알고리즘으로서의 적용 가능성을 평가한다.

4.1.3 실험 결과

[Fig. 2]는 iLQR, GTSAM, 그리고 제안하는 D-EKI를 이용하여 동일한 비선형 최적 제어 문제를 해결한 결과를 나타낸다. 상단의 그래프는 각 알고리즘이 생성한 위치, 속도 및 제어 입력 궤적을 비교한 결과이다. 세 알고리즘 모두 초기 상태에서 목표 상태 x_T까지 안정적으로 수렴하는 궤적을 생성하였으며, 이에 대응하는 제어 입력 역시 동일하게 생성되는 것을 확인할 수 있다.

[Fig. 2]

Comparison of the trajectories generated by each algorithm. Top: Finite-horizon state trajectories and control inputs obtained using iLQR, GTSAM, and D-EKI. Bottom: L1-norm differences of the trajectories generated by D-EKI (green) and GTSAM (orange) with respect to the iLQR solution

보다 정량적인 비교를 위해 하단에는 iLQR을 기준으로 계산한 궤적과 나머지 두 방법의 궤적 오차를 나타내었다. D-EKI는 GTSAM에 비해 전체 시간 구간에서 iLQR과 거의 동일한 궤적을 생성하였으며, 수치적으로 무시할 수 있는 수준인 것을 확인할 수 있다. 반면 GTSAM 역시 전반적으로 유사한 해에 수렴하였으나, 일부 구간에서 D-EKI에 비해 상대적으로 큰 오차가 발생하였다.

[Fig. 3]는 D-EKI의 수렴 특성을 나타낸다. 상단의 그래프는 ADMM 기반 메시지 패싱 과정에서 각 노드에서 발생하는 원시 잔차와 쌍대 잔차의 누적값의 변화를 보여준다. 두 잔차 모두 알고리즘이 반복됨에 따라 지속적으로 감소하였으며, 약 530번의 반복 이후 1e⁻⁶ 수준의 허용 오차에 도달하는 것을 확인할 수 있다. 이는 각 팩터 노드와 변수 노드 간의 합의가 점진적으로 형성되며, 제안하는 D-EKI가 비선형 최적화 문제에서도 안정적으로 수렴함을 의미한다.

[Fig. 3]

Convergence characteristics of the proposed D-EKI algorithm. Top: Primal and dual residuals of the ADMM iterations shown on a logarithmic scale. Bottom: Evolution of the total cost and dynamic error throughout the optimization process

하단의 총 비용의 변화도 ADMM의 특성을 잘 보여준다. 알고리즘 초기에는 각 팩터가 오직 자신의 국소 비용만을 독립적으로 최소화하려고 하므로 비용이 급격하게 감소한다. 이후 반복이 진행됨에 따라 변수 노드에서 누적된 쌍대 변수에 의해 각 이웃 간의 합의가 강하게 일어나게 되고, 최종적으로 전체 궤적의 총 비용이 iLQR, GTSAM과 동일한 수준으로 수렴한다. 또한 동역학 위반 오차 역시 지속적으로 감소하여 최종적으로는 GTSAM과 유사한 수준에 도달함을 확인할 수 있다.

다만 [Table 1]의 결과를 보면, D-EKI는 최종 해의 품질 측면에서는 iLQR 및 GTSAM과 유사한 성능을 달성하였으나, 수렴에 필요한 반복 횟수는 상대적으로 많은 것으로 나타났다. 이는 본 실험의 최적 제어 문제가 시간 축을 따라 연결된 긴 체인 구조의 팩터 그래프의 형태이기 때문에 제안하는 D-EKI가 인접 노드 간의 국소적인 메시지 교환을 통해 전역적인 합의를 형성하여 정보를 그래프 전체에 전파되기까지 상당한 반복 과정이 필요하기 때문이라고 분석하고 있다. 따라서 단일 궤적 최적화 문제에서는 수렴 속도 측면에서 기존의 중앙집중형 최적화 기법보다 불리할 수 있으나, 이러한 특성은 분산 환경에서의 확장성과 병렬화 가능성을 확보하기 위한 구조적 대가로 해석할 수 있다.

[Table 1]

Quantitative performance comparison of iLQR, GTSAM, and D-EKI

4.2 실험B : 다중 로봇 경로 계획

제안하는 D-EKI 기반 분산 팩터 그래프 최적화 기법이 단순 최적화 문제를 넘어 실제 응용 문제인 다중 로봇 경로 계획에서도 효과적으로 동작함을 검증하기 위해 수행된다. 특히, 비선형 동역학 및 비연속적 장애물 비용함수가 포함된 환경에서의 이를 선형화를 통해 최적화 하는 GBP와의 비교 분석을 통해 최적화 성능과 분산 수렴 특성을 평가하는 것을 목표로 한다.

4.2.2 실험 설정

본 실험은 다수의 정적 장애물이 배치된 2차원 공간을 가정하며, 여러 대의 로봇이 서로 충돌하지 않고 각자의 시작 위치에서 목표 위치까지 안전하게 이동하는 복잡한 시나리오들로 구성된다. 각 로봇의 궤적은 일정한 시간 간격 Δt를 가지는 이산 노드들의 시퀀스로 표현된다. [Fig. 4]에 도시된 바와 같이, 각 로봇은 자신만의 독립적인 로컬 팩터 그래프를 구축하여 최적화를 수행하며, 다른 로봇이 감지 되거나, 통신 반경 내에 진입 할 경우에만 동적으로 충돌 회피 팩터를 생성하여 상호작용한다. 이런 환경에서 각 로봇의 로컬 그래프의 네트워크 토폴로지는 이웃된 로봇이 가까워지거나 멀어지는 이벤트를 통해 계속해서 변화하는 상태가 된다. 이런 환경에서 메시지 패싱 기반 알고리즘은 각 노드가 독립적으로 동작할 수 있기 때문에 매우 효율적이며, 일관된 최적화 성능을 보인다.

[Fig. 4]

Dynamic factor graph formation in an ad-hoc multi-robot network. Inter-robot factors (black square) are created when neighboring robots establish communication links, enabling local graph coupling. The message-passing formulation naturally accommodates the creation and removal of graph elements as network connectivity evolves

실험에서 각 로봇의 궤적 최적화를 위한 초기 궤적 X_i,init은 시작점과 목표점을 잇는 단순 직선 보간을 통해 생성되며, 이를 평균으로 삼아 D-EKI의 초기 궤적 앙상블들을 분포시킨다. 로컬 팩터 그래프를 구성하는 각 팩터는 D-EKI와 GBP 모두 다음과 같이 각 팩터와 비용 함수 J를 정의한다.

• • Pose factor: 로봇이 지정된 시작점 및 목표점 상태와 일치하도록 강제하는 제약. $$ x_k^{ref}$$는 현재 위치 또는 최종 도달 상태를 나타낸다.
  

  $\[ \begin{array}{c}{J}_p\left(x_t\right)={\parallel x_t-x_k^{ref}\parallel }_{{\Lambda }_p}^2\\ {\Lambda }_p={\sigma }_p^{-2}\mathit{I}\end{array} \]$

• • Dynamics factor: 운동학 모델 f에 기반하여 인접한 시간 스텝 간의 상태 전이 연속성을 유지하는 제약
  

  $\[ J_d\left(x_t,u_t,x_{t+1}\right)={\parallel x_{t+1}-f\left(x_t,u_t\right)\parallel }_{{\Lambda }_d}^2 \]$

  
  

  $\[ \begin{array}{c}f(x_t,u_t)=\left[\begin{array}{c}{p}_{x,t}+v_t\cos ({\theta }_t)\mathrm{\Delta }t\\ p_{y,t}+v_t\sin ({\theta }_t)\mathrm{\Delta }t\\ {\theta }_t+{\omega }_t\mathrm{\Delta }t\\ v_t+a_t\mathrm{\Delta }t\end{array}\right]\\ {\Lambda }_d={\sigma }_d^{-\mathrm{2}}\mathit{I}\end{array} \]$

• • Obstacle factor: SDF 맵을 조회하여 장애물 영역 침범 시 비선형적인 페널티를 부과하는 회피 제약. R_safe는 장애물과 로봇 간의 충돌 방지를 위한 마진, R_r은 로봇의 반지름을 의미한다.
  

  $\[ \begin{array}{c}{J}_o\left(x_t\right)={\parallel \max \left(0,\left(R_{safe}+R_r\right)-f_{SDF}\left(p_t\right)\right)\parallel }_{{\Lambda }_o}^2\\ {\Lambda }_o={\sigma }_o^{-2}\mathit{I}\end{array} \]$

• • Collision factor: 이웃 로봇과의 유클리디안 거리가 설정된 안전 거리 이상을 유지하도록 하는 상호작용 제약
  

  $\[ \begin{array}{c}{J}_{col}={\parallel \max \left(0,R_{safe}-\parallel p_{i,t}-p_{j,t}\parallel \right)\parallel }_{{\Lambda }_{col}}^2\\ {\Lambda }_{col}={\sigma }_{col}^{-2}\mathit{I}\end{array} \]$

• • Velocity factor: 로봇의 물리적 구동 한계를 반영하여 최대 및 최소 선속도를 제한하는 부등식 제약
  

  $\[ \begin{array}{c}{J}_{vel}\left(x_t\right)={\parallel \max \left(0,v_t-v_{\max }\right)+\max \left(0,v_{\min }-v_t\right)\parallel }_{{\Lambda }_{vd}}^2\\ {\Lambda }_{vel}={\sigma }_{vel}^{-2}\mathit{I}\end{array} \]$

각 팩터는 동일하게 대응 되는 가중치를 정보행렬 형태로 반영한다. 일반적으로 동역학이나 시작점과 도착점을 고정하는 팩터의 경우에는 위반하면 큰 패널티를 주기 위해 매우 작은 분산 σ를 부여한다. 그리고 상대적으로 중요도가 떨어지는 속도 제약과 같은 팩터의 경우에는 상대적으로 큰 분산 σ를 부여한다. 이런 식으로 각 팩터의 분산을 통해 전체 비용함수에서의 가중치를 조절하는 튜닝이 가능하다.

4.2.3 실험 결과

[Table 2]는 [Fig. 5]와 같은 다중 장애물 환경에서 제안하는 D-EKI와 GBP를 각각 30회 반복 수행하여 비교한 결과이다. 실험 결과, 모든 로봇 수 설정에서 D-EKI가 GBP 대비 동등하거나 우수한 성공률을 보였으며, 로봇당 평균 이동 거리 또한 D-EKI가 더 짧았다. 평균 수행 시간의 경우, 로봇 수(N)가 10대 및 20대일 때는 D-EKI가 다소 길었으나, 30대일 때는 더 빠르게 수렴하는 것을 확인하였다. 특히 로봇 수가 증가할수록 두 방법 간의 성능 차이가 더욱 두드러졌으며, 이를 통해 복잡한 환경에서도 D-EKI가 안정적으로 경로를 생성함을 입증하였다.

[Table 2]

Quantitative comparison of GBP and D-EKI for multi-robot path planning

[Fig. 5]

Ensemble distributions at different optimization iterations, illustrating the transition from the initial linearly interpolated trajectory to the final converged trajectory

[Fig. 5]는 장애물이 많은 환경에서 반복에 따라 궤적이 최적화되는 과정을 나타낸다. 초기 단계에서는 앙상블이 넓게 분포하며 다양한 경로를 탐색하고, 반복이 진행됨에 따라 장애물이 없는 영역으로 수렴하면서 앙상블 분산이 점차 감소하는 모습을 확인할 수 있다. 반면 GBP는 초기 궤적이 장애물 내부를 통과하거나 국소해 부근에 위치한 경우 경로 탐색에 실패하는 사례가 관찰되었다. 이러한 차이는 두 방법이 비선형 제약을 처리하는 방식에서 비롯된다. GBP는 비선형 함수를 1차 테일러 선형화하여 처리하므로 현재 추정점에서 계산된 야코비안 정보에 크게 의존한다. 따라서 [Fig. 6]과 같이 일부 구간에서 야코비안이 불안정하거나 기울기 정보가 부정확한 경우에 생성되는 경로가 발산하는 것을 보인다. 특히 [Fig. 6]의 우측 사례에서는 초기 궤적이 장애물을 통과하도록 주어진 상황에서 수치적 미분으로 계산된 SDF 함수의 기울기가 장애물 내부에서 좁은 통로 방향의 정보를 충분히 제공하지 못하여 벽을 통과하는 궤적을 생성하는 것을 확인할 수 있다.

[Fig. 6]

In the GBP results, trajectory oscillations are observed in the region due to the non-smooth obstacle cost function

반면 D-EKI는 단일 점에서의 기울기 정보 대신 앙상블에 포함된 다수의 샘플을 이용하여 탐색을 수행한다. 따라서 초기 단계에서 넓은 영역을 탐색할 수 있으며, 국소해나 장애물 내부에 위치한 초기 궤적의 영향을 상대적으로 덜 받는다. 이러한 특성은 다중 로봇 환경에서도 유사하게 나타났다. GBP의 경우 로봇 수가 증가함에 따라 좁은 공간에서 충돌이나 데드록으로 인해 경로 생성에 실패하는 경우가 증가하였으나, D-EKI는 [Fig. 7]과 같이 다양한 시나리오에서 앙상블 기반 탐색과 로봇 간 합의 과정을 통해 우회 경로를 선택하거나 충돌을 회피하며 보다 높은 성공률을 유지하였다. 또한 평균 이동 거리 역시 상대적으로 짧게 나타나 보다 효율적인 경로를 생성하는 경향을 보였다.

[Fig. 7]

Multi-robot path planning results in various scenarios using the proposed D-EKI framework. The method generates collision-free trajectories and successfully avoids deadlock in complex environments

한편 기존 연구 ^[10]에서는 GBP의 이러한 문제를 완화하기 위해 사전에 생성된 맵을 이미지 형태로 변환한 후 조밀한 선형 보간을 수행하여 연속적인 SDF 맵과 유사하게 활용하였다. 또한 A^*와 같은 전역 탐색 기법을 사용하여 초기 궤적이 장애물을 통과하지 않도록 생성하는 방법을 채택하였다. 이에 비해 D-EKI는 별도의 복잡한 전처리 과정 없이 앙상블을 직접 비용 함수에 통과시켜 탐색을 수행하며, 이 과정에서 앙상블 분포가 이산적인 SDF 맵을 간접적으로 평활화하는 효과를 제공한다.

종합하면, 실험 결과는 D-EKI가 다중 로봇 경로 계획 문제에서 비교군 대비 높은 성공률과 효율적인 경로를 생성함을 보인다. 또한 앙상블 기반 탐색과 ADMM 기반 분산 최적화의 결합을 통해 기울기 정보의 품질에 크게 의존하지 않으면서도 비선형 및 비미분 제약을 포함하는 환경을 효과적으로 다룰 수 있음을 확인하였다.