6자유도 쿼드로터 정밀 착륙을 위한 자세-인식 Second-Order Conic 제약 궤적 최적화

1. 서 론

최근 자율비행체 기술은 물류 배송, 재난 구조, 시설 점검, 군사 정찰 등 다양한 산업 현장에서 빠르게 상용화되고 있다. 이러한 자율 비행 로봇의 핵심 역량은 비행 중 주변 환경을 정밀하게 인지하고, 그 정보를 토대로 안전한 비행 궤적을 계획하고 추종하는데 있다. 특히 쿼드로터(quadrotor)는 수직 이착륙과 안정적 호버링이 가능해 협소하거나 복잡한 공간에서도 정밀 착륙에 적합하다. 다만 이를 신뢰성 있게 달성하려면 접근 기하(예: 안전한 접근 각도) 뿐만 아니라, 추진 및 자세 한계 등 물리적 제약을 동시에 만족하는 실시간 6자유도(6-DOF) 궤적 생성 기술이 필수적으로 요구된다.

컴퓨팅 하드웨어의 비약적 발전과 최적화 알고리즘의 정교화에 힘입어, 최적 궤적 생성(optimal trajectory generation)은 학계와 산업계 전반에서 빠르게 성숙하고 있다. 최근에는 자동차, 로봇, 무인기, 우주 발사체 등 다양한 시스템에서 해당 기법의 실시간 적용이 활발히 이루어지고 있다. 그럼에도 안전하고 정확한 착륙을 보장하려면, 안전한 접근 각도 유지와 같은 기하학적 제약과 고정 모터 구성에 따른 추력 방향 한계 등 물리적 제약을 동시에 만족하는 6-DOF 궤적 계획이 필수적이다. 특히 위치뿐 아니라 자세까지를 동시 제어해야 하므로 문제는 고차원 비선형 최적제어로 귀결되며, 이를 효율적이고 견고하게 해결하는 계산적 방법이 여전히 핵심 과제로 남아 있다.

정밀 착륙은 본질적으로 복잡한 기하·물리 제약을 수반한다. 착륙 단계에서 비행체는 목표 착지점을 정점으로 하는 원뿔 내부로 접근해야 하며, 이는 장애물 충돌을 예방하고 급격한 자세 변화 없이 부드러운 하강을 보장한다. 동시에 고정 모터 구성에 따른 추력 방향 한계를 준수해야 하는데, 이러한 추력 방향 제약은 기체 자세를 간접적으로 구속하여 과도한 기동을 억제하고 궤적의 매끄러움을 확보한다. 이와 같은 제약들은 수학적으로 이차 원뿔(second-order cone, SOC) 제약으로 자연스럽게 표현될 수 있어, 최적 궤적 생성 문제에 효율적으로 통합이 가능하다.

이차 원뿔 제약 조건은 마찰 한계, 자세 범위, 추력 크기·방향과 같은 안전·물리 제약을 간결하고 계산 효율적으로 표현하는 표준 도구로 자리 잡았다. 다만 6-DOF 정밀 착륙과 같이 회전 동역학(예: 회전행렬 C(q))과 추력 항이 곱으로 결합되는 문제에서는, SOC 제약 자체는 볼록(convex)임에도 불구하고 비선형 동역학 그리고 목표함수와의 결합으로 전체 최적화가 비선형·비볼록(nonconvex)으로 변한다. 기존 궤적 최적화는 주로 반복적인 Successive Convexification (SCP)을 통해 비선형 제약을 국소 선형화하고 신뢰영역(trust region)과 가상입력 등을 도입하여, 각 반복마다 전체 계획 구간에 대한 batch SOCP (Second-Order Cone Programming) 문제를 풀어 간접적으로 제약을 처리해 왔다. 그러나 매 반복마다 대규모 SOCP를 해석해야 하므로 계산 비용이 크고, 복잡한 결합 비선형성으로 인해 실시간 적용에는 구조적 한계가 존재한다. 한편 확률적 최적제어·정책탐색 계열에서는 경로적분 기반의 PI2 (Path Integral Policy Improvement) 등 기법을 통해 매개변수화된 상태 피드백 정책을 직접 학습함으로써 실시간 성능을 높이려는 대안이 활발히 모색되고 있다^[1].

본 논문은 이러한 한계를 극복하기 위해, Interior-Point Differential Dynamic Programming (IPDDP)에 SOC 제약을 내재화한 자체 개발 C++ 솔버인 CoManDO (Conic-Constrained Manifold Dynamic Optimizer)를 활용하여 6-DOF 쿼드로터의 정밀 착륙 궤적 최적화를 직접 해결한다^[2]. 핵심 아이디어는 SOC 제약을 로그 배리어(log-barrier) 형태로 문제에 통합해, 반복적 선형화나 외부 SOCP 솔버의 호출 없이도 제약을 만족하는 최적 궤적을 산출하는 것이다. 이때, DDP 알고리즘의 stagewise Newton 업데이트는 perturbed KKT 선형 시스템을 풀면서 SOC 보완성(complementarity) 조건을 유클리드 조르단대수(Jordan Algebra)의 binary 연산(아다마르형 곱)과 Nesterov-Todd 스케일링을 통해 SOC 제약 블록과 선형 제약 블록으로 분리하여 처리한다. 뿐만 아니라, 선형 위치-속도와 사원수(quaternion)-각속도 기반 자세(attitude/orientation) 동역학을 단일 매니폴드 최적화 문제로 결합하여 동시적으로 다룬다.

특히 본 연구는 3D 자세를 구속하는 추력 원뿔(thrust cone)과 3D 접근 위치를 규정하는 착륙 원뿔(landing cone)에 대한 SOC 제약을 동시에 반영하여 6-DOF 정밀 착륙 궤적을 산출한다. 제안 방법은 Crazyflie 2.1 나노 쿼드로터 기반 실환경 하드웨어 실험으로 검증되었으며, 그 결과 접근법의 실용성과 계산 효율성이 입증되었다.

2. 배경지식 및 선행연구

쿼드로터와 쿼드로터와 같은 6-DOF 시스템의 궤적 최적화는 위치와 자세를 동시에 고려해야 하는 고차원 비선형 최적제어 문제이다. 정밀 착륙에서는 동역학뿐 아니라 복잡한 기하·물리 제약을 반드시 만족해야 하며, 이를 표현하기 SOC 제약 $$ {‖Ax+b‖}_2\le c^{\mathrm{T}}x+d$$ 형태를 널리 사용한다. 이 표현은 추력 한계, perception-aware line-of-sight (LOS) 제한 등 다양한 로봇 모델에 적용 가능하다. 본 연구는 안전한 접근 경로를 위한 착륙 원뿔과 자세–추력 결합을 반영한 추력 원뿔이라는 두 핵심 SOC 제약을 동시에 다룬다. 주목할 핵심은 SOC 제약 자체는 비선형이면서 convex임에도, 이를 6-DOF 비선형 동역학 및 비용함수와 결합하면 문제 전체가 비선형이고 nonconvex으로 변해 계산 복잡도가 크게 상승하고 반복법의 수렴성이 보장되지 않는다는 점이다. 따라서 이러한 결합 구조를 효율적으로 처리하면서 신뢰할 수 있는 수렴성과 실시간성을 동시에 확보하는 알고리즘 설계가 여전히 핵심 과제로 남아 있다.

이 문제를 해결하기 위해 다양한 접근이 제안되어 왔다. 현재 로보틱스와 항공우주 분야에서 가장 널리 쓰이는 방식은 Successive Convexification^[3,4]로, 전체 비선형 문제를 연속적인 convex 하위문제로 근사해 푸는 방법이다. [3]은 시야각(line-of-sight) 제약을 다루었고, [4]는 3-DOF와 6-DOF SCP를 결합한 2단계 파이프라인을 제안하였다. 그러나 SCP 기반 방법은 본질적으로 nonconvex 문제를 간접 처리하므로, 반복적 선형화와 고비용의 외부 SOCP 솔버 의존성 탓에 실시간성 확보에 한계가 있다. 한편 임베디드(마이크로컨트롤러) 환경을 겨냥한 연구^[5]에서는 ADMM 기반 Conic-Tiny MPC를 통해 SOCP를 고속으로 해결하고, Crazyflie 실험으로 실시간 가능성을 보였다. 다만 이 접근은 ADMM-기반 Convex MPC에 초점을 두고 있어, 본 논문에서 다루는 6-DOF 궤적 최적화의 결합 비선형성을 직접 완화하는 데에는 제약이 있다. 이와 달리 DDP는 비선형 최적제어 문제에서 강력한 성능을 보이는 궤적 최적화 기법이다. 하지만 기존의 vanilla DDP는 equality 제약만을 다루며, SOC와 같은 inequality hard constraint을 처리하기 어렵다. 이러한 한계를 극복하기 위해 IPDDP^[2]와 같이 DDP의 효율적인 backward/forward pass 구조에 interior point 방법론을 결합하여 부등식 제약조건을 직접 처리하는 방법이 제안되었다. 본 연구는 선행 연구들의 한계를 SOC-IPDDP^[2] 기반 솔버를 활용하여 해결이 가능함을 보인다.

본 논문은 SOC-IPDDP^[2]를 쿼터니언 기반 6-DOF 동역학의 궤적 최적화로 확장한 C++ 솔버 CoManDO를 활용한다. 착륙 원뿔과 추력 원뿔이라는 두 핵심 SOC 제약을 logarithmic barrier function로 내재화하여 DDP 프레임워크—특히 backward pass—내에서 직접 처리함으로써, SCP 기반 접근^[3,4]처럼 반복적 선형화나 외부 SOCP 솔버에 의존하지 않는다. 그 결과 제약 준수와 수렴 안정성을 유지한 채 계산 효율을 확보하며, 6-DOF 동역학과 복잡한 자세-위치 SOC 제약이 결합된 정밀 착륙 문제를 효과적으로 최적화할 수 있음을 시뮬레이션 및 실환경 하드웨어 실험을 통해 검증한다.

3. 문제 정의

3.2 연속시간 시스템 동역학

본 연구에서는 쿼드로터 시스템의 운동을 기술하기 위해 연속 시간 비선형 6-DOF 강체 동역학 모델을 사용한다^[3]. 시스템의 상태는 관성 좌표계와 동체 좌표계에서 정의된 위치, 속도, 쿼터니언, 각속도로 구성하였다. 각 상태의 다음과 같이 표현된다.

$\[ {\dot{\mathbf{\text{r}}}}_{\mathit{I}}\left(t\right)={\mathbf{\text{v}}}_{\mathit{I}}\left(t\right) \]$

(3)

$\[ {\dot{\mathbf{\text{v}}}}_{\mathit{I}}\left(t\right)=\frac{1}{m}\mathbf{\text{C}}\left({{}^I\mathbf{\text{q}}}_B\left(t\right)\right){\mathbf{\text{f}}}_B\left(t\right)+{\mathbf{\text{g}}}_I \]$

(4)

$\[ {{}^I\dot{\mathbf{\text{q}}}}_B\left(t\right)=\frac{1}{2}\mathbf{\text{Ω}}{\left({\mathbf{\text{ω}}}_B\left(t\right)\right)}^I{\mathbf{\text{q}}}_B\left(t\right) \]$

(5)

$\[ {\dot{\mathit{\omega }}}_B\left(t\right)={\mathbf{\text{J}}}_B^{-1}\left({\mathbf{\text{M}}}_B\left(t\right)-{\left[{\mathbf{\text{ω}}}_B\right]}_{\times }\left({\mathbf{\text{J}}}_B{ \mathbf{\text{ω}}}_B\left(t\right)\right)\right) \]$

(6)

식 (3)은 위치와 속도 관계를 나타내는 운동학식이며, 식 (4)에서의 $$ C\left({{}^I\mathbf{\text{q}}}_B\right)$$는 식 (7)과 같이, 동체 좌표계에서 관성 좌표계로의 회전을 나타내는 쿼터니언을 변환한 회전 행렬이다.

$\[ \begin{array}{c}C\left({{}^I\mathbf{\text{q}}}_B\right)\hfill \\ =\left[\begin{array}{ccc}1-2\left(q_2^2+q_3^2\right)& 2\left(q_1{q}_2-q_0{q}_3\right)& 2\left(q_1{q}_3+q_0{q}_2\right)\\ 2\left(q_1{q}_2+q_0{q}_3\right)& 1-2\left(q_1^2+q_3^2\right)& 2\left(q_2{q}_3-q_0{q}_1\right)\\ 2\left(q_1{q}_3-q_0{q}_2\right)& 2\left(q_2{q}_3+q_0{q}_1\right)& 1-2\left(q_1^2+q_2^2\right)\end{array}\right]\end{array} \]$

(7)

식 (5)는 쿼터니언 기반의 자세 운동학식으로, 각속도 $$ {\mathbf{\text{ω}}}_B\left(t\right)$$에 의해 자세 변화율은 식 (8)과 같이, $$ \mathbf{\text{Ω}}\left({\mathbf{\text{ω}}}_B\right)$$행렬을 통해 표현된다. 쿼터니언 운동학 행렬 $$ \mathbf{\text{Ω}}\left(\mathbf{\text{ω}}\right)\in {\mathbb{R}}^{\mathbf{4}\times \mathbf{4}}$$는 각속도 벡터에 따라 정의되며, 벡터 곱은 skew symmetric 행렬 $$ {\left[\mathbf{\text{ω}}\right]}_{\times }\in {\mathbb{R}}^{\mathbf{3}\times \mathbf{3}}$$을 사용해 표현된다.

$\[ \mathbf{\text{Ω}}\left(\mathbf{\text{ω}}\right)=\left[\begin{array}{cccc}0& -{\mathrm{\omega }}_x& -{\mathrm{\omega }}_y& -{\mathrm{\omega }}_z\\ {\mathrm{\omega }}_x& 0& {\mathrm{\omega }}_z& -{\mathrm{\omega }}_y\\ {\mathrm{\omega }}_y& -{\mathrm{\omega }}_z& 0& {\mathrm{\omega }}_x\\ {\mathrm{\omega }}_z& {\mathrm{\omega }}_y& -{\mathrm{\omega }}_x& 0\end{array}\right] \]$

(8)

식 (6)에서의 $$ {\mathbf{J}}_B\in {\mathbb{R}}^{\left(\mathbf{3}\times \mathbf{3}\right)}$$는 동체 좌표계에서의 관성 모멘트 행렬이다. m과 J_B는 동역학 모델 매개변수이며 사용한 매개변수는 [Table 1]에서 정리하였다.

[Table 1]

Aerial vehicle and simulation parameters setup

3.3 6-DOF 궤적 계획 최적 제어 문제

본 연구에서는 쿼드로터 착륙을 위한 6-DOF 궤적 최적화 문제(Optimal Control Problem)를 다음과 같이 정의한다. 목표는 주어진 시간 구간 [t_I, t_F]에서 초기 상태 $$ \mathit{x}\left(t_I\right)={\mathit{x}}_I$$로부터 최종 목표 상태 $$ \mathit{x}\left(t_F\right)={\mathit{x}}_F$$까지 제약조건을 만족하며 제어 입력의 에너지를 최소화하는 최적 궤적을 찾는 것이 목표이다. 여기서 t_I는 초기 시간, t_F는 최종 시간을 의미하며, 본 연구에서는 [Table 1]에서의 시간 구간에서 궤적을 생성한다. 최적화 문제는 다음과 같이 수식화 된다.

$\[ \underset{\mathit{u}\left(t\right), t_f, t_F}{\text{min}}{\int }_{t_I}^{t_F}l\left(\mathit{u}\left(t\right)\right) \]$

(9)

$\[ \mathbf{\text{s.t }}\mathbf{x}\left(\dot{t}\right)=f(\mathbf{x}\left(t\right), \mathbf{u}\left(t\right)), t\in \left[t_I, t_F\right] \]$

(10)

$\[ \begin{array}{cc}\mathbf{x}\left(t_I\right)={\mathbf{x}}_0, \hfill & \mathbf{x}\left(t_F\right)={\mathbf{x}}_f\hfill \end{array} \]$

(11)

$\[ 0\le ‖{\mathbf{f}}_{\mathit{B}}\left(t\right)‖\le {\mathrm{f}}_{max} \]$

(12)

$\[ ‖{\mathbf{f}}_{B,xy}\left(t\right)‖\le \tan \left({\theta }_{max}\right){\mathit{ }\mathit{ }\mathit{ }\mathbf{\text{f}}}_{B,z}\left(t\right) \]$

(13)

$\[ ‖{\mathit{r}}_{I,xy}\left(t\right)‖\le \mathrm{t}\mathrm{a}\mathrm{n}\left({\gamma }_{max}\right) {\mathit{r}}_{I,z}\left(t\right) \]$

(14)

여기서 식 (9)의 Running cost $$ \mathrm{l}\left(\mathit{u}\right)$$는 제어 입력의 에너지를 최소화하도록 다음과 같이 설계된다.

$\[ l\left(\mathit{u}\right)=w_f{‖{\mathit{f}}_B‖}^2+w_M{‖{\mathit{M}}_B‖}^2 \]$

(15)

식 (15)서 w_f는 추력에 대한 w_M는 모멘트에 대한 가중치이며 이러한 quadratic form 비용함수는 추력과 모멘트의 크기를 최소화함으로써 에너지 효율적인 착륙 궤적을 유도한다. 식 (10)은 쿼드로터의 6-DOF 연속시간 동역학 제약 조건이며 식 (11)는 경계 조건으로, $$ {\mathit{x}}_{\mathit{I}}$$는 초기 상태, $$ {\mathit{x}}_{\mathit{F}}$$는 목표 착륙 상태를 나타낸다. 식 (12)은 추력의 각 최소와 최대를 나타내는 nonnegative orthant 제약조건이다. 식 (13)은 추력 방향에 대한 SOC 제약조건으로, θ_max는 동체 z축으로부터 허용되는 최대 기울기 각이며, 식 (14)는 착륙 원뿔 제약조건으로 γ_max는 원뿔의 기울기 파라미터이다.

이 문제를 해결하기 위해 본 연구에서는 CoManDO 솔버를 사용한다. CoManDO는 [2]에서 소개된 DDP를 Primal-Dual Augmented Lagrangian Interior Point 방법을 통하여 식 (12)-(14)와 같은 복잡한 제약조건을 logarithmic barrier function을 통해 효율적으로 처리한다.

3.3.1 추력 제약조건(Thrust Cone Constraint)

추력 제약조건에 대해 우리는 식 (2)에서 제어 입력을 동체 좌표계에서 3차원 추력 벡터 $$ {\mathbf{f}}_B={\left[f_{B,x}, f_{B,y}, f_{B,z}\right]}^T$$를 모델링한다. 이러한 벡터 표현 방식은 개별 모터의 추력을 직접 변수로 사용하는 방식에 비해 최적화 과정에서 3차원 합성 추력 벡터 f를 최적화함으로써 변수의 개수를 줄여 최적화 문제의 차원을 감소하여 계산 효율성을 높인다. 모터 수준의 세부사항에 기반하여 개별 모터 명령으로 변환하는 과정 control allocation 문제와의 분리를 가능하게 해를 조기에 제약하지 않고, 실현 가능한 추력 공간을 탐색할 수 있는 최대 자유도를 제공한다.

그러나 쿼드로터는 네 개의 고정된 모터를 통해 추력을 생성하므로, 합성 추력 벡터가 임의의 방향을 가질 수 없다는 물리적 제약이 존재한다. 각 모터는 동체 좌표계의 z축과 평행한 방향으로만 추력을 생성하며, 네 모터의 추력을 합성하여도 결과적인 순 추력 벡터는 주로 z축 방향을 유지하게 된다. 이러한 물리적 한계를 수학적으로 표현하기 위해, 본 연구에서 [Fig. 1]에 나타난 바와 같이 식 (13)로 SOC 제약조건을 적용한다.

$\[ {‖\begin{array}{c}{f}_{B,x}\left(t\right)\hfill \\ f_{B,y}\left(t\right)\hfill \end{array}‖}_2\le \tan \left({\theta }_{max}\right){ f}_{B,z}\left(t\right) \]$

(16)

[Fig. 1]

Constraint Diagram: A diagram showing the quadrotor landing cone (green) and the thrust vector tilt cone (blue)

식 (16)에서 θ_max는 동체 z축으로부터 순 추력 벡터의 기울어질 수 있는 최대 각도를 나타내며, 고정 모터 배치에서 생성 가능한 추력 방향만을 허용한다. 즉, θ_max = 30°으로 설정하면, 순 추력 벡터가 z축으로부터 30° 이상 기울어지는 해는 최적화 과정에서 배제되도록 유도한다. 이러한 접근법은 별도의 명시적인 자세 제약 없이도 안정적인 비행 자세를 유도할 수 있다.

3.3.2 착륙 원뿔 제약조건(Landing Cone Constraint)

착륙 원뿔 제약은 비행체가 착륙 지점을 향해 하강할 때 지정된 원뿔 영역 내에서 접근하도록 식 (14)와 같이 제한한다. 이는 장애물 회피와 안전한 하강 궤적 확보에 필수적이며, 특히 제한된 공간이나 복잡한 환경에서의 착륙 시 중요한 역할을 한다. 본 제약조건은 SOC 제약조건으로 정의할 수 있다.

$\[ {‖\begin{array}{c}{r}_{I,x}\left(t\right)\hfill \\ r_{I,y}\left(t\right)\hfill \end{array}‖}_2\le \tan \left({\gamma }_{max}\right){ r}_{I,z}\left(t\right) \]$

(17)

식 (14)에서 $$ {\mathbf{r}}_{I,xy}\left(t\right)$$는 식 (17)과 같이 $$ {\mathbf{r}}_{I,xy}\left(t\right)={\left[r_{I,x}\left(t\right), r_{I,y}\left(t\right)\right]}^T$$로 관성 좌표계에서의 수평 위치를 나타내고, $$ r_{I,z}\left(t\right)$$는 고도를 나타내며 $$ {\gamma }_{max}\in \left(0^{\circ },{90}^{\circ }\right)$$는 [Fig. 1]와 같이 수직 z축으로부터의 최대 원뿔 반각을 의미한다. 이 제약조건은 비행체가 착륙 지점을 정점으로 하는 inverted cone 영역 내에서만 위치하도록 제한하며, 갑작스러운 궤적 변화나 급격한 각도의 하강을 방지한다. 원뿔 제약조건의 기하학적 특성은 비행체가 착륙 지점에 가까워질수록 허용 가능한 수평 편차가 선형적으로 감소하며 최종 접근 단계에서 위치 정확도를 자연스럽게 높이는 효과를 가져온다.

4. 실 험

본 장에서는 제안된 솔버가 쿼드로터 착륙 임무 수행 시 적용하는 SOC 제약 기반 궤적 계획 기법의 실험적 검증 결과를 다룬다. 먼저, 최적화 문제의 설정과 관련된 파라미터 선택, 제약조건 구성, 그리고 초기 상태 및 목표 상태 정의 등 솔버 구성 절차와 설정 과정을 구체적으로 기술한다. 이후, Crazyflie 2.1 쿼드로터 플랫폼을 이용하여 동일한 설정 하에서 수행된 시뮬레이션 환경 실험과 실제 하드웨어 실험 결과를 비교 및 분석함으로써 제안된 궤적 계획의 현실 적용 가능성과 일관성을 검증한다. 실제 하드웨어 실험에서는 [Fig. 2]의 3개의 Lighthouse 베이스 스테이션을 이용한 위치 측정 시스템으로 전역 위치와 자세를 보정하였다. 온보드 STM32F405RG MCU 에서EKF는 BMI088 IMU(가속도 및 자이로) 데이터를 약 500 Hz로, Lighthouse 위치/자세 데이터를 약 60 Hz로 융합하여 위치, 속도, 자세, 각속도를 추정한다. 이후 무선 라디오를 통해 위 장에서 생성된 궤적을 feedforward reference로 온보드 제어기에 전달되어 추종된다. 전송된 궤적을 내부 Mellinger 컨트롤러가 추종하며, 이를 통해 안정적이고 정확한 6D 상태 추정을 기반으로 최적화된 궤적을 수행한다. 마지막으로, 착륙 임무를 위한 SOC 제약조건을 활용하여 초기 기울기 각도로부터 회복하는 모습을 보인다.

[Fig. 2]

Crazyflie Lighthouse setup: three base stations and a geometrically arranged sensor deck enable fully onboard 6-DoF pose estimation[6]

4.1 SOC 제약 착륙(SOC Constrained Landing)

4.1.1 솔버 출력 결과(Solver Outputs)

본 연구에서는 Crazyflie 2.1 마이크로 쿼드로터를 사용한 정밀 착륙 임무에서 궤적 최적화 프레임워크를 확인한다. 비행체 파라미터와 최적화 설정은 [Table 1]에 상세히 나와 있다. 초기 상태는 $$ {\mathbf{r}}_I\left(0\right)={\left[\mathrm{1.0,1.0,1.0}\right]}^{\mathrm{\top }}$$ m로 설정되며 직립 자세(upright orientation)와 속도 0를 가진다. 다음으로 종단 등식 제약조건(terminal equality constraints)을 사용하여 비행체가 직립 자세로 원점에 속도를 멈추며 도달하도록 유도한다.

최적화 문제 식 (9)은 솔버를 통해 해결되는 안전하고 실현 가능한 궤적 실행을 보장하기 위해 여러 SOC 제약조건을 통합한다. θ_max = 40°를 갖는 추력 벡터 기울기 제약조건 식 (13)은 고정 모터 구성에서 실현 가능한 한계 내에서 순 추력 방향이 유지되도록 보장한다. 전체 추력 한계는 f_{max, total} = 0.64 N로 설정하였다. γ_max = 60°를 갖는 착륙 원뿔 제약조건 식 (14)은 접근 각도를 제한하여 지나치게 가파른 하강을 방지한다.

이러한 제약조건들의 조합은 물리적으로 실현 가능하면서도 최적에 가까운 착륙 궤적을 생성한다. 해당 궤적은 0.0616초의 연산시간을 통하여 생성이 되었으며 착륙 원뿔 제약조건은 비행체가 착륙 지점에 접근하면서 점진적으로 수평 편차를 줄이도록 강제하며, 추력 벡터 기울기 제약조건은 모터의 물리적 한계를 존중한다. 추력 크기 제약조건은 모터의 최대 출력을 초과하지 않도록 보장한다. 결과적으로 얻어진 최적 궤적은 [Fig. 3]에 나타나 있으며, 궤적은 초기 조건에서 시작하여 착륙 원뿔 내에서 하강하며, 모든 제약조건을 만족하면서 부드럽고 효율적인 착륙 궤적을 보여준다.

[Fig. 3]

Optimal trajectory visualization: The red line shows the optimal trajectory computed, including position and orientation of the quadrotor, with visual indication of the landing cone

4.1.2 시뮬레이션 실험 결과(Simulation Results)

생성된 궤적은 실제 하드웨어로의 실험에 앞서 시뮬레이션을 통하여 궤적과 통신 파이프라인의 정상 작동을 검증한다. 본 시뮬레이션은 사용 예정 하드웨어인 Crazyflie 드론과 통신을 담당하는 CFlib 라이브러리 호환이 가능한^[7] SITL (Software-In-The-Loop) Gazebo 시뮬레이션을 사용한다. 이런 시뮬레이션은 생성된 궤적이 실제 쿼드로터의 동역학 하에서 추적이 가능한지 확인하고 궤적을 전송하는 스크립트를 확인함으로써 실제 하드웨어의 손상을 방지한다.

솔버에서 생성된 궤적은 CSV 파일로 저장되며, CFlib Python API를 통해 시뮬레이션된 드론에 전송된다. CFlib는 Crazyflie 드론과 무선으로 통신하기 위한 고수준 API를 제공하며, 명령 전송, 텔레메트리 수신, 파라미터 관리 기능을 지원한다. 제어기로는 자세를 포함한 궤적을 사용할수 있게 Crazyflie 펌웨어에 내장된 Mellinger 제어기를 사용하며 250 Hz 자세 제어기와 100 Hz 위치 제어기로 구성되어 full-state control을 제공한다^[8]. [Fig. 4]에서 Gazebo내에서의 비행 결과는 [Fig. 5]에 나타나 있으며 통신 지연과 물리적 동역학에 의한 약 0.9초 시간 지연이 존재하지만, 시뮬레이션에서 계획된 궤적의 위치 및 자세를 성공적으로 추종함을 보여준다. 이를 통해 제안된 궤적 생성 및 제어 파이프라인이 실제 하드웨어에 적용 가능한 수준의 안정성과 일관성을 가짐을 확인하였으며, 동일한 스크립트를 기반으로 이후의 실제 하드웨어 실험을 수행하였다.

[Fig. 4]

Gazebo simulation tracking: Capture of the Crazyflie tracking the planned trajectory full state in a Gazebo simulation environment

[Fig. 5]

Trajectory comparison: optimal planned trajectory (red), Gazebo-simulated trajectory (blue), and hardware execution (green)

4.1.3 실제 하드웨어 실험 결과(Real-World Hardware Results)

실험은 SOC 제약조건 하에서 생성된 6-DOF 궤적을 쿼드로터가 하드웨어에서 안정적으로 추종할 수 있는지 확인하기 위하여 진행하였다. 하드웨어는 [Fig. 2] Lighthouse 위치 측정 시스템을 갖춘 Crazyflie 2.1을 사용하여 수행되었다. 사전 계산된 궤적은 시뮬레이션에서 사용한 동일한 CFlib 스크립트를 통해 전송되었으며, 동일한 Mellinger 제어기에 의해 온보드로 처리되었다.

[Fig. 6]는 하드웨어 실험의 궤적을 보여준다. CoManDO 솔버가 feedforward reference generator로서 작동하면서, 쿼드로터로부터 기록된 데이터로부터 [Fig. 5]는 계획된 궤적과 시뮬레이션에서 기록된 궤적과 하드웨어 실험에서의 기록된 궤적을 비교한다. 그래프에서 세 궤적 간의 차이가 시각적으로 관찰이 되며 특히 하드웨어 실험 결과는 약 1.4초의 시간 지연을 보였으며, 이는 Crazyradio를 통한 데이터 전송 지연과 실제 시스템의 응답 특성에 의한 것이다.

[Fig. 6]

Trajectory tracking snapshot: Hardware experiment showing a Crazyflie 2.1 quadrotor trajectory using Lighthouse positioning

궤적 추종 성능을 정량적으로 평가하기 위해, 하드웨어 실험 데이터에 1.4초 시간 지연을 보정한 후 계획된 궤적과의 오차를 분석하였다. [Fig. 7]는 위치와 자세에 대한 오차를 나타낸다. 본 연구에서 사용한 Mellinger 제어기는 Crazyflie 플랫폼 기반의 Gazebo 시뮬레이션 환경에서의 제어기 비교 연구^[8] 및 실제 하드웨어 실험^[9]을 통해 그 궤적 추종 성능이 검증된 바 있다. 특히 연구^[9]에서 Crazyflie에서의 figure-eight 궤적 추종 시 평균 위치 평균 위치 오차 0.11m, 평균 자세 오차 1.93°를 보고하였으며, 본 실험의 평균 위치 오차 및 평균 자세 오차는 이러한 보고된 성능 범위 내에서 안정적인 궤적 추종이 이루어졌음을 확인한다. 이러한 추적 오차는 SOC 제약조건 하에서 생성한 6-DOF 궤적이 하드웨어에서 효과적으로 위치 및 자세 제어가 이루어졌으며, 이로 자세 동역학을 고려한 SOC 제약 궤적 최적화 기법을 실험적으로 입증하였다. 해당 실험의 영상은 온라인 자료에서 확인할 수 있다

[Fig. 7]

Hardware experiment tracking error results: Box plot illustrating the distribution of position and attitude tracking errors

• https://drive.google.com/file/d/1NEhGoJlFfV_upKv9986oKnoeupGIeLhn/view?usp=drive_link

그러나, SOC 제약 착륙 임무의 안전한 초기 상태와 종단 등식 제약조건으로 인해, 착륙 중 추력 벡터 기울기에 대한 SOC 제약조건의 활성화를 시각적으로 명확하게 입증하는 것은 어렵다. 이는 초기 조건의 안정성을 만족하는 상태에서 시작하기에, 제약조건이 실제로 최적화 과정에서 능동적으로 작용하는 것을 관찰하기 위해서는 더 도전적인 시나리오가 필요함을 시사한다.

4.2 기울어진 초기 상태 복구

안전한 착륙 궤적 생성 내에서 추력 벡터의 SOC 제약조건의 효과를 입증하기 위해서 다음과 같은 기울어진 초기 조건을 복구하며 착륙하는 실험을 수행한다. [Fig. 8]은 각각 동일한 초기조건에서 위의 그림은 추력 SOC 제약조건 θ_max = 30°이 활성화된 궤적 결과이며 아래는 이 제약 조건이 없는 상태에서의 궤적 결과이다. 두 궤적 모두 동일한 초기조건 terminal equality constraint, 착륙 SOC 제약조건이 동일하게 적용이 되었다. 기울어진 초기조건 내에서의 문제를 해결하기 위하여 착륙 SOC 제약조건의 γ_max는 70°으로 증가하였다.

[Fig. 8]

3D Recovery from an initially tilted state in 3D. With the SOC thrust constraint (top), the vehicle regains nominal orientation; without the constraint (bottom), it fails to do so

계획된 궤적 전체에 걸쳐 제약조건의 만족을 검증하기 위하여, 각 타임스텝에서 솔버 출력으로부터 상태 벡터를 추출하고 확인한다. 착륙 원뿔 제약조건에 따라 각 지점에서, 착륙 목표로부터의 수평 거리를 해당 고도에서 허용되는 최대 수평 거리와 비교하여 쿼드로터가 원뿔 경계로부터 얼마나 떨어져 있는지를 측정한다. 원뿔은 고도가 증가함에 따라 넓어지므로, 더 높은 위치에서는 더 큰 측면 편차가 허용된다. 이러한 비교는 [Fig. 9] 내의 위의 그래프를 통하여 비행체가 원뿔 제약조건 내에서 얼마나 안전하게 동작하고 있는지를 정량적으로 나타낸다.

[Fig. 9]

Trajectories with the SOC thrust constraint (blue) and without (purple). The red-shaded area marks the landing-cone and thrust-tilt limits (infeasible region)

비행체 기울기 각도(Vehicle Tilt Angle)은 비행체의 자세를 쿼터니언 상태를 통해 표현되며, 이는 동체 좌표계에서 관성 좌표계로의 회전을 나타낸다. 이 쿼터니언으로부터, 관성 좌표계에서 나타나는 비행체의 동체 z축 방향(쿼드로터의 “위” 방향)을 추출하며 동체 z축이 수직 관성 z축으로부터 벗어난 각도 편차로 계산된다. 0°의 기울기 각도는 비행체가 완벽하게 직립 상태임을 의미하며, 더 큰 각도는 비행체가 기울어져 있음을 나타낸다. 이러한 각도를 통하여 비행체가 추력 벡터 제약조건에 의해 부과된 θ_max = 30° 기울기 한계를 존중하는지를 [Fig. 9]의 아래 그래프를 통하여 직접적으로 보여준다. 제약조건 경계로부터 충분히 떨어져 있는 구간에서도, 제약조건이 존재하는 경우와 없는 경우의 궤적이 달라지며 이는 SOC 제약조건이 barrier function 형태로 최적화 문제에 포함되어, 제약조건에 근접하지 않은 구간에서도 미래 제약 만족을 고려하여 안전한 경로를 선택하기 때문이다.

추력 SOC제약이 포함된 궤적은 안정적으로 안전한 각도를 찾아가며 착륙을 하는 모습을 보이는 반면, 제약조건이 없는 궤적은 제한된 각도 이상을 벗어나지는 않지만 최적화 과정에서 제약을 고려하지 않기 때문에 비행 도중 안정적인 자세를 찾지 못하며 불안전한 착륙을 시도하며 해당 추력 제약조건의 중요성을 보여준다. 만약 cost function에 추가적인 자세 항을 포함할 경우, SOC 제약조건은 정상 상태로 향하는 과정에서 과도한 추력 벡터 방향을 제한하는 역할을 수행할 수 있다. 그러나 본 연구에서는 cost function을 선택하거나 수정하지 않고도, 물리적 제약조건만으로 충분한 자세 안정성을 확보할 수 있음을 확인하였다.

5. 결 론

본 논문은 기존 IPDDP를 확장해 개발한 자율로봇 궤적 최적화 솔버 CoManDO를 활용하여, SOC 제약을 직접 처리하는 정밀 착륙용 6-DOF 궤적 최적화 프레임워크를 제안한다. CoManDO는 프라이멀–듀얼 보강 라그랑지안 기반 내부점법을 통해 SOC 제약을 logarithmic barrier로 내재화함으로써, 최적화 과정에서 착륙 원뿔과 추력 원뿔 제약을 명시적으로 집행한다. 우리는 두 제약을 포함한 6-DOF 쿼드로터 착륙 문제를 정식화하고, 쿼터니언 기반 자세 동역학을 명시적으로 포함해 위치–자세를 통합적으로 최적화하였다. Crazyflie 2.1 플랫폼을 이용한 시뮬레이션과 실환경 하드웨어 실험 결과, 큰 초기 기울기(예: roll 30°) 복구 시나리오에서 제약 미적용 또는 자세 비용 부재 시 실패하던 사례가 SOC 제약 적용 시 안정적 착륙으로 개선되어, 제약-유도(attitude-by-constraint) 자세 궤적 생성·제어의 실효성을 입증하였다. 평균 계산 시간은 약 0.0616 s (≈61.6 ms)로, 실시간 적용 가능성을 시사한다.

향후 연구로는 (i) 이동 플랫폼, 동적 장애물 등 시간에 따라 변화하는 환경과 바람·충격과 같은 외란에 대응하는 온라인 재계획(online replanning)으로의 확장, (ii) 다양한 착륙 표면·지형·센서 조건을 고려한 강건성 검증 및 성능 한계 분석, (iii) 장애물 회피, 노즐 포화, 충돌 회피 등 추가 물리 제약을 포함한 복합 시나리오로의 일반화 및 다중 목표 최적화 등을 수행하여, 제안 프레임워크를 보다 범용적인 차원 자율 착륙·접근 제어 플랫폼으로 확장할 계획이다.

Acknowledgments

This work is supported by the National Research Foundation of Korea (NRF) (RS-2025-00560008, RS-2025-25443510).

References

• M. Kazim, J. Hong, M.-G. Kim, and K.-K. Kim, “Recent advances in path integral control for trajectory optimization: An overview in theoretical and algorithmic perspectives,” Annual Reviews in Control, vol. 57, p. 100931, 2024. [https://doi.org/10.1016/j.arcontrol.2023.100931]
• M.-G. Kim and K.-K. Kim, “An extension of interior point differential dynamic programming for optimal control problems with second-order conic constraints,” Transactions of the Korean Institute of Electrical Engineers, vol. 71, no. 11, pp. 1666-1672, 2022. [https://doi.org/10.5370/KIEE.2022.71.11.1666]
• C. R. Hayner, J. M. Carson III, B. Açıkmeşe, and K. Leung, “Continuous-time line-of-sight constrained trajectory planning for 6-degree of freedom systems,” IEEE Robotics and Automation Letters, vol. 10, no. 5, pp. 4332-4339, 2025. [https://doi.org/10.1109/LRA.2025.3545299]
• Z. Shen, G. Zhou, H. Huang, C. Huang, Y. Wang, and F.-Y. Wang, “Convex optimization-based trajectory planning for qua drotors landing on aerial vehicle carriers,” IEEE Transactions on Intelligent Vehicles, vol. 9, no. 1, pp. 138-150, 2024. [https://doi.org/10.1109/TIV.2023.3327263]
• S. Schoedel, K. Nguyen, E. Nedumaran, B. Plancher, and Z. Manchester, “Code generation for conic model-predictive control on microcontrollers with TinyMPC,” arXiv preprint arXiv:2403.18149, 2024, [Online], Available: [https://doi.org/10.48550/arXiv.2403.18149]
• Bitcraze, “Lighthouse Positioning System,” [Online], Available: https://www.bitcraze.io/documentation/system/positioning/ligthouse-positioning-system/, , Accessed: Oct. 23, 2025.
• C. Llanes, Z. Kakish, K. Williams, and S. Coogan, “CrazySim: a software-in-the-loop simulator for the Crazyflie Nano Quadrotor,” 2024 IEEE International Conference on Robotics and Automation (ICRA), Yokohama, Japan, 2024, pp. 12248-12254. [https://doi.org/10.1109/ICRA57147.2024.10610906]
• A. E. Asslouj and H. Rastgoftar, “Quadcopter Tracking Using Euler-Angle-Free Flatness-Based Control,” European Control Conference (ECC), Bucharest, Romania, 2023, pp. 1-6. [https://doi.org/10.23919/ECC57647.2023.10178337]
• A. Molchanov, T. Chen, W. Hönig, J. A. Preiss, N. Ayanian, and G. S. Sukhatme, “Sim-to-(Multi)-Real: transfer of low-level robust control policies to multiple quadrotors,” IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS), Macau, China, 2019, pp. 59-66. [https://doi.org/10.1109/IROS40897.2019.8967695]