30.3.3.2. 이동(Waypoint): 3차원 공간상의 경로점 간 유클리드 거리(Euclidean Distance) 기반 벡터장(Vector Field) 생성

자율 비행의 가장 기본적이고 빈번하게 사용되는 형태는 지상 관제 시스템(GCS)에서 지정한 경유지(Waypoint)들을 순차적으로 방문하는 MAV_CMD_NAV_WAYPOINT 이동 명령어이다. 하지만 PX4-Autopilot의 내부 제어 메커니즘 관절부에서는, 단순히 “목표 위도/경도를 향해 날아가라“는 1차원적인 지시가 내려가지 않는다. 하위 위치 제어기(FlightTaskAuto)는 이전 웨이포인트(Previous)와 현재 목표 웨이포인트(Current) 사이를 잇는 3차원 유클리드 공간 공간선(Euclidean Spatial Line)을 기준 궤적(Reference Trajectory)으로 삼고, 기체를 이 선상에 묶어두는 매끄러운 유동적 벡터장(Vector Field)을 수학적으로 구축한다.

1. 궤적 기반 경로 추종(Trajectory-based Path Following)의 수학적 근거

단순히 목표점 $\mathbf{P}_{current}$ 을 향해 기수를 돌리고(Yaw) 전진만 수행하는 방식은 측면에서 부는 바람(Crosswind)에 극도로 취약하다. 이런 원시적인 유도 방식 하에서는 기체가 포물선을 그리며 오차를 누적하다가 목표물 부근에서 궤적을 심하게 뒤트는 현상(Dog-leg Effect)이 발생한다.

이를 억제하기 위해 PX4는 크로스 트랙 에러(Cross-track Error) 방식을 도입한다. 이전 목표점 $\mathbf{P}_{prev}$ 와 현재 목표점 $\mathbf{P}_{current}$ 을 공간 상에 직선으로 잇고, 현재 기체의 위치 $\mathbf{P}_{uav}$ 가 이 이상적인 직선 경로에서 얼마나 삼각투영 오차(Orthogonal Distance)만큼 벗어나 있는지를 계산하는 것이다.

방향 벡터(Direction Vector): $\vec{d} = \frac{\mathbf{P}_{current} - \mathbf{P}_{prev}}{||\mathbf{P}_{current} - \mathbf{P}_{prev}||}$
진행 거리(Along-track Distance): $s = (\mathbf{P}_{uav} - \mathbf{P}_{prev}) \cdot \vec{d}$
궤적 이탈 오차(Cross-track Error): $\vec{e}_{xtrack} = \mathbf{P}_{uav} - (\mathbf{P}_{prev} + s \cdot \vec{d})$

멀티로터의 위치 제어 모듈에서는 이 오차 $\vec{e}_{xtrack}$ 를 0으로 수렴시키기 위해 P-게인(Proportional Gain, MPC_XY_TRAJ_P 등)을 곱해 수직 복원 속도(Orthogonal Restoring Velocity)를 생성하고, 여기에 원래의 전진 순항 속도(Cruise Speed)를 벡터 합성하여 최종 이동 속도 지령치(Velocity Setpoint)를 만든다.

2. 벡터장 생성 도식화 (Mermaid Diagram)

다음은 멀티로터나 헬리콥터 비행체가 두 점 사이를 비행할 때 발생하는 속도 벡터의 기하학적 합성을 나타낸 다이어그램이다.

graph TD
    Prev[Previous Waypoint P_prev] -->|Ideal Trajectory Vector| Curr[Current Waypoint P_curr]
    UAV((UAV Position)) -->|Orthogonal Projection| Traj(Point on Trajectory)
    
    Traj -->|Cross-track Error| UAV
    
    UAV -->|V_cross: Restoring Velocity| Traj
    Traj -->|V_cruise: Feedforward Velocity| Curr
    
    Note1[Final Velocity Setpoint = V_cross * P_Gain + V_cruise]

이러한 벡터장(Vector Field) 설계로 인해, 기체가 강한 측풍으로 궤적을 이탈하더라도 단순히 머리(Heading)를 목표점으로 향하는 대신, 궤적 원상 복구를 위한 평행 이동(Crabbing) 모션을 동반하게 된다.

3. Ardupilot의 WPNav와의 설계 비교

Ardupilot은 AC_WPNav 라이브러리를 통해 웨이포인트 내비게이션을 수행하며, 궤적 생성을 위해 스플라인(Spline) 수학 및 키네마틱(Kinematic) 셰이퍼(Shaper)를 깊숙하게 활용한다. Ardupilot은 다소 보수적이고 엄격한 운동학(Kinematics) 제한을 걸어, 궤적이 매끄러우나 빠른 외란에 대한 즉각적이고 동력학적인 반응이 다소 느린 편이다.

반면 PX4는 FlightTaskAutoMapper 내에 저크 수렴(Jerk-limited) 알고리즘을 구현하여 비선형적으로 목표점을 재계산한다. 즉, PX4-Autopilot은 이산적인(Discrete) 웨이포인트들을 내부적으로 연속적이고 미분 가능한(Differentiable) 상태량(State) 곡선으로 치환하여 로터의 모터를 매끄럽게 제어한다. 이로인해 GCS 상에 그려진 단순한 다각형 경로는, 시스템 단에서는 사실 관성과 가속도를 고려하여 둥글게 깎인(Fillet) 3D 튜브 형태의 벡터 공간으로 대입된다.

4. 경유지 회전 반경과 감속 프로파일

웨이포인트에 도달했을 때 직각으로 꺾어지는 현상을 방지하기 위해 PX4는 수용 반경(Acceptance Radius, NAV_ACC_RAD)에 진입하기 몇 미터 전부터 삼각함수 연산을 통해 감속 제동(Braking)을 개시하거나, 매끄럽게 다음 목표점(Next Waypoint)으로 이행하는 통과(Pass-through) 로직을 구사한다.

MAVLink 매개변수인 param2(Acceptance Radius) 또는 GCS 상의 회전 반경 허용치 설정값에 대응하여 position_setpoint_triplet_s 내부의 next_valid 필드가 참(True)이면, 현재 웨이포인트에 완전히 정지하지 않고 다음 목표점 방향으로 포물선(Parabolic Curve) 궤적을 그리며 부드럽게 통과한다.

5. 장약(Conclusion) - 벡터장 제어의 엔지니어링 이점

자율 에이전트(Autonomous Agent)가 카메라 기반 장애물 회피(VIO 방식을 차용한 Obstacle Avoidance)를 수행할 때, 기존의 직선 궤적 위에 로컬 플래너(Local Planner)가 임의의 우회점(Reposition/Offboard 포인트)을 주사하더라도, PX4는 이 벡터장 시스템 덕분에 시스템 불안정성을 겪지 않는다. 유클리드 공간의 3차원 에러 계산 로직이 시스템 전반에 깊게 내재화되어 있으므로, 어떠한 좌표가 갑자기 삽입되더라도 즉시 그 좌표를 원점으로 하는 구심(Centripetal) 벡터장을 형성하여 부드럽고 강건한 임무 수행을 이어 나간다.