Visual-Inertial Navigation System on Matrix Lie Group with Semantic Objects

Abstract

A visual-inertial navigation system (VINS) estimates a state of a moving platform based on visual and inertial sensing: The state includes the position and orientation of the platform and its surrounding map. This has been a backbone of perception systems in autonomy in which accurate and real-time state estimation is indispensable for safe operation. Despite of breakthroughs in literature, vulnerability to degraded conditions hinders its deployment in a fail-critical system. To bring VINS in fail-critical systems one step closer, the estimator should be fail-safe meaning it outputs reliable estimates under any circumstances and fail-aware so that failures can be automatically detected for recovery.

To this end, this study develops VINS under three design principles. First, the estimator outputs estimation confidence as well as expected value based on sensor uncertainties. State uncertainty is valuable information for itself acting as an index to decide perception failure and for downstream tasks, such as path planning and feedback control. Second, the state-space is modeled on matrix Lie groups: This representation is a natural tool to propagate uncertainty in rigid body motion. Third, semantic objects are incorporated so that this high-level visual feature provides robustness to appearance and viewpoint changes.

Under the design principle, the theoretical contributions of this study are introducing the optimal image gradient, object-based SLAM formulation, and Gaussian merge on matrix Lie groups. The optimal image gradient minimizes the expectation of the linearization error squared within project uncertainty in an image domain. In object-based SLAM, the unobservable subspace is derived analytically to prove that the bases do not depend on linearized points so that the estimator yields consistent state uncertainty. In addition, a Gaussian midway-merge method is introduced to fuse Gaussian distributions on matrix Lie groups. Object-based SLAM with the presented merge addresses ambiguous object poses due to shape symmetry. As a technical contribution, this study finally integrates all developed elements to build an object-based VINS.

Through intensive simulations and real-world experiments, the optimal image gradient coupled with the photometric visual-inertial odometry shows robustness to the large initial velocity error up to 3m/s. This shows a clear contrast to the conventional approach in which it fails to cope with such a large initial error. Object-based SLAM formulation on matrix Lie groups yields consistent estimates giving an average normalized estimation error squared as 1.07, while the conventional method gains spurious information along the yaw direction. Ambiguity-aware object SLAM mitigates the large rotation error along the symmetric axis from a six-dimensional pose detector by 49.5% averaged over 10k images. Lastly, this study shows that the integrated system achieves cm-level localization and object mapping accuracy in a room-scale environment.

영상관성항법 시스템은 영상 및 관성 측정치를 기반으로 항체의 상태변수, 즉 위치, 자세 그리고 주변 지도를 추정한다. 안전한 자율 시스템을 위해서는 정확하고 실시간성이 보장되는 영상관성항법과 같은 상태변수 추정기가 필수적이다. 최근 많은 연구에도 불구하고 센서 성능이 저하되는 도전적인 상황에서 고장에 치명적인 시스템에 영상관성항법을 적용하기에 어려움이 있었다. 이를 위해서는 어떠한 환경에서도 안정적인 추정치를 출력할 수 있게 고장에 대해 강건해야 하며 고장이 발생하더라도 이를 자동으로 감지하여 회복할 수 있어야 한다.

본 논문에서는 강건한 영상관성항법 시스템을 설계하고자 세 가지의 연구 원칙을 제시한다. 첫번째로 해당 시스템은 상태변수에 대한 추정치 뿐만 아니라 센서의 불확실성을 고려하는 유효한 추정 신뢰도를 출력해야 한다. 추정된 불확실성은 그 자체로서 고장 감지를 위해 활용될 수 있고 경로 계획과 되먹임 제어와 같은 후속 작업에도 활용될 수 있다. 두번째로 상태변수 공간은 강체 운동의 불확실성을 전파하기에 가장 알맞은 리그룹에서 모델링되어야 한다. 마지막으로, 개발된 시스템은 겉보기 및 시점 변화에 강건한 의미론적 물체를 항법 측정치로서 활용할 수 있어야 한다.

앞서 제시한 연구 원칙에 입각하여 본 논문에서는 최적 이미지 기울기, 물체 기반 동시적 위치 추정 및 지도 작성 (SLAM) 그리고 리-행렬군에서의 정규분포 융합 방법을 제시한다. 최적 이미지 기울기는 이미지 영역에서 투영 불확실성에 대하여 선형화 오차 제곱의 기대값을 최소화 하게 설계되었다. 물체기반 SLAM에서는 비가관측 공간이 선형화 지점에 의존적이지 않음을 해석적으로 증명하여 추정기가 유효한 불확실성을 출력함을 보였다. 또한, 리-행렬군 상의 정규분포를 융합하는 방법을 제시하고 이를 대칭성이 포함된 물체기반 SLAM에 적용하여 측정치의 모호성을 해결하였다. 최종적으로 본 논문에서 제안하는 방법들을 통합하여 물체기반 영상관성시스템을 제안한다.

본 논문에서는 시뮬레이션 및 실제 실험을 통해 제안한 방법들의 타당성을 검증하였다. 영상관성 오도메트리에서 기존 방법은 최대 3m/s 속도 오차에서 수렴이 실패한 반면 제안한 최적 기울기를 적용할 경우 추정기가 성공적으로 수렴함을 보였다. 물체기반 SLAM에서 기존의 접근 방법은 추정치의 비일관성 문제 때문에 중력에 대한 회전 방향에 대해 유효한 신뢰도를 추정하지 못하였다. 하지만 본 연구에서는 물체기반 SLAM을 리그룹에서 모델링 하였고 이를 통해 유효한 추정 신뢰도를 추정할 수 있음을 보였다. 또한, 물체기반 SLAM의 대칭 모호성을 해결함으로써 10,000장의 이미지에 대해 6자유도 포즈 검출기보다 평균적으로 자세 오차를 49.5% 감소시켰다. 마지막으로 물체기반 영상관성항법 시스템을 제안하여 방 크기의 실내 환경에서 cm 수준의 항법 및 지도 작성이 가능함을 보였다.