27.5.5.1. 광학 흐름 센서의 자이로 레이트 보상 및 초점 거리 환산식 연산

27.5.5.1. 광학 흐름 센서의 자이로 레이트 보상 및 초점 거리 환산식 연산

옵티컬 플로우(Optical Flow) 센서 자체는 본질적으로 기체가 “앞으로 평행 이동(Translation)해서 바닥 패턴이 뒤로 흘러간 것“인지, 아니면 “제자리에 떠서 기수를 위로 치켜들어(Pitching up) 바닥 패턴이 뒤로 흘러간 것“인지를 결코 구분하지 못한다. 렌즈에 맺히는 픽셀의 이동 속도(Pixel Flow)만 측정할 뿐이기 때문이다.

이 치명적인 모호성(Ambiguity)을 해결하고 순수한 평행 이동 속도만을 발라내기 위해, PX4-Autopilot의 EKF2는 IMU 자이로스코프(Gyroscope) 데이터와 카메라 렌즈의 기하학적 매개변수(Focal Length)를 결합하는 복합 보상 파이프라인(src/lib/ecl/EKF/optflow_fusion.cpp)을 구동한다. 본 절에서는 이 보상 연산식의 수학적 전개와 C++ 구현을 심층 분석한다.

1. 관측 모델의 분리: 회전 성분과 병진 성분

카메라 이미지 평면에서 측정되는 총 광학 흐름 속도 벡터(\boldsymbol{\omega}_{\text{opt}})는 기하학적으로 다음 두 가지 성분의 합체로 정의된다.

\boldsymbol{\omega}_{\text{opt}} = \boldsymbol{\omega}_{\text{motion}} + \boldsymbol{\omega}_{\text{gyro}}

  1. \boldsymbol{\omega}_{\text{motion}} (병진 흐름): 기체가 X, Y축으로 수평 이동할 때 발생하는 진정한 이동 각속도. (우리가 구해야 하는 목표값)
  2. \boldsymbol{\omega}_{\text{gyro}} (회전 흐름): 기체가 롤(Roll)이나 피치(Pitch) 방향으로 기울어질 때(Rotation) 렌즈가 회전하며 발생하는 가짜 이동 각속도.

EKF는 이 두 성분 중 ’병진 흐름(\boldsymbol{\omega}_{\text{motion}})’만을 예상 관측치(Predicted Measurement)로 취하여 센서 측정값과 비교해야 한다.

2. IMU 자이로 레이트 보상 (Gyro Rate Compensation)

“가짜 흐름“인 \boldsymbol{\omega}_{\text{gyro}} 를 제거하기 위해 PX4는 비행 제어기의 내장 IMU 데이터, 그중에서도 자이로스코프의 초당 회전각(Rates) 데이터를 가져다 쓴다.

기체가 피치(Pitch) 방향으로 1초에 10도(deg) 기울어졌다면, 옵티컬 플로우 센서 역시 지면이 10도만큼 반대 방향으로 흘러간 것으로 인식한다. 따라서 센서 측정값에서 IMU 자이로 값을 미세 조정하여 단순히 빼주기만 하면, 기체의 회전에 의한 시야각 변동이 말끔히 상쇄된다. 이를 자이로 레이트 보상(Gyro Rate Compensation) 이라 부른다.

// 1. 센서가 측정한 총 광학 흐름 속도 (rad/s)
Vector2f flow_rad_s = get_flow_rates();

// 2. 가속도계/자이로스코프 연계 EKF 상태 변수에서 현재 기체 회전 각속도 추출
// (_state.rates는 자이로스코프 Raw 값에서 추정된 바이어스를 뺀 순수 기체 회전율)
Vector2f body_rates_rad_s = _state.rates.xy(); 

// 3. 자이로 보상 연산: 총 측정치에서 기체의 스스로 회전한 속도를 차감
Vector2f flow_compensated = flow_rad_s - body_rates_rad_s;

이 연산 방식을 통해 산출된 flow_compensated 가 바로 기체의 순수한 수평 이동(Translation)만을 대변하는 값으로 정제된다.

3. 원근 조절: 초점 거리와 지상고 환산식 연산

이제 순순한 수평 이동을 나타내는 ‘각속도(rad/s)’ 데이터(flow_compensated)를 획득했다. 하지만 이 데이터를 24차원 상태 벡터 내의 ‘선속도(m/s)’ 변수와 융합하려면, 기하학적 원근 환산(Perspective Projection) 이 필수적이다.

동일한 시속 10km/h로 날아가더라도 지상 1m 높이에 있을 때와 지상 10m 높이에 있을 때 렌즈를 스쳐 지나가는 바닥 패턴의 각속도(Flow Rate)는 10배 차이가 난다. 이 관계는 렌즈의 초점 거리(Focal Length) 파라미터와 지상고를 매개로 정의된다.

3.1 핀홀 카메라 모델(Pinhole Camera Model) 환산

PX4 EKF2 관측 업데이트 수식은 이를 핀홀 모델 역연산으로 풀어낸다.

v_{\text{body}} = \omega_{\text{comp}} \times \text{Distance}_{\text{ground}}

수식 자체는 간단한 곱셈이지만, EKF 야코비안 행렬 안에서는 이것이 예상 관측치(Predicted Measurement) 산출로 들어간다.

// src/lib/ecl/EKF/optflow_fusion.cpp 의 예상 관측치 로직 (의사코드)

// 1. EKF 상태 벡터에서 현재 수평 속도(X, Y)를 Body Frame으로 가져옴
Vector2f vel_body = _R_to_body.slice<2,2>() * _state.vel.xy();

// 2. EKF 고도 상태와 지면 고도 추정치를 이용해 현재 '지상고' 연산
float height_above_gnd = _terrain_vpos - _state.pos(2);

// 3. 렌즈 환산: 속도(m/s)를 지상고(m)로 나누어 예상 분당 각속도(rad/s) 역산
// 예상 관측치(Predicted Measurement) 도출
Vector2f flow_pred;
// 렌즈 투영 반전에 의해 Y축 속도가 X축 흐름을, X축 속도가 Y축 흐름을 유발함
flow_pred(0) =  vel_body(1) / height_above_gnd; 
flow_pred(1) = -vel_body(0) / height_above_gnd;

이렇게 도출된 flow_pred 가 실제 센서 측정치에서 자이로 보상을 마친 flow_compensated 와 비교되어 잔차(Innovation) 행렬을 형성하고, 최종적으로 칼만 게인을 통해 기체 수평 속도 추정치(v_N, v_E)를 갱신하게 된다.

2. 소결

옵티컬 플로우 퓨전 모듈은 단일 센서의 한계를 다종 센서 보상(IMU 자이로 + 거리 측정기 지상고)으로 극복하는 우아한 수학적 앙상블이다. 기체의 회전에 의한 허위 각속도는 IMU 데이터로 걷어내고, 고도에 따른 시야비 왜곡은 거리 센서를 통해 교정함으로써, GPS 신호가 한 칸도 터지지 않는 깊숙한 실내 창고 속에서도 드론이 10cm 오차 이내로 못 박히듯 호버링 할 수 있는 시각적 위치 고정(Visual Position Hold) 성능을 확보해 낸다.

다음 절에서는 2차원 평면 센서인 옵티컬 플로우를 넘어, 외부 보조 컴퓨터에서 연산된 파라미터인 3차원 VIO 오도메트리 데이터를 EKF로 끌어들일 때 발생하는 고질병인 ‘좌표계 회전 오프셋 표류 현상’ 을 어떻게 수학적으로 정렬(Alignment)하고 공분산 행렬에 녹여내는지 살펴본다.