KR20240015464A - 소실점을 이용한 특징선 기반 slam 시스템 - Google Patents

Abstract

본 발명은 소실점을 이용한 특징선 기반 SLAM 시스템에 관한 것으로서, 소실점을 통한 잔차(residual)를 이용하여, 위치 파악 정확도와 매핑 품질을 향상시킬 수 있는 기술에 관한 것이다.

Description

소실점을 이용한 특징선 기반 SLAM 시스템 {Line-feature-based SLAM system using vanishing points}

본 발명은 소실점을 이용한 특징선 기반 SLAM 시스템에 관한 것으로, 더욱 상세하게는 소실점을 이용하는 특징선 기반 SLAM(Simultaneous Localization And Mapping)을 통해서, 구조적 특징을 가지고 있는 실내에서, 정확한 위치 인식 및 지도 생성을 수행할 수 있는 소실점을 이용한 특징선 기반 SLAM 시스템에 관한 것이다.

특징 기반의 SLAM(Simultaneous Localization And Mapping)은 주로 특징점(point feature)을 기반으로 개발되어 왔다.

이는 특징점은 이미지 데이터의 특징을 표현할 수 있는 가장 작은 단위이기 때문에, 연산량이 적은 환경에서 유리한 장점이 있다. 또한, 연속적으로 입력되는 서로 다른 이미지 데이터에서 같은 특징점을 판단하여 이의 움직임 위치를 추적하는 과정이 용이한 장점이 있다.

이에 반해서, 특징점은 텍스쳐(texture)가 적은 환경에서 강력하지 않으며, 조도 변화에 약한 단점이 있다. 또한, 추출한 특징점에 따라 그 분포가 희박할 경우, 이들을 이용하여 3차원 지도를 생성하여 주변 환경을 시각화하기 어려운 문제점이 있다.

이에 따라, 이러한 문제점을 해소하기 위하여, 특징선(line feature) 기반의 SLAM 알고리즘들이 제안되고 있다.

특징선 기반의 SLAM 알고리즘의 경우, 특징점 기반의 SLAM 알고리즘이 갖고 있는 문제점인 텍스쳐가 낮은 환경에서 사용할 수 있을 뿐 아니라, 여러 개의 점으로 구성되어 있는 선이기 때문에, 조도 변화가 발생하더라도 특성이 유지될 가능성이 높다. 또한, 구조적 규칙성을 갖고 있는 선의 특징에 의해, 3차원 지도를 생성할 경우, 주변 환경을 쉽게 식별할 수 있는 장점이 있다.

그렇지만, 이러한 특징선 기반의 SLAM 알고리즘은 특징선의 3D 매핑 과정에서 축퇴(degeneracy) 문제가 발생하게 된다.

축퇴란, 삼각 측량을 통해 단일의 3차원 특징을 결정할 수 없는 것을 의미한다.

일 예를 들자면, 관찰된 2차원 특징점이 에피폴(epipole)에 근접할 때, 3차원 특징점이 베이스 라인에 존재하기 때문에, 하나의 특징점을 특정할 수 없게 된다.

이와 마찬가지로, 관찰된 2차원 특징선이 에피폴에 근접하여 지나갈 때, 3차원 특징선이 에피폴라 평면(epipolar plane)에 존재하기 때문에, 하나의 특징선으로 특정할 수 없으며, 특징선의 경우, 특징점보다 축퇴 현상이 더 자주 발생하기 때문에, 특징선 매핑이 정확하지 않은 문제점이 있다.

Plㆌker coordinate로 표현되는 3차원 선 좌표는 법선 벡터(normal vector)와 방향 벡터(direction vector)로 구성되어 있으며, 종래의 특징선 기반의 SLAM 알고리즘에서는 각 특징선의 법선 벡터만을 보정할 수 있으나, 특징선을 매핑하는 과정에서는 방향 벡터만을 사용하기 때문에, 정확한 3차원 지도를 생성하는 데 어려움이 있다.

이와 관련해서, 국내등록특허 제10-1175922호("천장영역 인식기반 SLAM 알고리즘 구현을 위한 내부구조물 라인 제거 방법")에서는 천장영역 인식기반 SLAM 알고리즘을 구현할 때 내부 구조물의 라인을 벽면과 천장영역 사이의 경계라인과 구분하여 이를 제거할 수 있는 기술을 개시하고 있다.

국내 등록 특허 제10-1175922호 (등록일자 2012.08.16.)

본 발명은 상기한 바와 같은 종래 기술의 문제점을 해결하기 위하여 안출된 것으로, 본 발명의 목적은 특징선(line feature)을 추출한 후, 이를 통해서 소실점(vanishing points) 관측값과 소실점 추정값을 연산하여, 이를 통해서 구조적 특징을 가지고 있는 실내에서, 정확한 위치 인식 및 지도 생성을 수행할 수 있는 소실점을 이용한 특징선 기반 SLAM 시스템을 제공하는 것이다.

본 발명의 일 실시예에 따른 소실점을 이용한 특징선 기반 SLAM 시스템은, 실내 지도를 생성하고자 하는 영역을 연속적으로 이동하는 로봇으로부터 IMU(Inertial Measurement Unit)를 통한 측정 정보와 카메라를 통한 이미지 데이터를 전송받아, 소실점을 이용한 특징선 기반 동시 위치 측정 및 지도 작성을 수행하는 시스템에 있어서, IMU 측정 정보를 이용하여, 사전 통합(pre-integration)을 수행하여 각 이미지 데이터 별 로봇의 위치를 추정하는 제1 처리부, 다수의 이미지 데이터를 각각 순차적으로 분석하여 특징점(point feature)을 추출하고, 각 이미지 데이터마다의 특징점을 트래킹하는 제2 처리부, 상기 다수의 이미지 데이터를 각각 순차적으로 분석하여 특징선(line feature)을 추출하고, 선택되는 어느 하나의 특징선이 새롭게 추출한 특징선인지 이전 이미지 데이터에 의해 추출한 특징선인지 판단하는 제3 처리부, 상기 제3 처리부에 의해 추출한 특징선을 이용하여, 각 특징선들에 의한 소실점(vanishing point)의 위치를 연산하는 제4 처리부 및 상기 제1 처리부 내지 제4 처리부에 의한 결과값들을 이용하여, 기저장된 비용 함수(cost functions)를 통해 최적화를 수행하여, 로봇의 위치 측정과 영역의 지도 작성을 수행하는 최종 처리부를 포함하는 것이 바람직하다.

더 나아가, 상기 제2 처리부는 추정한 로봇 위치와 트래킹한 특징점을 이용하여, 삼각측량(triangulation)을 통해 3차원 특징점을 추정하는 특징점 추정부를 더 포함하는 것이 바람직하다.

더 나아가, 상기 제3 처리부는 추정한 로봇 위치와 추출한 특징선을 이용하여, 삼각측량을 통해 3차원 특징선을 추정하는 특징선 추정부를 더 포함하는 것이 바람직하다.

더 나아가, 상기 최종 처리부는 새롭게 추출한 특징선에 대해 상기 특징선 추정부에 의해 추정한 3차원 특징선을 투영하여, 추출한 특징선과 추정한 3차원 특징선 간의 잔차를 연산하는 구성을 더 포함하는 것이 바람직하다.

더 나아가, 상기 최종 처리부는 상기 특징선 추정부에 의해 추정한 3차원 특징선을 이용하여, 각 3차원 특징선에 의한 소실점의 위치를 연산하여, 연산한 3차원 특징선의 소실점 위치와 상기 제4 처리부에 의한 소실점 위치 간의 잔차를 연산하는 구성을 더 포함하는 것이 바람직하다.

더 나아가, 상기 최종 처리부는 연산한 잔차들을 반영하여, 하기의 비용 함수를 통해 최적화를 수행하여, 로봇의 위치 측정과 영역의 지도 작성을 수행하는 것이 바람직하다.

상기와 같은 구성에 의한 본 발명의 실점을 이용한 특징선 기반 SLAM 시스템은 로봇 등이 이동하면서 탑재되어 있는 카메라를 이용하여 주변 영역의 이미지 데이터를 획득하고, 이를 분석하여 주변 영역의 3차원 지도를 생성하고, 로봇의 위치를 정확도 높게 파악할 수 있는 장점이 있다.

즉, 소실점을 이용함으로써, 위치 파악(인식) 정확도와 3차원 지도의 작성 품질을 향상시킬 수 있다.

도 1은 본 발명의 일 실시예에 따른 소실점을 이용한 특징선 기반 SLAM 시스템의 구성을 나타낸 예시도이다.
도 2는 본 발명의 일 실시예에 따른 소실점을 이용한 특징선 기반 SLAM 시스템에 의한 이미지 데이터 별 추출한 특징선에 의한 잔차를 연산하는 과정을 나타낸 예시도이다.
도 3은 본 발명의 일 실시예에 따른 소실점을 이용한 특징선 기반 SLAM 시스템에 의한 이미지 데이터에서 특징선을 추출하고, 추출한 각 특징선에 대한 소실점의 위치를 연산하는 과정을 나타낸 예시도이다.
도 4는 본 발명의 일 실시예에 따른 소실점을 이용한 특징선 기반 SLAM 시스템에 의한 추출한 소실점에 의한 잔차를 연산하는 과정을 나타낸 예시도이다.
도 5는 본 발명의 일 실시예에 따른 소실점을 이용한 특징선 기반 SLAM 시스템에 의한 각 잔차 변수들의 관계를 나타낸 예시도이다.
도 6은 종래의 특징선 기반 SLAM 시스템과 본 발명의 일 실시예에 따른 소실점을 이용한 특징선 기반 SLAM 시스템에 의한 실제 복도 환경에서 중첩된 LiDAR 포인트 클라우드를 이용한 3차원 선 매핑 결과를 나타낸 비교 예시도이다.

이하 첨부한 도면들을 참조하여 본 발명의 소실점을 이용한 특징선 기반 SLAM 시스템을 상세히 설명한다. 다음에 소개되는 도면들은 당업자에게 본 발명의 사상이 충분히 전달될 수 있도록 하기 위해 예로서 제공되는 것이다. 따라서, 본 발명은 이하 제시되는 도면들에 한정되지 않고 다른 형태로 구체화될 수도 있다. 또한, 명세서 전반에 걸쳐서 동일한 참조번호들은 동일한 구성요소들을 나타낸다.

이때, 사용되는 기술 용어 및 과학 용어에 있어서 다른 정의가 없다면, 이 발명이 속하는 기술 분야에서 통상의 지식을 가진 자가 통상적으로 이해하고 있는 의미를 가지며, 하기의 설명 및 첨부 도면에서 본 발명의 요지를 불필요하게 흐릴 수 있는 공지 기능 및 구성에 대한 설명은 생략한다.

본 발명의 일 실시예에 따른 소실점을 이용한 특징선 기반 SLAM 시스템은, 일명 'UV-SLAM(Unconstrained Line-based SLAM using Vanishing points for structural mapping)'으로서, 추출한 특징선(line feature)에서 얻을 수 있는 소실점(vanishing points)을 사용하여, 기존의 특징선 또는, 특징점(point feature)을 이용한 SLAM이 갖고 있는 단점을 해소할 수 있는 기술에 관한 것이다.

즉, 로봇 등이 이동하면서 탑재되어 있는 카메라를 이용하여 주변 영역의 이미지 데이터를 획득하고, 이를 분석하여 주변 영역의 3차원 지도를 생성하고, 로봇의 위치를 파악하는 것으로서, 보다 정확한 3차원 지도를 그리며, 보다 정확하게 자신의 위치를 파악하는 것이 중요하다.

상술한 바와 같이, 종래에는, 주변 영역의 이미지 데이터를 분석하여 포함되어 있는 특징점 또는, 특징선을 추출하여 3차원 지도와 자신의 위치를 파악하도록 구성되어 있다.

일 예를 들자면, 카메라가 A 지점에서 B 지점을 거쳐 C 지점으로 이동하면서, 각 지점에서 이미지를 획득할 경우, A 지점과 B 지점에서 획득한 이미지를 이용하여 포함되어 있는 특징점 또는 특징선을 분석하고, 이를 통해서 C 지점에서의 특징점 또는 특징선을 추정하고, 추정한 특징점 또는 특징선과 실제 C 지점에서 획득한 이미지에 포함되어 있는 특징점 또는 특징선을 분석하여, 그 오차에 기초하여 자신의 위치 보정과 특징점 또는 특징선을 보정을 수행하여 왔다.

특징점을 이용할 경우, 빛이 변하면 픽셀 정보가 쉽게 변하기 때문에, 같은 특징점이더라도 빛 변화에 따라 다른 특징점으로 인식되는 문제점이 있으며, 텍스쳐가 적은 환경, 즉, 밋밋한 주변 환경에서는 특징점을 추출하는 것이 어려운 문제점이 있다. 또한, 이미지의 특징 일부분만을 사용하는 특징점의 특성 상, 3차원 지도를 생성하더라도 식별 정도가 낮아지는, 다시 말하자면, 특징점 자체가 듬성듬성 추출되기 때문에, 이를 통해서 3차원 지도를 생성하더라도 특징점에 구조적 특징이 없기 때문에 식별도가 낮은 형태의 지도로 생성되게 된다.

또한, 특징선을 이용할 경우, A 지점에서 획득한 이미지에서 a 특징선을 추출하고, B 지점에서 획득한 이미지에서 b 특징선을 추출한 후, a 특징선에서 연장된 평면과 b 특징선에서 연장된 평면이 한 라인에서 만나야 특징선을 정확하게 추출할 수 있는데, a 특징선과 b 특징선이 실제로 같은 선이기 때문에, a 특징선에서 연장된 평면과 b 특징선에서 연장된 평면이 하나의 평면에 가까울 경우, 특징선을 추출하는 데 오차가 발생하게 된다.

일 예를 들자면, 2개의 선이 만나는 점을 판단하는 과정에서, 2개의 선이 서로 동일한 축에 존재할 경우, 만나는 점(접점)을 찾는 것이 어려운 것과 같은 의미한다.

이러한 문제점을 축퇴(degeneracy) 현상이라고 하며, 종래에는 이러한 축퇴 현상을 제거하기 위하여, 선이 에피폴라 선(epipolar line)에 근접할 경우, 가상 점을 사용하여 3차원 선 세그먼트를 재구성하는 기술을 개시하고 있다. 그렇지만, 이 경우, 축퇴된 선들이 다른 선들과 교차하는 경우에만 적용되기 때문에, 제한적으로 적용되게 된다.

본 발명의 일 실시예에 따른 소실점을 이용한 특징선 기반 SLAM 시스템은, 이러한 특징점 또는 특징점 기반의 SLAM 알고리즘이 갖고 있는 문제점을 해소하기 위하여, A 지점에서 획득한 이미지에서 a 특징선과 B 지점에서 획득한 이미지에서 b 특징선을 이용하여 C 지점에서의 추정 소실점을 연산하고, 실제 C 지점에서 획득한 이미지에서 c 특징선을 추출한 후 이를 통한 C 지점에서의 관측 소실점을 연산하여, 이 둘의 오차까지 고려하는 기술에 관한 것이다.

도 1은 이러한 본 발명의 일 실시예에 따른 소실점을 이용한 특징선 기반 SLAM 시스템을 나타낸 구성 예시도로서, 도 1을 참조로 하여 본 발명의 일 실시예에 따른 소실점을 이용한 특징선 기반 SLAM 시스템을 상세히 설명한다.

본 발명의 일 실시예에 따른 소실점을 이용한 특징선 기반 SLAM 시스템은 도 1에 도시된 바와 같이, 제1 처리부(100), 제2 처리부(200), 제3 처리부(300), 제4 처리부(400) 및 최종 처리부(500)를 포함하여 구성되는 것이 바람직하다. 각 구성들은 이동 수단(이동 로봇 등)으로부터 전달되는 이미지 데이터를 분석 처리할 수 있는 컴퓨터를 포함하는 적어도 하나 이상의 연산처리수단에 각각 또는 통합 포함되어 동작을 수행하는 것이 바람직하다.

또한, 본 발명의 일 실시예에 따른 소실점을 이용한 특징선 기반 SLAM 시스템은 실내 지도를 생성하고자 하는 영역(구역 등)을 연속적으로 이동하는 로봇(이동 수단 등)으로부터 탑재되어 있는 IMU(Inertial Measurement Unit)를 통한 측정 정보와 카메라를 통한 이미지 데이터를 이동에 따라 연속적으로 전송받게 된다. 상기 제1 처리부(100), 제2 처리부(200), 제3 처리부(300), 제4 처리부(400) 및 최종 처리부(500)에서 측정 정보와 이미지 데이터를 활용하여, 특징선 기반 소실점을 이용하여 동시 위치 측정 및 지도 작성을 수행하는 알고리즘인 SLAM 시스템을 구현하는 것이 바람직하다.

각 구성에 대해서 자세히 알아보자면,

먼저, 상기 제1 처리부(100), 제2 처리부(200)는 종래의 visual-SLAM 알고리즘에 의해 동작하게 되며, 일 예를 들자면, VINS-mono 알고리즘이 있다.

이에 대해서 간략히 설명하자면, 상기 제1 처리부(100)는 이동 물체의 속도와 방향, 중력 및 가속도를 측정한 IMU 측정 정보를 이용하여, 사전 통합(pre-integration)을 수행하여, 각 이미지 데이터 별 로봇의 위치를 추정하게 된다.

상세하게는, 상술한 바와 같이, 이동 로봇을 통해서 특정 지점마다 각 특정 지점에서 획득한 이미지 데이터를 입력받게 되며, 이 때, 각 특정 지점으로의 이동에 의한 IMU 측정 정보도 같이 전송받게 된다.

사전 통합 과정은 IMU 측정 정보가 전송되는 간격이 이미지 데이터보다 빠르기 때문에, 이미지 데이터가 전송되는 사이에 전송되는 IMU 측정 정보들을 하나로 이미지 데이터와 싱크를 맞추는 과정을 의미한다.

사전 통합 과정에 대한 일 예를 들자면, IMU pre-integration method를 사용하여, 인접한 두 프레임에 대해서 하기의 수학식 1과 같이 정의한다.

여기서, 는 위치를 의미하며,

는 속도를 의미하며,

는 회전 각을 의미하며,

로 정의되며,

로 정의된다.

이 때, 와 (accelerometer와 gyroscope bias)가 작을 경우, 하기의 수학식 와 같이 추정하게 된다.

또한, 두 인접 프레임(b_k, b_k+1) 사이에 대한 IMU measurement는 하기의 수학식 3과 같이 정의할 수 있다.

여기서, , , 는 두 인접 프레임 사이의 pre-integrated IMU measurement term이다. 이 때, 입력 데이터로는 IMU의 데이터인 각속도 및 가속도이며, 출력 데이터로는 인접 프레임 간의 위치, 속도 및 각도이다.

상기 제2 처리부(200)는 다수의 이미지 데이터를 각각 순차적으로 분석하여 각 이미지 데이터 별로 포함되어 있는 점 형상을 추출한 후, 이를 분석하여 특징점(point feature)을 추출하고, 각 이미지 데이터마다의 특징점을 트래킹하는 것이 바람직하다.

이를 통해서, 상이한 이미지 데이터에 포함되어 있는 동일한 특징점을 판단하여, 이를 통해서 공간(영역)을 인식하게 된다.

여기서, 특징점을 추출하는 경우, Shi-Tomasi corner detection 알고리즘을 사용하는 것이 바람직하다.

상세하게는, Shi-Tomasi corner dectiction 알고리즘은 기존의 특징점 검출 및 추적 알고리즘과 같이 단순한 평행 이동만 가정한 것이 아니라, affine 변환까지 고려하여 다양한 응용에서 우수한 성능을 보인다. 또한, 이렇게 추출된 특징점의 tracking을 위해 KLT sparse optical flow algorithm을 사용한다. 이 때, 입력 데이터로는 RGB 이미지이며, 출력 데이터로는 image에서 관측된 2D point feature의 위치 및 해당하는 2D point feature의 고유 정보(ID 등)이다.

이와 같이, 카메라 이미지를 통해 특징점이 추적되는 경우, point feature residual은 하기의 수학식 4와 같이 정의할 수 있다.

point feature의 triangulation으로 만들어진 3차원 특징점을 새롭게 관측된 특징점 과의 차이를 residual로 정의하게 된다. 각 프레임의 위치와 각도, 특징점의 2D 트래킹 결과 및 3차원 위치 데이터를 입력받아, point feature의 residual을 계산하게 된다.

상기 제3 처리부(300)는 상기 다수의 이미지 데이터를 각각 순차적으로 분석하여, 특징선(line feature)을 추출하게 된다. 이러한 특징선 추출 알고리즘은 종래 기술에 해당하는 라인 세그먼트 검출기(line segment detector)에 의해 추출되며, 이에 대해서 한정하는 것은 아니다.

즉, 상기 제3 처리부(300)는 각 이미지 데이터 별로 포함되어 있는 선 형상을 추출한 후, 이를 분석하여 둘 이상의 이미지 데이터에 연결되는 적어도 하나의 특징선을 추출하게 된다.

또한, 상기 제3 처리부(300)는 각 이미지 데이터 별로 추출한 특징선에 대해서 종래 기술에 해당하는 라인 이진 설명기(line binary descriptor)에 의해 추적함으로써, 선택되는 어느 하나의 특징선이 새롭게 추출(관측)한 특징선인지, 다른 이미지 데이터에 의해 추출한 특징선인지 판단하게 된다.

상기 제3 처리부(300)를 추출한 특징선에 대한 2차원 위치 좌표를 분석하게 된다. 또한, 각 특징선을 구분하기 위하여, 각각의 특징선 별로 고유의 ID를 부여하게 된다.

상기 제2 처리부(200)는 도 1에 도시된 바와 같이, 특징점 추정부(210)를 더 포함하게 된다.

상기 특징점 추정부(210)는 본 발명의 일 실시예에 따른 소실점을 이용한 특징점 기반 SLAM 시스템의 초기 정보에 해당하여 미리 입력받은 상기 이동 로봇의 최초 위치 정보와 상기 제1 처리부(100)로 전송된 상기 IMU 측정 정보를 이용하여, 로봇(이동 로봇)의 위치를 추정하는 것이 바람직하다.

상기 특징점 추정부(210)는 추정한 로봇의 위치와 트래킹한 특징점을 이용하여, 삼각측량(triangulation)을 적용하여, 3차원 특징점의 위치를 추정하게 된다.

이 때, 상기 3차원 특징점의 위치 표현은 x, y, z 좌표 형태로 직관적인 방법으로 나타낼 수 있다.

상기 제3 처리부(300) 역시도 도 1에 도시된 바와 같이, 특징선 추정부(310)를 더 포함하게 된다.

상기 특징선 추정부(310)는 본 발명의 일 실시예에 따른 소실점을 이용한 특징점 기반 SLAM 시스템의 초기 정보에 해당하여 미리 입력받은 상기 이동 로봇의 최초 위치 정보와 상기 제1 처리부(100)로 전송된 상기 IMU 측정 정보를 이용하여, 로봇(이동 로봇)의 위치를 추정하는 것이 바람직하다.

상기 특징선 추정부(310)는 추정한 로봇의 위치와 추출한 특징선을 이용하여, 삼각측량을 적용하여 3차원 특징선의 위치를 추정하게 된다.

이 때, 3차원 특징선의 위치 표현은 3차원 특징점과는 달리 복잡하게 표현된다. 본 발명에서는 플리커(Plㆌcker) 좌표와 정규 직규(orthonormal) 표현을 활용하고 있으나, 이에 대해서 한정하는 것은 아니나, 원활한 설명을 위해 이를 적용하여 설명하도록 한다.

플리커 좌표는 3차원 선을 표현하는 직관적인 방법으로, 3차원 선은 플리커 좌표에 의해 하기의 수학식 5와 같이 표현되게 된다.

여기서, n과 d는 각각 정규 벡터(normal vector)와 방향 벡터(direction vector)를 의미한다.

이러한 플리커 좌표는 삼각측량과 재투영(re-projection) 과정에 활용되며, 실제로 3차원 선은 4-DoF(Dopth of Field, 피사계심도)를 갖는데 반해, 플리커 좌표에 의한 선은 6-DoF이기 때문에, 과도한 매개 변수화 문제가 발생하게 된다.

이를 해결하기 위하여, 본 발명의 일 실시예에 따른 소실점을 이용한 특징점 기반 SLAM 시스템에서는 정규 직교 표현을 하기의 수학식 6과 같이 최적화에 활용하였다.

여기서, Ψ는 카메라 좌표계를 기준으로 오일러 각도에서 3차원 선의 회전 행렬을 의미하며, Φ는 카메라 중심에서 선까지의 최소 거리를 의미한다.

이와 같이, 플리커 좌표와 정규 직교 표현 사이의 변환은 종래 기술에 해당하며, 이에 대한 자세한 설명은 생략한다.

상기 제3 처리부(300)는 플리커 좌표의 3차원 특징선(L)의 재투영(re-projection)은 하기의 수학식 7과 같이 정의할 수 있다.

여기서, l은 재투영된 선, K'은 특징선의 투영 행렬, K는 카메라의 고유 매개변수를 의미하고, (fx, fy) 및 (cx, cy)는 각각 이미지의 초점 거리 및 주점(principal point)을 의미한다. 또한, 이는 정규화된 평면에 적용되기 때문에, K와 K'는 항등 행렬이다. 이를 통해서, 재투영된 선은 정규 벡터와 같다.

이 때, 최종 처리부(500)는 상기 제1 처리부(100), 제2 처리부(200), 제3 처리부(300) 및 제4 처리부(400)에 의한 결과값들을 통합 이용하여, 미리 저장된 비용 함수(cost functions)를 통해 최적화를 수행함으로써, 상기 이동 로봇의 위치 측정/보정과 상기 실내 지도를 생성하고자 하는 영역의 3차원 지도 작성을 수행하게 된다.

이러한 상기 최종 처리부(500)는 제1 잔차 연산부, 제2 잔차 연산부, 제3 잔차 연산부 및 제4 잔차 연산부를 포함하게 된다.

상기 제1 잔차 연산부는 상기 제1 처리부(100)에 의해 사전 통합을 수행한 IMU 측정 정보를 이용한 IMU 잔차(residual)를 연산하는 구성이다.

상기 제2 잔차 연산부는 상기 특징점 추정부(210)에 의해 추정한 3차원 특징점의 위치를 이용하여 특징점 잔차를 연산하는 구성이다.

상기 제3 잔차 연산부는 상술한 과정을 통해서 새롭게 추출한 특징선에 대해 상기 특징선 추정부(310)에 의해 추정한 3차원 특징선을 투영하여, 추출한 특징선과 추정한 3차원 특징선 간의 잔차를 연산하게 된다.

상세하게는, 상기 제3 잔차 연산부는 도 2를 참고로 하여, 하기의 수학식 8과 같이 3차원 특징선의 잔차를 연산하여 재투영 오차로 정의된다.

여기서, , , , , 는 잔차를 의미하며, d는 관찰된 선(추출 특징선)과 재투영된 선(추정 특징선)의 양쪽 끝 점 사이의 거리를 의미하며, Ps, Pe는 관찰된 선의 양 끝선을 의미한다.

이러한 3차원 특징선에 대한 대응하는 야코비안 행렬(Jacobian matrix)은 하기의 수학식 9와 같으며, 는 본체 상태 변화를 의미하며, 는 직교 표현 변화를 의미한다.

여기서, , , 는 카메라 좌표에서 플리커 좌표의 본체 좌표까지의 변환 행렬을 의미한다.

이 후, 상기 제4 처리부(400)는 상기 제3 처리부(300)에 의해 추출한 특징선을 이용하여, 각 특징선들에 의한 소실점(vanishing point)의 위치를 연산하게 되는데, 이 때, 소실점이란, 원근법에 의해 실제로는 평행한 두 직선이 만나는 한 지점을 의미한다. 상기 제4 처리부(400)를 통해서, 상기 제3 처리부(300)에 의해 추출한 특징선이 구조적 규칙성을 갖는지 여부를 결정하게 된다.

상기 제4 처리부(400)는 각 특징선들에 의한 소실점의 위치를 연산함에 있어서, 종래 기술인 J-linkage를 사용하고 있으며, 이에 대해서 한정하는 것은 아니다.

상기 제4 처리부(400)는 각 이미지에서 추출된 모든 특징선들에 대한 무작위 샘플링을 통해 예상 소실점을 생성한 후, 예상 소실점 간의 비교를 통해 유사한 것을 병합하여 소실점을 연산하게 된다.

상세하게는, 상기 제4 처리부(400)는 소실점을 연산한 후, 새롭게 입력되는 이미지 데이터에 의해 제3 처리부(300)로부터 추출한 새로운 특징선을 고려하여, 연산한 소실점의 새 관측치로 적용한다.

도 3은 소실점을 통해 라인이 그룹화되는 과정을 나타낸 도면으로, 도 3의 a)는 입력되는 이미지 데이터이며, 도 3의 b)는 추출한 특징선이며, 도 3의 c)는 추출한 특징선으로부터 소실점을 연산한 후, 동일한 소실점을 갖는 특징선을 동일한 색으로 표현한 것이다.

상기 제4 처리부(400)는 소실점을 연산하기 위하여, 상기 제3 처리부(300)에 의한 특징선을 이루는 각 점들을 하기의 수학식 10과 같이 정의한다.

여기서, , , 은 3차원 특징선의 점을 의미한다.

소실점은 3차원 특징선의 무한대에 있는 점을 투영하는 것과 같으며, 이는 하기의 수학식 11과 같다.

여기서, 은 카메라 투영 행렬을 의미한다.

상술한 바와 같이, 특징선에서의 소실점은 해당하는 특징선의 방향 벡터와 같기 때문에, 도 4에 도시된 바와 같이, 소실점은 와 이미지 평면의 교차점에 의해 연산된다.

더불어, 상기 제4 잔차 연산부는 상기 특징선 추정부(310)에 의해 추정한 3차원 특징선을 이용하여, 각 3차원 특징선에 의한 소실점의 위치를 연산하여, 연산한 3차원 특징선의 소실점 위치와 상기 제4 처리부(400)에 의한 소실점 위치 간의 잔차를 연산하게 된다.

즉, 하기의 수학식 12와 같이, 소실점 잔차 연산을 정의하게 된다.

여기서, 는 소실점 잔차를 의미하며, 는 연산 소실점을 의미한다.

소실점에 대한 대응하는 야코비안 행렬은 하기의 수학식 13과 같으며, , 로 연산할 수 있다.

여기서, , 이다.

상기 최종 처리부(500)는 상기 제1 잔차 연산부, 제2 잔차 연산부, 제3 잔차 연산부 및 제4 잔차 연산부에 의해 연산한 잔차들을 반영하여, 하기의 비용 함수를 통해 최적화를 수행하여, 로봇의 위치 측정과 3차원 지도 작성을 수행하게 된다.

상세하게는, 하기의 수학식 14에서 정의된 상태를 사용하였다.

여기서, , , 는 각각의 세계 좌표, 카메라 좌표 및 로봇 좌표를 의미하며, 은 회전 행렬(rotation matrix), 사분원(quaternion) 또는, 로봇 좌표에서 세계 좌표로 변환된 좌표 변환의 반영을 의미하며, 는 전체 상태를 나타내며, 는 i번째 이미지 데이터에 의한 로봇 상태를 나타내며, 위치, 사분원, 속도, 가속도계의 바이어스 및 자이로스코프의 바이어스로 구성된다.

또한, 전체 상태는 와 같이 표현되는 특징점의 역심도(inverse depths)를 포함하며, 여기서, 이다. 더불어, 정규 직교 표현으로 표현된 선은 로 추가하였으며, I, J, K는 각각 이미지 데이터의 개수, 특징점의 개수, 특징선의 개수를 의미한다.

상기 최종 처리부(500)에 의한 최적화의 전체 목표인 비용 함수는 하기의 수학식 15와 같으며, 최적화 공정에는 Ceres Solver이 사용되었다.

여기서, , , 는 각각 주변화(marginalization), IMU 및 측정한 점 잔차를 의미하며, , , , 는 각각 IMU, 점, 선 및 소실점의 관측치를 의미하며, B는 제1 처리부(100)에 의해 이미지 데이터에서 사전 통합된 모든 IMU 측정의 집합을 의미하며, P, L 및 V는 이미지 데이터에서 추출한 점, 선 및 소실점의 집합을 의미하며, , , 및 는 각각의 IMU, 점, 선 및 소실점 측정 공분한 행렬을 의미하며, ρ는 휴버 노름 함수(Huber norm function)를 의미한다.

도 5는 정의된 비용 함수에 대한 인자 그래프의 예를 나타낸 도면이다.

더 나아가, 본 발명의 일 실시예에 따른 소실점을 이용한 특징선 기반 SLAM 시스템에 대한 성능을 검증하기 위하여, 종래의 SLAM 알고리즘과 성능을 비교를 수행하였다.

상세하게는, EuRoC micro aerial vehicle(MAV) 데이터 세트를 사용하여, 종래의 SLAM 알고리즘과 본 발명의 일 실시예에 따른 소실점을 이용한 특징선 기반 SLAM 시스템에 의한 알고리즘의 비교 테스트를 수행하였다.

각 데이터 셋은 도명, 텍스터, MAV의 속도와 같은 요소에 따라 다양한 수준으로 복잡하게 구성하였으며, 비교 테스트 결과, 하기의 표 1(RMSE 결과)과 같이, 본 발명의 일 실시예에 따른 소실점을 이용한 특징선 기반 SLAM 시스템에 의한 알고리즘이 종래의 SLAM 알고리즘(VINS-Mono, PL-VINS, ALVIO)보다 작은 평균 오차를 보임을 알 수 있다.

또한, 소실점 측정을 통해서 선의 특징을 완전히 관찰할 수 있기 때문에, 보다 정확한 3차원 지도를 생성할 수 있는 장점이 있다.

또한, 도 6은 실제 환경에서 중첩된 LiDAR 포인트 클라우드를 사용하여, 종래의 SLAM 알고리즘(ALVIO)에 의한 3차원 라인 매핑 결과와 본 발명의 일 실시예에 따른 소실점을 이용한 특징선 기반 SLAM 시스템에 의한 알고리즘에 의한 3차원 라인 매핑 결과에 대한 상단 뷰 도면이다.

본 발명의 일 실시예에 따른 소실점을 이용한 특징선 기반 SLAM 시스템은 종래의 SLAM 알고리즘에 보다 구조적인 규칙성을 가지면서도, 높은 인식도를 갖도록 매핑되어있음을 알 수 있다.

이상과 같이 본 발명에서는 구체적인 구성 소자 등과 같은 특정 사항들과 한정된 실시예 도면에 의해 설명되었으나 이는 본 발명의 보다 전반적인 이해를 돕기 위해서 제공된 것 일 뿐, 본 발명은 상기의 일 실시예에 한정되는 것이 아니며, 본 발명이 속하는 분야에서 통상의 지식을 가진 자라면 이러한 기재로부터 다양한 수정 및 변형이 가능하다.

따라서, 본 발명의 사상은 설명된 실시예에 국한되어 정해져서는 아니 되며, 후술하는 특허 청구 범위뿐 아니라 이 특허 청구 범위와 균등하거나 등가적 변형이 있는 모든 것들은 본 발명 사상의 범주에 속한다고 할 것이다.

100 : 제1 처리부
200 : 제2 처리부 210 : 특징점 추정부
300 : 제3 처리부 310 : 특징선 추정부
400 : 제4 처리부
500 : 최종 처리부

Claims (6)

1. 실내 지도를 생성하고자 하는 영역을 연속적으로 이동하는 로봇으로부터 IMU(Inertial Measurement Unit)를 통한 측정 정보와 카메라를 통한 이미지 데이터를 전송받아, 소실점을 이용한 특징선 기반 동시 위치 측정 및 지도 작성을 수행하는 시스템에 있어서,
   IMU 측정 정보를 이용하여, 사전 통합(pre-integration)을 수행하여 각 이미지 데이터 별 로봇의 위치를 추정하는 제1 처리부;
   다수의 이미지 데이터를 각각 순차적으로 분석하여 특징점(point feature)을 추출하고, 각 이미지 데이터마다의 특징점을 트래킹하는 제2 처리부;
   상기 다수의 이미지 데이터를 각각 순차적으로 분석하여 특징선(line feature)을 추출하고, 선택되는 어느 하나의 특징선이 새롭게 추출한 특징선인지 이전 이미지 데이터에 의해 추출한 특징선인지 판단하는 제3 처리부;
   상기 제3 처리부에 의해 추출한 특징선을 이용하여, 각 특징선들에 의한 소실점(vanishing point)의 위치를 연산하는 제4 처리부; 및
   상기 제1 처리부 내지 제4 처리부에 의한 결과값들을 이용하여, 기저장된 비용 함수(cost functions)를 통해 최적화를 수행하여, 로봇의 위치 측정과 영역의 지도 작성을 수행하는 최종 처리부;
   를 포함하는, 소실점을 이용한 특징선 기반 SLAM 시스템.
   
2. 제 1항에 있어서,
   상기 제2 처리부는
   추정한 로봇 위치와 트래킹한 특징점을 이용하여, 삼각측량(triangulation)을 통해 3차원 특징점을 추정하는 특징점 추정부;
   를 더 포함하는, 소실점을 이용한 특징선 기반 SLAM 시스템.
   
3. 제 2항에 있어서,
   상기 제3 처리부는
   추정한 로봇 위치와 추출한 특징선을 이용하여, 삼각측량을 통해 3차원 특징선을 추정하는 특징선 추정부;
   를 더 포함하는, 소실점을 이용한 특징선 기반 SLAM 시스템.
   
4. 제 3항에 있어서,
   상기 최종 처리부는
   새롭게 추출한 특징선에 대해 상기 특징선 추정부에 의해 추정한 3차원 특징선을 투영하여, 추출한 특징선과 추정한 3차원 특징선 간의 잔차를 연산하는 구성;
   을 더 포함하는, 소실점을 이용한 특징선 기반 SLAM 시스템.
   
5. 제 4항에 있어서,
   상기 최종 처리부는
   상기 특징선 추정부에 의해 추정한 3차원 특징선을 이용하여, 각 3차원 특징선에 의한 소실점의 위치를 연산하여, 연산한 3차원 특징선의 소실점 위치와 상기 제4 처리부에 의한 소실점 위치 간의 잔차를 연산하는 구성;
   을 더 포함하는, 소실점을 이용한 특징선 기반 SLAM 시스템.
   
6. 제 5항에 있어서,
   상기 최종 처리부는
   연산한 잔차들을 반영하여, 하기의 비용 함수를 통해 최적화를 수행하여, 로봇의 위치 측정과 영역의 지도 작성을 수행하는, 소실점을 이용한 특징선 기반 SLAM 시스템.