1. 서 론

최근 생산, 서비스 분야에서 효율적인 업무를 위한 무인이동체의 활용이 확대되고 있다^[1]. 실내 항법은 실내 환경에서 무인이동체의 원활한 임무 수행을 위한 위치 파악에 중요한 주제로, GNSS(global navigation satellite system) 신호가 접근하기 어려운 실내 공간에서 정확한 위치 추정과 경로 탐색을 위해 연구되고 있다. 실내 항법은 센서를 기반으로 한 방법과 실내에 설치된 비콘(vicon), 와이파이, 자기장, UWB 등의 신호를 활용한 위치 추정 기술 등이 대표적으로 개발되고 있다^[2]-[5]. 신호를 기반으로 한 위치 추정 방식은 GNSS 신호를 기반으로 한 시스템과 유사하게 정확한 위치 추정 결과를 도출하지만, 해당 방식의 사용이 불가능한 상황에서는 다중 센서 융합을 이용한 항법 시스템 구성이 불가피하다. 일반적으로 3축 가속도계와 3축 각속도계를 통해 가속도와 각속도를 측정하는 관성측정장치(inertial measurement unit, IMU), 그리고 위치나 속도와 같은 항법 계산에 유용한 정보를 제공하는 센서를 융합한 항법 알고리즘을 구성한다^[6][7].

이 중 카메라와 IMU를 사용한 영상-관성항법시스템(visual inertial navigation system, VINS) 알고리즘은 영상으로부터 명확한 특징점을 검출하고, 각각의 프레임 사이에 같은 특징점의 매칭과 비교를 통해 이동체의 위치를 추정한다^[8]. 본 논문에서는 OpenCV에서 제공하는 ArUco 마커를 이미지의 랜드마크로 사용하였다. ArUco 마커는 특별한 패턴으로 구성된 2D 바코드로, 카메라 이미지에서 인식되고 추적될 수 있으며, 고유한 식별자(ID)를 가지고 있어 실시간으로 감지되고, 단안 카메라로도 마커의 깊이 추정이 가능하여 마커의 코너를 기준으로 3축 위치와 방향을 취득할 수 있다^[9]-[12].

마커를 사용하여 실내 항법을 수행하는 연구는 주로 사용자가 각각 다른 ID의 마커를 정해진 위치에 부착하고, 항법 좌표계 내부의 위치를 안 상태에서 탐지되는 마커의 ID 비교를 통해 칼만필터(Kalman filter)를 사용하여 로봇의 위치를 보정하는 시스템을 구성한다^[13]-[15]. 사전에 알고 있는 마커의 위치와 카메라, 로봇 간의 벡터 계산을 통해 마커로부터 계산된 로봇의 위치를 칼만필터의 측정치로 대입하며, 재귀적인 방식으로 추정한다.

본 논문에서 제안하는 알고리즘은 각 랜드마크의 위치를 모르는 상태에서도 초기 상태에서 추정되는 랜드마크의 위치를 저장하고, 이동 과정에서 추가적으로 탐지되는 랜드마크의 위치를 추정한다. 추정된 랜드마크의 위치를 통해 로봇의 위치를 계산하고, 취득할 수 있는 속도 정보 또한 활용하여, 확장칼만필터(extended Kalman filter, EKF)의 측정치로 사용하였고 로봇의 위치를 재추정하는 알고리즘 설계하였다. 근거리에 있는 랜드마크의 위치 추정 정확도가 장거리에서 탐지되는 랜드마크에 비해 더 높다고 판단하여 로봇이 정해진 경로를 이동하면서 탐지하는 랜드마크의 위치를 지속적으로 추정하였고, 랜드마크를 지속적으로 탐지할 수 있도록 환경을 조성하여 로봇의 위치를 계속해서 추정한다. 제안하는 알고리즘으로 사람이 탐색하기 어려운 곳을 저가형 센서를 부착한 로봇을 통해 이동 경로 지도를 제작하거나, 수색 환경이 주기적으로 변하여 로봇의 이동 경로를 계속해서 변경해야하는 경우 경로의 무작위 위치에 마커를 부착하는 것만으로 위치를 추정할 수 있다. 전반적인 알고리즘의 흐름은 그림 1로 나타내었다.

본 논문의 구성은 다음과 같다. 먼저 2장에서 알 수 없는 위치에 부착된 항법 좌표계 내 마커의 위치를 추정하는 방식에 대해 설명하고, 3장에서는 2장에서 추정한 마커의 위치를 기반으로 확장칼만필터를 사용한 영상-관성 복합 항법 위치 추정 기법에 대해 설명한다. 4장에서 실제 실험을 통해 취득한 데이터를 토대로 제안하는 알고리즘의 타당성을 검증하였으며, 마지막으로 5장에서 결론을 맺는다.

그림 1. 제안한 알고리즘 순서도

Fig. 1. The proposed algorithm flowchart

2. 마커의 위치 추정

2D의 이미지에서 3D 공간좌표로 복원하는 과정에서 보다 정확한 계산 결과를 얻기 위해 초점 거리, 주점 등과 같은 내부 요인을 보상하는데, 이 과정을 카메라의 캘리브레이션(Calibration)이라고 한다. 일반적으로 캘리브레이션을 통해 취득한 내부 파라미터를 이용하여 동일한 특징점을 서로 다른 위치에서 촬영한 2개의 이미지의 기하학적 대조 과정을 거쳐 깊이를 추정한다. 하지만, ArUco 마커는 인식한 코너를 통해 실제 마커 크기와 비교하기 때문에 하나의 이미지만으로도 깊이 추정이 가능하다. 따라서 탐지된 마커의 각 코너 좌표와, 배치한 마커의 크기, 그리고 내부 파라미터를 입력받아 마커의 좌표계에서 계산된 카메라의 3D 상의 이동 벡터(translation vector)와 회전 벡터(rotation vector)를 얻을 수 있다. 이동 벡터와 회전 벡터는 각각 원래 위치에서 새로운 위치로의 변위와 회전의 방향을 의미한다. 그림 2는 각 좌표계 사이의 위치 벡터를 도시하였다. 이때 $\{m\}$, $\{n\}$, $\{b\}$, $\{c\}$는 각각 마커, 항법, 동체, 카메라 좌표계를 의미하며, 항법 좌표계로는 NED(North-East-Down) 좌표계를 사용한다.

그림 2. 각 좌표계 간 위치 벡터

Fig. 2. Position vectors with respect to each frame

본 논문에서 사용되는 수식의 표기법 중 $\widetilde{A}$는 측정치를 $\hat{A}$은 추정치를 의미하며, 벡터 $p_{A}^{B}$는 B 좌표계에 대한 A의 위치 벡터를 나타낸다. 추정해야하는 값인 마커의 위치 $\hat{p}_{m}^{n}$과 로봇의 위치 $\hat{p}_{b}^{n}$은 빨간색으로 표현하였으며, 하드웨어의 기본 구성을 통해 알고 있는 벡터를 녹색, 센서로부터 취득 가능한 벡터를 파란색으로 표현하였다. 따라서, 추정하고자 하는 항법 좌표계 내 마커의 위치는 식 (1)을 통해 계산할 수 있다.

이때, $\hat{p}_{b,\: k}^{n -}$은 관성항법시스템을 거쳐 추정된 $k$단계의 위치 추정 결과이며, $\widetilde{p}_{c,\: k}^{m}$는 카메라를 통해 측정된 $k$단계의 마커 좌표계에 대한 카메라의 위치 측정치를 나타낸다. $R_{b}^{n}$은 동체 좌표계와 항법 좌표계 간의 변환 행렬이고, $R_{c}^{b}$와 $p_{c}^{b}$는 각각 카메라 캘리브레이션을 통해 얻는 카메라와 동체 좌표계 간의 변환 행렬과 동체 좌표계에 대한 카메라의 위치 벡터이며, 각각 상수 행렬 및 상수 벡터이다. $\widetilde{R}_{c,\: k}^{m}$은 마커로부터 측정되는 회전 벡터를 기반으로 한 변환 행렬이며, 위 식에서 사용된 $\widetilde{R}_{m}^{c,\: k}$는 이의 전치 행렬로 마커 좌표계에서 카메라 좌표계로의 변환 행렬이다.

한 프레임 내에 여러 개의 마커를 인식한 경우 매 시간마다 각각의 추정된 위치를 대입 및 비교하였으며, 반복적으로 위치를 추정하여 이상치가 계산되는 경우 이를 제거하여 정확한 추정치를 계산하도록 구성하였다.

3. 마커를 활용한 확장칼만필터 기반 항법 알고리즘

본 논문에서는 제안하는 알고리즘은 관성항법시스템(inertial navigation system, INS)에 의해 계산된 위치와 앞서 추정된 마커의 위치로부터 계산된 위치 측정치 사이의 오차를 추정하는 확장칼만필터를 사용한다. 추정된 오차를 통해 가속도계와 각속도계 바이어스를 지속적으로 업데이트하고, 정확한 추정을 할 수 있도록 보정한다^{[16], [17]}.

3.1 확장칼만필터 기반 영상-관성 복합 항법 알고리즘

관성항법시스템의 위치 오차($\delta p_{b}^{n}$), 속도 오차($\delta v_{b}^{n}$), 자세 오차($\delta\psi_{b}^{n}$), 가속도계 바이어스($b_{a}$), 각속도계 바이어스($b_{g}$)를 상태 변수로 정의하여 이를 추정하고 INS를 통해 추정된 항법 정보의 오차 보정 과정을 거쳐 최종적인 항법 정보를 계산한다. 이때, 상태 변수 추정 모델과 측정치 계산은 다음과 같이 계산된다^{[18], [19]}.

(2)

$\delta x=\begin{bmatrix}\delta p_{b}^{n^{T}}\delta v_{b}^{n^{T}}\delta\psi_{b}^{n^{T}}b_{a}^{T}b_{g}^{T}\end{bmatrix}^{T}\in ℝ^{15\times 1}$

(3)

$\delta x_{k}=F_{k-1}\delta x_{k-1}+w_{k-1},\: w\sim N(0,\: Q_{k})$

(4)

$z_{k}=H_{k}\delta x_{k}+\nu_{k,\:}\nu_{k}\sim N(0,\: R_{k})$

이때, 시스템 상태 변수 $\delta x$는 $15\times 1$의 벡터 형태로 표현되며, $z_{k}$는 측정치이고, $H_{k}$는 관측 행렬로 상태 변수와 측정치 간의 관계를 의미하며, 필터 공정 잡음 $w_{k}$와 측정치 잡음 $\nu_{k}$은 각각 평균이 0이고, $Q_{k}$, $R_{k}$을 분산으로 하는 가우시안 모델이다. $F_{k}$는 시스템 모델이며, 아래 식 (5)와 (6)과 같이 정의된다.

(5)

$F_{k-1}=\begin{bmatrix}0_{3\times 3}&I_{3\times 3}&0_{3\times 3}&0_{3\times 3}&0_{3\times 3}\\0_{3\times 3}&0_{3\times 3}&F_{23}&\hat{R}_{b}^{n}&0_{3\times 3}\\0_{3\times 3}&0_{3\times 3}&0_{3\times 3}&0_{3\times 3}&-\hat{R}_{b}^{n}\\0_{3\times 3}&0_{3\times 3}&0_{3\times 3}&-b_{a}I_{3\times 3}&0_{3\times 3}\\0_{3\times 3}&0_{3\times 3}&0_{3\times 3}&0_{3\times 3}&-b_{g}I_{3\times 3}\end{bmatrix}$

(6)

$F_{23}=\begin{bmatrix}0&\hat{f}_{z}&-\hat{f}_{y}\\-\hat{f}_{z}&0&\hat{f}_{x}\\\hat{f}_{y}&-\hat{f}_{x}&0\end{bmatrix}$

식 (6)의 $\begin{bmatrix}\hat{f}_{x}\hat{f}_{y}\hat{f}_{z}\end{bmatrix}^{T}=\hat{R}_{b}^{n}(f-\hat{b}_{a})$으로 정의되며 보상된 동체 좌표계의 비중을 나타낸다. 일반적으로, 칼만필터의 구조는 크게 예측 단계(time update)와 측정치 갱신(measurement update)으로 나뉜다. 예측 단계에서는 식 (7), (8)을 통해 시스템 모델을 갱신하고 시스템 공분산 $P_{k}^{-}$를 예측한다. 칼만필터의 전체 단계에서 상태 변수의 추정이 두 차례 이루어지며, 추정 과정에서 변수의 지수에 위치한 “‒” 기호는 $k$ 시간의 예측 단계에서 추정된 아 프리오리(a priori) 상태 추정 값을 의미한다.

(7)

$\hat{x}_{k}^{-}=F_{k-1}\hat{x}_{k-1}^{+}+w_{k-1},\: w\sim N(0,\: Q_{k})$

(8)

$P_{k}^{-}=F_{k-1}P_{k-1}^{+}F_{k-1}^{T}+Q_{k-1}$

예측 단계에서 추정된 시스템 공분산을 통해 식 (9)과 같이 칼만 이득 $K_{k}$를 계산하고, 식 (10), (11)을 통해 측정치를 갱신하여 상태 변수 $\delta x_{k}^{+}$와 시스템 공분산 $P_{k}^{+}$를 추정하고, 이때 “+” 기호는 측정치 갱신을 거쳐 추정된 아 포스테리어리(a posteriori) 상태 추정 값을 의미한다. 오차 추정 결과를 이용하여 상태 변수를 보정하는 오차 보상 단계는 식 (12)과 같이 계산된다.

(9)

$K_{k}=P_{k}^{-}H_{k}^{T}(H_{k}P_{k}^{-}H_{k}^{T}+ R_{k})^{-1}$

(10)

$\delta\hat{x}_{k}^{+}=K_{k}(z_{k}-H_{k}\hat{x}_{k}^{-})$

(11)

$P_{k}^{+}=(I-K_{k}H_{k})P_{k}^{-}(I-K_{k}H_{k})^{T}+K_{k}R_{k}K_{k}^{T}$

(12)

$\hat{x}_{k}^{+}=\hat{x}_{k}^{-}+\delta\hat{x}_{k}^{+}$

그림 3. 마커 기반 EKF 알고리즘 구조도

Fig. 3. Marker-based EKF algorithm overall structure.

일반적으로, 칼만필터를 통한 항법 알고리즘은 측정치로부터 항법 정보를 보정하기 때문에, 측정치의 정확도가 전반적인 항법 정확도에 큰 영향을 미치는 것을 확인할 수 있다. 실내 항법에서는 사용할 수 있는 센서가 제한됨에 따라 더 많은 측정치를 사용하거나, 고성능의 센서를 사용하여 관성항법시스템의 결과를 보정할 수 있도록 한다. 본 논문에서 사용된 영상-관성항법시스템은 영상에서 인식된 ArUco 마커를 통해 계산된 위치와 속도 정보를 측정치로 사용하며, 보다 정확한 위치를 추정하기 위해 이상치를 제거하는 방법을 제안한다.

마커 기반의 확장칼만필터 알고리즘의 세부 알고리즘 구조는 그림 3의 블록선도로 나타내었다.

3.3 마커 기반 영상-관성항법 알고리즘

앞서 구한 위치와 속도 측정치를 사용하여 확장칼만필터를 구성하였다. 이때 측정치 $z_{k}$는 관성항법시스템에서 추정한 상태 변수와 측정치 간의 오차로 식 (20)과 같이, 측정치 모델에 의한 관측 행렬 $H_{k}$는 식 (21)로 정의하였다.

(20)

$z_{k}=\begin{bmatrix}\hat{p}_{k}^{-}-\widetilde{p}_{b}^{n}\hat{v}_{k}^{-}-\widetilde{v}_{b}^{n}\end{bmatrix}$

(21)

$H_{k}=\begin{bmatrix}I_{3\times 3}0_{3\times 3}0_{3\times 3}0_{3\times 3}0_{3\times 3}\\0_{3\times 3}I_{3\times 3}0_{3\times 3}0_{3\times 3}0_{3\times 3}\end{bmatrix}$

여기서 $\hat{p}_{k}^{-}$와 $\hat{v}_{k}^{-}$는 관성항법시스템으로부터 추정된 위치와 속도이며, 앞서 추정된 바이어스에 비해 측정치의 잔차(residual)가 상당히 클 경우 이를 이상치로 판단하고, 적응형 규칙을 적용하여 측정치 공분산 $R_{k}$를 조정하였다.

카메라와 관성측정장치를 통해 취득된 데이터를 기반으로 실내 항법 정확도 향상을 위한 간접 확장칼만필터의 알고리즘 구성은 아래의 Algorithm 1과 같이 요약된다.

4. 실내 항법 검증 실험 및 결과 분석

위성 항법 및 신호 기반 항법이 불가능한 환경에서 제안한 알고리즘의 성능을 검증하기 위해 마커의 위치를 모르는 상태로 경로를 설정한 후 실험을 진행하였다. 실험 간 사용된 로봇은 WeGo 사에서 제작한 Scout Mini에 항법 센서를 부착할 수 있도록 개조하여 사용하였으며, 본 실험은 로봇의 초기 정지 상태에서 하나 이상의 마커가 인식될 수 있도록 설치하였고, 매 순간 마커가 탐지될 수 있도록 곳곳에 위치하여야 한다는 가설을 설정하였다. 또한, 제안한 항법 알고리즘은 3D 상에서 작동할 수 있도록 설계하였으나, 실험은 항법 좌표계의 Z축을 제외하고 2D 결과를 도출하였다. 그림 4와 같이 ‘ㄷ’자 형태의 경로로 직진 및 제자리 회전 주행하였고, 초기 정지 상태에서 마커 ①을 포함한 인식 가능한 모든 마커의 위치를 추정하고, 추후 주행 과정에서 지속적으로 업데이트하였다. 제안된 알고리즘이 저가형 센서를 사용하였을 때에도 원활하게 위치 추정이 가능한지 확인하기 위해 IMU는 ROBOR 社의 RB-SDA-v1을 사용하였고, 카메라는 intel 社의 D435i의 RGB 카메라를 사용하였다. 표 1은 실험에 사용된 센서의 사양을 도시하였다.

이미지에서 마커를 인식한 경우 녹색 상자를 그리며 카메라와의 이동 벡터 $\widetilde{p}_{c}^{m}$을 계산할 수 있으며, 동체 좌표계와 항법 좌표계는 동일한 방향으로 설정하였다. 또한, 카메라와 마커의 좌표계는 항법 좌표계 위에서 각각 다르게 위치하고 있으며, 상세한 3축 방향에 대해 그림 5에 나타내었다.

그림 4. 알고리즘 검증 실험 환경 및 주행 경로

Fig. 4. Experimental environment and path

그림 5. 실내 항법 실험 환경 설치

Fig. 5. The indoor experimental environment setup

4.1 마커 측정치 기반 로봇 위치 추정

실험을 통해 취득한 데이터를 토대로 제안한 알고리즘의 신뢰성을 판단하였다. 그림 6은 실내 항법 알고리즘을 적용한 위치 추정 결과를 도시하였다. 그림 내에 그려진 점은 각각의 마커로부터 추정된 로봇의 위치 즉 위치 측정치를 나타낸 것이며, 빨간색으로 그려진 선은 참값으로 실제 로봇의 이동 경로를 나타낸다. 또한, 마커의 속도 측정치만을 사용하여 로봇의 위치를 추정한 결과는 녹색 점선으로 나타내었고, 위치와 속도 측정치를 모두 사용한 결과를 파란색 점선으로 표시하였다. 추가로 도시되어 있는 점으로 표시된 데이터는 각각 마커로부터 계산된 위치 측정치를 의미한다. 인식된 마커 별로 다른 색과 모양으로 표현하여 구분하였다. 표 2는 속도 측정치만 사용한 결과와 위치 및 속도 측정치를 모두 사용하여 로봇의 위치를 추정한 결과에 대한 RMSE(root mean square error)를 나타내었다.

표 2 측정치에 따른 위치 및 방향각 추정 결과에 대한 RMSE

Table 2 Specifications of the sensors used during the experiments

구 분	X	Y	Heading
Only velocity Measurement	2.8757	0.2855	0.2557
Position & Velocity Measurement	0.3285	0.1021	0.0145

그림 6. 위치 추정치 비교 분석: 카메라에서 도출된 속도 측정치만 사용한 경우와 위치 및 속도 측정치를 모두 사용한 결과 분석

Fig. 6. Comparative analysis of position estimation: Evaluating results with only velocity measurement versus combined position and velocity measurement from the Camera.

위치를 추정한 결과 구상 단계에서 예상했던 것과 같이 정확도가 개선된 것을 확인할 수 있다. 본 논문에서는 마커의 항법 좌표계 위 위치를 추정하며, 마커의 정확한 위치인 $p_{m}^{n}$을 알 수 없어 로봇 위치 추정치 $\hat{p}_{b}^{n}$과 카메라를 통해 측정되는 $\widetilde{p}_{c}^{m}$ 간의 상관관계를 계산할 수 없기 때문에 칼만필터에서 필요한 위치 측정치에 대한 모델을 구할 수 없다. 하지만, 속도 측정치의 경우 $p_{m}^{n}$와 관계없이 $\widetilde{p}_{c}^{m}$로부터 측정 가능하기에 위치 측정치를 통한 결과가 아닌 속도 측정치만 사용한 결과를 통해 검증하였다. 속도 측정치를 사용한 경우 위치 추정의 관측가능성이 없기 때문에 로봇의 위치 추정치가 발산하여 참값과 크게 벗어난 부분을 확인하였고, 위치 측정치를 함께 사용할 때 이 점이 크게 개선된 것을 볼 수 있었다.

5. 결 론

본 논문에서는 비콘이나 와이파이 등 신호 기반 실내 항법이 불가능한 상황에서 알 수 없는 위치에 배치된 마커의 위치를 단안 카메라를 통해 추정하고, 추정된 마커의 위치를 기반으로 운행 중인 모바일 로봇의 위치를 재추정하는 알고리즘을 제안한다. 확장칼만필터를 기반으로 항법 알고리즘을 구성하였으며, 마커로부터 위치와 속도 측정치를 사용하여 상태 변수의 추정 성능을 향상시켰다. 알고리즘의 신뢰성 검증을 위해 모바일 로봇에 센서를 탑재한 후 마커의 위치를 무작위로 설치하여 실험을 진행하였으며, 위치 측정치만을 사용한 항법 알고리즘과 비교하여 개선된 결과를 확인하였다. 이를 통해 신호 수신 가능 구역이 한정되어 있고, 신호의 세기에 영향을 받는 신호 기반의 항법 알고리즘에 비해, 보다 넓은 범위에서 마커의 설치만으로 단안 카메라와 관성측정장치를 통해 항법 해를 계산하는 제안된 알고리즘이 넓은 범용성과 안정적인 위치 추정 가능성을 확인하였다.